SLAM（七）-卡尔曼滤波

参考《概率机器人》、《Principles of GNSS，lnertial and Multisensor lntegrated Navigation Systems
(Second Edition)》

一、卡尔曼滤波(KF)

卡尔曼滤波(KF,kalman filter）是一种线性滤波方法。
状态方程：噪声为Rt

观测方程：

预测更新：

1、如果 P/R小，相应的卡尔曼增益会很小并且状态估计收敛到真值的速度会更慢 对应到
系统的变化，状态估计过程会变慢 相反，如果 P/R 太大，卡尔曼增益会非常
大，这将使滤波更侧重于最近的观测值，从而会因为观测噪声对状态估计产生了
太大的影响而导致滤波不稳，或产生估计偏差。有时，通过系统模型状态估计与
观测噪声会形成正反馈，并最终导致滤波器的迅速发散，远离真值。这里假设观测是比较准的，预测步方差P过大/观测噪声过小，类似于比例因子K比较大，超调量会比较大，容易振荡；预测步方差P过小/观测噪声过大，K较小，收敛速度比较慢

二、扩展卡尔曼滤波(EKF)

《概率机器人》P40
扩展卡尔曼滤波（EKF，extend kalman filter）是一种非线性滤波方法。先将非线性状态转移和观测方程使用一阶泰勒展开线性化，然后使用卡尔曼方法预测和更新即可。
非线性方程：

线性化后：

预测：

三、误差状态卡尔曼滤波(ESKF)

误差传播
1）惯性传感器误差，即陀 螺和加速度计的噪声、零偏、比例因子误差和交轴耦合等各类误差。
2）导航初始化误差，包括位置、速度和姿态等参数的初值误差。
3）重力误差，主要是重力模型误差和位置误差带来的重力计算误差等
4）算法和计算误差
基于扰动方法推导误差状态方程和观测方程
状态方程推导
1）首先将有误差的变量建模为误差状态δx=x_true - x,x_true为真值，x为估计值，姿态误差需要单独定义，在计算或得到变量 x 的过程中是否引入含有误差的变量，或者误差幅度是否可以小到忽略不计。
2）求解误差状态δx导数，得到连续状态的状态转移函数，其导数与x_true 和x导数差相等，根据x导数解析解，x_true误差变量加上扰动δx，x导数不变，再将两者相减得到误差状态转移方程；姿态根据q_true导数解析解和扰动方差2种不同的求导方式，然后取等号进行求解
3）离散化：δx导数=(δx_k-δx_k-1)/δt,根据中值法、欧拉法不同方法离散化，如VINS推导；或采用如下图牛小骥团队《组合导航》课程方式离散化。注意一般给定的是连续状态的噪声，需要转为离散化误差状态噪声再套卡尔曼公式，噪声为一段时间的累积

观测方程：
4）观测误差更新方程不需要求导，将观测减去估计值构建，需要把观测噪声转为误差观测状态噪声，得到误差状态协方差更新方程与状态协方差不同，噪声是某个时刻的

5)状态反馈：使用误差状态δx_k更新当前状态