SLAM001.filter based - Kalman filter and state estimation

---

花了一晚上读懂了一篇综述中提及的滤波方法下的SLAM鼻祖论文MonoSLAM中提到的vSLAM下的Kalman滤波方法， 为了避免自己遗忘，特意撰写此文。本文将从符号推导到具体数值案例，系统地讲解MonoSLAM中的EKF状态估计方法。

相机内参矩阵 描述了相机坐标系到像素坐标系的映射关系：

其中 为焦距， 和 为主点坐标。

对于世界坐标系中的点 ，其在相机坐标系下的坐标为 ，其中 为由四元数 得到的旋转矩阵， 为相机位置。

归一化平面坐标为 ，像素坐标为：

状态向量 描述系统在时刻 的完整状态。对于MonoSLAM：

其中 为相机位置（3维）， 为相机姿态四元数（4维）， 为线速度（3维）， 为角速度（3维）， 为第 个地图点坐标（3维）。

状态维度：

观测方程描述状态到观测的映射：

其中 为投影函数， 为观测噪声。

对于单个地图点 ，其观测预测为：

其中 为透视投影函数。

系统方程描述状态随时间的演化：

其中 为运动模型函数， 为过程噪声。

对于匀速运动模型：

（地图点假设为静态）

在深入数值案例之前，我们需要先建立完整的符号体系和数学框架。这一节会详细推导每一个公式，解释每一个符号的含义，让读者真正理解EKF的每一步在做什么，而不仅仅是记住公式。

首先，我们需要建立一个世界坐标系作为参考。假设机器人在这个世界坐标系中移动，相机固定在机器人上的某个位置。在初始时刻（第0帧），我们已知相机位于世界坐标原点 ，并且我们有 个已知的特征点（靶点）在世界坐标系中的真实位置，第 个靶点坐标记为 。

相机的内参矩阵 描述了相机自身的光学特性：

这里的 是焦距（单位：像素）， 和 是主点坐标（图像中心点的像素坐标）。例如，对于一个 分辨率的相机，如果焦距为500像素，则内参矩阵为：

状态向量是卡尔曼滤波器的核心，它包含了我们想要估计的所有未知量。在MonoSLAM中，我们需要估计相机的运动状态和环境中的特征点位置。

相机状态 包含四个部分：

让我们逐一解释每个分量：

：相机位置。这是一个3维向量 ，表示相机在世界坐标系中的位置。例如， 表示相机在世界坐标原点上方0.5米处。

：相机姿态四元数。这是一个4维向量 ，用于表示相机的旋转。四元数相比欧拉角的优势在于没有万向锁问题，相比旋转矩阵的优势在于参数更少（4个vs 9个）且易于插值。单位四元数满足 ，例如 表示无旋转。

：相机线速度。这是一个3维向量 ，表示相机在世界坐标系中的运动速度（单位：m/s）。

：相机角速度。这是一个3维向量 ，表示相机的旋转速度（单位："rad"/s）。

除了相机状态，状态向量还包含所有已知的地图点：

（第 个地图点在世界坐标系中的位置）

因此，完整的状态向量为：

状态向量的维度：相机状态占 维，每个地图点占3维，总共 维。例如，如果有4个地图点，状态向量就是 维。

卡尔曼滤波器的精髓在于它不仅估计状态本身，还估计状态的不确定性。这种不确定性用协方差矩阵来表示。

是一个 维的方阵，表示初始状态估计的不确定性。对角线元素 表示第 个状态分量的方差（不确定性的平方），非对角线元素 表示第 个和第 个状态分量之间的相关性。

在实际应用中，我们通常假设初始时刻各状态分量相互独立，因此 设为对角矩阵：

这里的每个 块本身也是一个对角矩阵。例如， 是 的对角矩阵，表示位置三个分量的方差：

如果 ，意味着位置的标准差为 米，即我们初始估计的位置可能有 米的误差。

为什么需要过程噪声？

让我们思考一个问题：如果我们完全相信运动模型是完美的，会发生什么？

假设我们使用匀速运动模型预测相机的下一帧位置。随着预测的进行，我们会越来越"相信"自己的预测，状态的不确定性（协方差）会越来越小。最终，滤波器会变得"过度自信"，完全忽略新的观测数据。这就像一个人只相信自己的推测，从不听取别人的意见，最终会走向错误的方向。

过程噪声 的作用就是防止这种"过度自信"。它在每一步预测中，为状态的不确定性注入一个"最小保障量"，告诉滤波器："运动模型不是完美的，要保留一定程度的怀疑。"

的数学形式：

是一个 维的对角矩阵：

每个对角块的含义：

：位置预测噪声方差。单位是 。值越大，表示越不相信匀速模型能准确预测位置。例如， 表示每一步预测可能引入约 米的位置误差。

：姿态预测噪声方差。单位是 。例如， 表示每一步预测可能引入约 "rad"（约 度）的姿态误差。

：速度预测噪声方差。单位是 。速度的变化意味着加速度的存在，这个噪声反映了加速度的不确定性。

：角速度预测噪声方差。单位是 。反映了角加速度的不确定性。

：地图点噪声方差。由于我们假设地图点是静态的，通常设为0或极小值。

如何选择 的值？

这是一个需要经验的问题。一般来说：

• 如果运动比较平稳（如轮式机器人在平坦地面行驶）， 可以设小一些
• 如果运动比较剧烈（如手持相机快速移动）， 需要设大一些
• 可以通过实验调整：如果滤波器响应太慢，增大 ；如果估计结果抖动太大，减小

观测噪声的来源

当我们从图像中提取特征点时，得到的像素坐标 并不是完全精确的。这是因为：

1. 图像传感器的噪声
2. 特征点检测算法（如FAST、ORB）的定位误差
3. 镜头畸变
4. 运动模糊

观测噪声协方差 量化了这种测量误差。

的数学形式：

对于单个特征点的观测 （像素坐标）， 是一个 矩阵：

通常假设 和 方向的误差独立且方差相同：

这表示特征点提取的标准差为 像素。也就是说，我们提取的像素坐标可能有 像素的误差。

卡尔曼增益 决定了我们在预测值和观测值之间如何权衡。让我们从直观上理解这个权衡：

的计算公式是：

这里有一个关键的分母： ，称为"创新协方差"（Innovation Covariance）。

情况1：观测很可靠（ 很小）

如果 相对于 很小，则 小， 大，因此 大。大的 意味着我们更信任观测，状态更新会更多地偏向观测值。

情况2：观测不可靠（ 很大）

如果 相对于 很大，则 大， 小，因此 小。小的 意味着我们更信任预测，状态更新会更多地保留预测值。

情况3：预测不确定（ 很大）

受 的影响： 大，则 大。如果预测的不确定性很大， 就大， 就大，我们就会更信任观测。

这符合直觉：如果我们对自己的预测不太确定，就应该多听听观测的意见。

我们假设相机做匀速运动，即速度和角速度保持不变。这是一个简化模型，实际中相机可能会有加速和减速，这部分误差由过程噪声 来补偿。

在时间间隔 内：

位置更新：相机沿速度方向移动

写成矩阵形式（只考虑位置和速度）：

姿态更新：相机绕角速度方向旋转

四元数的更新公式为：

其中 是小角度旋转对应的四元数：

当角速度为0时， ，姿态不变。

速度和角速度：假设保持不变

地图点：假设为静态

雅可比矩阵 描述了状态转移函数对状态的敏感程度。它的每个元素定义为：

对于匀速运动模型， 是分块对角矩阵：

其中 是 的矩阵，描述相机状态的转移：

让我们详细解释这个矩阵的每一块：

位置对位置的导数： （单位矩阵，因为 直接包含 ）

位置对速度的导数： （位置变化量与速度成正比）

姿态对姿态的导数： （小角度近似）

姿态对角速度的导数： （小角度近似）

速度和角速度的导数：都是单位矩阵，因为假设不变

地图点的导数：单位矩阵，因为假设静态

预测步骤分为两部分：状态预测和协方差预测。

状态预测就是根据运动模型，从当前状态估计下一时刻的状态：

这里的下标 表示"在 时刻对 时刻的估计"。

具体计算：

协方差预测描述不确定性的传播。根据误差传播定律：

这个公式可以这样理解：

• ：当前不确定性通过运动模型传播到下一时刻
• ：加上运动模型本身的不确定性

让我们展开这个矩阵乘法。假设 是对角矩阵（各状态分量独立）：

则 的结果中，位置协方差会变成：

这意味着位置的不确定性会随时间增长，增长速度与速度的不确定性成正比。这符合直觉：如果我们不确定速度，时间越长，位置的不确定性就越大。

更新步骤是将观测信息融入状态估计的过程。

首先，我们需要根据预测的状态计算"应该观测到什么"。对于地图点 ，它在相机坐标系下的位置是：

这里 是从四元数 计算得到的旋转矩阵。设 ，则归一化坐标为 ，像素坐标为：

观测残差（也称为"创新"）是实际观测与预测观测之间的差异：

这个残差反映了预测状态与真实状态之间的偏差。

观测雅可比矩阵 描述了观测函数对状态的敏感程度。对于单个地图点 ， 是一个 的矩阵。

我们需要计算观测 对状态各分量的偏导数。设 ，则：

这些公式看起来复杂，但可以这样理解：

• 当相机位置 变化时，地图点在相机坐标系下的位置 会变化
• 的变化会导致归一化坐标 变化
• 最终导致像素坐标 变化

类似地，可以计算观测对地图点 的偏导数：

注意对 和对 的偏导数符号相反，因为 和 在 中的符号相反。

创新协方差描述了观测残差的不确定性：

这里：

• ：状态不确定性对观测的影响
• ：观测本身的噪声

是一个 矩阵（对于单个特征点观测）。

卡尔曼增益是最优权重，它决定了我们应该在多大程度上相信观测残差：

是一个 的矩阵。它的每一行告诉我们：当观测残差在 或 方向变化时，对应的状态分量应该如何调整。

状态更新公式：

这个公式可以这样理解：我们用观测残差 乘以卡尔曼增益 ，得到一个修正量，加到预测状态上。

协方差更新公式：

这个公式描述了观测如何减小不确定性。 是一个小于1的因子（在某种意义上），使得更新后的协方差小于更新前。

现在，让我们通过一个完整的数值案例，一步步演示EKF的工作过程。我们会计算每一个中间结果，让读者能够验证每一步。

我们使用一个模拟相机，参数如下：

内参矩阵：

这意味着：

• 焦距 像素
• 主点 ，即图像中心

图像尺寸： 像素

帧率：30fps，即帧间时间间隔 秒

我们在世界坐标系中设置4个特征点，它们都位于 平面上：

这四个点构成一个 的正方形。

位置：

相机位于世界坐标原点上方0.5米处，俯视 平面。

姿态：

单位四元数，表示无旋转。相机光轴指向 轴负方向（向下看）。

线速度：

相机以 m/s的速度沿 轴正方向移动。

角速度：

无旋转。

过程噪声协方差 ：

我们设置如下对角元素：

具体含义：

• 位置噪声： （标准差 米）
• 姿态噪声： （标准差约 度）
• 速度噪声：
• 角速度噪声：
• 地图点噪声：

观测噪声协方差 ：

这意味着特征点提取的标准差为 像素。

根据相机状态和地图点，构造完整的状态向量：

代入具体值：

状态维度： 维

是一个 的对角矩阵，对角元素按顺序为：

• 位置（3个）：
• 四元数（4个）：
• 速度（3个）：
• 角速度（3个）：
• 地图点 （3个）：
• 地图点 （3个）：
• 地图点 （3个）：
• 地图点 （3个）：

让我们计算每个地图点在帧0图像中的投影位置。

对于 ：

第一步，计算相机坐标系下的位置：

由于 表示无旋转， （单位矩阵），所以：

注意 是负数，这是因为相机在 处向下看， 平面上的点在相机坐标系中 坐标为负。

第二步，计算归一化坐标：

第三步，计算像素坐标：

所以 的观测为 ，正好在图像中心。

对于 ：

归一化坐标：

像素坐标：

超出了图像范围 ，所以 在帧0中不可见。

类似地， 和 也不可见。因此，在帧0中，我们只能观测到 。

根据匀速运动模型，预测帧1的状态：

位置预测：

相机沿 轴移动了 米（约 毫米）。

姿态预测：

由于 ，所以 ，因此：

姿态不变。

速度和角速度预测：

地图点预测：

（假设静态）

对于匀速模型， 是分块对角矩阵。我们只关注相机状态部分（ ）：

代入 ， 是一个 的分块矩阵。其关键特点是：位置部分的前3行包含速度项 ，其余部分接近单位矩阵。

位置部分（前3行，前6列）的具体数值：位置对位置的导数为 ，位置对速度的导数为 。

由于 接近单位矩阵，且 和 都是对角矩阵，我们可以近似计算对角元素：

位置协方差：

位置不确定性从 米增加到 米。

速度协方差：

速度不确定性增加。

在实际系统中，观测值来自相机图像。这里我们模拟一个"真实"的运动，然后计算观测。

假设真实运动有轻微扰动（模拟过程噪声）：

注意真实位置 与预测位置 略有不同。

使用真实位姿计算 的观测：

第一步，计算相机坐标系下的位置：

由于 接近单位四元数， ，所以：

第二步，计算归一化坐标：

第三步，计算像素坐标：

所以真实观测为 。

使用预测状态计算"应该观测到什么"：

归一化坐标：

像素坐标：

所以预测观测为 。

像素

这个残差告诉我们：预测的观测位置与实际观测位置有 像素的偏差。

我们需要计算观测 对位置 和地图点 的偏导数。

已知：

• （单位矩阵，因为姿态无旋转）

对位置 的偏导数：

对地图点 的偏导数：

符号与对 的偏导数相反：

因此， 是一个 的稀疏矩阵，只有对应 和 的位置有非零元素。

注意，我们没有对四元数部分求偏导，因为我们一开始数值上就假设没有旋转，这导致了这一部分求导为0，在一般情况下观测函数都需要对四元数求偏导。

由于 是对角矩阵，且 只有少数非零元素，我们可以简化计算：

的主对角元素近似为： ， 。

所以：

加上观测噪声 ：

首先计算 ：

然后计算 。对于位置 的前两行：

对于位置 ：

可以看到，更新后的位置更接近真实位置 。

由于 很小（约 量级）， ，所以 ，但略有减小。这说明观测确实减小了状态的不确定性，但减小量很小，这是因为：

1. 观测只提供了一个点的信息
2. 卡尔曼增益很小（状态不确定性相对于观测噪声较大）

当相机移动到新位置时，可能会检测到新的特征点。这些新点需要被添加到状态向量中。

假设在帧2，我们在图像坐标 处检测到一个新特征点 。

新检测的特征点深度未知，我们使用逆深度参数化来初始化。

第一步，从像素坐标反投影射线方向：

计算 ：

第二步，归一化射线方向：

第三步，假设初始深度。我们假设初始深度 米，则逆深度 。

第四步，计算新点的世界坐标：

假设当前相机位置 ，姿态 ：

将新点 添加到状态向量末尾：

状态维度从25增加到28。

其中：

• ：原有的协方差矩阵
• ：新点的协方差，设为 （逆深度不确定性较大）
• ：新点与旧状态的协方差，初始设为0（假设独立）

让我们总结一下整个过程中关键数值的变化：

帧0（初始）：

• 相机位置：
• 点 观测：
• 状态维度：25
• 位置协方差：

帧1（预测后）：

• 相机位置预测：
• 点 预测观测：
• 位置协方差： （不确定性增加）

帧1（更新后）：

• 相机位置更新：
• 点 实际观测：
• 位置协方差：略小于 （观测减小了不确定性）

观测残差： 像素

卡尔曼增益：约

这个案例展示了EKF如何通过预测和更新两个步骤，逐步融合运动模型和观测信息，得到更准确的状态估计。

本文从符号推导到具体数值案例，系统地讲解了MonoSLAM中的EKF状态估计方法。让我们回顾几个关键要点：

状态向量的设计：状态向量包含了我们想要估计的所有未知量——相机位姿、速度和地图点位置。维度为 ，其中 是地图点数量。

协方差矩阵的意义：协方差矩阵量化了状态估计的不确定性。对角元素表示各状态分量的方差，非对角元素表示分量之间的相关性。

过程噪声 的作用： 防止滤波器过度自信。它告诉滤波器：运动模型不是完美的，要保留一定程度的怀疑。

观测噪声 的作用： 描述观测的可靠性。 小意味着观测精确，滤波器会更信任观测； 大意味着观测嘈杂，滤波器会更信任预测。

卡尔曼增益 的本质： 是在预测和观测之间的最优权重，由 和 共同决定。

通过具体数值案例，我们看到了EKF如何一步步计算每个中间结果。这种"白盒"的理解对于调试和改进SLAM系统至关重要。希望读者通过本文，不仅学会了EKF的公式，更理解了公式背后的物理意义和设计思想。