目标跟踪(一)线性卡尔曼滤波（KF）

其实，目标跟踪是属于滤波理论中的一种。说到滤波，从最开始的贝叶斯滤波，维纳滤波，在到卡尔曼滤波。本系列将从卡尔曼滤波开始讲述。有对贝叶斯滤波等有兴趣的童鞋们可以自行在B站上面进行学习。

Kalman滤波原理

我们考虑离散时间线性随机动态系统用如下状态空间模型描述：
在这里插入图片描述

则当前的状态估计可以用下式表达：

则预测的协方差矩阵为：

我们滤波的目的是为了使得传感器的测量值能够更好的去估计我们的状态值。所以我们当然希望上式的误差协方差矩阵越小越好。也就是，我们需要找到一个最佳的增益矩阵，也就是卡尔曼增益，使的上式值最小。

Kalman增益矩阵求解过程(小编直接把整理好的文档截图了)

在这里插入图片描述
为了方便对上式计算，在这里提出3个定理。

然后我们去求使得P最小的最佳的Kalman增益矩阵K

经过上面的推导，我们已经得出了卡尔曼滤波的最佳增益矩阵，那么我们下面来设计卡尔曼滤波器。

Kalman滤波器的设计

卡尔曼滤波器其实就是根据上面的描述，利用我们对状态的先验估计，结合传感器的测量值，去修正我们对状态的估计，使的我们最后的估计(也就是后验估计)能够更加逼近真实的状态值。

在这里插入图片描述
上面便是线性卡尔曼滤波器的的五大核心公式。

写在最后：在这里，小编想着在用大白话来解释一下卡尔曼滤波的过程，帮助各位朋友们的理解

我们以后面的仿真过程为例，匀速运动过程

首先，搞清楚我们为什么需要使用Kalman滤波？

1，目标在机动过程中，会受到其他的干扰，比如风吹等。因为这些干扰是我们无法事先知道的，所以就使得我们不能使用物理过程精确描述，也就是我们的状态过程方程无法精确描述目标的机动过程。

2，我们的传感器，在测量目标的X,Y位置肯定也是存在误差的。
以上两点就是我们为什么要使用Kalman滤波。

Kalman滤波过程是怎样的呢？

1，我们根据目标的机动特性，可以建立物理模型来预测目标的下一个状态，也就是我们使用我们的过程状态方程，进行预测，预测值也成为先验估计。

扫描二维码关注公众号，回复： 15462180 查看本文章

2，我们知道，我们这个先验估计肯定是不准确的，存在误差的，所以就有了先验估计协方差矩阵P，这个矩阵就是定量的描述了我们先验估计的误差。

3，我们滤波的目的是为了使的这个误差尽量最小，所以根据这一准则，也就得到了Kalman增益矩阵的计算公式。得到我们的最佳Kalman增益矩阵。

4，我们利用我们的先验估计，结合传感器的测量值，得到我们的最佳估计，也就是后验估计，这也是我们滤波后的结果。

写在最后的最后

从Kalman滤波五大公式中，我们可以看到，Kalman滤波是带有反馈的性质的，也就是Kalman滤波过程是一个收敛的过程的。

因为我们生活中的很多过程或者测量值是非线性的，这是线性卡尔曼滤波便失去了作用，我们后面将针对非线性过程，讲述拓展卡尔曼滤波和无迹卡尔曼滤波。

Matlab仿真分析

仿真参数如下

在这里插入图片描述

仿真结果

在这里插入图片描述

我们可以看到，滤波后的误差明显比测量值的误差小了很多。

Matalb代码如下：

clear;close all;
T = 1;N = 60;
F = [1,T,0,0;
     0,1,0,0;
     0,0,1,T;
     0,0,0,1]; %二维CV模型
H = [1 0 0 0;
     0 0 1 0];
X0 = [100 2 200 20]';Qk = diag([0.1 0.01 0.1 0.01]);Rk = diag([1 1]);
P0 = diag([0.1 0.01 0.1 0.01]);
Xk = Track(X0,Qk,F,N/T);Zk = Measurements(Xk,Rk,H); 
Xkf = EKF(X0,F,Zk,Qk,Rk,H,P0);
figure()
plot(Xk(1,:),Xk(3,:),'LineWidth',1);hold on;
plot(Xkf(1,:),Xkf(3,:),'+');hold on;
plot(Zk(1,:),Zk(2,:),'*');grid on;
xlabel('X轴/m');ylabel('Y轴/m');
legend('真实轨迹','估计轨迹','观测轨迹')
figure()
delta = sqrt((Xk(1,:)-Xkf(1,:)).^2+(Xk(3,:)-Xkf(3,:)).^2);
delta1 = sqrt((Xk(1,:)-Zk(1,:)).^2+(Xk(3,:)-Zk(2,:)).^2);
plot(delta);hold on;
plot(delta1);grid on;
legend('滤波后估计误差','直接观测的误差')
xlabel('时间/s');ylabel('距离均方误差');
%% 子函数
function Xk = Track(X0,Qk,F,N)
% 目标的真实航迹
%%输入参数
% X0 - 状态初值
% deltaX - 过程噪声标准差
% Fx - 转移矩阵
% N - 仿真总结拍
%%输出参数
% Xk - 真实航迹
% Qk - 过程噪声协方差

n_x = length(X0); % 状态向量维度
Xk = zeros(n_x,N);Xk(:,1) = X0;
for ii = 2:N
    Xk(:,ii) = F*Xk(:,ii-1) + Qk*randn(n_x,1);
end
end
%--------------------------------------------------------------------------
function Zk = Measurements(Xk,Rk,H)
% 传感器量测
%输入参数
% Xk - 真实航迹
% Rk - 量测噪声协方差
% Xstation - 观测站的坐标
%输出参数
% Zk - 传感器量测值

N = length(Xk(1,:)); % 仿真总节拍
n_z = length(H(:,1));
Zk = zeros(n_z,N);
for ii = 1:N
    Zk(:,ii) = H*Xk(:,ii) + Rk*randn(n_z,1);
end
end
%--------------------------------------------------------------------------
function [Xkf] = EKF(X0,F,Zk,Qk,Rk,H,P0)
% 卡尔曼滤波
%输入参数
% X0 - 滤波初值
% F - 转移矩阵
% Zk - 传感器量测
% Qk - 过程噪声协方差
% Rk - 量测噪声协方差
%输出参数
% Xekf - 状态估计

n_x = length(X0); % 状态向量维数
N = length(Zk); % 仿真总节拍
Xkf = zeros(n_x,N);Xkf(:,1) = X0;
Pk_Last = P0; % 协方差初始化
for ii = 2:N
    Xekf_hat = F*Xkf(:,ii-1); % 先验估计
    Pk = F*Pk_Last*F' + Qk; % 先验协方差矩阵
    Kk = Pk*H'/(H*Pk*H'+Rk); % 卡尔曼增益
    Xkf(:,ii) = Xekf_hat + Kk*(Zk(:,ii)-H*Xekf_hat); % 后验估计
    Pk_Last = (eye(n_x)-Kk*H)*Pk; % 更新误差协方差
end
end
	}

线性卡尔曼滤波(KF)