应用于组合导航系统的代码Word文档格式.doc

1、fai=（pi/180）.*1/36 1/36 5/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）*si

2、n（p）*cos（r） -cos（p）*sin（r） sin（p） cos（p）*cos（r）;%由转换矩阵Rbl计算四元数的初始值Q0=q1,q2,q3,q4QQ=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）

3、+Rbl（3,3）;%IMU系统给出的初始值：速度（ve,vn,vu），位置（l,m,h）（经度，纬度,高度），姿态误差角fai（e,n,u）vIMU=-21.775011-71.6310273.10094;point_IMU=0.1470229860.818373653122.826;T=1; %UKF滤波的采样周期（s）%陀螺常值漂移初始值tuo（e,n,u）（单位：。/s）tuo=0 0 0;%陀螺一阶相关时间Tt（s）Tt=60 60 60;%加速度计常值漂移初始值jiasu（e,n,u）（单位：m/s2）jiasu=0 0 0;%加速度计一阶反相关时间Ta（s）Ta=60 60 60

4、;%IMU系统的状态向量Xx=vIMU point_IMU fai tuo jiasu;%观测量噪声V的斜方差矩阵R=;%GPS系统的量测值Z（vn,ve,vd,m,l,h）z Rz=gps_tiqushuju;%Vk的噪声序列方差为：RkRz=（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-

5、6 2e-6 2e-6; % 200g （2e-6 m/s2）%加速度计常值漂移误差waa（e,n,u）waa=2e-6 2e-6 2e-6./4; % 50g （0.5e-6 m/s2）%姿态误差角噪声wgwg=wt; %状态向量X的斜方差矩阵P = eye（length（x）*eps; % note: for stability, P should never be quite zero%速度误差：（0.1m/s） 位置误差：水平（1m）,高度（5m）u=0.1 0.1 0.1 0.000474429 0.000474429 2 wg wtt waa%IMU系统在载体坐标系下的比力值输出值

6、fbfb=;%IMU系统中陀螺输出量wib=; %为载体坐标系（b）相对于惯性坐标系（i）的角速度向量f w=IMU_tiqushuju; %IMU系统输出的比力值和角速度%通过GPS观测值计算得到的姿态角zitaijiao_gps=xlsread（D:myfileUKFkalmanfilter_MATLABgermany_ukf原始数据zitiajiao-gps.xls）;%-% 循环开始 ：for 1:noutx=;outp=;detajiao=zeros（3,1）;NN =1000;for i=1:NN outx=outx;x; outp=outp;diag（P） %大地子午圈曲率半径：

7、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（1 2 5 6,:）-zd（1 2 5 6,: %观测值zz,及观测噪声R

8、 zz=z（i+1,: v=Rz（i+1,: %zitai_v=0.001 0.0266 0.9664 %GPS观测值姿态角的误差 zitai_v=0.00001 7.0983e-004 0.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

9、）;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

10、）;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）下的角速

11、度向量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=0 0 we %wie 为地球自转角速度向量%omiga12=; for k=0:1 wib=w（i+k,:+T.*x（10） x（11） x（12） %初始对准，角速度加上陀螺漂移 wiel=0 we*cos（x（5） we*sin（x（5） %wiel=Rel

12、*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=0 wlb（3） -wlb（2） wlb（1） -wlb（3） 0 wlb（1） wlb（2） wlb（2） -wlb（1） 0 wlb（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+