卡尔曼滤波做轨迹跟踪 鲁棒卡尔曼滤波做野值剔除后的预测 扩展卡尔曼滤波对GPS数据进行状态估计滤波

轨迹跟踪这活儿听起来玄乎，其实咱们每天都在用——手机导航里那个蓝色小圆点，背后八成藏着卡尔曼滤波的数学魔法。今天咱们扯点实在的，用Python代码带大家玩转三种滤波器的实战场景，保准比教科书里的公式推导带劲。

当轨迹开始蛇皮走位

普通卡尔曼滤波就像个稳重的会计，假设所有数据都老老实实。咱们先看个二维坐标跟踪的例子：

import numpy as np class BasicKalman: def __init__(self): self.dt = 1.0 # 采样间隔 self.F = np.array([[1,0,self.dt,0], [0,1,0,self.dt], [0,0,1,0], [0,0,0,1]]) # 观测矩阵只取位置 self.H = np.array([[1,0,0,0], [0,1,0,0]]) # 过程噪声（给加速度留点余地） self.Q = np.eye(4) * 0.1 # 观测噪声（GPS误差约5米） self.R = np.eye(2) * 25 def predict(self, state, covariance): new_state = self.F @ state new_cov = self.F @ covariance @ self.F.T + self.Q return new_state, new_cov def update(self, state, covariance, measurement): y = measurement - self.H @ state S = self.H @ covariance @ self.H.T + self.R K = covariance @ self.H.T @ np.linalg.inv(S) return (state + K @ y), (np.eye(4) - K @ self.H) @ covariance

关键在预测时的状态外推和测量更新时的卡尔曼增益计算。那个F矩阵里的dt参数特别有意思——如果把采样频率调高，相当于让滤波器更相信自己的运动模型。实测中遇到过无人机急转弯时，适当增大过程噪声Q能让跟踪更灵敏。

当GPS突然发神经

卡尔曼滤波做轨迹跟踪 鲁棒卡尔曼滤波做野值剔除后的预测 扩展卡尔曼滤波对GPS数据进行状态估计滤波

野外实测最怕遇到信号跳变，这时候鲁棒卡尔曼就该上场了。核心思路是动态调整观测噪声——发现异常测量值时，让滤波器别太当真：

class RobustKalman(BasicKalman): def update(self, state, covariance, measurement): residual = measurement - self.H @ state # 计算马氏距离（统计学上的异常值检测） S = self.H @ covariance @ self.H.T + self.R mahalanobis = residual.T @ np.linalg.inv(S) @ residual if mahalanobis > 7.815: # 卡方检验(0.05置信度, 自由度2) # 遇到野值，临时增大观测噪声 R_adjusted = self.R * 10 else: R_adjusted = self.R # 重新计算卡尔曼增益 S = self.H @ covariance @ self.H.T + R_adjusted K = covariance @ self.H.T @ np.linalg.inv(S) return (state + K @ residual), (np.eye(4) - K @ self.H) @ covariance

这个马氏距离的判断阈值不是拍脑袋来的——卡方分布表里查的，对应自由度为测量维度。实际部署时有个坑：动态调整R会导致协方差矩阵突然变化，可能引发震荡。后来加了个指数衰减的调整策略，效果稳如老狗。

当传感器开始画龙

处理GPS的经纬度数据时，扩展卡尔曼滤波（EKF）才是正解。因为地球坐标转换涉及非线性运算，直接上代码看怎么处理：

class GPSEKF: def __init__(self): from math import cos self.R_earth = 6378137 # 地球半径 self.state = np.zeros(4) # 纬度, 经度, 速度东, 速度北 self.P = np.eye(4) * 100 def predict(self): # 状态转移函数（考虑了经纬度与米换算） lat, lon, ve, vn = self.state dt = 1.0 new_lat = lat + (vn / self.R_earth) * (180/np.pi) * dt new_lon = lon + (ve / (self.R_earth * cos(lat*np.pi/180))) * (180/np.pi) * dt F = self.jacobian_F() # 计算雅可比矩阵 self.state = np.array([new_lat, new_lon, ve, vn]) self.P = F @ self.P @ F.T + np.diag([0.1, 0.1, 0.5, 0.5]) def jacobian_F(self): # 计算状态转移的雅可比矩阵 lat = self.state[0] * np.pi/180 ve = self.state[2] R = self.R_earth return np.array([ [1, 0, 0, (180/(np.pi*R))], [(ve * np.tan(lat))/(R * np.cos(lat)) * (180/np.pi), 1, (180/(np.pi*R*np.cos(lat))), 0], [0,0,1,0], [0,0,0,1] ])

这里最烧脑的是雅可比矩阵的计算——纬度变化会导致经度方向上每米对应的角度值变化。有个工程师在北极点附近调试时发现定位漂移，后来发现是没考虑cos(lat)接近零的情况，加了保护阈值才解决。所以EKF这玩意儿，数学推导再严谨，实际落地还是得和物理世界的边界条件死磕。

三种滤波器摆在一起看特别有意思：基础版是理想主义，鲁棒版是现实主义者，EKF则是戴着数学镣铐跳舞的艺术家。下次看到导航软件里的小箭头，不妨想想背后这些数字炼金术——毕竟能把卫星信号变成靠谱的轨迹，本身就是个挺酷的魔法。