SLAM学习——后端（一）

1.概述

对于里程计而言，只有短暂的记忆。而在后端优化中，我们更加考虑一段更长时间内（或所有时间内）的状态估计问题。
与之前略有不同，我们令 xk x k 为k时刻的所有未知量，包含了当前位姿与m个路标点，则写成表达式可为：

xk=Δ{xk,y1⋯ym} x k = Δ { x k , y 1 ⋯ y m }

现在考虑第k时刻的情况，希望用过去0到k的数据来估计现在的状态分布：

P(xk|x0,u1:k,z1:k) P ( x k | x 0 , u 1 : k , z 1 : k )

下标0:k表示从0时刻到k时刻的所有数据。 zk z k 表示所有在k时刻的观测数据。按照贝叶斯法则，我们可得：

P(x|x0,u1:k,z1:k)∝P(zk|xk)P(xk|x0,u1:k,z1:k−1) P ( x | x 0 , u 1 : k , z 1 : k ) ∝ P ( z k | x k ) P ( x k | x 0 , u 1 : k , z 1 : k − 1 )

在前面已经讲过，第一项称为似然，第二项为先验。对于先验部分，明白 xk x k 是基于过去所有的状态估计得来的。至少会受 xk−1 x k − 1 影响，于是按照 xk−1 x k − 1 时刻为条件概率展开得：

P(xk|x0,u1:k,z1:k−1)=∫P(xk|xk−1,x0,u1:k,z1:k−1)P(xk−1|x0,u1:k,z1:k−1)dxk−1 P ( x k | x 0 , u 1 : k , z 1 : k − 1 ) = ∫ P ( x k | x k − 1 , x 0 , u 1 : k , z 1 : k − 1 ) P ( x k − 1 | x 0 , u 1 : k , z 1 : k − 1 ) d x k − 1

基于上式，有若干种选择：
1.马尔可夫性，该假设认为k时刻状态只有k-1时刻状态有关，而与再之前的无关，得到扩展卡尔曼滤波（EKF）。
2.考虑k时刻状态与之前所有状态的关系，得到非线性优化为主体的优化框架。SLAM主流为非线性优化。

2.线性系统和KF

如果我们假设马尔可夫性，从数学角度来讲，该方程

P(xk|x0,u1:k,z1:k−1)=∫P(xk|xk−1,x0,u1:k,z1:k−1)P(xk−1|x0,u1:k,z1:k−1)dxk−1 P ( x k | x 0 , u 1 : k , z 1 : k − 1 ) = ∫ P ( x k | x k − 1 , x 0 , u 1 : k , z 1 : k − 1 ) P ( x k − 1 | x 0 , u 1 : k , z 1 : k − 1 ) d x k − 1

右边第一项 P(xk|xk−1,x0,u1:k,z1:k−1)=P(xk|xk−1,uk) P ( x k | x k − 1 , x 0 , u 1 : k , z 1 : k − 1 ) = P ( x k | x k − 1 , u k )

化简原因：基于马尔科夫性， xk x k 状态只与 xk−1 x k − 1 有关，因此可以把 x0 x 0 拿掉，同时与观测方程无关，因此可以把他给拿掉，而且输入数据 "u1:k " u 1 : k 可变为 uk u k

第二部分可简化为： P(xk−1|x0,u1:k,z1:k−1)=P(xk−1|x0,u1:k−1,z1:k−1) P ( x k − 1 | x 0 , u 1 : k , z 1 : k − 1 ) = P ( x k − 1 | x 0 , u 1 : k − 1 , z 1 : k − 1 )

化简原因：由于第二部分只考虑从0时刻到k-1时刻，所以在k时刻的输入数据是没用的，这里可以去掉。这一项其实就是k-1时刻的状态分布。

线性高斯系统是说：运动方程和观测方程可以由线性方程来描述：

{xk=Akxk−1+uk+wkzk=Ckxk+vk  { x k = A k x k − 1 + u k + w k z k = C k x k + v k  

在已知 xk−1 x k − 1 状态分布的后验状态估计 x∧k−1 x ∧ k − 1 和其协方差 P∧k−1 P ∧ k − 1 ，根据输入数据和观测数据，确定 xk x k 的后验分布。 x∧k x ∧ k 表示后验， x−k x − k 表示先验。

根据高斯分布的性质，我们可以有： P(xk|x0,u1:k,z1:k−1)=N(Akx−k−1+uk,AkP−k−1ATk+R)=N(x−k,P−k) P ( x k | x 0 , u 1 : k , z 1 : k − 1 ) = N ( A k x − k − 1 + u k , A k P − k − 1 A k T + R ) = N ( x − k , P − k )

另一方面，由观测方程，我们可以计算在某一状态应该产生怎样的观测数据： P(zk|xk)=N(Ckxk,Q) P ( z k | x k ) = N ( C k x k , Q )

则将状态分布写成高斯分布的样子可得： N(x∧k,P∧k)=N(Ckxk,Q)N(x−k,P−k) N ( x ∧ k , P ∧ k ) = N ( C k x k , Q ) N ( x − k , P − k )

由于因为知道是高斯分布，那么我们只需要用比较指数部分即可，把指数部分展开得到： (xk−x∧k)TP∧−1k(xk−x∧k)=(zk−Ckxk)TQ−1(zk−Ckxk)+(xk−x−k)TP−−1k(xk−x−k) ( x k − x ∧ k ) T P ∧ k − 1 ( x k − x ∧ k ) = ( z k − C k x k ) T Q − 1 ( z k − C k x k ) + ( x k − x − k ) T P − k − 1 ( x k − x − k )

（注：这里的等号并不完全相等，实际允许相差与 xk x k 无关的常数）

定义一个中间变量： K=P∧kCTkQ−1 K = P ∧ k C k T Q − 1

忽略中间的推导过程，我们可以得：

overset∧Pk=(I−KCk)P−k o v e r s e t ∧ P k = ( I − K C k ) P − k

( 也许上式中K是由 P∧k P ∧ k 定义，加上这个式子，感觉有点循环的意思，但是K实际上可通过其他方式求得）

x∧k=x−k+K(zk−Ckx−k) x ∧ k = x − k + K ( z k − C k x − k )

因此，根据上面推导，“预测”和“更新”可分为俩个步骤 ：
1.预测：求出k时刻时候的先验，由于现在分析的是线性高斯系统，因此k时刻的先验为：

{xk=Akxk−1+uk+wkzk=Ckxk+vk  { x k = A k x k − 1 + u k + w k z k = C k x k + v k  

2.更新：先计算K，它又称为卡尔曼增益：

K=P−kCTk(CkP−kCTk+Qk)−1 K = P − k C k T ( C k P − k C k T + Q k ) − 1

3.然后计算后验概率分布：

P∧k=(I−KCk)P−k P ∧ k = ( I − K C k ) P − k

x∧k=x−k+K(zk−Ckx−k) x ∧ k = x − k + K ( z k − C k x − k )

这就是经典的卡尔曼滤波器的推导过程。在线性高斯系统中，卡尔曼滤波器构成了该系统中的最大后验概率估计，由于高斯分布经过线性后仍服从高斯分布，卡尔曼滤波器构成了线性系统的最优无偏估计。

3.非线性系统和EKF

在SLAM中，运动方程和观测方程通常是非线性的，所以不能直接用卡尔曼滤波器。一个高斯分布，经过非线性方程后，往往不再是高斯分布，所以在非线性系统中，我们必须取近似值，将一个非高斯分布近似成高斯分布。

将非高斯分布近似成高斯分布的做法是在某点附近考虑运动方程的一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统进行推导。

令k-1时刻的均值与协方差矩阵为

x∧k−1 x ∧ k − 1 ， P∧k−1 P ∧ k − 1 ，在 x∧k−1 x ∧ k − 1 ， P∧k−1 P ∧ k − 1 处进行线性化，有：

xk≈f(x∧k−1,uk)+∂f∂xk−1|x∧k−1(F)(xk−1,x∧k−1)+wk x k ≈ f ( x ∧ k − 1 , u k ) + ∂ f ∂ x k − 1 | x ∧ k − 1 ( F ) ( x k − 1 , x ∧ k − 1 ) + w k （运动方程）

zk≈h(x−k)+∂h∂xk|x−k(H)(xk−x−k)+nk z k ≈ h ( x − k ) + ∂ h ∂ x k | x − k ( H ) ( x k − x − k ) + n k （观测方程）

那么我们指导这里的先验和协方差的均值为：

x−=f(x∧k−1,uk) x − = f ( x ∧ k − 1 , u k ) （不知道这里的先验为什么只有一项，不应该是整个运动方程吗（除去噪声））

P−k=FP∧k−1FT+Rk P − k = F P ∧ k − 1 F T + R k

则运动方程分布为：N ( x−=f(x∧k−1,uk) x − = f ( x ∧ k − 1 , u k ) , P−k=FP∧k−1FT+Rk P − k = F P ∧ k − 1 F T + R k )

对于观测方程也可按照线性高斯分布卡尔曼来滤波器推导过程进行推导出其增益方程和后验分布。

对于SLAM这种非线性的情况下，卡尔曼滤波器给出了单次线性近似下的最大后验估计。

4.讨论

总体而言，在SLAM中，我们常用非线性优化的方式来进行，在同等计算量，非线性优化能取得较好的成果。

EKF的局限？

1.首先，滤波器方法在一定程度上假设了马尔科夫性，也就是k时刻的状态只与k-1时刻相关。如果当前帧与很久之间的帧有关，例如回环，那么滤波器就不能用了，这时偏向于使用非线性优化方法。

2.EKF滤波器仅在 x∧k−1 x ∧ k − 1 处做了一次优化，然后根据这个结果就把后验给算出来，我们认为该点处的线性化近似在后验概率处仍然有效的。但是离开工作点时，一阶泰勒展开不一定能够近似整个函数，所以这取决于运动模型和观测模型的非线性情况。如果有强烈的非线性的话，那么线性近似就只能在很小范围内成立，这就是EKF的非线性误差，也是主要问题所在。

3.EKF需要储存很多的均值和方差，对他们维护和更新，如果把路标也放进去，那么会很多数据，EKF不适用于大场景。