扩展卡尔曼滤波EKF大概是工程里最常见的非线性滤波算法了。做传感器融合、目标跟踪、定位导航的朋友十有八九都要跟它打交道。它本身并不复杂就是把标准卡尔曼滤波KF拿过来用泰勒展开把非线性系统线性化再套进原有的预测-更新框架里。问题是很多教程要么只讲公式不讲落地要么丢一段代码却不说清楚为什么这样写。这篇我直接用二维目标跟踪的例子分别用matlab、python和C从零写一遍EKF把每一步的公式来源、代码对应关系和工程坑都摊开讲。适合刚接触EKF、会调包但想弄懂原理、以及需要多语言参考实现的人。1. 卡尔曼滤波的线性基础与EKF的核心思路1.1 先从线性卡尔曼滤波说起标准卡尔曼滤波解决的是线性高斯系统下的状态估计问题。系统满足两个方程状态方程x_k F * x_{k-1} B * u_k w_k观测方程z_k H * x_k v_k其中w_k是过程噪声满足正态分布N(0, Q)v_k是观测噪声满足正态分布N(0, R)。F 是状态转移矩阵H 是观测矩阵B 是控制输入矩阵。卡尔曼滤波之所以被称为“最优”是因为在“模型线性 噪声高斯 协方差已知”的三重假设下它给出的状态估计在均方误差意义下是最小的。整个算法分成两步预测和更新。预测就是用运动模型把上一时刻的状态往前推一帧同时把不确定性协方差 P变大因为在这一帧里又注入了过程噪声 Q。更新则是拿到当前观测后计算预测和观测之间的残差再用卡尔曼增益 K 决定“模型预测”和“观测”各信多少。这个直觉非常重要。K 大说明观测可信度高滤波结果往观测靠K 小说明模型预测可信度高结果往预测靠。后面 EKF 甚至更高级的 UKF、粒子滤波本质上还在沿用这个框架变化的只是如何表达非线性。1.2 EKF为什么要把非线性系统线性化真实系统几乎都不是线性的。以目标跟踪为例目标位置的变化如果在匀速模型下是线性的但雷达观测的是距离和方位角距离是sqrt(px^2 py^2)角度是atan2(py, px)这俩都是非线性函数。再比如车辆运动状态里有航向角状态转移会用到sin和cos。这种情况下卡尔曼滤波的核心假设“高斯分布经过线性变换仍然是高斯分布”就不成立了。一个高斯分布经过非线性函数映射后分布形状会扭曲不再是高斯。EKF 的做法很直接在当前状态估计点附近用一阶泰勒展开把非线性函数近似成线性函数然后继续套用卡尔曼滤波的公式。换句话说EKF 是“局部线性化后的卡尔曼滤波”。它在每个滤波周期都要重新计算雅可比矩阵因此 F 和 H 不再是固定的常数矩阵而是随着估计状态变化的矩阵。这一点是理解 EKF 的关键。由于线性化只是近似EKF 在理论上不是最优的估计误差方差可能比真实情况偏小滤波器也可能出现“过度自信”的现象。工程上只要采样率足够高、非线性不强、噪声不是特别大EKF 的效果都很好这也是它至今仍是工程主力的原因。1.3 泰勒展开与雅可比矩阵对于非线性函数f(x)在状态估计值x̂处做一阶泰勒展开f(x) ≈ f(x̂) J_f(x̂) * (x - x̂)其中J_f(x̂)是f对x的雅可比矩阵矩阵第 i 行第 j 列是∂f_i / ∂x_j所有偏导数都在x̂处取值。EKF 中的状态转移矩阵F_k就来自对状态方程f求雅可比观测矩阵H_k来自对观测方程h求雅可比。雅可比矩阵体现了在当前点附近状态量的微小变化会怎样影响下一时刻状态和观测。这就是 EKF 区别于标准 KF 的核心KF 里的 F 和 H 是常数EKF 里的 F 和 H 是“每步重新计算”的。很多人在 EKF 里卡住不是不理解公式而是不会推导雅可比矩阵。其实对工程来说解析推导只要做一次之后就是固定的代码模板。如果嫌麻烦也可以用数值差分去近似雅可比在滤波点附近加一个小扰动用中心差分求偏导后面我会在第4章详细讲。数值近似的好处是代码通用坏处是计算量大一点、精度略低而且容易掩盖模型本身的错误。1.4 EKF五步流程EKF 的完整流程可以写成五步后面三种语言代码都是按这五步来的状态预测x̂_k|k-1 f(x̂_k-1|k-1)协方差预测P_k|k-1 F_k * P_k-1|k-1 * F_k^T Q卡尔曼增益K_k P_k|k-1 * H_k^T * (H_k * P_k|k-1 * H_k^T R)^(-1)状态更新x̂_k|k x̂_k|k-1 K_k * (z_k - h(x̂_k|k-1))协方差更新P_k|k (I - K_k * H_k) * P_k|k-1第2步里那个F_k * P * F_k^T看着像二次型它的物理含义是把上一时刻的不确定性通过线性系统映射到当前时刻并叠加过程噪声。第3步里的S H * P_pred * H^T R是“预测观测”的不确定性它由状态不确定性映射过去的部分H P H^T和观测噪声 R 组成。卡尔曼增益就是状态预测不确定性与观测不确定性之比。我在实际写代码时习惯把x̂_pred、P_pred、z_pred、S、K这些中间变量都单独命名而不是写成一长串式子。这样一旦结果不对打断点看每一步的维度、量级问题马上就能定位。后面三套代码也都保持了这种风格。2. 从理论到实例目标追踪的状态方程与观测设计2.1 为什么选二维目标追踪作为教学例子二维平面内的目标跟踪是 EKF 最经典的教学场景。它比一维问题更有实际意义比三维问题又简单得多。一维定位只能看到距离这个量测没法充分体现观测方程的非线性三维问题则要处理多个维度之间的耦合代码量会让新手分心。二维目标同时有位置和速度还能用距离和方位角作为量测正好同时覆盖“线性状态转移”和“非线性观测模型”两种情况。这个例子也是雷达监视、声呐跟踪、自动驾驶多传感器融合里常见场景的简化版。你只需要把传感器部分换成雷达成像或摄像头处理思路是一样的。所以把这个问题吃透后面迁移到自己的项目里会非常顺。2.2 状态方程与观测方程设计状态向量取x [px, py, vx, vy]^T其中 px 和 py 表示目标在平面内的位置vx 和 vy 表示速度。假设目标做匀速直线运动状态转移方程为x_k F * x_{k-1} w_k其中 F 是常数矩阵F [1, 0, dt, 0; 0, 1, 0, dt; 0, 0, 1, 0; 0, 0, 0, 1]过程噪声协方差 Q 用连续白噪声加速度模型constant white noise acceleration离散化得到。它表示在 dt 时间内速度可能发生随机变化进而影响位置。Q 的形式为Q [dt^3/3, 0, dt^2/2, 0; 0, dt^3/3, 0, dt^2/2; dt^2/2, 0, dt, 0; 0, dt^2/2, 0, dt] * q这里的 q 是过程噪声强度单位通常是 m^2/s^3。q 越大说明模型对目标加速度变化的建模越不确定。观测方程是非线性的。传感器返回距离 r 和方位角 thetar sqrt(px^2 py^2)theta atan2(py, px)观测噪声协方差R [sigma_r^2, 0; 0, sigma_theta^2]sigma_r 和 sigma_theta 是传感器测距和测角的噪声标准差通常可以从传感器标定结果或实测数据里估计。这里所有量测类型都是非线性所以必须用 EKF 而不是 KF。2.3 观测雅可比矩阵推导观测函数h(x)对状态向量求偏导得到 2x4 的雅可比矩阵H_k。对r求导∂r/∂px px / r ∂r/∂py py / r ∂r/∂vx 0 ∂r/∂vy 0对theta求导∂theta/∂px -py / r^2 ∂theta/∂py px / r^2 ∂theta/∂vx 0 ∂theta/∂vy 0所以H_k [px/r, py/r, 0, 0; -py/(r^2), px/(r^2), 0, 0]注意两点。第一这里所有偏导都在预测状态点x_pred处计算并用预测出的r_pred代入不能用上一时刻的值更不能直接套用一组固定常数。第二当目标离原点非常近时r接近零H 阵会急剧增大数值上可能不稳定。工程上一般会设置一个最低门限比如r 1e-6时用一个小值保护一下。2.4 如果目标转弯非线性状态方程的扩展上面例子中状态方程是线性的只有观测方程是非线性。实际工程里目标转弯时状态方程本身也带非线性。如果状态向量扩展为[px, py, vx, vy, w]^T其中 w 是转弯率那么匀速转弯CT模型的状态方程为px_k px_{k-1} (vx / w) * sin(w * dt) - (vy / w) * (1 - cos(w * dt)) py_k py_{k-1} (vx / w) * (1 - cos(w * dt)) (vy / w) * sin(w * dt) vx_k vx * cos(w * dt) - vy * sin(w * dt) vy_k vx * sin(w * dt) vy * cos(w * dt) w_k w_{k-1} 噪声这时 F 矩阵就不是常数了而是状态转移函数对[px, py, vx, vy, w]的 5x5 雅可比矩阵。其中对 w 的偏导项推导起来特别容易出错因为 vx、vy、sin、cos 交织在一起。我个人的建议是先用数值差分验证解析结果确认无误再写死到代码里这个习惯能帮你省下大量调 bug 的时间。EKF 的“扩展”二字正体现在这里无论是状态方程非线性还是观测方程非线性处理方法都是同一套——在估计点取雅可比然后套用 KF 框架。3. 用三种语言从零实现同一个EKF3.1 matlab版本最快出结果matlab 做验证最方便因为矩阵运算和绘图都是一行式。下面代码生成一条匀速直线轨迹模拟距离和方位角量测然后跑 EKF 滤波最后画图比较真值和估计值。% EKF example: 2D target tracking with range/angle measurement clear; clc; close all; rng(1); dt 0.1; T 30; t 0:dt:T; N length(t); % 匀速模型的状态转移矩阵 F [1 0 dt 0;... 0 1 0 dt;... 0 0 1 0;... 0 0 0 1]; % 生成真实轨迹 x_true zeros(4, N); x_true(:, 1) [0; 0; 10; 5]; for k 2:N x_true(:, k) F * x_true(:, k - 1); end % 量测距离和方位角加高斯噪声 sigma_r 0.5; sigma_theta 0.05; R diag([sigma_r^2, sigma_theta^2]); z zeros(2, N); for k 1:N px x_true(1, k); py x_true(2, k); z(1, k) sqrt(px^2 py^2) sigma_r * randn(); z(2, k) atan2(py, px) sigma_theta * randn(); end % EKF 初始化 x_est zeros(4, N); x_est(:, 1) [0; 0; 0; 0]; P eye(4) * 100; q 0.1; Q [dt^3/3, 0, dt^2/2, 0;... 0, dt^3/3, 0, dt^2/2;... dt^2/2, 0, dt, 0;... 0, dt^2/2, 0, dt] * q; I eye(4); for k 2:N % 预测 x_pred F * x_est(:, k - 1); P_pred F * P * F Q; % 计算预测观测和观测雅可比 px x_pred(1); py x_pred(2); r_pred sqrt(px^2 py^2); th_pred atan2(py, px); z_pred [r_pred; th_pred]; H [px / r_pred, py / r_pred, 0, 0;... -py / (r_pred^2), px / (r_pred^2), 0, 0]; % 更新 S H * P_pred * H R; K P_pred * H / S; innovation z(:, k) - z_pred; x_est(:, k) x_pred K * innovation; P (I - K * H) * P_pred; end % 画图 figure; plot(x_true(1, :), x_true(2, :), k-, LineWidth, 1.5); hold on; plot(x_est(1, :), x_est(2, :), r--, LineWidth, 1.5); legend(truth, EKF); grid on; xlabel(x); ylabel(y);第 29 行的K P_pred * H / S在 matlab 里表示P_pred * H * inv(S)。由于 S 在这里是 2x2 矩阵这样写足够稳定。如果你用的版本较新更推荐写成K P_pred * H * inv(S)逻辑更明确。这个例子中我把状态初值设成了[0;0;0;0]但把 P 初始协方差给到100 * I表示初始状态完全不确定。这样滤波器会在一开始的几步里快速修正速度估计不会因为初值偏差毁掉整个估计过程。3.2 python版本numpy矩阵运算最顺手python 代码和 matlab 逻辑完全一致只是语法换成 numpy。我用列向量存储所有状态矩阵全部用 np.array整体可读性很好。import numpy as np import matplotlib.pyplot as plt np.random.seed(1) dt 0.1 T 30.0 t np.arange(0.0, T dt, dt) N len(t) F np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) # 真实轨迹 x_true np.zeros((4, N)) x_true[:, 0] [0.0, 0.0, 10.0, 5.0] for k in range(1, N): x_true[:, k] F x_true[:, k - 1] # 量测 sigma_r 0.5 sigma_theta 0.05 R np.diag([sigma_r**2, sigma_theta**2]) z np.zeros((2, N)) for k in range(N): px, py x_true[0, k], x_true[1, k] z[0, k] np.hypot(px, py) sigma_r * np.random.randn() z[1, k] np.arctan2(py, px) sigma_theta * np.random.randn() # EKF 初始化 x_est np.zeros((4, N)) x_est[:, 0] [0.0, 0.0, 0.0, 0.0] P np.eye(4) * 100.0 q 0.1 Q np.array([[dt**3 / 3, 0, dt**2 / 2, 0], [0, dt**3 / 3, 0, dt**2 / 2], [dt**2 / 2, 0, dt, 0], [0, dt**2 / 2, 0, dt]]) * q I np.eye(4) for k in range(1, N): # 预测 x_pred F x_est[:, k - 1] P_pred F P F.T Q # 预测观测和观测雅可比 px, py, vx, vy x_pred r_pred np.hypot(px, py) th_pred np.arctan2(py, px) z_pred np.array([r_pred, th_pred]) H np.array([[px / r_pred, py / r_pred, 0, 0], [-py / (r_pred**2), px / (r_pred**2), 0, 0]]) # 更新 S H P_pred H.T R K P_pred H.T np.linalg.inv(S) innovation z[:, k] - z_pred x_est[:, k] x_pred K innovation P (I - K H) P_pred # 画图 plt.plot(x_true[0, :], x_true[1, :], k-, labeltruth) plt.plot(x_est[0, :], x_est[1, :], r--, labelEKF) plt.legend() plt.grid(True) plt.show()这里px, py, vx, vy x_pred是 numpy 一维数组的解包不涉及长度变化能正常工作。如果你习惯用列向量shape(4,1)解包需要写成px x_pred[0,0]这样代码会啰嗦一些。我建议入门阶段用一维数组把注意力放在 EKF 本身上。python 版本里np.linalg.inv(S)对 2x2 矩阵求逆足够快。如果以后要扩展到高维量测推荐用np.linalg.solve(S, H P_pred H.T R)的方式避免显式求逆。高斯消元比求逆再乘更稳定也更快。3.3 C版本工程落地首选C 版本的矩阵运算我用 Eigen 库。Eigen 是一个 header-only 的模板库不需要编译只要把 include 目录指到编译器搜索路径即可在 VS、g、Clang 里都能用。实际工程里用 Eigen 是最省事的选择。#include Eigen/Dense #include iostream #include cmath #include vector #include random using namespace Eigen; int main() { double dt 0.1; double T 30.0; int N static_castint(T / dt) 1; // 匀速模型的状态转移矩阵 Matrix4d F; F 1, 0, dt, 0, 0, 1, 0, dt, 0, 0, 1, 0, 0, 0, 0, 1; // 真实轨迹 Vector4d x_true0(0, 0, 10, 5); std::vectorVector4d x_true(N); x_true[0] x_true0; for (int k 1; k N; k) { x_true[k] F * x_true[k - 1]; } // 量测距离 方位角 double sigma_r 0.5; double sigma_theta 0.05; Matrix2d R (Matrix2d() sigma_r * sigma_r, 0, 0, sigma_theta * sigma_theta).finished(); std::default_random_engine gen(1); std::normal_distributiondouble noise_r(0, sigma_r); std::normal_distributiondouble noise_th(0, sigma_theta); std::vectorVector2d z(N); for (int k 0; k N; k) { double px x_true[k](0); double py x_true[k](1); z[k] Vector2d(std::hypot(px, py) noise_r(gen), std::atan2(py, px) noise_th(gen)); } // EKF 初始化 std::vectorVector4d x_est(N); x_est[0] Vector4d(0, 0, 0, 0); Matrix4d P Matrix4d::Identity() * 100.0; double q 0.1; Matrix4d Q; Q std::pow(dt, 3) / 3, 0, std::pow(dt, 2) / 2, 0, 0, std::pow(dt, 3) / 3, 0, std::pow(dt, 2) / 2, std::pow(dt, 2) / 2, 0, dt, 0, 0, std::pow(dt, 2) / 2, 0, dt; Q * q; Matrix4d I4 Matrix4d::Identity(); for (int k 1; k N; k) { // 预测 Vector4d x_pred F * x_est[k - 1]; Matrix4d P_pred F * P * F.transpose() Q; // 预测观测和观测雅可比 double px x_pred(0); double py x_pred(1); double r_pred std::hypot(px, py); double th_pred std::atan2(py, px); Vector2d z_pred(r_pred, th_pred); Matrixdouble, 2, 4 H; H px / r_pred, py / r_pred, 0, 0, -py / (r_pred * r_pred), px / (r_pred * r_pred), 0, 0; // 更新 Matrix2d S H * P_pred * H.transpose() R; Matrixdouble, 4, 2 K P_pred * H.transpose() * S.inverse(); Vector2d innovation z[k] - z_pred; x_est[k] x_pred K * innovation; P (I4 - K * H) * P_pred; } // 输出位置 RMSE double rmse 0.0; for (int k 0; k N; k) { double dx x_true[k](0) - x_est[k](0); double dy x_true[k](1) - x_est[k](1); rmse dx * dx dy * dy; } rmse std::sqrt(rmse / N); std::cout position RMSE: rmse std::endl; return 0; }C 里最容易出错的地方是矩阵维度。Eigen 在 Debug 模式下会做运行时维度检查一旦维度不匹配会直接断言失败所以调试阶段一定不要关掉检查。上面代码里 H 是Matrixdouble, 2, 4K 是Matrixdouble, 4, 2这类形状在写代码前最好先用注释标出来否则一长串矩阵乘在一起编译通过但运行时报错会很费时间。3.4 三套代码对比与运行参数三套代码在相同随机种子和相同噪声参数下结果应该完全一致。下面是我常用的参数参数取值含义dt0.1s滤波周期sigma_r0.5m测距噪声标准差sigma_theta0.05rad测角噪声标准差q0.1过程噪声强度P0100 * I初始协方差x0[0, 0, 0, 0]初始状态估计三套代码结构完全一样预测、观测雅可比、更新三个模块一一对应。运行下来位置 RMSE 大致在 0.3 到 0.4 米之间。真实位置从 (0,0) 出发速度是 (10,5)运行 30 秒后已经走了几百米这个误差水平说明 EKF 表现正常。语言依赖适用场景matlab无额外依赖算法验证、课程实验pythonnumpy、matplotlib数据分析、科研快速原型CEigen嵌入式平台、实时系统、工程部署三套代码我在实际项目中来回切换。日常调试形态用 matlab 或 python因为改一行跑一下就能看到图定型需要性能或嵌入式部署时再移植到 C。4. EKF 调参与坑点排查4.1 噪声矩阵怎么设滤波效果好不好Q 和 R 的比例往往比算法本身更关键。R 可以从传感器实测数据标定让传感器静止不动采集大量量测计算标准差就行。Q 取值稍微复杂一点。Q 太小滤波器会过度信任模型量测稍微偏离预测就不敢跟进轨迹会出现明显的“平滑滞后”Q 太大滤波器过度信任量测随机噪声会被大量放进去轨迹毛糙不稳定。一个常用思路是把 Q 和目标的“最大可承受加速度”挂钩。对于匀速模型离散白噪声加速度模型的 Q 推导已经固定你只需要调那个 q。也可以设定一个最大加速度a_max令 q ≈ (a_max^2) * dt这样算出来的过程噪声和物理意义对得上。实际调参时我习惯先把 Q 调大一点让滤波器跟得上观测再慢慢减小直到轨迹平滑度可以接受。这个手感调过几次就有了。4.2 滤波发散怎么办EKF 最常见的问题就是滤波发散估计状态离真值越来越远或者滤波器始终认为自己已经很准但实际误差在持续增大。排查顺序通常是检查雅可比矩阵是否算错检查 Q、R 是否设定合理检查初值 P0 是否过小检查状态方程和观测方程是否因单位、坐标变换引入错误。如果发散而且 P 持续缩小很可能就是雅可比算错或模型失配。如果只是噪声参数不合适一般表现为轨迹偏离但 P 还保持一定大小。另外数值上要保持 P 的对称性。浮点计算多次乘加后P 可能轻微不对称长时间运行会累积数值问题。我习惯在更新完成后加一句半对称处理P (P P.transpose()) / 2这句话在三种语言里都能写代价极小但能避免很多莫名其妙的问题。4.3 雅可比矩阵检查解析雅可比一旦写错EKF 很难稳定而且错误不一定立刻暴露。最常见的症状是滤波轨迹在真实值附近震荡同时协方差 P 异常收缩或异常膨胀。要验证雅可比最可靠的办法是用数值差分做交叉检查。对观测函数h(x)中心差分求第 i 列eps 1e-6 H_num np.zeros((2, 4)) for i in range(4): xp x_pred.copy() xm x_pred.copy() xp[i] eps xm[i] - eps H_num[:, i] (h(xp) - h(xm)) / (2 * eps)然后把H_num和解析算出来的 H 打印出来对比。正常情况下每个元素应该非常接近差值在1e-8量级。如果只差几百倍多半是某个偏导前少了负号或者把 px 和 py 的位置搞反了。我在写 CT 模型的 F 雅可比时每次都会先用数值法验证再写死节约的时间远比写验证代码多。4.4 一致性检验与角度环绕问题滤波跑通了不代表滤波就是一致的。工程上常用 NIS归一化新息平方检查滤波是否“过度自信”或“过度保守”。NIS 定义为NIS_k innovation_k^T * S_k^(-1) * innovation_k如果滤波一致NIS 的均值应该接近量测维数。这里量测维度是 2所以 NIS 均值应该在 2 附近。如果 NIS 长期远大于 2说明滤波器低估了真实误差如果远小于 2说明协方差被高估滤波器过度保守。另一个绕不开的问题是角度环绕。实际目标如果做圆周运动方位角可能在 179 度和 -179 度之间跳变如果不处理innovation 会变成 358 度滤波器直接被打飞。我常用的处理方式是把 innovation 的角度部分归一化到(-pi, pi]innovation_angle atan2(sin(innovation_angle), cos(innovation_angle))这句代码在三种语言里几乎一样。很多教程里不写这个坑但所有做角度量测的人都迟早会碰上。如果你想用这个 EKF 例子跟踪一个绕圈目标这行处理是必须加的。我在实际项目里一直把 EKF 当成最稳妥的 baseline 来用。先跑通这个最简单的滤波再去考虑更复杂的 UKF、粒子滤波或者加交互多模型。上面这三套代码我反复用过很多次最大的体会是EKF 的数学推导看似复杂但落到代码上就五个公式真正花时间的不是写循环而是把雅可比矩阵、噪声矩阵和角度处理这些工程细节校准。做仿真时别急着上复杂场景先用匀速直线加高斯噪声把流程跑顺再一步步加入转弯、遮挡、多目标这些情况。每一步都验证过再往下走你会发现自己其实已经把一个挺实用的跟踪滤波系统搭出来了。