基于目标观测距离的EKF目标跟踪算法

matlab算法

function EKF_For_TargetTracking
clc ;clear;
T=1;
N=60/T;
delta_w=1e-3;
Q=delta_w*diag([0.5,1]);
R=5;
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
G=[0.5*T^2,0;T,0;0,0.5*T^2;0,T];
x=zeros(4,N);
x(:,1)=[-100,2,200,20];
z=zeros(1,N);
x0=200;
y0=300;
xstation=[x0,y0];
%真实轨迹
for t=2:N
    x(:,t)=F*x(:,t-1)+G*sqrtm(Q)*randn(2,1);
end
%对目标进行观测
for t=1:N
    z(t)=Dist(x(:,t),xstation)+sqrtm(R)*randn;
end
%扩展卡尔曼滤波
P0=eye(4);
Xefk=zeros(4,N);
Xefk(:,1)=x(:,1);
In=eye(4);
for t=2:N
    Xn=F*Xefk(:,t-1);             %预测
    Yn=Dist(Xn,xstation);       %观测预测
    P1=F*P0*F'+G*Q*G';          %协方差
    H=[(Xn(1,1)-x0)/Yn,0,(Xn(3,1)-y0)/Yn,0];      %通过雅可比矩阵求H
    K=P1*H'*inv(H*P1*H'+R);     %增益
    Xefk(:,t)=Xn+K*(z(:,t)-Yn); %状态更新
    P0=(In-K*H)*P1;             %协方差更新
end
%误差分析
for i=1:N
    Err_EKF(i)=Dist(x(:,i),Xefk(:,i));
end
    
figure 
hold on; box on;
plot(x(1,:),x(3,:),'-k.')  %真实轨迹
plot(Xefk(1,:),Xefk(3,:),'-b+');
legend('真实轨迹','EKF轨迹');
xlabel('横坐标X/m');
ylabel('纵坐标Y/m');

figure
hold on; box on;
plot(Err_EKF,'-ks','MarkerFaceColor','r')
xlabel('时间/s');
ylabel('位置估计偏差/m');

%求两点间距离的子函数
function d=Dist(X1,X2)
if length(X2)<=2
    d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);
else
     d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
end
 

 图一  EKF对目标跟踪的曲线

 图二  EKF跟踪误差曲线