自己对于卡尔曼滤波的理解和部分推导

阅读了一篇论文：Comparison of Two Measurement Fusion Methods for Kalman-Filter-Based Multisensor Data Fusion，大概内容是讲基于卡尔曼滤波的多传感器融合的两种测量融合方法的比较，这篇论文里面大量的公式推导没有搞清楚，倒是把卡尔曼滤波的基础内容看了很多。以下主要记录一下查阅很多资料学到的关于卡尔曼滤波的知识（主要是讲卡尔曼滤波的思路，意义及部分推导），以后再整理一下Comparison of Two Measurement Fusion Methods for Kalman-Filter-Based Multisensor Data Fusion这篇论文的相关知识。

I.先来说说卡尔曼滤波,给出卡尔曼滤波的公式：
Prediction：
$$\begin{gathered} {\hat{x}}_{t|t-1}=A_t{\hat{x}}_{t-1|t-1}+B_t{\hat{u}}_t \\ {P}_{t|t-1}=A_t{P}_{t-1|t-1}{A}_t^T+Q_t\text{\hspace{1em}(I)} \end{gathered}$$
Correction:
$$\begin{gathered} K_t=P_{t|t-1}{C}_t^T[C_t{P}_{t|t-1}{C}_t^T+R_t{]}^{-1} \\ {\hat{x}}_{t|t}={\hat{x}}_{t|t-1}+K_t[{\hat{y}}_t-C_t{\hat{x}}_{t|t-1}] \\ {P}_{t|t}=[I-K_t{C}_t]P_{t|t-1}\text{\hspace{1em}(II)} \end{gathered}$$

上式是卡尔曼滤波的公式，包括Prediction（预测）和Correction（校正）模块.先大概有个概念吧，知道卡尔曼滤波是有预测和校正两部分即可，不必详记符号的下标，因为在下文中会对符号下标的意义及变化做阐述。下面以一个小车的例子入手，谈谈两个模块各符号的来源及意义，以及整体的意义。

I.Prediction：

考虑一个在水平面向右运动的小车：

我们想要表示小车在某一时刻的状态$x_t$,可以引入$s_t$来表示小车在此时刻的位移，$v_t$表示小车此刻的速度，则我们可以用
$${\hat{x}}_t=\left(\begin{array}{c}{s}_t\\ v_t\end{array}\right)\text{\hspace{1em}(1)}$$
来表示小车在t时刻的运动状态，由运动方程：
$$\begin{array}{l}{s}_t=s_{t-1}+v_{t-1}\times {\Delta }_t+u_t\times \frac{{\Delta }_t^2}{2}\\ v_t=v_{t-1}+u_t\times {\Delta }_t\end{array}\text{\hspace{1em}(2)}$$
写成矩阵形式：
$$\left(\begin{array}{c}{s}_t\\ v_t\end{array}\right)=\left[\begin{array}{cc}1& {\Delta }_t\\ 0& 1\end{array}\right]\times \left(\begin{array}{c}{s}_{t-1}\\ v_{t-1}\end{array}\right)+\left(\begin{array}{c}\frac{{\Delta }_t^2}{2}\\ \\ {\Delta }_t\end{array}\right)\times {\hat{u}}_t\text{\hspace{1em}(3)}$$
即
$${\hat{x}}_t=A{\hat{x}}_{t-1}+B{\hat{u}}_t\text{\hspace{1em}(4)}$$
我们把A叫做状态转移矩阵，把B叫做控制矩阵，这个矩阵表达式十分优美，只要我们建立起了小车的运动模型，表示出来A和B，我们就可以无限用上一时刻的小车状态计算下一时刻的小车状态（这是句废话，高中物理就是在把这种理想情况拿来算算算了）。然鹅，实际上，小车运动中会不可避免的遭受其他情况的影响，比如，
I.第一类情况：比如说，为了方便起见，我们把模型中的$u_t$当做常数,即假设小车作匀减速运动，这种模型不够精确造成的系统误差我们把它叫过程噪声，它是高斯白噪声（服从正态分布，纯随机过程，期望为0，方差为常数）记作${\hat{q}}_t$，在此例中为二维列向量，对应了${\hat{x}}_t$中两个维度的噪声，所以更为精确一点的模型应该表示为：
$${\hat{x}}_t=A{\hat{x}}_{t-1}+B{\hat{u}}_t+{\hat{q}}_t\text{\hspace{1em}(5)}$$
好，现在我们把系统误差补上去以后，还做不到万事大吉，因为可能受到以下情况的影响（确切的说是一定会受到其中某些影响，比如风这种）：
II.第二类情况：小车轮胎松动了，来了一阵风，地震了。我们如何对付这种意想不到的，随机性很强的，可能对小车运动产生影响的因素呢？也就是说，这些因素使得小车的状态产生了不确定性，如何表征这种不确定性呢？
假设：受第二种情况影响后，小车状态$x_t$的两个分量$s_t$和$v_t$在每个时刻都不是精确可得的，而是服从正态分布的（别问我为什么是正态分布，因为卡尔曼滤波的前提就是假设这些噪声都是正态分布的，实际中也非常接近正态分布了），而且这两个分量是有相关性的（位移和速度当然有相关性）。这样一来，面对两个分别服从正态分布（本质上为不确定性），又彼此之间有相关性的两个分量，如何既能刻画分量本身的不确定性，又能刻画两个分量之间的相关性呢？我们引入噪声协方差矩阵P来刻画二维的正态分布，先介绍一下协方差矩阵：
$$P=cov({\hat{x}}_t,{\hat{x}}_t)\text{\hspace{1em}(6)}$$
$$P_{ij}=cov(x_i,x_j)\text{\hspace{1em}(7)}$$
协方差矩阵P（向量与向量的协方差，是一个矩阵）中的第i行第j列个元素是向量$\hat{x}$中的第i个元素和第j个元素（变量$x_i$和变量$x_j$）之间的协方差（是一个值）。对于此例来说：向量$\hat{x}$有两个分量$s_t$和$v_t$，噪声协方差矩阵P为：
$$P=\left[\begin{array}{cc}cov(s_t,s_t)& cov(s_t,v_t)\\ cov(v_t,s_t)& cov(v_t,v_t)\end{array}\right]\text{\hspace{1em}(8)}$$
这是在 t 时刻刻画小车状态的噪声协方差矩阵，观察此矩阵，为对称矩阵，且对角线上的元素是原向量中各个分量的方差，所以为非负定矩阵。那么问题来了，如果已知$t-1$时刻的噪声协方差矩阵,还需要费时费力的去求t时刻的噪声协方差矩阵吗？并不需要！噪声协方差矩阵是可以传递的!这一点非常非常重要，基于此，卡尔曼滤波的优势才体现出来了：既能通过上一时刻小车的状态${\hat{x}}_{t-1}$预测当前时刻${\hat{x}}_t$小车的状态,还能将小车的状态的不确定性传递下去，这将使得迭代能够进行下去，并且每次都是单步迭代，只用上一时刻的${\hat{x}}_{t-1}$和${\hat{P}}_{t-1}$进行当前时刻的预测（虽然只是预测，还没有引入校正，后面会讲到），而不用管之前的状态，大大减少了运算量。下面，我们对噪声协方差矩阵的传递进行推导：
$$\begin{array}{l}{P}_t=cov({\hat{x}}_t,{\hat{x}}_t)\\ \\ =cov(A{\hat{x}}_{t-1}+B{\hat{u}}_t+{\hat{q}}_t,A{\hat{x}}_{t-1}+B{\hat{u}}_t+{\hat{q}}_t)\\ \\ =Acov({\hat{x}}_{t-1},{\hat{x}}_{t-1})A^T\\ \\ =A{P}_{t-1}{A}^T\end{array}\text{\hspace{1em}(9)}$$
不要忘了，在第一类情况下，我们还引入了过程噪声${\hat{q}}_t$,他也有一个对应的协方差矩阵$Q_t$,所以每一时刻实际的噪声协方差矩阵应该为：
$$P_t=A{P}_{t-1}{A}^T+Q_t\text{\hspace{1em}(10)}$$
（4）式和（10）式组合即是卡尔曼滤波中的预测（状态向量的预测和协方差矩阵的预测）部分：
$$\begin{array}{l}{\hat{x}}_t=A{\hat{x}}_{t-1}+B{\hat{u}}_t\\ P_t=A{P}_{t-1}{A}^T+Q_t\end{array}\text{\hspace{1em}(11)}$$
对比$(I)$式和$(11)$式，咦？下标怎么不一样呢，$(I)$式比$(11)$式下标好像复杂那么一丢丢，复杂到哪里去了呢？这就要牵扯到Correction了。先大概有这么一个认识:$(11)$式的得来是我们构建模型一步步推导来的，是纯预测，而预测加上校正以后，${\hat{x}}_{t-1|t-1}$表示校正后的$t-1$时刻的状态向量$\hat{x}$，用这个校正后的${\hat{x}}_{t-1|t-1}$去预测当前时刻的${\hat{x}}_{t|t-1}$，不是就比之前纯预测的迭代更为精确了吗？
那么，究竟是怎么校正的呢？

II.Measurement：

第III部分的校正是建立在此部分的测量的基础上的，所以我们先对测量进行阐述。还是用这个小车的例子加以说明：

我们添加了一个激光测距仪，用以测量小车的位移，从而对预测模型中得到的每一时刻的预测值进行校正。
我们用${\hat{y}}_t$表示对小车的观测状态向量，并引入一个观测矩阵来联系观测向量和小车的预测状态向量：
$${\hat{y}}_t=C_t{\hat{x}}_t+{\hat{w}}_t\text{\hspace{1em}(12)}$$
$w_t$为观测噪声，是高斯白噪声，对应协方差矩阵$R_t$。在此例中，${\hat{x}}_t$是二维列向量，${\hat{y}}_t$是一维列向量（只测距，不测速），则$C_t$的形式为$\left[\begin{array}{cc}c& 0\end{array}\right]$,即：
$${\hat{y}}_t=\left[\begin{array}{cc}c& 0\end{array}\right]\left(\begin{array}{c}{s}_t\\ v_t\end{array}\right)\text{\hspace{1em}(13)}$$
所以，在${\hat{x}}_t$形式已定的前提下，观测矩阵$C_t$的形式是根据选取的观测向量的维数确定的。那么那个小c的意义又是什么呢？一般 $c$ 都取1，只是起一个映射的作用（map），即把预测的状态向量的维度映射成测量向量的维度。（注意：只是映射维度，$(12)$式实际上只是一个关系式，而不是说测量值是由预测值确定的！${\hat{y}}_t$是实际中测量的值，比如传感器等数据获取设备得到的值，是真实测量到的，是多少就是多少）

III.Correction:

III.1 卡尔曼增益
这个时候，我们已经建立了预测模型和观测模型，那么如何把这两个模型结合在一起呢？如何把预测模型和观测模型得到的结果进行结合呢？应该信哪个模型多一点呢？卡尔曼增益解决了这个问题！先给出卡尔曼增益：
$$K_t=P_{t|t-1}{C}_t^T[C_t{P}_{t|t-1}{C}_t^T+R_t{]}^{-1}\text{\hspace{1em}(14)}$$
观察卡尔曼增益的表达式，都是由哪些矩阵构成的呢：预测模型中的噪声协方差矩阵$P$,测量矩阵$C$,测量噪声对应的协方差矩阵$R$,
这就体现了卡尔曼增益的意义之一：

权衡预测噪声协方差矩阵$P_{t|t-1}$和观测模型中的噪声协方差矩阵$R_t$的大小，决定预测模型和观察模型的权重。

（至于卡尔曼滤波怎么推导的，我还没有看，后面要补上。目前先理解它的意义，便于自己应用）

III.2 状态校正
使用卡尔曼增益对预测模型中的${\hat{x}}_{t|t-1}$进行校正，得到校正后的${\hat{x}}_{t|t}$:
$${\hat{x}}_{t|t}={\hat{x}}_{t|t-1}+K_t[{\hat{y}}_t-C_t{\hat{x}}_{t|t-1}]\text{\hspace{1em}(15)}$$
上式中的$[{\hat{y}}_t-C_t{\hat{x}}_{t|t-1}]$叫实际观测值与预测值的残差.
这就体现了卡尔曼增益的意义之二：

把残差的表现形式从状态域转到观察域。

III.3 噪声协方差矩阵$P$的校正
更新（校正）噪声协方差矩阵，用于下一轮预测：
$$P_{t|t}=[I-K_t{C}_t]P_{t|t-1}\text{\hspace{1em}(16)}$$
用系数矩阵对预测模型中的噪声协方差矩阵进行校正。
如此一来，将状态校正后得到的$(15)$式和噪声协方差矩阵校正后得到的$(16)$式带入原预测模型中$(11)$中，即用${\hat{x}}_{t-1|t-1}$ 代替原来纯预测中的 ${\hat{x}}_{t-1}$（P同理）即可。

至此，卡尔曼滤波中的五个公式就整理的差不多了

Prediction：
$$\begin{gathered} {\hat{x}}_{t|t-1}=A_t{\hat{x}}_{t-1|t-1}+B_t{\hat{u}}_t \\ {P}_{t|t-1}=A_t{P}_{t-1|t-1}{A}_t^T+Q_t\text{\hspace{1em}(I)} \end{gathered}$$
Correction:
$$\begin{gathered} K_t=P_{t|t-1}{C}_t^T[C_t{P}_{t|t-1}{C}_t^T+R_t{]}^{-1} \\ {\hat{x}}_{t|t}={\hat{x}}_{t|t-1}+K_t[{\hat{y}}_t-C_t{\hat{x}}_{t|t-1}] \\ {P}_{t|t}=[I-K_t{C}_t]P_{t|t-1}\text{\hspace{1em}(II)} \end{gathered}$$
不足：没有阅读卡尔曼滤波的原论文，对卡尔曼滤波增益等的推导不清楚，只是按碎片化的学习进行了一下理解。
后续：实际中怎么用（大概就是给一个初值后，预测一次校正一次，用校正后的值再预测如此循环往复），下周进行整理，模拟一个实例出来。