runme.m

一个关于卫星导航方面的ekf滤波程序

%   EKF ----- Driving Code
%   Initialize, Generate, Plot
%   (c) Shi Heng (2007)

clc;
clear;
% warning off MATLAB:SingularMatrix;

epher      %----- 导入星历 -----

R = input('输入计算次数后按回车: 大于0的整数.计算R次估计的平均性能.一般就选 1\n');
N = input('输入数据点数后按回车: 建议20-50的整数\n');                                                % 数据点数

  disp(' ');
  disp('---------- EKF ----------');
  disp(' ');

  %----- 模型初始化 -----    

  X  = zeros(6,N);                           % 状态值数组
  z  = zeros(6,N);                           % 观测值数组

  pNoise.dim = 6;                      % 系统噪声
  pNoise.mu = zeros(6,1);              % 噪声均值                  
  pNoise.cov  = (0.005^2)*eye(6);      % 噪声协方差 E0.005 U posi~0.01              
  pnoise = chol(pNoise.cov)'*randn(6,N);% + cvecrep(pNoise.mu,N);                       
  
  oNoise.dim = 6;                      % 观测噪声
  oNoise.mu = zeros(6,1);              % 噪声均值                    
  oNoise.cov  = (0.005^2)*eye(6);      % 噪声协方差 E0.01 U smooth~0.28        
  onoise = chol(oNoise.cov)'*randn(6,N);% + cvecrep(oNoise.mu,N);  
 
  
  for k=1:R             %------------ 按计算次数循环计算 ------------
       
    X(:,1) = (probe(52,:))';                                 % ------------状态初值
    z(:,1) = feval(@hfun, X(:,1), onoise(:,1));    
        
    for j=2:N,
      X(:,j) = feval(@ffun, X(:,j-1), pnoise(:,j-1));
      z(:,j) = feval(@hfun, X(:,j), onoise(:,j));
    end

    Xh = zeros(6,N);          
    Xh(:,1) = (probe(52,:))';               % --------------滤波初值
    Px = 3/4*eye(6);          

    [Xh, Px] = ekf(Xh(:,1), Px, pNoise, oNoise, z);   
      
    %--- Plot -----      
      figure(1); clf;
      p1 = plot(X(1,:),'k:'); hold on
      p2 = plot(probe(52:N+51,1),'k+');
      p3 = plot(Xh(1,:),'k'); hold off;
      legend([p1 p2 p3],'理论预测值','带有噪声的观测值','EKF估计值',0);
      xlabel('step');
      ylabel('km');
      title(['基于EKF的导航仿真结果－－X向位移']);

      figure(2); clf;
      p1 = plot(X(2,:),'k:'); hold on
      p2 = plot(probe(52:N+51,2),'k+');
      p3 = plot(Xh(2,:),'k'); hold off;
      legend([p1 p2 p3],'理论预测值','带有噪声的观测值', 'EKF估计值',0);
      xlabel('step');
      ylabel('km');
      title(['基于EKF的导航仿真结果－－Y向位移']);
      
      figure(3); clf;
      p1 = plot(X(3,:),'k:'); hold on
      p2 = plot(probe(52:N+51,3),'k+');
      p3 = plot(Xh(3,:),'k'); hold off;
      legend([p1 p2 p3],'理论预测值','带有噪声的观测值','EKF估计值',0);
      xlabel('step');
      ylabel('km');
      title(['基于EKF的导航仿真结果－－Z向位移']);
      
      figure(4); clf;
      p1 = plot(X(4,:),'k:'); hold on
      p2 = plot(probe(52:N+51,4),'k+');
      p3 = plot(Xh(4,:),'k'); hold off;
      legend([p1 p2 p3],'理论预测值','带有噪声的观测值','EKF估计值',0);
      xlabel('step');
      ylabel('km/s');
      title(['基于EKF的导航仿真结果－－X向速度']);
      
      figure(5); clf;
      p1 = plot(X(5,:),'k:'); hold on
      p2 = plot(probe(52:N+51,5),'k+');
      p3 = plot(Xh(5,:),'k'); hold off;
      legend([p1 p2 p3],'理论预测值','带有噪声的观测值','EKF估计值',0);
      xlabel('step');
      ylabel('km/s');
      title(['基于EKF的导航仿真结果－－Y向速度']);
      
      figure(6); clf;
      p1 = plot(X(6,:),'k:'); hold on
      p2 = plot(probe(52:N+51,6),'k+');
      p3 = plot(Xh(6,:),'k'); hold off;
      legend([p1 p2 p3],'理论预测值','带有噪声的观测值','EKF估计值',0);
      xlabel('step');
      ylabel('km/s');
      title(['基于EKF的导航仿真结果－－Z向速度']);
    
figure(7);
p1=plot3(X(1,:),X(2,:),X(3,:),'k+');hold on
p2=plot3(Xh(1,:),Xh(2,:),Xh(3,:),'k');hold off;
legend([p1 p2],'理论预测值','EKF估计值',0);
grid on;
view([1 -1 -0.5]);
title(['EKF导航仿真结果三维空间示意']);
xlabel('x/km'),ylabel('y/km'),zlabel('z/km');

for i=1:N
Xer(1,i)=Xh(1,i)-X(1,i);
Xer(2,i)=Xh(2,i)-X(2,i);
Xer(3,i)=Xh(3,i)-X(3,i);
Xer(4,i)=Xh(4,i)-X(4,i);
Xer(5,i)=Xh(5,i)-X(5,i);
Xer(6,i)=Xh(6,i)-X(6,i);
end
figure(8); clf;
      p1 = plot(Xer(1,:),'-.'); hold on
      p2 = plot(Xer(2,:),'--'); 
      p3 = plot(Xer(3,:),':'); hold off;
      legend([p1 p2 p3],'X向位移估计误差','Y向位移估计误差','Z向位移估计误差',0);
      xlabel('step');  
      ylabel('km');
      title(['基于EKF的导航仿真结果－－位移估计误差']);

figure(9); clf; 
      p4 = plot(Xer(4,:),'-.'); hold on
      p5 = plot(Xer(5,:),'--'); 
      p6 = plot(Xer(6,:),':'); hold off;
      legend([p4 p5 p6],'X向速度估计误差','Y向速度估计误差','Z向速度估计误差',0);
      xlabel('step');   
      ylabel('km/s');
      title(['基于EKF的导航仿真结果－－速度估计误差']);
      
    drawnow

    %--- Calculate error -----
    rmse(1,k) = sqrt(mean((Xh(1,2:end)-X(1,2:end)).^2));
    rmse(2,k) = sqrt(mean((Xh(2,2:end)-X(2,2:end)).^2));
    rmse(3,k) = sqrt(mean((Xh(3,2:end)-X(3,2:end)).^2));
    rmse(4,k) = sqrt(mean((Xh(4,2:end)-X(4,2:end)).^2));
    rmse(5,k) = sqrt(mean((Xh(5,2:end)-X(5,2:end)).^2));
    rmse(6,k) = sqrt(mean((Xh(6,2:end)-X(6,2:end)).^2));

  end
  
  for i=1:6
    mean_RMSE(i) = mean(rmse(i,k));
  end
  disp(' ');
  disp(['-----本次估计平均均方根误差: ' num2str(mean_RMSE)]);