ekf.m

来自「飞行器跟踪-EKF方法,仿真效果比较好」· M 代码 · 共 55 行

clear all;
clc;
det=1;
q=1e-5;
r=0.1;
PNC=0.01;
A=[1 0 det 0 ;
   0 1 0 det ;
   0 0 1 0   ;
   0 0 0 1 ]; 
Q=[0 0 0 0 ;
    0 0 0 0 ;
    0 0 1e-4 0 ;
    0 0 0 1e-4];
C=[r 0;
    0 PNC];
M=[100 0 0 0;
      0 100 0 0;
      0  0 100 0;
      0  0 0 100];
x(:,1)=[10,-5,-0.2,0.2];
H=[];
T=100;
uv1=0.001;
uv2=0.001;
s(:,1)=[3 -5 0 0];
for i=1:T
    x(:,i+1)=A*x(:,i)+[0 0 uv1*randn(1) uv2*randn(1)]';
   Y(:,i)=[sqrt(x(1,i)^2+x(2,i)^2);atan(x(2,i)/x(1,i))]+[sqrt(0.1)*randn(1);sqrt(0.01)*randn(1)];
   
    s(:,i+1)=A*s(:,i);
    
    a=1/sqrt(s(1)^2+s(2)^2);
    H=[s(1,i)*a       s(2,i)*a  0  0;
       -s(2,i)*a^2    s(1,i)*a^2 0 0];
   M1=A*M*A'+Q;
   K=M1*H'*inv(C+H*M1*H');
   s(:,i+1)=s(:,i+1)+K*(Y(:,i)-[sqrt(s(1,i+1)^2+s(2,i+1)^2);atan(s(2,i+1)/s(1,i+1))]);
   M=M1-K*(C+H*M1*H')*K';
  end
  
  figure(1)
  plot(x(1,:),x(2,:),'r',s(1,:),s(2,:),'b')
 % axis([-15 15 -10 20])
  grid on;
  
  figure(2)
  plot(1:T+1,x(1,:)-s(1,:),'r',1:T+1,x(2,:)-s(2,:),'b')
  legend('X error','Y error')
  grid on
  toc