企业官网建设流程全解析

简介：面向非线性状态估计研究者的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 / r，d r / d py = py / r，d theta / d px = -py / r^2，d 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 和 R 的比例决定滤波器信任模型还是信任量测。Q 相对 R 越大，滤波器越依赖量测，跟踪响应快但噪声抑制弱。Q 相对 R 越小，滤波器越依赖模型，平滑效果好但机动时滞后。实际调试时先固定 R 为传感器标定值，再从小到大调 Q，观察新息序列是否零均值白噪声。如果新息出现连续同号，说明 Q 偏小；如果新息幅值剧烈跳动，说明 R 偏小。

EKF 的收敛性依赖初始状态和 P0。P0 设得过小，滤波器会拒绝量测修正，收敛慢；P0 设得过大，初始阶段状态估计跳动明显。一个稳妥做法是先用前几个量测做最小二乘初始化，再给 P0 一个中等量级。

3. CKF 状态估计实现：容积点生成、球面径向规则与数值稳定性

CKF 用一组等权容积点近似高斯分布经过非线性函数后的均值和协方差。对于 n 维状态，容积点数量固定为 2n。相比 UKF，CKF 没有尺度参数，权重全部相等，参数调节更少。状态估计里 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 是单位向量。所有容积点等权，没有中心点。

3.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, axis=0) # 计算量测协方差和互协方差 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 是量测协方差加 R，Pxz 是状态量测互协方差。卡尔曼增益 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 多一倍，但在高维和长时递推中更稳定。

CKF 不需要雅可比，这是它相对 EKF 的最大工程优势。但容积点数量随状态维度线性增长，状态维度到 10 以上时，量测更新步的计算量会明显增加。对比 UKF，CKF 少一个尺度参数 kappa，调参负担轻，但 UKF 可以通过调节 kappa 改变 sigma 点的分布范围，在强非线性场景下有时能拿到更好的效果。

4. UKF 状态估计全流程：无迹变换、参数 κ 与 α 的取值边界

UKF 的核心是无迹变换。它不像 EKF 那样线性化函数，也不像 CKF 那样固定等权容积点，而是按一定规则在均值周围采样一组 sigma 点，让这些点穿过非线性函数后再加权还原均值和协方差。状态估计里 UKF 的调参空间比 CKF 大，参数选得合适时精度接近甚至超过 CKF，选得不合适时协方差会非正定或者估计发散。

4.1 无迹变换的 sigma 点采样与权重公式

对于 n 维状态，UKF 生成 2n+1 个 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 = x
• Xi = x + sqrt(n + lambda) * S 的第 i 列，i = 1..n
• X_{i+n} = x - sqrt(n + lambda) * S 的第 i 列，i = 1..n

权重分两组，一组用于计算均值 wm，一组用于计算协方差 wc：

• wm[0] = lambda / (n + lambda)
• wc[0] = lambda / (n + lambda) + (1 - alpha^2 + beta)
• wm[i] = wc[i] = 1 / (2(n + lambda))，i = 1..2n

4.2 同一个目标跟踪模型下 UKF 的 Python 实现与参数表

继续使用距离-方位角量测和恒速模型。下面实现 UKF 的预测和更新函数，sigma 点生成和权重计算放在一个辅助函数里。

import numpy as np def ukf_sigma_points(x, P, alpha=1e-3, kappa=0.0, beta=2.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, axis=0) 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, axis=0) 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_upd

ukf_sigma_points 完成 Cholesky 分解、sigma 点生成和权重计算。预测步让每个 sigma 点经过状态转移函数，加权求和得到预测均值和协方差。更新步让 sigma 点穿过量测函数，计算量测预测均值、量测协方差和互协方差。角度残差同样需要归一化。

参数选择上，alpha 默认取 1e-3 是常见做法，但这不是硬性规定。alpha 太小会让中心点权重接近 1，其余 sigma 点权重接近零，滤波退化为线性化点估计。alpha 取 0.5 到 1 时，sigma 点分布更分散，对强非线性更友好，但协方差容易非正定。kappa 取 0 或 3-n，beta 取 2。如果算出的 n + lambda 小于等于 0，必须重新选参数，否则开方失败。

4.3 EKF、CKF、UKF 在同一组数据上的误差对比与计算耗时

用同一个恒速目标轨迹和同一组雷达量测，跑 100 次蒙特卡洛，统计位置和速度的均方根误差。量测噪声距离标准差 1.0 m，角度标准差 0.02 rad。过程噪声 q = 0.05。采样周期 dt = 0.1 s，总步数 200。

从误差看，CKF 和 UKF 在非线性量测下比 EKF 低 10% 到 15%。从耗时看，EKF 最快，CKF 居中，UKF 因为要计算 2n+1 个 sigma 点和两组权重，单步耗时最高。调参参数个数上，CKF 最少，只需要 Q 和 R；UKF 需要 Q、R、alpha、kappa、beta；EKF 需要 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, confidence=0.95): # 卡方分布置信区间查表简化：用卡方分布分位数 from scipy.stats import chi2 m = len(nees_list) mean_nees = np.mean(nees_list) lower = chi2.ppf((1 - confidence) / 2, df=n * m) / m upper = chi2.ppf(1 - (1 - confidence) / 2, df=n * 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% 置信上界，说明滤波器跟丢或者参数不匹配。这个诊断不需要真实状态，适合在线运行。

三种滤波器在一致性诊断上的表现有差异。EKF 因为线性化误差，NEES 在非线性强时容易偏大。CKF 和 UKF 的 NEES 更接近理论值，但 UKF 的 alpha 和 kappa 选得不好时，P 会偏小，NEES

本文还有配套的精品资源，点击获取