声明： 本文图文均来源于https://www.kalmanfilter.net/，如有侵权请联系删除。 ❞
本文由微信公众号【DeepDriving】整理，由于全文内容较多所以分成3部分发出来。 「关注公众号【DeepDriving】，后台回复关键字【卡尔曼滤波器】可获取全文PDF」 。 ❞
多维卡尔曼滤波器
前面介绍了一维卡尔曼滤波器，相信大家已经对卡尔曼滤波器有了一定的认识，但是在实际应用中，我们通常需要处理有多维状态数据的系统。比如，对于一个三维空间中的飞机，我们需要一个9维向量来描述其位置、速度和加速度：
假设我们采用恒加速度（constant acceleration）动态模型，那么在时刻飞机的状态可以写为
在实际应用中，我们通常会使用矩阵的方式来描述多维数据处理的过程。接下来，我们将用矩阵的方式来介绍多维卡尔曼滤波器的几个方程。
状态外推方程
状态外推方程的作用是在当前时刻基于现有的知识去预测时刻系统的状态，所以也叫状态预测方程或者状态转移方程，其矩阵形式的公式如下：
其中，是预测的时刻的系统状态向量；是估计的时刻的系统状态向量；是控制变量（输入变量），对系统来说是一个可测量的（确定性的）输入；是过程噪声，是会影响系统状态的不可测量的输入量；是状态转移矩阵；是控制矩阵，也叫输入转移矩阵，用于将控制变量映射为状态变量。
以前面的飞机为例，用状态向量描述其在三维空间中的位置、速度和加速度
假如采用恒加速模型，如果不考虑有控制变量输入，那么状态外推方程为
状态转移方程为
那么
算得结果
假如我们有加速度传感器可以提供飞机的加速度信息作为系统的输入，所提供的加速度测量值为
那么状态外推方程为
转移矩阵和控制矩阵分别如下：
那么
协方差外推方程
协方差外推方程如下：
其中，描述了当前估计值的不确定度，是当前系统状态的协方差矩阵；描述了当前预测值的不确定度，是预测的系统状态的协方差矩阵；是状态转移矩阵；是过程噪声协方差矩阵。
现在我们从头来推导一下这个方程。
假设过程噪声为零（），那么
由前面的背景知识我们知道系统状态向量的协方差矩阵为
因此
根据状态外推方程
可得
该如何构建过程噪声协方差矩阵呢？
假设系统的状态为位置、速度和加速度，分别用，和来表示。对于恒速模型来说，过程噪声的协方差矩阵为
我们将用随机加速度方差来表示位置、速度的方差和协方差。由前面的背景知识可以知道
那么
这样求矩阵比较麻烦，我们可以通过下面的方式快速求出来。
如果动态模型不包含控制输入，那么我们可以直接通过状态转移矩阵将加速度的随机方差映射到动态模型中。定义矩阵为:
那么过程噪声协方差矩阵可由下面的公式得到
由运动模型可知状态转移矩阵为
则
如果动态模型包含控制输入，那么过程噪声协方差矩阵可由下面的公式得到：
其中为控制矩阵（或者叫输入转移矩阵）。由运动模型可知
那么
状态更新方程
状态更新方程的表达式如下所示：
其中是时刻估计的系统状态向量；是在时刻预测的系统状态向量；是卡尔曼增益；是测量值；为观测矩阵。
在前面测量金条重量的例子中我们已经介绍了状态更新方程，在这里就不做更多介绍了。在多维的情况下，我们需要注意的是矩阵的维度。举个例子，假如系统的状态是一个5维的向量，但是只有第1、3、5维是可测量的：
那么观测矩阵应该是一个的矩阵：
那么观测残差为
此时卡尔曼增益的维度应该是。
协方差更新方程
协方差更新方程的表达式如下：
其中，为当前状态估计值的协方差矩阵；是当前状态的先验估计（基于前一个状态的预测）的协方差矩阵；为卡尔曼增益；为观测矩阵；为测量噪声的协方差矩阵。
接下来，我们将对这个公式进行详细推导。推导过程中会用到下面几个公式：
| 方程 | 说明 |
|---|---|
| 状态更新方程 | |
| 测量方程，为测量噪声 | |
| 状态协方差矩阵 | |
| 测量噪声协方差矩阵 |
对于每一个估计，估计误差为
再由上式来推导估计值的协方差矩阵
是先验估计与真实值之间的误差，它与当前时刻的测量噪声是不相关的。有前面的背景知识我们知道，两个相互独立变量乘积的期望值为零，所以
那么
由
可得
好了，终于把这个公式推导完了。
卡尔曼增益
卡尔曼增益的表达式如下：
其中，为卡尔曼增益；是当前状态的先验估计（基于前一个状态的预测）的协方差矩阵；为观测矩阵；为测量噪声的协方差矩阵。
在开始推导卡尔曼增益之前，让我们先对协方差更新公式再做一些变换：
卡尔曼滤波器是最优滤波器，因此我们需要寻求能最小化估计方差的卡尔曼增益。为了最小化估计方差，我们需要最小化状态协方差矩阵的主对角线，也就是最小化它的迹。为了求得的极小值，我们需要求迹关于卡尔曼增益的导数，并设导数为零。
求导数并令其为零：
这里用到了两个计算公式：
❞
由上面的导数为零，可得
因此可求得卡尔曼增益：
因为协方差矩阵是对称矩阵，所以
。 ❞
简化的协方差更新方程
在很多资料中，协方差更新方程并不是前面我们推导出来的那样，而是将卡尔曼增益公式代入后再化简得到的。
化简后的式子显得更加优雅而且方便记忆。然而需要注意的是，即使是计算卡尔曼增益时由于四舍五入引起的一点小误差也可能会导致巨大的计算误差，因为相减项会因为浮点数精度误差而导致出现非对称矩阵。因此，这个方程是数值不稳定的！
总结
前面已经用矩阵形式详细推导了卡尔曼滤波器的5个方程，把这5个方程联系到一起可以知道，卡尔曼滤波器执行的操作就是一个不断“预测-更新”的迭代过程，如下图所示：
在初始化之后，卡尔曼滤波器将会预测系统在下一个时刻的状态，同时提供预测的不确定度。在得到测量值之后，卡尔曼滤波器会根据测量值对预测值进行更新（校正）从而估计出一个相对准确的当前状态及其不确定度。然后，卡尔曼滤波器将基于当前状态去预测下一时刻的系统状态，如此循环迭代下去......
下图展示了卡尔曼滤波器在执行”预测-更新“操作过程中所使用的5个方程：
下表对卡尔曼滤波器的5个方程进行了总结：
| 操作 | 方程 | 方程名 |
|---|---|---|
| 预测 | 状态外推方程 | |
| 预测 | 协方差外推方程 | |
| 更新 | 状态更新方程 | |
| 更新 | 协方差更新方程 | |
| 更新 | 卡尔曼增益 |
其中
| 方程 | 说明 |
|---|---|
| 测量方程 | |
| 测量噪声协方差矩阵 | |
| 过程噪声协方差矩阵 | |
| 状态协方差矩阵 |
下标是对5个方程中所用到的符号的说明：
| 符号 | 说明 |
|---|---|
| 状态向量 | |
| 观测向量 | |
| 状态转移矩阵 | |
| 输入向量 | |
| 控制矩阵 | |
| 状态协方差矩阵 | |
| 过程噪声协方差矩阵 | |
| 测量噪声协方差矩阵 | |
| 过程噪声向量 | |
| 测量噪声向量 | |
| 观测矩阵 | |
| 卡尔曼增益 | |
| 离散时间索引 |