卡尔曼滤波（Kalman filtering）及相关基础

---

前言

参考书籍：
《视觉SLAM十四讲》
《机器人学中的状态估计》
知乎：如何通俗并尽可能详细地解释卡尔曼滤波？云羽落的回答
还需要一些高数、线性代数和概率论的基础。

本文主要是一些学习笔记与个人的总结，主要引用自上述的参考书籍中。

---

一、SLAM问题的数学表述

早期的SLAM问题是一个状态估计问题，被人们称为“空间状态不确定性估计”，正是如今的后端优化问题。这也反映了SLAM问题的本质：对运动主体自身和周围空间不确定的估计。为了解决这个问题，需要用到 状态估计 理论。而状态估计理论的经典代表就是以卡尔曼滤波为核心的 卡尔曼系列滤波器。

状态估计：通过初始状态、各时刻的观测数据、输入数据，估计系统的真实状态。

系统模型

运动主体携带各种传感器在未知环境中运动，主要由以下两个事情描述：
（假设为：考虑离散时间的线性时变系统）

1. 运动方程：$x_k=A_{k-1}{x}_{k-1}+v_k+w_k,k=1,\cdots ,K$
2. 观测方程：$y_k=C_k{x}_k+n_k,k=0,\cdots ,K$

除了$v_k$以外，其他变量均为随机变量；各个时刻的噪声互不相关；$A$称为转移矩阵；$C$称为观测矩阵。

其中各个变量含义：

• 系统状态：$x_k\in {\mathbb{R}}^N$ ; 输入：$v_k\in {\mathbb{R}}^N$
• 初始状态：$x_0\in {\mathbb{R}}^N\sim \mathcal{N}(\check{x_0},\check{P_0})$
• 过程噪声：$w_k\in {\mathbb{R}}^N\sim \mathcal{N}(0,Q_k)$
• 测量：$y_k\in {\mathbb{R}}^M$ ; 测量噪声：$n_k\in {\mathbb{R}}^{\mathbb{M}}\sim \mathcal{N}(0,R_k)$

除系统模型之外，我们还知道：
带下帽子的变量称为先验，上帽子的称为后验。

1. 初始状态$\check{x_0}$,以及它的初始协方差矩阵$\check{P_0}$。有时候不知道初始信息，必须在没初始信息的情况下进行推导.
2. 输入量$v_k$,通常来自控制器，是已知的；它的噪声协方差矩阵是$Q_k$.
3. 观测数据$y_{k,meas}$是观测变量$y_k$的一次实现（realization），它的协方差矩阵是$R_k$.

---

二、离散时间线性高斯系统批量式状态估计

1.最大后验估计（Maximum A Posteriori, MAP）

MAP:已知输入和观测，求最大概率的状态
$$\hat{x}=\underset{x}{\mathrm{argmax}}p(x|v,y)$$
用贝叶斯公式重写目标函数：
$$\hat{x}=\underset{x}{\mathrm{argmax}}p(x|v,y)=\underset{x}{\mathrm{argmax}}\frac{p(y|x,v)p(x|v)}{p(y|v)}=\underset{x}{\mathrm{argmax}}p(y|x)p(x|v)$$
在这里，需要把不带下标的变量定义为宏观变量：
$$\begin{gathered} x=x_{0:K}=(x_0,\cdots ,x_K) \\ v=(\check{x_0},v_{1:K})=(\check{x},v_1,\cdots ,v_K) \\ y=y_{0:K}=(y_0,\cdots ,y_K) \end{gathered}$$
由于各时刻观测、输入的噪声都是无关的，上面两个项可以因式分解：
$$\begin{gathered} p(y|x)=\prod _{k=0}^{K}p(y_k|x_k) \\ p(x|v)=p(x_0|\check{x_0})\prod _{k=1}^{K}p(x_k|x_{k-1},v_k) \end{gathered}$$
同时，对目标函数两边取对数，对数是个单调映射，不影响最优解：
$$\ln \left(p(y|x)p(x|v)\right)=\ln p(x_0|\check{x_0})+\sum _{k=1}^K\ln p(x_k|x_{k-1},v_k)+\sum _{k=0}^K\ln p(y_k|x_k)$$
这时因子相乘变成了对数项相加。
高斯分布取对数之后有较好的形式：
$$\begin{gathered} \ln p(x_0|\check{x_0})=-\frac{1}{2}(x_0-\check{x_0}{)}^T{\check{P}}_0^{-1}(x-\check{x_0}) \\ -\frac{1}{2}\ln \left((2\pi {)}^N\det {\check{P}}_0\right) \end{gathered}$$$$\begin{gathered} \ln p(x_k|x_{k-1},v_k)=-\frac{1}{2}(x_k-A_{k-1}{x}_{k-1}-v_k{)}^T{Q}_k^{-1}(x_k-A_{k-1}{x}_{k-1}-v_k) \\ -\frac{1}{2}\ln \left((2\pi {)}^N\det Q_k\right) \end{gathered}$$$$\begin{gathered} \ln p(y_k|x_k)=-\frac{1}{2}(y_k-C_k{x}_k{)}^T{R}_k^{-1}(y_k-C_k{x}_k) \\ -\frac{1}{2}\ln \left((2\pi {)}^N\det R_k\right) \end{gathered}$$
舍掉那些与$x$无关的项，定义：
$$J_{v,k}(x)=\{\begin{array}{ccc}& \frac{1}{2}(x_0-\check{x_0}{)}^T{\check{P}}_0^{-1}(x-\check{x_0}),& k=0\\ & \frac{1}{2}(x_k-A_{k-1}{x}_{k-1}-v_k{)}^T{Q}_k^{-1}(x_k-A_{k-1}{x}_{k-1}-v_k),& k=1,\cdots ,K\end{array}$$$$\begin{array}{cc}{J}_{y,k}(x)=\frac{1}{2}(y_k-C_k{x}_k{)}^T{R}_k^{-1}(y_k-C_k{x}_k),& k=0,\dots ,K\end{array}$$
于是目标函数变为求这个式的最小化：
$$\{\begin{array}{c}\hat{x}=\underset{x}{\mathrm{argmin}}J(x)\\ J(x)={\sum }_{k=0}^K\left(J_{v,k}(x)+J_{y,k}(x)\right)\end{array}$$
这个问题就是常见的无约束最小二乘。

写成更紧凑的矩阵形式（提升形式）：
$$z=\left[\begin{array}{c}\check{x_0}\\ v_1\\ ⋮\\ v_K\\ y_0\\ y_1\\ ⋮\\ y_K\end{array}\right],x=\left[\begin{array}{c}{x}_0\\ ⋮\\ x_K\end{array}\right],H=\left[\begin{array}{cccc}1& & & \\ -A_0& 1& & \\ & \ddots & \ddots & \\ & & -A_{K-1}& 1\\ C_0& & & \\ & C_1& & \\ & & \ddots & \\ & & & C_K\end{array}\right]$$$$W=\left[\begin{array}{cccccccc}\check{P_0}& & & & & & & \\ & Q_1& & & & & & \\ & & \ddots & & & & & \\ & & & Q_K& & & & \\ & & & & R_0& & & \\ & & & & & R_1& & \\ & & & & & & \ddots & \\ & & & & & & & R_K\end{array}\right]$$把运动和观测写在一起：
$$z=Hx+W$$
提升形式的目标函数：$$J(x)=\frac{1}{2}(z-Hx{)}^T{W}^{-1}(z-Hx)$$它是个二次的，求其最小值，只要令自变量导数为零：
$$\begin{gathered} \frac{\partial J(x)}{\partial x^T}{\mid }_{\hat{x}}=-H^T{W}^{-1}(z-H\hat{x})=0 \\ \Rightarrow (H^T{W}^{-1}H)\hat{x}=H^T{W}^{-1}z \end{gathered}$$
于是就解析地得到了最优解

• 这个解等价于经典的批量最小二乘法，也等价于固定区间平滑算法，或者也等价于伪逆
• 由于$H$具有特殊的稀疏结构，这个问题也有特殊的解法，不需要暴力算矩阵求逆

2.贝叶斯推断（Bayesian Inference）

在线性高斯系统中，可以根据运动方程和观测方程显式写出状态变量分布的变化过程：
运动方程：

• k时刻状态更新：$x_k=A_{k-1}{x}_{k-1}+v_k+w_k$
• 提升形式：$x=A(v+w)$
  其中$A=\left[\begin{array}{cccccc}1& & & & & \\ A_0& 1& & & & \\ A_1{A}_0& A_1& 1& & & \\ ⋮& ⋮& ⋮& \ddots & & \\ A_{k-2}\cdots A_0& A_{k-2}\cdots A_1& A_{k-2}\dots A_2& \dots & 1& \\ A_{k-1}\cdots A_0& A_{k-2}\cdots A_0& A_{k-2}\cdots A_1& A_{k-2}\dots A_2& \dots & 1\end{array}\right]$

在上式中，右侧只有$v，w$,故容易求得均值和协方差($v$为确定的输入量，$w$是高斯，所以此处也是高斯分布的线性变换)：

• 均值：$\check{x}=\text{E}[x]=\text{E}[A(v+w)]=Av$
• 协方差：$\check{P}=\text{E}[(x-\text{E}[x])(x-\text{E}[x]{)}^T]=\text{E}[(x-Av)(x-Av{)}^T]=AQ{A}^T$

所以，先验部分写作：$p(x|v)=\mathcal{N}(\check{x},\check{P})=\mathcal{N}(Av,AQ{A}^T)$
此处先验的意思为：仅考虑运动方程时的条件概率分布

观测模型：

• 单次观测：$y_k=C_k{x}_k+n_k$
• 提升形式：$y=Cx+n$
  其中$C=diag(C_0,C_1,\dots ,C_k)$

联合分布：
(由于已知$v$时，$x$的先验分布已经确定，所以$y$的分布也可确定)
$$p(x,y|v)=\mathcal{N}\left(\left[\begin{array}{c}\check{x}\\ C\check{x}\end{array}\right],\left[\begin{array}{cc}\check{P}& \check{P}{C}^T\\ C\check{P}& C\check{P}{C}^T+R\end{array}\right]\right)$$

条件分布：
下面是已知联合分布，求条件分布。
联合=条件*边缘
$$\begin{gathered} p(x,y|v)=p(x|v,y)p(y|v) \\ =\mathcal{N}\left(\left[\begin{array}{c}\check{x}\\ C\check{x}\end{array}\right],\left[\begin{array}{cc}\check{P}& \check{P}{C}^T\\ C\check{P}& C\check{P}{C}^T+R\end{array}\right]\right) \end{gathered}$$

由下列高斯推断：

• 联合分布：$p(x,y)=p(x|y)p(y)$
• 条件分布：$p(x|y)=\mathcal{N}({\mu }_x+{\Sigma }_{xy}{\Sigma }_{yy}^{-1}(y-{\mu }_y),{\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{yx})$
• 边缘分布：$p(y)=\mathcal{N}({\mu }_y,{\Sigma }_{yy})$

代入得：$$p(x|v,y)=\mathcal{N}\left(\check{x}+\check{P}{C}^T(C\check{P}{C}^T+R{)}^{-1}(y-C\check{x}),\check{P}-\check{P}{C}^T(C\check{P}{C}^T+R{)}^{-1}C\check{P}\right)$$

SMW式：

• $AB(D+CAB{)}^{-1}=(A^{-1}+B{D}^{-1}C{)}^{-1}B{D}^{-1}$
• $(A^{-1}+B{D}^{-1}C{)}^{-1}=A-AB(D+CAB{)}^{-1}CA$

故化简可得：
$$p(x|v,y)=\mathcal{N}\left(({\check{P}}^{-1}+C^T{R}^{-1}C{)}^{-1}({\check{P}}^{-1}\check{x}+C^T{R}^{-1}y),({\check{P}}^{-1}+C^T{R}^{-1}C{)}^{-1}\right)$$

• $({\check{P}}^{-1}+C^T{R}^{-1}C{)}^{-1}({\check{P}}^{-1}\check{x}+C^T{R}^{-1}y)$即均值$\hat{x}.$
• $({\check{P}}^{-1}+C^T{R}^{-1}C{)}^{-1}$ 即后验协方差 $\hat{P}$.

进一步整理：
均值部分：$({\check{P}}^{-1}+C^T{R}^{-1}C)\hat{x}={\check{P}}^{-1}\check{x}+C^T{R}^{-1}y$
代入$\check{x}=Av$和${\check{P}}^{-1}=(AQ{A}^T{)}^{-1}={(A^{-1})}^T{Q}^{-1}{A}^{-1}$,
得：$({(A^{-1})}^T{Q}^{-1}{A}^{-1}+C^T{R}^{-1}C)\hat{x}={(A^{-1})}^T{Q}^{-1}v+C^T{R}^{-1}y$

这里由于A为下三角矩阵，故A逆有特殊的形式：
$$A^{-1}=\left[\begin{array}{cccccc}1& & & & & \\ -A_0& 1& & & & \\ & -A_1& 1& & & \\ & & -A_2& \ddots & & \\ & & & \ddots & 1& \\ & & & & -A_{k-1}& 1\end{array}\right]$$

得出一些结论：

• 按均值式：${\hat{P}}^{-1}\hat{x}=({(A^{-1})}^T{Q}^{-1}{A}^{-1}+C^T{R}^{-1}C)\hat{x}={(A^{-1})}^T{Q}^{-1}v+C^T{R}^{-1}y$
• 定义:$z=\left[\begin{array}{c}v\\ y\end{array}\right],H=\left[\begin{array}{c}{A}^{-1}\\ C\end{array}\right],W=\left[\begin{array}{cc}Q& \\ & R\end{array}\right]$
• 得到：$(H^T{W}^{-1}H)\hat{x}=H^T{W}^{-1}z$

与MAP结果一致。

线性高斯系统的最优估计显然有以下要求：

• $(H^T{W}^{-1}H)\hat{x}=H^T{W}^{-1}z\iff \hat{x}=(H^T{W}^{-1}H{)}^{-1}{H}^T{W}^{-1}z$
• 即要求$(H^T{W}^{-1}H)\hat{x}$可逆，$\text{rank}(H^T{W}^{-1}H)=N(k+1)$
• 协方差矩阵的对称正定性要求：$\text{rank}(H^{T}H)=\text{rank}(H^T)=N(k+1)$

---

三、离散时间线性高斯系统递归式平滑算法

1.平滑算法的引出——Cholesky解法

2.Rauch-Tung-Striebel平滑算法（RTS Smoother）

---

四、离散时间线性高斯系统的滤波算法

卡尔曼滤波（Kalman filtering）

---

总结

• 卡尔曼滤波器给出了线性高斯系统下最优线性无偏估计（Best Linear Unbiased Estimate, BLUE）
• 卡尔曼滤波器依赖于初始状态
• 卡尔曼滤波器即Rauch-Tung-Striebel平滑算法（RTS Smoother）的前向部分
• 在线性高斯系统中，最大后验估计（Maximum A Posteriori, MAP）和贝叶斯推断（Bayesian Inference）的结果等价于卡尔曼滤波器，原因是高斯分布的均值和模在同一点上
• 在非线性系统中，可以使用扩展卡尔曼滤波器（EKF），此时最大后验估计（Maximum A Posteriori, MAP）、贝叶斯推断（Bayesian Inference）、扩展卡尔曼滤波器（EKF）估计的结果均不一样

该处使用的url网络请求的数据。