python 卡尔曼滤波器 gprs

卡尔曼滤波（Kalman filtering）基础

• 前言
• 一、SLAM问题的数学表述

• 系统模型

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

• 1.最大后验估计（Maximum A Posteriori, MAP）
• 2.贝叶斯推断（Bayesian Inference）

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

• 1.平滑算法的引出——Cholesky解法
• 2.Rauch-Tung-Striebel平滑算法（RTS Smoother）

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

• 卡尔曼滤波（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^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}{\argmax}p(x|v,y)$
用贝叶斯公式重写目标函数：
$\hat x = \underset{x}{\argmax}p(x|v,y) = \underset{x}{\argmax} \frac{p(y|x,v)p(x|v)}{p(y|v)} =\underset{x}{\argmax} p(y|x)p(x|v)$
在这里，需要把不带下标的变量定义为宏观变量：
$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)$
由于各时刻观测、输入的噪声都是无关的，上面两个项可以因式分解：
$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)$
同时，对目标函数两边取对数，对数是个单调映射，不影响最优解：
$\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)$
这时因子相乘变成了对数项相加。
高斯分布取对数之后有较好的形式：
$\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)$$\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)$$\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)$
舍掉那些与$x$无关的项，定义：
$J_{v,k}(x) = \left\{\begin{matrix} &\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{matrix}\right.$$\begin{matrix} 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{matrix}$
于是目标函数变为求这个式的最小化：
$\left\{\begin{matrix} \hat{x}=\underset{x}{\argmin}J(x) \\ J(x)=\sum_{k=0}^{K}\left( J_{v,k}(x) + J_{y,k}(x)\right) \end{matrix}\right.$
这个问题就是常见的无约束最小二乘。

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

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

2.贝叶斯推断（Bayesian Inference）

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

• k时刻状态更新：$x_k= A _{k-1}x_{k-1}+v_k+w_k$
• 提升形式：$x=A(v+w)$
  其中$A=\begin{bmatrix} 1& & \\ A_0& 1 &\\ A_1A_0 &A_1 &1\\ \vdots& \vdots& \vdots& \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{bmatrix}$

在上式中，右侧只有$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]=AQA^T$

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

观测模型：

• 单次观测：$y_k = C_kx_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( \begin{bmatrix} \check{x}\\ C\check{x} \end{bmatrix}, \begin{bmatrix} \check{P} & \check{P}C^T \\ C\check{P} & C\check{P}C^T+R \end{bmatrix} \right)$

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

由下列高斯推断：

• 联合分布：$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}+BD^{-1}C)^{-1}BD^{-1}$
• $(A^{-1}+BD^{-1}C)^{-1}=A-AB(D+CAB)^{-1}CA$

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

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

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

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

得出一些结论：

• 按均值式：$\hat{P}^{-1}\hat{x}=({(A^{-1})}^{T}Q^{-1}A^{-1}+C^TR^{-1}C)\hat{x}={(A^{-1})}^{T}Q^{-1}v+C^TR^{-1}y$
• 定义:$z=\begin{bmatrix}v\\y\end{bmatrix},H=\begin{bmatrix}A^{-1}\\C\end{bmatrix},W=\begin{bmatrix}Q\\&R\end{bmatrix}$
• 得到：$(H^TW^{-1}H)\hat{x} = H^TW^{-1}z$

与MAP结果一致。

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

• $(H^TW^{-1}H)\hat{x} = H^TW^{-1}z \Leftrightarrow \hat{x}=(H^TW^{-1}H)^{-1}H^TW^{-1}z$
• 即要求$(H^TW^{-1}H)\hat{x}$可逆，$\text{rank}(H^TW^{-1}H)=N(k+1)$
• 协方差矩阵的对称正定性要求：$\text{rank}(H^TH)=\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网络请求的数据。