卡尔曼滤波处理天文导航和捷联惯导组合导航程序matlabWord文档格式.docx
《卡尔曼滤波处理天文导航和捷联惯导组合导航程序matlabWord文档格式.docx》由会员分享，可在线阅读，更多相关《卡尔曼滤波处理天文导航和捷联惯导组合导航程序matlabWord文档格式.docx（7页珍藏版）》请在冰豆网上搜索。
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*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,
zeros（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转换阵实现由姿态偏差到平台误差的转换
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%设置好循环周期进行滤波
if
mod（t,
mc）
==0%由于CNS与SINS周期有差异，故进行定期滤波。
20倍的关系。
dangle=[yy-angle2（1,
tc）
*pi/180;
yaw-angle2（2,
roll-angle2（3,
*pi/180]
%姿态偏差
0]
dfai=M*dangle;
%平台误差角
dp=[l0-R1（1,
a0-R1（2,
h（t）
-R1（3,
%位置偏差经纬高
%量测噪声
dz=[star*randn
（1）
star*randn
（1）
star1*randn
（1）
star
2*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=（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=[theta5,
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）
end
t=t+1;
%part3;
Untitled3
%part4
t=0:
0.4:
400-0.4;
figure;
plot（t,
cv（1,
:
-cv1（1,
grid
on;
xlabel（'
时间/秒'
ylabel（'
东向速度误差米/秒'
cv（2,
-cv1（2,
北向速度误差米/秒'
cv（3,
-cv1（3,
天向速度误差米/秒'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%原始
-cv2（1,
-cv2（2,
-cv2（3,
升级会员 