fusion.m

基于kalman滤波的信息融合程序

%（导入GPS526）
% get longitude input(提取经度)
for i=1:558
 lon(i)=GPS526(i,3);
end	

%get latitude input(提取纬度)
for i=1:558
 lat(i)=GPS526(i,2);
end

%get speed input(提取速度)
for i=1:558
 speed(i)=GPS526(i,5);
end

%get angular velocity input(提取角度)
for i=1:558
 wgps(i)=mod(90-GPS526(i,6),360);
end

%for i=1:558
% sat(i)=GPS526(i,7);
%end

%（导入INS526）
%get accelerated speed ax (提取加速度ax)
for i=1:47992
 a(i)=INS526 (i,2);
end	

%get angular velocity input wz(提取角速度wz)
for i=1:47992
 w(i)=INS526 (i,1);
end

%get time interval Δt((提取时间间隔Δt)
for i=1:47992
 t(i)=INS526 (i,3);
end

%get time tip((提取时间标号)
for i=1:47992
 sag(i)=INS526 (i,4);
end

%coordinate conversion for gps data (gps坐标转换 BL to XY)
c0=111134.0047;
c1=32009.8185306;
c2=133.9598897;
c3=0.6975483;
c4=0.0039431;
a1=6378137;
b=6356752.3141586055;
e=sqrt(a1*a1-b*b)/a1;
ee2=(a1*a1-b*b)/(b*b);
for i=1:558
 tlat=lat(i)*pi/180;
 tlon=(lon(i)- 115.756)*pi/180;    %中央子午线经度115.756
 s=sin(tlat);
 c=cos(tlat);
x0=c0*tlat*180/pi -c*(c1*s+c2*(s^3)+c3*(s^5)+ c4*(s^7));
t0=tan(tlat);
N=a1/(sqrt(1-(e^2)*( s^2)));
ita2=(c^2)*ee2;

xgps(i)=x0+0.5*N*t0*(c^2)*(tlon^2)+(5-t0*t0+9*ita2+4*ita2*ita2)*N*t0*(c^4)*(tlon^4)/24+(61-58*t0*t0+t0^4+270*ita2-330*ita2*t0*t0)*N*t0*(c^6)*(tlon^6)/720;
ygps(i)=N*c*tlon+(1-t0^2+ita2)*N*(c^3)*(tlon^3)/6+(5-18*t0*t0+t0^4+14*ita2*t0*t0)*N*(c^5)*(tlon^5)/120;
end

%设置gps-ins滤波器参数%
T=1;      %设置gps采样周期
c1=1;
a1=3;
a2=3;
a3=200;
a4=100;

b1=100;
b2=100;
b3=2;
b4=2;

Rx=b1^2;
Ry=b2^2;
Rv=b3^2;
Rj=b4^2;

R=[Rx 0 0 0
   0 Ry 0 0
   0 0 Rv 0
   0 0 0 Rj];
Qx=[T^5/20 T^4/8 T^3/6
   T^4/8 T^3/3 T^2/2
   T^3/6 T^2/2 T]*2*a1^2/c1;
Qy=[T^5/20 T^4/8 T^3/6
   T^4/8 T^3/3 T^2/2
   T^3/6 T^2/2 T]*2*a2^2/c1;
Qv=[T^5/20 T^4/8 T^3/6
   T^4/8 T^3/3 T^2/2
   T^3/6 T^2/2 T]*2*a3^2/c1;
Qj=[T^5/20 T^4/8 T^3/6
   T^4/8 T^3/3 T^2/2
   T^3/6 T^2/2 T]*2*a4^2/c1;
tA1=[1 T T^2/2
     0 1 T
     0 0 1];
tA=[1 T c1^2*(T/c1-1+exp(-T/c1))
    0 1 c1*(1-exp(-T/c1))
    0 0 exp(-T/c1)];
Q=[Qx zeros(3,3) zeros(3,3) zeros(3,3)
    zeros(3,3) Qy zeros(3,3)  zeros(3,3)
    zeros(3,3) zeros(3,3) Qv  zeros(3,3)
    zeros(3,3) zeros(3,3) zeros(3,3) Qj];
A=[tA zeros(3,3) zeros(3,3) zeros(3,3) 
    zeros(3,3) tA zeros(3,3) zeros(3,3) 
    zeros(3,3) zeros(3,3) tA zeros(3,3)
    zeros(3,3) zeros(3,3) zeros(3,3) tA];
A1=[tA1 zeros(3,3) zeros(3,3) zeros(3,3)
    zeros(3,3) tA1 zeros(3,3) zeros(3,3)
    zeros(3,3) zeros(3,3) tA1 zeros(3,3)
    zeros(3,3) zeros(3,3) zeros(3,3) tA1];
H=[1 0 0 0 0 0 0 0 0 0 0 0
   0 0 0 1 0 0 0 0 0 0 0 0
   0 0 0 0 0 0 1 0 0 0 0 0
   0 0 0 0 0 0 0 0 0 1 0 0]; 

%initialization（初始化）
v=zeros(47992,1);
ang=zeros(47992,1);
d=zeros(47992,1);  
xins=zeros(47992,1);
yins=zeros(47992,1);


v(1)=speed(1);                  %ins初始速度校准
yins(1)= ygps(1);               %ins初始位置校准
xins(1)= xgps(1);
ang(1)=mod(90-wgps(1),360);     %ins初始角度对准    

z=1;                            %gps初始点
cor=5;                          %ins对准标志位
P0=Q;
X0=[0 0 0 0 0 0 0 0 0 0 0 0]';
mil(1)=0;

%fusion and navigation（融合与导航）
for i=2:47992
 %speed
 v(i)=v(i-1)+a(i-1)*t(i)+0.5*(a(i)-a(i-1))*t(i);
 %angle
 ang(i)=mod(ang(i-1)-w(i-1)*t(i),360);
 
 D=(v(i)-a(i-1)*t(i))*t(i)+0.5*a(i-1)*t(i)*t(i);
 XX= xins(i-1)+D*cos(ang(i)*pi/180);
 YY= yins(i-1)+D*sin(ang(i)*pi/180);  
 
 
 if sag(i)==cor               %ins与gps的对准提取，以标志位为准，保证两种数据的一致
    %if lat(z)~=0 & lon(z)~=0    %当gps失效时不进行融合
     ex=xgps(z)-XX;      %东向位置，北向位置，速度，角度量测误差值
     ey=ygps(z)-YY; 
     ev=speed(z)-v(i);
     ej=mod(90-wgps(z),360)-ang(i);
     Z=[ex;ey;ev;ej];         %量测矢量
     %angerror(z)=ej;
     %angerror(z)=ev;
    
    %计算滤波
     K=P0*H'*(inv(H*P0*H'+R));
     
     X=X0+K*(Z-H*X0);
     P=(eye(12)-K*H)*P0;

    errorx(z)=X(1);
    errory(z)=X(4);

     
    %状态量修正
    % if sat(z)>3
       v(i)=v(i)+X(7);          %速度状态量修正，考虑卫星数改变权值
       %else
       %v(i)=(v(i)+X(7))*0.8+v(i)*0.2;
    % end
     ang(i)=ang(i)+X(10);

    
    %一步预测值
     X0=A1*X;
     P0=A*P*A'+Q;
        
 %distance interval（此处的d(i)是距离的间隔，不是距离）
 d(i)=(v(i)-a(i-1)*t(i))*t(i)+0.5*a(i-1)*t(i)*t(i);
 
 %mileage acc（里程）
 mil(i)=d(i)+mil(i-1);
 
 %position calculation
 xins(i)= xins(i-1)+d(i)*cos(ang(i)*pi/180);
 yins(i)= yins(i-1)+d(i)*sin(ang(i)*pi/180);  
 
     xins(i)=xins(i)+X(1);
     yins(i)=yins(i)+X(4);
   
  %对准标志位修正
  z=z+1;
  cor=cor+1;
  
else
   
 %distance interval（此处的d(i)是距离的间隔，不是距离）
 d(i)=(v(i)-a(i-1)*t(i))*t(i)+0.5*a(i-1)*t(i)*t(i);
 
 %mileage acc（里程）
 mil(i)=d(i)+mil(i-1);
 
 %position calculation
 xins(i)= xins(i-1)+d(i)*cos(ang(i)*pi/180);
 yins(i)= yins(i-1)+d(i)*sin(ang(i)*pi/180);  
end
end

%drawing（作图）
plot(yins-4.93e4,xins-4.4239e6,'r.');
grid;
title('普通卡尔曼信息融合结果');
xlabel('高斯投影坐标X（单位：米）');
ylabel('高斯投影坐标Y（单位：米）');

hold on;

for i=1:558
 if lat(i)~=0
  if lon(i)~=0
   plot( ygps(i)-4.93e4, xgps(i)-4.4239e6,'bo');
   hold on;
  end
 end
end

legend( '信息融合结果','GPS单独导航结果',1);