机器人学中的状态估计——学习笔记

离散时间的批量估计问题

问题描述：在离散线性高斯模型下，利用所有时刻的数据估计某一时刻的状态。
符号：

方法：

1、最大后验概率法(Maximum A Posteriori, MAP)

$$\hat{\mathbf{x}}=\arg \underset{\mathbf{x}}{\max }p(\mathbf{x}|\mathbf{v},\mathbf{y})$$
where
$$\mathbf{x}={\mathbf{x}}_{0:K}=\left({\mathbf{x}}_0,\dots ,{\mathbf{x}}_K\right),\mathbf{v}=\left({\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:K}\right),\mathbf{y}={\mathbf{y}}_{0:K}=\left({\mathbf{y}}_0,\dots ,{\mathbf{y}}_K\right)$$
Note: $p$是概率密度函数，不是概率。
重要步骤：

• 由贝叶斯公式得：
  
• 由于噪声之间相互独立（假设1）：

因此，(3.3)式被分解成了三个式子。

• 用条件高斯分布公式分别求解以上三个式子。这一步能够引出$costfunction$
  先给出条件高斯分布公式：
  对于联合概率分布是高斯分布$p(x_a,x_b)$的两个变量，其条件高斯分布$p\left(x_a|x_b\right)$和边缘分布${}^1$都是高斯分布，其中$p\left(x_a|x_b\right)$的均值和协方差矩阵的公式为：
  $$\begin{array}{rl}{\mu }_{\mathrm{a}\mathrm{l}\mathrm{b}}& ={\mu }_a+{\Sigma }_{ab}{\Sigma }_{bb}^{-1}\left(x_b-{\mu }_b\right)\\ {\Sigma }_{a|b}& ={\Sigma }_{aa}-{\Sigma }_{ab}{\Sigma }_{bb}^{-1}{\Sigma }_{ba}\end{array}$$
  1、 边缘分布（Marginal Distribution）指在概率论和统计学的多维随机变量中，只包含其中部分变量的概率分布。
  那么, 对（3.3）等式右端取对数，并假设${\check{P}}_0,Q_k$和$R_k$都是可逆的（假设2），可得：
  
  化简掉与$x$无关的$ln(\cdot )$项，可得:
  
  
  最终将原始问题化成了：
  
  将以上式子化成矩阵形式：
  
  求偏导：
  
  最终结果如上。

2、贝叶斯推断(Bayesian inference)

暂略。

离散时间的迭代平滑问题

某些叫法：矩阵有稀疏结构，代表他含有对角块，如：

本节内容的目的：上一节虽然能得到最优解，但是效率很低，因为要求矩阵的逆等等。因此，可以利用矩阵稀疏结构加速求解。采用的方法：一次前向递推和一次后向地推，称为固定区间平滑算法。
本节的问题：利用稀疏矩阵的特性求解方程：
$$\left({\mathbf{H}}^{\mathrm{T}}{\mathbf{W}}^{-1}\mathbf{H}\right)\hat{\mathbf{x}}={\mathbf{H}}^{\mathrm{T}}{\mathbf{W}}^{-1}\mathbf{z}$$

1、Cholesky平滑算法

步骤：

• 利用稀疏Cholesky分解可得：
  $${\mathbf{H}}^{\mathrm{T}}{\mathbf{W}}^{-1}\mathbf{H}=\mathbf{L}{\mathbf{L}}^{\mathrm{T}}$$
  其中，
  
  下面首先利用前向迭代求解其中的每一个小矩阵：
• 由${\mathbf{H}}^{\mathrm{T}}{\mathbf{W}}^{-1}\mathbf{H}=\mathbf{L}{\mathbf{L}}^{\mathrm{T}}$得：
  
  先求$L_0$，这需要利用一次⼩型块的稠密矩阵分解。然后一项一项地求，可得到$L$。
• 由上可得，$L{L}^T\hat{x}=H^T{W}^{-1}z$, 令$L^T\hat{x}=d$, 那么先计算$d$:
  用类似的方法：
  
  一项一项求解$d_0,...,d_K$。
• 最后求解： $L^{\mathrm{T}}\hat{x}=d$，展开后：
  
  先求${\hat{\mathbf{x}}}_K$,一项一项求解，称为逆向迭代。

总结：

初值：

其中的五个向前迭代又是***卡尔曼滤波***。

2、Rauch-Tung-Striebel 平滑算法

该算法是Cholesky平滑算法的标准形式，更接近于我们平时用的卡尔曼滤波。

离散时间的迭代滤波问题