SLAM中的基础数学4:非线性优化

笼统的讲,SLAM问题其实就是一个优化问题,因此,我们有必要讨论一下优化中非线性优化。具体的包括了最小二乘、梯度下降法、搞死牛顿法以及LM方法。

状态估计问题

假设我们有一个系统。这个系统的模型我们用

\begin{equation}\left\{ \begin{aligned}x_k=f(x_{k-1},u_k)+w_k  \\ z_{k,j}=h(y_j,x_k)+v_{j,k} \end{aligned} \right. \end{equation}

来表示,其中\(z_k\)是k时刻的观察,\(x_k\)是k时刻的状态,\(u_k\)是k时刻的输入控制量,\(y_j\)是航标j的位置。\(w_k\)与\(v_k\)分别是相应过程的噪声。

我们要估计机器人当前的最佳状态,就是要估计\(P(x|z,u)\)的最大值,即在当前观测和输入下,我们估计机器人出现概率最大的位姿。因为正面计算有难度,我们变通一下。根据Bayes公式,我们有

$$P(x|z)=\frac{P(z|x)P(x)}{P(z)} \varpropto P(z|x)P(x)$$

这个告诉我们,如果要想知道在当前的观测z下,x最可能出现的地方的概率,我们可以去找“一个位置”使得最可能观测到当前的观测数据z,这里的`一个位置”就是我们的“最似然位置”,然后在抛开观测信息,根据实际经验来确定x的可能分布就确定了P(x)的先验分布,我们就可以说,x最可能出现的位置\(x^*\)一定是既“最似然”又“最先验”的地方。打比方说:机器人对太阳进行了观测,因为我如果知道机器人大概在地球,就算机器人在月球的时候才最有可能得出对太阳的观测数据,那我也只能放弃“可能在月球”这个假定,转而寻找在地球最可能得到当前观测数据的地方,这个地方应该是在地球才合理。即

$$x^*=argmaxP(x|z)=argmaxP(z|x)P(x)$$

有不少的时候,我们并没一个“机器人大概在某位置”这样的先验信息。就是说,机器人可能出现在全宇宙的任何角落,所以,我只需要求得使得“似然”最大的地方就可以了。即

$$x^*=argmaxP(x|z)=argmaxP(z|x)$$

最小二乘

最小二乘,这个名字真的是听得耳朵都生茧了,做实际工程的人,很多时候对采集的数据处理都离不开这个工具。

 

发表评论

电子邮件地址不会被公开。 必填项已用*标注