状态估计

状态估计，是根据系统的先验模型和测量序列，对系统内在状态进行重构的问题

背景

$\left\{\begin{matrix} 运动方程（相机位姿）：x_k = f(x_{k-1},u_k,w_k) \\ 观测方程（针孔模型）：z_{k,j}=h(y_j,x_k,v_{k,j}) \end{matrix}\right.$

$x_k = T_k = \exp({\xi_k}^\wedge)$：x_k是一个六自由度的位姿，可以由一个变换矩阵来描述，也可以用李代数的指数来描述。

$sz_{k,j}=K\exp(\xi^\wedge)y_j$：内参×外参×路标点 = 像素点距离×像素

考察问题

当已知观测方程和运动方程的具体形式，如何对估计值进行优化？

即：如何通过观测量z 和输入量u 去优化机器人的轨迹、位置x 和路标点的位置y （SLAM过程）？

这是一个状态估计问题：通过已知的带有噪声的数据去推断出无法直接获得的状态

该问题下的状态估计

状态变量：

$x={x_1,…,x_N,y_1,…y_M}$：所有时刻的位姿x 和所有时刻的路标（地图）y

所以状态估计等同于求解条件分布： P(x | z, u) x在z，u条件下的概率分布（x, z, u都表示所有时刻的统称）

状态估计问题分类：

1 线性系统：若f和h是线性方程（有几个线性量，加和，数乘组成）

2 高斯噪声：噪声属于高斯分布

3 非线性系统

4 非高斯噪声

此处的状态估计具有马尔科夫性（下一个时刻的状态依赖于上一个时刻的状态）

状态估计描述

在k个时间点上，基于初始状态信息、一系列观测数据、一系列输入，以及系统的运动模型和观测模型，来计算系统的真实状态估计值。

从概率学角度求状态估计

在不知道输入控制u，只有图像时，忽略u并只考虑观测数据

P(x|z)：已知图像，推断x的分布：称为后验概率

贝叶斯：

似然概率：p(z|x)：在某一个状态x下观测图像（确定图像与当前图像的相似程度）

先验概率：P(x)：x自己的状态

对于p(x|z)：x的维度很高，难以写出其分布，但是可以求：

1）最大后验估计

使得 p(x|z)最大的 x 的估计 (${x^*}_{MAP}$)，即在某一种分布下 x 的最优估计，此时是一个估计出的量

${x^*}_{M A P} = \arg \max P(x|z) = \arg \max P(z|x)P(x)$

注释：寻求一个x使得p(x|z)达到最大，此时称为x为最大后验估计，表示：在怎样的x下拿到的相机数据和现在数据（已有的）是最像的

2）最大似然估计

当最大后验估计中无法得知 x时（不知道机器人轨迹的先验是什么样子的时候），则直接最大化似然

${x^*}_{MLE} = \arg \max P(z|x)$

注释：当不知道先验概率时（x原本应该是什么样子，或者x就是一个随机的运动）即没有先验时，则此时最大化似然就可以了。表示：最大化x，使得在此状态下看到的数据最像我们看到的数据，P(z|x) 即“在哪种状态下，最容易产生当前的观测”

求解最大似然估计

某次观测：$z{k,j}=h(y_j, x _k)+v{k, j}$，噪声服从0均值高斯分布$vk\sim \mathcal{N}(0,Q{k,j})$

所以观测数据条件概率（将h加到噪声分布上）

$P(z_{j,k}|x_k,y_j)=N(h(y_j, x _k), Q_{k,j})$

对于N维高斯分布 $x\sim \mathcal{N}(\mu,\Sigma)$的概率密度展开式：

$p(x)=\frac{1}{\sqrt{(2\pi)^N\det(\Sigma)} }\exp(-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu))$

它的右侧指数部分为一个二次型，若$\Sigma=I$，则指数部分就是一个二范数（误差的平方）

将展开式两边取对数并添加负号，得到p(x)只与马氏距离$-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)$有关。

所以要求最大似然估计，就是求最小$-\ln(p(x))$

最小二乘

代入SLAM观测模型，

$(x_k, y_j)^*=\arg\min( \;(z-h(x,y))^T Q^{-1} (z-h(x,y))\;)$

等式右侧为一个最小二乘，即最小化噪声项（误差）的一个二次型（马氏距离）

其中，$Q^{-1}$为信息矩阵（协方差矩阵的逆，协方差反映两个变量相向的程度），用于噪声的控制调整

$P(z,u|x,y)=\prod_kP(u_k|x_{k-1},x_k)\prod_{k,j}P(z_{k,j}|x_k, y_j)$

定义运动误差和观测误差：$e{u, k}，e{z, j, k}$，最小化估值和真实值之间的误差（即马氏距离）时，每一个误差对应一个最小二乘，所以得到一个最小二乘问题 $\min J(x, y)$ ，它等于所有时刻运动误差和观测误差的总和。

此最小二乘问题等价于状态的最大似然估计

非线性最小二乘

探讨通用的无约束非线性最小二乘问题的求解

一阶梯度

最速下降法，取增量为反向梯度 $\Delta x^*=-J(x_k)$

二阶梯度

牛顿法，保留了二阶梯度，$\Delta x = -H^{-1}J$

高斯牛顿法

将误差一阶展开，代入最小二乘，求梯度求解

$J(x)J^T(x)\Delta x=-J(x)f(x) \; \Rightarrow\;H(x)\Delta x=g(x)$

称为高斯牛顿方程（正规方程）$H\Delta x=g$ .

其中：J(x)为f(x)关于x的导数，是一个1×n的向量

缺陷：

1）$H = JJ^T$可能不正定，无法求$H^{-1}$

2）不考虑步长的取值造成误差

求解过程：

1 最小二乘式，确定待估计的变量

2 误差式

3 计算雅克比矩阵J 和误差e

4 求解增量方程 Hx=g

5 迭代

LM

增加指标

$\rho = \frac{f(x + \Delta x) - f(x)}{ J(x)^T\Delta x}$

以该指标为 $\Delta x$ 添加一个信赖区域，以$\rho$值的大小来控制信赖区域的半径，使得在此区域内取得的$JJ^T \sim H$，从而用雅克比矩阵运算代替海塞矩阵的运算，同时，在约束项的控制下，保证了求解项左边矩阵可逆。

将约束项加入到最小二乘中：

$\mathcal{L}(\Delta x_k,\lambda)=\frac{1}{2}\left \| f(x_k) + J(x_k)^T\Delta x_k \right \|^2+\frac{\lambda}{2}(\left \| D\Delta x_k \right \|^2-\mu)$

其中，D为非负数对角阵，表示了对空间的约束。

对$\Delta x$求导后得：$(H+\lambda D^TD)\Delta x_k=g$

矩阵微分

先使用微分法则展开复合微分项，再通过迹技巧，结合下式求解

$df=tr(\frac{\partial f}{\partial X}^TdX)$

参考资料：

视觉SLAM十四讲：从理论到实践（第2版）. 高翔.