目录
[1. 概率基础知识](#1. 概率基础知识)
[1.1 独立](#1.1 独立)
[1.2 全概率公式](#1.2 全概率公式)
[1.3 条件概率公式](#1.3 条件概率公式)
[1.4 贝叶斯公式](#1.4 贝叶斯公式)
[1.5 高斯概率密度函数](#1.5 高斯概率密度函数)
[1.6 联合高斯概率密度函数](#1.6 联合高斯概率密度函数)
[1.7 高斯随机变量的线性分布](#1.7 高斯随机变量的线性分布)
[2.1 状态估计模型](#2.1 状态估计模型)
[2.2 贝叶斯滤波](#2.2 贝叶斯滤波)
[2.3 卡尔曼滤波(KF)推导](#2.3 卡尔曼滤波(KF)推导)
[2.4 扩展卡尔曼滤波(EKF)推导](#2.4 扩展卡尔曼滤波(EKF)推导)
[3.1 ESKF的引出](#3.1 ESKF的引出)
[3.2 ESKF的具体形式](#3.2 ESKF的具体形式)
[3.2.1 运动方程](#3.2.1 运动方程)
[3.2.2 观测方程](#3.2.2 观测方程)
[3.3 ESKF的使用](#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并不独立，所以：
而二者的联合概率密度函数可以表示为：
由于高斯分布中指数项中包含方差的求逆，而此处联合概率的方差是一个高维矩阵，对它求逆的简洁办法是运用舒尔补。
舒尔补的主要目的是把矩阵分解成上三角矩阵、对角阵、下三角矩阵乘积的形式，方便运算，即：
其中
利用舒尔补，联合分布的方差矩阵可以写为：
其逆矩阵为：
则高斯分布指数部分
可以表示为：
又因为：
所以高斯分布指数部分可以表示为：
经过化简，原本复杂的联合分布二次项被拆分成了两个独立的部分 。第二项对应的是边缘分布的指数项 ：
第一项对应的是条件分布的指数项：
从中可以直接读出后验均值和方差 ，所以：
这一步推导在数学上证明了：两个变量的联合高斯分布，其关于其中一个变量的条件分布仍然是高斯分布 。
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得到的机器人位姿T），g 代表观测函数，表示根据当前状态x推导出理论的观测值。
图模型中体现了马尔可夫性，即当前状态只跟前一时刻状态相关，和其他历史时刻状态无关。该性质的数学表达：
（这个图中没有表示如何根据已知观测y去更新估计值x）
2.2 贝叶斯滤波
根据贝叶斯公式，𝑘时刻后验概率密度可以表示为：
其中：
一时刻的位姿以及运动信息（imu测量的角速度和加速度等 ），通过运动模型预测当前时刻机器人可能的位姿。这是对机器人状态的一个初步估计。
其中，可被看作归一化因子并省略，得到：
- 在 SLAM 等实际应用场景中，我们通常更关心的是在给定观测 y 和已知条件 z 下，机器人状态 x 的概率分布情况。
- 在条件贝叶斯公式中， p(y∣z) 表示在已知条件 z 下观测数据 y 出现的概率。从数学角度看，对于特定的观测 y 和已知条件 z ， p(y∣z) 是一个固定的值，它不依赖于机器人状态 x 。也就是说，无论 x 取何值，只要 y 和 z 确定， p(y∣z) 都是恒定的。**p(y∣z)**作为分母，只影响后验概率的相对大小。
再回到后验概率公式。根据观测方程，只与
相关，则上式可以简写为：
再利用全概率公式与马尔可夫性可得：
全概率公式的应用：我们要推断现在的状态
经过以上化简，最终后验概率可以写为：
上式完美地体现出滤波的过程：
预测更新 ：根据当前的先验 （即上一时刻的后验 ）与内传感器的输入 ，通过运动学解算，递推出当前时刻的状态预测值。
观测更新 ：根据当前的观测 ，修正状态量的预测值（纠正imu积分产生的漂移）。
2.3 卡尔曼滤波(KF)推导
再回头看运动方程和观测方程。常规的运动方程和观测方程如下所示：
运动方程：
观测方程：
其中：
在线性高斯假设下，对于线性系统，运动方程和观测方程可以改写为：
运动方程：
观测方程：
把上一时刻的后验状态写为：
（1）当前时刻状态的预测值（先验均值）为：
（这个预测值在数学本质上是运动方程的期望均值。由于我们假设过程噪声为零均值白噪声，对其求期望后该项为零。因此，在推算先验状态时噪声项自然消退，而非被忽略；噪声的真实影响将在随后的协方差预测方程中予以体现。）
（2）根据高斯分布的线性变化，此时的先验方差为（仿照1.7节中的推导）：
（3）观测y的期望均值为：
（4）观测y的方差为：
再根据1.6中推导的后验概率分布：
以及1.7中的推导：
令：
这个K 即为卡尔曼增益，则后验均值为：
后验方差为：
如此，便得到了卡尔曼经典的五个方程：
从贝叶斯滤波到卡尔曼滤波：如何让概率落地？在上一节中，我们看到了贝叶斯滤波的终极公式。它在理论上非常完美（融合了全概率公式与贝叶斯推断），但它是一个包含了无穷积分的连续函数表达。在实际工程和计算机中，我们不可能遍历连续空间去硬算积分（维度灾难）。
为了让这个完美的理论框架能够真正在工程中落地，卡尔曼（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）观测阶段（后验）：
期望：
**方差：**此时的偏差项为：
观测的不确定性
由两部分组成：
（条件方差） ：由于传感器本身不准带来的误差。
（状态预测误差）
此时，得到了EKF经典的五个方程：
注：状态预测和状态更新时的计算，直接代入非线性方程计算更准。
KF中的矩阵F 和G 是常数矩阵；而EKF中的F 和G 是雅可比矩阵。
3.误差状态卡尔曼滤波（ESKF）
3.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的值并不由观测方程计算得到，观测方程主要用于问题建模，进行观测均值和方差的推导。
观测量中的计算过程为：
其中，为imu积分得到的预测值，
为lidar位姿T中的位置。
的计算需要先计算旋转误差矩阵：
再进行对数映射得到失准角:
或者可以取巧，由于预测值与观测值之间的关系为：
所以：
将以上各变量，带入kalman滤波的五个方程，即可构建完整的滤波器更新流程。
3.3 ESKF的使用
（1）位姿初始化
在点云地图中实现初始定位，并给以下变量赋值：
（2）Kalman 初始化
初始方差理论上可设置为各变量噪声的平方，实际中一般故意设置大一些，这样可加快收敛速度。
过程噪声与观测噪声一般在 kalman 迭代过程中保持不变。
（3）惯性解算
1）姿态解算：
该位姿属于先验预测位姿。
2）速度解算：
3）位置解算：
（4）Kalman 预测更新
执行kalman五个步骤中的前两步，即：
（由于我们假设过程噪声为零均值白噪声，对其求期望后噪声项为零。因此，在推算先验状态时噪声项自然消退，而非被忽略；噪声的真实影响将在随后的协方差预测方程中予以体现。）
这里需要实时根据公式计算
和
。
（5）无观测时后验更新
无观测时，不需要执行kalman剩下的三个步骤，后验等于先验，即：
（6）有观测时的量测更新
执行kalman滤波后面的三个步骤，得到后验状态量：
（7）有观测时计算后验位姿
真值 = 名义状态 - 误差状态
根据后验状态量，更新后验位姿：
（将误差从名义状态中剔除）
（8）有观测时状态量清零
状态量已经用来补偿，因此需要清零：
后验方差保持不变。（状态的不确定性保持不变）
（9）输出位姿
把后验位姿输出给其他模块使用。