ekf_lvlh.m

自己写的matlab的EKF算法

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%                  输入各种数据                   %
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
clear all
clc
mu=3.986e5;
n=(mu/(7178.14^3))^(0.5);       %中心星角速度,单位是rad 
L=1441;
Tspan=[0 60];                     %STK仿真真实轨道所用的时间间隔，也是求解系统方程的求解步长

randn('state',rand);
Rk=[(1e-7)^2 0 0
    0 (1e-7)^2 0
    0 0 (1e-7)^2];                      %测量方差，相对距离测量值的误差为0.01m，方位角、俯仰角测量值的误差为0.01°
Vk1=normrnd(0,1e-7,[1,L]);
Vk2=normrnd(0,1e-7,[1,L]);
Vk=[Vk1
    Vk2
    Vk2];                                %生成测量误差矩阵

Qk=[1e-12 0 0 0 0 0
    0 1e-12 0 0 0 0
    0 0 1e-12 0 0 0
    0 0 0 1e-12 0 0
    0 0 0 0 1e-12 0
    0 0 0 0 0 1e-12];                    %系统方差
Wk=normrnd(0,1e-6,[6,L]);                %生成系统误差矩阵 
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%                  产生信号                   %  
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%
load lvlh.dat;              %加载真实轨道数据
for i=1:L   
            x=lvlh(i,1);                  %X轴方向数据，单位是km
            y=lvlh(i,2);                  %Y轴方向数据，单位是km
            z=lvlh(i,3);                  %Z轴方向数据，单位是km
        
            zz(:,i)=[x
                     y
                     z];
                               
         Zk(:,i)=zz(:,i)+Vk(:,i);                 %生成测量方程 
end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%            卡尔曼过滤                       %
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Pk=[0.01^2 0 0 0 0 0
    0 0.01^2 0 0 0 0
    0 0 0.01^2 0 0 0
    0 0 0 0.0001^2 0 0
    0 0 0 0 0.0001^2 0
    0 0 0 0 0 0.0001^2];                    %初值P(0/0)

Xk=[lvlh(1,1)+0.01
    lvlh(1,2)+0.01                           
    lvlh(1,3)+0.01
    lvlh(1,4)+0.0001
    lvlh(1,5)+0.0001
    lvlh(1,6)+0.0001];                             %初值X0

        a11=[4-3*cos(n*60) 0  0
             6*(sin(n*60)-n*60) 1 0  
             0 0 cos(n*60)];
        a12=[sin(n*60)/n  2*(1-cos(n*60))/n 0 
             2*(cos(n*60)-1)/n 4*sin(n*60)/n-3*60 0
             0 0 sin(n*60)/n];
        a21=[3*n*sin(n*60) 0 0
             6*n*(cos(n*60)-1) 0 0
             0 0 -n*sin(n*60)];
        a22=[cos(n*60) 2*sin(n*60) 0
             -2*sin(n*60) 4*cos(n*1*60)-3 0
             0 0 cos(n*60)];
     A=[a11 a12
        a21 a22];   
        
H=[1 0 0 0 0 0
   0 1 0 0 0 0
   0 0 1 0 0 0];
       
for t=1:L  
    
     Pkk=A*Pk*A+Qk;
     Xkk=A*Xk;
  
     Kk=Pkk*H'*inv(H*Pkk*H'+Rk);
     Pk=(eye(6)-Kk*H)*Pkk;
    
     Xk=Xkk+Kk*(Zk(:,t)-H*Xkk);
 
    dX(:,t)=1000*(Xk-lvlh(t,:)');
end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%                 图形输出                    %
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
c=1:L;
subplot(3,1,1);
plot(c,dX(1,c));
grid

subplot(3,1,2);
plot(c,dX(2,c));
grid

subplot(3,1,3);
plot(c,dX(3,c));
grid

figure;
subplot(3,1,1);
plot(c,dX(4,c));
grid

subplot(3,1,2);
plot(c,dX(5,c));
grid

subplot(3,1,3);
plot(c,dX(6,c));
grid
% 
% %%%%%%%%%%%%%% the end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
%