卡尔曼滤波处理天文导航和捷联惯导组合导航程序matlabWord文档格式.docx

1、f27=-w*sin（l0）f31=w*cos（l0）f32=vv（2）f34=1/（rm+h（t）f37=w*cos（l0）*sec（l0）2;f42=-fp（3）f43=fp（2）f44=vv（2）-vv（3）f45=2*w*sin（l0）f46=-（2*w*cos（l0）f47=2*w*cos（l0）*vv（2）2+2*w*sin（l0）*vv（3）f51=fp（3）f53=-fp（1）f54=-2*（w*sin（l0）f55=-vv（3）f56=-vv（2）f57=-（2*w*cos（l0）2）*vv（1）f61=-fp（2）f62=fp（1）f64=2*（w*cos（l0）f65=2

2、*vv（2）f67=-2*vv（1）*w*sin（l0）f75=1/（rn+h（t）f84=sec（l0）f87=vv（1）f96=1;FN=0,f12,f13,0,f15,f21,f23,f24,f27,f31,f32,f34,f37,f42,f43,f44,f45,f46,f47,f51,f53,f54,f55,f56,f57,f61,f62,f64,f65,f67,f75,f84,f87,f96,0;FS=-T,zeros（3,3）3）,T;FM=zeros（6,6）G=T,9）T,zeros（9,eye（9,9%15*15噪声W=wex,wey,wez,wdx,wdy,wdz,zero

3、s（1,%we,wd分别为陀螺加速度计的随机误差。%F=FN,FS;zeros（6,FM;%15*15状态转移阵PHI=eye（15,15）+F*Tc;%15*15状态转移阵的离散化TA=Tc*（eye（15,+*F*Tc）*G;%15*15噪声驱动阵H=eye（3,12）eye（3,%量测矩阵M=0,cos（yaw）-cos（yy）*sin（yaw）sin（yaw）cos（yy）*cos（yaw）-1,%M转换阵实现由姿态偏差到平台误差的转换%设置好循环周期进行滤波ifmod（t,mc）=0%由于CNS与SINS周期有差异，故进行定期滤波。20倍的关系。dangle=yy-angle2（1,

4、tc）*pi/180;yaw-angle2（2,roll-angle2（3,*pi/180%姿态偏差0dfai=M*dangle;%平台误差角dp=l0-R1（1,a0-R1（2,h（t）-R1（3,%位置偏差经纬高%量测噪声dz=star*randn（1）star*randn（1）star1*randn（1）star2*randn（1）%观测量Z=dfai;dp+dz;%量测方程%五个滤波公式XK=PHI*X;%滤波公式（1）PK=PHI*P*PHI+TA*QF*TA%滤波公式（2）KK=PK*H*inv（H*PK*H+RF）%滤波公式（3）X=XK+KK*（Z-H*XK）%滤波公式（4）P

5、=（eye（15,-KK*H）*PK;%滤波公式（5）tc=tc+1;%else%X=PHI*X;dfai2=inv（M）*X（1）X（2）X（3）%滤波后平台误差角到姿态的转换。yyy=yy-dfai2（1）%姿态修正yaww=yaw-dfai2（2）rolll=roll-dfai2（3）l00=l0-X（7）%X8代表纬度，X7代表经度，l0为经度%位置修正a00=a0-X（8）%滤波后vv1=vv-X（4）X（5）X（6）%速度修正cv1=cv1,vv1;%标准cv=cv,Vet_tt（:t）cv2=cv2,cvv（:%解算l5=l5,l00;a5=a5,a00;theta5=theta

6、5,yyy;%俯仰psi5=psi5,yaww;%偏航gamma5=gamma5,rolll;%滚转theta4=theta4,theta2（t）psi4=psi4,psi2（t）gamma4=gamma4,gamma2（t）l4=l4,l2（t）a4=a4,a2（t）l6=l6,lambdaa（t）a6=a6,phii（t）theta6=theta6,thetaa（t）psi6=psi6,psii（t）gamma6=gamma6,gammaa（t）endt=t+1;%part3;Untitled3%part4t=0:0.4:400-0.4;figure;plot（t,cv（1,:-cv1（1,gridon;xlabel（时间/秒ylabel（东向速度误差米/秒cv（2,-cv1（2,北向速度误差米/秒cv（3,-cv1（3,天向速度误差米/秒%原始-cv2（1,-cv2（2,-cv2（3,