无迹卡尔曼滤波_目标跟踪卡尔曼滤波仿真

思考过程

在本次作业中，首先生成三条轨迹，一条真实轨迹，另外一条是观测信号的轨迹，最后一条是经过卡尔曼滤波器之后的轨迹！

真实的轨迹直接按照状态转移方程得到
观测信号的轨迹则是在真实轨迹上面加一个随机的高斯噪声
滤波的轨迹则是通过一步预测以及观测信号的滤波得到的。

实验本身是比较简单的，但是一不小心的话，就得不到想要的结果，我就是一些就修改一个程序

clear all;
clc;
close all;
%%
%初始化
T1 = 400;
T2 = 600;
T3 = 610;
T4 = 660;
T5 = 900;
Sample_T = 2;%观测间隔
b = 1000;
X_init = [1000 0 0 8000 -12]';
P = diag([b b b b b]);  %代表对于初始的状态十分不确定，影响的收敛速度而已

measurement_sigma = 100;
measurement_noise = diag([measurement_sigma   measurement_sigma]);

process_sigma = 10;
process_noise = diag([process_sigma process_sigma/Sample_T process_sigma/Sample_T.^2 process_sigma process_sigma/Sample_T]);

%状态转移矩阵
F = [1 Sample_T 0.5*Sample_T*Sample_T 0 0;
     0 1    Sample_T    0 0;
     0 0    1    0 0;
     0 0    0    1 Sample_T;
     0 0    0    0 1];
 %观测方程
 H =[1 0 0 0 0;
     0 0 0 1 0];

 X_ture_traj = X_init;%真实轨迹
 Z_meas_traj = [1000;8000];%测量的轨迹
 X_estimate_traj = [1000;8000];%估计的轨迹
 X_init_copy = X_init;
 %%
 %卡尔曼滤波
for i = 1:T5/2%因为每两秒才测量一次
    if i>=T1/2 &&i<=T2/2
        a      = 0.075;
        X_init(3,i) = a;
        X_init_copy(3,i) =a;
    end
    if i>=T2/2 &&i<=T3/2
        a      = 0;
        X_init(3,i) = a;
        X_init_copy(3,i) =a;

    end  
     if i>=T3/2 &&i<T4/2
        a      = -0.3;
        X_init(3,i) = a;
        X_init_copy(3,i) =a;

     end  
    if i>=T4/2
        a           = 0;
        X_init(3,i) = a;  
        X_init_copy(3,i) =a;

    end

    %产生真实的轨迹
    X_true     = F * X_init(:,i);
    X_init(:,i+1)     = X_true;  
    X_ture_traj(:,i+1) = X_true;

    Z_meas_traj(:,i+1) = [X_true(1)+wgn(1,1,40);X_true(4)+wgn(1,1,40)];

    %卡尔曼滤波
    %一步预测
    X_estimate =  F * X_init_copy(:,i);
    P_estimate =  F * P * F' + process_noise;

    %滤波更新
    K = P_estimate * H' * inv(H *P_estimate* H' + measurement_noise);
    X_correct = X_estimate + K * ( Z_meas_traj(:,i+1) - H * X_estimate);
    P_correct = (eye(5) - K * H) * P_estimate;

    X_init_copy(:,i+1) = X_correct;
    P = P_correct;

    %缺少知道一个加速度，如果多知道一个加速度的话，确实效果会好很多，那么缺少一个加速度，然后只有一个测量的话，
    X_estimate_traj(:,i+1) = [X_correct(1); X_correct(4)];

end
plot(X_ture_traj(1,:),X_ture_traj(4,:),Z_meas_traj(1,:),Z_meas_traj(2,:),X_estimate_traj(1,:),X_estimate_traj(2,:))
title('目标轨迹');
xlabel('X(米)');
ylabel('Y(米)');
legend('目标真实轨迹','目标测量轨迹','目标估计轨迹');

效果如下:

对应着过程噪声是10的情况下

局部放大图：

可以看到好像这个估计器很垃圾，估的都不知道是什么东西。。。。

如果过程噪声设置为0的情况，看下效果：

可以看到已经相当接近了，不过还是有点距离，这是因为我们充分相信我们的预测，但是我们的测量本身就差得很，所以影响还是挺大的。这个实际上是有点夸张了，噪声不可能这么大的。只是想说明在绝对实力（噪声非常大的情况下），任何估计器都会失效。

接下来看一下如果传感器比较精确的情况下面，会长什么样子呢？

将这里的wgn稍微减小下

Z_meas_traj(:,i+1) = [X_true(1)+wgn(1,1,20);X_true(4)+wgn(1,1,20)];

可以看到非常吻合了。。。说明还是有点用处的。。

接下来看以下过程中出现的一个问题以及发现的信息：

X_estimate =  F * X_init_copy;

    P_estimate =  F * P * F' ;

    %滤波更新
    K = P_estimate * H' * inv(H *P_estimate* H' + measurement_noise);
    X_correct = X_estimate + K * ( Z_meas_traj(:,i+1) - H * X_estimate);
    P_correct = (eye(5) - K * H) * P_estimate;

    X_init_copy = X_correct;
    P = P_correct;

在卡尔曼处理时，我想要直接利用起始点，然后根据预测方程以及量测方程得到估计轨迹，效果如下：

可以看到，轨迹已经便宜了许多，这是以为我们在上面的那个卡尔曼里面并没有对利用加速度信息，所以可以看到他在转弯的时候普遍表现得不好，特别是对于后面得急转弯，更是相差甚远，跟踪效果滞后了许多。

而如果有转弯处得加速度信息得话，可以马上判断，如前面得图所示。

总结：一个看似简单得实验，背后却又许多知识值得我们探究，如果继续修改其他参数得话，相信还会有更多得体会。纸上谈兵终觉浅，觉知此事需躬行！！！！本题如果要利用匀速圆周运动得话，需要使用CTRV模型，我大概画了下图，关于这个模型可以看下我的上一篇无迹卡尔曼滤波。一个好的模型对于整个系统是非常重要的。

还有另外一个就是有一种叫做IMM模型，好像是目标跟踪领域非常常用d的东西，就是把几个模型混合在一起，然后能够自适应判断处于什么模型状态。

X = zeros(5,100);
X(:,1) = [0 0 12 -pi/2 0.45/180*pi]';
T = 200;%总长度
v = 12;
yaw_rate = 0.45/180*pi;
a = linspace(1,200);
sample_time = T /100;

tmp_state = zeros(5,1);
for i = 2:100
    v_k = X(3,i-1);
    yaw = X(4,i-1);
    yaw_rate = X(5,i-1);   
    tmp_state(1) = v_k/yaw_rate * (sin(yaw+yaw_rate*sample_time) - sin(yaw))
    tmp_state(2) = v_k/yaw_rate * (-cos(yaw + yaw_rate * sample_time)+ cos(yaw))
    tmp_state(3) = 0;
    tmp_state(4) = yaw_rate* sample_time;
    tmp_state(5) = 0;

    X(:,i) = X(:,i-1) + tmp_state;
end
plot(X(1,:),X(2,:))