📚 课程目录第4章卡尔曼滤波平滑相机轨迹

4. 卡尔曼滤波平滑相机轨迹

任务说明

本案例针对运动相机拍摄视频中因手持抖动导致的画面不稳定问题，对前一阶段估计出的相机运动轨迹（位移与速度）应用卡尔曼滤波进行平滑处理，去除高频抖动分量，保留平滑的主动运动（如平移、旋转），最终利用平滑后的轨迹对视频帧进行重映射，生成稳定视频。核心目标是实现一个可独立运行的一维卡尔曼滤波平滑器，并验证其能有效衰减叠加在匀速轨迹上的随机抖动。

卡尔曼滤波模型定义

采用恒定速度模型（Constant Velocity Model）描述相机运动状态。状态向量为 x = [p, v]^T，其中 p 为位移（像素），v 为速度（像素/帧）。系统方程为：

x_k = A * x_{k-1} + w,   A = [[1, 1], [0, 1]]
z_k = H * x_k + v,       H = [[1, 0]]

其中过程噪声协方差 Q 和测量噪声协方差 R 为可调参数。平滑强度由 Q/R 比值控制：比值越小，滤波结果越平滑，但对真实运动的响应延迟越大。

代码实现

import numpy as np

class KalmanSmoother:
    """一维恒定速度卡尔曼滤波器，用于平滑相机轨迹"""
    def __init__(self, q=0.01, r=0.1):
        # 状态转移矩阵 A (2x2)
        self.A = np.array([[1.0, 1.0],
                           [0.0, 1.0]])
        # 观测矩阵 H (1x2)
        self.H = np.array([[1.0, 0.0]])
        # 过程噪声协方差 Q
        self.Q = np.array([[q, 0.0],
                           [0.0, q]])
        # 测量噪声协方差 R
        self.R = np.array([[r]])
        # 状态协方差初始值 P
        self.P = np.eye(2) * 100.0
        # 初始状态 [位置, 速度]
        self.x = np.zeros((2, 1))
        self.initialized = False

    def reset(self):
        """重置滤波器状态"""
        self.P = np.eye(2) * 100.0
        self.x = np.zeros((2, 1))
        self.initialized = False

    def update(self, measurement):
        """
        输入单个位置测量值，返回平滑后的位置
        measurement: float 或 int，当前帧估计的相机位移
        """
        z = np.array([[float(measurement)]])
        if not self.initialized:
            # 首次测量直接初始化位置，速度设为0
            self.x[0, 0] = z[0, 0]
            self.x[1, 0] = 0.0
            self.initialized = True
            return self.x[0, 0]

        # 预测步骤
        x_pred = self.A @ self.x
        P_pred = self.A @ self.P @ self.A.T + self.Q

        # 更新步骤
        S = self.H @ P_pred @ self.H.T + self.R
        K = P_pred @ self.H.T @ np.linalg.inv(S)
        innovation = z - self.H @ x_pred
        self.x = x_pred + K @ innovation
        self.P = (np.eye(2) - K @ self.H) @ P_pred

        return self.x[0, 0]

    def smooth_trajectory(self, measurements):
        """平滑整条轨迹，返回平滑后的位置数组"""
        self.reset()
        smoothed = []
        for m in measurements:
            smoothed.append(self.update(m))
        return np.array(smoothed)

def generate_test_trajectory(n_frames=200, true_speed=2.0, noise_std=15.0):
    """生成模拟测试轨迹：匀速运动 + 高斯抖动噪声"""
    true_pos = np.arange(n_frames) * true_speed
    noise = np.random.normal(0, noise_std, n_frames)
    noisy_pos = true_pos + noise
    return true_pos, noisy_pos

if __name__ == "__main__":
    # 生成测试数据
    np.random.seed(42)
    true_traj, noisy_traj = generate_test_trajectory()

    # 创建滤波器并平滑
    smoother = KalmanSmoother(q=0.001, r=100.0)
    smoothed_traj = smoother.smooth_trajectory(noisy_traj)

    # 计算平滑误差（与真实轨迹对比）
    smooth_error = np.mean(np.abs(smoothed_traj - true_traj))
    noisy_error = np.mean(np.abs(noisy_traj - true_traj))

    print(f"原始噪声轨迹平均误差: {noisy_error:.2f} 像素")
    print(f"卡尔曼平滑后平均误差: {smooth_error:.2f} 像素")
    print(f"误差降低比例: {(1 - smooth_error/noisy_error)*100:.1f}%")

落地与运行方式

将上述代码保存为 kalman_smoother.py，在装有 numpy 的 Python 3.8+ 环境中运行：

python kalman_smoother.py

代码会生成一条包含200帧的模拟轨迹：真实轨迹为每帧2像素的匀速运动，叠加标准差15像素的高斯抖动。滤波器参数设置为 q=0.001（过程噪声小，信任运动模型）、r=100.0（测量噪声大，强平滑）。

验证方法与判定标准

运行程序后观察终端输出。判定标准如下：

功能正确性：程序无异常退出，输出三行统计信息。
平滑有效性：平滑后平均误差应显著小于原始噪声轨迹误差，误差降低比例应大于50%。若未达到，可适当增大 r 值（如 r=200）或减小 q 值（如 q=0.0005）增强平滑力度。
轨迹保真度：平滑轨迹应保持整体趋势（斜率接近2像素/帧），不能出现严重滞后或畸变。可通过修改代码打印前10帧数据人工检查。

对于真实视频应用，将本滤波器替换为对每帧估计的相机位移序列（由光流或特征匹配获得）进行平滑，再将平滑后的位移转换为仿射变换矩阵，对原始帧进行 cv2.warpAffine 重映射即可输出稳定视频。