SLAM卡尔曼滤波&&非线性优化

SLAM卡尔曼滤波&&非线性优化

高斯分布（正态分布）是一个常见的连续概率分布。正态分布的数学期望值或期望值等于位置参数，决定了分布的位置；其方差 的开平方或标准差 等于尺度参数，决定了分布的幅度。

1.高斯分布

一个随机变量服从高斯分布，那么它的概率密度函数为：

它的高维形式为：

2.高斯分布的运算

2.1 线性运算

设两个独立的高斯分布：

那么他们的和仍然是高斯分布：

如果以常数乘以，那么满足：

如果取,那么满足：

2.2乘积

设两个高斯分布的乘积满足,那么：

2.2复合运算

同时考虑到和，当他们不独立时，其复合分布为：

其中和为两个变量的协方差，满足：

协方差为正，则说明这两个变量呈正相关，为零则不相关，为负则为负相关。
由条件分布（贝叶斯法则）展开式可以推出，条件概率满足：

舒尔补

现在来推导如何得出该结论：
舒尔补理论：把矩阵分解成上三角矩阵、对角阵、下三角矩阵乘积的形式，方便运算，即：

其中称为矩阵的舒尔补。
同样地：

具体推导过程（了解）

由于,根据其指数部分展开形式可以得到：

由于的指数部分为：

因此，

3.复合运算例子

考虑到随机变量，另一变量满足：

其中为线性变量的系数矩阵和偏移量，为噪声项，为零均值的高斯分布：.因此：

4.SLAM问题的数学表述

通常，机器人或者无人车在运动过程中会携带一个测量自身运动的传感器（IMU,轮速里程计等），通过积分运算可以抽象出一个关于本体感知的数学模型：

这里的是运动传感器的读数（有时候也称为输入），为噪声，该方程称为运动方程，目前没有给出具体的运动意义，是因为运动传感器可以是选择的，根据不同的运动传感器可以列出不同的方程，或者是他们的组合，即多传感器融合。
同样的，在SLAM中还有一个环境感知传感器（激光雷达、相机、红外设备）的观测部分，环境感知所对应的方程为观测方程，当机器人或者无人车在的位置上观测到某个路标点，此时产生了一个观测数据z_{k,j}，同样的抽象出其数学模型为：

其中，为观测噪声，同样观测方程根据环境传感器的不同而有不同的形式。
因此SLAM问题可以总结出两个基本的方程：

从上述方程中可以看出：
1.观测方程中，只有在位置看到了时才会产生观测数据，否则就没有。然而在实际过程中，一个位置通常可以看到一小部分路标，同时无论是相机还是激光雷达观察特征时，特征点数量较多，所以实际过程中观测方程会远远大于运动方程数量。
2.运动方程中，实际过程中，如果做纯视觉SLAM或者纯激光SLAM，往往会缺少运动方程；在这种时候的处理方式通常假设为车辆匀速运动或者不动或者匀加速运动等。
3.位姿（定位）和路标（建图）看成服从某种概率分布的随机变量，而不是单独的一个数。由于两者都是待估计的量，改变一下记号，令为时刻的所有未知量，同时包含了当前时刻的车辆位姿（假设已经完成传感器与车体的标定）与m个路标点，则：

同样的把时刻所有的观测量都记为，于是：

直接给出待估计的后验概率密度为：

其中：
表示状态的初识值；表示到运动传感器的输入；表示的到时刻所有的观测,该问题表示的是：**根据所有历史数据(输入、观测、初始状态)得出最终的融合结果，即滤波问题。**用图表示为：

5.卡尔曼滤波（KF）

对（A）公式进行马尔可夫假设，当前时刻只与上一时刻有关，预测部分为：

先验部分可以简化为：

从图上可以看出输入量与时刻无关。上面的两个方程：以时刻的状态估计，推导到时刻。
假设：
1.这里的状态量服从高斯分布，那只需考虑维护状态量的均值和协方差即可。
2.所有的状态和噪声均满足高斯分布。
线性高斯系统可以使用运动方程和观测方程来表示：

噪声服从零均值高斯分布，则：

记为后验，为先验分布，假设已知时刻的后验状态估计：和此时的协方差。此时的目的比较明确：根据时刻的后验状态估计，以及输入和观测数据确定时刻的后验分布。
1.预测部分
根据运动方程和高斯分布确定为：

记：

2.观测方程
观测方程可以计算在某个状态下，应该观测数据，根据观测方程和高斯分布确定为：

设结果为,根据公式：

可以得到前面推导很熟悉的一个公式：

直接用结论：

这里可以直接看到：

可以推导出

定义：

则有

整理一下，五个方程为：

1.预测：

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

然后计算后验概率分布：

至此，我们推导了经典的卡尔曼滤波器的整个过程。事实上卡尔曼滤波器有若干种推导方式，而我们使用的是从概率角度出发的最大后验概率估计的形式。我们看到，在线性高斯系统中，卡尔曼滤波器构成了该系统中的最大后验概率估计。而且，由于高斯分布经
过线性变换后仍服从高斯分布，所以整个过程中我们没有进行任何的近似。可以说，卡尔曼滤波器构成了线性系统的最优无偏估计。

6.扩展卡尔曼滤波（EKF）

卡尔曼滤波器通常适用于线性系统，但是在SLAM中，运动方程和观测方程通常为非线性函数。无论视觉SLAM模型还是激光SLAM，使用李代数表示位姿，更不可能是一个线性系统。一个高斯分布，经过非线性变换后，往往不再是高斯分布，所以在非线性系统中，需要进行近似处理，将一个非高斯分布近似成一个高斯分布。
通常把卡尔曼滤波器的结果拓展到非线性系统中来，称为扩展卡尔曼滤波器（Extended Kalman Filter，EKF），通常的做法是，在某个点附近考虑运动方程以及观测方程的一阶泰勒展开，只保留一阶项，即线性部分，然后按照线性系统进行推导。非线性化的运动方程和观测方程为：
运动方程：

观测方程为：

令时刻点均值与协方差矩阵为为,。在时刻，把运动方程和观测方程，在,处进行线性化（相当于一阶泰勒展开）：

记这里的偏导数为：


对于观测方程，同样地有：

记:


展开线性化结果方程为：

这里和服从零均值分布即：

因此,
这里
结果为：

协方差矩阵为：

*这里的推导根据高斯分布协方差的传递性：


综合可以得到
$$

$$