状态估计之非线性最小二乘

本文主要介绍一下视觉slam中的状态估计问题，并介绍如何将非线性最小二乘与其联系起来。

状态估计问题

首先介绍一下slam中的状态估计问题，这个问题由一个运动方程与一个观测方程构成。

\[ \begin{equation} \begin{cases} x_{k} = f(x_{k-1}, u_{k}) + w_k \\ z_{k,j} = h(y_j, x_{k}) + {\upsilon}_{k,j} \end{cases} \end{equation} \]

这个方程中，第一个方程是运动方程，第二个方程是观测方程。其中 - \(x_{k}\) 是相机在k时刻的位姿，我们一般可以使用变换矩阵或者其李代数表示它。 - \(u_{k}\) 是k时刻系统的输入（在slam中通常是运动传感器的读数） - \(w_{k}\) 是运动过程中的噪声 - \(y_j\) 是第j个路标点 - \(z_{k,j}\) 是指在\(x_k\)处对路标\(y_{j}\)进行了一次观测得到的观测值（vslam中是对应到图像上的像素位置） - \({\upsilon}_{k,j}\) 是观测过程中的噪声

在vslam中，我们可以将观测方程这样表示：

\[ s z_{k,j} = K exp({\xi}^{\wedge}y_{j}) \]

其中，\(K\)为相机内参，s为像素点的距离。同时，上述应该使用齐次坐标来表示\(z\)与\(y\)

---

一般情况下，在运动与观测方程中的两个噪声项\(w_{k}\)与\({\upsilon}_{k,j}\)满足零均值的高斯分布。

\[ w_{k} \sim N(0, R_{k}) , {\upsilon}_{k,j} \sim N(0, Q_{k,j}) \]

在这个问题中，我们的目的是通过带噪声的观测数据\(z\)与输入\(u\)来推断位姿\(x\)和地图\(y\)。这就是状态估计问题。

贝叶斯与最大似然

在已知输入数据\(u\)与观测数据\(z\)的条件下，我们可以用下式表示状态\(x\)的条件概率分布

\[ P(x {\mid} z,u) \]

此处的x与u是对所有数据的统称，比如可能有的系统没有观测运动的传感器，只有一张张图片。此时只考虑观测方程带来的数据，相当于估计\(P(x {\mid} z)\)的条件概率分布。一般估计状态变量的条件分布，我们会利用贝叶斯法则

\[ P(x {\mid} z) = \dfrac{P(z {\mid} x)P(x)}{P(z)} \propto P(z {\mid} x)P(x) \]

上述贝叶斯法则左侧通常称为后验概率，右侧的\(P(z {\mid} x)\) 称为似然，另一部分\(P(x)\)称为先验。直接求后验分布是困难的，而求一个状态最优估计，使得在该状态下后验概率最大化(Maximize a Posterior, MAP)是比较可行的。

\[ {x^{\ast}}_{MAP} = arg max P(x {\mid} z) = arg max P(z {\mid} x)P(x) \]

上面忽略了贝叶斯法则的分母部分，这是因为我们求的是在x为什么值时后验概率最大，而这部分与x无关并且为正，去掉这部分对结果不会产生影响。贝叶斯法则告诉我们求解最大后验概率等价于最大化似然与先验的乘积。而大部分的情况下，我们并不知道机器人位姿在哪里的概率分布，因此也就没有了先验，此时，上述估计就变成了最大似然估计(Maximize Likelihood Estimation, MLE)：

\[ {x^{\ast}}_{MAP} = arg max P(z {\mid} x) \]

最大似然估计的直观意义是：在什么样的状态下，最可能产生现在的观测结果

转化为最小二乘问题

首先对于vslam来说，一般我们首先考虑观测模型。

\[ z_{k,j} = h(y_{j}, x_{k}) + {\upsilon}_{k,j} \]

由于其中函数h是一个确定的映射，噪声项\({\upsilon}_{k,j} \sim N(0, Q_{k,j})\),因此观测数据的条件概率为

\[ P(z_{k,j} {\mid} x_{k}, y_{j}) = N(h(y_{j}, x_{k}), Q_{k,j}) \]

这依然是一个高斯分布。我们想计算使其最大化的\(x_{k}\)与\(y_{j}\)，此时我们通常会对其进行负对数形式变换。

---

对于一个任意高维度高斯分布\(x \sim N(\mu, \Sigma)\)，它的概率密度函数展开形式为

\[ P(x) = \dfrac{1}{\sqrt{(2\pi)^{N} det(\Sigma)}}exp(-{\dfrac{1}{2}}(x-{\mu})^{T} {\Sigma}^{-1}(x-{\mu})) \]

上式中\(\Sigma\)是这个多元高斯分布的协方差矩阵，它的逆是信息矩阵。

对上式取负对数，则变为

\[ -ln(P(x)) = \dfrac{1}{2} ln((2{\pi})^N det(\Sigma)) + \dfrac{1}{2} (x-\mu)^T {\Sigma}^{-1} (x-\mu) \]

对原分布求最大化相当于对负对数求最小化。在最小化上式的x时，第一项与x无关，可以省略。于是只要最小化右侧的二次型项，就完成了对状态的最大似然估计。将上述分析带入slam的观测模型，相当于：

\[ x^{\ast} = arg min ((z_{k,j} - h(x_{k}, y_{j}))^T {Q_{k,j}}^{-1} (z_{k,j} - h(x_{k}, y_{j}))) \]

在上式中，我们已经将最大似然估计转换成了求解最小二乘问题。该式等价于最小化噪声项（误差）的平方。因此对于所有的运动和任意的观测，我们可以定义数据与估计值之间的误差。

\[ e_{\upsilon, k} = x_{k} - f(x_{k-1}, u_{k}) \]

\[ e_{y, j, k} = z_{k, j} - h(x_{k}, y_{j}) \]

同时考虑观测模型与运动模型，最终我们可以得到整个slam系统的误差平方和

\[ J(x) = \sum_{k}e^T_{v, k} R^{-1}_{k} e_{\upsilon, k} + \sum_{k} \sum_{j}e^T_{y,k,j} Q^{-1}_{k,j} e_{y,k,j} \]

运动方程的对状态的最大似然估计转化成最小二乘的推导也是上述方式。注意：！此处的最小二乘和高斯牛顿推导的区别是两个残差相乘中间加了一个信息矩阵（协方差矩阵的逆）

总结

由上我们知道对于误差的最小二乘问题的求解最优值等价与状态的最大似然估计。

误差函数由许多个误差的平方和组成，虽然总体的状态变量维数很高，但每个误差项都是简单的，仅与一脸个状态变量有关。如果使用李代数表示这个优化问题，则该问题是一个无约束的最小二乘问题。

回顾

对于一个线性测量方程

\[ z = f(x) + n \]

其中\(z\)为测量，\(H\)为测量矩阵，\(x\)为系统状态，\(n\)为测量噪声，假设测量噪声服从零均值高斯分布，协方差矩阵为\(R\)，对上述公式进行变形

此处假设x已知，由此可以写出\(z\)的条件概率密度

\[ P(z|x) = N(f(x), R) = \dfrac{1}{\sqrt{(2\pi)^{N} det(R)}}exp(-{\dfrac{1}{2}}(z-f(x))^{T} {R}^{-1}(z-f(x))) \]

事实是\(z\)已知，因此最大似然估计为

\[ argmax{P(z|x)} \]

取负对数

\[ -ln(P(z|x)) = ln({\sqrt{(2\pi)^{N} det(R)}}) + {\dfrac{1}{2}}(z-f(x))^{T} {R}^{-1}(z-f(x)) \]

由于对数函数是线性递增的，所以对\(P(z|x)\)最大化，也就是求负对数的最小化 \[ argmax{P(z|x)} = argmin{((z-f(x))^{T} {R}^{-1}(z-f(x)))} \]

参考文献

• [1]. 视觉slam十四讲