扩展卡尔曼滤波（EKF）这个名词，搞机器人、自动驾驶、导航定位、目标跟踪的朋友一定不陌生。很多人在学校里学过卡尔曼滤波（KF），一进项目发现要处理的是非线性系统——无人机姿态解算里的欧拉角、雷达测距测角、GPS/惯性导航融合，全是非线性的。拿线性卡尔曼硬套，滤波器轻则精度下降，重则直接发散。我最早踩这个坑是在做室内定位融合的时候，UWB测距方程里带根号，我拿标准KF去滤波，估计出来的轨迹肉眼可见地抖动，当时还没意识到问题出在线性化上，折腾了好几天。后来把EKF的雅可比矩阵真正搞明白，整套系统才稳定下来。
这篇文章我打算把EKF从理论到实践完整讲透，内容包括非线性问题到底难在哪、EKF是怎么通过泰勒展开做线性化的、雅可比矩阵怎么求，以及一个经典的目标跟踪实例分别用MATLAB、Python、C++实现。三套代码我都跑过，里面的坑也都会点出来。不管你是在校学生还是转行做算法部署的工程师，照着捋一遍，EKF这块基本就通了。
1. 线性卡尔曼滤波的"舒适区"与非线性系统的真实困境
1.1 标准KF的四个前提假设
要理解EKF为什么存在，得先看清楚标准卡尔曼滤波到底在什么条件下才能成立。KF的本质是最小均方误差意义下的最优估计，但它能成为"最优"是建立在四个严格假设之上的：
第一，系统状态转移是线性的——也就是当前时刻的状态是上一时刻状态的线性组合，可以写成状态转移矩阵乘以状态向量再加上控制输入的形式。
第二，测量方程是线性的——测量值等于观测矩阵乘以状态向量加上测量噪声。
第三，过程噪声和测量噪声都是零均值的高斯白噪声，并且两者互不相关。
第四，状态的初始估计误差也服从高斯分布。
这四个假设凑齐了，卡尔曼滤波才能递推地给出均值和协方差，而且这两个量就能完整描述状态的后验分布。
现实世界的物理系统哪有这么听话。以最常见的目标跟踪为例，用雷达测量一个运动目标，雷达能直接测到的是距离和方位角，而我们要估计的状态往往是目标在笛卡尔坐标系下的位置和速度。从直角坐标到极坐标的转换：
distance = sqrt(x^2 + y^2) angle = atan2(y, x)这个转换关系里既有平方根又有反正切，妥妥的非线性。状态方程倒还好，不少场景下目标近似匀速直线运动，状态转移是线性的；但只要测量方程非线性，标准KF就没法直接套。
1.2 非线性系统的两种处理思路：直接线性化还是无迹变换
面对非线性问题，工程界主流的破解思路有两条。
一条是解析法的思路，也就是EKF的做法：把非线性函数在当前估计值附近做一阶泰勒展开，用切线近似替代原曲线，得到一个近似的线性模型，然后再套标准卡尔曼滤波的框架。EKF的好处是计算量小、实现简单、对算力要求低，在很多嵌入式和实时性要求高的场景里是首选。
另一条是基于统计采样的思路，代表是无迹卡尔曼滤波（UKF）和粒子滤波（PF）。UKF不直接线性化函数，而是选取一组Sigma点，让这些点经过非线性变换后，用变换后的点的均值和协方差来近似真实分布。粒子滤波更彻底，直接用大量随机样本逼近后验分布。这两者在强非线性场景下的精度更高，但计算量也更大，并且对调参更敏感。
EKF和UKF的关系我在后面会再展开聊。这里只说我的经验：EKF不是万能的，但绝大多数工程场景里，EKF的精度已经足够了。先把EKF吃透，再去碰UKF、粒子滤波这些进阶方法，事半功倍。
2. EKF的核心原理：泰勒展开、雅可比矩阵与线性化误差
2.1 一阶泰勒展开到底在做什么
EKF的思想一句话就能概括：既然系统是非线性的，那就用线性函数去近似它，而且只在当前估计点附近做局部近似。
假设非线性测量方程是 h(x)，它的真实函数图像可能是一条弯曲的曲线。我们在当前估计值 x_hat 处做一阶泰勒展开，本质上是用这条曲线在 x_hat 处的切线去近似曲线本身。只要真实状态离 x_hat 不远，切线近似和原曲线的差距就很小，线性化的效果就靠谱。
这个处理方式和我们初中物理学过的"局部线性化"是一模一样的思路——单摆在小角度下的简谐运动近似，就是典型的泰勒展开应用。
展开后的测量方程变成：
h(x) ≈ h(x_hat) + H * (x - x_hat)这里的 H 是 h(x) 对状态向量求偏导得到的雅可比矩阵。经过这一步，非线性测量方程就被转化成了关于 (x - x_hat) 的线性方程，可以塞进标准卡尔曼滤波的更新框架里了。
2.2 雅可比矩阵的手工推导方法
雅可比矩阵听起来高大上，其实就是多元函数的一阶偏导数矩阵。设状态向量是 n 维的，共有 m 个测量分量，那么雅可比矩阵 H 的维度是 m × n，其中第 i 行第 j 列的元素是第 i 个测量分量对第 j 个状态变量的偏导数。
以雷达测距测角跟踪为例，假设状态向量定义为 x = [px, py, vx, vy]^T，测量向量是 z = [distance, angle]^T。
测量方程为：
distance = sqrt(px^2 + py^2) angle = atan2(py, px)逐一求偏导：
∂distance/∂px = px / sqrt(px^2 + py^2) ∂distance/∂py = py / sqrt(px^2 + py^2) ∂distance/∂vx = 0 ∂distance/∂vy = 0 ∂angle/∂px = -py / (px^2 + py^2) ∂angle/∂py = px / (px^2 + py^2) ∂angle/∂vx = 0 ∂angle/∂vy = 0于是雅可比矩阵就是：
H = [ px/sqrt(px^2+py^2) py/sqrt(px^2+py^2) 0 0 ] [ -py/(px^2+py^2) px/(px^2+py^2) 0 0 ]注意这个矩阵里的 px, py 都是当前的状态估计值，也就是滤波过程中每个时刻都要用最新的估计值重新计算一遍雅可比矩阵，这在代码里体现为一个在循环里反复调用的函数。
矩阵形式的协方差传递与增益计算，和标准卡尔曼滤波完全一致，区别只是把公式里的 F 和 H 都换成了雅可比矩阵。
2.3 EKF的线性化误差从哪来
很多初学者会有个疑问：EKF既然是做了近似，那误差到底有多大？
误差的来源主要有两个方面。
第一，被忽略的高阶项。一阶泰勒展开丢掉了二阶及以上的项。如果非线性函数在展开点附近的曲率很大，或者状态估计误差很大导致展开点偏离真实状态很远，被忽略的高阶项就会带来显著的误差。
第二，误差传播的偏差。EKF用当前估计点做线性化，但真实状态是在估计点附近的一个分布。当这个分布范围比较大的时候，用一个点的切线代表整个分布区域内的函数行为，偏差就会显现出来。这就是为什么EKF在初始误差很大或者过程噪声很大的时候容易性能骤降甚至发散——本质上都是线性化点的代表性变差了。
我在实际调EKF的时候，最常遇到的发散场景就是初始协方差矩阵 P 设置得过小。如果 P 的初始值比真实误差小了几个数量级，滤波器会"过度自信"，增益算出来非常小，测量值几乎不被信任，估计结果就沿着预测一直跑偏。后面讲到代码时会专门演示这个坑。
3. 实例建模：带加速度扰动的匀速目标跟踪
3.1 状态方程与测量方程的定义
整个实操环节，我选目标跟踪作为演示场景。原因很简单：这是EKF最经典的应用领域，并且运动学模型和测量模型都好理解，推导雅可比的过程也不会让人劝退。
状态向量取 x = [px, py, vx, vy]^T，分别是目标在 x 轴位置、y 轴位置、x 轴速度、y 轴速度。采用匀速运动模型，采样周期为 dt。状态转移方程是线性的：
px_new = px + vx * dt py_new = py + vy * dt vx_new = vx vy_new = vy写成矩阵形式就是：
F = [ 1 0 dt 0 ] [ 0 1 0 dt ] [ 0 0 1 0 ] [ 0 0 0 1 ]实际系统中目标不可能严格匀速，总会有随机加速度扰动。我们把过程噪声建模为零均值高斯白噪声，加到速度分量上。过程噪声协方差矩阵 Q 的典型形式是：
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 设得越大，滤波器就越信任测量值；q 设得越小，滤波器越信任模型预测。这个值是整个系统里最需要根据实际场景手工调整的参数之一。
测量方程就是前面说的雷达极坐标模型，测量向量 z = [r, theta]^T，测量噪声协方差矩阵 R 反映雷达的测距和测角精度。
3.2 协方差矩阵Q、R的初值与物理意义
对于初学者，Q 和 R 的设置可能是最让人头大的部分。我的经验是要理解它们的物理意义，而不是机械地抄公式。
R 矩阵相对好设，因为雷达厂商一般会给出距离测量精度（比如标准差是1米）和角度测量精度（比如标准差是1度的弧度值），那么 R 就是这些精度值的平方组成的对角矩阵：
R = [ σr^2 0 ] [ 0 σθ^2 ]Q 矩阵就麻烦一点，因为过程噪声不是直接能测量的。通常的做法是先按经验给一个初始量级，然后观察滤波输出的平滑度和跟踪延迟。Q 设得偏大，估计轨迹会变得毛糙，噪声容易串进来；Q 设得偏小，轨迹会很平滑但跟踪滞后明显，目标转向的时候跟不上。
一个还算靠谱的调试顺序是：先把 R 设定为传感器的真实精度参数，然后逐步增大 q 值，观察滤波输出在"平滑"和"跟随"之间的权衡，找到一个折中点。代码里我会给出两组对比参数，让大家直观看到效果差异。
4. MATLAB实现：从矩阵搭建到滤波效果逐帧验证
4.1 代码结构与关键函数解读
MATLAB做算法验证确实是最快的，矩阵运算写起来非常自然，调试也方便。我先把核心代码贴出来，然后逐段解释。
仿真场景设置：目标从 (0, 0) 出发，初始速度是 (10 m/s, 5 m/s)，运动 100 步，采样周期 dt = 0.1s。模拟生成真实轨迹，再在真实轨迹上叠加高斯噪声作为雷达测量。
%% 参数设置 dt = 0.1; % 采样周期，单位秒 steps = 100; % 仿真步数 q = 1.0; % 过程噪声功率谱密度 r_range = 1.0; % 测距标准差，单位米 r_theta = 1 * pi / 180; % 测角标准差，单位弧度 % 状态转移矩阵 F = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; % 过程噪声协方差矩阵 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]; % 测量噪声协方差矩阵 R = diag([r_range^2, r_theta^2]); % 真实的运动轨迹 true_pos = zeros(2, steps); true_vel = [10; 5]; pos = [0; 0]; for k = 1:steps pos = pos + true_vel * dt; true_pos(:, k) = pos; end % 生成带噪声的测量值 meas = zeros(2, steps); for k = 1:steps px = true_pos(1, k); py = true_pos(2, k); r = sqrt(px^2 + py^2); theta = atan2(py, px); meas(1, k) = r + randn * r_range; meas(2, k) = theta + randn * r_theta; end4.2 EKF主循环的雅可比更新细节
EKF的主循环是核心中的核心。每一步要做两件事：预测（用状态转移方程推算先验）和更新（用测量修正得到后验）。
%% EKF主循环 x_hat = [0; 0; 10; 5]; % 初始状态估计 P = eye(4) * 10; % 初始协方差矩阵 ekf_pos = zeros(2, steps); ekf_pos(:, 1) = x_hat(1:2); for k = 2:steps % 预测 x_pred = F * x_hat; P_pred = F * P * F' + Q; % 计算测量雅可比矩阵 H，基于预测状态 px = x_pred(1); py = x_pred(2); r = sqrt(px^2 + py^2); H = [px/r py/r 0 0; -py/r^2 px/r^2 0 0]; % 预测测量值 z_pred = [r; atan2(py, px)]; % 更新 S = H * P_pred * H' + R; K = P_pred * H' / S; z_meas = meas(:, k); % 角度差需要做归一化，防止 -pi 和 pi 之间的跳变 innovation = z_meas - z_pred; innovation(2) = atan2(sin(innovation(2)), cos(innovation(2))); x_hat = x_pred + K * innovation; P = (eye(4) - K * H) * P_pred; % 保证协方差矩阵对称 P = 0.5 * (P + P'); ekf_pos(:, k) = x_hat(1:2); end这里有几个细节必须重点说明。
第一，测量雅可比 H 用的是预测状态 x_pred 处的值，不是上一时刻的后验状态。这是EKF的一个约定：卡尔曼增益的计算要基于最当前的先验估计。
第二，角度差归一化。atan2 的输出范围是 [-pi, pi]，如果目标恰好从第三象限跨到第二象限，真实角度从接近 -pi 变成接近 pi，直接相减会得到接近 2pi 的巨大差值，这个虚假的"大新息"会让滤波器产生剧烈抖动。解决办法是把差值重新映射回 [-pi, pi] 区间，用 sin/cos 归一化是最稳妥的写法。
第三，每一步更新完协方差矩阵之后，最好强制对称化。数值计算中 P 矩阵可能因为舍入误差稍微偏离对称，这会在后续迭代中被放大，导致滤波器不稳定。
4.3 运行结果分析与参数敏感度测试
跑完上面的代码，把真实轨迹、带噪观测折算成的坐标轨迹、EKF估计轨迹画在一起，能看到很明显的结果：直接测量换算出来的坐标轨迹毛刺很大，而EKF输出的轨迹基本贴着真实轨迹走。
这里我强烈建议读者做一个参数敏感性实验。把初始协方差 P 从 10 改成 0.1 再跑一遍，你会发现滤波初期出了一个大弯，要过好几步才收敛回来。原因就是 P 设小了，滤波器认为初始估计很准，Kalman增益很小，前几步几乎不信任测量，全靠模型预测硬撑——而模型初始速度恰好是有误差的，自然就带偏了。
如果把 q 从 1 改成 100，估计轨迹会变得抖动，但转向跟踪能力会提升。把这些参数都摆在一起调一遍，对EKF的理解会非常通透。
5. Python实现：用NumPy把EKF逻辑拆得更直观
5.1 面向对象的EKF类封装
Python的语法比MATLAB更接近自然语言，配合NumPy做矩阵运算，可读性非常好。我倾向于把EKF封装成一个类，因为这样逻辑分层清晰，后面如果要扩展到UKF、粒子滤波也方便对比。
先写一个基类框架：
import numpy as np class EKFFilter: def __init__(self, dt, q, r_range, r_theta): self.dt = dt self.F = np.array([ [1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1] ]) q_val = q self.Q = q_val * 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] ]) self.R = np.diag([r_range**2, r_theta**2]) self.x = np.zeros(4) self.P = np.eye(4) * 10 def reset(self, x0, P0): self.x = np.array(x0, dtype=float) self.P = np.array(P0, dtype=float) def predict(self): self.x = self.F @ self.x self.P = self.F @ self.P @ self.F.T + self.Q return self.x def jacobian_h(self, x_pred): px, py = x_pred[0], x_pred[1] r = np.sqrt(px**2 + py**2) H = np.array([ [px/r, py/r, 0, 0], [-py/(r**2), px/(r**2), 0, 0] ]) return H def h(self, x): return np.array([np.sqrt(x[0]**2 + x[1]**2), np.arctan2(x[1], x[0])]) def update(self, z): x_pred = self.predict() H = self.jacobian_h(x_pred) z_pred = self.h(x_pred) S = H @ self.P @ H.T + self.R K = self.P @ H.T @ np.linalg.inv(S) innovation = z - z_pred innovation[1] = np.arctan2(np.sin(innovation[1]), np.cos(innovation[1])) self.x = x_pred + K @ innovation self.P = (np.eye(4) - K @ H) @ self.P self.P = 0.5 * (self.P + self.P.T) return self.x把滤波逻辑和模型逻辑分开之后，代码非常直白。predict 做预测，update 做修正，jacobian_h 每次调用都基于最新的预测状态重新计算。这种写法在MATLAB里也能实现，但Python的类封装会让主程序的循环格外干净。
5.2 仿真数据生成与滤波精度指标
仿真数据的生成逻辑和MATLAB版本一致，这里不重复贴。我想重点讲的是滤波精度的量化评价。
很多人跑完EKF觉得"看起来差不多"就完事了，其实指标化评价非常必要，尤其是你要在多个算法之间做横向对比的时候。常用的评价指标有两个：
RMSE（均方根误差）最直观，反映的是估计值和真实值之间整体偏差的量级：
def rmse(est, true): return np.sqrt(np.mean((est - true)**2, axis=1))NEES（归一化估计误差平方）则用来评价滤波器的一致性，也就是滤波器对自己不确定度的估计是否诚实：
def nees(est, true, P_list): n = len(est) nees_val = 0.0 for i in range(n): diff = est[i] - true[i] nees_val += diff @ np.linalg.inv(P_list[i]) @ diff return nees_val / nNEES的值如果远大于状态维数，说明滤波器过度自信（协方差估小了）；如果远小于状态维数，说明滤波器过于保守（协方差估大了）。在调试阶段，NEES比RMSE更容易定位问题出在模型的哪个环节。
我在Python环境里对比过EKF和UKF的NEES表现。在测量模型这种适度非线性的场景下，两者差异并不大；但如果在初始化阶段状态误差很大，UKF因为Sigma点能更好捕捉分布形状，收敛速度会快一些。这也是为什么很多开源导航库同时提供EKF和UKF两套实现。
5.3 Python调试中的几个坑
用Python写EKF，最常见的坑有三个。
第一个是矩阵维度不匹配。NumPy中一维数组和二维行向量/列向量的行为很容易搞混。比如 x_pred = self.F @ self.x 如果 self.x 是 shape (4,) 的一维数组，结果也是 (4,) 的；但如果你在某些地方不小心把它变成了 shape (4,1)，后面的矩阵运算就可能出现广播错误。建议在初始化时就明确用列向量，并定期 print 每个中间变量的 shape 来排查。
第二个是 np.linalg.inv 在大矩阵时性能不佳。EKF状态维度一般不高，用 inv 没问题；但如果状态扩展到十几维以上，建议改成 np.linalg.solve 解线性方程组，gains 计算更高效也更数值稳定。
第三个是随机种子。仿真实验一定要固定 np.random.seed，否则每次跑出来的测量噪声不同，算法对比的结果就没有可重复性。这是做研究型工作最容易忽略的一步。
6. C++实现：从线性代数库选型到工程化落地
6.1 Eigen库的选型理由与代码结构
到了C++这一层，场景完全不一样了。MATLAB和Python适合算法验证，但真要放到自动驾驶、无人机飞控或者嵌入式设备里跑，性能是硬指标。C++的EKF实现重点在两个事情：一是矩阵运算库的选择，二是代码结构的组织。
矩阵库我首选Eigen。原因总结一下：它是纯头文件库，不需要编译安装，直接 include 就能用；API设计接近MATLAB，上手成本低；支持表达式模板，编译期优化后性能极好。Armadillo也很优秀，风格更像MATLAB，但需要链接额外库，在嵌入式交叉编译场景下麻烦一些。
EKF类在C++里的核心数据结构是这样：
#include <Eigen/Dense> using Eigen::MatrixXd; using Eigen::VectorXd; class EKF { public: EKF(double dt, double q, double r_range, double r_theta); void reset(const VectorXd &x0, const MatrixXd &P0); void predict(); void update(const VectorXd &z); VectorXd getState() const { return x_; } MatrixXd getCovariance() const { return P_; } private: VectorXd h(const VectorXd &x) const; MatrixXd jacobianH(const VectorXd &x) const; double dt_; MatrixXd F_; MatrixXd Q_; MatrixXd R_; VectorXd x_; MatrixXd P_; };这个结构把模型参数（F、Q、R）和算法状态（x、P）都收进类里。如果你要在项目中复用，只需要替换 h 和 jacobianH 两个私有函数，就能适配完全不同的系统模型，算法主体不需要动。
6.2 关键实现细节：角度归一化与数值稳定
C++实现里最需要小心的就是测量方程和雅可比矩阵的计算，因为C++没有MATLAB那么方便的向量化语法，所有偏导数都得明确写出来。
VectorXd EKF::h(const VectorXd &x) const { double px = x(0); double py = x(1); VectorXd z(2); z(0) = std::sqrt(px*px + py*py); z(1) = std::atan2(py, px); return z; } MatrixXd EKF::jacobianH(const VectorXd &x) const { double px = x(0); double py = x(1); double r = std::sqrt(px*px + py*py); double r2 = r * r; MatrixXd H(2, 4); H << px/r, py/r, 0, 0, -py/r2, px/r2, 0, 0; return H; }C++里尤其要注意角度归一化的写法，和Python/MATLAB类似：
double innovation_angle = z(1) - z_pred(1); innovation_angle = std::atan2(std::sin(innovation_angle), std::cos(innovation_angle)); innovation(1) = innovation_angle;如果忘了这一步，当目标环绕原点运动、角度从 pi 跳到 -pi 的时候，滤波输出会出现肉眼可见的尖刺。这个问题在MATLAB里也会出现，但因为MATLAB的坐标绘图往往能自动处理角度回绕，初学者反而不容易注意到。
另一个工程细节是数值稳定性。矩阵运算在浮点数层面会出现舍入误差，协方差矩阵 P 可能在长时运行后不再保持对称正定。业内常见的做法是定期对 P 做对称化处理，或者用 Joseph form 的协方差更新公式：
MatrixXd I_KH = MatrixXd::Identity(4, 4) - K * H; P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose();Joseph形式在计算上比标准的 P = (I - KH)P_ 贵一点，但它能更好地维持协方差矩阵的对称正定性。如果你的EKF要长时间不间断运行，建议直接用Joseph形式。
6.3 实时性对比与嵌入式移植注意事项
聊一下性能。以这个四维状态、二维测量的EKF为例，在普通PC上单次预测+更新大概在几十微秒级别（Eigen的表达式模板高度优化，编译器开O2后几乎零开销）。在Cortex-M4这类单片机上，没有硬件浮点单元的话，单次运行大概在几百微秒到1毫秒之间，足以跑到100Hz以上的控制频率。
嵌入式移植有几点建议。首选，关闭异常和RTTI，减少代码体积。其次，Eigen的很多高级特性在嵌入式裸机环境下不适用，建议固定矩阵维度，不要用动态大小的 MatrixXd——静态矩阵 Matrix<double, 4, 4> 的运算不会触发堆内存分配，实时性更可预期。第三，检查目标平台是否支持 double，有些低成本单片机 double 和 float 是一样精度的，设计Q、R参数时要考虑这个精度损失。
7. 三语言实现横向对比与调试经验总结
7.1 不同语言写EKF的差异对比
三套代码跑下来，我做个横向对比，供不同需求的读者参考：
| 对比维度 | MATLAB | Python | C++ |
|---|---|---|---|
| 代码可读性 | 较高，矩阵运算语法直接 | 最高，类封装清晰 | 一般，模板语法稍多 |
| 开发效率 | 高，内置绘图方便 | 高，NumPy + matplotlib | 低，需要手动管理一切 |
| 运行速度 | 中等 | 中等偏慢（NumPy有开销） | 最快 |
| 部署能力 | 不适合产品化 | 可跑原型，可和ROS集成 | 工业级首选 |
| 学习门槛 | 低 | 低 | 较高 |
| 适用场景 | 算法研究、快速验证 | 算法研究、数据处理 | 自动驾驶、飞控、嵌入式 |
我的建议是这样的链路：在MATLAB里做算法原型验证，确认模型和参数；然后迁到Python里做更灵活的实验和可视化；最后落地到C++做产品化部署。每一步之间的迁移成本都不高，因为核心的数学逻辑完全一致，只是语法表达不同。
7.2 我自己调试EKF时按什么顺序排查问题
EKF跑出来结果不对，很多人第一反应是动手调Q、R，其实大部分情况根本不是参数问题，而是代码模型问题。我按自己踩坑的经验，给出一个排查优先级：
第一优先级是检查雅可比矩阵。这是EKF最容易出错的地方，而且错得隐蔽。验证方法是数值微分对比：用 (f(x+epsilon) - f(x-epsilon)) / (2*epsilon) 近似求偏导数，和解析求出的矩阵逐项对比，误差在1e-6量级说明解析结果是正确的。
第二优先级是检查量纲和单位。角度用弧度还是度，速度单位是m/s还是km/h，这些一旦混了，滤波器表现会非常诡异——表现为某一维度误差特别大但其他维度正常。
第三优先级是检查角度回绕处理。凡是涉及atan2或者角度测量的系统，这一步没有做好，滤波输出会在特定运动区域出现尖刺。
第四优先级才是调Q、R。如果以上都没问题，EEF还是发散，再从物理意义上调整过程噪声和测量噪声的取值。
7.3 EKF、UKF、粒子滤波怎么选
最后聊几句算法选型。很多初学者会纠结要不要直接上UKF甚至粒子滤波。我的观点是：一切以系统非线性程度和精度需求为准。
如果你的测量方程只是像雷达测距测角这种"温和非线性"——雅可比矩阵变化平缓，没有强烈的分段特性——EKF的精度已经足够，没必要花多余算力。
如果你的系统有强非线性，比如大角度姿态变化、大曲率运动轨迹，或者观测方程含有三角函数的高次组合，UKF的统计近似会比EKF的一阶线性化更稳健。
粒子滤波一般只在非高斯噪声、多模态分布场景下才会考虑，比如复杂环境中的定位。它的计算代价比EKF高出几个数量级，工程上要慎用。
我给一个快速选择的判断标准：如果EKF调试中反复出现因为线性化误差导致的发散问题，并且你已经确认雅可比矩阵是对的，此时才考虑升级到UKF。否则，EKF就是最平衡的选择。
我在实际项目中的体会是，EKF真正难的不是算法本身——核心公式就那么几条——而是你对系统模型的理解。雅可比矩阵的每一项偏导数是不是写对了，Q/R参数是否反映了真实物理过程的噪声特性，这些才是决定滤波效果的关键。把模型吃透了，用什么语言实现只是表达形式的区别。希望这篇把EKF的原理和三语言实现讲清楚的文章，能让你少走一些我当年走过的弯路。
