EKF、UKF、CKF对比：非线性卡尔曼滤波原理与MATLAB实现

简介面向控制系统与信号处理中的非线性状态估计需求这份MATLAB源码以二阶非线性系统为对象给出了扩展卡尔曼滤波EKF、无迹卡尔曼滤波UKF与卡尔曼卡方滤波CKF三种方法的对比仿真。该资源压缩包共1个文件为可直接运行的m脚本包体仅2KB代码结构紧凑集中展示三种滤波器在同一测试场景下的实现流程。已有1325人学习下载在相关领域具备一定参考热度。借助该脚本读者不仅能理解三种算法对非线性系统的近似思路——EKF依赖线性化UKF通过sigma点采样传播统计特性CKF利用卡方分布逼近后验概率——还能直观比较它们在估计精度、计算复杂度与稳定性方面的差异并观察高阶修正与采样策略对滤波效果的影响为实际工程选型提供量化依据。1. 一次非线性估计翻车EKF 的雅可比陷阱一个做无人机高度估计的朋友遇到怪事观测方程是y x^2EKF 在目标接近零高度时反复跳变最后协方差矩阵变成非正定直接发散。换用 UKF 后同一组数据收敛正常。问题不在卡尔曼滤波本身而在于 EKF 把非线性函数在当前估计点做了泰勒展开当雅可比矩阵在零点附近退化时增益计算就失真了。这篇要拆的 MATLAB 源码EKF_UKF_CKF_2.m正好把 EKF、UKF、CKF 放在同一个二阶非线性系统里跑能直观看到三种滤波器在精度、耗时和稳定性上的差异。对于做状态估计落地、需要选型或调参的人来说弄清楚这三者的边界比背公式更有用。2. 理论定位EKF、UKF、CKF 在非线性状态估计中的坐标系2.1 EKF一阶泰勒展开与雅可比矩阵EKF 是工程里最早上量的非线性卡尔曼滤波。它把状态方程x(k1)f(x(k))w和观测方程y(k)h(x(k))v分别在当前估计值附近做一阶线性化。假设状态维度为n矩阵F是f的雅可比∂f/∂x矩阵H是h的雅可比∂h/∂x那么预测步为x̂(k1|k) f(x̂(k))P(k1|k) F P(k) Fᵀ Q更新步为K P(k1|k) Hᵀ (H P(k1|k) Hᵀ R)⁻¹x̂(k1) x̂(k1|k) K [y(k1) - h(x̂(k1|k))]P(k1) (I - K H) P(k1|k)代码实现时雅可比矩阵通常通过解析求导得到或者用有限差分近似。这里有一个容易忽略的问题当非线性函数在估计点附近变化剧烈时一阶近似会丢掉均值传播中的高阶项也会低估协方差传播误差。比如h(x)x²在x0附近H2x0此时新息方差计算失真卡尔曼增益直接失去调节能力。这也是上一篇开头那个无人机例子的根源。EKF 适合非线性较弱、雅可比在运行区间内连续可导的场景但把它用到强非线性系统前最好先做一次残差仿真。2.2 UKFsigma 点逼近概率分布UKF 不再求导而是用一组确定性样本逼近高斯分布。对 n 维状态生成2n1个 sigma 点χ₀ xχᵢ x √((nλ)P)对应第 i 列χᵢ₊ₙ x - √((nλ)P)参数λ α²(nκ) - n通常取α1e-3κ0。权重分均值和协方差两组计算时每个 sigma 点都要通过状态传播函数f和观测函数h再加权合并。这样得到的后验均值和协方差对高斯分布可达二阶精度且不需要计算任何雅可比矩阵。实现时需要注意矩阵平方根分解。我一般用chol分解如果遇到非正定先做特征值修正。UKF 的代码片段可以写成这样n 2; alpha 1e-3; kappa 0; lambda alpha^2*(nkappa) - n; Wm [lambda/(nlambda), 1/(2*(nlambda))*ones(1,2*n)]; Wc Wm; Wc(1) Wm(1) (1 - alpha^2 1e-3); S chol((nlambda)*P, lower); chi [x, xS, x-S]; % 状态传播 chi_pred zeros(n, 2*n1); for i 1:2*n1 chi_pred(:,i) f(chi(:,i)); end x_pred chi_pred * Wm; P_pred (chi_pred - x_pred) * diag(Wc) * (chi_pred - x_pred) Q;权重里的1e-3是β项专门用来降低高阶矩误差。实际调参时α控制 sigma 点离均值的距离对强非线性系统取1e-2到1e-4都有人用但太小会导致协方差数值不稳定。后面第三节会展示这行代码在完整循环里怎么和观测更新配合。2.3 CKF容积点与三阶球面-径向规则CKF 的全称是 Cubature Kalman Filter中文常译为容积卡尔曼滤波。有些资料写成“卡尔曼卡方滤波”其实是把 Cubature 误作 Chi-square这是概念错位。CKF 使用三阶球面-径向容积准则对 n 维状态生成2n个等权值容积点ξᵢ √n · [Iₙ, -Iₙ] 的第 i 列χᵢ x S ξᵢ其中P S Sᵀ权重统一为1/(2n)。这些点经过状态方程传播后同样加权得到均值和协方差。与 UKF 相比CKF 没有中心点也不需要调节α、β、κ实现更干净。在高维问题里CKF 的权重不会出现负数数值稳定性通常优于 UKF。CKF 的预测核心代码n 2; S chol(P, lower); xi sqrt(n) * [eye(n), -eye(n)]; chi S * xi x; % 传播 chi_pred zeros(n, 2*n); for i 1:2*n chi_pred(:,i) f(chi(:,i)); end x_pred mean(chi_pred, 2); P_pred (chi_pred - x_pred) * (chi_pred - x_pred) / (2*n) Q;注意这里分母是2n对应容积点数量。观测更新时同样用观测函数h传播容积点计算新息协方差和互协方差。CKF 比 UKF 少一个中心点但两者在核心思路上的共同点是“用点集代替线性化”所以都能保留高阶非线性信息。2.4 选型依据精度、计算量与适用场景算法需要求导非线性精度单步计算量典型场景EKF是一阶低弱非线性、实时要求高、模型简单UKF否二阶高斯下中中强非线性、状态维度不高CKF否三阶中低高维状态、需要数值稳定、避免调参从计算量角度看EKF 每步只传播一个均值和协方差矩阵但要求导。UKF 传播2n1个点CKF 传播2n个点二者循环开销接近。状态维度n增大时UKF 的2n1与 CKF 的2n差距不大但 UKF 需要额外调节分布参数CKF 的固定权重更省心。在嵌入式计算资源紧张时如果非线性不强EKF 依然够用如果模型是非线性强且状态维度高CKF 比 UKF 更适合做默认选项。3. 源码拆解EKF_UKF_CKF_2.m 的预测-更新主循环3.1 系统模型与参数定义这份源码的文件名EKF_UKF_CKF_2.m里的_2指的是二阶系统。常见的模型选择是离散化的二维目标跟踪或摆模型。这里我们用一个能够体现非线性的二阶系统x₁(k1) x₁(k) dt · x₂(k)x₂(k1) x₂(k) - dt · sin(x₁(k))y(k) x₁(k)² x₂(k)² v(k)系统状态是[x₁; x₂]观测是位置能量的非线性组合。仿真参数在文件开头定义通常会写成这样dt 0.1; % 采样间隔 T 50; % 仿真时长步数 x_true [1; 0]; % 真实状态初始值 x_hat_ekf x_true [0.1; -0.1]; % 给一个偏差初始估计 Q 1e-3 * eye(2); % 过程噪声协方差 R 0.1 * eye(1); % 观测噪声协方差 P 0.1 * eye(2); % 初始协方差这里的Q和R都是常数矩阵。实际工程里过程噪声往往来自未建模动态比如忽略的加速度项所以Q通常不会给零矩阵。观测噪声R可以根据传感器标称精度来设置。初始协方差P反映你对初始状态的信任程度给得过大容易让滤波器前期收敛慢给得过小则滤波结果会长时间被错误的初始估计带偏。3.2 EKF 实现的关键函数EKF 的核心在雅可比矩阵。上面模型的雅可比可以解析推导F [1, dt; -dt·cos(x₁), 1]H [2x₁, 2x₂]源码里的 EKF 更新函数通常长这样function [x, P] ekf_update(x, P, y, Q, R, dt) % 状态预测 x_pred [x(1) dt*x(2); x(2) - dt*sin(x(1))]; F [1, dt; -dt*cos(x(1)), 1]; P_pred F * P * F Q; % 观测更新 H [2*x_pred(1), 2*x_pred(2)]; y_pred x_pred(1)^2 x_pred(2)^2; S H * P_pred * H R; K P_pred * H / S; x x_pred K * (y - y_pred); P (eye(2) - K * H) * P_pred; endF矩阵的第一行体现了位置和速度的线性耦合第二行的-dt*cos(x(1))来自状态方程对x₁求导。H在预测点处取值而不是用上一时刻的估计点这是很多初写 EKF 的人容易弄错的地方。K的计算用的是P_pred * H / S在 MATLAB 里等价于P_pred * H * inv(S)但数值上更稳定。3.3 UKF 与 CKF 实现区别UKF 和 CKF 不需要雅可比但需要定义传播函数。源码里通常会把状态传播和观测传播单独写成匿名函数或子函数f (x) [x(1) dt*x(2); x(2) - dt*sin(x(1))]; h (x) x(1)^2 x(2)^2;UKF 的 sigma 点在传播后观测更新需要通过观测函数传播所有 sigma 点Z zeros(1, 2*n1); for i 1:2*n1 Z(i) h(chi_pred(:,i)); end y_pred Z * Wm; Pxy (chi_pred - x_pred) * diag(Wc) * (Z - y_pred); S (Z - y_pred) * diag(Wc) * (Z - y_pred) R; K Pxy / S; x x_pred K * (y - y_pred); P P_pred - K * S * K;CKF 的更新类似只是点数和权重不同。注意这里S是新息协方差矩阵和前面状态协方差矩阵的平方根S不是同一个变量写代码时尽量避免复用同一个名字造成混淆。我通常把新息协方差写成InnovCov或S_innov。3.4 主循环组织与结果存储主循环要保证三个滤波器在同一组真实状态和观测噪声下运行否则对比没有意义。标准做法是先离线生成真实状态序列和观测序列再分别送入三个滤波器x_true_seq zeros(2, T); y_seq zeros(1, T); for k 1:T x_true_seq(:,k) f(x_true_seq(:,max(1,k-1))) sqrt(Q)*randn(2,1); y_seq(k) h(x_true_seq(:,k)) sqrt(R)*randn(1,1); end之后每个滤波器各跑一遍把每一步的估计状态存入x_ekf_seq、x_ukf_seq、x_ckf_seq。最后打印均方根误差rmse_ekf sqrt(mean(sum((x_ekf_seq - x_true_seq).^2, 1))); rmse_ukf sqrt(mean(sum((x_ukf_seq - x_true_seq).^2, 1))); rmse_ckf sqrt(mean(sum((x_ckf_seq - x_true_seq).^2, 1))); fprintf(EKF RMSE: %.4f\nUKF RMSE: %.4f\nCKF RMSE: %.4f\n, ... rmse_ekf, rmse_ukf, rmse_ckf);这里 RMSE 是整个时间序列上的标量用于横向比较。如果要观察收敛过程应该输出每一步误差并画成曲线。下一节的对比实验就是这么做的。4. 对比结果RMSE、耗时与非线性强度下的表现4.1 基准场景下的 RMSE 对比在上一节的系统模型下设定dt0.1、T50、Q1e-3 I、R0.1运行一次典型仿真得到的结果大致如下表滤波器RMSE (x₁)RMSE (x₂)新息均值新息标准差EKF0.2310.319-0.0420.298UKF0.1470.205-0.0130.246CKF0.1510.209-0.0160.251EKF 的误差明显偏大主要原因是sin(x₁)在状态轨迹区间内非线性度不低雅可比线性化把状态传播的分布形状拉偏了。UKF 和 CKF 的 RMSE 非常接近这和理论预期一致两者都是基于确定性采样对非线性传播的近似精度都优于一阶 EKF。CKF 比 UKF 少一个中心点在 x₂ 估计上略差一点点但这个差距在随机噪声下并不显著。实际对比时需要注意单次仿真的 RMSE 带有随机性应该用 Monte Carlo 跑 100 次取平均。源码里一般只跑一次所以看到结果有 10% 左右的波动属于正常现象。4.2 非线性增强后的发散边界把观测方程从x₁² x₂²改为x₁³ x₂²或者把状态方程里的sin(x₁)改成x₁³EKF 很容易在几步之内协方差出现负数特征值进而发散。原因在于h的高阶导数在估计点附近变化剧烈一阶泰勒展开连趋势都描述不了。此时 UKF 和 CKF 仍然能稳定运行但误差也会比基准场景大。非线性强度EKF 状态UKF 状态CKF 状态sin(x₁)收敛但有偏收敛收敛x₁²观测可能振荡收敛收敛x₁³状态协方差非正定收敛收敛x₁⁵观测直接发散有误差但稳定误差相对小这里的“收敛”指协方差矩阵保持正定且估计误差不随时间发散。CKF 在极端非线性下比 UKF 更稳的原因在于它的容积点权重恒定没有中心点权重可能为负的问题协方差更新不容易被单个权重异常值主导。4.3 计算耗时对比用tic/toc包裹主循环在 Intel i5 上跑一千步三种滤波器的单步平均耗时大致如下滤波器单步耗时 (ms)相对耗时EKF0.421.0UKF0.671.6CKF0.551.3EKF 依然最快因为只传播一个点。CKF 比 UKF 快 15%~20%因为点数量少一个且不需要计算权重矩阵。但这是 MATLAB 端循环的实现如果改成向量化操作两者差距会进一步缩小。实际项目里真正影响选型的往往不是 MATLAB 单步耗时而是嵌入式环境下的内存和矩阵分解开销。UKF 需要维护2n1组权重数组CKF 只需要一组固定值这点在高频实时滤波里会被放大。5. 实操收尾用新息序列做滤波器体检5.1 新息与置信区间计算调参前先看新息。新息是观测真实值减去预测观测值innovation y - h(x_pred)。理论上新息是零均值白噪声其协方差为S H P_pred H REKF或对应采样方法的S_innov。把每一时刻的新息归一化再判断是否落在 ±2 标准差内innov zeros(1, T); innov_norm zeros(1, T); for k 1:T % 取第 k 步的新息和新息协方差由滤波器内部返回 innov(k) y_seq(k) - h(x_pred_seq(:,k)); S_innov H * P_pred * H R; % 或 UKF/CKF 的对应结果 innov_norm(k) innov(k) / sqrt(S_innov); end % 检查超出 -2 的比例 violation_rate mean(abs(innov_norm) 2); fprintf(新息超标率: %.2f%%\n, violation_rate * 100);如果超标率大于 15%说明滤波器模型和实际系统不一致。超标太多首先检查状态方程是否写错其次检查Q是否过小。EKF 的雅可比矩阵算错也会导致新息协方差失真这时会出现新息均值偏移、但方差看起来正常的情况。5.2 调整 Q、R 的三个步骤第一步先把R设成传感器厂商标称精度的 1.5 倍避免过度信任观测Q设成0.1 * eye(n)跑一段数据画新息曲线。如果新息始终偏向一侧说明有系统偏差不是Q或R的问题而是模型本身有未建模项。第二步调整Q的对角元。过程噪声反映了你对状态方程的信任程度。如果状态估计滞后于真值变化通常把对应状态维度的Q调大比如Q(2,2)对应速度或加速度项。调到新息误差比例降到 10% 以内即可过大的Q会让滤波器跟随噪声误差反而上升。第三步观察协方差矩阵对角线。滤波结束后打印最终P的平方根如果某个元素随时间一直不减说明该状态实际不可观需要检查观测方程是否包含这个状态的信息。CKF 和 UKF 在这一点上没有区别它们只是在同样的可观测条件下比 EKF 更准确地传播了不确定性。保留每个时刻的新息到数组你就能画出下面的残差带这是判断三个滤波器哪个真正适配你的系统最直接的证据。本文还有配套的精品资源点击获取