卡尔曼--目标跟踪详解

卡尔曼滤波器介绍

卡尔曼滤波器又叫最佳线性滤波器，他的好处有很多，比如实现简单而且又是纯时域的滤波器不需要进行频域变换所以在工程上有很多应用。

卡尔曼滤波器原理

假设有一辆汽车在公路上行驶我们用他的位置和速度来表示他当前的状态，
写成矩阵的形式状态 $x_t$
$$x_t=\left[\begin{array}{c}{p}_t\\ v_t\end{array}\right]$$

状态转移

驾驶员可以踩油门或者踩刹车，所以车还有一个加速度 $u_t$,如果我们已经知道了上一时刻的状态$x_{t-1}$,那么当前状态 $x_t$会是什么呢？
位置$p_t$和速度$v_t$与上一时刻的关系就如下公式所示：
$$\begin{gathered} p_t=p_{t-1}+{\Delta }_t\times v_{t-1}+\frac{u_t}{2}\times {\Delta }_t^2 \\ {v}_t=v_{t-1}+{\Delta }_t\times u_t \end{gathered}$$
$p_{t-1}$上一时刻的位置
$v_{t-1}$上一时刻的速度
我们观察这两个公式发现输出变量都只是输入变量的线性组合，这就是为什么说卡尔曼滤波器是最佳的线性滤波器，因为它只能描述状态与状态的线性关系，既然是线性关系就可以写成矩阵的形式，如下所示
$$\left[\begin{array}{c}{p}_t\\ v_t\end{array}\right]=\left[\begin{array}{cc}1& {\Delta }_t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{p}_{t-1}\\ v_{t-1}\end{array}\right]+\left[\begin{array}{c}\frac{{\Delta }_t^2}{2}\\ {\Delta }_t\end{array}\right]u_t$$
在进一步把两个状态变换矩阵提取出来，变成
$$F_t=\left[\begin{array}{cc}1& {\Delta }_t\\ 0& 1\end{array}\right],B_t=\left[\begin{array}{c}\frac{{\Delta }_t^2}{2}\\ {\Delta }_t\end{array}\right]$$公式就可以简化为
$${\hat{x}}^{-}=F_t{\hat{x}}_{t-1}+B_t{u}_t\text{\hspace{1em}(1)}$$
公式（1）就是卡尔曼滤波器中的第一个公式 状态预测公式，其中$F_t$就叫做状态转移矩阵，它表示我们如何从上一时刻的状态来推测当前时刻的状态，$B_t$就控制矩阵，他表示控制量$u_t$如何来作用于当前状态。

公式中的$x$带有帽子表示是$x$估计量而不是真实值，因为汽车的真实值我们是永远无法知道，只能根据观测来尽可能的估计$x$的值。等号左边的x还加了一个减号的上标表示这个值是根据上一时刻的状态推测而来的，后面会根据观测量去修正这个${\hat{x}}^{-}$的值，修正之后才是最佳的估计值也就是没有减号上标的$x$的值。

协方差矩阵

有了状态预测公式就可以推测当前时刻的状态，但是我们只能所有的推测都是包含噪声的，噪声越大不确定性就越大，如何来表示推测带有多少不确定性呢？这就要用协方差矩阵来表示。

假设我们有一个一维的包含噪声的数据，每次测量的值都不同，但都是围绕在一个中心值的周围，那我们表示它的分布状况最简单的方法就是记下它的中心值和方差，这实际上是假设了它是一个高斯的分布。


二维包含噪声的数据看起来如下所示

分别对两个坐标轴进行投影，在两个轴上都是高斯分布。

那我们在表示它的分布时是不是分别记下两个高斯分布的中心值和方差就可以了呢，如果两个维度的噪声是独立的时候可以这么表示，但是在两个维度上噪声有相关性的时候就不可以。如下图

为了表示这两个维度的相关性除了要记住两个维度的方差之外，还要有一个协方差来表示两个维度的相关程度，写成矩阵的形式
$$cov(x,x)=\left[\begin{array}{cc}{\sigma }_{11}& {\sigma }_{12}\\ {\sigma }_{12}& {\sigma }_{22}\end{array}\right]$$对角线上的两个值是方差，反对角线上的两个值是协方差他们两个是相等的，
在卡尔曼滤波器中所有有关不确定的表述都要用到协方差矩阵

噪声协方差矩阵的传递

在我们的小汽车的例子中每一个时刻的状态的不确定性都是由协方差矩阵$P$来表示
$$P_t^{-}=F{P}_{t-1}{F}^T$$
当前状态的协方差就等于上一时刻的协方差两边乘以状态转移矩阵，为什么要乘两边这是协方差矩阵的性质 协方差矩阵的性质：$cov(Ax,Bx)=Acov(x,x)B^T$
预测模型并不是百分之百准确的，所以我们要在后面加上一个协方差矩阵$Q$来表示预测模型本身带来的噪声
$$P_t^{-}=F{P}_{t-1}{F}^T+Q\text{\hspace{1em}(2)}$$
公式（2）就是卡尔曼滤波器中的第二个公式 ，它表示不确定性在各个时刻之间的传递关系。

观察矩阵

假设我们这公路的一端放了一个激光测距仪，在每个时刻都可以观测到汽车的位置。

观测到的值我们记为$Z_t$,那么从汽车本身的状态$x_t$到$Z_t$之间有一个变换关系我们记为$H$，当然这个变换关系也只能是线性关系，所以要把$H$写成矩阵的形式也就是观测矩阵
$$H=\left[\begin{array}{cc}1& 0\end{array}\right]$$
$x$和$Z$的维度不一定是相同的，在我们的例子里$x_t$是一个二维的列向量，$Z$只是一个标量的值，所以$H$应该是一个一行两列的矩阵，元素为1和0，这样当$H$和$x_t$相乘的时候就得到了一个标量的值$Z$，$Z$也就是汽车的位置，他和$x_t$的第一个元素是相等的，观测值也不是百分之百可靠的，所以也要加上一个观测的噪声$V$
$$Z_t=H{x}_t+V$$
而这个噪声的协方差矩阵用$R$来表示，由于在我们的例子里观测值是一个一维的值，所以这个$R$的形式也不是一个矩阵而是一个单独的值仅仅表示$Z_t$的方差。假设我们除了激光测距仪之外还有其他的测量方法可以观测到汽车的某项特征，那么$Z$就会变成一个多维的列向量，他会包含每一种测量方式的测量值，而每一种测量值都只是真实状态的不完全的表现，我们可以从几种不完全的表述里面推断真实的状态，卡尔曼的数据融合的功能正是在这个测量矩阵中体现出来的。

状态更新

前面已经得到了带有减号的$x_t$现在我们只要在他后面加上一项来修正它的值就可以得到我们的最佳估计值了
$${\hat{x}}_t={{\hat{x}}_t}^{-}+K_t(Z_t-H{{\hat{x}}_t}^{-})\text{\hspace{1em}(3)}$$
括号里面的表示实际的观测值和预期的观测值的残差，这个残差乘上系数$K_t$就可以修正$x_t$的值了，$K_t$叫卡尔曼系数他也是一个矩阵
$$K_t=P_t^{-}{H}^T(H{P}_t^{-}{H}^T+R{)}^{-1}\text{\hspace{1em}(4)}$$公式的推导比较复杂，这里只是用来定性的分析一下，卡尔曼系数的作用主要有两个方面，一是权衡预测状态协方差$P$和观测量的协方差矩阵$R$的大小来决定我们是相信预测模型多一点还是相信观测模型多一点，如果相信预测模型多一点那个这个残差的权重就会小一点，如果相信观测模型多一点这个残差的权重就会大一点。
二是把残差的表现形式从观测域转换到状态域。我们前面讲到观测值$Z$只是一个一维的向量状态$x$是一个二维的向量，他们所用的单位甚至是描述的特征都可能是不同的那我们怎么用观测值的残差去更新状态值，实际那个卡尔曼系数$K_t$就是在替我们做这样的转换。在我们的例子里我们只观测到汽车的位置，但是$K_t$里面已经包含了协方差矩阵$P$的信息，所以它利用速度和位置这两个维度的相关性从位置的残差里面推算出速度的残差，从而让我们可以对状态$x$的两个维度同时进行修正。

噪声协方差矩阵的更新

最后一步就是更新最佳估计值的噪声分布，这个值是留给下一轮迭代时用的。在这一步里状态的不确定性是减小的，而在下一轮中由于传递噪声的引入不确定性又会增大。
$$P_t=(I-K_{t}H)P_t^{-}\text{\hspace{1em}(5)}$$
卡尔曼滤波器就是在这样一种不确定性变化中寻求一种平衡的。

总结

$$\begin{gathered} 预测公式： \\ {\hat{x}}^{-}=F{\hat{x}}_{t-1}+B{u}_{t-1} \\ {P}_t^{-}=F{P}_{t-1}{F}^T+Q \end{gathered}$$
$$\begin{gathered} 更新公式： \\ {K}_t=P_t^{-}{H}^T(H{P}_t^{-}{H}^T+R{)}^{-1} \\ {\hat{x}}_t={{\hat{x}}_t}^{-}+K_t(Z_t-H{{\hat{x}}_t}^{-}) \\ {P}_t=(I-K_{t}H)P_t^{-} \end{gathered}$$
$F_t$叫状态转移矩阵
$B_t$叫控制矩阵，他表示控制量$u_t$如何来作用于当前状态。
$P$叫协方差矩阵表示每一个时刻的状态的不确定性，预测状态协方差
$Q$表示预测模型本身带来的噪声
$H$观测矩阵
$R$观测量的协方差矩阵
$Z$观测值
$I$为单位矩阵