不同的教材对卡尔曼滤波的描述方式不尽相同，但实际原理是一样的，这里以《概率机器人》为准。
本篇博客中的符号约定参阅前面的博客《机器人学中的概率论基础》。
下面的“状态转移概率”一词，指state transition probability，其实就是“状态方程”。“测量概率”指measurement probability，就是“观测方程”。不同教材的说法不同。

多维正态分布定义回顾

高斯滤波器，Gaussian filters，是指一个状态估计器族，包括卡尔曼滤波及其变体、信息滤波等算法。
首先回顾一下多维正态分布（Multivariate Normal Distribution）的定义，对于一个具有 $k$ 个维度的多维正态分布，其概率密度函数（PDF）可以写为：

其中：

• $\mathbf{x}$ 是一个包含 $k$ 个随机变量的列向量。
• $\boldsymbol{\mu}$ 是一个包含 $k$ 个均值的列向量。
• $\boldsymbol{\Sigma}$ 是一个 $k \times k$ 的协方差矩阵，其中对角线元素是各个变量的方差，而非对角线元素是对应变量之间的协方差。

上述概率密度函数描述了多维正态分布中随机变量的联合分布。与一维正态分布类似，多维正态分布也是关于均值 $\boldsymbol{\mu}$ 对称的，其形状由协方差矩阵 $\boldsymbol{\Sigma}$ 决定。

高斯滤波中参数均值和方差被称为矩参数(moments parameterization) ，均值和方差是概率分布的一阶矩和二阶矩；正态分布的其他矩都是零。

卡尔曼滤波

线性高斯系统

卡尔曼滤波（Kalman Filter, KF），KF用矩参数表示置信度。
如果线性系统满足马尔科夫假设前提下，还满足以下三个条件，则后验分布就是满足正态分布的（高斯的）

1. 状态转移概率（state transition probability，状态方程） $p(\boldsymbol{x}_t | \boldsymbol{u}_t, \boldsymbol{x}_{t-1})$ 必须是带有随机高斯噪声参数的线性函数：

其中， $\boldsymbol{x}_t$ 和 $\boldsymbol{x}_{t-1}$ 是状态向量，$\boldsymbol{u}_t$ 是时刻 $t$ 的控制向量（机器人动作），它们都是列向量。
$\boldsymbol{A}_t$ 是 $n×n$ 的方阵，$n$是状态向量 $\boldsymbol{x}_t$ 的维数。
$\boldsymbol{B}_t$是 $n×m$ 的矩阵，$m$ 是控制向量 $\boldsymbol{u}_t$ 的维数。
随机变量 ${\boldsymbol{\varepsilon}}_t$ 是一个高斯随机向量，表示由于状态变化而引起的不确定性，维数与状态向量维数相同，均值为 $0$ ，方差 $R_t$
代入上面的正态分布定义式（1），有：

1. 测量概率（measurement probability，测量方程） $p(\boldsymbol{z}_t | \boldsymbol{x}_t)$ 也与带有高斯噪声的自变量呈线性关系：

其中 $\boldsymbol{C}_t$ 是 $k \times n$ 的矩阵，$k$ 是测量矩阵 $\boldsymbol{z}_t $ 的维数，向量 $\boldsymbol{\delta}_t$ 的分布是均值为 $0$ 、方差为 $\boldsymbol{Q}_t$ 的高斯分布，因此，同样地，测量概率(观测方程)有：

1. 初始置信度 $bel(\boldsymbol{x}_0)$ 必须是正态分布的，这里用$\boldsymbol{\mu}_0$ 表示置信度的均值， $\boldsymbol{\Sigma}_0$ 表示方差：

这三个假设足以确保后验 $bel(x_t)$ 在任何时刻t下总符合高斯分布。这个重要结果的证明可以在下面卡尔曼滤波器的数学推导中找到。（The proof of this non-trivial result can be found below, in the mathematical derivation of the Kalman filter）

卡尔曼滤波算法

Kalman filter algorithm，与前面的贝叶斯滤波十分相似。
输入是 $t-1$ 时刻的置信度，置信度的均值和方差分布用 $\boldsymbol{\mu}_{t-1}$ 和 $\boldsymbol{\Sigma}_{t-1}$ 表示，为了更新这两个参数，需要有控制向量（机器人动作） $\boldsymbol{u}_{t}$ 和测量向量 $\boldsymbol{z}_{t}$ ，输出为t时刻的置信度，同样用均值和方差表示。

Kalman filter algorithm

卡尔曼滤波的数学推导

第一部分：预测

首先回顾前面贝叶斯滤波博客中最终推导出的递归更新方程：

代入式（3），得到的式（7）仍然是一个高斯函数，具有均值 $\overline{\boldsymbol{\mu}}_t$ 和方差 $\overline{\boldsymbol{\Sigma}}_t$ ，现在将上式改写成高斯形式：

其中：

$L_t$ 是 $\boldsymbol{x}_{t-1}$ 的二次型，也是 $\boldsymbol{x}_{t}$ 的二次型。
现在，我们要求解式（8）中的积分。
对（9）式重新排序，目的是先提取出常数：

现在，我们要找到一个不依赖于 $\boldsymbol{x}_{t}$ 的 $L_t(\boldsymbol{x}_{t-1},\boldsymbol{x}_{t})$ ，使积分式变为一个常数，再把这个常数包含到前面的常数 $\eta$ 里面：

得到了一个更易于计算的高斯函数。

现在来尝试这个分解。
先计算 $L_t$ 的一二阶导数：

（为写作方便计，下面不再把 $\boldsymbol{x}$ 等向量写作粗体，笔者相信这并不会引起歧义）
$\varPsi_t$ 定义了 $L_t(x_{t-1},x_t)$ 的曲率，令 $L_t$ 的一阶导数为0，有：

现在定义函数 $L_t(x_{t-1},x_t)$ 如下：

函数 $L_t(x_{t-1},x_t)$ 是满足正态分布的负指数的常见二次型形式。下面看一个变量为 $x_{t-1}$ 的概率密度函数：

仔细看，这与本文开头的公式（1）具有相同的形式，它满足正态分布，它是一个概率密度函数。所以有：

注意到，这个积分值与目标变量 $x_t$ 无关，在计算 $x_t$ 的分布时，它是一个常数，按照上面论述的，我们将它归一到因子 $\eta$ 中去，代入上面的式（11）有：

这就是我们前面要找的式（12），其中上下两行的 $\eta$ 并不相同。
现在我们由式（9）、（10）、（17）来计算函数 $L_t(x_t)$ ：

下面，代入 $\varPsi = (A_t^T R_t^{-1} A_t + \Sigma_{t-1}^{-1})^{-1}$ 有：

所有 $x_{t-1}$ 项都消去了，这正是我们要找的 $L_t(x_t)$ ：

上式含有 $x_T$ 的二次项，这印证了 $\overline{bel}(x_t)$ 确实服从正态分布，该分布的均值和方差就是 $L_t(x_t)$ 的极小值和曲率，下面计算它的一阶导数和二阶导数：

下面，根据Sherman-Morrison公式（谢尔曼／莫里森公式，求逆引理(inversion lemma)），有：

看一下谢尔曼（Sherman）-莫里森（Morrison）公式的简单证明：
对于任意可逆二次矩阵R和Q以及任意具有适当维数的矩阵P，当下面的逆矩阵都存在时有：

证明如下：
定义 $\varPsi=(Q^{-1} + P^T R^{-1} P)^{-1}$，有：

证毕。

因此，回到式（25），有：

令一阶导数为0，可以得到 $L_t(x_t)$ 的极小值：

至此，我们求得了式（7）中的 $\overline{bel}(x_t)$ 的均值为 $B_t u_t + A_t\mu_{t-1}$，也就是上文卡尔曼滤波算法图中第二行的内容。
下面计算 $L_t(x_t)$ 的二阶导数：

这是二次函数 $L_t(x_t)$ 的曲率，其逆矩阵就是 $\overline{bel}(x_t)$ 的协方差。

第二部分：测量更新

下面继续推导。
回忆一下之前贝叶斯滤波中的 $bel(x_t)$ ，前面的博客，贝叶斯滤波推导的公式（36）：

由上面的式（5），有：

其中：

这个函数是 $x_t$ 的二次型，因此 $bel(x_t)$ 是一个高斯分布，要求解它的参数，那么仍然是计算一、二阶导数：

恰有 $bel(x_t)$ 协方差的逆：

$bel(x_t)$ 的均值是这个二次型函数的极小值，令 $J_t$ 的一阶导，式（35），等于0，有：

现在，定义 卡尔曼增益(Kalman gain) 为：

那么有：

注意到，现在定义的 $K_t$ 是关于 $\Sigma_t$ 的函数，这似乎与卡尔曼滤波算法中利用 $K_t$ 来计算 $\Sigma_t$ 有冲突，下面我们证明卡尔曼增益与 $\Sigma_t$ 无关：

OK。
基于上面的式（37）定义的卡尔曼增益，能避免计算状态方差阵的逆。
下面，再一次引用上面的式（27）Sherman-Morrison求逆公式，展开式（37），恰好代入卡尔曼增益：

与前文给出的算法一致，证毕。

扩展卡尔曼滤波

高斯随机变量的任何线性变换都将产生另一个高斯随机变量，例如有某个随机变量 $X$ 的分布满足 $X \sim N(x; \mu , \sigma ^2)$ ，那么它经过线性变换 $y=ax+b$后，产生的随机变量 $Y$ ，服从均值为 $a \mu +b$ ，方差为 $a^2 \sigma ^2$ 的高斯分布。上文的推导利用了这个特性，因此卡尔曼滤波要求系统是线性系统。

当然，我们知道这个纷繁复杂的世界中，线性是理想的，多数情况下系统并不是线性的，例如恒定线速度和角速度的移动机器人的运动轨迹是圆，它无法用线性状态方程来描述。

下面，我们假设状态转移概率（state transition probability，状态方程）和测量概率（measurement probability，观测方程）分别由非线性函数g和h控制，根据上文中的式（2）、（4），有：

函数 $g$ 替换了（2）式中的矩阵 $A_t$ 和 $B_t$ ， $h$ 替换了式（4）中的 $C_t$ ，对于任意函数g和h, 置信度不再满足一个高斯分布。

准确地实现置信度更新对于非线性函数g 和h 通常是不可能的，贝叶斯滤波不存在闭式解。

下面，我们引入扩展卡尔曼滤波（Extended Kalman Filter，EKF）来解决这个问题，基于 EKF 近似的主要思想就是线性化(linearization)，对非线性函数进行线性化有很多种方法，这里的扩展卡尔曼滤波使用的是一阶泰勒展开（first order Taylor expansion.）先复习一下泰勒展开：

同样地，我们写出状态转移概率（状态方程），即上面的（3）式：

其中 $G_t$ 是 $n \times n$ 矩阵，n是维数，常被称为雅可比矩阵（Jacobian），雅可比矩阵的值取决于 $u_t$ 和 $\mu_{t-1}$ ，因此不同时刻是不同的。
机器人认为t时刻最可能的状态是 $\overline \mu_t$ ，在此处将观测方程进行泰勒展开：

同样的，代入上面的式（5）有：

扩展卡尔曼滤波算法流程图，可以看到它与上面的卡尔曼滤波是很相似的。

扩展卡尔曼滤波算法

扩展卡尔曼滤波的数学推导

与上面的类似。

由前面的式（9）有：

参照上面的式（17），分解 $L_t$ ：

其中：

求得：

求导后可以注意到 $L_t(x_t)$ 的一阶导数为0，与前面的推导类似，可以得到 $\mu _t =g(u_t,\mu_{t-1})$和二阶导数 $(R_t+G_t \Sigma_{t-1} G_t^T)^{-1}$ 。下面推导观测方程，同样的，有：

继续有：

得到结果均值和方差为：

推导过程参照前文即可。