SLAM中的估计方法（最大似然和最小二乘）及概率论基础

1.概率论基础

• 概率加法运算：

通过累加来求随机变量A的概率分布，该分布也成边缘分布。
$$P(A)=\sum _D\sum _C\sum _{B}P(A,B,C,D)\text{\hspace{1em}(1-1)}$$

• 概率乘法运算：

通过链式乘法来求随机变量集合X={A,B,C,D}的整体分布，该分布也称联合分布。
$$\begin{gathered} P(A,B,C,D)=P(A,B,C)P(D|A,B,C) \\ =P(A,B)P(C|A,B)P(D|A,B,C) \\ =P(A)P(B|A)P(C|A,B)P(D|A,B,C)\text{\hspace{1em}(1-2)} \end{gathered}$$
可以看到式（1-1）和式（1-2）计算比较复杂，不能用各个随机变量的概率分布直接相乘和相加，如果随机变量集合X={A,B,C,D}中各个随机变量是相互独立的，那么直接相加或者相乘是可行的，在实际情况中，随机变量之间往往具有相关性，导致计算变得复杂。

• 条件独立性

$$\begin{gathered} P(x,y|z)=P(x|z)P(y|z) \\ P(x|y,z)=P(x|z) \\ P(y|x,z)=P(y|z)\text{\hspace{1em}(1-3)} \end{gathered}$$

• 贝叶斯准则（贝叶斯公式）如（1-4），更一般的形式(条件贝叶斯公式)为式（1-5）：

$$P(B|A)=\frac{P(A|B)P(B)}{P(A)}\text{\hspace{1em}(1-4)}$$

$$P(B|A,C)=\frac{P(B,A,C)}{P(A,C)}=\frac{P(A|B,C)P(B,C)}{P(A,C)}=\frac{P(A|B,C)\frac{P(B|C)}{P(C)}}{\frac{P(A|C)}{P(C)}}=\frac{P(A|B,C)P(B|C)}{P(A|C)}\text{\hspace{1em}(1-5)}$$
贝叶斯准则等式的左边是后验概率，等式的右边是先验概率。

2. 状态估计问题

在我其他文章中说到过经典SLAM模型，他由一个运动方程和一个观测方程构成，如式：
$$\{\begin{array}{l}{x}_k=f(x_{k-1},u_k)+w_k\\ z_{k,j}=h(y_j,x_k)+v_{k,j}\end{array}\text{\hspace{1em}(2-1)}$$
这里$x_k$是相机的位姿，可以用SE(3)来描述。

3.最大似然估计（MLE）

假设知道概率分布模型，而概率分布模型中的参数θ未知。观测得到的样本数据Z={z₁，z₂，……，z_k}是模型的输出结果，那么θ取什么值能让模型输出这些样本数据呢？尝试θ的不同取值，看模型是否能输出与观测结果一样的数据。当某个θ取值能让模型输出与观测结果一样的数据的概率最大，那么这个θ取值就是最合适的估计参数$\hat{\theta }$。

构建似然函数，用于表示观测样本Z={z₁，z₂，……，z_k}与参数θ之间的概率关系，如式（3-1）所示，假设概率分布模型是已知的，即P(z_k|θ)的概率分布情况为已知信息。
$$L(\theta |Z)=P(z_1|\theta )P(z_2|\theta )\cdots P(z_k|\theta )=\prod P(z_k|\theta )=f(Z|\theta )\text{\hspace{1em}(3-1)}$$
可以看出，似然函数是由观测样本中每个样本点概率累乘的结果。似然函数L(x,θ)是一个概率密度函数，“似然性”和“概率”意思相近，都是指某事件发生的可能性。但是似然函数和f(Z|θ)理解过程正好相反，不再关注事件发生的概率，而是已知发生了某些事件，即已知数据样本的情况下，希望知道θ是多少，所以似然函数L(θ|Z)是关于θ的函数。

概率函数是关于随机变量的函数，函数值表示随机变量的样本点在不同地方出现时对应的概率大小；似然函数是关于参数的函数，函数值表示模型参数去不同值时同一样本点发生的概率是多少。

如果某个样本点概率为0，将导致整个似然函数取值变为0,。为了避免个别数据对计算的一个影响，一般将似然函数L(θ|Z)取对数，如式（3-2）。这样累乘运算就转换为累加运算，计算将不会因个别概率项而失效。
$$l(\theta |Z)=ln(L(\theta |Z))=\sum _{k}ln(P(z_k|\theta ))\text{\hspace{1em}(3-2)}$$
最大似然估计，其实就是求使似然函数l(θ|Z)取最大值时的θ值。所以，最大似然估计的目标函数如式（3-3）
$${\hat{\theta }}_{MLE}=argma{x}_{\theta }l(\theta |Z)\text{\hspace{1em}(3-3)}$$
求解最大似然估计的目标函数最值的方法很简单，直接对似然函数求导，令导数等于0，就能求出θ，如下式。
$$\frac{d}{d\theta }l(\theta |Z)=0\text{\hspace{1em}(3-3)}$$

4.最小二乘估计（LSE）

• 最大似然估计的缺点是必须先知道假定模型的概率分布情况P(z_k|θ)。
• 最小二乘估计的思路是计算观测样本点与模型实际点之间的平方误差，求使得该平方误差最小的参数值θ，求出来的这个参数值θ就是估计参数$\hat{\theta }$。
• 最小二乘估计相比最大似然估计，优点是能解决模型未知的问题，只关心样本数据拟合，并不关心模型到底长什么样。

曲线的模型一般形式可以用幂函数级数来表示，如式（4-1）。h(x,θ)就是我们要研究的曲线模型，θ是模型中的参数，这里为幂级数前面加权系数的统称。
$$h(x,\theta )=\sum _{w_1,w_2,\cdots ,w_i\in \theta }{w}_i{x}^i=w_{1}x+w_2{x}^2+\cdots +w_i{x}^i\text{\hspace{1em}(4-1)}$$
理论上给定h(x,$\theta$)一个输入值就能得到一个对应的输出值z，现在的情况是输出值z已经通过观测知道了，要反推模型参数θ。如果θ取值完全正确，那么h(x,θ)输出与通过观测得到的z是完全吻合的。由于θ的取值不完全正确，所以在该θ参数下h(x,θ)模型的实际点与通过观测得到的样本点z之间会有误差。对于每个样本点与模型实际点之间的误差取平方（也就是二乘运算）然后将所有的平方误差求和，养着就构建出了所谓的地阿基啊函数，如式（4-2）所示
$$J(\theta )=\sum _k(z_k-h(x_k,\theta ){)}^2\text{\hspace{1em}(4-2)}$$
通过不断调整θ的值，理论上可以使代价函数J(θ)=0，也就是说样本点与模型实际点之间的平方差误差达到了最小，样本与模型完全拟合。实际情况中，θ=

{$w_1,w_2,\cdots ,w_i$}这个加权系数的个数是有限值，观测样本Z={z₁，z₂，……，z_k}的数量也有限，这样就会可能导致无论怎么调整θ都不能使代价函数J(θ)=0。因此我们认为代价函数J(θ)到达非0的最小值即可。

与最大似然估计中求最值的方法一样，求解最小二乘估计的代价函数最值的方法很简单，直接对代价函数求导，令导数等于0，就能求解出θ，如式（3-3）所示，这里的θ={$w_1,w_2,\cdots ,w_i$}是向量，所以函数对向量求导会稍有不同。
$$\frac{d}{d\theta }J(\theta )=0\text{\hspace{1em}(4-3)}$$

5.MLE和LSE的联系以及在slam的使用

当模型的观测噪声服从$\epsilon$~N(0,${\sigma }^2$)的高斯分布时，最大似然估计等价于最小二乘估计。

在第二部分状态估计问题中提到观测模型：
$$z_{k,j}=h(y_j,x_k)+v_{k,j}\text{\hspace{1em}(5-1)}$$
由我们假设了噪声项$v_k$~$N(0,Q_{k,j})$,所以观测数据的条件概率为：
$$P(z_{k,j}|x_k,y_j)=N(h(y_j,x_k),Q_{k,j})\text{\hspace{1em}(5-2)}$$
这依然是一个高斯分布，考虑单次观测的最大似然估计，可以使用最小化负对数来求一个高斯分布的最大似然，因为高斯分布在负对数下有较好的数学形式。

任意高维高斯分布$x$~$N(\mu ,\Sigma )$,他的概率密度函数展开形式为：
$$P(x)=\frac{1}{(2\pi {)}^{N}det(\Sigma )}exp(-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu ))\text{\hspace{1em}(5-3)}$$
对其取负对数，则变为
$$-ln(P(x))=\frac{1}{2}ln((2\pi {)}^{N}det(\Sigma ))+\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )\text{\hspace{1em}(5-4)}$$
因为对数函数是单调递增的，所以对原函数求最大化相当于对负对数求最小化。在最小化上式的x时，第一项与x无关，可以略去。于是只要最小化最右侧的二次型项，就得到了对状态的最大似然估计。带入SLAM的观测模型，相当于在求：
$$\begin{gathered} (x_k,y_j{)}^{\ast }=argmaxN(h(y_j,x_k),Q_{k,j}) \\ =argmin(x-\mu {)}^T{Q}_{k,j}^{-1}(x-\mu ) \\ =argmin(z_{k,j}-h(x_k,y_j){)}^T{Q}_{k,j}^{-1}(z_{k,j}-h(x_k,y_j))\text{\hspace{1em}(5-5)} \end{gathered}$$
我们发现，该式等价于最小化噪声项（即误差）的一个二次型，这个二次型称为马哈拉诺比斯距离，又叫马氏距离，他可以看成由$Q_{k,j}^{-1}$加权之后的欧氏距离，这里$Q_{k,j}^{-1}$也被叫做信息矩阵，即高斯分布协方差矩阵之逆。

以上是某一次观测的数据，现在考虑批量时刻的数据。通常假设各个时刻输入和观测时相互独立的，这意味着更输入之间是独立的，更观测之间是独立的，并且输入和观测也是独立的。于是我们可以对联合分布进行因式分解
$$P(z,u|x,y)={\Pi }_{k}P(u_k|x_{k-1},x_k)\Pi P(z_{k,j}|x_k,y_j)\text{\hspace{1em}(5-6)}$$
这说明我们可以独立地处理各时刻的运动和观测。定义各次输入和观测数据与模型之间的误差：
$$\begin{gathered} e_{u,k}=x_k-f(x_{k-1},u_k) \\ {e}_{z,j,k}=z_{k,j}-h(x_k.y_j)\text{\hspace{1em}(5-7)} \end{gathered}$$
那么，最小化所有时刻估计值与真实读数之间的马氏距离，等价于求最大似然估计。负对数允许我们把乘积变成求和：
$$minJ(x,y)=\sum _k{e}_{u,k}{R}_k^{-1}{e}_{u,k}+\sum _k\sum _j{e}_{z,k,j}{Q}_{k,j}^{-1}{e}_{z,k,j}\text{\hspace{1em}(5-8)}$$
这样就得到了一个最小二乘问题，他的解等价于状态的最大似然估计。

6.贝叶斯估计

上面讨论的最大似然估计和最小二乘估计中，待测估计参数θ被当成一个确定量，因此这些方法被称为经典估计。

如果将带估计参数θ当成一个不确定量，即随机变量，就可以将θ的先验知识引入估计以提高精度，先验知识与后验知识之间通过贝叶斯准则建立联系，因此被叫做贝叶斯估计。

这一方面可以看一下我的博客：《贝叶斯滤波和粒子滤波》。