卡尔曼滤波器推导

Kalman Filter

1、卡尔曼滤波器

首先卡尔曼滤波器所要解决的问题—状态估计出发，状态估计过程即SLAM过程可以由运动方程和观测方程来描述，假设从$t=0$时刻到$t=n$时刻，有系统状态${\mathbf{x}}_0,{\mathbf{x}}_1,\cdots ,{\mathbf{x}}_n$，路标${\mathbf{y}}_1,\cdots ,{\mathbf{y}}_m$，则上述两方程的基本形式为
$$\{\begin{array}{ll}{\mathbf{x}}_k=\mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{u}}_k,{\mathbf{w}}_k)& k=1,...,n\\ {\mathbf{z}}_{k,j}=\mathbf{h}({\mathbf{y}}_j,{\mathbf{x}}_k,{\mathbf{v}}_{k,j})& (k,j)\in O\end{array}$$
其中${\mathbf{w}}_k$为运动过程产生的噪声，${\mathbf{u}}_k$是传感器读数或输入，$O$是一个集合，记录在哪个时刻观察到了哪些路标（通常不是每个路标在所有时刻都可见，在单个时刻通常只能看到其中一小部分），${\mathbf{v}}_{k,j}$是本次观测产生的噪声。由于系统状态和路标点都是待估计量，即要解决定位（估计系统状态）和建图（估计路标点）问题，故可将二者统一为一个未知量
$${\mathbf{x}}_k\stackrel{\mathrm{def}}{=}[{\mathbf{x}}_k^T,{\mathbf{y}}_1^T,\cdots ,{\mathbf{y}}_m^T{]}^T$$
重新整理运动方程和观测方程，函数符号保持不变，可将二者写得更简洁
$$\{\begin{array}{l}{\mathbf{x}}_k=\mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{u}}_k,{\mathbf{w}}_k)\\ {\mathbf{z}}_k=\mathbf{h}({\mathbf{x}}_k,{\mathbf{v}}_k)\end{array}k=1,...,n\text{\hspace{1em}(1)}$$
从最简单的线性高斯系统开始，即假设为运动方程与观测方程为线性（等价地，可以忽略常数项，和输入前的与其相乘的矩阵，因为这些量均可归入输入，在数学上没有影响），噪声服从高斯分布，并以加法的形式出现（考虑到方程的线性性）（此时同样称观测方程中不带噪声的部分为观测方程）。此时，该方程组具有如下形式
$$\{\begin{array}{l}{\mathbf{x}}_k={\mathbf{A}}_k{\mathbf{x}}_{k-1}+{\mathbf{u}}_k+{\mathbf{w}}_k\\ {\mathbf{z}}_k={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{v}}_k\end{array}k=1,...,n\text{\hspace{1em}(2)}$$
严格地写出上式所涉及的量的维数，${\mathbf{x}}_k\in {\mathbb{R}}^N$为系统状态，其初始状态为${\mathbf{x}}_0\in {\mathbb{R}}^N\sim \mathcal{N}({\hat{\mathbf{x}}}_0,{\hat{\mathbf{P}}}_0)$，状态递推的过程噪声为${\mathbf{w}}_k\in {\mathbb{R}}^N\sim \mathcal{N}(0,{\mathbf{R}}_k)$。输入${\mathbf{u}}_k\in {\mathbb{R}}^N$为确定性变量，有时也将输入写为${\mathbf{B}}_k{\mathbf{u}}_k$，因输入是确定性变量，故二者等价。测量量为${\mathbf{z}}_k\in {\mathbb{R}}^{M_k}$，测量噪声为${\mathbf{v}}_k\in {\mathbb{R}}^{M_k}\sim \mathcal{N}(0,{\mathbf{Q}}_k)$。${\mathbf{A}}_k\in {\mathbb{R}}^{N\times N}$称为状态转移矩阵，${\mathbf{C}}_k\in {\mathbb{R}}^{M_k\times N}$称为观测矩阵。

设计滤波器的目的是求后验概率分布$p({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k})$。

欲求此分布，可依据贝叶斯法则对其进行如下转换
$$\begin{array}{rl}p({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k})& =\frac{p({\mathbf{x}}_k,{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k})}{p({\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k})}\\ & =\frac{p({\mathbf{z}}_k|{\mathbf{x}}_k,{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})p({\mathbf{x}}_k,{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})}{p({\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k})}\\ & =\frac{p({\mathbf{z}}_k|{\mathbf{x}}_k,{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})p({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})p({\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})}{p({\mathbf{z}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})p({\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})}\\ & =\frac{p({\mathbf{z}}_k|{\mathbf{x}}_k,{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})p({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})}{p({\mathbf{z}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})}\end{array}$$
其中$p({\mathbf{z}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})$的取值不受${\mathbf{x}}_k$影响，为定值，因为由${\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}$可以完全确定出${\mathbf{z}}_k$的分布。故关于${\mathbf{x}}_k$的函数$p({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k})$可以写为
$$p({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k})=\eta \ast p({\mathbf{z}}_k|{\mathbf{x}}_k,{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})p({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})\text{\hspace{1em}(3)}$$
即其形式完全由似然$p({\mathbf{z}}_k|{\mathbf{x}}_k,{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})$和先验置信度$p({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})$决定，求出二者形式即可得到$p({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k})$。

对于先验，以${\mathbf{x}}_{k-1}$为条件做全概率展开
$$p({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})=\int p({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})p({\mathbf{x}}_{k-1}|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})d{\mathbf{x}}_{k-1}$$
实际上，上述的贝叶斯转换并没有假设系统的具体形式。注意到将上面的积分式第二个因子中条件${\mathbf{u}}_k$和${\mathbf{x}}_{k-1}$无关，故在该条件去掉，并积分式代入$(3)$，得到求解后验概率分布的公式
$$p({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k})=\eta \ast p({\mathbf{z}}_k|{\mathbf{x}}_k,{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})\int p({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})p({\mathbf{x}}_{k-1}|{\mathbf{x}}_0,{\mathbf{u}}_{1:k-1},{\mathbf{z}}_{1:k-1})d{\mathbf{x}}_{k-1}\text{\hspace{1em}(4)}$$
此公式称贝叶斯滤波器，因为该方法可以减少或剔除数据中的噪声部分，从而保持信号的真实特征，这与传统的滤波器去除信号中某一频率范围内噪声的功能类似，故称为“滤波器”，但二者的主要区别在于贝叶斯滤波器是基于概率和统计理论进行的高级滤波操作。卡尔曼滤波器是贝叶斯滤波器在线性高斯系统下的实现，其它不仅可以滤波（处理当前信号），还可以结合系统动态模型和之前的状态估计对未来状态进行预测。

因为${\mathbf{x}}_0\sim \mathcal{N}({\hat{\mathbf{x}}}_0,{\hat{\mathbf{P}}}_0)$，故不妨假设从${\mathbf{x}}_{k-1}$递推得到的状态${\mathbf{x}}_k$和根据状态${\mathbf{x}}_k$得到的观测${\mathbf{z}}_k$仍服从正态分布，故由观测方程得出
$$p({\mathbf{z}}_k|{\mathbf{x}}_k,{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})=p({\mathbf{z}}_k|{\mathbf{x}}_k)=\mathcal{N}({\mathbf{C}}_{\mathbf{k}}{\mathbf{x}}_k,{\mathbf{Q}}_k)\text{\hspace{1em}(5)}$$
由运动方程得出
$$\begin{array}{rl}p({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})& =p({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{u}}_k)=\mathcal{N}({\mathbf{A}}_k{\mathbf{x}}_{k-1}+{\mathbf{u}}_k,{\mathbf{R}}_{\mathbf{k}})\end{array}$$
并定义
$$p({\mathbf{x}}_{k-1}|{\mathbf{x}}_0,{\mathbf{u}}_{1:k-1},{\mathbf{z}}_{1:k-1})=\mathcal{N}({\hat{\mathbf{x}}}_{k-1},{\hat{\mathbf{P}}}_{k-1})$$
根据§1，可以得到
$$p({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})=\mathcal{N}(\mathbf{A}{\hat{\mathbf{x}}}_{k-1}+{\mathbf{u}}_k,{\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^T+{\mathbf{R}}_k)\text{\hspace{1em}(6)}$$
上式也可以看成已知${\mathbf{x}}_{k-1}$的后验均值${\hat{\mathbf{x}}}_{k-1}$和后验协方差${\hat{\mathbf{P}}}_{k-1}$，据此对${\mathbf{x}}_k$进行先验估计，得到其先验均值${\check{\mathbf{x}}}_k$和先验协方差${\check{\mathbf{P}}}_k$，即
$${\check{\mathbf{x}}}_k=\mathbf{A}{\hat{\mathbf{x}}}_{k-1}+{\mathbf{u}}_k,{\check{\mathbf{P}}}_k={\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^T+{\mathbf{R}}_k$$
故可以$(4)$即先验置信度可以写成
$$p({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1})=\mathcal{N}({\check{\mathbf{x}}}_k,{\check{\mathbf{P}}}_k)\text{\hspace{1em}(7)}$$
该过程也称为预测。通过$(3)$式可以看到，预测阶段完成后，还须要通过观测数据${\mathbf{z}}_k$完成更新（或称为校正），得到后验置信度。

将$(5),(7)$代入$(3)$，并将等号右边展开得到
$$p({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k})={\eta }^{\prime }\ast \exp \left(({\mathbf{z}}_k-{\mathbf{C}}_k{\mathbf{x}}_k{)}^T{\mathbf{Q}}_k({\mathbf{z}}_k-{\mathbf{C}}_k{\mathbf{x}}_k)\right)\exp \left(({\mathbf{x}}_k-{\check{\mathbf{x}}}_k{)}^T{\check{\mathbf{P}}}_k({\mathbf{x}}_k-{\check{\mathbf{x}}}_k)\right)$$
其中${\eta }^{\prime }$是归一化常数。

根据§2，可以知道后验置信度服从高斯分布，记为
$$p({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k})=\mathcal{N}({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k)$$
且其后验均值和后验协方差满足
$$\begin{array}{rl}{\hat{\mathbf{P}}}_k^{-1}& ={\mathbf{C}}_k^T{\mathbf{Q}}_k^{-1}{\mathbf{C}}_k+{\check{\mathbf{P}}}_k^{-1}\\ {\hat{\mathbf{P}}}_k^{-1}{\hat{\mathbf{x}}}_k& ={\mathbf{C}}_k^T{\mathbf{Q}}_k^{-1}{\mathbf{z}}_k+{\check{\mathbf{P}}}_k^{-1}{\check{\mathbf{x}}}_k\end{array}\text{\hspace{1em}(8)(9)}$$
要求后验均值和后验协方差，需要§3的Sherman-Morrison-Woodbury恒等式变形1，将$(8)$等号两侧求逆即可得到恒等式中的形式，注意到多元正态分布要求其协方差矩阵为正定矩阵，可以验证其对应矩阵满足可逆条件。故有
$${\hat{\mathbf{P}}}_k={\check{\mathbf{P}}}_k-{\check{\mathbf{P}}}_k{\mathbf{C}}_k^T({\mathbf{Q}}_k+{\mathbf{C}}_k{\check{\mathbf{P}}}_k{\mathbf{C}}_k^T{)}^{-1}{\mathbf{C}}_k{\check{\mathbf{P}}}_k$$
定义中间变量，即卡尔曼增益
$${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{C}}_k^T({\mathbf{Q}}_k+{\mathbf{C}}_k{\check{\mathbf{P}}}_k{\mathbf{C}}_k^T{)}^{-1}\text{\hspace{1em}(10)}$$
故
$${\hat{\mathbf{P}}}_k=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k){\check{\mathbf{P}}}_k\text{\hspace{1em}(11)}$$
为化简$(9)$，对$(10)$应用§3的Sherman-Morrison-Woodbury恒等式变形3和$(8)$，得到
$$\begin{array}{r}{\mathbf{K}}_k={\hat{\mathbf{P}}}_k{\mathbf{C}}_k^T{\mathbf{Q}}_k^{-1}\end{array}\text{\hspace{1em}(12)}$$
化简$(9)$，并将$(10),(12)$代入，得到
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\hat{\mathbf{P}}}_k{\mathbf{C}}_k^T{\mathbf{Q}}_k^{-1}{\mathbf{z}}_k+{\hat{\mathbf{P}}}_k{\check{\mathbf{P}}}_k^{-1}{\check{\mathbf{x}}}_k\\ & ={\mathbf{K}}_k{\mathbf{z}}_k+(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k){\check{\mathbf{P}}}_k{\check{\mathbf{P}}}_k^{-1}{\check{\mathbf{x}}}_k\\ & ={\mathbf{K}}_k{\mathbf{z}}_k+(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k){\check{\mathbf{x}}}_k\\ & ={\check{\mathbf{x}}}_k+{\mathbf{K}}_k({\mathbf{z}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_k)\end{array}\text{\hspace{1em}(13)}$$
至此求出了后验高斯概率分布的具体形式，因此可以继续迭代下去。上面的过程可以归纳为“预测”和“更新”两个步骤：

---

1. 预测：
   • ${\check{\mathbf{x}}}_k=\mathbf{A}{\hat{\mathbf{x}}}_{k-1}+{\mathbf{u}}_k,{\check{\mathbf{P}}}_k={\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^T+{\mathbf{R}}_k$
2. 更新：
   • 先计算卡尔曼增益${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{C}}_k^T({\mathbf{Q}}_k+{\mathbf{C}}_k{\check{\mathbf{P}}}_k{\mathbf{C}}_k^T{)}^{-1}$
   • 然后计算后验均值${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k({\mathbf{z}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_k)$和后验协方差${\hat{\mathbf{P}}}_k=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k){\check{\mathbf{P}}}_k$，即得到后验高斯概率分布${\mathbf{x}}_k\sim \mathcal{N}({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k)$

---

2、扩展卡尔曼滤波器

回到$(1)$式，实际上大部分的运动方程和观测方程是非线性的，即实际系统是非线性高斯系统。对非线性系统的运动方程和观测方程进行线性化，然后将线性系统中推导出的卡尔曼滤波器的结论应用于非线性系统中的算法，称为扩展卡尔曼滤波器，与此相对，前述在线性高斯系统推出的卡尔曼滤波器也称为经典卡尔曼滤波器。

根据§4，将$(1)$中的运动方程在$({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k,0)$处做一阶泰勒展开
$$\begin{array}{rl}{\mathbf{x}}_k=\mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{u}}_k,{\mathbf{w}}_k)\approx & \mathbf{f}({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k,0)+{\frac{\partial \mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{u}}_k,{\mathbf{w}}_k)}{\partial {\mathbf{x}}_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k,0}({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1})\\ & +{\frac{\partial \mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{u}}_k,{\mathbf{w}}_k)}{\partial {\mathbf{w}}_k}|}_{{\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k,0}{\mathbf{w}}_k\end{array}$$
注意，原运动方程中的输入是自变量，而展开处的输入是具体的值，为简便起见，采用改写发法，后文某些量同理。还应注意到，无论实际输入是什么，均在其上展开，故输入对应的泰勒展开项为$0$，下同。考虑到${\check{\mathbf{x}}}_k=\mathbf{f}({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k,0)$，且记第二项中对系统状态的偏导数矩阵为${\mathbf{F}}_k$，记第三项中经线性变换后的过程噪声为${\mathbf{w}}_k^{\prime }\sim \mathcal{N}(0,{\mathbf{R}}_k^{\prime })$，故上式可以写为
$$\begin{array}{rl}{\mathbf{x}}_k& \approx {\check{\mathbf{x}}}_k+{\mathbf{F}}_k({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1})+{\mathbf{w}}_k^{\prime }\end{array}\text{\hspace{1em}(14)}$$
注意此式中，${\mathbf{x}}_{k-1},{\mathbf{w}}_k^{\prime }$为不确定的变量，其余量为可确定的已知量。

将$(1)$中的观测方程在$({\check{\mathbf{x}}}_k,0)$处做一阶泰勒展开
$${\mathbf{z}}_k=\mathbf{h}({\mathbf{x}}_k,{\mathbf{v}}_k)\approx \mathbf{h}({\check{\mathbf{x}}}_k,0)+{\frac{\partial \mathbf{h}({\mathbf{x}}_k,{\mathbf{v}}_k)}{\partial {\mathbf{x}}_k}|}_{{\check{\mathbf{x}}}_k,0}({\mathbf{x}}_k-{\check{\mathbf{x}}}_k)+{\frac{\partial \mathbf{h}({\mathbf{x}}_k,{\mathbf{v}}_k)}{\partial {\mathbf{v}}_k}|}_{{\check{\mathbf{x}}}_k,0}{\mathbf{v}}_k$$
记第二项中对系统状态的偏导数矩阵为${\mathbf{H}}_k$，记第三项中经线性变换后的过程噪声为${\mathbf{v}}_k^{\prime }\sim \mathcal{N}(0,{\mathbf{Q}}_k^{\prime })$，故上式可以写为
$$\begin{array}{rl}{\mathbf{z}}_k& \approx \mathbf{h}({\check{\mathbf{x}}}_k,0)+{\mathbf{H}}_k({\mathbf{x}}_k-{\check{\mathbf{x}}}_k)+{\mathbf{v}}_k^{\prime }\end{array}\text{\hspace{1em}(15)}$$
注意此式中，${\mathbf{x}}_k,{\mathbf{v}}_k^{\prime }$为不确定的变量，其余量为可确定的已知量。

得出观测方程和运动方程的线性近似后，仍根据贝叶斯滤波求系统状态的后验概率分布，具体方法是将$(14),(15)$代入$(3),(4)$求解（不可将$(14),(15)$直接代入线性卡尔曼滤波的结论，因为二者不能完全符合$(2)$​的形式）。此过程同样可以归纳为“预测”和“更新”两个步骤：

---

1. 预测：

   • ${\check{\mathbf{x}}}_k=\mathbf{f}({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k,0),{\check{\mathbf{P}}}_k={\mathbf{F}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_k^T+{\mathbf{R}}_k^{\prime }$

2. 更新：

   • 先计算卡尔曼增益${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{H}}_k^T({\mathbf{Q}}_k^{\prime }+{\mathbf{H}}_k{\check{\mathbf{P}}}_k{\mathbf{H}}_k^T{)}^{-1}$

   • 然后计算后验均值${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k({\mathbf{z}}_k-\mathbf{h}({\check{\mathbf{x}}}_k,0))$和后验协方差${\hat{\mathbf{P}}}_k=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\check{\mathbf{P}}}_k$，即得到后验高斯概率分布${\mathbf{x}}_k\sim \mathcal{N}({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k)$

---

附录

§1、高斯分布随机变量的带噪声变换的分布

若$p(\mathbf{x})=\mathcal{N}({\mathbf{\mu }}_x,{\mathbf{\Sigma }}_{xx})$，略写维数，其带噪声变换为$\mathbf{y}=\mathbf{g}(\mathbf{x})+\mathbf{\delta }$，$\mathbf{\delta }\sim \mathcal{N}(0,\mathbf{R})$，则在$\mathbf{x}$条件下$\mathbf{y}$的分布为
$$p(\mathbf{y}|\mathbf{x})=\mathcal{N}(\mathbf{g}(\mathbf{x}),\mathbf{R})\text{\hspace{1em}(A1)}$$
在该条件下做全概率展开
$$p(\mathbf{y})=\int p(\mathbf{y}|\mathbf{x})p(\mathbf{x})d\mathbf{x}\text{\hspace{1em}(A2)}$$
然而当$\mathbf{g}(\cdot )$为非线性时变换，难以得到上式的闭式解，故需要对非线性变换做线性化处理（见附录§4），
$$\mathbf{g}(\mathbf{x})\approx {\mathbf{\mu }}_y+\mathbf{G}(\mathbf{x}-{\mathbf{\mu }}_x)\text{\hspace{1em}(A3)}$$
其中${\mathbf{G}}_{ij}={\frac{\partial f_i(\mathbf{x})}{\partial x_j}|}_{{\mathbf{\mu }}_x},{\mathbf{\mu }}_y=\mathbf{g}({\mathbf{\mu }}_x)$。将$(\mathrm{A}3)$代入$(\mathrm{A}1)$，再将$(\mathrm{A}1)$代入$(\mathrm{A}2)$，可得
$$p(\mathbf{y})=\mathcal{N}({\mathbf{\mu }}_y,{\mathbf{\Sigma }}_{yy})=\mathcal{N}(\mathbf{g}({\mathbf{\mu }}_x),\mathbf{R}+\mathbf{G}{\mathbf{\Sigma }}_{xx}{\mathbf{G}}^T)$$
§2、高斯概率密度函数的归一化积
$${\eta }_1\exp \left(-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }){\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })\right)\equiv {\eta }_2\prod _{k=1}^K\exp \left(-\frac{1}{2}({\mathbf{A}}_k\mathbf{x}-{\mathbf{\mu }}_k){\mathbf{\Sigma }}_k^{-1}({\mathbf{A}}_k\mathbf{x}-{\mathbf{\mu }}_k)\right)$$
其中
$$\begin{array}{rl}{\mathbf{\Sigma }}^{-1}& =\sum _{k=1}^K{\mathbf{A}}_k^T{\mathbf{\Sigma }}_k^{-1}{\mathbf{A}}_k\\ {\mathbf{\Sigma }}^{-1}\mathbf{\mu }& =\sum _{k=1}^K{\mathbf{A}}_k^T{\mathbf{\Sigma }}_k^{-1}{\mathbf{\mu }}_k\end{array}$$
${\mathbf{A}}_k\in {\mathbb{R}}^{M_k\times N}$，${\eta }_1,{\eta }_2$为归一化常数，保证等号两侧积分为$1$

§3、Sherman-Morrison-Woodbury恒等式（矩阵求逆引理）

若$\mathbf{A}$可逆，$\mathbf{D}$可逆，$(\mathbf{D}+\mathbf{CAB})$可逆，$({\mathbf{A}}^{-1}+\mathbf{B}{\mathbf{D}}^{-1}\mathbf{C})$可逆，则
$$\begin{array}{rl}({\mathbf{A}}^{-1}+\mathbf{B}{\mathbf{D}}^{-1}\mathbf{C}{)}^{-1}& \equiv \mathbf{A}-\mathbf{AB}(\mathbf{D}+\mathbf{CAB}{)}^{-1}\mathbf{CA}\\ (\mathbf{D}+\mathbf{CAB}{)}^{-1}& \equiv {\mathbf{D}}^{-1}-{\mathbf{D}}^{-1}\mathbf{C}({\mathbf{A}}^{-1}+\mathbf{B}{\mathbf{D}}^{-1}\mathbf{C}{)}^{-1}\mathbf{B}{\mathbf{D}}^{-1}\\ \mathbf{AB}(\mathbf{D}+\mathbf{CAB}{)}^{-1}& \equiv ({\mathbf{A}}^{-1}+\mathbf{B}{\mathbf{D}}^{-1}\mathbf{C}{)}^{-1}\mathbf{B}{\mathbf{D}}^{-1}\\ (\mathbf{D}+\mathbf{CAB}{)}^{-1}\mathbf{CA}& \equiv {\mathbf{D}}^{-1}\mathbf{C}({\mathbf{A}}^{-1}+\mathbf{B}{\mathbf{D}}^{-1}\mathbf{C}{)}^{-1}\end{array}$$
参考：《机器人学中的状态估计》，《视觉SLAM十四讲：从理论到实践》