简介这份文档是北京航空航天大学卡尔曼滤波课程的GPS静/动态滤波实验报告面向学习卡尔曼滤波与导航定位的高年级本科生和研究生。报告围绕GPS定位精度提升展开分别建立静态与动态卡尔曼滤波模型推导状态方程与离散化滤波方程并用程序对实测GPS数据完成滤波处理。内容涵盖滤波前后导航轨迹对比、估计均方差P阵对角线开根号的变化趋势以及动态模型与静态模型的区别、R阵Q阵与P0阵选取对滤波精度与收敛速度的影响、最小二乘与卡尔曼滤波的优劣对比等思考题分析静态部分给出Q阵取零、按克拉索夫斯基椭球模型设定R阵的做法动态部分采用当前统计模型与一阶马尔科夫位置误差建模。资源包为单个docx文档约862KB公式推导与结果图完整适合作为实验报告参考或建模调参思路的复习资料。目前已有108人学习下载。1. 从北航卡尔曼滤波实验报告看 GPS 静动态滤波要解决什么很多人做 GPS 静动态滤波实验第一反应是调 Q/R结果静态点平滑得像模像样动态轨迹却在转弯处滞后半条街。北航卡尔曼滤波实验报告这类任务真正要交代清楚的是GPS 观测在静态和动态下的误差特性完全不同卡尔曼滤波的状态模型、观测模型和噪声矩阵必须跟着场景换。静态实验通常拿固定点数据验证滤波能否压低随机抖动动态实验拿车载或手持轨迹验证滤波能否在噪声、丢星和跳点中估计位置与速度。它适合导航、测绘、无人车和机器人定位方向的入门者也适合已经会写 numpy 卡尔曼滤波、但想把实验报告写成可复现流程的人。核心不是背公式而是让每一组 Q/R 都有物理来源。2. GPS 静动态滤波实验里的卡尔曼滤波状态方程与观测方程卡尔曼滤波的数学思想可以压成两句话预测时用状态转移矩阵传播均值和协方差更新时用观测残差和新息协方差修正状态。GPS 静动态滤波实验的难点不在矩阵乘法而在于状态向量里放什么、观测矩阵 H 怎么对应 GPS 经纬度、Q 和 R 的量纲怎么统一。静态实验可以只估计位置动态实验通常要加入速度否则滤波轨迹会像被橡皮筋拽住。下面把模型选型、坐标转换和噪声整定拆开讲每一步都能直接落到代码。2.1 静态与动态实验的模型选型CV、CA 还是位置随机游走静态 GPS 数据的特点是真实位置基本不变但接收机解算出的经纬度会随机跳动。此时用位置随机游走模型最省事状态向量只取[x, y]^T状态转移矩阵FI过程噪声Q设得很小表示“位置几乎不会自己动”。动态实验如果还用这个模型滤波器会认为目标没有速度GPS 点一跳估计值就慢半拍。车载和步行场景常用常速度模型状态向量[x, y, vx, vy]^TF里出现dt能估计速度并预测下一时刻位置。无人机或急加速场景可以用常加速模型但加速度状态会放大噪声实验报告里如果没有高频 IMU 辅助不建议一上来就用 CA。模型状态向量状态转移 F适用场景主要风险位置随机游走[x, y]^TIGPS 静态固定点动态下滞后严重常速度 CV[x, y, vx, vy]^T位置加dt*v车载、步行、低速机器人急转弯速度估计不准常加速 CA[x, y, vx, vy, ax, ay]^T速度加dt*a高动态无人机噪声放大、Q 难整定选择逻辑很直接静态实验用位置随机游走动态实验优先 CV。CV 模型不是越复杂越好GPS 采样率通常 1Hz 到 10Hz低于 1Hz 时 CV 的dt很大预测误差也会变大。实验报告里最好把模型假设写清楚静态段目标静止动态段目标近似匀速转弯和加减速作为模型未建模误差进入 Q。2.2 GPS 经纬度到局部平面坐标的观测矩阵 H 怎么定GPS 模块输出的是 WGS-84 经纬度和海拔卡尔曼滤波通常在局部平面坐标里做因为经纬度一度对应的东向距离随纬度变化。常见做法是选第一个点或轨迹中心作为原点把经纬度转成 ENU 东向、北向坐标。小范围实验用等距圆柱近似就够公式简单误差在百米级范围内可以接受。转换后观测向量z[east, north]^T如果状态是[x, y]^T观测矩阵HI如果状态是[x, y, vx, vy]^T观测矩阵只取位置import numpy as np def wgs84_to_enu(lat, lon, lat0, lon0): # 以参考点为原点将 WGS-84 经纬度转局部 ENU 平面坐标 R 6378137.0 # WGS-84 地球长半轴单位 m lat_rad np.deg2rad(lat) lon_rad np.deg2rad(lon) lat0_rad np.deg2rad(lat0) lon0_rad np.deg2rad(lon0) east R * (lon_rad - lon0_rad) * np.cos(lat0_rad) north R * (lat_rad - lat0_rad) return east, north # 动态 CV 模型的观测矩阵只观测位置不直接观测速度 H_cv np.array([ [1, 0, 0, 0], [0, 1, 0, 0] ], dtypefloat)逻辑说明wgs84_to_enu先取参考点再用经度差乘cos(lat0)得到东向距离用纬度差得到北向距离适合校园、园区、城市街区级实验。H_cv把四维状态映射到二维位置观测速度状态靠状态转移矩阵间接估计。参数说明R是地球半径lat0/lon0是参考点必须全轨迹统一如果实验范围超过几公里建议改用 UTM 或 pyproj 做更严格的投影。静态实验里也可以直接对经纬度做滤波但 Q/R 的量纲会变成度平方不如转平面坐标直观。注意如果后续要把轨迹叠加到高德底图WGS-84 和 GCJ-02 不是同一套坐标。python 将 gps 经纬度转换为高德经纬度这一步不能省常见做法是调用坐标转换库或公开偏移公式先转 GCJ-02 再导出 GeoJSON。2.3 Q 和 R 的物理含义与参数表Q 是过程噪声协方差代表状态转移模型没描述到的运动比如静态点被风吹动、车辆加减速、转弯。R 是观测噪声协方差代表 GPS 解算误差包括多路径、电离层、接收机噪声。静态实验里 GPS 误差可能呈现明显自相关R 不是越小越好R 设得太小滤波器会过度相信跳点轨迹跟着 GPS 抖动。动态实验里 R 可以按时段调整卫星数多、HDOP 小时 R 小一点城市峡谷里 R 大一点。参数物理含义静态实验建议动态实验建议调大后果Q 位置未建模位置扰动0.01~0.1 m^20.1~1 m^2跟随快抖动变大Q 速度未建模加速度扰动静态可关闭0.1~1 (m/s)^2速度估计噪声大R 位置GPS 水平观测方差由静态数据方差估可按时段时变平滑强转弯滞后P0初始状态不确定度10~100 m^2位置10 m^2速度1 (m/s)^2初期收敛慢CV 模型的 Q 常用连续白噪声加速度模型离散化sigma_a表示目标加速度标准差。步行取0.2~0.5 m/s^2城市车辆取0.5~1.5 m/s^2。代码构建如下def make_q_cv(dt, sigma_a0.5): # 连续白噪声加速度模型离散化dt 单位秒 dt2 dt * dt dt3 dt2 * dt dt4 dt2 * dt2 q sigma_a ** 2 Q q * np.array([ [dt4 / 4, 0, dt3 / 2, 0], [0, dt4 / 4, 0, dt3 / 2], [dt3 / 2, 0, dt2, 0], [0, dt3 / 2, 0, dt2] ]) return Q逻辑说明sigma_a越大Q 越大滤波器越愿意相信 GPS 观测动态跟随更快但平滑变弱。参数说明dt必须来自实际时间戳差值不能默认 1 秒sigma_a是唯一需要按场景调的加速度尺度实验报告里可以用静态段 GPS 位置方差反推 R再用动态段速度变化估计sigma_a。3. 用 Python 跑通 GPS 静态滤波实验的最小流程静态滤波实验的目标是回答三个问题原始 GPS 数据抖动多大滤波后标准差降到多少滤波结果是否引入明显偏差。流程不复杂但数据清洗和时间对齐容易被跳过。很多实验报告的 RMSE 差异其实来自时间戳没排序、重复点没去重、或者把经纬度当成平面坐标直接滤波。下面用一份 CSV 格式的 GPS 数据为例从读取、转换到滤波和评估走一遍。3.1 读取 GPS 数据并做时间对齐与异常值初筛常见 GPS 数据字段包括timestamp/lat/lon/hdop/satellites也可能来自 NMEA 语句解析后的 CSV。第一步按时间排序计算相邻时间差检查是否有重复时间戳和过大间隔。HDOP 大于 5 或卫星数小于 4 的点可以先标记不一定直接删除但要留给后续新息门限处理。import pandas as pd import numpy as np # 读取 GPS 数据假设已解析为 CSV df pd.read_csv(gps_static.csv) df[timestamp] pd.to_datetime(df[timestamp]) df df.sort_values(timestamp).drop_duplicates(timestamp).reset_index(dropTrue) # 计算实际采样间隔 df[dt] df[timestamp].diff().dt.total_seconds() print(df[dt].describe()) # 以第一点为原点转 ENU lat0, lon0 df.loc[0, lat], df.loc[0, lon] east, north [], [] for _, row in df.iterrows(): e, n wgs84_to_enu(row[lat], row[lon], lat0, lon0) east.append(e) north.append(n) df[east], df[north] east, north # 初筛HDOP 过大或卫星数不足的点打标签 df[bad_dop] (df[hdop] 5.0) | (df[satellites] 4)逻辑说明时间排序和去重保证dt可信ENU 转换让后续 Q/R 有米制量纲。参数说明hdop 5和satellites 4只是初筛阈值实验报告里应记录筛选前后点数被标记的点不直接删除后续用新息门限决定是否降权。3.2 静态卡尔曼滤波代码从经纬度到平滑坐标静态实验用位置随机游走模型状态[x, y]^TFIHI。R 可以用原始 ENU 坐标的方差估计Q 取一个很小的值表示固定点位置几乎不变。下面实现一个通用 KF 类后续动态实验也能复用。class KalmanFilter: def __init__(self, F, H, Q, R, x0, P0): self.F F # 状态转移矩阵 self.H H # 观测矩阵 self.Q Q # 过程噪声协方差 self.R R # 观测噪声协方差 self.x x0.reshape(-1, 1) # 状态列向量 self.P P0 # 状态协方差 def predict(self): self.x self.F self.x self.P self.F self.P self.F.T self.Q return self.x.copy(), self.P.copy() def update(self, z): z z.reshape(-1, 1) y z - self.H self.x # 新息 S self.H self.P self.H.T self.R K self.P self.H.T np.linalg.inv(S) self.x self.x K y I np.eye(self.P.shape[0]) self.P (I - K self.H) self.P return self.x.copy(), self.P.copy(), y, S # 静态实验状态为 [east, north] x0 np.array([df[east].iloc[0], df[north].iloc[0]], dtypefloat) P0 np.eye(2) * 10.0 F np.eye(2) H np.eye(2) Q np.eye(2) * 0.01 R np.eye(2) * 4.0 # 假设 GPS 水平误差约 2 m方差 4 m^2 kf KalmanFilter(F, H, Q, R, x0, P0) static_traj [] for z in np.c_[df[east], df[north]]: kf.predict() x_hat, P, y, S kf.update(z) static_traj.append(x_hat.ravel()) static_traj np.array(static_traj)逻辑说明predict传播状态和协方差update用新息y和卡尔曼增益K修正。静态模型里FI预测步骤只增加 Q因此 Q 很小时估计值接近加权平均。参数说明R4对应 2 米标准差如果实测静态点标准差 3 米R 应设 9Q0.01表示固定点位置随机游走极弱Q 再大滤波会重新跟随 GPS 抖动。3.3 静态实验的 RMSE、CEP95 与置信区间验证静态实验没有绝对真值常用滤波后坐标均值作为参考中心计算原始坐标和滤波坐标相对中心的误差。RMSE 看整体离散程度CEP95 看 95% 点落入的圆半径新息均值看模型是否无偏。实验报告里最好把原始、滤波、均值中心画在同一张散点图上。指标含义静态实验关注动态实验关注RMSE均方根误差越小越平滑需有参考轨迹CEP9595% 点落入圆半径定位精度轨迹误差包络新息均值观测与预测差均值是否接近零模型偏差检测新息方差新息离散程度与 S 是否一致异常点检测依据center static_traj.mean(axis0) raw_err np.linalg.norm(np.c_[df[east], df[north]] - center, axis1) kf_err np.linalg.norm(static_traj - center, axis1) rmse_raw np.sqrt(np.mean(raw_err ** 2)) rmse_kf np.sqrt(np.mean(kf_err ** 2)) cep95_raw np.percentile(raw_err, 95) cep95_kf np.percentile(kf_err, 95) print(rmse_raw, rmse_kf, cep95_raw, cep95_kf)逻辑说明center是滤波后轨迹均值作为静态参考点。参数说明RMSE 和 CEP95 单位都是米比较时保持同一参考中心如果原始 GPS 存在系统性偏移均值中心也会偏实验报告里应说明参考点定义。滤波后 RMSE 通常下降但 CEP95 不一定同比例下降因为卡尔曼滤波对长尾跳点的抑制取决于 R 和门限。4. 动态 GPS 滤波实验CV 模型、丢星与跳点处理动态实验和静态实验最大的差别是目标真的会动状态模型必须包含速度观测噪声 R 也不能全程恒定。城市道路里 GPS 跳点、多路径和隧道丢星会同时出现单纯调大 R 会让转弯轨迹切内角调小 R 又会把跳点当真实运动。动态实验报告里如果只放一张滤波前后轨迹对比图信息量不够至少要给出速度估计、新息序列和异常点标记。4.1 动态卡尔曼滤波的 F 矩阵和 dt 处理CV 模型的状态是[x, y, vx, vy]^TF用实际dt构造。很多实验数据不是严格 1Hz如果代码里写死dt1车辆加速时速度估计会漂。每个时刻都应根据时间戳差值重新生成F和QH保持只观测位置。def make_f_cv(dt): return np.array([ [1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1] ], dtypefloat) # 动态初始化位置取第一个点速度取前两个点差分 dt0 df[dt].iloc[1] if not np.isnan(df[dt].iloc[1]) else 1.0 vx0 (df[east].iloc[1] - df[east].iloc[0]) / dt0 vy0 (df[north].iloc[1] - df[north].iloc[0]) / dt0 x0 np.array([df[east].iloc[0], df[north].iloc[0], vx0, vy0], dtypefloat) P0 np.diag([10.0, 10.0, 1.0, 1.0]) H np.array([[1, 0, 0, 0], [0, 1, 0, 0]], dtypefloat) R np.eye(2) * 9.0 # 动态 GPS 误差可能比静态大 dynamic_traj [] for i in range(1, len(df)): dt df[dt].iloc[i] if np.isnan(dt) or dt 0: dt 0.1 kf.F make_f_cv(dt) kf.Q make_q_cv(dt, sigma_a0.8) kf.predict() z np.array([df[east].iloc[i], df[north].iloc[i]]) x_hat, P, y, S kf.update(z) dynamic_traj.append(x_hat.ravel()) dynamic_traj np.array(dynamic_traj)逻辑说明每个历元重新构造F和Q保证不同采样间隔下速度传播正确。参数说明sigma_a0.8适合城市车辆步行可降到0.3R9对应 3 米标准差如果使用 RTK 或高精度模块R 可以降到0.01~0.25。动态实验里P0的速度方差不要设为零否则滤波器初期会过度相信差分速度。4.2 用新息门限识别 GPS 跳点与多路径误差新息y z - Hx是观测与预测的差正常情况下应接近零均值协方差为S HPH^T R。把新息做马氏距离归一化d^2 y^T S^{-1} y二维位置观测下d^2近似卡方分布自由度 2。95% 门限约 5.991超过门限的点可能是跳点、多路径也可能是模型突然转弯。处理方式不是直接删而是降权或跳过更新让预测值暂时顶替。from scipy.stats import chi2 threshold chi2.ppf(0.95, df2) # 二维观测95% 门限约 5.991 accepted [] for i in range(1, len(df)): dt df[dt].iloc[i] if np.isnan(dt) or dt 0: dt 0.1 kf.F make_f_cv(dt) kf.Q make_q_cv(dt, sigma_a0.8) kf.predict() z np.array([df[east].iloc[i], df[north].iloc[i]]) x_hat, P, y, S kf.update(z) d2 float(y.T np.linalg.inv(S) y) if d2 threshold: # 标记为异常不回滚状态仅记录也可选择不执行 update accepted.append(False) else: accepted.append(True) dynamic_traj.append(x_hat.ravel())逻辑说明d2越大观测越不符合当前模型预测。参数说明threshold由卡方分布决定自由度等于观测维度如果 GPS 观测包含速度自由度变为 4门限约 9.488。阈值不能设得太小否则正常转弯会被误杀也不能太大否则跳点会污染状态。新息卡方检验也是 gps 生成式欺骗失效保护里常用的监测入口实验报告里可以把它作为异常检测小节。异常类型新息表现常见原因处理策略单点跳变d2瞬间超限多路径、遮挡恢复跳过更新或增大 R连续偏移d2持续偏大坐标偏移、欺骗干扰检查坐标系降权丢星无观测或d2缺失隧道、城市峡谷只预测不更新急转弯短时d2超限模型未建模增大 Q 或切换 CA4.3 动态轨迹可视化、速度估计与地图导出动态实验的可视化至少包含原始轨迹、滤波轨迹、异常点标记和速度曲线。速度可以由状态向量直接取vx, vy再合成sqrt(vx^2vy^2)。如果要把轨迹导出到地图先把 ENU 转回 WGS-84再按底图要求转换坐标系。gps 数据导出地图时GeoJSON 的坐标顺序是[lon, lat]顺序写反会导致轨迹跑到南极。import json import matplotlib.pyplot as plt def enu_to_wgs84(east, north, lat0, lon0): R 6378137.0 lat lat0 np.rad2deg(north / R) lon lon0 np.rad2deg(east / (R * np.cos(np.deg2rad(lat0)))) return lat, lon plt.figure(figsize(8, 6)) plt.plot(df[east], df[north], o, ms2, alpha0.4, labelraw GPS) plt.plot(dynamic_traj[:, 0], dynamic_traj[:, 1], -, lw2, labelKF) plt.axis(equal) plt.legend() plt.savefig(dynamic_track.png, dpi150) features [] for i, row in enumerate(dynamic_traj): lat, lon enu_to_wgs84(row[0], row[1], lat0, lon0) features.append({ type: Feature, geometry: {type: Point, coordinates: [lon, lat]}, properties: {idx: i} }) geojson {type: FeatureCollection, features: features} with open(filtered_track.geojson, w, encodingutf-8) as f: json.dump(geojson, f, ensure_asciiFalse)逻辑说明enu_to_wgs84是wgs84_to_enu的逆运算只适合小范围反向转换。参数说明plt.axis(equal)保证东西向和南北向比例一致否则轨迹形状会变形GeoJSON 坐标是[lon, lat]高德底图需要 GCJ-02导出前应完成 WGS-84 到 GCJ-02 的转换。速度曲线可以用np.hypot(dynamic_traj[:,2], dynamic_traj[:,3])得到再和 GPS 原始差分速度对比看滤波是否抑制了差分噪声。5. 实验报告之外的整定技巧Q/R 自适应、RTS 平滑与惯性导航融合5.1 R 自适应与 Q 分段整定固定 Q/R 在静动态混合数据里很难兼顾。工程上常用滑动窗口估计 R取最近 N 个新息计算R_hat mean(y y^T) - H P H^T再对 R 做上下限约束。Q 可以按运动状态分段静止段用位置随机游走小 Q运动段切 CV 大 Q转弯段进一步增大sigma_a。判断运动状态可以用滤波速度幅值超过0.5 m/s视为运动超过2 m/s提高 Q。这样静态精度不丢动态跟随也不至于滞后。场景Q 位置Q 速度R 位置判据静止0.01可关闭4~9速度小于0.2 m/s步行0.10.14~9速度0.5~1.5 m/s城市车辆0.50.5~1.09~25速度大于2 m/s高动态1.01.5~3.04~16加速度变化大5.2 RTS 平滑和卡尔曼滤波与惯性导航融合的接口如果实验数据是离线处理RTS 平滑能进一步降低轨迹抖动。做法是在普通卡尔曼滤波时保存每步的x_pred, P_pred, F, x_filt, P_filt然后从最后一帧向前递推。RTS 平滑后的位置和速度可以作为真值参考比较在线滤波的 CEP95 损失。卡尔曼滤波与惯性导航融合时状态向量加入 IMU 零偏和尺度因子GPS 更新作为观测IMU 预积分作为预测。动态实验里可以把 GPS 速度估计和 IMU 速度积分对比检查时间同步和杆臂误差。把 RTS 平滑后的速度作为 IMU 预积分因子初值再比较下一轮实验的 CEP95 变化。本文还有配套的精品资源点击获取 GPS静动态滤波卡尔曼滤波实验：Q/R整定与新息门限实践
简介这份文档是北京航空航天大学卡尔曼滤波课程的GPS静/动态滤波实验报告面向学习卡尔曼滤波与导航定位的高年级本科生和研究生。报告围绕GPS定位精度提升展开分别建立静态与动态卡尔曼滤波模型推导状态方程与离散化滤波方程并用程序对实测GPS数据完成滤波处理。内容涵盖滤波前后导航轨迹对比、估计均方差P阵对角线开根号的变化趋势以及动态模型与静态模型的区别、R阵Q阵与P0阵选取对滤波精度与收敛速度的影响、最小二乘与卡尔曼滤波的优劣对比等思考题分析静态部分给出Q阵取零、按克拉索夫斯基椭球模型设定R阵的做法动态部分采用当前统计模型与一阶马尔科夫位置误差建模。资源包为单个docx文档约862KB公式推导与结果图完整适合作为实验报告参考或建模调参思路的复习资料。目前已有108人学习下载。1. 从北航卡尔曼滤波实验报告看 GPS 静动态滤波要解决什么很多人做 GPS 静动态滤波实验第一反应是调 Q/R结果静态点平滑得像模像样动态轨迹却在转弯处滞后半条街。北航卡尔曼滤波实验报告这类任务真正要交代清楚的是GPS 观测在静态和动态下的误差特性完全不同卡尔曼滤波的状态模型、观测模型和噪声矩阵必须跟着场景换。静态实验通常拿固定点数据验证滤波能否压低随机抖动动态实验拿车载或手持轨迹验证滤波能否在噪声、丢星和跳点中估计位置与速度。它适合导航、测绘、无人车和机器人定位方向的入门者也适合已经会写 numpy 卡尔曼滤波、但想把实验报告写成可复现流程的人。核心不是背公式而是让每一组 Q/R 都有物理来源。2. GPS 静动态滤波实验里的卡尔曼滤波状态方程与观测方程卡尔曼滤波的数学思想可以压成两句话预测时用状态转移矩阵传播均值和协方差更新时用观测残差和新息协方差修正状态。GPS 静动态滤波实验的难点不在矩阵乘法而在于状态向量里放什么、观测矩阵 H 怎么对应 GPS 经纬度、Q 和 R 的量纲怎么统一。静态实验可以只估计位置动态实验通常要加入速度否则滤波轨迹会像被橡皮筋拽住。下面把模型选型、坐标转换和噪声整定拆开讲每一步都能直接落到代码。2.1 静态与动态实验的模型选型CV、CA 还是位置随机游走静态 GPS 数据的特点是真实位置基本不变但接收机解算出的经纬度会随机跳动。此时用位置随机游走模型最省事状态向量只取[x, y]^T状态转移矩阵FI过程噪声Q设得很小表示“位置几乎不会自己动”。动态实验如果还用这个模型滤波器会认为目标没有速度GPS 点一跳估计值就慢半拍。车载和步行场景常用常速度模型状态向量[x, y, vx, vy]^TF里出现dt能估计速度并预测下一时刻位置。无人机或急加速场景可以用常加速模型但加速度状态会放大噪声实验报告里如果没有高频 IMU 辅助不建议一上来就用 CA。模型状态向量状态转移 F适用场景主要风险位置随机游走[x, y]^TIGPS 静态固定点动态下滞后严重常速度 CV[x, y, vx, vy]^T位置加dt*v车载、步行、低速机器人急转弯速度估计不准常加速 CA[x, y, vx, vy, ax, ay]^T速度加dt*a高动态无人机噪声放大、Q 难整定选择逻辑很直接静态实验用位置随机游走动态实验优先 CV。CV 模型不是越复杂越好GPS 采样率通常 1Hz 到 10Hz低于 1Hz 时 CV 的dt很大预测误差也会变大。实验报告里最好把模型假设写清楚静态段目标静止动态段目标近似匀速转弯和加减速作为模型未建模误差进入 Q。2.2 GPS 经纬度到局部平面坐标的观测矩阵 H 怎么定GPS 模块输出的是 WGS-84 经纬度和海拔卡尔曼滤波通常在局部平面坐标里做因为经纬度一度对应的东向距离随纬度变化。常见做法是选第一个点或轨迹中心作为原点把经纬度转成 ENU 东向、北向坐标。小范围实验用等距圆柱近似就够公式简单误差在百米级范围内可以接受。转换后观测向量z[east, north]^T如果状态是[x, y]^T观测矩阵HI如果状态是[x, y, vx, vy]^T观测矩阵只取位置import numpy as np def wgs84_to_enu(lat, lon, lat0, lon0): # 以参考点为原点将 WGS-84 经纬度转局部 ENU 平面坐标 R 6378137.0 # WGS-84 地球长半轴单位 m lat_rad np.deg2rad(lat) lon_rad np.deg2rad(lon) lat0_rad np.deg2rad(lat0) lon0_rad np.deg2rad(lon0) east R * (lon_rad - lon0_rad) * np.cos(lat0_rad) north R * (lat_rad - lat0_rad) return east, north # 动态 CV 模型的观测矩阵只观测位置不直接观测速度 H_cv np.array([ [1, 0, 0, 0], [0, 1, 0, 0] ], dtypefloat)逻辑说明wgs84_to_enu先取参考点再用经度差乘cos(lat0)得到东向距离用纬度差得到北向距离适合校园、园区、城市街区级实验。H_cv把四维状态映射到二维位置观测速度状态靠状态转移矩阵间接估计。参数说明R是地球半径lat0/lon0是参考点必须全轨迹统一如果实验范围超过几公里建议改用 UTM 或 pyproj 做更严格的投影。静态实验里也可以直接对经纬度做滤波但 Q/R 的量纲会变成度平方不如转平面坐标直观。注意如果后续要把轨迹叠加到高德底图WGS-84 和 GCJ-02 不是同一套坐标。python 将 gps 经纬度转换为高德经纬度这一步不能省常见做法是调用坐标转换库或公开偏移公式先转 GCJ-02 再导出 GeoJSON。2.3 Q 和 R 的物理含义与参数表Q 是过程噪声协方差代表状态转移模型没描述到的运动比如静态点被风吹动、车辆加减速、转弯。R 是观测噪声协方差代表 GPS 解算误差包括多路径、电离层、接收机噪声。静态实验里 GPS 误差可能呈现明显自相关R 不是越小越好R 设得太小滤波器会过度相信跳点轨迹跟着 GPS 抖动。动态实验里 R 可以按时段调整卫星数多、HDOP 小时 R 小一点城市峡谷里 R 大一点。参数物理含义静态实验建议动态实验建议调大后果Q 位置未建模位置扰动0.01~0.1 m^20.1~1 m^2跟随快抖动变大Q 速度未建模加速度扰动静态可关闭0.1~1 (m/s)^2速度估计噪声大R 位置GPS 水平观测方差由静态数据方差估可按时段时变平滑强转弯滞后P0初始状态不确定度10~100 m^2位置10 m^2速度1 (m/s)^2初期收敛慢CV 模型的 Q 常用连续白噪声加速度模型离散化sigma_a表示目标加速度标准差。步行取0.2~0.5 m/s^2城市车辆取0.5~1.5 m/s^2。代码构建如下def make_q_cv(dt, sigma_a0.5): # 连续白噪声加速度模型离散化dt 单位秒 dt2 dt * dt dt3 dt2 * dt dt4 dt2 * dt2 q sigma_a ** 2 Q q * np.array([ [dt4 / 4, 0, dt3 / 2, 0], [0, dt4 / 4, 0, dt3 / 2], [dt3 / 2, 0, dt2, 0], [0, dt3 / 2, 0, dt2] ]) return Q逻辑说明sigma_a越大Q 越大滤波器越愿意相信 GPS 观测动态跟随更快但平滑变弱。参数说明dt必须来自实际时间戳差值不能默认 1 秒sigma_a是唯一需要按场景调的加速度尺度实验报告里可以用静态段 GPS 位置方差反推 R再用动态段速度变化估计sigma_a。3. 用 Python 跑通 GPS 静态滤波实验的最小流程静态滤波实验的目标是回答三个问题原始 GPS 数据抖动多大滤波后标准差降到多少滤波结果是否引入明显偏差。流程不复杂但数据清洗和时间对齐容易被跳过。很多实验报告的 RMSE 差异其实来自时间戳没排序、重复点没去重、或者把经纬度当成平面坐标直接滤波。下面用一份 CSV 格式的 GPS 数据为例从读取、转换到滤波和评估走一遍。3.1 读取 GPS 数据并做时间对齐与异常值初筛常见 GPS 数据字段包括timestamp/lat/lon/hdop/satellites也可能来自 NMEA 语句解析后的 CSV。第一步按时间排序计算相邻时间差检查是否有重复时间戳和过大间隔。HDOP 大于 5 或卫星数小于 4 的点可以先标记不一定直接删除但要留给后续新息门限处理。import pandas as pd import numpy as np # 读取 GPS 数据假设已解析为 CSV df pd.read_csv(gps_static.csv) df[timestamp] pd.to_datetime(df[timestamp]) df df.sort_values(timestamp).drop_duplicates(timestamp).reset_index(dropTrue) # 计算实际采样间隔 df[dt] df[timestamp].diff().dt.total_seconds() print(df[dt].describe()) # 以第一点为原点转 ENU lat0, lon0 df.loc[0, lat], df.loc[0, lon] east, north [], [] for _, row in df.iterrows(): e, n wgs84_to_enu(row[lat], row[lon], lat0, lon0) east.append(e) north.append(n) df[east], df[north] east, north # 初筛HDOP 过大或卫星数不足的点打标签 df[bad_dop] (df[hdop] 5.0) | (df[satellites] 4)逻辑说明时间排序和去重保证dt可信ENU 转换让后续 Q/R 有米制量纲。参数说明hdop 5和satellites 4只是初筛阈值实验报告里应记录筛选前后点数被标记的点不直接删除后续用新息门限决定是否降权。3.2 静态卡尔曼滤波代码从经纬度到平滑坐标静态实验用位置随机游走模型状态[x, y]^TFIHI。R 可以用原始 ENU 坐标的方差估计Q 取一个很小的值表示固定点位置几乎不变。下面实现一个通用 KF 类后续动态实验也能复用。class KalmanFilter: def __init__(self, F, H, Q, R, x0, P0): self.F F # 状态转移矩阵 self.H H # 观测矩阵 self.Q Q # 过程噪声协方差 self.R R # 观测噪声协方差 self.x x0.reshape(-1, 1) # 状态列向量 self.P P0 # 状态协方差 def predict(self): self.x self.F self.x self.P self.F self.P self.F.T self.Q return self.x.copy(), self.P.copy() def update(self, z): z z.reshape(-1, 1) y z - self.H self.x # 新息 S self.H self.P self.H.T self.R K self.P self.H.T np.linalg.inv(S) self.x self.x K y I np.eye(self.P.shape[0]) self.P (I - K self.H) self.P return self.x.copy(), self.P.copy(), y, S # 静态实验状态为 [east, north] x0 np.array([df[east].iloc[0], df[north].iloc[0]], dtypefloat) P0 np.eye(2) * 10.0 F np.eye(2) H np.eye(2) Q np.eye(2) * 0.01 R np.eye(2) * 4.0 # 假设 GPS 水平误差约 2 m方差 4 m^2 kf KalmanFilter(F, H, Q, R, x0, P0) static_traj [] for z in np.c_[df[east], df[north]]: kf.predict() x_hat, P, y, S kf.update(z) static_traj.append(x_hat.ravel()) static_traj np.array(static_traj)逻辑说明predict传播状态和协方差update用新息y和卡尔曼增益K修正。静态模型里FI预测步骤只增加 Q因此 Q 很小时估计值接近加权平均。参数说明R4对应 2 米标准差如果实测静态点标准差 3 米R 应设 9Q0.01表示固定点位置随机游走极弱Q 再大滤波会重新跟随 GPS 抖动。3.3 静态实验的 RMSE、CEP95 与置信区间验证静态实验没有绝对真值常用滤波后坐标均值作为参考中心计算原始坐标和滤波坐标相对中心的误差。RMSE 看整体离散程度CEP95 看 95% 点落入的圆半径新息均值看模型是否无偏。实验报告里最好把原始、滤波、均值中心画在同一张散点图上。指标含义静态实验关注动态实验关注RMSE均方根误差越小越平滑需有参考轨迹CEP9595% 点落入圆半径定位精度轨迹误差包络新息均值观测与预测差均值是否接近零模型偏差检测新息方差新息离散程度与 S 是否一致异常点检测依据center static_traj.mean(axis0) raw_err np.linalg.norm(np.c_[df[east], df[north]] - center, axis1) kf_err np.linalg.norm(static_traj - center, axis1) rmse_raw np.sqrt(np.mean(raw_err ** 2)) rmse_kf np.sqrt(np.mean(kf_err ** 2)) cep95_raw np.percentile(raw_err, 95) cep95_kf np.percentile(kf_err, 95) print(rmse_raw, rmse_kf, cep95_raw, cep95_kf)逻辑说明center是滤波后轨迹均值作为静态参考点。参数说明RMSE 和 CEP95 单位都是米比较时保持同一参考中心如果原始 GPS 存在系统性偏移均值中心也会偏实验报告里应说明参考点定义。滤波后 RMSE 通常下降但 CEP95 不一定同比例下降因为卡尔曼滤波对长尾跳点的抑制取决于 R 和门限。4. 动态 GPS 滤波实验CV 模型、丢星与跳点处理动态实验和静态实验最大的差别是目标真的会动状态模型必须包含速度观测噪声 R 也不能全程恒定。城市道路里 GPS 跳点、多路径和隧道丢星会同时出现单纯调大 R 会让转弯轨迹切内角调小 R 又会把跳点当真实运动。动态实验报告里如果只放一张滤波前后轨迹对比图信息量不够至少要给出速度估计、新息序列和异常点标记。4.1 动态卡尔曼滤波的 F 矩阵和 dt 处理CV 模型的状态是[x, y, vx, vy]^TF用实际dt构造。很多实验数据不是严格 1Hz如果代码里写死dt1车辆加速时速度估计会漂。每个时刻都应根据时间戳差值重新生成F和QH保持只观测位置。def make_f_cv(dt): return np.array([ [1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1] ], dtypefloat) # 动态初始化位置取第一个点速度取前两个点差分 dt0 df[dt].iloc[1] if not np.isnan(df[dt].iloc[1]) else 1.0 vx0 (df[east].iloc[1] - df[east].iloc[0]) / dt0 vy0 (df[north].iloc[1] - df[north].iloc[0]) / dt0 x0 np.array([df[east].iloc[0], df[north].iloc[0], vx0, vy0], dtypefloat) P0 np.diag([10.0, 10.0, 1.0, 1.0]) H np.array([[1, 0, 0, 0], [0, 1, 0, 0]], dtypefloat) R np.eye(2) * 9.0 # 动态 GPS 误差可能比静态大 dynamic_traj [] for i in range(1, len(df)): dt df[dt].iloc[i] if np.isnan(dt) or dt 0: dt 0.1 kf.F make_f_cv(dt) kf.Q make_q_cv(dt, sigma_a0.8) kf.predict() z np.array([df[east].iloc[i], df[north].iloc[i]]) x_hat, P, y, S kf.update(z) dynamic_traj.append(x_hat.ravel()) dynamic_traj np.array(dynamic_traj)逻辑说明每个历元重新构造F和Q保证不同采样间隔下速度传播正确。参数说明sigma_a0.8适合城市车辆步行可降到0.3R9对应 3 米标准差如果使用 RTK 或高精度模块R 可以降到0.01~0.25。动态实验里P0的速度方差不要设为零否则滤波器初期会过度相信差分速度。4.2 用新息门限识别 GPS 跳点与多路径误差新息y z - Hx是观测与预测的差正常情况下应接近零均值协方差为S HPH^T R。把新息做马氏距离归一化d^2 y^T S^{-1} y二维位置观测下d^2近似卡方分布自由度 2。95% 门限约 5.991超过门限的点可能是跳点、多路径也可能是模型突然转弯。处理方式不是直接删而是降权或跳过更新让预测值暂时顶替。from scipy.stats import chi2 threshold chi2.ppf(0.95, df2) # 二维观测95% 门限约 5.991 accepted [] for i in range(1, len(df)): dt df[dt].iloc[i] if np.isnan(dt) or dt 0: dt 0.1 kf.F make_f_cv(dt) kf.Q make_q_cv(dt, sigma_a0.8) kf.predict() z np.array([df[east].iloc[i], df[north].iloc[i]]) x_hat, P, y, S kf.update(z) d2 float(y.T np.linalg.inv(S) y) if d2 threshold: # 标记为异常不回滚状态仅记录也可选择不执行 update accepted.append(False) else: accepted.append(True) dynamic_traj.append(x_hat.ravel())逻辑说明d2越大观测越不符合当前模型预测。参数说明threshold由卡方分布决定自由度等于观测维度如果 GPS 观测包含速度自由度变为 4门限约 9.488。阈值不能设得太小否则正常转弯会被误杀也不能太大否则跳点会污染状态。新息卡方检验也是 gps 生成式欺骗失效保护里常用的监测入口实验报告里可以把它作为异常检测小节。异常类型新息表现常见原因处理策略单点跳变d2瞬间超限多路径、遮挡恢复跳过更新或增大 R连续偏移d2持续偏大坐标偏移、欺骗干扰检查坐标系降权丢星无观测或d2缺失隧道、城市峡谷只预测不更新急转弯短时d2超限模型未建模增大 Q 或切换 CA4.3 动态轨迹可视化、速度估计与地图导出动态实验的可视化至少包含原始轨迹、滤波轨迹、异常点标记和速度曲线。速度可以由状态向量直接取vx, vy再合成sqrt(vx^2vy^2)。如果要把轨迹导出到地图先把 ENU 转回 WGS-84再按底图要求转换坐标系。gps 数据导出地图时GeoJSON 的坐标顺序是[lon, lat]顺序写反会导致轨迹跑到南极。import json import matplotlib.pyplot as plt def enu_to_wgs84(east, north, lat0, lon0): R 6378137.0 lat lat0 np.rad2deg(north / R) lon lon0 np.rad2deg(east / (R * np.cos(np.deg2rad(lat0)))) return lat, lon plt.figure(figsize(8, 6)) plt.plot(df[east], df[north], o, ms2, alpha0.4, labelraw GPS) plt.plot(dynamic_traj[:, 0], dynamic_traj[:, 1], -, lw2, labelKF) plt.axis(equal) plt.legend() plt.savefig(dynamic_track.png, dpi150) features [] for i, row in enumerate(dynamic_traj): lat, lon enu_to_wgs84(row[0], row[1], lat0, lon0) features.append({ type: Feature, geometry: {type: Point, coordinates: [lon, lat]}, properties: {idx: i} }) geojson {type: FeatureCollection, features: features} with open(filtered_track.geojson, w, encodingutf-8) as f: json.dump(geojson, f, ensure_asciiFalse)逻辑说明enu_to_wgs84是wgs84_to_enu的逆运算只适合小范围反向转换。参数说明plt.axis(equal)保证东西向和南北向比例一致否则轨迹形状会变形GeoJSON 坐标是[lon, lat]高德底图需要 GCJ-02导出前应完成 WGS-84 到 GCJ-02 的转换。速度曲线可以用np.hypot(dynamic_traj[:,2], dynamic_traj[:,3])得到再和 GPS 原始差分速度对比看滤波是否抑制了差分噪声。5. 实验报告之外的整定技巧Q/R 自适应、RTS 平滑与惯性导航融合5.1 R 自适应与 Q 分段整定固定 Q/R 在静动态混合数据里很难兼顾。工程上常用滑动窗口估计 R取最近 N 个新息计算R_hat mean(y y^T) - H P H^T再对 R 做上下限约束。Q 可以按运动状态分段静止段用位置随机游走小 Q运动段切 CV 大 Q转弯段进一步增大sigma_a。判断运动状态可以用滤波速度幅值超过0.5 m/s视为运动超过2 m/s提高 Q。这样静态精度不丢动态跟随也不至于滞后。场景Q 位置Q 速度R 位置判据静止0.01可关闭4~9速度小于0.2 m/s步行0.10.14~9速度0.5~1.5 m/s城市车辆0.50.5~1.09~25速度大于2 m/s高动态1.01.5~3.04~16加速度变化大5.2 RTS 平滑和卡尔曼滤波与惯性导航融合的接口如果实验数据是离线处理RTS 平滑能进一步降低轨迹抖动。做法是在普通卡尔曼滤波时保存每步的x_pred, P_pred, F, x_filt, P_filt然后从最后一帧向前递推。RTS 平滑后的位置和速度可以作为真值参考比较在线滤波的 CEP95 损失。卡尔曼滤波与惯性导航融合时状态向量加入 IMU 零偏和尺度因子GPS 更新作为观测IMU 预积分作为预测。动态实验里可以把 GPS 速度估计和 IMU 速度积分对比检查时间同步和杆臂误差。把 RTS 平滑后的速度作为 IMU 预积分因子初值再比较下一轮实验的 CEP95 变化。本文还有配套的精品资源点击获取