一、简介

运动物体跟踪实际上就是对运动物体位置的测量和估计，和称小兔兔体重一样，我们也有两个渠道可以知道运动物体的位置，一个是我们观察到的，目标A在的某一帧图像的某个坐标点，另一个是我们根据前面几帧里目标的运动情况估计出来的，这个估计是假定目标运动是光滑的（当然也可以有其他模型），在这个根据以前运动做出的估计和我们测出来的目标点位置之间做个加权平均，就是现在的估计值，对下一帧做同样处理，加权平均的权值就是Kalman增益，根据Kalman提供的算法可由以往的误差大小和分布推出，这就是全部的用Kalman滤波做目标跟踪的概念。
Kalman滤波理论主要应用在现实世界中个，并不是理想环境。主要是来跟踪的某一个变量的值，跟踪的依据是首先根据系统的运动方程来对该值做预测，比如说我们知道一个物体的运动速度，那么下面时刻它的位置按照道理是可以预测出来的，不过该预测肯定有误差，只能作为跟踪的依据。另一个依据是可以用测量手段来测量那个变量的值，当然该测量也是有误差的，也只能作为依据，不过这2个依据的权重比例不同。最后kalman滤波就是利用这两个依据进行一些列迭代进行目标跟踪的。
在这个理论框架中，有2个公式一定要懂，即：

二、源代码

function main()
%%@project:飞行器跟踪模拟
%
%@date:2021.5.10
%@descripition:主函数

%产生观测数据
total=3*60;%总的时间长度
global T;%采样周期
T=1;
N=total/T;%数据长度
a=20;
var_rx=400;
var_ry=400;

X=[];%观测数据
X_ideal=[];%理想数据

for i=1:N
    [rx,ry]=track(i*T,a);
    X_ideal=[X_ideal,[rx;ry]];
    rx=rx+var_rx*randn(1,1);
    ry=ry+var_ry*rand(1,1);
    X=[X,[rx;ry]];
end

X_filter=zeros(size(X));%滤波后数据
X_mean=X_filter;%蒙特卡洛平均数据
Error_var=zeros(size(X));
M=10;%蒙特卡洛仿真次数

for iCount=1:M
    X_filter=Trace(X);
    X_mean=X_mean+X_filter;
    Error_var=Error_var+(X_ideal-X_filter).^2;
end

X_mean=X_mean/M;
Error_var=Error_var/M;
Error_mean=X_ideal-X_mean;%误差均值
Error_var=sqrt(Error_var-Error_mean.^2);


plot(X_ideal(1,:),X_ideal(2,:),X(1,:),X(2,:),X_mean(1,:),X_mean(2,:));
axis equal;
legend('理想轨迹','观测轨迹','滤波轨迹');

figure;
k=1:N;
subplot(2,1,1),plot(k,Error_mean(1,:));title('x方向误差均值');xlabel('采样次数'),ylabel('误差均值(米)');
subplot(2,1,2),plot(k,Error_var(1,:));title('x方向误差标准值');xlabel('采样次数'),ylabel('误差标准值值(米)');

figure;
subplot(2,1,1),plot(k,Error_mean(2,:));title('y方向误差均值');xlabel('采样次数'),ylabel('误差均值(米)');
subplot(2,1,2),plot(k,Error_var(2,:));title('y方向误差标准值');xlabel('采样次数'),ylabel('误差标准值值(米)');

%@subfunction    
%理想航迹方程
function [x,y]=track(t,a)
%parameter:
%   t:时间
%   x:横轴位移
%   y:纵轴位移
%   a:转弯处加速度 
%   r:初始位置
%   v:初始速度


   [x,y]=r0';
elseif t>0&&t<=t1
    x=r(1)+v*t;
    y=r(2);
elseif t>t1&&t<=t2
    angel=(t-t1)*w;
    x=D/2*sin(angel);
    y=-D*(sin(angel/2))^2;
else
    x=-v*(t-t2);
    y=-D;
end

    function R=Trace(X)
%@project:飞行器跟踪模拟
% 
%@date:2021.5.10
%@parameter:
%    X:观测数据
%    R:输出坐标

%观测时间间隔
global T;

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

%位移测量误差
var_rx=100;
var_ry=100;
var_rx2=var_rx^2;
var_ry2=var_ry^2;

%观测噪声协方差矩阵
C=[var_rx2,0;...
   0,var_ry2];

%驱动噪声协方差矩阵
var_v=30;
var_a=1;
var_v2=var_v^2;
var_a2=var_a^2;

Q=zeros(5,5);
Q(4,4)=var_v2;
Q(5,5)=var_a2;

%初始状态
s0=[-10000,2000,0,300,0]';

%Kalman滤波跟踪
N=size(X,2);%观测数据长度
s=s0;
a=@traverse;
M=Q;
Xplus=[];%修正后的航迹
for icurrent=1:N
    [s,M]=Karlman(s,M,X(:,icurrent),a,Q,C,H);
    Xplus=[Xplus;(s(1:2))'];
end
    

三、运行结果

四、备注

版本：2014a