SLAM7-后端的卡尔曼滤波器推导、理解以及扩展

1、为什么需要后端
前面的视觉里程计虽然通过非线性优化获得了理想的T，但是还是有一定的误差，随着距离的逐渐增加，误差也会进行累计，所以需要用后端把这些误差每隔一定的时间进行一次消除。
2、卡尔曼滤波器的理解
滤波器：波在SLAM中指的是概率波，滤波是指把某个正确位置处对应的概率，让他越高越好，把错误位置处的概率，越低越好
滤波器原理：
假定机器人在下面的场景出现

用横坐标表示机器人的位置，纵坐标表示机器人出现在此处的概率，如果机器人知道一共有三扇门，知道三扇门的位置，此时在一个门的前面但是不知道自己在哪个门的前面，可以有下面的分布波：

之后继续往前走到第二扇门前面，假设机器人有轮式里程计，知道自己又走了d的长度，那么正态分布变为；

继续向下进行更新，当机器人走到第三扇门前面：

与卡尔曼滤波器的联系：

3、后端的状态估计是以概率的形式来进行解释的
状态估计的概率解释：位姿x和路标y服从某种概率分布，目的是通过某些运动数据u（比如惯性测量传感器IMU输入）和观测数据z（比如拍摄到的照片像素点的值）来确定状态量x和y的分布。
状态方程和观测方程：
第一个状态方程表明了根据上一时刻k-1的状态、k-1时刻发出的传感器的读数uk和噪声误差wk计算得到这一时刻k的状态，
第二个观测方程实际上指的就是拍摄的图像，是根据k时刻的实际状态xk和地标y（把两者写到一起）、像素误差vk得到这一时刻的观测数据zk

先验：根据0~k-1时刻的各种数据来推测k时刻的状态xk
后验：结合了k时刻的观测数据得到的k时刻状态
后端估计的概率公式：假定我们有了初始状态x0，我们希望的目的，就是根据0~k时刻所有的数据，来观测当前的状态。那么这就可以写成：

根据贝叶斯法则：贝叶斯等式左边的正比于贝叶斯等式右边的分子，因为分母是在求和，不会有选择和改变的余地。

公式改写为：左边先正比于P（X0，U1：k，Z1：k | Xk）P(Xk)，
对于第一项，k时刻的先验并不能决定除了Zk以外的在k时刻之前的数据，因此第一项就等于P（Zk | Xk)
对于第二项，P（Xk)是没有Zk情况下的k时刻状态，它也就等于在前面的记录的x0，z1：k-1……发生情况下产生的数据。
进一步改写为：

右边第一项，是“给定了一个先验Xk的情况下，什么观测数据Zk最能符合这个先验“的概率。因此这就是所谓的似然。
右边第二项，是根据0~k-1时刻的观测数据，推测得知k时刻的先验状态
后验=似然先验
马尔科夫链：对于第二项，由于xk受到xk-1的影响，我们将它在k-1时刻展开，根据全概率公式：xk是B，Xk-1是A

把全概率公式调换左右顺序，第二项变为：

整体概率估计公式变为：
左边代表似然，意味着给定状态解释当前测量的概率，
右边代表先验，意味着系统的Xk的动态变化，Zk-1影响Xk-1,Xk-1影响Xk

概率公式的解答：
如果认为k时刻的状态只与k-1时刻有关，那就是用扩展卡尔曼滤波器（EKF）来做。如果觉得和所有状态都有关，那就是用非线性优化来做。目前主流是非线性优化。
卡尔曼滤波器：
推导准备工作：
因为认为k时刻的状态只与k-1时刻有关，根据概率估计公式，把第二项进行逐次化简:
step1:概率估计公式
step2:概率估计公式第二项
step3:第二项的左边
step4:第二项的右边，k-1时刻的状态与之后的传感器读数uk无关

我们发现，第二项的右边类似于状态估计方程，说明我们只需要维护一个状态量xk-1,之后进行不断的迭代更新，就可以推到xk.
推导过程：我们的最终是求xk的后验，后验=似然*先验

先假设状态量都服从高斯分布，这是一个线性高斯系统：

根据定义，我们可以先求先验：

这里，
假设我们已经有了xk-1的后验和协方差
那么可以写出xk的先验公式是：尖帽子xk表示后验，横线帽子xk表示先验

进一步标记：

最终得到：

似然的公式是：

最终概率估计公式：用尖帽子表示后验概率，那么主线的“后验=似然*先验”就是：

把概率估计公式进行左右展开，因为估计公式是满足高斯分布，只看exp的指数部分，可以得到：


进一步展开，通过两边对于xk的一次与二次系数相等，从而确定等式左边的后验Xk（尖帽子）和等式右边的先验Xk（横线帽子）之间的关系！
对于二次项系数相等，我们有：

对于一次项系数相等，我们有：
通过上面的公式我们可以知道怎么通过先验来求后验了，后验的位姿状态与先验的位姿状态的差异，事实上就是（“观测值zk”与“估计的观测值”之间的误差 ）乘以一个（卡尔曼增益）！
卡尔曼滤波器实际的使用步骤：

扩展卡尔曼滤波器：
上面的卡尔曼假定了运动方程和观测方程都是线性的，可以有系数矩阵A,C，但是实际中两个方程可能不是线性的，一个高斯分布经过非线性的变换，结果往往不再是高斯分布。
过程推导：由于观测方程不是线性的，所以我们进行泰勒展开


把偏导数记为F和H，那么上面的式子就写成

由于左右两边都满足正态分布：


所以先验就是：

似然就是：

最后根据一次和二次型，得到


讨论：EKF 的局限：

1.是基于马尔科夫性的，但如果当前帧与很久前有关，滤波器难以处理。（不如非线性优化可以考虑很久前的状态）

2.EKF是用泰勒展开一次导数的系数作为线性近似，但如果实际情况有着强烈的非线性，那就会有非线性误差。

3.EKF slam由于视觉slam中路标数量很大，因此存储量呈平方增长。