卡尔曼滤波理论+应用

 卡尔曼滤波实现单目标跟踪

clear all;
close all;

%%
%卡尔曼滤波(KF) 实现单目标跟踪
%假定目标在二维水平面上运动，从一点出发沿某直线方向前进。

%%
%初始化参数：CV模型——匀速运动模型

dt=1;          %雷达扫描周期=1s
T=80/dt;       %总采样次数=80次

%这些参数是根据公式推出来的
F=[1,dt,0,0; 
    0,1,0,0;
    0,0,1,dt;
    0,0,0,1];  %状态转移矩阵

G=[0.5*dt^2,0;
    dt,0;
    0,0.5*dt^2;
    0,dt];     %过程噪声驱动矩阵

H=[1,0,0,0;
   0,0,1,0];   %观测矩阵

%改变过程噪声
%a=1e-2;        %如果增大这个参数，目标真实轨迹将变成曲线 %1*10^(-2)
a=1e-1;
Q=a*eye(2);    %过程噪声方差=a

%改变观测噪声
b=100;
%b=100;
R=b*eye(2);    %观测噪声方差=b

%过程噪声和观测噪声均为高斯白噪声，均值为0，方差分别是Q和R
W=sqrtm(Q)*randn(2,T);  %过程噪声：产生均值为0，方差为Q，的一个（4*T）随机数矩阵
V=sqrtm(R)*randn(2,T);  %观测噪声：高频随机干扰信号，均值为0，方差为R，的一个（2*T）随机数矩阵

%%
%初始化参数

X=zeros(4,T);           %目标真实位置、速度。%生成4×T全零阵
X(:,1)=[100,5,300,20];  %目标初始位置、速度。%X的第1列的所有元素，这是一个列向量
%从(100m,300m)处出发，水平方向运动速度5m/s，垂直方向运动速度为20m/s

Z=zeros(2,T);           %传感器对位置的观测
Z(:,1)=[X(1,1),X(3,1)]; %观测初始化 %[100,300]

Xkf=zeros(4,T);         %Kalman滤波状态初始化
Xkf(:,1)=X(:,1); 

P=eye(4);               %协方差阵初始化

%%
%模拟目标运动，观测站对目标进行观测，卡尔曼滤波，生成真实轨迹，观测轨迹和滤波轨迹

for k=2:T
    
    %目标真实轨迹
    X(:,k)=F*X(:,k-1)+G*W(:,k-1);      %系统的状态方程
    %目标观测轨迹
    Z(:,k)=H*X(:,k)+V(:,k);            %系统的观测方程
    
    %开始滤波
    Xpred=F*Xkf(:,k-1);                %第一步：状态预测
    Ppred=F*P*F'+G*Q*G';               %第二步：协方差预测
    K=Ppred*H'*inv(H*Ppred*H'+R);      %第三步：求增益  %inv:矩阵求逆
    Xkf(:,k)=Xpred+K*(Z(:,k)-H*Xpred); %第四步：状态更新
    P=(eye(4)-K*H)*Ppred;              %第五步：协方差更新
end

%绘制轨迹图
figure                                 
hold on; 
box on;                                %对当前坐标图加上边框
title('跟踪轨迹图');
xlabel('X/m');
ylabel('Y/m');
plot(X(1,:),X(3,:),'-k');              %真实轨迹 %显示（x,y）位置信息
plot(Z(1,:),Z(2,:),'-r.');             %观测轨迹
plot(Xkf(1,:),Xkf(3,:),'-b*');         %Kalman滤波轨迹
legend('真实轨迹','观测轨迹','滤波轨迹')

%%
%误差分析

for i=1:T
    Err_Observation(i)=RMS(X(:,i),Z(:,i));      %滤波前的误差
    Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));   %滤波后的误差
end

%绘制误差图
figure 
hold on;
box on;
title('跟踪误差图');
xlabel('Time/s');
ylabel('Value of the deviation/m');
plot(Err_Observation,'-ko','MarkerFace','g');
plot(Err_KalmanFilter,'-ks','MarkerFace','r');
legend('观测偏差','滤波后偏差')

%%
%子函数：计算偏差
function deviation=RMS(X1,X2)
    if length(X2)<=2
        deviation=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);  %每时刻，轨迹真实位置和观测位置的距离
    else
        deviation=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);  %每时刻，轨迹真实位置和预测位置的距离
    end
end