简介：这份文档是北京航空航天大学卡尔曼滤波课程的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，状态转移矩阵F=I，过程噪声Q设得很小，表示“位置几乎不会自己动”。动态实验如果还用这个模型，滤波器会认为目标没有速度，GPS 点一跳，估计值就慢半拍。车载和步行场景常用常速度模型，状态向量[x, y, vx, vy]^T，F里出现dt，能估计速度并预测下一时刻位置。无人机或急加速场景可以用常加速模型，但加速度状态会放大噪声，实验报告里如果没有高频 IMU 辅助，不建议一上来就用 CA。
| 模型 | 状态向量 | 状态转移 F | 适用场景 | 主要风险 |
|---|---|---|---|---|
| 位置随机游走 | [x, y]^T | I | GPS 静态固定点 | 动态下滞后严重 |
| 常速度 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，观测矩阵H=I；如果状态是[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] ], dtype=float)逻辑说明：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^2 | 0.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_a=0.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(drop=True) # 计算实际采样间隔 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]^T，F=I，H=I。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]], dtype=float) 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修正。静态模型里F=I，预测步骤只增加 Q，因此 Q 很小时估计值接近加权平均。参数说明：R=4对应 2 米标准差，如果实测静态点标准差 3 米，R 应设 9；Q=0.01表示固定点位置随机游走极弱，Q 再大滤波会重新跟随 GPS 抖动。
3.3 静态实验的 RMSE、CEP95 与置信区间验证
静态实验没有绝对真值，常用滤波后坐标均值作为参考中心，计算原始坐标和滤波坐标相对中心的误差。RMSE 看整体离散程度，CEP95 看 95% 点落入的圆半径，新息均值看模型是否无偏。实验报告里最好把原始、滤波、均值中心画在同一张散点图上。
| 指标 | 含义 | 静态实验关注 | 动态实验关注 |
|---|---|---|---|
| RMSE | 均方根误差 | 越小越平滑 | 需有参考轨迹 |
| CEP95 | 95% 点落入圆半径 | 定位精度 | 轨迹误差包络 |
| 新息均值 | 观测与预测差均值 | 是否接近零 | 模型偏差检测 |
| 新息方差 | 新息离散程度 | 与 S 是否一致 | 异常点检测依据 |
center = static_traj.mean(axis=0) raw_err = np.linalg.norm(np.c_[df["east"], df["north"]] - center, axis=1) kf_err = np.linalg.norm(static_traj - center, axis=1) 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]^T，F用实际dt构造。很多实验数据不是严格 1Hz，如果代码里写死dt=1，车辆加速时速度估计会漂。每个时刻都应根据时间戳差值重新生成F和Q，H保持只观测位置。
def make_f_cv(dt): return np.array([ [1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1] ], dtype=float) # 动态初始化：位置取第一个点，速度取前两个点差分 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], dtype=float) P0 = np.diag([10.0, 10.0, 1.0, 1.0]) H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], dtype=float) 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_a=0.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_a=0.8适合城市车辆，步行可降到0.3；R=9对应 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, df=2) # 二维观测，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_a=0.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 或切换 CA |
4.3 动态轨迹可视化、速度估计与地图导出
动态实验的可视化至少包含原始轨迹、滤波轨迹、异常点标记和速度曲线。速度可以由状态向量直接取vx, vy，再合成sqrt(vx^2+vy^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", ms=2, alpha=0.4, label="raw GPS") plt.plot(dynamic_traj[:, 0], dynamic_traj[:, 1], "-", lw=2, label="KF") plt.axis("equal") plt.legend() plt.savefig("dynamic_track.png", dpi=150) 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", encoding="utf-8") as f: json.dump(geojson, f, ensure_ascii=False)逻辑说明：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.1 | 0.1 | 4~9 | 速度0.5~1.5 m/s |
| 城市车辆 | 0.5 | 0.5~1.0 | 9~25 | 速度大于2 m/s |
| 高动态 | 1.0 | 1.5~3.0 | 4~16 | 加速度变化大 |
5.2 RTS 平滑和卡尔曼滤波与惯性导航融合的接口
如果实验数据是离线处理，RTS 平滑能进一步降低轨迹抖动。做法是在普通卡尔曼滤波时保存每步的x_pred, P_pred, F, x_filt, P_filt，然后从最后一帧向前递推。RTS 平滑后的位置和速度可以作为真值参考，比较在线滤波的 CEP95 损失。卡尔曼滤波与惯性导航融合时，状态向量加入 IMU 零偏和尺度因子，GPS 更新作为观测，IMU 预积分作为预测。动态实验里可以把 GPS 速度估计和 IMU 速度积分对比，检查时间同步和杆臂误差。把 RTS 平滑后的速度作为 IMU 预积分因子初值，再比较下一轮实验的 CEP95 变化。
本文还有配套的精品资源，点击获取
