卡尔曼滤波。 1、卡尔曼滤波的含义是现时刻的最佳估计为在前一时刻的最佳估计的基础上根据现时刻的...

卡尔曼滤波。 1、卡尔曼滤波的含义是现时刻的最佳估计为在前一时刻的最佳估计的基础上根据现时刻的观测值作线性修正。 2、卡尔曼滤波在数学上是一种线性最小方差统计估算方法，它是通过处理一系列带有误差的实际测量数据而得到物理参数的最佳估算。 1、包含噪声的，对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。 2、这个估计可以是对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。

卡尔曼滤波像是给系统装了个自动纠偏的导航仪。想象你在开车，GPS定位偶尔飘移，仪表盘车速也有延迟。这时候大脑会自动综合地图信息和实时车速，估算出车辆的真实位置——这种直觉般的融合能力，就是卡尔曼滤波的魔法。

先看个一维运动的例子。假设有个小车匀速向右移动，但我们只能通过带噪声的传感器观测它的位置。下面这段代码模拟了这个场景：

import numpy as np import matplotlib.pyplot as plt real_pos = 0 real_vel = 2 # 固定速度2m/s noise_scale = 3 # 观测噪声强度 # 生成100个时刻的观测数据 observations = [] for _ in range(100): real_pos += real_vel observations.append(real_pos + np.random.randn()*noise_scale) plt.plot(observations,'r.',label='观测值') plt.plot([real_vel*i for i in range(100)],'g-',label='真实轨迹') plt.legend()

运行这段代码会看到红色散点是带噪声的观测，绿线是真实轨迹。现在要用卡尔曼滤波从这些抖动数据中还原真实运动。

# 卡尔曼参数初始化 x = np.array([[0], # 初始位置估计 [0]]) # 初始速度估计 P = np.eye(2)*50 # 初始估计误差协方差（很大不确定性） F = np.array([[1, 1], # 状态转移矩阵（假设匀速） [0, 1]]) Q = np.eye(2)*0.01 # 过程噪声协方差 H = np.array([[1, 0]]) # 观测矩阵（只能观测位置） R = np.array([[3]]) # 观测噪声协方差 estimates = [] for z in observations: # 预测步骤 x = F @ x P = F @ P @ F.T + Q # 更新步骤 K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R) x = x + K @ (z - H @ x) P = (np.eye(2) - K @ H) @ P estimates.append(x[0,0]) plt.plot(estimates,'b-',label='卡尔曼估计')

代码中的状态转移矩阵F像时间推进器，[1,1;0,1]结构意味着位置变化=原位置+速度*1秒，速度保持不变。但Q矩阵留了个后门——允许系统存在轻微的运动突变。

观测更新时的K矩阵（卡尔曼增益）是精妙所在，它自动计算该相信模型预测还是传感器数据。当观测噪声R增大时，K会变小，系统更依赖模型；反之当模型预测不准（P增大），则更相信新数据。

卡尔曼滤波。 1、卡尔曼滤波的含义是现时刻的最佳估计为在前一时刻的最佳估计的基础上根据现时刻的观测值作线性修正。 2、卡尔曼滤波在数学上是一种线性最小方差统计估算方法，它是通过处理一系列带有误差的实际测量数据而得到物理参数的最佳估算。 1、包含噪声的，对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。 2、这个估计可以是对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。

把三条曲线画在一起，蓝色估计线几乎贴着绿色真实轨迹，完美过滤了红色观测点的抖动。这就是线性最优估计的魅力——用最基础的矩阵运算，在噪声中抓住了真实运动的脉搏。

进阶玩法可以修改F矩阵实现变速模型，比如把速度项改为随时间变化。或者调整Q和R的值，感受系统在不同信任度下的表现。甚至修改H矩阵实现多传感器融合，比如同时处理GPS和里程计数据，让估计更稳健。

这算法在阿波罗登月时处理过地面测控数据，如今每部手机的定位模块里都藏着它的身影。下次用地图导航时，或许会想起这几个矩阵在后台默默工作的样子——用数学之美，对抗现实世界的噪声与不确定。