一种简单的卡尔曼滤波器设计

一个不精确的比喻

想象你在雾里看一个人走路。

雾不算太浓，但足够让你看不清他的脚。你只能模模糊糊看到一个身影，每隔一小会儿，雾散开一点，你瞥见他的位置。但那个位置不太准，有时候你觉得他在左边一点，有时候又觉得偏右了。视觉检测就是这样，它给你的是一串带噪声的坐标，像透过毛玻璃看世界。

你想知道他现在到底在哪儿，速度是多少。更重要的是，你要预测他下一步会走到哪里——因为你的无人机反应慢半拍，等看到位置再追过去，他早就不在那个点了。

说实话，这个问题挺头疼的。手上只有一串抖动的坐标，要从中反推出一个平滑的运动轨迹，还要预测未来。听起来像是在猜谜。

但有一个很巧妙的方法，叫卡尔曼滤波。名字有点唬人，核心想法却出奇地简单。

你不需要知道他的真实位置。你只需要一个假设：这个人大部分时间在匀速走路。他不会每一秒都在急停急转。基于这个假设，你可以用物理定律来猜测他下一步的位置。

第一步：定义状态

我们把“这个人此刻在哪儿、走多快”打包成一个状态向量。四个数就够了——两个方向的位置，两个方向的速度。

$$
\mathbf{X} = \begin{bmatrix} x \ y \ v_x \ v_y \end{bmatrix}
$$
每一帧我们要做的，就是根据上一帧的状态，推算这一帧的状态，然后再用新来的测量值去修正。

第二步：用模型做预测

这是每一帧的第一步，叫“先验估计”。我们还没看到新的测量，只是按照匀速假设，用上一帧的位置和速度，推一个当前的位置出来。

状态转移方程长这样：

$$
\mathbf{X}k^- = \mathbf{F} \mathbf{X}{k-1}
$$
展开就是：

$$
\begin{aligned}
x_k^- &= x_{k-1} + v_{x,k-1} \cdot \Delta t \
y_k^- &= y_{k-1} + v_{y,k-1} \cdot \Delta t \
v_{x,k}^- &= v_{x,k-1} \
v_{y,k}^- &= v_{y,k-1}
\end{aligned}
$$
矩阵 $$\mathbf{F}$$ 把这一切打包成一次乘法：

$$
\mathbf{F} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \ 0 & 1 & 0 & \Delta t \ 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 1 \end{bmatrix}
$$
在代码里，它就是一个 numpy 数组：

self.F = np.array([
    [1, 0, dt, 0],
    [0, 1, 0, dt],
    [0, 0, 1,  0],
    [0, 0, 0,  1]
])

预测位置之后，还要更新协方差矩阵$$\mathbf{P}$$，它代表我们对这个预测有多不确定。每预测一步，不确定性都会增加一点点——因为我们承认模型并不完美。

$$
\mathbf{P}k^- = \mathbf{F} \mathbf{P}{k-1} \mathbf{F}^T + \mathbf{Q}
$$
这里的$$\mathbf{Q}$$ 是过程噪声协方差矩阵。它只有唯一一个需要手动调节的参数$$q$$——你预期这个人加速有多猛。

$$
\mathbf{Q} = q \begin{bmatrix}
\frac{\Delta t^4}{4} & 0 & \frac{\Delta t^3}{2} & 0 \
0 & \frac{\Delta t^4}{4} & 0 & \frac{\Delta t^3}{2} \
\frac{\Delta t^3}{2} & 0 & \Delta t^2 & 0 \
0 & \frac{\Delta t^3}{2} & 0 & \Delta t^2
\end{bmatrix}
$$

代码里这样构造：

dt2 = dt ** 2
dt3 = dt ** 3
dt4 = dt ** 4
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  ]
])

第三步：用测量做修正

现在，我们手上有了两个东西：一个是根据模型推出来的先验估计$$\mathbf{X}_k^-$$，一个是视觉系统刚给出的、带噪声的测量$$\mathbf{z}_k$$。两者都不完美，但可以结合。

视觉系统只能看到位置，看不到速度。所以观测矩阵$$\mathbf{H}$$ 很简单，只从状态里提取前两个分量：

$$
\mathbf{z}_k = \mathbf{H} \mathbf{X}_k + \mathbf{v}_k, \quad
\mathbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0 \ 0 & 1 & 0 & 0 \end{bmatrix}
$$

self.H = np.array([
    [1, 0, 0, 0],
    [0, 1, 0, 0]
])

测量噪声矩阵$$\mathbf{R}$$ 需要提前测好——把目标摆在那里不动，录几十帧坐标，算它们的方差：
$$
\mathbf{R} = \begin{bmatrix} \sigma_x^2 & 0 \ 0 & \sigma_y^2 \end{bmatrix}
$$

1	`self.R = np.array([[0.64, 0], [0, 0.64]])`

接下来是卡尔曼滤波最核心的一步：计算权重。我们测量值与预测值的差距叫“新息”：

$$
\tilde{\mathbf{y}}_k = \mathbf{z}_k - \mathbf{H} \mathbf{X}_k^-
$$
然后计算新息的协方差$$\mathbf{S}_k$$，以及卡尔曼增益$$\mathbf{K}_k$$：

$$
\begin{aligned}
\mathbf{S}_k &= \mathbf{H} \mathbf{P}_k^- \mathbf{H}^T + \mathbf{R} \[4pt]
\mathbf{K}_k &= \mathbf{P}_k^- \mathbf{H}^T \mathbf{S}_k^{-1}
\end{aligned}
$$
$$\mathbf{K}_k$$ 是一个$$4 \times 2$$ 的矩阵，它决定了“相信模型多还是相信测量多”。用这个增益，我们把新息按比例注入先验估计，得到后验估计：

$$
\mathbf{X}_k = \mathbf{X}_k^- + \mathbf{K}_k \tilde{\mathbf{y}}_k
$$
展开来看，连速度也会被修正：

$$
\begin{aligned}
\hat{x}k &= \hat{x}k^- + K{11}(z{x,k} - \hat{x}k^-) + K{12}(z_{y,k} - \hat{y}k^-) \[2pt]
\hat{y}k &= \hat{y}k^- + K{21}(z{x,k} - \hat{x}k^-) + K{22}(z{y,k} - \hat{y}k^-) \[2pt]
\hat{v}{x,k} &= \hat{v}{x,k}^- + K{31}(z_{x,k} - \hat{x}k^-) + K{32}(z_{y,k} - \hat{y}k^-) \[2pt]
\hat{v}{y,k} &= \hat{v}{y,k}^- + K{41}(z_{x,k} - \hat{x}k^-) + K{42}(z_{y,k} - \hat{y}_k^-)
\end{aligned}
$$
最后更新协方差，让它变小——因为我们刚融合了测量，不确定性降低了：

$$
\mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k \mathbf{H}) \mathbf{P}_k^-
$$
在代码里，这几步合在一个 step 函数中：

def step(self, z):
    # z = [x_meas, y_meas]
    
    # 预测步
    self.X = self.F @ self.X
    self.P = self.F @ self.P @ self.F.T + self.Q
    
    # 更新步
    y_res = 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_res
    self.P = (np.eye(4) - K @ self.H) @ self.P

第四步：预测未来

至此，我们得到了当前时刻的最优位置$$\hat{x}k, \hat{y}k$$ 和速度$$\hat{v}{x,k}, \hat{v}{y,k}$$。但无人机有视觉延迟，假设延迟为$$t_{delay}$$，我们现在看到的其实是$$t_{delay}$$ 秒前的影像。所以要向前外推：

$$
\begin{aligned}
x_{pred} &= \hat{x}k + \hat{v}{x,k} \cdot t_{delay} \
y_{pred} &= \hat{y}k + \hat{v}{y,k} \cdot t_{delay}
\end{aligned}
$$

1 2	`def predict_ahead(self, delay): return self.X[:2] + self.X[2:] * delay`

这个预测坐标$$(x_{pred},; y_{pred})$$ 才是最终发给无人机控制器的目标点。

完整流程

把它串起来，一个完整的类长这样。初始化的时候把矩阵都建好，每来一帧视觉测量就调用一次 step，然后调用 predict_ahead 取预测值。

import numpy as np

class SimpleKalman:
    def __init__(self, dt, q=50.0, r_meas=0.64):
        self.dt = dt
        
        self.X = np.zeros(4)
        self.P = np.eye(4) * 500
        
        self.F = np.array([
            [1, 0, dt, 0],
            [0, 1, 0, dt],
            [0, 0, 1,  0],
            [0, 0, 0,  1]
        ])
        
        self.H = np.array([
            [1, 0, 0, 0],
            [0, 1, 0, 0]
        ])
        
        self.R = np.array([[r_meas, 0], [0, r_meas]])
        
        dt2 = dt ** 2
        dt3 = dt ** 3
        dt4 = dt ** 4
        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  ]
        ])
    
    def step(self, z):
        self.X = self.F @ self.X
        self.P = self.F @ self.P @ self.F.T + self.Q
        
        y_res = 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_res
        self.P = (np.eye(4) - K @ self.H) @ self.P
    
    def predict_ahead(self, delay):
        return self.X[:2] + self.X[2:] * delay

运行时就是这样（这里只是模拟实现，z_meas取值手动给予了一些点集）：

kf = SimpleKalman(dt=0.01, q=50.0, r_meas=0.64)

# 每一帧视觉输出到来时
for z_meas in vision_detections:
    kf.step(z_meas)
    target = kf.predict_ahead(delay=0.3)
    # 把 target 发给飞行控制器
    drone_controller.set_target(target[0], target[1])

回到那个雾中的比喻。卡尔曼滤波就像是你不再被雾气中的幻影牵着走，而是在脑子里保持了一个不断修正的动态草图——你知道他大概往哪个方向走，走多快，所以即使他暂时消失在雾里，你也能预判他的位置。

预测不是完美的。他会突然停下，会拐一个你没料到的弯。但没关系，下一帧观测来了，卡尔曼滤波会立刻修正。它总是在预测和修正之间摇摆，像一个不停自我怀疑又不停自我确认的人。

这大概就是卡尔曼滤波的美感所在。它不追求一次性的精确，而是在持续的对话中逼近真实。用不完美的模型和不完美的观测，拼凑出一个相对可靠的现在。

以及一个可以提前追赶的未来。