📄 insandgps.m
字号:
%%%%%%%%%%%%%%%%%捷联惯导GPS组合导航Kalman滤波器%%%%%%%%%%%%%%%%
clc;
clear;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Wie=7.292115e-5; %地球自转角速
e=1/298.257; %地球扁律
Re=6378137; %地球半径
STEP=0.1; %惯导解算周期
G0=9.7803; %重力加速度初值
arc=pi/180; %角度转换
T=0.3; %滤波周期，单位秒
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load y_Ishuju;
load y_Gshuju;
S_dat_I=y_I;
S_dat_G=y_G;
%%%%%%%%%%%%%%%%%%%%数据加载%%%%%%%%%%%%%%%%%%%%%%%%%%%
I=eye(13);
A=zeros(13);
X00=zeros(13,1);
X00(1,1)=(10/60)*arc;
X00(2,1)=(10/60)*arc;
X00(3,1)=(10/60)*arc;
X00(4,1)=10;
X00(5,1)=10;
X00(6,1)=(4/60)*arc;
X00(7,1)=(4/60)*arc;
X00(8,1)=(0.5/3600)*arc;
X00(9,1)=(0.5/3600)*arc;
X00(10,1)=(0.5/3600)*arc;
X00(11,1)=(1e-4)*9.81;
X00(12,1)=(1e-4)*9.81;
X00(13,1)=(1e-4)*9.81;
P00=zeros(13); %Klman滤波中估计误差方差阵初值
P00(1,1)=((10/60)*arc)*((10/60)*arc);
P00(2,2)=((10/60)*arc)*((10/60)*arc);
P00(3,3)=((10/60)*arc)*((10/60)*arc);
P00(4,4)=10*10;
P00(5,5)=10*10;
P00(6,6)=((4/60)*arc)*((4/60)*arc);
P00(7,7)=((4/60)*arc)*((4/60)*arc);
P00(8,8)=((0.5/3600)*arc)*((0.5/3600)*arc);
P00(9,9)=((0.5/3600)*arc)*((0.5/3600)*arc);
P00(10,10)=((0.5/3600)*arc)*((0.5/3600)*arc);
P00(11,11)=((1e-4)*9.81)*((1e-4)*9.81);
P00(12,12)=((1e-4)*9.81)*((1e-4)*9.81);
P00(13,13)=((1e-4)*9.81)*((1e-4)*9.81);
Q0=zeros(13); %Klman滤波中过程噪声方差阵初值
Q0(1,1)=((1/3)*((0.5/3600)*arc))*((1/3)*((0.5/3600)*arc));
Q0(2,2)=((1/3)*((0.5/3600)*arc))*((1/3)*((0.5/3600)*arc));
Q0(3,3)=((1/3)*((0.5/3600)*arc))*((1/3)*((0.5/3600)*arc));
Q0(4,4)=((1/3)*(1e-4)*9.81)*((1/3)*(1e-4)*9.81);
Q0(5,5)=((1/3)*(1e-4)*9.81)*((1/3)*(1e-4)*9.81);
R1=zeros(4); %Klman滤波中观测噪声方差阵初值
R1(1,1)=(0.3)^2;
R1(2,2)=(0.3)^2;
R1(3,3)=(5/Re)^2;
R1(4,4)=(5/Re)^2;
H1=zeros(4,13); %Klman滤波中观测阵初值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%滤波开始%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mmm=round(T/STEP);
for j=0:1:length(S_dat_I)/mmm-2
Sum=0;
for DRDER=(j*mmm+1):1:(j+1)*mmm
vx_G=S_dat_G(DRDER,1);
vy_G=S_dat_G(DRDER,2);
vz_G=S_dat_G(DRDER,3);
jd_G=S_dat_G(DRDER,4);
wd_G=S_dat_G(DRDER,5);
Vx_I=S_dat_I(DRDER,1);
Vy_I=S_dat_I(DRDER,2);
Vz_I=S_dat_I(DRDER,3);
Q=S_dat_I(DRDER,4);
R=S_dat_I(DRDER,5);
W=S_dat_I(DRDER,6);
JD=S_dat_I(DRDER,7);
WD=S_dat_I(DRDER,8);
H=S_dat_I(DRDER,9);
fx_n=S_dat_I(DRDER,10);
fy_n=S_dat_I(DRDER,11);
fz_n=S_dat_I(DRDER,12);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
q000=cos(Q/2)*cos(R/2)*cos(W/2)-sin(Q/2)*sin(R/2)*sin(W/2); %惯导四元数初值
q100=sin(Q/2)*cos(R/2)*cos(W/2)-cos(Q/2)*sin(R/2)*sin(W/2);
q200=cos(Q/2)*cos(R/2)*sin(W/2)+sin(Q/2)*sin(R/2)*cos(W/2);
q300=cos(Q/2)*sin(R/2)*cos(W/2)+sin(Q/2)*cos(R/2)*sin(W/2);
Four_n(1)=q000/sqrt(q000*q000+q100*q100+q200*q200+q300*q300);
Four_n(2)=q100/sqrt(q000*q000+q100*q100+q200*q200+q300*q300);
Four_n(3)=q200/sqrt(q000*q000+q100*q100+q200*q200+q300*q300);
Four_n(4)=q300/sqrt(q000*q000+q100*q100+q200*q200+q300*q300);
Nq0=Four_n(1);
Nq1=Four_n(2);
Nq2=Four_n(3);
Nq3=Four_n(4);
Cb_n(1,1)=1-2*(Nq2*Nq2+Nq3*Nq3);
Cb_n(1,2)=2*(Nq1*Nq2-Nq0*Nq3);
Cb_n(1,3)=2*(Nq1*Nq3+Nq0*Nq2);
Cb_n(2,1)=2*(Nq1*Nq2+Nq0*Nq3);
Cb_n(2,2)=1-2*(Nq1*Nq1+Nq3*Nq3);
Cb_n(2,3)=2*(Nq2*Nq3-Nq0*Nq1);
Cb_n(3,1)=2*(Nq1*Nq3-Nq0*Nq2);
Cb_n(3,2)=2*(Nq1*Nq3+Nq0*Nq1);
Cb_n(3,3)=1-2*(Nq1*Nq1+Nq2*Nq2);
Cn_b=Cb_n';
%
Rm=Re/(1+2*e-3*e*sin(WD)*sin(WD));
Rn=Re/(1-e*sin(WD)*sin(WD));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A(1,2)=Wie*sin(WD)+Vx_I*tan(WD)/(Rn+H);
A(1,3)=-(Wie*cos(WD)+Vx_I/(Rn+H));
A(1,5)=-1/(Rn+H);
A(1,8)=Cb_n(1,1);
A(1,9)=Cb_n(1,2);
A(1,10)=Cb_n(1,3);
A(2,1)=-(Wie*sin(WD)+Vx_I*tan(WD)/(Rn+H));
A(2,3)=-Vy_I/(Rm+H);
A(2,4)=1/(Rn+H);
A(2,6)=-Wie*sin(WD);
A(2,8)=Cb_n(2,1);
A(2,9)=Cb_n(2,2);
A(2,10)=Cb_n(2,3);
A(3,1)=Wie*cos(WD)+Vx_I/(Rn+H);
A(3,2)=Vy_I/(Rm+H);
A(3,4)=tan(WD)/(Rn+H);
A(3,6)=Wie*cos(WD)+Vx_I/((Rn+H)*cos(WD)*cos(WD));
A(3,8)=Cb_n(3,1);
A(3,9)=Cb_n(3,2);
A(3,10)=Cb_n(3,3);
A(4,2)=-fz_n;
A(4,3)=fy_n;
A(4,4)=Vy_I*tan(WD)/(Rm+H)-Vz_I/(Rm+H);
A(4,5)=Vx_I*tan(WD)/(Rn+H)+2*Wie*sin(WD);
A(4,6)=2*Wie*Vy_I*cos(WD)+Vx_I*Vy_I/((Rn+H)*cos(WD)*cos(WD))+2*Wie*Vz_I*sin(WD);
A(4,11)=Cb_n(1,1);
A(4,12)=Cb_n(1,2);
A(4,13)=Cb_n(1,3);
A(5,1)=fz_n;
A(5,3)=-fx_n;
A(5,4)=-2*(Vx_I*tan(WD)/(Rn+H)+Wie*sin(WD));
A(5,5)=-Vz_I/(Rm+H);
A(5,6)=-(2*Wie*cos(WD)+Vx_I/((Rn+H)*cos(WD)*cos(WD)))*Vx_I;
A(5,11)=Cb_n(2,1);
A(5,12)=Cb_n(2,2);
A(5,13)=Cb_n(2,3);
A(6,5)=1/(Rm+H);
A(7,4)=1/((Rn+H)*cos(WD));
A(7,6)=Vx_I*tan(WD)/((Rn+H)*cos(WD));
Sum=Sum+A;
end
%%%%%%%%%%%%%%%%%%%%%%%构造H矩阵%%%%%%%%%%%%%%%%%%%%%%
H1(1,4)=1;
H1(2,5)=1;
H1(3,7)=1;
H1(4,6)=1;
%
Z1=zeros(4,1);
Z1(1,1)=S_dat_I((j+1)*mmm+1,1)-S_dat_G((j+1)*mmm+1,1);
Z1(2,1)=S_dat_I((j+1)*mmm+1,2)-S_dat_G((j+1)*mmm+1,2);
Z1(3,1)=S_dat_I((j+1)*mmm+1,7)-S_dat_G((j+1)*mmm+1,4);
Z1(4,1)=S_dat_I((j+1)*mmm+1,8)-S_dat_G((j+1)*mmm+1,5);
%%%%%%%%%%%%%%%%%%%%%%Standard Kalman Filter%%%%%%%%%%%%%%%%%%%%%%%%
Fai10=I+STEP*Sum;
X10=Fai10*X00;
P10=Fai10*P00*Fai10'+Q0;
K1=P10*H1'*inv(H1*P10*H1'+R1);
X11=X10+K1*(Z1-H1*X10);
P11=(I-K1*H1)*P10;
%
XX(j+1,:)=X11';
%
Q1=(Q0+Fai10*Q0*Fai10')*T/2;
Q0=Q1;
X00=X11;
P00=P11;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
figure(1)
subplot(311)
plot(XX(:,1)/arc*60,'r');
title ('姿态失准角估计')
xlabel('t/s');
ylabel('\phix，角分');
grid on
%
subplot(312)
plot(XX(:,2)/arc*60,'r');
xlabel('t/s');
ylabel('\phiy，角分');
grid on
%
subplot(313)
plot(XX(:,3)/arc*60,'r');
xlabel('t/s');
ylabel('\phiz，角分');
grid on
%
figure(2)
subplot(311)
plot(XX(:,4),'r');
title ('速度误差估计')
xlabel('t/s');
ylabel('\DeltaVx，m/s');
grid on
%
subplot(312)
plot(XX(:,5),'r');
xlabel('t/s');
ylabel('\DeltaVy，m/s');
grid on
%
figure(3)
subplot(311)
plot(XX(:,6)/arc*60,'r');
title ('位置误差估计')
xlabel('t/s');
ylabel('\DeltaL，角分');
grid on
%
subplot(312)
plot(XX(:,7)/arc*60,'r');
xlabel('t/s');
ylabel('\Delta\lambda，角分');
grid on
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -