Quaternion kinematics for ESKF

0, 贝叶斯滤波

0）联合概率

对离散随机变量 X， Y 而言，联合分布概率密度函数如下：

，当 X和 Y 相互独立 (independent) , 有 

1）条件概率

假定已经知道 Y 的值是 y, 想知道基于以上事实条件 X 为 x 的概率表示为  称为条件概率 (conditional probability) 。如果 p(y) >0, 则条件概率定义为

如果 X 和 Y 相互独立,则有

注意多变量的情形：

P(A,B,C)=P(C,A,B)=P(C|A,B)*P(A,B)=P(C|A,B)*P(B,A)=P(C|A,B)*P(B|A)*P(A)
P(A, B, C)还能等价于P(A|B,C)*P(B,C)=P(A|B,C)*P(B|C)*P(C)等

2）全概率

先举个例子，小张从家到公司上班总共有三条路可以直达（如下图），但是每条路每天拥堵的可能性不太一样，由于路的远近不同，选择每条路的概率如下：

每天上述三条路不拥堵的概率分别为：

假设遇到拥堵会迟到，那么小张从Home到Company不迟到的概率是多少？

其实不迟到就是对应着不拥堵，设事件C为到公司不迟到，事件Li为选择第i条路，则：

  全概率就是表示达到某个目的，有多种方式（或者造成某种结果，有多种原因），问达到目的的概率是多少（造成这种结果的概率是多少）？

    设事件L1、L2 ..... 是一个完备事件组，则对于任意一个事件Ｃ，则全概率公式为：

通用形式：

3）贝叶斯概率

上面问题修改为：到达公司未迟到选择第１条路的概率是多少？可不是，因为０．５这个概率表示的是，选择第一条路的时候并没有靠考虑是不是迟到，只是因为距离公司近才知道选择它的概率，

而现在我们是知道未迟到这个结果，是在这个基础上问你选择第一条路的概率，所以并不是直接就可以得出的

即在已知条件概率和全概率的基础上，贝叶斯公式为：

 

参考：浅谈全概率公式和贝叶斯公式

上面的贝叶斯公式简单梳理一下为通用形式：

在slam系统中 x通常为状态变量，y 通常为检测数据 (data), 也就是传感器测量值，一个应注意的重点是贝叶斯准则的分母, p(y) 不依赖 x，即随着状态x变化，检测数据的

全概率 p(y) 是不变的，因子 对任何 x 的后验概率 p(x/y) 都是相同的。由于这个原因, 经常写成贝叶斯准则中的归一化变量,通常用 n 表示,为

4）联合概率

贝叶斯公式中  ，可得=  = 

5）证明常用贝叶斯公式

当 只要 p(y | x) >0 时

注意：

1、P(x,y,z)也可以是其他形式，如 P(z|x,y) 、P(x|y,z)等

2、当 x、y是以其他变量z为条件的相互独立的随机变量时，

当 只要 p(z) >0 时

6）隐马尔可夫模型

时刻t 的状态随机地依赖 t -1 时刻的状态和控制 ut ，得状态转移概率 ，因为机器人的环境是随机的,因此状态转移是一个概率分布而不是一个确定函数;

测量 zt 随机地依赖时刻 t 的状态，得测量概率。

时刻 t 的状态随机地依赖 t -1 时刻的状态和控制 ut 。测量 zt 随机地依赖时刻 t 的状态。这样的时间生成模型也称为隐马尔可夫模型

似然（likelihood）：

举个栗子，假设 Thrun 智商 180 Sebastian Thrun 概率机器人的作者），我 智商 150 ， 我室友 智商 81我们每个人在考试中考出不同分数的概率是不同的。

P(x分I智商y)是条件概率，即为在智商为y时，得x分的概率

条件概率分布通常是可以知道的，比如我们已知一个传感器，我们知道它的测量误差，可以把一个测量值的概率分布写出来。

假设有个人考了100分满分，你说这个分是谁考出来的概率比较大？Thrun对吧，因为他智商高。

对应每一个分数，是多大智商的人考出来的这件事，的概率是多少这件事，也可以写个P(智商y I x分)，

所谓最大似然就是说，最可能得到这个结果的系统参数。（最可能考这个分的那个人。）换成slam就是，最可能得这个观测值zi的位置xt

0.1 贝叶斯滤波

上式中，因为x1 x2 x3 都是独立事件，其概率和其他无关，所以，可以直接就以自身的概率来进行当前概率的展示;

状态xt的置信度  通过两个分布：（xt-1 的置信度 和 由控制变量ut 引起的从 xt-1 到 xt的转移概率）的积分（求和）得到，即 

朴素贝叶斯定理在进行卡尔曼滤波公式的推导中有着重要的使用，由上一步的对应后验  计算当前后验分布 

这个时候，就用到了朴素贝叶斯的特性：

由于当前的观测信息 zt 和控制指令 ut 以及之前的时刻的姿态 xt-1 无关，只与当前时刻的状态量 xt 有关，因此，可以简化为：

和  = 

而当前的状态量 xt 与 观测信息 zt  ，控制指令 ut 以及之前的状态量 xt-1 都有关系。所以，最终的结果就为：

由于  为常量，所以，用  来表示

因此有：

其中 由全概率定理（贝叶斯公式变化而来） 得到

又一次采用状态是完整的这个假设。这意味着如果知道  , 过去的测量和控制不会传递任何关于状态  的信息，因此有：

 这里保留控制变量ut，是因为它不能先于状态 

贝叶斯滤波伪代码实现

其中为初始置信度，  为测量概率、为状态转移概率 、 为置信度

贝叶斯滤波器分为两步：运动模型 和观测模型

参考：

通俗地解释卡尔曼滤波器（一）——从贝叶斯滤波器说起

0.2 粒子滤波

1, 四元数常用性质

 

2 ESKF

首先明确imu为什么要使用ESKF---为了避免漂移，避免漂移是将这些信息与绝对位置读数（例如GPS或视觉）融合在一起的问题

ESKF的全过程可以描述为如下步骤：

• 对高频率的IMU数据 um ​进行积分，获得nomimal state x 。（注意：nominal state里面不考虑噪声，因此势必会累积误差。这部分误差可依据error state进行修正）
• 利用Kalman Filter估计error state，这里同样也包括时间更新和量测更新两部分。（注意：这个过程考虑了噪声，并且由于误差状态方程是近似线性的，所以可以直接用卡尔曼滤波）
• 利用error state δx 修正nominal state，获取true state
• 重置error state

使用ESKF的优势：

1. error state 中的参数数量与运动自由度是相等的，避免了过参数化（over-parameterization or redundancy）引起的协方差矩阵奇异的风险。
2. error state 总是接近于0，Kalman Filter工作在原点附近。因此，远离奇异值、万向节锁，并且保证了线性化的合理性和有效性。
3. error state 总是很小，因此二阶项都可以忽略，因此雅可比矩阵的计算会很简单，很迅速。
4. error state 的变化平缓，因此KF修正的频率不需要太高。

2.1 ESKF 局部坐标系下误差推导

首先需要明白两个坐标系的定义。一是惯性系，二是IMU坐标系。

这里的惯性系就是指静止或者其速度的改变可以忽略不记的坐标系。通常在机器人的应用中，由于所用的IMU都是低成本IMU，对于地球自转角速度不够敏感，所以可以认为与地面固连的坐标系都是惯性坐标系。然而对于，航空航天应用而言，用到的IMU精度很高，对于惯性导航的要求也很严格，则需要考虑地球自转角速度，这是惯性坐标系为地球惯性坐标系。IMU测量的实际上就是IMU坐标系相对于惯性系的加速度和角速度。值得注意的是，这里的相对于惯性系的加速度不仅包括载体的运动加速度，还包括重力加速度

        （2.1.0）

需要注意的是，这里定义的旋转的误差状态是local的，  在R的右侧，  的参考系是局部坐标系。而IMU的gyro给出的测量也是相对于局部坐标系的。这种表达更容易使用IMU的测量

其中：                                    （2.1.1）

直接从IMU获得的加速度 at 、 角速度 ωt 和 body坐标系下带噪音的IMU测量值 am 、ωm的关系如下 ： 

                           （2.1.2）

因此有：

                            （2.1.3）

（2.1.3）代入（2.1.1）得IMU的运动方程：

                              （2.1.4）

• pt​：表示IMU系相对惯性系的位置，在惯性系下表示的，默认是三维的
• vt：表示IMU系相对惯性系的速度，同样是在惯性系下表示的，默认是三维的
• qt​：表示从IMU系到惯性系的四元数，表示从IMU系到惯性系的旋转变换

将true-state部分的kinematics方程中减去nominal-state就可以得到error-state的kinematics方程。

下面详细推导一下，速度和姿态部分。推导的方法是把true-state写成nominal+error的形式，然后把error部分提到方程的左侧即得到error-state的方程。

总结如下：error-state = true - nominal

2.2.0 推导位置

 其导数为

2.2.1 推导速度

其导数为 

由  

其中 

得惯性系中的真实加速度有两种形式（nominal形式不考虑噪音） 和 

          和   因此得：

左右消除  得：

忽略第二项的二次小项，利用叉乘性质 ，

加速度计的噪声为白色不相关且各向同性得

也就是说，协方差椭球是一个以原点为中心的球体，这意味着它的均值和协方差矩阵在旋转时是不变的，因此得：

2.2.2 推导旋转部分

首先有

的计算形式有nominal-state 和 True state  两种形式：

  

其中 

在这里

单独提取出  和 得：

最后一部分为二次小项，可以忽略：

 

2.2.3 The error-state Jacobian and perturbation matrices

误差状态更新 : 

协方差矩阵更新 : 

注意：滤波算法还会更新P 来控制协方差的权重

2.2.4 Correction equations

参考了VINS-Fusion的做法,采用了 GeographicLib 库,先把GPS数据转换至ENU笛卡尔坐标系: 这样我们就可以定义GPS位置观测方程为:

相对于error state的Jacobian为：

2.2.5 量测方程

通常，量测方程的基本形式如下：

为了实现量测更新，我们需要求 H 对 δx 的导数，H被定义为对errer求导而不是对nomial state求导：

所以有：

true state 关于 error state 求导

上式中

 

2.2.6 状态合并以及Error State 重置

在量测更新过后,把error state与nominal state合并，即修正nominal state随时间的漂移

状态合并 ： 

即

由于nominal state的误差已经修正了，所以error state要置零，同时协方差矩阵也要相应的修改，Error state 重置：

其中 

 ，g() 为 reset function  ，G是新误差关于观测误差的求导，以角度 θ 为例：

有 新角度误差  、老角度误差  、 观测误差 

1）错误复位后，真实方向不变

由  得

2）观测误差投影到 nominal state

结合1）2）得

设  得 

忽略 二阶 得

最终得

注意：Local angular error 和 global angular error 之间的各种变量推导结果的区别，上面推导的都是global angular error

上表中

其中