卡尔曼滤波处理天文导航和捷联惯导组合导航程序matlabWord文档格式.docx
《卡尔曼滤波处理天文导航和捷联惯导组合导航程序matlabWord文档格式.docx》由会员分享,可在线阅读,更多相关《卡尔曼滤波处理天文导航和捷联惯导组合导航程序matlabWord文档格式.docx(7页珍藏版)》请在冰豆网上搜索。
![卡尔曼滤波处理天文导航和捷联惯导组合导航程序matlabWord文档格式.docx](https://file1.bdocx.com/fileroot1/2022-10/4/97338956-5609-489a-8263-35c580582da9/97338956-5609-489a-8263-35c580582da91.gif)
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,