应用于组合导航系统的代码Word文档格式.doc
- 文档编号:14441566
- 上传时间:2022-10-22
- 格式:DOC
- 页数:18
- 大小:91.50KB
应用于组合导航系统的代码Word文档格式.doc
《应用于组合导航系统的代码Word文档格式.doc》由会员分享,可在线阅读,更多相关《应用于组合导航系统的代码Word文档格式.doc(18页珍藏版)》请在冰豆网上搜索。
fai=(pi/180).*[1/361/365/36];
%(100'
'
100'
500'
)
r=zitai
(1)+fai
(1);
p=zitai
(2)+fai
(2);
y=zitai(3)+fai(3);
%当地坐标系(l)相对于载体坐标系(b)的转换矩阵:
Rbl(在e,n,u坐标系下)
Rbl=[cos(r)*cos(y)-sin(r)*sin(y)*sin(p)-sin(y)*cos(p)cos(y)*sin(r)+sin(y)*sin(p)*cos(r)
cos(r)*sin(y)+sin(r)*cos(y)*sin(p)cos(y)*cos(p)sin(y)*sin(r)-cos(y)*sin(p)*cos(r)
-cos(p)*sin(r)sin(p)cos(p)*cos(r)];
%由转换矩阵Rbl计算四元数的初始值Q0=[q1,q2,q3,q4]'
QQ=[0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3))
0.25*(Rbl(3,2)-Rbl(2,3))/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)))
0.25*(Rbl(1,3)-Rbl(3,1))/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)))
0.25*(Rbl(2,1)-Rbl(1,2))/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)))];
%IMU系统给出的初始值:
速度(ve,vn,vu),位置(l,m,h)=(经度,纬度,高度),姿态误差角fai(e,n,u)
vIMU=[-21.775011 -71.631027 3.10094];
point_IMU=[0.147022986 0.81837365 3122.826];
T=1;
%UKF滤波的采样周期(s)
%陀螺常值漂移初始值tuo(e,n,u)(单位:
。
/s)
tuo=[000];
%陀螺一阶相关时间Tt(s)
Tt=[606060];
%加速度计常值漂移初始值jiasu(e,n,u)(单位:
m/s^2)
jiasu=[000];
%加速度计一阶反相关时间Ta(s)
Ta=[606060];
%IMU系统的状态向量X
x=[vIMUpoint_IMUfaituojiasu]'
;
%观测量噪声V的斜方差矩阵
R=[];
%GPS系统的量测值Z(vn,ve,vd,m,l,h)
[zRz]=gps_tiqushuju;
%Vk的噪声序列方差为:
Rk
Rz=(Rz./T);
%陀螺常值漂移wt(e,n,u)
wt=(pi/180).*[1/(3600)1/(3600)1/(3600)];
%1(。
/h)
%陀螺常值漂移误差wtt(e,n,u)
wtt=(pi/180).*[0.08/(3600)0.08/(3600)0.08/(3600)];
%0.08(。
%加速度计常值漂移wa(e,n,u)
wa=[2e-62e-62e-6];
%200μg(2e-6m/s^2)
%加速度计常值漂移误差waa(e,n,u)
waa=[2e-62e-62e-6]./4;
%50μg(0.5e-6m/s^2)
%姿态误差角噪声wg
wg=wt;
%状态向量X的斜方差矩阵
P=eye(length(x))*eps;
%note:
forstability,Pshouldneverbequitezero
%速度误差:
(0.1m/s)位置误差:
水平(1m),高度(5m)
u=[0.10.10.10.0004744290.0004744292wgwttwaa]'
%IMU系统在载体坐标系下的比力值输出值fb
fb=[];
%IMU系统中陀螺输出量
wib=[];
%为载体坐标系(b)相对于惯性坐标系(i)的角速度向量
[fw]=IMU_tiqushuju;
%IMU系统输出的比力值和角速度
%%%%%%%%%通过GPS观测值计算得到的姿态角
zitaijiao_gps=xlsread('
D:
\myfile\UKF\kalmanfilter_MATLAB\germany_ukf\原始数据\zitiajiao-gps.xls'
);
%%%------------------------------------------------------------
%%循环开始:
for1:
n
outx=[];
outp=[];
detajiao=zeros(3,1);
NN=1000;
fori=1:
NN
outx=[outx;
x'
];
outp=[outp;
diag(P)'
%大地子午圈曲率半径:
RM
RM=a*(1-e2)/(1-e2*(sin(x(5)))^2)^(2/3);
%地球卯酉圈的曲率半径:
RN
RN=a/sqrt(1-e2*(sin(x(5)))^2);
%当地坐标系下的比力值fl
%IMU系统在载体坐标系下的比力值输出值fb转换到当地坐标系(l)下fl
fb=f(i,:
)'
%fl=Rbl*(fb+[x(13)x(14)x(15)]'
%初始对准,比力值加上加速度计的测量偏差
fl=Rbl*fb;
%计算IMU的速度、位置输出减去GPS相应的输出值:
deta(ve,vn,l,h)
zd=z(i,:
deta=x([1256],:
)-zd([1256],:
%观测值zz,及观测噪声R
zz=z(i+1,:
v=Rz(i+1,:
%zitai_v=[0.0010.02660.9664]'
%GPS观测值姿态角的误差
zitai_v=[0.000017.0983e-0040.0006]'
v=[v;
zitai_v];
R=diag(v.^2);
%%通过GPS观测值,计算得到roll=0,pitch=atan(ve/vn),yaw=asin(h12/s12),将姿态误差角加入观测量中进行滤波计算
zz=[zz;
detajiao];
%%%%GPS计算得到的姿态角
z_zitai=zitaijiao_gps(i+1,:
%%IMU系统的力学编排方程和姿态误差方程离散化之后的截断误差:
ve=x
(1);
vn=x
(2);
vu=x(3);
l=x(4);
m=x(5);
h=x(6);
faie=x(7);
fain=x(8);
faiu=x(9);
tuo1=x(10);
tuo2=x(11);
tuo3=x(12);
fl1=fl
(1);
fl2=fl
(2);
fl3=fl(3);
deta1=deta
(1);
deta2=deta
(2);
deta3=deta(3);
deta4=deta(4);
wg1=wg
(1);
wg2=wg
(2);
wg3=wg(3);
jiasu1=x(13);
jiasu2=x(14);
jiasu3=x(15);
Ta1=Ta
(1);
Ta2=Ta
(2);
Ta3=Ta(3);
wa1=wa
(1);
wt2=wa
(2);
wt3=wa(3);
Q=diag((u).^2);
%predict
[x,P]=predict(x,P,u,Q,T);
%update
[x,P]=update(x,P,zz,R);
%%
xx=x;
x=xx(1:
15,1);
u=xx(16:
30,1);
%u
(1)=u
(1)+x(13);
%u
(2)=u
(2)+x(14);
%u(3)=u(3)+x(15);
%u(7)=x(10);
%u(8)=x(11);
%u(9)=x(12);
PP=P;
P=PP(1:
15,1:
15);
%利用四元数Q计算转换矩阵Rbl
%首先计算在当地坐标系(l)下的角速度向量wbl
%地固坐标系(e)相对于当地坐标系(l)的转换矩阵:
Rel=Rle'
%Rel=[-sin(x(4))cos(x(4))0
%-sin(x(5))*cos(x(4))-sin(x(5))*sin(x(4))cos(x(5))
%cos(x(5))*cos(x(4))cos(x(5))*sin(x(4))sin(x(5))];
wie=[00we]'
%wie为地球自转角速度向量
%%%%%%%
omiga12=[];
fork=0:
1
wib=w(i+k,:
+T.*[x(10)x(11)x(12)]'
%初始对准,角速度加上陀螺漂移
wiel=[0we*cos(x(5))we*sin(x(5))]'
%wiel=Rel*wie;
well=[-x
(2)/(RM+x(6))x
(1)/(RN+x(6))x
(1)*tan(x(5))/(RN+x(6))]'
will=wiel+well;
wlb=wib-Rbl'
*will;
%四元数的更新
%计算反对称矩阵omiga
omiga=[0wlb(3)-wlb
(2)wlb
(1)
-wlb(3)0wlb
(1)wlb
(2)
wlb
(2)-wlb
(1)0wlb(3)
wlb
(1)wlb
(2)wlb(3)0];
omiga12=[omiga12,omiga];
end
omiga1=omiga12(1:
4,1:
4);
omiga2=omiga12(1:
4,5:
8);
%采用四阶龙格-库塔法数值积分解Q(K+1)
QQ=QQ+
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 应用于 组合 导航系统 代码