基于卡尔曼滤波的目标跟踪matlab测试小程序

给定一个轨迹y=x^2+x+1，

加上高斯噪声randn 后输入设计的卡尔曼滤波器，

使用当前测出的数据来修正预估值，理论上，

随着时间变久，数据增多，对轨迹的预测值会越来越准确。

%卡尔曼滤波测试
clc;
clear all;
%假设存在一个目标轨迹为 y=x^2+x+1,对其进行加噪声后再通过卡尔曼滤波器进行轨迹预测。
%取N个采样点
N=100;
x=0:N-1;

for i=1:N
    y(i)=x(i)^2+x(i)+1;
end
%看看实际轨迹
figure(1);plot(x,y); 
%加一个均值为0的白噪声。
y_zao=y+400*rand(1,N);
%看看加噪声后的轨迹
figure(2);plot(x,y_zao); 

%构建滤波器，思路：用上一时刻的保留值计算出当前时刻的预估值，再用此时观测到的值对其进行修正，然后将修正后的值保留，即为跟踪的目标轨迹
%轨迹模板
temp=[1 1 1/2;
      0 1  1 ;
      0 0  1 ];

a=[];%保存每一时刻的                   保留值
b=[];%用上一时刻的保留值计算出的此刻的  预估值
%y(i)表示此刻的                        观测值

%直接假设初始加速度为2，初始速度为1,初始位置为1
a(1,1)=1;
a(2,1)=1;
a(3,1)=2;

b(1,1)=1;
b(2,1)=1;
b(3,1)=2;

for i=2:N
    b(1,i)=[1 0 0]*temp*a;  %预估值
    b(2,i)=[0 1 0]*temp*a;
    b(3,i)=[0 0 1]*temp*a;
    if abs(y_zao(i)-b(1,i))<250
        k=abs((y_zao(i)-b(1,i))/b(1,i));   %用差值比上预估值，相差不大时就使得k无限接近于0，即相信预估值
        a(1,1)=b(1,i)+k*(y_zao(i)-b(1,i)); 
        a(2,1)=b(2,i);
        a(3,1)=b(3,i);
    else
        k=1;
        a(1,1)=b(1,i)+k*(y_zao(i)-b(1,i));  %相差很大时就取实际观测值，表示此刻目标已经不以模板来运动了
        a(2,1)=b(2,i);
        a(3,1)=b(3,i);
    end

end
%卡尔曼滤波追踪后的轨迹
figure(3);plot(x,b(1,:)); 

 原目标的运动轨迹为

 

接受到的加噪轨迹为

使用卡尔曼滤波目标追踪的轨迹为

可以看到，最初由于数据量过小，存在跳变轨迹

随着数据量的增多

轨迹逐渐平滑且越来越接近真实的轨迹。

课程小作业

本人原创，仅供参考学习，用之点赞，欢迎讨论