误差状态卡尔曼滤波（ESKF）推导

目录1. 概率基础知识1.1 独立1.2 全概率公式1.3 条件概率公式1.4 贝叶斯公式1.5 高斯概率密度函数1.6 联合高斯概率密度函数1.7 高斯随机变量的线性分布2.滤波器基本原理2.1 状态估计模型2.2 贝叶斯滤波2.3 卡尔曼滤波(KF)推导2.4 扩展卡尔曼滤波(EKF)推导3.误差状态卡尔曼滤波ESKF3.1 ESKF的引出3.2 ESKF的具体形式3.2.1 运动方程3.2.2 观测方程3.3 ESKF的使用注本篇笔记的主体内容来源于对深蓝学院《多传感器融合定位》课程的学习推荐有一定基础的SLAM 初学者学习该课程。课程中提供了一个完整的滤波器代码框架可供体会从理论到代码实现的过程。符号约定、预测值先验更新值后验误差状态1. 概率基础知识1.1 独立“独立” 描述两个随机变量互不影响。若 x 和 y 独立说明 “ x 发生的概率不会因 y 是否发生而改变 ” 反之亦然。联合概率p(x, y)独立时 “ x 且 y 发生 ” 的概率等价于 “ x 发生概率 ” 乘以 “ y 发生概率” 比如抛两枚独立硬币同时正面的概率 第一枚正面概率 × 第二枚正面概率 。条件概率p(x|y)“ 已知 y 发生时 x 发生的概率 ” 因独立无影响所以等于p(x)本身同理p(y|x) p(y)。1.2 全概率公式全概率公式用于“由局部推整体”若 y 是一组 “ 互斥且穷尽所有可能的事件 ” 比如 y 代表 “ 天气晴、雨、雪等 ” 覆盖所有可能那么 x 发生的总概率等于 “ y 取每个可能值时 x 发生的条件概率 ” 乘以 “ y 本身的概率 ” 再累加或积分比如 “ 求出门概率 p(x) ” 可拆成 “ 晴天时出门概率 × 晴天概率 雨天时出门概率 × 雨天概率 …” 。1.3 条件概率公式该公式描述了x与y不独立时的联合概率表达了联合概率与条件概率的转换 “ x 和 y 同时发生的概率 ” 既可以理解为 “ 已知 y 发生时 x 发生的概率乘以 y 发生的概率 ” 比如 “ 已知下雨 y 时打伞 x 的概率乘以下雨的概率 ” 也可以对称地理解为“ 已知 x 发生时 y 发生的概率乘以 x 发生的概率 ” 。1.4 贝叶斯公式该公式的核心是“由结果推原因” 逆概率 已知 “ 结果 y 发生 ” 求 “ 原因 x 导致 y ” 的概率。分子p(y|x)p(x)先验部分p(x)是 “ 原因 x 本身的概率先验概率 ” p(y|x)是 “ x 导致 y 的可能性似然 ” 。分母p(y)通过全概率公式计算的 “ 结果 y 发生的总概率证据 ” 用于归一化保证p(x|y)是合法概率和为 1 。比如 “ 已知检测阳性y求真正患病x的概率 ” 就需要用贝叶斯公式结合 “ 患病概率p(x)、患病时检测阳性概率p(y|x)、全体检测阳性概率p(y)” 推导 。1.5 高斯概率密度函数1一维情况下高斯概率密度函数表示为其中为均值为方差。2多维情况下高斯概率密度函数表示为其中为均值为方差。一般把高斯分布写成。1.6 联合高斯概率密度函数在SLAM过程中我们是根据内感传感器如IMU、轮速计的运动感知和外感传感器对当前环境的观测来进行自身定位的。使用概率来描述的话假设当前机器人位姿为x当前对环境的观测为y有高斯分布我们需要求解在已知当前观测y的条件下机器人位姿x是怎样的从概率的角度来看即p(x|y)。求解的过程可以从二者的联合概率密度出手因为x与y并不独立所以而二者的联合概率密度函数可以表示为由于高斯分布中指数项中包含方差的求逆而此处联合概率的方差是一个高维矩阵对它求逆的简洁办法是运用舒尔补。舒尔补的主要目的是把矩阵分解成上三角矩阵、对角阵、下三角矩阵乘积的形式方便运算即其中称为矩阵D关于原矩阵的舒尔补。此时有:利用舒尔补联合分布的方差矩阵可以写为其逆矩阵为则高斯分布指数部分可以表示为又因为所以高斯分布指数部分可以表示为经过化简原本复杂的联合分布二次项被拆分成了两个独立的部分 。第二项对应的是边缘分布的指数项 第一项对应的是条件分布的指数项从中可以直接读出后验均值和方差 所以这一步推导在数学上证明了两个变量的联合高斯分布其关于其中一个变量的条件分布仍然是高斯分布。1.7 高斯随机变量的线性分布在上面的例子中若已知和之间有如下关系其中G和C可以看作是两个系数矩阵为零均值白噪音在实际中指的是观测噪音。下面推导y与x的均值和方差之间的关系2.滤波器基本原理2.1 状态估计模型状态估计任务中待估计的状态的后验概率密度可以表示为其中代表状态初始值代表从1到k时刻的内感传感器输入如imu、轮速计代表从0到k时刻的外感传感器对环境的观测如激光雷达。因此滤波问题可以直观表示为根据所有历史数据(初始状态、输入、观测得出最终的融合结果。历史数据之间的关系可以用下面的图模型表示其中w代表imu积分的过程噪音imu测量的随机游走和bias随机游走f代表状态转移函数通常对应imu的积分过程x代表惯性解算递推出的状态估计值n代表观测噪音y代表该时刻的观测如激光雷达的扫描点云或者更直接的是scan to map得到的机器人位姿Tg代表观测函数表示根据当前状态x推导出理论的观测值。图模型中体现了马尔可夫性即当前状态只跟前一时刻状态相关和其他历史时刻状态无关。该性质的数学表达这个图中没有表示如何根据已知观测y去更新估计值x2.2 贝叶斯滤波根据贝叶斯公式时刻后验概率密度可以表示为其中代表先验概率。表示在获取当前观测数据 y 之前根据机器人上一时刻的位姿以及运动信息imu测量的角速度和加速度等 通过运动模型预测当前时刻机器人可能的位姿。这是对机器人状态的一个初步估计。代表似然函数。表示在已知机器人状态和先验条件的情况下得到观测数据 y 的概率。例如当机器人处于某个确定的位姿 x 且已知部分地图信息时激光雷达获取到特定距离数据 y 的概率。如果机器人的实际位姿与当前估计的位姿 x 越匹配那么得到观测数据 y 的可能性就越大。后验概率。这是我们最终想要得到的结果即在已知观测数据 y 和条件信息 v,y 的情况下机器人状态 x 的概率分布。它结合了先验信息和观测信息对机器人的状态进行了更准确的估计。其中可被看作归一化因子并省略得到可被忽略的原因之一可能为是k时刻的已知条件z系统已经运行到了k时刻那么k时刻之前的机器人状态x、内传感器输入、外传感器的观测都已知。在 SLAM 等实际应用场景中我们通常更关心的是在给定观测 y 和已知条件 z 下机器人状态 x 的概率分布情况。在条件贝叶斯公式中p(y∣z)表示在已知条件 z 下观测数据 y 出现的概率。从数学角度看对于特定的观测 y 和已知条件 z p(y∣z)是一个固定的值它不依赖于机器人状态 x 。也就是说无论 x 取何值只要 y 和 z 确定p(y∣z)都是恒定的。p(y∣z)作为分母只影响后验概率的相对大小。再回到后验概率公式。根据观测方程只与相关则上式可以简写为再利用全概率公式与马尔可夫性可得全概率公式的应用我们要推断现在的状态需要考虑上一时刻所有可能的状态并根据运动模型把它们“推演”到现在k时刻。经过以上化简最终后验概率可以写为上式完美地体现出滤波的过程预测更新根据当前的先验即上一时刻的后验与内传感器的输入通过运动学解算递推出当前时刻的状态预测值。观测更新根据当前的观测修正状态量的预测值纠正imu积分产生的漂移。2.3 卡尔曼滤波(KF)推导再回头看运动方程和观测方程。常规的运动方程和观测方程如下所示运动方程观测方程其中k时刻状态的先验预测值。k-1时刻状态的后验值经过k-1时刻观测修正后的。k时刻imu的测量值角速度、加速度。状态转移函数描述系统状态如何随时间演变对应imu积分的过程。过程噪音由imu的测量白噪音随机游走和bias随机游走组成服从高斯分布。过程噪音系数矩阵描述了过程噪音如何影响当前状态。k时刻的观测如激光雷达的scan to map得到的位姿T。观测函数描述观测的过程。观测噪音描述观测的误差服从高斯分布。观测噪音系数矩阵描述了观测噪音如何影响当前观测。在线性高斯假设下对于线性系统运动方程和观测方程可以改写为运动方程观测方程把上一时刻的后验状态写为1当前时刻状态的预测值先验均值为这个预测值在数学本质上是运动方程的期望均值。由于我们假设过程噪声为零均值白噪声对其求期望后该项为零。因此在推算先验状态时噪声项自然消退而非被忽略噪声的真实影响将在随后的协方差预测方程中予以体现。2根据高斯分布的线性变化此时的先验方差为仿照1.7节中的推导3观测y的期望均值为4观测y的方差为再根据1.6中推导的后验概率分布以及1.7中的推导令这个K即为卡尔曼增益。卡尔曼增益是按不确定度自动计算的、逐方向的最优加权矩阵标量情形最直观。设先验方差、观测方差取激光很不可信如退化环境、匹配残差大结果信 IMUIMU 先验很不准如长时间无观测、偏置未收敛信激光两者相当时折中。所以就是观测占多大话语权是先验占多大话语权。同时负责跨状态传播把激光的位姿残差通过状态间的相关性传递给速度、偏置、重力等未被直接观测的量。后验均值为后验方差为如此便得到了卡尔曼经典的五个方程从贝叶斯滤波到卡尔曼滤波如何让概率落地在上一节中我们看到了贝叶斯滤波的终极公式。它在理论上非常完美融合了全概率公式与贝叶斯推断但它是一个包含了无穷积分的连续函数表达。在实际工程和计算机中我们不可能遍历连续空间去硬算积分维度灾难。为了让这个完美的理论框架能够真正在工程中落地卡尔曼Kalman引入了两个极其伟大的假设系统是线性的且噪声服从高斯分布。高斯分布有一个极其迷人的数学特权它的线性组合依然是高斯分布。这直接为贝叶斯滤波的两个步骤带来了“降维打击”般的捷径1.化解“积分”噩梦预测阶段贝叶斯滤波中预测阶段那庞大复杂的积分因为高斯分布的线性映射性质现在根本不需要积分了只需要利用我们在1.7 节提到的高斯随机变量的线性变换做简单的矩阵乘法和加法均值和方差的递推就能搞定。2.化解“相乘配方”噩梦更新阶段贝叶斯滤波的更新阶段本质是“似然乘以先验”即两个高斯分布的概率密度函数相乘。如果硬算这两个包含指数项的式子并强行凑成一个新的高斯分布代数过程极其痛苦。此时我们在1.6 节中辛苦推导的联合高斯概率密度函数和舒尔补终于派上了用场因为状态和观测必定构成联合高斯分布我们只需利用舒尔补对联合协方差矩阵进行降维分解就能直接得出相乘后的条件分布。核心总结卡尔曼滤波推导中使用“联合分布求条件概率”在数学本质上与贝叶斯公式的“似然乘以先验”是完全等价的。它只是借用了联合高斯的数学特权巧妙地避开了微积分和繁琐的指数配方。2.4 扩展卡尔曼滤波(EKF)推导在标准 KF 中假设系统是严格线性的矩阵乘法。但在真实的 SLAM 或机器人世界里运动和观测比如scan to map几乎全是非线性的。如果把高斯分布强行塞进非线性函数输出的就不再是高斯分布了KF 的理论基础就会崩塌。EKF 的核心思想就是“既然世界是非线性的那我就在当前工作点附近把它切成一小段一小段的直线局部线性化然后强行套用 KF 的公式。”1定义非线性的真实世界。用非线性函数和来替换掉 KF 中的常数矩阵F和G非线性运动方程非线性观测方程2由于f和g没法直接算方差需要使用一阶泰勒展开将它们线性化:1对运动方程展开。选择在上一时刻的最佳后验估计值处进行展开并假设噪声服从高斯分布为了书写方便我们定义无噪声下的名义预测值状态雅可比矩阵 (Jacobian)噪声雅可比矩阵 (Jacobian)代入后非线性运动方程被成功“拍扁”成了线性方程2对观测方程展开。同理我们在当前时刻的先验预测值处对观测方程进行展开并假设观测噪声服从高斯分布定义无噪声下的理论观测值观测雅可比矩阵观测噪声雅可比矩阵展开后的线性化观测方程为3推导均值与方差。1预测阶段先验因为且的最佳估计就是此时所以先验均值就是非线性函数的结果方差2观测阶段后验期望方差此时的偏差项为观测的不确定性由两部分组成1.条件方差由于传感器本身不准带来的误差。2.状态预测误差此时得到了EKF经典的五个方程注状态预测和状态更新时的计算直接代入非线性方程计算更准。KF中的矩阵F和G是常数矩阵而EKF中的F和G是雅可比矩阵。3.误差状态卡尔曼滤波ESKF3.1 ESKF的引出EKF 的精度极大地依赖于线性化点的准确性。EKF中会进行两次线性化展开一是对运动方程展开在上一时刻的最佳后验估计值处进行展开二是对观测方程在当前时刻的先验预测值处对观测方程进行展开。如果初始误差太大或者运动太过剧烈导致非线性极强被一阶展开无情抛弃的高阶项就会引入巨大的截断误差导致滤波器发散。并且当系统引入3D姿态如四元数q或旋转矩阵R时直接使用基于名义状态的滤波模型会遇到致命问题卡尔曼滤波的假设是状态变量可以进行线性叠加即状态变量可以加上高斯噪声。但是四元数必须满足模长为 1旋转矩阵必须是正交的。如果给四元数直接加上一个高斯噪声相加后的结果不再是一个合法的四元数这就好比你让一个只能在球面上移动的点加上了一个直线向量它直接飞出了球面。为了解决这两大痛点误差状态卡尔曼滤波ESKF应运而生。它放弃了直接估计高度非线性的系统“名义状态”转而采用“分而治之”的思想把非线性的大范围运动交给名义状态去积分而滤波器只负责估计帧间微小的、线性的不确定性噪声误差状态。对于非线性系统虽然系统的大范围运动名义状态是非线性的但只要传感器的更新频率足够高帧间累积的误差往往非常微小。只要及时进行状态清零与补偿误差状态就会始终被限制在极小的范围内。这使得我们可以在工作点附近进行局部线性化认为误差状态的传播是一个高精度的线性马尔可夫过程。此时对于旋转不直接估计四元数而是估计一个三维的误差角。这个三维向量是在切空间李代数空间中的它没有任何长度或正交性约束完美符合高斯分布的假设。在更新时利用指数映射或一阶近似将误差“乘”回名义姿态中天然保证了旋转的合法性。每次观测更新后ESKF都会把误差注入名义状态并将误差状态清零。这意味着我们的误差状态永远是一个围绕着 0 的无穷小量。在 0 附近对误差模型进行线性化截断误差微乎其微使得滤波器的表现更接近理论最优。3.2 ESKF的具体形式ESKF属于KF所以运动方程和观测方程沿用线性表示运动方程观测方程此时来看两个方程中每个量的具体形式3.2.1 运动方程运动方程设当前待估计的状态为分别为位置误差、姿态误差用旋转向量表示、速度误差、加速度计零偏的误差、陀螺仪零偏的误差。过程噪音为分别加速度计的测量白噪音随机游走、陀螺仪的测量白噪音、加速度计bias随机游走、陀螺仪bias随机游走。根据上一篇笔记《惯性导航解算及误差分析》中推导得到连续时间下的误差微分方程得其中将连续微分方程变成离散化的状态递推公式对于齐次部分系统本身的矩阵指数进行一阶泰勒展开得对于非齐次部分噪音简化得到其中为滤波周期即两帧imu数据间的时间差。注关于的离散化形式不同资料有差异但对实际调试影响不大。3.2.2 观测方程观测方程假设此时以lidar的scan to map得到的位姿T作为观测量则观测量包含位置、失准角即观测噪音。 此时有G为观测雅可比矩阵C为观测噪音雅可比矩阵。需要注意的是观测量y的值并不由观测方程计算得到观测方程主要用于问题建模进行观测均值和方差的推导。1观测量中的计算过程为其中为imu积分得到的预测值为lidar位姿T中的位置。2的计算需要先计算旋转误差矩阵再进行对数映射得到失准角旋转误差向量:或者可以取巧由于预测值与观测值之间的关系为所以将以上各变量带入kalman滤波的五个方程即可构建完整的滤波器更新流程。3.3 ESKF的使用1位姿初始化在点云地图中实现初始定位并给以下变量赋值2Kalman 初始化初始方差理论上可设置为各变量噪声的平方实际中一般故意设置大一些这样可加快收敛速度。过程噪声与观测噪声一般在 kalman 迭代过程中保持不变。3惯性解算1姿态解算该位姿属于先验预测位姿。2速度解算3位置解算4Kalman 预测更新执行kalman五个步骤中的前两步即由于我们假设过程噪声为零均值白噪声对其求期望后噪声项为零。因此在推算先验状态时噪声项自然消退而非被忽略噪声的真实影响将在随后的协方差预测方程中予以体现。这里需要实时根据公式计算和。5无观测时后验更新无观测时不需要执行kalman剩下的三个步骤后验等于先验即6有观测时的量测更新执行kalman滤波后面的三个步骤得到后验状态量7有观测时计算后验位姿真值 名义状态 - 误差状态根据后验状态量更新后验位姿将误差从名义状态中剔除8有观测时状态量清零状态量已经用来补偿因此需要清零后验方差保持不变。状态的不确定性保持不变9输出位姿把后验位姿输出给其他模块使用。