www.pudn.com > INSGPS.rar > INSGPS.m, change:2007-12-13,size:10685b
%%%%%%%%%%%%%%%%%修改后运用新公式的捷联惯导GPS组合导航局部Kalman滤波器%%%%%%%%%%%%%%%%
%%%%%这里轨迹发生器所用的陀螺和加速度计的误差均视为随机常值漂移和零均值正态分布的白噪声的组合体
clc;clear;
Wie=7.292115e-5; %地球自转角速
e=1/298.257; %地球扁律
Re=6378137; %地球半径
step=0.1; %惯导解算周期
g0=9.7803; %重力加速度初值
hudu=pi/180; %角度转换
T=0.3; %滤波周期，单位秒
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load('date\gps');y_GPS=y_G; %y_GPS=GPS;
load('date\ins');y_ins=y_I;%y_ins=INSshuchu;
load('date\guiji');% YY=WCgj;
% y_GPS(end,:)=[];
dat_INS=y_ins;
dat_GPS=y_GPS;
%%%%%%%%%%%%%%%%%%%%数据加载%%%%%%%%%%%%%%%%%%%%%%%%%%%
A=zeros(21);
X00=zeros(21,1);
X00(1,1)=(0.01/60)*hudu; %%初始经纬度误差 4'
X00(2,1)=(0.01/60)*hudu;
X00(3,1)=15; %%初始高度误差 10m
X00(4,1)=5; %%%%初始速度误差10m/s
X00(5,1)=5;
X00(6,1)=5;
X00(7,1)=(20/60)*hudu; %%%初始姿态失准角10'
X00(8,1)=(20/60)*hudu;
X00(9,1)=(20/60)*hudu;
X00(10,1)=(0.1/3600)*hudu; %%%陀螺量测的总的漂移误差 0.05度/h
X00(11,1)=(0.1/3600)*hudu;
X00(12,1)=(0.1/3600)*hudu;
X00(13,1)=(1e-3)*g0; %%%加速度计总误差 1e-4*g0
X00(14,1)=(1e-3)*g0;
X00(15,1)=(1e-3)*g0;
X00(16,1)=(0.05/3600)*hudu; %%%陀螺的马尔科夫飘逸误差初值
X00(17,1)=(0.05/3600)*hudu;
X00(18,1)=(0.05/3600)*hudu;
X00(19,1)=(0.5e-4)*g0; %%%加速度计的马尔科夫飘逸误差初值
X00(20,1)=(0.5e-4)*g0;
X00(21,1)=(0.5e-4)*g0;
P00=zeros(21); %Klman滤波中估计误差方差阵初值
x=X00.^2;
P00=diag(x);
%X00=zeros(21,1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Klman滤波中过程噪声方差阵初值Q0的求取21*21
error_cz=0.01*pi/180/3600;%%%陀螺相关漂移 0.5度/时，
tg=1.5*500; %%%相关时间tg=3000s
kt=sqrt(2*error_cz^2/tg);%%%驱动噪声强度均方根
%%%%加速度计的一阶马尔可夫过程误差模型部分%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ta=0.5*3600;%%相关时间为5小时,用秒表示
Ra=1.0e-3*g0;%%均方根Ra=0.0001*g
ka=sqrt(2*Ra^2/ta);%%%%驱动噪声强度均方根
wz=[0.001/60*pi/180*randn(1) 0.001/60*pi/180*randn(1) 1.0*randn(1)]; %%%%零均值位置噪声均方根向量
sd=0.01*ones(1,3); %%%%%零均值速度噪声均方根向量
zt=0.5*hudu*ones(1,3); %%%%零均值姿态误差噪声均方根向量
fc=[wz sd zt kt kt kt ka ka ka kt kt kt ka ka ka].^2;%%%%%新的方差阵对角线元素
Q0=diag(fc);
Rlamt=0.001*pi/(60*180); %%%经纬度误差均方根,弧度制
Rl=0.001*pi/(60*180);
Rh=2; %%%高度误差均方根,单位 米
Rvx=0.50; %%%速度误差均方根,单位 米/秒
Rvy=0.50;
Rvz=0.50;
%K=[Rlamt Rl Rh Rvx Rvy Rvz];
K=[Rlamt Rl Rh Rvx Rvy Rvz].^2;%%%%该向量平方之后作为误差方差时的滤波 结果不是很好，没有平方前的好
R1=diag(K); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Klman滤波中观测噪声方差阵初值R1 6*6
H1=zeros(6,21); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Klman滤波中观测阵初值H1
h1=eye(6);
h2=zeros(6,15);
H1=[h1 h2];
I=eye(21);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%滤波开始%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mmm=round(T/step); %%%每次滤波过程中惯导解算的次数为mmm
for j=0:1:(fix(length(dat_INS)/mmm)-1) %%%%j为由0~(滤波总次数-2) )
Sum=0; E=0;
for k=fix(j*mmm+1):1:fix((j+1)*mmm) %%%%每个j时进行了由1~mmm共mmm次解算
Vgx=dat_GPS(k,1); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%GPS输出的值
Vgy=dat_GPS(k,2);
Vgz=dat_GPS(k,3);
jingdug=dat_GPS(k,4);
weidug=dat_GPS(k,5);
hg=dat_GPS(k,6);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%INS输出的值
Vx_ins=dat_INS(k,1);
Vy_ins=dat_INS(k,2);
Vz_ins=dat_INS(k,3);
theter=dat_INS(k,4); %%%theter,posi,gama,lamt,L
posi=dat_INS(k,5);
gama=dat_INS(k,6);
lamt_ins=dat_INS(k,7); %%%lamt_ins L_ins h_ins
L_ins=dat_INS(k,8);
h_ins=dat_INS(k,9);
fx_n=dat_INS(k,10); %%%惯导解算时导航系中表示的外力
fy_n=dat_INS(k,11);
fz_n=dat_INS(k,12);
q000=cos(theter/2)*cos(posi/2)*cos(gama/2)-sin(theter/2)*sin(posi/2)*sin(gama/2); %%%%%%惯导四元数初值
q100=sin(theter/2)*cos(posi/2)*cos(gama/2)-cos(theter/2)*sin(posi/2)*sin(gama/2);
q200=cos(theter/2)*cos(posi/2)*sin(gama/2)+sin(theter/2)*sin(posi/2)*cos(gama/2);
q300=sin(theter/2)*cos(posi/2)*sin(gama/2)+cos(theter/2)*sin(posi/2)*cos(gama/2);
N(1)=q000/sqrt(q000*q000+q100*q100+q200*q200+q300*q300); %%%%%%%四元数归一化
N(2)=q100/sqrt(q000*q000+q100*q100+q200*q200+q300*q300);
N(3)=q200/sqrt(q000*q000+q100*q100+q200*q200+q300*q300);
N(4)=q300/sqrt(q000*q000+q100*q100+q200*q200+q300*q300);
Nq0=N(1);
Nq1=N(2);
Nq2=N(3);
Nq3=N(4);
Cb_n(1,1)=1-2*(Nq2*Nq2+Nq3*Nq3); %%%%%%用四元数表示的由本体系到导航系的坐标变换阵 Cb_n
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*(Nq2*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(L_ins)*sin(L_ins));
Rn=Re/(1-e*sin(L_ins)*sin(L_ins));
A(1,2)=Vx_ins*tan(L_ins)/(Rn+h_ins)/cos(L_ins);
A(1,3)=-Vx_ins/(Rn+h_ins)^2/cos(L_ins);
A(1,4)=1/(Rn+h_ins)/cos(L_ins);
A(2,3)=-Vy_ins/(Rm+h_ins)^2;
A(2,5)=1/(Rm+h_ins);
A(3,6)=1;
A(4,2)=2*Wie*Vy_ins*cos(L_ins)+Vx_ins*Vy_ins/((Rn+h_ins)*cos(L_ins)*cos(L_ins))+2*Wie*Vz_ins*sin(L_ins);
A(4,3)=(Vx_ins*Vz_ins-Vx_ins*Vy_ins*tan(L_ins))/(Rn+h_ins)^2;
A(4,4)=(Vy_ins*tan(L_ins)-Vz_ins)/(Rn+h_ins);
A(4,5)=2*Wie*sin(L_ins)+Vx_ins*tan(L_ins)/(Rn+h_ins);
A(4,6)=-(2*Wie*cos(L_ins)+Vx_ins/(Rn+h_ins));
A(4,8)=-fz_n;
A(4,9)=fy_n;
A(4,13)=Cb_n(1,1);
A(4,14)=Cb_n(1,2);
A(4,15)=Cb_n(1,3);
A(5,2)=-2*Wie*cos(L_ins)*Vx_ins-Vx_ins*Vx_ins/((Rn+h_ins)*cos(L_ins)*cos(L_ins));
A(5,3)=Vx_ins^2*tan(L_ins)/(Rn+h_ins)^2+Vy_ins*Vz_ins/(Rm+h_ins)^2;
A(5,4)=-2*(Wie*sin(L_ins)+Vx_ins*tan(L_ins)/(Rn+h_ins));
A(5,5)=-Vz_ins/(Rm+h_ins);
A(5,6)=-Vy_ins/(Rm+h_ins);
A(5,7)=fz_n;
A(5,9)=-fx_n;
A(5,13)=Cb_n(2,1);
A(5,14)=Cb_n(2,2);
A(5,15)=Cb_n(2,3);
A(6,2)=-2*Wie*Vx_ins*sin(L_ins);
A(6,3)=-Vy_ins^2/(Rm+h_ins)^2-Vx_ins^2/(Rn+h_ins)^2;
A(6,4)=2*Wie*cos(L_ins)+2*Vx_ins/(Rn+h_ins);
A(6,5)=2*Vy_ins/(Rm+h_ins);
A(6,7)=-fy_n;
A(6,8)=fx_n;
A(6,13)=Cb_n(3,1);
A(6,14)=Cb_n(3,2);
A(6,15)=Cb_n(3,3);
A(7,3)=Vy_ins/(Rm+h_ins)^2;
A(7,5)=-1/(Rm+h_ins);
A(7,8)=Wie*sin(L_ins)+Vx_ins*tan(L_ins)/(Rn+h_ins);
A(7,9)=-Wie*cos(L_ins)-Vx_ins/(Rn+h_ins);
A(7,10)=Cb_n(1,1);
A(7,11)=Cb_n(1,2);
A(7,12)=Cb_n(1,3);
A(8,2)=-Wie*sin(L_ins);
A(8,3)=-Vx_ins/(Rn+h_ins)^2;
A(8,4)=1/(Rn+h_ins);
A(8,7)=-(Wie*sin(L_ins)+Vx_ins*tan(L_ins)/(Rn+h_ins));
A(8,9)=-Vy_ins/(Rm+h_ins);
A(8,10)=Cb_n(2,1);
A(8,11)=Cb_n(2,2);
A(8,12)=Cb_n(2,3);
A(9,2)=Wie*cos(L_ins)+Vx_ins/(Rn+h_ins)/cos(L_ins)^2;
A(9,3)=-Vx_ins*tan(L_ins)/(Rn+h_ins)^2;
A(9,4)=tan(L_ins)/(Rn+h_ins);
A(9,7)=Wie*cos(L_ins)+Vx_ins/(Rn+h_ins);
A(9,8)=Vy_ins*1/(Rm+h_ins);
A(9,10)=Cb_n(3,1);
A(9,11)=Cb_n(3,2);
A(9,12)=Cb_n(3,3);
A(10,16)=-1/tg; A(11,17)=-1/tg;A(12,18)=-1/tg;
A(13,19)=-1/ta;A(14,20)=-1/ta;A(15,21)=-1/ta;
A(16,16)=-1/tg; A(17,17)=-1/tg;A(18,18)=-1/tg;
A(19,19)=-1/ta;A(20,20)=-1/ta;A(21,21)=-1/ta;
Sum=Sum+A;
end
Z1=zeros(6,1); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%观测阵Z,Z的各行分别同Vx,Vy,Vz的误差值相对应
Z1(1,1)=dat_INS((j+1)*mmm,7)-dat_GPS((j+1)*mmm,1);
Z1(2,1)=dat_INS((j+1)*mmm,8)-dat_GPS((j+1)*mmm,2);
Z1(3,1)=dat_INS((j+1)*mmm,9)-dat_GPS((j+1)*mmm,3);
Z1(4,1)=dat_INS((j+1)*mmm,1)-dat_GPS((j+1)*mmm,4);
Z1(5,1)=dat_INS((j+1)*mmm,2)-dat_GPS((j+1)*mmm,5);
Z1(6,1)=dat_INS((j+1)*mmm,3)-dat_GPS((j+1)*mmm,6);
%%%%%%%%%%%%%%%%%%%%%%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,[1:21])=X11';
Q1=(Q0+Fai10*Q0*Fai10')*T/2; %%循环处理
X_IG(j+1,[1:21])=XX(j+1,[1:21]);
tr_IG(j+1,:)=trace(P11);
trwz=trace(P11([1:3],[1:3]));trsd=trace(P11([4:6],[4:6]));trzt=trace(P11([7:9],[7:9]));
Tr_IG2(j+1,:)=[trwz trsd trzt];
% P_IG2([1:3],[1:3],j+1)=inv(P11([1:3],[1:3]));P_IG2([4:6],[4:6],j+1)=inv(P11([4:6],[4:6]));
% P_IG2([7:9],[7:9],j+1)=inv(P11([7:9],[7:9]));
% P_IG(:,:,j+1)=inv(P11);
Q0=Q1;
X00=X11;
P00=P11;
cishu=j+1;
end
XX_IG=X_IG;
Tr_IG=tr_IG;
save('date\XX_IG','XX_IG');
save('date\Tr_IG','Tr_IG');
save('date\Tr_IG2','Tr_IG2');
% save('P_IG','P_IG');
% save('P_IG2','P_IG2');
%%%%%%%%%%%%%%%%%%%%%图形输出%%%%%%%%%%%%%%%%%%%%
% guiji=WCgj;
% tt=guiji(:,16);
tt=YY(:,16);
t0=tt(1:3:end);
t=t0(1:end-1);
figure(11)
subplot(311)
plot(t,XX(:,7)/hudu*60,'k-');
title ('INS&GPS---姿态失准角估计')
xlabel('t/s');
ylabel('\phix(\prime)');
grid on
subplot(312)
plot(t,XX(:,8)/hudu*60,'k-');
xlabel('t/s');
ylabel('\phiy(\prime)');
grid on
subplot(313)
plot(t,XX(:,9)/hudu*60,'k-');
xlabel('t/s');
ylabel('\phiz(\prime)');
grid on
figure(12)
subplot(311)
plot(t,XX(:,4),'k-');
title ('INS&GPS---速度误差估计')
xlabel('t/s');
ylabel('\DeltaVx(m/s)');
grid on
subplot(312)
plot(t,XX(:,5),'k-');
xlabel('t/s');
ylabel('\DeltaVy(m/s)');
grid on
subplot(313)
plot(t,XX(:,6),'k-');
xlabel('t/s');
ylabel('\DeltaVz(m/s)');
grid on
figure(13)
subplot(311)
plot(t,XX(:,1)/hudu*60,'k-');
title ('INS&GPS---位置误差估计')
xlabel('t/s');
ylabel('\Delta\lambda(\prime)');
grid on
subplot(312)
plot(t,XX(:,2)/hudu*60,'k-');
xlabel('t/s');
ylabel('\DeltaL(\prime)');
grid on
subplot(313)
plot(t,XX(:,3),'k-');
xlabel('t/s');
ylabel('\Deltah(m)');
grid on