kalman filter and state estimation
0.Perface
花了一晚上读懂了一篇综述中提及的滤波方法下的SLAM鼻祖论文MonoSLAM中提到的vSLAM下的Kalman滤波方法， 为了避免自己遗忘，特意撰写此文。本文将从符号推导到具体数值案例，系统地讲解MonoSLAM中的EKF状态估计方法。
1.Foundation
1.1. 相机模型，相机坐标系，归一化平面
相机内参矩阵 描述了相机坐标系到像素坐标系的映射关系：
其中 为焦距， 和 为主点坐标。
对于世界坐标系中的点 ，其在相机坐标系下的坐标为 ，其中 为由四元数 得到的旋转矩阵， 为相机位置。
归一化平面坐标为 ，像素坐标为：
1.2. 状态方程，观测方程，系统方程
状态方程
状态向量 描述系统在时刻 的完整状态。对于MonoSLAM：
其中 为相机位置（3维）， 为相机姿态四元数（4维）， 为线速度（3维）， 为角速度（3维）， 为第 个地图点坐标（3维）。
状态维度：
观测方程
观测方程描述状态到观测的映射：
其中 为投影函数， 为观测噪声。
对于单个地图点 ，其观测预测为：
其中 为透视投影函数。
系统方程（运动模型）
系统方程描述状态随时间的演化：
其中 为运动模型函数， 为过程噪声。
对于匀速运动模型：
（地图点假设为静态）
2.Kalman Filter 符号推导
2.1. 初始设置
世界坐标系与相机参数
首先我们有一个世界坐标系，假设机器人在世界坐标系中移动，相机在机器人上的某个位置。初始相机位置为 。我们拥有 个靶点的真实位置，第 个靶点坐标为 。
相机内参矩阵为：
状态向量构造
构造输入卡尔曼滤波器的第0帧状态向量：
其中相机状态 包含：
各分量含义：
- ：相机在世界坐标系中的位置（3维）
- ：相机姿态四元数（4维）
- ：相机线速度（3维）
- ：相机角速度（3维）
地图点 为第 个靶点在世界坐标系中的位置 。
状态向量维度：
2.2. 协方差矩阵与噪声模型
初始协方差矩阵
为 维的对角矩阵，表示初始状态的不确定性：
对角线元素反映了各状态分量的初始不确定性（方差）。
过程噪声协方差矩阵
什么是过程噪声？
过程噪声描述了运动模型的不完美性。我们假设相机做匀速运动，但实际运动会有抖动、加速、减速。 量化了这种"假设不成立"的程度。
为什么需要 ？
如果没有 （即 ），意味着完全相信运动模型是完美的。随着预测进行，状态估计的不确定性会越来越小，滤波器会变得"过度自信"，完全忽略新的观测数据，导致估计发散。
的作用是在每一步预测中，为状态的不确定性注入一个"最小保障量"，防止滤波器过度自信。
如何设置 ？
通常设为对角矩阵，假设各状态分量的噪声相互独立：
对角线元素含义：
- ：位置预测噪声方差，单位 。值越大，表示越不相信匀速模型能准确预测位置。
- ：姿态预测噪声方差，单位 。
- ：速度预测噪声方差，单位 。
- ：角速度预测噪声方差，单位 。
- ：地图点噪声方差。由于地图点假设为静态，通常设为0或极小值。
物理意义示例：
- 位置噪声方差 表示位置预测可能有约 米的误差
- 姿态噪声方差 表示约 "rad"或 度的误差
观测噪声协方差矩阵
什么是观测噪声？
观测噪声描述了传感器测量的不精确性。特征点提取算法（如FAST、ORB）无法完美定位像素坐标， 量化了这种测量误差。
为什么需要 ？
告诉滤波器观测数据有多"嘈杂"。如果 很小，滤波器会更信任观测；如果 很大，滤波器会更信任自己的预测。
如何设置 ？
对于单个特征点观测 ， 为 矩阵：
通常假设 和 方向的误差独立且方差相同：
（单位：像素²）
这表示特征点提取精度约为 像素（标准差）。
与 在卡尔曼增益中的作用
卡尔曼增益 的计算：
关键权衡：
- 如果 相对于 很大（观测不可靠），则 大， 小，更信任预测
- 如果 相对于 很小（观测精确），则 小， 大，更信任观测
- 受 影响： 大 大 大 更信任观测
2.3. 运动模型假设
假设运动模型为匀速运动模型：
状态转移雅可比矩阵 ：
对于匀速模型， 是分块对角矩阵：
其中：
2.4. EKF预测步骤
状态预测：
协方差预测：
2.5. EKF更新步骤
观测残差：
观测雅可比矩阵：
创新协方差：
卡尔曼增益：
状态更新：
协方差更新：
3.Kalman Filter 具体数值案例
本节通过一个完整的数值案例，演示MonoSLAM中EKF的工作过程。
3.1. 场景设定
相机参数
- 内参矩阵 ：
- 图像尺寸： 像素
- 帧率：30fps， 秒
初始地图点（世界坐标系）
假设有4个特征点在同一平面上（ ）：
初始相机状态（帧0）
（相机在 米上方）
（无旋转）
（ m/s沿X轴）
（无角速度）
噪声参数
过程噪声协方差 ：
各分量含义：位置噪声 ，姿态噪声 ，速度噪声 ，角速度噪声 ，地图点噪声 。
观测噪声协方差 ：
（单位：像素²）
3.2. 初始化（帧0）
状态向量
维度： 维
具体值：
各分量：位置 ，四元数 ，速度 ，角速度 ，地图点 ， ， ， 。
初始协方差矩阵
对角矩阵：
各分量：位置 ，四元数 ，速度 ，角速度 ，地图点 。
初始观测（帧0）
将地图点投影到图像：
对于 ：
- 相机坐标系：
- 归一化坐标：
- 像素坐标： ，
同理计算其他点：
- 观测：
- 观测： （超出图像）
- 观测： （超出图像）
- 观测： （超出图像）
注意：由于相机在 米上方看向 平面，部分点投影为负值。实际中只选择可见的点进行跟踪。
3.3. 帧1处理流程
预测步骤
运动模型预测
（无旋转）
地图点不变：
状态转移雅可比矩阵
对于匀速模型， 是分块对角矩阵：
其中 为 矩阵：
具体数值（位置部分）：
协方差预测
由于 接近单位矩阵，且 较小：
对角线元素增加：
- 位置协方差：
- 速度协方差：
观测步骤
生成观测值
假设真实运动有轻微扰动：
使用真实位姿投影 ：
归一化坐标：
像素坐标：
所以
预测观测值
使用预测位姿 投影 ：
归一化：
像素： ，
观测残差
像素
更新步骤
观测雅可比矩阵
对于点 ， 是 矩阵。
设 ，投影： ，
计算 ：
对于预测状态： ， ， ， 为单位矩阵
计算 ：
所以 为稀疏矩阵，仅对位置 和地图点 有非零元素。
创新协方差
由于 近似对角，且 只有少数非零元素：
加上观测噪声 ：
卡尔曼增益
对于位置 的增益：
状态更新
对于位置 ：
协方差更新
由于 很小， ，但略有减小。
3.4. 状态扩展（帧2检测新点）
检测新特征点
假设在帧2图像坐标 检测到新点。
逆深度参数化初始化
从像素 反投影射线方向：
归一化：
初始逆深度： ，假设 米，则
深度不确定性： 米，则
新点坐标：
状态向量扩展
维度从25增加到28
协方差矩阵扩展
其中
表示新点与旧状态的协方差，初始设为0。
3.5. 关键数值总结
帧0初始值：
- 相机位置：
- 点1观测：
- 状态维度：25
- 协方差迹：3.48
帧1预测值：
- 相机位置：
- 点1观测：
- 协方差迹：约3.58
帧1更新值：
- 相机位置：
- 点1观测：
- 协方差迹：约3.57
这个案例展示了MonoSLAM如何通过EKF逐步更新相机位姿和地图点，以及如何扩展状态向量处理新特征点。实际系统中会同时跟踪多个点，计算更复杂，但基本原理相同。
4. 总结
本文从符号推导到具体数值案例，系统地讲解了MonoSLAM中的EKF状态估计方法。关键要点：
- 状态向量：包含相机位姿、速度和地图点，维度为
- 过程噪声 ：描述运动模型的不完美性，防止滤波器过度自信
- 观测噪声 ：描述传感器测量的不精确性，决定对观测的信任程度
- 卡尔曼增益 ：在预测和观测之间进行权衡，由 和 共同决定
通过具体数值案例，我们看到了EKF如何逐步更新状态估计，以及如何处理新特征点的加入。这些是理解MonoSLAM乃至所有基于滤波的SLAM方法的基础。