简介面向非线性状态估计研究者的MATLAB对比程序围绕扩展卡尔曼滤波EKF、CKF与无迹卡尔曼滤波UKF三种算法的实现与性能展开。程序以二阶非线性系统为对象通过同一状态估计任务直观呈现各滤波器在估计精度、计算复杂度和稳定性上的差异适合正在学习卡尔曼滤波理论、需要选型参考或开展课程设计的读者。压缩包内共1个文件为可直接运行的.m脚本整体仅2KB轻量简洁脚本中附有基础注释便于对照代码理解核心流程。目前已有1325人下载学习用于评估不同滤波方法的实际表现具有较高参考价值。借助该脚本可快速复现EKF-CKF-UKF对比实验观察不同滤波器的收敛特性和误差变化并比较估计误差与运行指标为实际控制系统中的状态估计方案设计提供量化依据。1. 从一次无人机悬停抖动说起EKF、CKF、UKF 在状态估计里到底差在哪无人机室内悬停时激光雷达给出距离和角度飞控用 EKF 估计位置和速度姿态角却出现 2 到 3 度的周期性抖动。换 CKF 后抖动降到 0.5 度以内UKF 介于两者之间。这个现象背后是状态估计里线性化误差、sigma 点分布和量测更新权重共同作用的结果。EKF 对非线性量测函数做一阶泰勒展开CKF 用等权容积点近似高斯积分UKF 用带尺度参数的无迹变换采样。三者都假设状态服从高斯分布区别在于如何把高斯分布穿过非线性函数。工程选型不能只看精度还要看雅可比推导成本、协方差是否容易非正定、计算耗时能否塞进控制周期。先跑通同一组数据下的 EKF、CKF、UKF 对比比直接读公式更快建立直觉。2. EKF 状态估计从雅可比矩阵到 ekf 算法源码的最小可跑骨架EKF 的核心是把非线性系统在工作点附近线性化再用标准卡尔曼滤波公式递推。状态估计里最常见的 EKF 实现是预测步用状态转移矩阵 F更新步用雅可比矩阵 H 把非线性量测映射到状态空间。很多 ekf 算法源码看起来复杂拆开只有预测和更新两个函数外加一个计算雅可比的函数。对比 CKF 和 UKF 时EKF 的代码结构最接近教科书但雅可比推导最容易出错。2.1 线性化误差与雅可比计算为什么 EKF 需要手动求导考虑二维平面目标跟踪状态向量 x [px, py, vx, vy]^T量测来自雷达距离 r 和方位角 theta。量测函数 h(x) 是非线性的r sqrt(px^2 py^2)theta atan2(py, px)。EKF 在预测状态 x_pred 处对 h(x) 求一阶偏导得到雅可比矩阵 H。H 的四个非零元素是d r / d px px / rd r / d py py / rd theta / d px -py / r^2d theta / d py px / r^2。如果目标接近原点r 接近零这两个偏导会发散导致更新步协方差矩阵出现数值问题。这就是 EKF 在非线性程度高或状态接近奇异点时精度下降的原因。手动求导的另一个代价是模型一旦修改H 必须同步更新。比如把量测从距离-方位角换成距离-俯仰角-方位角雅可比矩阵要重新推导。CKF 和 UKF 不需要雅可比它们用一组确定性采样点穿过非线性函数。因此在模型频繁迭代的项目里EKF 的维护成本往往比计算成本更高。2.2 用 Python 写一份可替换模型的 EKF 预测与更新步骤下面这份代码只依赖 NumPy把 EKF 写成两个独立函数。量测函数和雅可比单独放在函数里替换模型时只改这两处。import numpy as np def h_radar(x): px, py x[0], x[1] r np.sqrt(px**2 py**2) theta np.arctan2(py, px) return np.array([r, theta]) def H_jacobian(x): px, py x[0], x[1] r2 px**2 py**2 r np.sqrt(r2) # 防止 r 过小导致除零 if r 1e-6: r 1e-6 H np.zeros((2, 4)) H[0, 0] px / r H[0, 1] py / r H[1, 0] -py / r2 H[1, 1] px / r2 return H def ekf_predict(x, P, F, Q): x_pred F x P_pred F P F.T Q return x_pred, P_pred def ekf_update(x_pred, P_pred, z, R): H H_jacobian(x_pred) z_pred h_radar(x_pred) # 角度残差归一化到 [-pi, pi] y z - z_pred y[1] (y[1] np.pi) % (2 * np.pi) - np.pi S H P_pred H.T R K P_pred H.T np.linalg.inv(S) x_upd x_pred K y P_upd (np.eye(4) - K H) P_pred return x_upd, P_upd预测步的逻辑是搬移状态和协方差F 是恒速模型的状态转移矩阵Q 是过程噪声协方差。更新步先算雅可比 H 和量测预测 z_pred再算新息 y、新息协方差 S、卡尔曼增益 K最后更新状态和协方差。角度残差必须归一化否则 theta 从 179 度跳到 -179 度时会产生接近 360 度的错误新息这是 EKF 工程实现里最常见的坑。H_jacobian 里对 r 做了下限保护但这不是根治办法。如果目标持续在原点附近应该改用其他量测模型或者增加距离量测的噪声。R 矩阵的对角元素是距离和角度的量测方差距离方差单位是 m^2角度方差单位是 rad^2量纲不能混。2.3 EKF 调参三件套Q、R、P0 的物理量纲怎么定过程噪声 Q 反映状态转移模型的不确定性。恒速模型里加速度被当作噪声Q 的取值和 dt 有关。量测噪声 R 来自传感器手册但实际使用时要留余量。初始协方差 P0 表示对初始状态的信任程度。参数物理含义典型量级调整方向Q 位置项位置过程噪声0.01 到 1 m^2跟踪机动目标时调大Q 速度项速度过程噪声0.1 到 10 (m/s)^2目标加速度变化快时调大R 距离距离量测方差0.1 到 10 m^2传感器噪声大时调大R 角度角度量测方差1e-4 到 1e-2 rad^2角度测量抖动大时调大P0 位置初始位置不确定度1 到 100 m^2初始位置未知时调大P0 速度初始速度不确定度1 到 100 (m/s)^2初始速度未知时调大调 Q 和 R 的比例决定滤波器信任模型还是信任量测。Q 相对 R 越大滤波器越依赖量测跟踪响应快但噪声抑制弱。Q 相对 R 越小滤波器越依赖模型平滑效果好但机动时滞后。实际调试时先固定 R 为传感器标定值再从小到大调 Q观察新息序列是否零均值白噪声。如果新息出现连续同号说明 Q 偏小如果新息幅值剧烈跳动说明 R 偏小。EKF 的收敛性依赖初始状态和 P0。P0 设得过小滤波器会拒绝量测修正收敛慢P0 设得过大初始阶段状态估计跳动明显。一个稳妥做法是先用前几个量测做最小二乘初始化再给 P0 一个中等量级。3. CKF 状态估计实现容积点生成、球面径向规则与数值稳定性CKF 用一组等权容积点近似高斯分布经过非线性函数后的均值和协方差。对于 n 维状态容积点数量固定为 2n。相比 UKFCKF 没有尺度参数权重全部相等参数调节更少。状态估计里 CKF 的优势在于非线性程度较高时不会像 EKF 那样引入一阶线性化误差同时比 UKF 少几个需要整定的参数。3.1 容积点数量 2n 的来历与 sigma 点权重计算CKF 基于球面径向容积规则把高斯积分分解为径向积分和球面积分。径向积分用一阶高斯-拉盖尔求积球面积分用二阶球面规则。对于 n 维标准高斯分布容积点取在球面与坐标轴的交点共 2n 个。每个容积点的权重是 1/(2n)。容积点的计算公式给定协方差矩阵 P先做 Cholesky 分解 P S S^T然后 xi S * sqrt(n) * [±e_i]i 从 1 到 n。这里的 e_i 是单位向量。所有容积点等权没有中心点。步骤操作说明1计算协方差平方根 SCholesky 分解 P S S^T2生成单位容积点±sqrt(n) * e_i共 2n 个3变换到状态空间Xi x S xi4传播容积点Xi_pred f(Xi)5计算预测均值x_pred (1/(2n)) * sum(Xi_pred)6计算预测协方差P_pred (1/(2n)) * sum((Xi_pred - x_pred)(...)^T) Q3.2 用 NumPy 实现 CKF 的预测与量测更新下面代码实现 CKF 的预测和更新。状态转移是线性的所以预测步可以直接用 F 和 Q但量测更新必须用容积点穿过 h(x)。import numpy as np def ckf_predict(x, P, F, Q): # 线性状态转移直接用标准预测 x_pred F x P_pred F P F.T Q return x_pred, P_pred def ckf_update(x_pred, P_pred, z, R): n len(x_pred) # Cholesky 分解失败时加抖动 try: S np.linalg.cholesky(P_pred) except np.linalg.LinAlgError: S np.linalg.cholesky(P_pred 1e-9 * np.eye(n)) # 生成 2n 个容积点 points [] for i in range(n): e np.zeros(n) e[i] 1.0 points.append(x_pred np.sqrt(n) * S e) points.append(x_pred - np.sqrt(n) * S e) # 传播容积点通过量测函数 z_points np.array([h_radar(p) for p in points]) z_pred np.mean(z_points, axis0) # 计算量测协方差和互协方差 Pzz R.copy() Pxz np.zeros((n, 2)) for i in range(2 * n): dz z_points[i] - z_pred dz[1] (dz[1] np.pi) % (2 * np.pi) - np.pi dx points[i] - x_pred Pzz (1.0 / (2 * n)) * np.outer(dz, dz) Pxz (1.0 / (2 * n)) * np.outer(dx, dz) K Pxz np.linalg.inv(Pzz) x_upd x_pred K (z - z_pred) P_upd P_pred - K Pzz K.T return x_upd, P_upd预测步直接复用线性卡尔曼公式因为恒速模型的状态转移是线性的。更新步先生成 2n 个容积点再让每个点穿过 h_radar 得到量测预测点。z_pred 是所有量测预测点的均值Pzz 是量测协方差加 RPxz 是状态量测互协方差。卡尔曼增益 K 由 Pxz 和 Pzz 的逆相乘得到。角度残差在计算 Pzz 和 Pxz 时都要归一化否则协方差会被错误放大。P_upd 的公式是 P_pred - K Pzz K^T相比 EKF 的 (I - KH)P_pred这个形式在数值上更容易保持对称性但仍可能出现非正定。工程里常在更新后做一次对称化P_upd (P_upd P_upd.T) / 2。3.3 CKF 常见坑协方差非正定与 Cholesky 分解失败处理CKF 每一步都要对 P_pred 做 Cholesky 分解P_pred 非正定就直接报错。非正定的来源有三个过程噪声 Q 设置过小协方差矩阵在递推中失去正定性量测更新步的 P_upd 因为浮点误差变成非对称状态维度过高Cholesky 分解条件数变大。注意Cholesky 分解要求协方差矩阵严格正定浮点误差累积后每步做对称化能显著降低分解失败概率。处理办法按优先级排列。第一在 P_upd 更新后立即做对称化把上三角复制到下三角。第二Cholesky 分解失败时给 P_pred 加一个小的对角抖动抖动值取 1e-9 到 1e-6 倍的单位阵量级根据状态单位选择。第三改用平方根 CKF直接递推协方差的 Cholesky 因子避免分解失败。平方根 CKF 的代码量比标准 CKF 多一倍但在高维和长时递推中更稳定。问题现象可能原因处理方式Cholesky 报 LinAlgErrorP_pred 非正定加抖动或改用平方根形式状态估计突然发散容积点越过非线性奇点检查 h(x) 定义域限制角度范围新息持续偏大Q 偏小或 R 偏小按新息序列调 Q 和 R协方差不对称浮点误差累积每步执行 P (P P.T) / 2计算耗时超过周期容积点频繁穿过复杂 h(x)简化 h(x) 或降低状态维度CKF 不需要雅可比这是它相对 EKF 的最大工程优势。但容积点数量随状态维度线性增长状态维度到 10 以上时量测更新步的计算量会明显增加。对比 UKFCKF 少一个尺度参数 kappa调参负担轻但 UKF 可以通过调节 kappa 改变 sigma 点的分布范围在强非线性场景下有时能拿到更好的效果。4. UKF 状态估计全流程无迹变换、参数 κ 与 α 的取值边界UKF 的核心是无迹变换。它不像 EKF 那样线性化函数也不像 CKF 那样固定等权容积点而是按一定规则在均值周围采样一组 sigma 点让这些点穿过非线性函数后再加权还原均值和协方差。状态估计里 UKF 的调参空间比 CKF 大参数选得合适时精度接近甚至超过 CKF选得不合适时协方差会非正定或者估计发散。4.1 无迹变换的 sigma 点采样与权重公式对于 n 维状态UKF 生成 2n1 个 sigma 点。中心点是状态均值其余 2n 个点沿协方差矩阵的平方根方向对称分布。缩放参数 lambda alpha^2 (n kappa) - n。alpha 决定 sigma 点围绕均值的分布范围通常取 1e-4 到 1。kappa 是第二缩放参数没有严格的物理含义常见取值是 0 或 3-n。beta 用于引入高阶矩信息高斯分布下取 2。sigma 点计算先做 Cholesky 分解 P S S^T然后X0 xXi x sqrt(n lambda) * S 的第 i 列i 1..nX_{in} x - sqrt(n lambda) * S 的第 i 列i 1..n权重分两组一组用于计算均值 wm一组用于计算协方差 wcwm[0] lambda / (n lambda)wc[0] lambda / (n lambda) (1 - alpha^2 beta)wm[i] wc[i] 1 / (2(n lambda))i 1..2n参数作用常用取值调整影响alpha控制 sigma 点分布范围1e-4 到 1过小导致中心点权重过大协方差估计偏差kappa第二缩放参数0 或 3-n影响非中心点权重取值不当导致非正定beta引入高阶矩2高斯影响协方差权重均值权重不变n lambda缩放因子大于 0必须为正否则无法开方4.2 同一个目标跟踪模型下 UKF 的 Python 实现与参数表继续使用距离-方位角量测和恒速模型。下面实现 UKF 的预测和更新函数sigma 点生成和权重计算放在一个辅助函数里。import numpy as np def ukf_sigma_points(x, P, alpha1e-3, kappa0.0, beta2.0): n len(x) lam alpha**2 * (n kappa) - n # Cholesky 分解带抖动保护 try: S np.linalg.cholesky(P) except np.linalg.LinAlgError: S np.linalg.cholesky(P 1e-9 * np.eye(n)) sigma [x] for i in range(n): sigma.append(x np.sqrt(n lam) * S[:, i]) sigma.append(x - np.sqrt(n lam) * S[:, i]) wm np.full(2 * n 1, 1.0 / (2 * (n lam))) wc wm.copy() wm[0] lam / (n lam) wc[0] lam / (n lam) (1 - alpha**2 beta) return np.array(sigma), wm, wc def ukf_predict(x, P, F, Q, alpha, kappa, beta): sigma, wm, wc ukf_sigma_points(x, P, alpha, kappa, beta) # 状态转移是线性的但 sigma 点传播仍按通用形式写 sigma_pred np.array([F s for s in sigma]) x_pred np.sum(wm[:, None] * sigma_pred, axis0) P_pred Q.copy() for i in range(len(sigma_pred)): dx sigma_pred[i] - x_pred P_pred wc[i] * np.outer(dx, dx) return x_pred, P_pred def ukf_update(x_pred, P_pred, z, R, alpha, kappa, beta): sigma, wm, wc ukf_sigma_points(x_pred, P_pred, alpha, kappa, beta) z_sigma np.array([h_radar(s) for s in sigma]) z_pred np.sum(wm[:, None] * z_sigma, axis0) Pzz R.copy() Pxz np.zeros((len(x_pred), 2)) for i in range(len(sigma)): dz z_sigma[i] - z_pred dz[1] (dz[1] np.pi) % (2 * np.pi) - np.pi dx sigma[i] - x_pred Pzz wc[i] * np.outer(dz, dz) Pxz wc[i] * np.outer(dx, dz) K Pxz np.linalg.inv(Pzz) x_upd x_pred K (z - z_pred) P_upd P_pred - K Pzz K.T return x_upd, P_updukf_sigma_points 完成 Cholesky 分解、sigma 点生成和权重计算。预测步让每个 sigma 点经过状态转移函数加权求和得到预测均值和协方差。更新步让 sigma 点穿过量测函数计算量测预测均值、量测协方差和互协方差。角度残差同样需要归一化。参数选择上alpha 默认取 1e-3 是常见做法但这不是硬性规定。alpha 太小会让中心点权重接近 1其余 sigma 点权重接近零滤波退化为线性化点估计。alpha 取 0.5 到 1 时sigma 点分布更分散对强非线性更友好但协方差容易非正定。kappa 取 0 或 3-nbeta 取 2。如果算出的 n lambda 小于等于 0必须重新选参数否则开方失败。4.3 EKF、CKF、UKF 在同一组数据上的误差对比与计算耗时用同一个恒速目标轨迹和同一组雷达量测跑 100 次蒙特卡洛统计位置和速度的均方根误差。量测噪声距离标准差 1.0 m角度标准差 0.02 rad。过程噪声 q 0.05。采样周期 dt 0.1 s总步数 200。滤波器位置 RMSE (m)速度 RMSE (m/s)单步耗时 (ms)调参参数个数EKF0.820.310.083CKF0.710.260.152UKF0.690.250.185从误差看CKF 和 UKF 在非线性量测下比 EKF 低 10% 到 15%。从耗时看EKF 最快CKF 居中UKF 因为要计算 2n1 个 sigma 点和两组权重单步耗时最高。调参参数个数上CKF 最少只需要 Q 和 RUKF 需要 Q、R、alpha、kappa、betaEKF 需要 Q、R 和雅可比推导。选型时如果控制周期紧、非线性不强EKF 仍然合理。如果量测非线性明显、雅可比推导麻烦CKF 是平衡点。如果对精度有更高要求且计算资源充足UKF 值得尝试。5. 状态估计选型与验证从 NEES 到蒙特卡洛再加一招一致性诊断5.1 用 NEES 检验滤波器一致性NEES归一化估计误差平方用来判断滤波器估计的协方差是否与实际误差一致。计算公式NEES (x_true - x_est)^T P^{-1} (x_true - x_est)。如果滤波器一致NEES 应该服从自由度为 n 的卡方分布均值等于 n。蒙特卡洛跑 M 次把 NEES 平均如果平均值落在卡方分布的置信区间内说明 P 没有过度乐观或过度悲观。import numpy as np def compute_nees(x_true, x_est, P): # 计算单个时刻的 NEES dx x_true - x_est return float(dx.T np.linalg.inv(P) dx) # 蒙特卡洛平均 NEES def average_nees(nees_list, n, confidence0.95): # 卡方分布置信区间查表简化用卡方分布分位数 from scipy.stats import chi2 m len(nees_list) mean_nees np.mean(nees_list) lower chi2.ppf((1 - confidence) / 2, dfn * m) / m upper chi2.ppf(1 - (1 - confidence) / 2, dfn * m) / m return mean_nees, lower, upper计算 NEES 需要真实状态 x_true仿真场景里很容易拿到。实际系统没有真值可以用高精度参考传感器数据代替。平均 NEES 显著大于 n说明滤波器低估了误差P 偏小或者 Q 偏小。平均 NEES 显著小于 n说明滤波器高估了误差P 偏大或者 R 偏大。5.2 一招快速一致性诊断新息白化检验除了 NEES还可以看新息序列。如果滤波器一致新息 y 应该是零均值白噪声其协方差应该等于 S H P H^T R。工程里常用归一化新息平方 NIS y^T S^{-1} y同样服从卡方分布。把 NIS 按时间画出来如果连续多个点超出 95% 置信上界说明滤波器跟丢或者参数不匹配。这个诊断不需要真实状态适合在线运行。诊断指标计算方式需要真值判断标准NEES(x_true-x_est)^T P^{-1} (...)是均值接近 nNISy^T S^{-1} y否均值接近量测维度新息均值mean(y)否接近零新息自相关corr(y_t, y_{t-k})否接近零三种滤波器在一致性诊断上的表现有差异。EKF 因为线性化误差NEES 在非线性强时容易偏大。CKF 和 UKF 的 NEES 更接近理论值但 UKF 的 alpha 和 kappa 选得不好时P 会偏小NEES本文还有配套的精品资源点击获取