扩展卡尔曼滤波理论+仿真

 

matlab仿真代码 (CV+CT运动模型)

clc;
clear;
close all
%***************************初始化参数**************************************
T=1;%采样间隔
S=200 %仿真步数
T_f=1; %sample time of fusion center
w=-pi/180*3;% 转完角速度
A= [1 sin(w*T_f)/w 0 -(1-cos(w*T_f))/w
       0 cos(w*T_f)   0 -sin(w*T_f)
       0 (1-cos(w*T_f))/w 1 sin(w*T_f)/w
       0  sin(w*T_f) 0 cos(w*T_f) ]; %
%A=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];;%实际模型CV 
r0=20;
Q=[r0 0 0 0;0 r0 0 0; 0 0 r0 0; 0 0 0 r0];%运动过程噪声协方差
wk=[sqrt(r0)*randn;sqrt(r0)*randn;sqrt(r0)*randn;sqrt(r0)*randn];%运动过程噪声
r1=20;
r2=0.001*pi/180;
R=[r1 0;0 r2];%雷达量测噪声协方差
vk(:,1)=[sqrt(r1)*randn;sqrt(r2)*randn];%雷达量测噪声
X0=[200;5;1000;15];%初始状态
X(:,1)=X0;
Zk(:,1)=[sqrt(X(1,1)^2+X(3,1)^2); atan2(X(3,1),X(1,1))]+vk(:,1);
%Zk(:,1)=[sqrt(X(1,1)^2+X(3,1)^2); atan(X(3,1)/X(1,1))]+vk(:,1);
X0=[200;1;1000;15];%初始估计状态
Pk=[400 0 0 0; 0 400 0 0; 0 0 400 0;0 0 0 400];%初始协方差
X_pre(:,1)=X0;
X_f(:,1)=[200;1;1000;15]
P_pre(:,:,1)=A*Pk*A'+Q;
rr=X_pre(1,1)^2+X_pre(3,1)^2;
H(:,:,1)=[X_pre(1,1)/sqrt(rr) 0 X_pre(3,1)/sqrt(rr) 0; -1.*X_pre(3,1)/rr 0 X_pre(1,1)/rr 0];
%****************************************滤波*******************************
for i=2:S
    wk(:,i)=[sqrt(r0)*randn;sqrt(r0)*randn;sqrt(r0)*randn;sqrt(r0)*randn];%运动过程噪声
    X(:,i)=A*X(:,i-1)+wk(:,i);%真实状态
    vk(:,i)=[sqrt(r1)*randn;sqrt(r2)*randn];

    Zk(:, i) = [sqrt(X(1, i)^2 + X(3, i)^2); atan2(X(3, i),X(1, i))] + vk(:, i);
    %Zk(:,i)=[sqrt(X(1,i)^2+X(3,i)^2); atan(X(3,i)/X(1,i))]+vk(:,i);
    K(:,:,i)=P_pre(:,:,i-1)*H(:,:,i-1)'*inv(H(:,:,i-1)*P_pre(:,:,i-1)*H(:,:,i-1)'+R);
    re(:,i)=Zk(:,i)-[sqrt(X_pre(1,i-1)^2+X_pre(3,i-1)^2); atan2(X_pre(3,i-1),X_pre(1,i-1))];
    %re(:,i)=Zk(:,i)-[sqrt(X_pre(1,i-1)^2+X_pre(3,i-1)^2); atan(X_pre(3,i-1)/X_pre(1,i-1))];
    X_f(:,i)=X_pre(:,i-1)+K(:,:,i)*re(:,i);
    P_f(:,:,i)=P_pre(:,:,i-1)-K(:,:,i)*H(:,:,i-1)*P_pre(:,:,i-1);
    X_pre(:,i)=A*X_f(:,i);
    P_pre(:,:,i)=A*P_f(:,:,i)*A'+Q;
    rr=X_pre(1,i)^2+X_pre(3,i)^2;
    H(:,:,i)=[X_pre(1,i)/sqrt(rr) 0 X_pre(3,i)/sqrt(rr) 0;-1.*X_pre(3,i)/rr 0 X_pre(1,i)/rr 0];
end
%***********误差图*************
for i=1:S
    rms(i)=sqrt((X(1,i)-X_f(1,i))^2+(X(3,i)-X_f(3,i))^2);
end
figure
t=50:100;
plot(t,rms(50:100));
%***********绘制轨迹图**********************
figure                                 
hold on; 
box on;                                %对当前坐标图加上边框
title('跟踪轨迹图');
xlabel('X/m');
ylabel('Y/m');
plot(X(1,:),X(3,:),'-k');              %真实轨迹 %显示（x,y）位置信息
plot(X_f(1,:),X_f(3,:),'-b','LineWidth', 1);         %Kalman滤波轨迹
plot(Zk(1,:).*cos(Zk(2,:)),Zk(1,:).*sin(Zk(2,:)),'-r')
legend('真实轨迹','滤波轨迹','观测轨迹')
正在学习机动目标跟踪算法，会持续更新，做相关方向的同学可以点个关注，大家共同进步。