卡尔曼滤波及其应用

2301_76512411

177人浏览 · 2026-09-13 17:42:19

2301_76512411 · 2026-09-13 17:42:19 发布

前言

做机器人开发的同学几乎都绕不开卡尔曼滤波。编码器测速抖动、IMU姿态飘、GPS定位跳变，很多人的第一反应就是套卡尔曼代码。但很多人的现状是：代码能跑，不懂原理；只会改Q、R两个参数，不知道为什么这么调；换一个传感器、换运动模型，滤波直接失效。
这篇文章的目标，不是推导高斯分布、贝叶斯估计。而是从工程视角讲清楚：卡尔曼到底在干什么，五步公式每一步工程含义是什么，矩阵参数怎么理解，有哪些坑。读完你不仅能看懂代码，还能根据你的机器人硬件，修改模型、调试参数。

1. 卡尔曼滤波在干什么

机器人的核心痛点：数据永远不准。机器人所有传感器的测量值，都存在两类误差：

随机噪声：IMU、编码器、激光雷达的高频抖动，每次采样值上下跳；
模型误差/漂移：陀螺仪长时间积分漂移、里程计累积误差，越跑偏差越大。

简单粗暴的滑动平均滤波能抑制抖动，但会带来严重滞后。机器人高速运动、转弯的时候，滞后会直接导致控制不稳，定位跑偏。所以必须把它们合成一个“目前最可信”的状态，并且知道自己有多不确定。卡尔曼滤波（Kalman Filter, KF）就是在做这件事。它不是魔法，也不是“把噪声滤掉”那么简单。更准确的说法是：

卡尔曼滤波每来一拍，都在问两个问题：

按物理模型往前推一步，我现在应该在哪？我对这个猜测有多不确定？
传感器刚报了一个数，这个数跟我猜的差多少？我该信模型多一点，还是信传感器多一点？

信多少，由一个叫 卡尔曼增益 K 的量自动算出来。K 大，说明测量更可信，状态往测量那边靠；K 小，说明模型更可信，状态几乎不改。

假设你在给无人机做定位。IMU 积分出来的位置，短时间内很丝滑，但会慢慢漂；GPS 偶尔跳一下，但长期不会跑飞。你不会只用其中一个，你会下意识地做这种事：

IMU 告诉你“我刚往前走了 0.3 米”
GPS 告诉你“你现在在 10.8 米附近”
你心里权衡一下：这拍 GPS 看起来还行，就往 10.8 靠一点；如果 GPS 明显跳飞了，就先信 IMU

卡尔曼滤波做的，就是把这种“权衡”写成可复现的算法。它比拍脑袋加权高级的地方在于：

它同时维护均值和方差。 不只告诉你“位置大概是 10.4”，还告诉你“我现在对这个数的信心有多大”。
权重是时变的。 模型漂得越久，你越不信模型；传感器越吵，你越不信传感器。这个权重每拍都在变。
它能估计你没直接测到的量。 比如你只测到位置，它能顺带把速度、加速度估出来。动态障碍物跟踪就是这么干的。

2. 卡尔曼滤波核心思想

先把矩阵全部丢掉，只看一维。

你有两个对同一件事的估计：

模型预测：均值 x_pred，方差 P_pred（越大越不信）
传感器测量：z，方差 R（越大越不信）

最合理的合成方式，不是取平均，而是 按可信度加权：
$x^=(1−K) xpred+K z \hat{x} = (1-K)\, x_{\text{pred}} + K\, z$

其中
$\frac{P_{\text{pred}}}{P_{\text{pred}} + R}$
这就是卡尔曼滤波最重要的一张面孔。把它读成人话：

如果预测很准（P_pred 很小），K 接近 0，新估计几乎等于预测
如果测量很准（R 很小），K 接近 1，新估计几乎等于测量
如果两边差不多不准，K 大约 0.5，两边各信一半

更新之后，新的不确定度也会变小：
$(1-K)\, P_{\text{pred}}$
这很符合直觉：你刚融合了两份独立信息，应该比只听其中一份更有把握。

在这里插入图片描述

多维卡尔曼滤波看起来吓人，其实只是把上面这套加权，从标量换成了矩阵。K 不再是一个数，而是一个矩阵，因为它要决定：位置测量该不该去改速度估计、哪个轴该信得多一点。但精神没变。

3. 卡尔曼公式常见记号说明

教科书喜欢一上来丢一串字母。我们反过来：先给每个符号一个岗位说明书。

符号	代码里常见名字	含义	是否为可调参数
(x)	`states`	你想估计的状态。位置、速度、加速度、姿态……	初值要给，后面算法自己更新
(P)	`P`	状态的协方差。对角元大 = 你对这个量没把握	初值要给，后面算法自己更新
(F) 或 (A)	`A`	状态转移矩阵。描述“没有噪声时，状态怎么随时间走”	由运动模型决定，一般不当超参拧
(B)	`B`	控制输入矩阵。油门、加速度指令怎么进状态	有控制输入才需要
(u)	`u`	已知的控制量	来自控制器，不是滤波参数
(H)	`H`	观测矩阵。状态里哪些量能被传感器直接看到	由传感器模型决定
(z)	`z`	这一拍的测量	来自传感器
(Q)	`Q`	过程噪声协方差。模型有多不可信	可调
(R)	`R`	测量噪声协方差。传感器有多不可信	可调
(K)	`K`	卡尔曼增益。这一拍信测量多少	不要手调，每拍重算

有两个特别容易混的点。
第一，P 不是“误差”。P 是滤波器 自己认为 的不确定度。如果 Q、R 设错了，P 会骗你：它可能很小，但估计其实已经偏了。这叫“过度自信”。工程上最危险的情况之一，就是 P 很小、估计却是错的，因为后面的模块会把这个数当真理用。
第二，Q 和 R 的绝对大小往往没有相对大小重要。真正决定 K 的是 过程不确定度和测量不确定度的比值。把 Q 和 R 同时乘 10，稳态 K 几乎不变；只改其中一个，行为会明显变。

4. 五步核心公式：预测两步，更新三步

经典卡尔曼滤波每一拍都做同一件事。
设上一拍结束后的估计是 $x^k−1\hat{x}_{k-1}$ 、 $P_{k-1}$ 。这一拍控制输入是 $u_{k-1}$ ，测量是 $z_k$ 。

4.1 预测：让模型先走一步

1）状态预测
$x^k∣k−1=Ax^k−1+Buk−1 \hat{x}_{k|k-1} = A \hat{x}_{k-1} + B u_{k-1}$
人话：如果世界完全按你的运动学走，状态现在应该在这里。无人机里，这一步常常是用上一拍速度积分位置；障碍物跟踪里，常常是匀加速模型往前推 dt。
2）协方差预测
$P_{k|k-1} = A P_{k-1} A^{\top} + Q$
人话：两件事会让你更不确定。

$A^{\top}$ ：旧的不确定度被运动学放大。位置不准，积分后更不准；速度不准，时间越长位置越飘。
$+ Q$ ：模型本身就有错，每走一步都要再撒一把不确定性。

预测这一步 只让 P 变大（或至少不变），不会变小。因为你没有新证据，只是在用一个不完美的模型往前猜。

4.2 更新：让测量来纠偏

先算“我猜的测量”和“真实测量”差多少。这个差叫 新息（innovation）：
$yk=zk−Hx^k∣k−1 y_k = z_k - H \hat{x}_{k|k-1}$
新息是调参时最值得盯的量。如果滤波器健康，新息应该在 0 附近抖，不应该长期偏向一边。
新息的协方差是：
$S_k = H P_{k|k-1} H^{\top} + R$
$S$ 的含义是：这个差，在当前信念下“应该有多大”。差得比 $S$ 指示的还离谱，测量可能是野值。
然后才是卡尔曼增益：
$K_k = P_{k|k-1} H^{\top} S_k^{-1}$
最后两步才是真正改状态和改信心：
状态更新
$x^k=x^k∣k−1+Kkyk \hat{x}_k = \hat{x}_{k|k-1} + K_k y_k$
协方差更新
$P_k = (I - K_k H) P_{k|k-1}$
有的实现会用 Joseph 形式，数值上更稳：
$P_k = (I - K_k H) P_{k|k-1} (I - K_k H)^{\top} + K_k R K_k^{\top}$
工程上，短周期、维度不高时，用简化形式通常够用。如果 $P$ 出现非对称或负特征值，再换 Joseph 形式。

在这里插入图片描述
请特别注意图里的“没有测量”分支。真实机器人经常丢 GPS、检测框跟丢、雷达漏检。这时正确做法不是强行用旧测量，而是 只做预测。 $P$ 会变大，这是诚实的：你已经有一段时间没看见了，因此不确定性增大。

5. 运动模型从哪来：机器人里最常用的三个线性模型

很多教程把 A 写得像天经地义。工程里 A 是你对目标运动的假设。假设错了，后面再怎么调 Q、R 都是在补洞。
机器人里最常用的三个线性模型：

5.1 恒定位置（几乎不动的东西）

状态：位置。适合停着的障碍、缓慢漂移的偏置。
$1,\quad x_k = x_{k-1} + w$
$Q$ 这时表示“它其实可能在慢慢动”。

5.2 恒定速度 CV

状态 $v]^{\top}$ 。行人、匀速车辆、很多跟踪器的默认选择。
$[pv]k=[1dt01][pv]k−1+w\begin{bmatrix} p \\ v \end{bmatrix}_{k} = \begin{bmatrix} 1 & dt \\ 0 & 1 \end{bmatrix} \begin{bmatrix} p \\ v \end{bmatrix}_{k-1} + w$
位置这一拍等于上一拍位置加 $\cdot dt$ 。速度默认不变，变化全部丢给过程噪声。

5.3 恒定加速度 CA

状态 $a]^{\top}$ 。无人机、突然起跑的行人、动态障碍跟踪用的就是这个。
$\begin{bmatrix} 1 & dt & \tfrac{1}{2}dt^{2} \\ 0 & 1 & dt \\ 0 & 0 & 1 \end{bmatrix}$
选模型有一条实用规则：

模型阶数低，平滑但跟不上机动；阶数高，跟得上但噪声会被放大。

能用 CV 就别上 CA。

$Q$ 的职责，就是承认“加速度并不是真的恒定”。你把加速度过程噪声 e_q_acc 设大一点，等于告诉滤波器：匀加速只是近似，目标随时可能改加速度。这比把模型改成加加速度（jerk）模型便宜得多。

6. 卡尔曼滤波实现代码

以恒定速度、恒定加速度运动模型为例，给出卡尔曼滤波示例代码。

6.1 恒定速度

from __future__ import annotations

import numpy as np

class KalmanCV2D:
    def __init__(self, dt, q_acc=1.0, r_pos=0.05, p0=1.0):
        self.dt = dt
        self.x = np.zeros((4, 1))
        self.A = np.array(
            [
                [1, 0, dt, 0],
                [0, 1, 0, dt],
                [0, 0, 1, 0],
                [0, 0, 0, 1],
            ],
            dtype=float,
        )
        self.H = np.array(
            [
                [1, 0, 0, 0],
                [0, 1, 0, 0],
            ],
            dtype=float,
        )
        dt2, dt3, dt4 = dt**2, dt**3, dt**4
        q = q_acc
        self.Q = q * np.array(
            [
                [dt4 / 4, 0, dt3 / 2, 0],
                [0, dt4 / 4, 0, dt3 / 2],
                [dt3 / 2, 0, dt2, 0],
                [0, dt3 / 2, 0, dt2],
            ]
        )
        self.R = np.eye(2) * r_pos
        self.P = np.eye(4) * p0

    def predict(self):
        self.x = self.A @ self.x
        self.P = self.A @ self.P @ self.A.T + self.Q

    def update(self, z):
        z = np.asarray(z, dtype=float).reshape(2, 1)
        y = z - self.H @ self.x
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)
        self.x = self.x + K @ y
        eye = np.eye(4)
        self.P = (eye - K @ self.H) @ self.P
        return y, K

    def step(self, z):
        self.predict()
        return self.update(z)

	def demo_cv_tracking():
	    print()
	    print("=" * 60)
	    print("示例 1：二维 CV 跟踪，行人沿 x 匀速，中途加速")
	    print("=" * 60)
	    rng = np.random.default_rng(0)
	    dt = 0.05
	    kf = KalmanCV2D(dt=dt, q_acc=2.0, r_pos=0.04, p0=1.0)
	
	    truth = np.array([0.0, 0.0, 1.0, 0.0])  # px, py, vx, vy
	    kf.x[:, 0] = [0.0, 0.0, 0.0, 0.0]
	
	    print(f"{'t':>6} {'true_px':>8} {'meas_px':>8} {'est_px':>8} {'est_vx':>8}")
	    for i in range(40):
	        t = i * dt
	        if t > 1.0:
	            truth[2] = 2.5  # 突然加速
	        truth[0] += truth[2] * dt
	        truth[1] += truth[3] * dt
	        meas = truth[:2] + rng.normal(0.0, 0.2, size=2)
	        kf.step(meas)
	        if i % 5 == 0:
	            print(
	                f"{t:6.2f} {truth[0]:8.3f} {meas[0]:8.3f} "
	                f"{kf.x[0,0]:8.3f} {kf.x[2,0]:8.3f}"
	            )

6.2 恒定加速度

from __future__ import annotations

import numpy as np

class KalmanCA2D:
    """状态 [px, py, vx, vy, ax, ay]，观测同样 6 维。"""

    def __init__(
        self,
        dt,
        e_p=0.25,
        e_q_pos=0.01,
        e_q_vel=0.05,
        e_q_acc=0.05,
        e_r_pos=0.04,
        e_r_vel=0.3,
        e_r_acc=0.6,
    ):
        self.dt = dt
        dt2 = 0.5 * dt * dt
        self.A = np.array(
            [
                [1, 0, dt, 0, dt2, 0],
                [0, 1, 0, dt, 0, dt2],
                [0, 0, 1, 0, dt, 0],
                [0, 0, 0, 1, 0, dt],
                [0, 0, 0, 0, 1, 0],
                [0, 0, 0, 0, 0, 1],
            ],
            dtype=float,
        )
        self.H = np.eye(6)
        self.Q = np.diag([e_q_pos, e_q_pos, e_q_vel, e_q_vel, e_q_acc, e_q_acc])
        self.R = np.diag([e_r_pos, e_r_pos, e_r_vel, e_r_vel, e_r_acc, e_r_acc])
        self.P = np.eye(6) * e_p
        self.x = np.zeros((6, 1))

    def setup_from_box(self, px, py):
        self.x[:, 0] = [px, py, 0, 0, 0, 0]

    def estimate(self, z):
        self.x = self.A @ self.x
        self.P = self.A @ self.P @ self.A.T + self.Q
        z = np.asarray(z, dtype=float).reshape(6, 1)
        S = self.R + self.H @ self.P @ self.H.T
        K = self.P @ self.H.T @ np.linalg.inv(S)
        self.x = self.x + K @ (z - self.H @ self.x)
        self.P = (np.eye(6) - K @ self.H) @ self.P
        return self.x.copy(), K

	def demo_ca():
	    print()
	    print("=" * 60)
	    print("示例 2：6 维 CA，用历史帧差分造观测")
	    print("=" * 60)
	    rng = np.random.default_rng(1)
	    dt = 0.033
	    k_avg = 10
	    kf = KalmanCA2D(dt=dt)
	    kf.setup_from_box(0.0, 0.0)
	
	    hist_px = [0.0]
	    hist_vx = [0.0]
	    px, vx, ax = 0.0, 0.0, 0.0
	
	    print(f"{'t':>6} {'true_vx':>8} {'meas_vx':>8} {'est_vx':>8} {'P_vx':>8}")
	    for i in range(80):
	        t = i * dt
	        ax = 1.2 if 0.8 < t < 1.6 else 0.0
	        vx += ax * dt
	        px += vx * dt
	        meas_px = px + rng.normal(0.0, 0.05)
	        hist_px.insert(0, meas_px)
	        k = min(k_avg, len(hist_px) - 1) or 1
	        meas_vx = (meas_px - hist_px[k]) / (dt * k)
	        meas_ax = (meas_vx - hist_vx[min(k, len(hist_vx) - 1)]) / (dt * k)
	        hist_vx.insert(0, meas_vx)
	
	        z = [meas_px, 0.0, meas_vx, 0.0, meas_ax, 0.0]
	        kf.estimate(z)
	        if i % 10 == 0:
	            print(
	                f"{t:6.2f} {vx:8.3f} {meas_vx:8.3f} "
	                f"{kf.x[2,0]:8.3f} {kf.P[2,2]:8.3f}"
	            )

7. 什么时候选卡尔曼滤波

标准卡尔曼滤波（KF）有两个假设，工程上非常重要，不满足就不要硬套KF：

系统是线性的：状态变化满足线性方程，即运动模型是线性的。
噪声是零均值高斯白噪声：过程噪声、观测噪声服从高斯分布，即观测模型是线性的。

机器人很多场景是非线性的（例如旋转、角度），这时候标准KF失效，需要EKF/UKF。

DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

ROS机器人-从零开始每日日志记录day12

DAMO开发者矩阵

基于蚁群优化算法的直流电机模糊PID控制（Matlab实现）

直流电机因其调速性能优异、响应速度快等特点，在工业控制、电动汽车、机器人等领域有着广泛应用。然而，传统PID控制存在参数固定、难以应对非线性及环境扰动等问题。模糊PID控制虽通过引入模糊逻辑实现了参数自适应调整，但在复杂场景下仍面临规则库构建与参数优化的难题。本文将蚁群优化算法（Ant Colony Optimization, ACO）引入直流电机模糊PID控制系统中，利用其全局寻优能力对模糊PI