机器人学中的状态估计第3章 线性高斯系统的状态估计

离散时间的批量估计

1. 离散时间的线性时变系统
   $$\begin{array}{l}运动方程:{\mathbf{x}}_k={\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}+v_k+{\mathbf{w}}_k,k=1,\cdots ,K\\ 观测方程:{\mathbf{y}}_k={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{n}}_k,k=0,\cdots ,K\end{array}$$
   各个变量含义：
   $$\begin{gathered} 系统状态:x_k\in {\mathbb{R}}^N \\ 初始状态:x_0\in {\mathbb{R}}^N\sim \mathcal{N}\left({\check{x}}_0, \\ {\check{P}}_0\right) \\ 输入:v_k\in {\mathbb{R}}^N \\ 过程噪声:{\mathbf{w}}_k\in {\mathbb{R}}^N\sim \mathcal{N}\left(0,{\mathbf{Q}}_k\right) \\ 测量:{\mathbf{y}}_k\in {\mathbb{R}}^M \\ 测量噪声:n_k\in {\mathbb{R}}^M\sim \mathcal{N}\left(0, \\ {\mathbf{R}}_k\right) \end{gathered}$$
   其中，$v_k$为确定性变量之外，其他变量都是随机变量。噪声和初始状态一般假设为互不相关，并且在各个时刻与自己也互不相关。A称为转移矩阵（transition matrix），B称为观测矩阵（observation matrix）。
2. 状态估计问题:在k个（一个或多个）时间点上，基于初始的状态信息${\check{x}}_0$、一系列观测数据、一系列输入，以及系统的运动模型和观测模型，来计算系统的真实状态的估计值 ${\hat{x}}_k$
3. 讨论最简单的情形：批量LG（linear Gausisan）线性高斯系统。（批量允许一次性使用所有数据）
4. 常用两种思路解决估计问题：

• 贝叶斯推断（Bayesian inference）:得到后验概率密度函数（得到分布）
• 最大后验估计（MAP）：求给定信息下的最大后验估计（最优解）

离散时间的批量估计

• “批量”：系统允许我们一次性使用所有数据，来推算所有时刻的状态。
• 推导过程参照书籍，蓝色字体对过程的补充：

1. 最大后验估计
   
2. 贝叶斯推断
   

• 结论：在我们假设的线性高斯系统中，最大后验估计的结果恰好等于贝叶斯推断的均值。正是因为我们假设了噪声为白噪声，且彼此独立，且线性变换使得高斯分布在传递中始终保持高斯分布。估计的状态符合高斯分布，高斯分布有均值和模相等的性质，所以有这样的结果。

离散时间的递归平滑算法

• 应用最大后验估计和贝叶斯推断，在批量估计问题得到了
  $$\left({\mathbf{H}}^{\mathrm{T}}{\mathbf{W}}^{-1}\mathbf{H}\right)\hat{\mathbf{x}}={\mathbf{H}}^{\mathrm{T}}{\mathbf{W}}^{-1}\mathbf{z}$$
  等式左侧的逆协方差矩阵有稀疏结构（对角块），可以利用cholesky分解，可以加速方程的求解：通过一次前向递推和后向递推。

• Cholesky平滑算法计算得到的结论可以推导出其他算法，或者说可以指示与其他算法的联系。分解得到的5个前向方程和1个后向方程。6个递归方程在代数上等价传统的Rauch-Tung-Striebel平滑算法；5个前向迭代，等价于卡尔曼滤波。（可以说明卡尔曼滤波在线性高斯系统是最优的，最可信的。当然卡尔曼滤波还有其他简单的方法可推导，后面再介绍。）

• 作用：加速求解批量问题方程。

• Cholesky平滑算法推导过程：
  推导过程见书易推出。这里只给出结论：
  理解
  $$\begin{gathered} 前向:k=1,\cdots ,K \\ \begin{array}{rl}{\mathbf{L}}_{k-1}{\mathbf{L}}_{k-1}^{\mathrm{T}}& ={\mathbf{I}}_{k-1}+A_{k-1}^{\mathrm{T}}{Q}_k^{-1}{A}_{k-1}\\ {\mathbf{L}}_{k-1}{d}_{k-1}& =q_{k-1}-A_{k-1}^{\mathrm{T}}{Q}_k^{-1}{v}_k\\ L_{k,k-1}{L}_{k-1}^{\mathrm{T}}& =-Q_k^{-1}{A}_{k-1}\\ I_k& =-L_{k,k-1}{L}_{k,k-1}^{\mathrm{T}}+Q_k^{-1}+C_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}{C}_k\\ q_k& =-L_{k,k-1}{d}_{k-1}+Q_k^{-1}{v}_k+C_k^{\mathrm{T}}{\mathbf{R}}_k^{-1}{\mathbf{y}}_k\end{array} \end{gathered}$$
  $$\begin{gathered} 后向:k=K,\cdots ,1 \\ {\mathbf{L}}_{k-1}^{\mathrm{T}}{\hat{x}}_{k-1}=-{\mathbf{L}}_{k,k-1}^{\mathrm{T}}{\hat{x}}_k+{\mathbf{d}}_{k-1} \end{gathered}$$
  $$初始值:\begin{array}{l}{I}_0={\check{P}}_0^{-1}+C_0^{\mathrm{T}}{R}_0^{-1}{C}_0\\ q_0={\check{P}}_0^{-1}{\check{\mathbf{x}}}_0+C_0^{\mathrm{T}}{R}_0^{-1}{y}_0\\ {\hat{\mathbf{x}}}_K={\mathbf{L}}_K^{-\mathrm{T}}{\mathbf{d}}_K\end{array}$$

• Rauch-Tung-Striebel平滑算法

  • 代数上等价于Cholesky平滑算法，可以由它推出。
  • 推导过程省略，结论：前向方程即为经典形式的卡尔曼滤波方程。
    $$\begin{array}{rl}前向:k=1,\cdots ,K& \\ {\check{P}}_{k,f}& ={\mathbf{A}}_{k-1}{\hat{P}}_{k-1,f}{\mathbf{A}}_{k-1}^{\mathrm{T}}+Q_k\\ {\check{x}}_{k,f}& ={\mathbf{A}}_{k-1}{\hat{x}}_{k-1,f}+v_k\\ {\mathbf{K}}_k& ={\check{P}}_{k,f}{C}_k^{\mathrm{T}}{\left(C_k{\check{P}}_{k,f}{C}_k^{\mathrm{T}}+{\mathbf{R}}_k\right)}^{-1}\\ {\hat{P}}_{k,f}& =\left(1-K_k{C}_k\right){\check{P}}_{k,f}\\ {\hat{x}}_{k,f}& ={\check{x}}_{k,f}+K_k\left(y_k-C_k{\check{x}}_{k,f}\right)\\ 后向:k=K,\cdots ,1& \\ \begin{array}{r}{\hat{x}}_{k-1}={\hat{x}}_{k-1,f}+{\hat{P}}_{k-1,f}{A}_{k-1}^{\mathrm{T}}{\check{P}}_{k,f}^{-1}\left({\hat{x}}_k-{\check{x}}_{k,f}\right)\end{array}& \\ 初始值：& \\ {\check{P}}_{0,f}& ={\check{P}}_0\\ {\check{x}}_{0,f}& ={\check{x}}_0\\ {\hat{x}}_K& ={\hat{x}}_{K,f}\end{array}$$

离散时间的滤波算法（卡尔曼滤波）

• 引入：在批量问题，我们使用到未来的数据来估计过去的状态，所以不能在线运行。适合的是离线建图的问题。对于在实习时刻对当前状态的估计，只能利用当前和过去的信息进行估计。卡尔曼滤波就是对于这样一个问题的传统解决方案。
• 推导：

  1. MAP推导卡尔曼滤波
     

  2. 贝叶斯推断卡尔曼滤波