《视觉SLAM十四讲》第6讲非线性优化，似然问题P(z,u|x,y)是如何变为最小二乘min J(x,y) (式6-13)的？

最近在学习高翔《视觉SLAM十四讲》中的理论知识，在第六章非线性优化中，解释了为何求解最大概率问题能够转化为最小二乘的优化问题。然而书中有些证明省略掉了，经过请教搞清楚了，特此记在这里。或许能够对其他朋友有帮助。

关键词：如何从求： $$\max P(z,u|x,y)$$ 得到求：
$$\min J(x,y)=\sum _k{e}_{u,k}^T{R}_k^{-1}{e}_{u,k}+\sum _k\sum _j{e}_{z,k,j}^T{Q}_{k,j}^{-1}{e}_{z,k,j}$$ 采用的是《视觉SLAM十四讲（第二版）》，公式编号与第一版稍微有一点儿不一样，但不影响理解。

1. 从后验到似然

我们需要估计的量为：所有的状态 $x$ 和路标 $y$，不加角标表示所有的时刻，即 $x_{0,1,2...K}$。而我们已知的是实际的观测 $z$ 和我们的输入量（控制量）$u$，所以我们要求的是 $x,y$ 的条件概率分布（式6.4）$$P(x,y|z,u)$$ 利用贝叶斯公式写成（式-6.5）：
$$P(x,y|z,u)=\frac{P(z,u|x,y)P(x,y)}{P(z,u)}\propto P(z,u|x,y)P(x,y)$$ 由于我们要求的是 $x,y$ 所以不用考虑分母的 $z,u$ ，而后面的 $P(x,y)$ 是先验，我们在未知可能的先验时这部分没有用，（一般设为均匀分布？）所以省略，于是关于最优的 $x,y$ 写成求解最大似然估计（式-6.7）：
$$(x,y{)}_{MLE}^{\ast }=arg\max P(z,u|x,y)$$ 从而得出了从后验到似然。

2. 后验式子的变形

$$P(z,u|x,y)=P(z|x,y)P(u|x,y)=P(z|x,y)P(u|x)$$
因为观测与控制独立，所以有第一个等号；而控制只和实际的位置有关，与标志$y$无关，所以有第二个等式。
对于观测，第 $z_{k,j}$ 个观测（用 $k$ 表示第 $k$ 个位置，$j$ 表示第 $j$ 个标志物）只和第 $k$ 个位置的状态 $x$，和第 $j$ 个标志物有关，所以有：
$$P(z|x,y)=P(z_{1,1},z_{2,1},z_{2,3},....z_{k,j}|x,y)=\prod _{k,j}P(z_{k,j}|x_k,y_j)$$ 这里把 $z$ 展开了，随意写了第二个式子（可以认为是在第1个位置看到了路标1，第2个位置看到了路标1和3，等），由于每次观测相互独立，所以可以写成条件概率连乘的形式。
对于控制，第 $k$ 个控制量只和前一次与后一次的位置状态 $x$ 有关（这也是马尔科夫假设）而与其它的位置无关，所以可以写成：
$$P(u|x)=P(u_{1:K}|x_{0:K})=\prod _{k}P(u_k|x_{k-1},x_k)$$ 于是整理两个式子，可以得到书中的 式6.11
$$P(z,u|x,y)=\prod _{k}P(u_k|x_{k-1},x_k)\prod _{k,j}P(z_{k,j}|x_k,y_j)$$ 然而按照这个式子继续推，不能直观的得到后续结果，因为观测部分有明确的观测模型，而控制部分没有！ 具体的介绍请看第4部分。

3. 观测部分变形

首先补充观测方程：
$$z_{k,j}=h(y_j,x_k)+v_{k,j}$$ 即观测 $z$ 是实际的（与位置 $x$ 和实际路标 $y$ 有关的）观测模型（例如相机成像模型）的真实测量，$h(x,y)$ 叠加一个观测噪声 $v$。
如果我们假设实际的观测结果，是以：真实观测值为期望，协方差矩阵为噪声协方差的正态分布，即（省略了不重要的角标）：
$$P(z|x,y)=N(h(x,y),Q)$$ 则我们在计算最大的$P(z|x,y)$即为计算最小的 $-ln(P(z|x,y))$，而这部分可以进一步写成（省略与$x,y$无关项）：
$$(z-h(x,y){)}^T{Q}^{-1}(z-h(x,y))$$ 这就是式6.10，而如果定义观测误差如下：
$$e_z=z-h(x,y)$$ 则我们通过对（式-6.11），即连乘式子求对数，连乘变为累加，最小化后半部分的观测，则式6.11的观测部分写成：
$$\sum _k\sum _j{e}_{z,k,j}^T{Q}^{-1}{e}_{z,k,j}$$ 便得到了观测部分的目标函数形式。

4. 运动部分变形

而对于控制部分并不能直接从（式-6.11）得到！
因为运动方程写作：
$$x_k=f(x_{k-1},u)+w_k$$
即这一时刻的位置由上一时刻的位置和施加的控制量产生的运动模型 $f$ 叠加一个噪声 $w$ 确定。并不能写作（式-6.11）中的控制量由一个包含前一时刻和当前时刻的位置的方程决定。（其实也可以这样定义一个方程，但不能写成控制方程 $f$ 的形式）。
于是需要将 $P(u|x)$ 进行变形。
首先，有式子：
$$P(x|u)=P(x_0|\overline{x_0})\prod _{k=1}^{K}P(x_k|x_{k-1},u_k)$$ 这个关键的式子来自于《机器人学中的状态估计》第3章（式-3.5），具体解释为，在控制下的全部 $x$ 可以写成在初始分布的估计值 $\overline{x_0}$ 的一个条件概率分布和后面的连乘。补充一下个人的证明过程：
$$P(x|u)=P(x_{0:K}|u_{1:K},\overline{x_0})=P(x_0|\overline{x_0})P(x_1|x_0,u_0)P(x_2|x_1,u_1)...P(x_K|x_{K-1},u_K)$$ 第一个等号为展开向量为所有单个的状态（注意 $\overline{x_0}$ 是定义在条件中的），后面为条件独立+马尔科夫。
那么我重写了（式-6.11）的控制部分：
$$arg\max P(u|x)=arg\max \frac{P(x|u)P(u)}{P(x)}=arg\max P(x|u)$$ 第一个等号为贝叶斯公式，第二个等号由于所求 $x$ 与控制 $u$ 无关，且分母为先验是未知的，所以省略。再带入《机器人学中的状态估计》这个式子，便写成了（还不太具体明白$P(x_0|\overline{x_0})$这部分如何处理）：
$$\prod _{k}P(x_k|x_{k-1},u_k)$$ 于是，式6.11在求取最大值时的参数 $x,y$ 被改写成了：
$$arg\max P(z,u|x,y)=arg\max \prod _{k}P(x_k|x_{k-1},u_k)\prod _{k,j}P(z_{k,j}|x_k,y_j)$$ 这样前半部分利用控制方程，与观测部分相同，定义运动误差 $e_{u,k}=x_k-f(x_{k-1},u_k)$，再假设正态分布的期望与方差，从而最终写成目标函数（式-6.13）形式：
$$\min J(x,y)=\sum _k{e}_{u,k}^T{R}_k^{-1}{e}_{u,k}+\sum _k\sum _j{e}_{z,k,j}^T{Q}_{k,j}^{-1}{e}_{z,k,j}$$

5. 后续

后面将目标函数变形，得到求解最小二乘问题即可。

6. 小结

首先，书上没有写出来的可能不是那么直接就能看出来，实际自己一写就能发现问题。其次，折腾半天挺有成就感的。最后，多看别的书籍虽然很痛苦但确实有帮助。以及，写博客敲公式好累。