lmekf.m

ekf的程序

%目标以匀速直线运动
clc
clear
N=70;  %观测次数
T=1;    %采样间隔
F=[1 0 T 0;  
   0 1 0 T;
   0 0 1 0;
   0 0 0 1];    %状态转移矩阵
v1=normrnd(0,0.01,[1,N]);   %目标在x速度方向上的噪声
v2=normrnd(0,0.05,[1,N]);   %目标在y速度方向上的噪声
Q=[0 0 0     0    ;
   0 0 0     0    ;
   0 0 0.1^2 0    ;
   0 0 0     0.5^2];   %状态噪声协方差
X(:,1)=[100 100 10+v1(1) 10+v2(1)]';      %目标初始状态[x坐标 y坐标 x速度 y速度]'
for k=2:N                    
    X(:,k)=F*X(:,k-1)+[0 0 v1(k) v2(k)]' ;   %状态方程
end  
w=normrnd(0,0.01,[1,N]); %产生的随即观测噪声：方位角
R=0.01^2;        %观测噪声协方差
M(:,1)=[0 0 2 2]';
for k=2:20
    L=[1 0 1 0;
       0 1 0 0;
       0 0 1 0;
       0 0 0 1];
    M(:,k)=L*M(:,k-1);   
end
for k=21:45
     L=[1 0 0 0;
       0 1 0 1;
       0 0 1 0;
       0 0 0 1];
    M(:,k)=L*M(:,k-1);
end
for k=45:70
     L=[1 0 1 0;
       0 1 0 0;
       0 0 1 0;
       0 0 0 1];
    M(:,k)=L*M(:,k-1);
end
for k=1:N
    Zact(k)=atan((X(2,k)-M(2,k))/(X(1,k)-M(1,k)))+w(k);  %实际量测值
end 
SX(:,1)=[100 100 10 10]';    %初始状态估计量
SP=[100 10  2   1;
    10  100 1   2;
    2   1   1   0.5;
    1   2   0.5 1];      %初始协方差
%滤波算法
for k=2:N
    PX=F*SX(:,k-1);  %状态预测
    PZ=atan((PX(2,1)-M(2,k))/(PX(1,1)-M(1,k)));  %量测预测
    Zres(k)=Zact(k)-PZ;  %测量residual(残差)
    PP=F*SP*F'+Q;  %预测协方差
    %求观测函数的雅可比矩阵
    H(1,1)=-(PX(2,1)-M(2,k))/((PX(1,1)-M(1,k))^2+PX(2,1)^2);
    H(1,2)=0;
    H(1,3)=(PX(1,1)-M(2,k))/sqrt((PX(1,1)-M(1,k))^2+PX(2,1)^2);
    H(1,4)=0;
    S(k)=R+H*PP*H';  %残差协方差
    W=PP*H'*inv(S(k));  %滤波增益
    SX(:,k)=PX+W*Zres(k); %状态更新
    SP=PP-W*S(k)*W'; %协方差更新
end
%绘制航迹图
k=1:N;
plot(X(1,:),X(2,:),'r.',SX(1,:),SX(2,:),'b.');
xlabel('x坐标');
ylabel('y坐标');
title('扩展卡尔曼滤波');
legend('真实轨迹','跟踪轨迹');
grid on;