摘要

在机器人、无人车、SLAM、目标跟踪、IMU 融合、雷达定位等场景中，我们经常会遇到一个问题：

传感器数据有噪声，运动模型也不完全可靠，那到底应该相信谁？

Kalman 滤波就是用来解决这个问题的经典算法。

它的核心思想非常简单：

先根据模型预测当前位置，再根据传感器观测修正预测结果。

如果预测很准，就多相信预测；如果传感器很准，就多相信观测。这个“相信多少”的权重，就是 Kalman 增益。

---

1. 为什么需要 Kalman 滤波？

假设机器人沿着直线运动，我们想估计它的位置 x。

但是现实中会遇到两个问题：

1.1 运动模型不准

例如我们知道机器人速度是 v，理论上：

x = x + v * dt;

但是实际机器人可能会打滑、电机响应延迟、地面不平，所以预测位置会有误差。

1.2 传感器观测也不准

例如雷达、相机、GPS、编码器都能测位置，但是传感器存在噪声。

比如真实位置是：

x = 10.0 m

传感器可能测到：

z = 9.8 m
z = 10.3 m
z = 10.1 m

所以问题变成：

预测不完全可信，观测也不完全可信，如何融合出一个更可信的结果？

Kalman 滤波的答案是：

用不确定性来决定权重。

---

2. Kalman 滤波的核心流程

Kalman 滤波每一轮都分为两个阶段：

1. 预测阶段 Predict
2. 更新阶段 Update

预测阶段根据模型推算状态。

更新阶段根据传感器观测修正状态。

可以简单理解为：

预测：我觉得机器人应该在这里
观测：传感器说机器人在那里
融合：根据两边的可信度，算一个折中的结果

---

3. 一维 Kalman 滤波：只估计位置 x

我们先从最简单的一维情况开始：只估计一个位置 x。

3.1 变量说明

符号	含义	工程理解
x	当前状态估计值	机器人当前位置
P	当前状态估计的不确定性	我对 x 有多不自信
z	传感器观测值	传感器测到的位置
R	观测噪声	传感器有多不准
Q	过程噪声	运动模型有多不准
K	Kalman 增益	更相信预测还是观测

其中最重要的是三个量：

P：我对当前估计的不确定性
R：传感器的不确定性
Q：运动模型的不确定性

---

4. 预测阶段

假设机器人匀速运动，上一时刻位置是 x，速度是 v，时间间隔是 dt。

预测公式：

x = x + v * dt;
P = P + Q;

含义如下：

4.1 状态预测

x = x + v * dt;

这一步表示：根据运动模型预测机器人新的位置。

4.2 不确定性预测

P = P + Q;

这一步非常关键。

因为机器人运动了一段时间，模型不可能完全准确，所以不确定性应该变大。

也就是说：

机器人跑得越久，我越不确定它到底在哪里。

---

5. 更新阶段

当传感器给出观测值 z 后，需要用观测值修正预测值。

更新公式：

K = P / (P + R);
x = x + K * (z - x);
P = (1 - K) * P;

---

5.1 Kalman 增益 K

K = P / (P + R);

K 决定了更相信预测还是观测。

情况一：P 很大，R 很小

P 大：我对预测很不自信
R 小：传感器很准

那么：

K 接近 1

更新时：

x = x + K * (z - x);

由于 K 很大，最终结果会更接近观测值 z。

也就是说：

我不相信自己预测的，我更相信传感器。

---

情况二：P 很小，R 很大

P 小：我对预测很自信
R 大：传感器很不准

那么：

K 接近 0

最终结果会更接近预测值 x。

也就是说：

传感器太飘了，我不太信它。

---

6. 用一句话理解 Kalman 更新

更新公式是：

x = x + K * (z - x);

其中：

z - x

叫做残差，也可以理解为：

传感器观测值和预测值之间的差距

如果残差很大，说明预测和观测差别很大。

但是这个差距不能直接全部加到 x 上，而是要乘以 K。

所以：

Kalman 滤波不是盲目相信观测，而是根据 Kalman 增益有选择地修正预测值。

---

7. 一个简单数值例子

假设当前预测位置：

x = 10.0

预测不确定性：

P = 4.0

传感器观测位置：

z = 12.0

传感器噪声：

R = 1.0

计算 Kalman 增益：

K = P / (P + R)
K = 4 / (4 + 1)
K = 0.8

更新位置：

x = x + K * (z - x)
x = 10 + 0.8 * (12 - 10)
x = 11.6

最终结果是：

x = 11.6

它更接近传感器观测值 12.0，因为此时 P 比较大，说明我们对预测结果不太自信。

更新不确定性：

P = (1 - K) * P
P = (1 - 0.8) * 4
P = 0.8

更新之后，P 变小，说明经过传感器修正后，我们对状态更自信了。

---

8. C++ 实现：一维 Kalman 滤波

下面是一个最小可用版本。

#include <iostream>

class KalmanFilter1D {
public:
    KalmanFilter1D(double init_x, double init_P, double Q, double R)
        : x_(init_x), P_(init_P), Q_(Q), R_(R) {}

    void predict(double v, double dt) {
        // 1. 状态预测
        x_ = x_ + v * dt;

        // 2. 协方差预测
        P_ = P_ + Q_;
    }

    void update(double z) {
        // 1. 计算 Kalman 增益
        double K = P_ / (P_ + R_);

        // 2. 根据观测修正状态
        x_ = x_ + K * (z - x_);

        // 3. 更新估计不确定性
        P_ = (1.0 - K) * P_;
    }

    double getState() const {
        return x_;
    }

    double getUncertainty() const {
        return P_;
    }

private:
    double x_;  // 状态估计
    double P_;  // 估计不确定性
    double Q_;  // 过程噪声
    double R_;  // 观测噪声
};

int main() {
    KalmanFilter1D kf(
        0.0,   // 初始位置
        1.0,   // 初始不确定性
        0.1,   // 过程噪声
        0.5    // 观测噪声
    );

    double dt = 1.0;
    double v = 1.0;

    double measurements[] = {1.2, 1.9, 3.1, 3.9, 5.2};

    for (double z : measurements) {
        kf.predict(v, dt);
        kf.update(z);

        std::cout << "Estimate x = " << kf.getState()
                  << ", P = " << kf.getUncertainty()
                  << std::endl;
    }

    return 0;
}

这个程序每一轮都会先根据速度预测位置，然后用传感器观测值修正位置。

---

9. 从一维到二维状态：同时估计位置 x 和速度 v

在机器人中，我们很多时候不只想估计位置，还想估计速度。

这时状态量可以写成：

X = [x, v]^T

其中：

x：位置
v：速度

假设机器人做匀速运动，那么运动模型是：

x_new = x + v * dt
v_new = v

写成矩阵形式：

X_new = F * X

其中：

X = [x, v]^T

状态转移矩阵：

F = [1, dt
     0, 1 ]

所以：

[x_new]   [1, dt] [x]
[v_new] = [0,  1] [v]

也就是：

x_new = x + v * dt
v_new = v

---

10. 如果传感器只能测位置，速度怎么更新？

这是很多初学者最容易卡住的地方。

假设传感器只能测位置：

z = x

不能直接测速度 v。

那速度还能被更新吗？

答案是：

可以。

原因是位置和速度通过运动模型产生了关联。

如果系统预测：

x = 10
v = 2

但是传感器观测：

z = 13

说明预测位置偏小。

在匀速模型中，位置偏小可能意味着：

速度估计也偏小

因此 Kalman 滤波会通过位置残差间接修正速度。

这就是矩阵形式 Kalman 滤波的价值。

---

11. 二维状态 Kalman 滤波变量表

符号	维度	含义
X	2x1	状态向量 [x, v]^T
F	2x2	状态转移矩阵
P	2x2	状态协方差矩阵
Q	2x2	过程噪声矩阵
Z	1x1	观测值
H	1x2	观测矩阵
R	1x1	观测噪声
K	2x1	Kalman 增益

---

12. 二维状态的预测阶段

状态预测：

X = F * X

协方差预测：

P = F * P * F^T + Q

其中：

F = [1, dt
     0, 1 ]

P 不再是一个普通数字，而是矩阵。

它不仅表示 x 和 v 各自的不确定性，还表示它们之间的相关性。

---

13. 二维状态的更新阶段

观测模型：

Z = H * X

如果传感器只能测位置，那么：

H = [1, 0]

表示只从状态向量里取出 x，不直接观测 v。

残差：

Y = Z - H * X

残差协方差：

S = H * P * H^T + R

Kalman 增益：

K = P * H^T * S^-1

状态更新：

X = X + K * Y

协方差更新：

P = (I - K * H) * P

注意，K 是一个 2x1 矩阵：

K = [Kx
     Kv]

所以更新时：

x = x + Kx * y
v = v + Kv * y

这就是为什么即使没有直接测量速度，速度仍然可以被修正。

---

14. 二维 Kalman 滤波 C++ 示例

下面是一个不依赖 Eigen 的简单版本，便于理解公式。

#include <iostream>

class KalmanFilterCV {
public:
    KalmanFilterCV(double init_x, double init_v)
        : x(init_x), v(init_v) {
        // 初始协方差矩阵 P
        P00 = 1.0; P01 = 0.0;
        P10 = 0.0; P11 = 1.0;

        // 过程噪声
        Q00 = 0.1; Q01 = 0.0;
        Q10 = 0.0; Q11 = 0.1;

        // 观测噪声
        R = 0.5;
    }

    void predict(double dt) {
        // 状态预测
        x = x + v * dt;
        v = v;

        // F = [1, dt;
        //      0, 1 ]

        // P = F * P * F^T + Q
        double newP00 = P00 + dt * P10 + dt * P01 + dt * dt * P11 + Q00;
        double newP01 = P01 + dt * P11 + Q01;
        double newP10 = P10 + dt * P11 + Q10;
        double newP11 = P11 + Q11;

        P00 = newP00;
        P01 = newP01;
        P10 = newP10;
        P11 = newP11;
    }

    void update(double z) {
        // H = [1, 0]
        // 预测观测值 z_pred = H * X = x
        double z_pred = x;

        // 残差 y = z - H * X
        double y = z - z_pred;

        // S = H * P * H^T + R
        double S = P00 + R;

        // K = P * H^T * S^-1
        double K0 = P00 / S;
        double K1 = P10 / S;

        // 状态更新
        x = x + K0 * y;
        v = v + K1 * y;

        // P = (I - K * H) * P
        double newP00 = (1.0 - K0) * P00;
        double newP01 = (1.0 - K0) * P01;
        double newP10 = P10 - K1 * P00;
        double newP11 = P11 - K1 * P01;

        P00 = newP00;
        P01 = newP01;
        P10 = newP10;
        P11 = newP11;
    }

    void print() const {
        std::cout << "x = " << x
                  << ", v = " << v
                  << ", P00 = " << P00
                  << ", P11 = " << P11
                  << std::endl;
    }

private:
    double x;
    double v;

    // P 矩阵
    double P00, P01;
    double P10, P11;

    // Q 矩阵
    double Q00, Q01;
    double Q10, Q11;

    // R 矩阵，这里是一维观测，所以用 double
    double R;
};

int main() {
    KalmanFilterCV kf(0.0, 0.0);

    double dt = 1.0;

    double measurements[] = {
        1.0, 2.1, 2.9, 4.2, 5.1, 6.0
    };

    for (double z : measurements) {
        kf.predict(dt);
        kf.update(z);
        kf.print();
    }

    return 0;
}

这个例子中，传感器只测位置 z，但是滤波器会同时估计位置 x 和速度 v。

---

15. Q 和 R 怎么调？

Kalman 滤波能不能好用，很大程度上取决于 Q 和 R。

15.1 R：观测噪声

R 表示传感器观测有多不可靠。

如果传感器很准：

R 小

滤波器会更相信传感器。

如果传感器很飘：

R 大

滤波器会更相信模型预测。

举例：

激光雷达定位比较稳定：R 可以小一点
视觉深度相机深度跳变大：R 应该大一点
低成本 GPS 漂移明显：R 应该更大

---

15.2 Q：过程噪声

Q 表示运动模型有多不可靠。

如果机器人运动很平稳，模型比较可信：

Q 小

如果机器人经常急加速、打滑、震动：

Q 大

举例：

室内低速轮式机器人：Q 可以小一点
四轮滑移底盘：Q 应该稍大
机器狗高震动运动：Q 应该更大

---

16. Q 和 R 调参的直觉

可以用一句话记住：

R 越小，越相信传感器；
R 越大，越相信预测。

Q 越小，滤波结果越平滑，但响应更慢；
Q 越大，滤波结果响应更快，但更容易抖。

如果滤波结果过于抖动：

增大 R 或减小 Q

如果滤波结果跟不上真实变化：

减小 R 或增大 Q

---

17. Kalman 滤波在机器人中的应用

17.1 目标检测结果滤波

比如 YOLO 检测苹果的位置，检测框中心点会抖动。

可以把状态设计成：

X = [u, v, Z, vu, vv, vZ]^T

其中：

u：像素 x 坐标
v：像素 y 坐标
Z：深度
vu：像素 x 方向速度
vv：像素 y 方向速度
vZ：深度变化速度

观测值为：

Z_meas = [u, v, depth]^T

这样可以同时滤波目标中心点和深度，减少 TF 抖动。

---

17.2 机器人定位融合

比如融合轮速计和 IMU：

轮速计提供速度和里程
IMU 提供角速度和加速度
雷达里程计提供位姿

Kalman 滤波可以把多个来源的信息融合成一个更稳定的状态估计。

在 ROS2 中，常见的使用场景包括：

/odom
/imu/data
/gps/fix
/robot_localization/odometry/filtered

典型工具包是：

robot_localization

其中的 EKF 节点本质上就是扩展 Kalman 滤波。

---

18. Kalman、EKF、ESKF 有什么区别？

18.1 Kalman Filter

适用于线性系统。

例如：

x = x + v * dt

这种模型可以直接写成矩阵形式。

18.2 EKF

EKF 是 Extended Kalman Filter，扩展 Kalman 滤波。

适用于非线性系统。

例如机器人位姿：

x
y
yaw

运动模型中会出现：

cos(yaw)
sin(yaw)

这就不是线性系统了，需要对非线性函数做一阶线性化。

18.3 ESKF

ESKF 是 Error-State Kalman Filter，误差状态 Kalman 滤波。

它常用于 IMU、VIO、LIO 中。

例如 FAST-LIO、VINS、LIO-SAM 这类系统，经常会涉及 ESKF 思想。

ESKF 的核心思想是：

主状态负责积分传播
误差状态负责小量修正

相比直接估计完整状态，ESKF 对 IMU 这种高频积分系统更稳定。

---

19. Kalman 滤波常见误区

19.1 误区一：Kalman 滤波就是简单平均

不是。

简单平均是固定权重。

Kalman 滤波的权重是动态变化的，由 P 和 R 决定。

---

19.2 误区二：Q 和 R 随便设置也能用

不对。

Q 和 R 直接决定滤波器的表现。

参数不合理时，可能出现：

滤波结果严重滞后
滤波结果剧烈抖动
状态估计发散
速度估计异常

---

19.3 误区三：没有测速度，就不能估计速度

不对。

只要位置和速度在运动模型中有关联，就可以通过位置观测间接修正速度。

这也是 [x, v] 状态模型的意义。

---

19.4 误区四：Kalman 滤波可以修复所有噪声

不对。

Kalman 滤波的前提是：

模型基本正确
噪声假设基本合理
观测没有大量离群点

如果传感器数据里有大量错误值，例如深度相机突然测到 0 或极大值，应该先做异常值剔除，再送入 Kalman 滤波。

---

20. 工程实践建议

在机器人项目中使用 Kalman 滤波时，可以按下面的流程设计：

20.1 第一步：确定你要估计什么

例如：

只估计位置：[x]
估计位置和速度：[x, v]
估计二维目标：[u, v, Z, vu, vv, vZ]
估计机器人位姿：[x, y, yaw, vx, vy, wz]

---

20.2 第二步：确定运动模型

例如匀速模型：

x = x + v * dt
v = v

如果目标运动更复杂，可以引入加速度：

x = x + v * dt + 0.5 * a * dt^2
v = v + a * dt

---

20.3 第三步：确定观测模型

传感器能测什么，就把什么写进观测模型。

比如传感器只能测位置：

H = [1, 0]

如果传感器能同时测位置和速度：

H = [1, 0
     0, 1]

---

20.4 第四步：设置 Q 和 R

初始建议：

传感器很准：R 小一点
传感器很飘：R 大一点
模型很可靠：Q 小一点
模型不可靠：Q 大一点

然后根据实际效果调。

---

20.5 第五步：处理异常值

Kalman 滤波前最好先做基础数据检查。

例如深度相机：

if (depth <= 0 || depth > max_depth) {
    // 丢弃异常观测，只做 predict，不做 update
}

目标检测：

if (confidence < threshold) {
    // 置信度太低，不更新
}

这样可以避免错误观测把滤波器带偏。

---

21. 总结

Kalman 滤波的核心不是公式，而是思想：

预测：根据模型估计当前状态
更新：根据传感器修正当前状态
权重：由不确定性自动决定

一维 Kalman 滤波可以帮助我们理解基本概念。

二维 [x, v] 模型可以进一步说明：即使传感器只测位置，也可以通过状态相关性间接估计速度。

在机器人系统中，Kalman 滤波广泛应用于：

目标跟踪
传感器融合
机器人定位
IMU 融合
视觉/雷达里程计
深度数据平滑

如果要真正用好 Kalman 滤波，重点不是背公式，而是搞清楚三个问题：

我在估计什么状态？
我的运动模型是什么？
我的传感器观测了什么？

把这三个问题想清楚，Kalman 滤波就不再是玄学公式，而是一个非常实用的工程工具。

---

参考理解口诀

P 大：我不相信自己
R 大：我不相信传感器
Q 大：我不相信模型

K 大：多信观测
K 小：多信预测

这就是 Kalman 滤波。