从传感器噪声到平滑轨迹:用Python实战卡尔曼滤波与RTS平滑
如果你在自动驾驶项目中调试过融合定位模块,或者在物联网平台上处理过漂移的传感器数据,大概率会对“数据融合”这四个字又爱又恨。爱的是,它能把GPS、IMU、轮速计等一堆不靠谱的数据,变成一个相对靠谱的结果;恨的是,理论公式看起来清晰,一写代码就各种发散、滞后、调参调到怀疑人生。今天,我们不谈复杂的贝叶斯推导,就从一个工程师的视角,手把手带你用Python实现一个真正能跑起来、效果可见、参数可调的卡尔曼滤波器,并深入它的“好搭档”——RTS平滑算法,看看如何让历史轨迹也变得“光滑”起来。
这篇文章面向的是已经了解卡尔曼滤波基本概念,但卡在工程实现和调优阶段的开发者。我们会用具体的代码、可视化的结果和我在实际项目中踩过的坑,帮你把理论落地。你会发现,抛开那些吓人的协方差矩阵,核心逻辑其实非常直观。
1. 工程视角下的卡尔曼滤波:超越教科书公式
很多教科书把卡尔曼滤波讲成了一门线性代数课,满屏的矩阵运算让人望而却步。但在工程师眼里,它本质上是一个带反馈的预测-校正循环。想象一下你在用IMU(惯性测量单元)估计无人机姿态:IMU本身测量(加速度计、陀螺仪)很快但有累积误差(漂移),而其他传感器(比如视觉或磁力计)可能更新慢但绝对准确。卡尔曼滤波就是那个聪明的“裁判”,它相信IMU的短期预测,但时不时用其他传感器的观测来纠正它的长期偏差。
1.1 定义你的状态与模型:一切从简开始
动手编码的第一步,是为你面对的具体问题建立状态空间模型。这是最关键的一步,模型定义错了,后面再怎么调参都是徒劳。我们以一个经典的一维匀速运动模型为例,来跟踪一个在直线上运动的物体。
在这个模型里,我们关心两个量:物体的位置(p) 和速度(v)。所以我们的状态向量 x 就是 [p, v]^T。
-
状态转移矩阵(A):描述状态如何随时间演变。对于匀速运动,过了一个时间间隔
dt后,新位置 = 旧位置 + 速度 * dt,而速度保持不变。因此:import numpy as np dt = 0.1 # 时间步长,秒 A = np.array([[1, dt], [0, 1]])这个矩阵就是我们的“物理规律”。
-
控制输入矩阵(B)与输入(u):如果物体受外力(如加速度),我们需要这个。本例假设无外力,所以忽略。
-
过程噪声协方差(Q):承认我们的模型不完美。匀速假设可能不严格成立(有微小加速度),或者有风阻等未建模因素。
Q矩阵量化了这种不确定性。通常我们假设位置和速度的噪声是独立的:# 假设速度噪声的方差为0.1,位置噪声由速度噪声积分得到 velocity_noise = 0.1 Q = np.array([[dt**4/4, dt**3/2], [dt**3/2, dt**2]]) * velocity_noise**2这个公式来源于将连续时间的过程噪声离散化,是工程中的常见做法。
-
观测矩阵(H):我们用什么传感器观测状态?假设我们只有一个GPS,只测量位置,不直接测速度。那么:
H = np.array([[1, 0]]) # 只观测位置(状态向量的第一个元素) -
观测噪声协方差(R):传感器不准,GPS有误差。这个值通常可以从传感器数据手册或实测数据中估算。
R = np.array([[5.0]]) # 假设GPS测量的位置方差是5平方米 -
初始状态与协方差(x0, P0):滤波器启动时需要的一个“猜测”。协方差
P0初始可以设大一些,表示我们最初很不确定,滤波器会快速收敛。x0 = np.array([0, 1]) # 猜测初始位置0,速度1 m/s P0 = np.eye(2) * 100 # 初始不确定性很大
提示:
Q和R是卡尔曼滤波的“调参旋钮”。R大表示你更相信模型预测,Q大表示你更相信观测。它们的相对大小决定了滤波器的“性格”。
1.2 核心循环的Python实现:拆解每一步
现在,我们把著名的预测-更新两步写成代码。我会用详细的注释解释每一行在做什么。
class KalmanFilter:
def __init__(self, A, H, Q, R, x0, P0):
self.A = A # 状态转移矩阵
self.H = H # 观测矩阵
self.Q = Q # 过程噪声协方差
self.R = R # 观测噪声协方差
self.x = x0 # 当前状态估计
self.P = P0 # 当前估计误差协方差
def predict(self):
"""预测步骤:基于模型推算下一时刻状态"""
# 状态预测: x_k|k-1 = A * x_k-1|k-1
self.x = self.A @ self.x
# 协方差预测: P_k|k-1 = A * P_k-1|k-1 * A^T + Q
self.P = self.A @ self.P @ self.A.T + self.Q
# 返回预测值,可供使用
return self.x.copy(), self.P.copy()
def update(self, z):
"""更新步骤:用传感器观测z修正预测"""
# 计算观测残差(新息): y = z - H * x_k|k-1
y = z - self.H @ self.x
# 计算新息协方差: S = H * P_k|k-1 * H^T + R
S = self.H @ self.P @ self.H.T + self.R
# 计算卡尔曼增益: K = P_k|k-1 * H^T * S^-1
# 注意:这里使用np.linalg.inv,实际中对于标量观测或大矩阵有更高效稳定的解法
K = self.P @ self.H.T @ np.linalg.inv(S)
# 状态更新: x_k|k = x_k|k-1 + K * y
self.x = self.x + K @ y
# 协方差更新: P_k|k = (I - K * H) * P_k|k-1
I = np.eye(self.P.shape[0])
self.P = (I - K @ self.H) @ self.P
# 返回更新后的状态
return self.x.copy(), self.P.copy()
def run(self, measurements):
"""运行滤波器处理整个观测序列"""
estimates = []
for z in measurements:
self.predict()
est, _ = self.update(z)
estimates.append(est)
return np.array(estimates)
让我们在模拟数据上跑一下这个滤波器,看看效果:
# 生成模拟数据:一个匀速运动物体,受到过程噪声干扰,并被带有噪声的GPS观测
true_velocity = 1.2
true_positions = []
measured_positions = []
time_steps = 50
for t in range(time_steps):
true_pos = true_velocity * t * dt + np.random.randn() * 0.05 # 真实位置带微小噪声
true_positions.append(true_pos)
measured_pos = true_pos + np.random.randn() * np.sqrt(R[0,0]) # GPS观测噪声
measured_positions.append(measured_pos)
# 初始化并运行滤波器
kf = KalmanFilter(A, H, Q, R, x0, P0)
estimated_states = kf.run(np.array(measured_positions).reshape(-1, 1))
estimated_positions = estimated_states[:, 0]
# 可视化
import matplotlib.pyplot as plt
plt.figure(figsize=(10, 6))
plt.plot(np.arange(time_steps)*dt, true_positions, 'g-', label='真实轨迹', linewidth=2)
plt.plot(np.arange(time_steps)*dt, measured_positions, 'r+', label='GPS观测(含噪声)', markersize=5)
plt.plot(np.arange(time_steps)*dt, estimated_positions, 'b-', label='卡尔曼滤波估计', linewidth=1.5)
plt.xlabel('时间 (秒)')
plt.ylabel('位置 (米)')
plt.legend()
plt.grid(True)
plt.title('卡尔曼滤波效果对比:平滑噪声跟踪趋势')
plt.show()
运行这段代码,你会看到红色的“+”号是嘈杂的GPS信号,绿色的线是隐藏的真实轨迹,而蓝色的线是我们的卡尔曼滤波估计结果。蓝色线不仅平滑了噪声,还很好地跟踪了绿色线的趋势——这就是数据融合的魔力:它得到了比任何一个单一传感器都更好的结果。
2. 当滤波不够“顺滑”:深入RTS平滑算法
卡尔曼滤波是“在线”算法,它只利用当前和过去的观测来估计当前状态。这带来一个问题:在时间点 k 的估计,只基于 y1:k 的信息。但当整个数据序列 y1:T 都采集完毕后,我们回头看历史时刻 k,其实拥有了更多“未来”的信息(y_k+1:T)。RTS(Rauch–Tung–Striebel)平滑就是利用全部数据,重新估计历史状态,从而得到更准确、更平滑的结果。
2.1 为什么需要平滑?一个直观案例
想象一个自动驾驶汽车正在定位。滤波器实时输出位置用于控制。但在事后分析轨迹、绘制高清地图或进行事故复盘时,我们不再需要实时性,而是追求全局最优的轨迹估计。平滑算法可以利用车辆“后来”走过的路的信息,来修正“之前”位置估计的误差。
平滑与滤波的核心区别在于信息的使用:
| 特性 | 卡尔曼滤波 | RTS平滑 |
|---|---|---|
| 信息范围 | 因果的,仅用过去和现在数据 (y1:k) | 非因果的,使用全部数据 (y1:T) |
| 运行方向 | 前向(从过去到未来) | 后向(从未来回溯到过去) |
| 输出时机 | 在线,每个时刻k立即输出 | 离线,需等所有数据采集完后处理 |
| 估计精度 | 最优线性无偏估计(基于当前信息) | 均方误差更小(基于全部信息) |
| 主要用途 | 实时控制、导航、预警 | 轨迹后处理、数据分析、建图、参数辨识 |
注意:平滑不会改变最新时刻
T的估计,因为对它来说没有“未来”信息。平滑主要提升的是中间时刻和历史时刻的估计质量。
2.2 RTS平滑的Python实现:前向滤波,后向平滑
RTS平滑分为两个阶段:
- 前向滤波:用普通的卡尔曼滤波跑一遍全部数据,保存下每个时刻的滤波结果
(m_k, P_k)和预测结果(m_k-, P_k-)。 - 后向平滑:从最后一个时刻
T开始,倒着往回计算平滑估计(m_k^s, P_k^s)。
class RTS_Smoother:
def __init__(self, A, Q):
self.A = A
self.Q = Q
def smooth(self, filtered_means, filtered_covs, predicted_means, predicted_covs):
"""
执行RTS平滑。
参数:
filtered_means: 列表,每个元素是k时刻滤波后的状态均值 m_k
filtered_covs: 列表,每个元素是k时刻滤波后的状态协方差 P_k
predicted_means:列表,每个元素是k时刻预测的状态均值 m_k- (索引与filtered对应)
predicted_covs: 列表,每个元素是k时刻预测的状态协方差 P_k-
返回:
smoothed_means: 平滑后的状态均值序列 m_k^s
smoothed_covs: 平滑后的状态协方差序列 P_k^s
"""
n_steps = len(filtered_means)
dim_state = filtered_means[0].shape[0]
# 初始化平滑结果,末尾时刻与滤波结果相同
smoothed_means = [None] * n_steps
smoothed_covs = [None] * n_steps
smoothed_means[-1] = filtered_means[-1].copy()
smoothed_covs[-1] = filtered_covs[-1].copy()
# 后向递归 (从 T-1 到 0)
for k in reversed(range(n_steps - 1)):
m_k = filtered_means[k]
P_k = filtered_covs[k]
m_kplus1_minus = predicted_means[k+1] # m_{k+1}^-
P_kplus1_minus = predicted_covs[k+1] # P_{k+1}^-
m_kplus1_s = smoothed_means[k+1] # m_{k+1}^s
P_kplus1_s = smoothed_covs[k+1] # P_{k+1}^s
# 计算平滑增益 G_k
# G_k = P_k * A_k^T * (P_{k+1}^-)^-1
G_k = P_k @ self.A.T @ np.linalg.inv(P_kplus1_minus)
# 平滑状态更新: m_k^s = m_k + G_k (m_{k+1}^s - m_{k+1}^-)
smoothed_means[k] = m_k + G_k @ (m_kplus1_s - m_kplus1_minus)
# 平滑协方差更新: P_k^s = P_k + G_k (P_{k+1}^s - P_{k+1}^-) G_k^T
smoothed_covs[k] = P_k + G_k @ (P_kplus1_s - P_kplus1_minus) @ G_k.T
return smoothed_means, smoothed_covs
让我们把平滑算法应用到之前的模拟数据上,对比一下滤波和平滑的结果:
# 首先,运行卡尔曼滤波并保存中间结果
kf_for_smoothing = KalmanFilter(A, H, Q, R, x0, P0)
filtered_means_list = []
filtered_covs_list = []
predicted_means_list = []
predicted_covs_list = []
for z in measured_positions:
# 预测并保存预测值
pred_mean, pred_cov = kf_for_smoothing.predict()
predicted_means_list.append(pred_mean.copy())
predicted_covs_list.append(pred_cov.copy())
# 更新并保存滤波值
filt_mean, filt_cov = kf_for_smoothing.update(np.array([z]))
filtered_means_list.append(filt_mean.copy())
filtered_covs_list.append(filt_cov.copy())
# 执行RTS平滑
smoother = RTS_Smoother(A, Q)
smoothed_means, smoothed_covs = smoother.smooth(filtered_means_list, filtered_covs_list,
predicted_means_list, predicted_covs_list)
smoothed_positions = [m[0] for m in smoothed_means]
# 可视化对比
plt.figure(figsize=(12, 8))
plt.subplot(2, 1, 1)
plt.plot(np.arange(time_steps)*dt, true_positions, 'g-', label='真实轨迹', linewidth=2, alpha=0.7)
plt.plot(np.arange(time_steps)*dt, estimated_positions, 'b-', label='卡尔曼滤波', linewidth=1.5)
plt.plot(np.arange(time_steps)*dt, smoothed_positions, 'm--', label='RTS平滑', linewidth=2)
plt.xlabel('时间 (秒)')
plt.ylabel('位置 (米)')
plt.legend()
plt.grid(True)
plt.title('滤波 vs. 平滑:轨迹估计对比')
plt.subplot(2, 1, 2)
# 计算并绘制估计误差
filter_error = np.array(true_positions) - np.array(estimated_positions)
smooth_error = np.array(true_positions) - np.array(smoothed_positions)
plt.plot(np.arange(time_steps)*dt, filter_error, 'b-', label='滤波误差', linewidth=1)
plt.plot(np.arange(time_steps)*dt, smooth_error, 'm--', label='平滑误差', linewidth=1)
plt.xlabel('时间 (秒)')
plt.ylabel('估计误差 (米)')
plt.legend()
plt.grid(True)
plt.title('估计误差对比:平滑通常误差更小、更稳定')
plt.tight_layout()
plt.show()
# 定量比较:均方根误差 (RMSE)
filter_rmse = np.sqrt(np.mean(filter_error**2))
smooth_rmse = np.sqrt(np.mean(smooth_error**2))
print(f"卡尔曼滤波位置估计RMSE: {filter_rmse:.4f} 米")
print(f"RTS平滑位置估计RMSE: {smooth_rmse:.4f} 米")
从图表和RMSE数值中,你能清晰地看到,紫色的平滑轨迹通常比蓝色的滤波轨迹更贴近绿色的真实轨迹,尤其是在轨迹的中段。误差曲线也显示,平滑后的误差(紫虚线)其波动幅度往往比滤波误差(蓝实线)要小。这就是利用了全局信息带来的优势。
3. 从仿真到实战:处理真实传感器数据的技巧与坑点
在干净的数据集上跑通算法只是第一步。真实项目中的数据往往充满挑战:传感器不同步、数据丢失、噪声非高斯、模型非线性等等。下面分享几个我从实际物联网和机器人项目中总结的关键点。
3.1 参数调优:Q和R不是猜出来的
Q(过程噪声)和 R(观测噪声)是滤波器的灵魂。设置不当,滤波器要么反应迟钝(过于相信模型),要么跟着噪声跳舞(过于相信观测)。
- 获取R:最靠谱的方法是从传感器静态测试中计算。将传感器静止放置一段时间,采集数据,计算其输出的方差。这就是
R的初始值。对于多传感器,R是对角矩阵,对角线元素就是各传感器的噪声方差。 - 调试Q:
Q更难直接测量,它体现了你对模型的不信任程度。一个实用的方法是试凑法结合新息序列。- 新息(Innovation) 就是代码中的
y = z - H * x_pred,即观测与预测的差值。理论上,如果模型和噪声参数完全正确,新息序列应该是一个零均值、协方差为S的白噪声。 - 你可以绘制新息序列的自相关图。如果它不是白噪声(存在明显自相关),说明模型有未考虑的动态,或者
Q设小了。 - 一个调试流程:
- 根据物理直觉给
Q一个量级合理的初始值(例如,速度噪声0.1 m/s)。 - 运行滤波器,计算并绘制新息序列。
- 如果新息均值明显不为零,检查观测模型
H或初始状态。 - 如果新息序列变化缓慢(低频成分多),说明滤波器过于相信模型,增大Q。
- 如果新息序列像纯粹的观测噪声(高频抖动),说明滤波器过于相信观测,减小Q。
- 根据物理直觉给
- 新息(Innovation) 就是代码中的
# 示例:计算并分析新息序列
def analyze_innovation(kf_instance, measurements):
innovations = []
for z in measurements:
kf_instance.predict()
# 在update内部计算新息,这里我们手动算一次以便记录
y = z - kf_instance.H @ kf_instance.x
innovations.append(y[0])
kf_instance.update(z)
innovations = np.array(innovations)
# 绘制新息序列
plt.figure(figsize=(10, 4))
plt.subplot(1, 2, 1)
plt.plot(innovations)
plt.axhline(y=0, color='r', linestyle='--', alpha=0.5)
plt.xlabel('时间步')
plt.ylabel('新息')
plt.title('新息序列')
plt.grid(True)
# 绘制新息直方图(应近似正态分布)
plt.subplot(1, 2, 2)
plt.hist(innovations, bins=20, edgecolor='black', alpha=0.7)
plt.xlabel('新息值')
plt.ylabel('频次')
plt.title('新息分布')
plt.grid(True)
plt.tight_layout()
plt.show()
# 计算新息均值与标准差(应与理论S比较)
print(f"新息均值: {np.mean(innovations):.4f}")
print(f"新息标准差: {np.std(innovations):.4f}")
# 理论上,新息的方差应为 S = H*P-*H^T + R,可以计算一个平均的理论S来对比
3.2 应对数据丢失与异步:健壮性设计
真实系统中,GPS可能进入隧道丢失信号,不同传感器数据到达时间可能错位。
-
处理数据丢失:当某个传感器的观测
z缺失时,只执行预测步骤,跳过更新步骤。此时,状态估计的不确定性(P)会因预测而增大,这符合直觉:没有新信息来纠正,我们对自己的估计信心下降。# 在运行循环中处理可能丢失的数据 for sensor_data in data_stream: if sensor_data['gps'] is not None: z = sensor_data['gps'] kf.update(z) else: # 只有预测,没有更新 kf.predict() # 注意:这里predict了两次?需要根据循环结构调整。通常循环体以predict开始。 # 更常见的模式是:每次循环先predict,然后如果有数据就update。 # 记录当前状态 estimated_states.append(kf.x.copy()) -
处理异步数据:对于不同频率、不同时刻到达的数据,有更高级的算法如多速率卡尔曼滤波或异步卡尔曼滤波。一个简单但有效的工程近似是:为每个传感器维护一个独立的滤波器状态副本,或者使用一个状态扩增的滤波器,在每次收到任何传感器数据时,都进行预测(推进到当前数据时间戳)然后更新。这涉及到更复杂的状态转移和插值,是实际系统中的一个难点。
3.3 扩展至非线性:EKF与UKF初探
我们的模型是线性的(A, H 是矩阵)。但很多实际系统是非线性的,例如车辆的运动模型(涉及转向角)、机器人的姿态估计(涉及三角函数)。这时就需要非线性滤波。
-
扩展卡尔曼滤波(EKF):核心思想是局部线性化。在当前估计点,对非线性状态转移函数
f(x)和观测函数h(x)进行一阶泰勒展开,用雅可比矩阵F和H代替原来的A和H。- 优点:概念相对直接,计算量适中。
- 缺点:线性化误差在非线性程度高时会导致滤波器发散。
- 何时用:模型非线性度不高,且你有办法计算雅可比矩阵(解析或数值)。
-
无迹卡尔曼滤波(UKF):采用一种更巧妙的思路。它不进行线性化,而是精心挑选一组样本点(Sigma点),让这些点经过真实的非线性函数变换,再用变换后的点来估计新的均值和协方差。
- 优点:通常比EKF精度更高,尤其对于强非线性系统;无需计算雅可比矩阵。
- 缺点:计算量比EKF稍大。
- 何时用:模型非线性较强,或者雅可比矩阵难以求取。
对于大多数工程应用,如果线性模型勉强可用,就尽量用线性KF。如果必须处理非线性,UKF通常是比EKF更鲁棒和简单的选择。Python的 filterpy 库提供了优秀的EKF和UKF实现,可以避免你重复造轮子。
4. 案例拆解:用卡尔曼滤波融合IMU与轮速计
让我们看一个更贴近实际的迷你案例:一个地面机器人,有一个便宜的IMU(陀螺仪测量角速度,加速度计测量加速度)和两个轮子的编码器(通过里程计推算位移)。IMU高频但漂移,里程计低频但相对准确。我们将用卡尔曼滤波来融合它们,估计机器人的2D平面位姿(x, y, 航向角θ)。
状态定义: x = [x, y, θ, v, ω]^T, 其中 v 是前进速度,ω 是转向角速度。
观测来源:
- 里程计:提供位移增量
Δx_odom,Δy_odom,可以转换成位置观测(需注意坐标系转换)。 - IMU陀螺仪:直接提供角速度
ω_gyro。 - IMU加速度计:在机器人坐标系下测量加速度,经过姿态旋转和重力补偿后,可以(很粗略地)积分得到速度变化,但这里噪声极大,我们可能只把它作为辅助或用于检测运动。
融合策略:
- 以IMU的高频角速度作为状态
ω的“过程模型”主导,因为它是瞬时测量。 - 用里程计的低频位移来强力校正位置
(x, y)和航向θ的累积误差。 - 这是一个典型的松耦合融合方案。
Q矩阵需要为IMU速度/角速度估计设置较大的噪声(因为积分会漂移),R矩阵为里程计设置较小的噪声(因为它相对准,但可能有打滑)。
实现这个滤波器需要更复杂的模型(涉及三角函数,是非线性的),因此通常会采用EKF或UKF。这个案例的代码量较大,但遵循的模式与本文的线性例子一致:定义状态、定义非线性函数 f 和 h、设置噪声矩阵、运行滤波循环。调试的关键在于仔细校准IMU与里程计之间的坐标系关系和时间同步。
我在一个室内扫地机器人项目中使用过类似的方案。最初只用车轮里程计,在光滑地板上打滑时定位会完全失效。加入IMU进行融合后,虽然不能完全消除打滑影响,但系统在短时间打滑时能依靠IMU维持一个大致正确的航向,等轮子恢复抓地力后,里程计观测又能把位置拉回来。这种冗余和互补,正是多传感器融合的价值所在。
调试这样的系统,除了代码,还需要大量的实地测试和数据记录分析。我习惯把滤波器每个时刻的状态、协方差、新息都记录下来,事后用Jupyter Notebook进行可视化分析,看看误差在哪里变大,新息是否异常,从而有针对性地调整模型或噪声参数。这个过程没有捷径,但每一次调试成功,你对系统和算法的理解都会加深一层。
3530



被折叠的 条评论
为什么被折叠?



