目标跟踪基础——3.卡尔曼滤波

卡尔曼滤波中重要的值有两个, 一个是上一个时刻的估计值（它是由上一时刻进行估计计算得出), 另外一个就是当前时刻的测量值或者叫做观测值（它可以是通过传感器获取得到）。
在卡尔曼滤波算法的过程中, 首先我们需要设定预测和估计的初始值, 以及其中的一些参数:

A/F:​ 状态转移矩阵
B​ : 控制系统参数，控制系统矩阵
u​ : 系统的输入或者是控制量
P​ : 状态估计协方差矩阵
K​ : 卡尔曼增益，用于估计值更新时的后验估计
H​ : 测量系统的参数， 状态观测矩阵
I: 通过先验P矩阵更新后验P矩阵的参数，一般为单位矩阵
Q：P矩阵计算中的超参数
R​ : 卡尔曼增益K计算时的超参数

$$\begin{array}{cc}\text{预测}& \text{更新}\\ {\hat{x}}_t^{-}=F{\hat{x}}_{t-1}+B{u}_{t-1}& K_t=P_t^{-}{H}^T{\left(H{P}_t^{-}{H}^T+R\right)}^{-1}\\ P_t^{-}=F{P}_{t-1}{F}^T+Q& {\hat{x}}_t={\hat{x}}_t^{-}+K_t\left(z_t-H{\hat{x}}_t^{-}\right)\\ & P_t=\left(I-K_{t}H\right)P_t^{-}\end{array}$$

对状态转移的理解:
假设一辆小车在路上行驶, 那么我们要求当前行驶的路程 $x_k$​ 的话, 就可以用上一时刻小车行驶的路程 ​$x_{k-1}$​ 来计算:
$$x_k=x_{k-1}+v\ast dt$$
其中, ​v​ 代表小车行驶的速度,dt 代表一个时刻的单位时间, 这时候小车行驶路程的状态就从上一时刻转移到了当前时刻, 这个例子中的状态转移矩阵F​ 就是 1 , 而v​ 则是状态控制量 ​u,dt​ 则是控制系统矩阵B, 如果我们将小车行驶中的风阻等因素考虑进去的话, 此时这个系统中小车行驶路程的计算方程就是:
$$x_k=x_{k-1}+v\ast dt+w_k$$
$w_k$就是 k​ 时刻下的风阻等因素, 也可以叫做这个系统中的噪声。
在目标跟踪中，我们可以将初始状态X设定为$[x,y,w,h,dx,dy{]}^T$​​ ，其中x，y，w，h分别代表检测框的中心点坐标和宽高，dx和dy分别为x和y的运动估计量，这时候我们的状态转移矩阵就可以设为：
$$\left[\begin{array}{cccccc}1& 0& 0& 0& 1& 0\\ 0& 1& 0& 0& 0& 1\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$$
由于目标跟踪模型中是不存在输入的，所以我们只需要将它与（k-1）时刻的状态进行矩阵运算就能够得到当前时刻的先验估计值：
$$x_k=x_{k-1}\ast F=\left[\begin{array}{cccccc}1& 0& 0& 0& 1& 0\\ 0& 1& 0& 0& 0& 1\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]\ast [x,y,w,h,dx,dy{]}^T=[x+dx,y+dy,w,h,dx,dy{]}^T$$
显然，（k-1）时刻的状态$x_{k-1}$通过与状态转移矩阵运算就将中心点坐标加上了对应的运动估计量，这时候我们就用卡尔曼滤波中的状态转移矩阵通过上一时刻的估计值计算出了当前的估计值。
卡尔曼滤波流程：
1.先验估计：通过先验估计方程将当前时刻的估计值计算出来
$${{\hat{x}}_t}^{-}=F{\hat{x}}_{t-1}+B{u}_{t-1}$$
2.先验估计协方差：更新先验估计协方差矩阵
$$P_t^{-}=F{P}_{t-1}{F}^T+Q$$
3.测量方程：通过测量方程更新测量值
$$z_t=H{x}_t+v$$
4.更新卡尔曼增益：更新当前时刻的卡尔曼增益值
$$K_t=\frac{P_t^{-}{H}^T}{H{P}_t^{-}{H}^T+R}$$
5.修正估计：根据测量方程和当前时刻的卡尔曼增益$K_t$​修正当前估计值，得到当前时刻的最优估计值
$${\hat{x}}_t={\hat{x}}_t^{-}+K_t(z_t-H{\hat{x}}_t^{-})$$
6.更新后验估计协方差：最后更新后验估计协方差矩阵
$$P_t=(I-K_{t}H)P_t^{-}$$
估计方式：在卡尔曼滤波过程中，如果测量值未丢失就使用修正后的测量值作为最终的结果，但如果测量值丢失，将会使用上一时刻运动状态估计出当前的估计值作为最终的结果，也就是说如果测量值丢失时仅仅有先验估计一步操作，此时卡尔曼增益和先验后验协方差矩阵都不会更新。
$$\left\{\begin{array}{cc}测量值未丢失：& 完整流程，卡尔曼增益和先验后验协方差矩阵都正常更新\\ 测量值丢失：& 仅进行第一步流程更新当前估计值，卡尔曼增益和先验后验协方差矩阵都不会更新\end{array}\right\}$$