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

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

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

import numpy as np

class BasicKalman:
    def __init__(self):
        self.dt = 1.0  # 采样间隔
        # 状态矩阵 [x, y, vx, vy]
        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突然发神经
野外实测最怕遇到信号跳变，这时候鲁棒卡尔曼就该上场了。核心思路是动态调整观测噪声——发现异常测量值时，让滤波器别太当真：

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则是戴着数学镣铐跳舞的艺术家。下次看到导航里的小箭头，不妨想想背后这些数字炼金术——毕竟能把卫星信号变成靠谱的轨迹，本身就是个挺酷的魔法。
卡尔曼滤波做轨迹跟踪
鲁棒卡尔曼滤波做野值剔除后的预测
扩展卡尔曼滤波对GPS数据进行状态估计滤波