★ FEATURED ARTICLE 1. 先从磁针信号说起为什么直接算角度不可靠1.1 磁力计的测量链路如果你做过四旋翼、两轮平衡小车或者室内机器人手上多半有一颗磁力计——也有人叫它磁针、电子罗盘、地磁传感器。它测的是地球磁场在传感器三个轴上的分量理论上拿着它的 x、y 轴读数做一个 atan2 就能得到航向角θ atan2(my, mx)可真正接上采样数据之后你会发现这个角度根本没法直接用。最明显的问题是读数一路抖而且抖得毫无规律。传感器本身有白噪声电机转动会带来磁场变化附近有铁磁性材料时还会叠加一个几乎固定的偏置磁场温度漂移也在里面掺和。你做一个滑动平均或者低通滤波确实能压住一部分抖动但低通滤波的本质是滞后动态响应会变得很迟钝小车一转快角度的输出就跟不上了。1.2 从单点的 atan2到递推的状态估计这才是问题的核心手里只有磁力计这一个传感器量到的又是非线性的三角函数关系噪声还不小但你又希望拿到一条平滑、实时、可预测的角度轨迹。单点转换做不到因为它没有过去和未来的概念。所以要做的是状态估计把航向角 θ、角速度 ω 之类的变量当成系统内部状态用运动学方程去预测再用每一时刻的观测值去修正。这个框架下角度不是直接从 atan2 暴力解出来的而是在一整套概率模型里被估出来的带有协方差信息也天然考虑了测量噪声和运动不确定性。标准卡尔曼滤波只能用在线性高斯系统上可磁针观测方程里有 cos 和 sin必须是 EKF扩展卡尔曼滤波或者 UKF无迹卡尔曼滤波这种能处理非线性的变体上场。这篇文章就把我在仿真和实际板子上跑过的完整做法摊开来说怎么建模、EKF 和 UKF 分别怎么写、Matlab 代码骨架是什么、踩了哪些坑。2. 状态模型怎么建矩阵怎么定、噪声从哪里来2.1 状态向量和状态转移矩阵在做任何滤波之前先把系统的状态空间表达式写清楚。磁针系统里最常用的状态向量是x_k [θ_k, ω_k]^T其中 θ 是航向角ω 是角速度。角速度不是传感器直接给出来的而是状态模型里运动规律自带的隐变量。有了 ω即使磁力计连续若干拍没有有效更新滤波器也能靠运动方程把角度的先验估计推下去。状态转移方程取常见的匀速旋转离散模型θ_{k1} θ_k ω_k · Δt w_1 ω_{k1} ω_k w_2写成矩阵就是x_{k1} F · x_k w_kF [1, Δt; 0, 1]这个 F 是长这样的Δt 是采样周期一般取 10ms 到 100ms。如果你实际面对的是一辆经常会加减速的小车可以把 F 扩展成带角加速度的形式或者把角加速度建模为过程噪声里的有色分量。不过在两维状态已经够用的情况下刻意加维度反而会让滤波器更容易过拟合噪声所以先从这个最紧凑的模型开始比较明智。2.2 观测方程为什么带着三角函数磁力计的输出模型写成z_k h(x_k) v_k其中 z_k [mx; my]是水平面两个轴上的磁场强度测量值。如果磁场模长基本固定观测函数是h(x_k) [A·cos(θ_k); A·sin(θ_k)]A 是当地磁场在水平面的投影强度。实际工程里A不常精确知道一个很实用的处理是先用静止数据做归一化。把 A 吸收到观测里归一化之后它可以近似为常数 1不过要注意在磁场受干扰的场景里 A 会漂移后面我会专门讲这种情况怎么办。这里也要强调一点为什么观测用 cos/sin 而不直接用角度因为它有两个好处。第一cos 和 sin 是连续光滑函数在 π 和 -π 边界处没有跳变避免了角度环绕wrap带来的误差爆炸问题。第二这种表示利用了 x、y 两轴的信息观测噪声也更接近各向同性滤波器的数学结构干净很多。2.3 两个噪声矩阵的物理含义状态方程里的过程噪声 w_k 和观测方程里的测量噪声 v_k分别对应协方差矩阵 Q 和 R。Q 反映的是你有多相信状态转移模型。角速度不是恒定的模型说上一拍的角速度继续保持不变但实际有小车加减速、外力扰动这些不确定性都要折算进 Q。Q 给得越小滤波器越固执倾向于相信模型、不相信观测Q 给得越大滤波器越灵活但估计结果也会跟着观测噪声一起去抖。R 则直接对应磁力计的量测噪声水平。你可以这样标定让传感器静止放在桌面上采 1000 个 mx、my 样本算出标准差R 的对角线就用这个方差的量级。实测下来R 定得和真实噪声匹配时滤波器的稳态误差最小R 被低估时状态估计会出现明显的高频抖动。在 Matlab 里初始定义可以这样写dt 0.01; F [1, dt; 0, 1]; Q diag([0.01, 0.1]); % 过程噪声 R diag([0.02, 0.02]); % 磁力计测量噪声 P0 diag([0.1, 0.1]); % 初始协方差 x0 [0; 0]; % 初始状态注意 P0 别给得太小否则滤波器会过早自以为是如果初值又给得不对收敛过程会非常痛苦。3. EKF实现拿着雅可比走入线性化的世界3.1 每一轮递推做四件事EKF 的思路很简单粗暴既然系统是非线性的那就把非线性函数在当前估计点做一阶泰勒展开得到雅可比矩阵然后完全照搬标准卡尔曼滤波的预测 更新骨架。每一轮递推其实就做四件事用 F 对状态做一步预测用 F 对协方差 P 做一步传播计算观测函数 h(x) 在预测点的雅可比 H_k用 H_k 计算卡尔曼增益执行状态和协方差的更新。这个流程听着平平无奇但每一步都有陷阱。第三点的雅可比求错整个滤波器可能在几拍之后就发散了第四点的创新量innovation如果角度环绕处理不好误差会被算成 2π 的量级直接把状态修正打歪。3.2 磁针场景的雅可比矩阵推导我们用的观测函数是h(x) [A·cos(θ); A·sin(θ)]对状态向量 x [θ, ω] 求偏导得到雅可比H_k ∂h/∂x [-A·sin(θ), 0; A·cos(θ), 0]这个矩阵看着简单但它是 EKF 全部线性化近似的浓缩。注意第二列是 0说明在观测方程里角速度 ω 不直接参与测量——只能通过状态转移影响 θ 的预测值。这意味着如果角速度初值或过程噪声设置不对EKF 观测更新的频率和幅度会受到很大限制。在 Matlab 里实现时一个细节值得注意计算 H_k 用的 θ 应该是预测值 x_pred(1)而不是上一次更新后的估计值。不少人在这里写成上一拍的 θ_est导致雅可比始终晚一步相位滞后就出来了。function H ekf_JacobianH(theta, A) H [-A * sin(theta), 0; A * cos(theta), 0]; end3.3 EKF的Matlab核心代码下面给一个完整的 EKF 主循环骨架可以直接在仿真脚本里跑x_est x0; P P0; for k 1:N % 预测 x_pred F * x_est; P_pred F * P * F Q; % 雅可比 theta_pred x_pred(1); H [-A * sin(theta_pred), 0; A * cos(theta_pred), 0]; % 观测预测 z_pred [A * cos(theta_pred); A * sin(theta_pred)]; % 卡尔曼增益 S H * P_pred * H R; K P_pred * H / S; % 更新 innovation z_meas(:, k) - z_pred; x_est x_pred K * innovation; P (eye(2) - K * H) * P_pred; end用这个结构跑出来的结果在噪声水平适中、运动不是特别剧烈的场景下角度估计已经相当能打。但我一开始直接把这套代码放上实木板时发现效果并没有仿真里那么漂亮——原因是 EKF 的一阶线性化在真值附近还算准一旦状态误差比较大或者磁干扰造成的误差远离线性化点雅可比导出的增益方向就不对了滤波器会明显变迟钝。这也是为什么后来我更倾向于用 UKF至少在某些工况下它可以少操很多线性化出偏的心。4. UKF实现不做泰勒展开的Sigma点方案4.1 为什么可以绕过雅可比UKF 的出发点完全不一样我不去对非线性函数求导而是用一个精心挑选的 Sigma 点集合来采样状态的分布让这些点经过非线性函数传播之后再统计得到均值、协方差的近似值。这句话听起来抽象落到实际操作上就一句话一个点过非线性函数以后是什么样你直接把它代进去就可以了不需要展开、不需要导数、也不关心函数平不平滑。这正是它比 EKF 省心的地方——EKF 里每次 Jacobian 更新算错一个小符号整个滤波器就废掉了而 UKF 里这个环节被彻底绕开了。对磁针系统来说还有个实质性好处当角度误差离真值比较远时EKF 的第一阶泰勒近似会严重失真UKF 的 Sigma 点因为本身散布在状态均值周围能够感知到观测非线性函数的实际弯曲程度相当于在局部多采了几个点去拟合非线性鲁棒性会好一截。4.2 Sigma点与权重设置面对二维状态向量 xUKF 一共需要 2n1 5 个 Sigma 点。生成规则是χ_0 x̄ χ_i x̄ sqrt((n λ) · P) 的第 i 列i 1, ..., n χ_{ni} x̄ - sqrt((n λ) · P) 的第 i 列其中 λ α²(nκ) - n。α 控制 Sigma 点到均值的距离一般取 1e-3 到 1κ 通常取 0 或者 3-nβ 用于融入状态的先验分布信息高斯分布时取 2。对应权重W_0^m λ / (n λ) W_0^c λ / (n λ) (1 - α² β) W_i^m W_i^c 1 / (2(n λ)), i 1, ..., 2n这里最容易糊涂的是 W_0^m 和 W_0^c 不一样一个用于均值合成一个用于协方差合成。不少人直接把两个权重用同一个数协方差传播的精度会打折扣。4.3 UKF核心代码与参数经验Matlab 里生成 Sigma 点可以用下面这种写法function [X_sp, wm, wc] ut_sigma_points(x, P, alpha, kappa, beta) n length(x); lambda alpha^2 * (n kappa) - n; [U, S, ~] svd(P); P_sqrt U * diag(sqrt(diag(S))) * U; % 也可以用 chol(P) X_sp zeros(n, 2*n1); X_sp(:, 1) x; for i 1:n X_sp(:, i1) x sqrt(n lambda) * P_sqrt(:, i); X_sp(:, ni1) x - sqrt(n lambda) * P_sqrt(:, i); end wm zeros(1, 2*n1); wc zeros(1, 2*n1); wm(1) lambda / (n lambda); wc(1) lambda / (n lambda) (1 - alpha^2 beta); for i 2:2*n1 wm(i) 1 / (2 * (n lambda)); wc(i) wm(i); end end求平方根的时候我其实更推荐 svd 而不是 chol因为 P 在半途可能出现微小的非正定情况chol 会直接报错svd 至少能兜底算出个正定的主方向。这个细节在长时间跑滤波器的时候会救你一命。UKF 主循环for k 1:N % 生成Sigma点 [X_sp, wm, wc] ut_sigma_points(x_est, P, 1e-3, 0, 2); % 状态传播 X_pred zeros(2, 2*21); for i 1:5 X_pred(:, i) F * X_sp(:, i); end x_pred sum(wm .* X_pred, 2); P_pred Q; for i 1:5 dX X_pred(:, i) - x_pred; P_pred P_pred wc(i) * (dX * dX); end % 观测传播 Z_pred zeros(2, 5); for i 1:5 theta_i X_pred(1, i); Z_pred(:, i) [A * cos(theta_i); A * sin(theta_i)]; end z_pred sum(wm .* Z_pred, 2); % 协方差 P_zz R; P_xz zeros(2, 2); for i 1:5 dZ Z_pred(:, i) - z_pred; dX X_pred(:, i) - x_pred; P_zz P_zz wc(i) * (dZ * dZ); P_xz P_xz wc(i) * (dX * dZ); end % 更新 K P_xz / P_zz; x_est x_pred K * (z_meas(:, k) - z_pred); P P_pred - K * P_zz * K; end注意这里状态传播用的是 F是因为我们建立的模型本身就是线性的状态转移如果哪天把状态转移也改成非线性的同样把每个 Sigma 点丢进 f(x) 算一遍就行逻辑完全一致。这也是 UKF 让人省心的地方——算法层面一视同仁地对待状态转移和观测函数。5. 同一套磁针数据下的实测对比与结果解读5.1 仿真设置与评价指标为了公平对比我在仿真里造了一条包含匀速旋转、短暂加减速和静止三种工况的航向轨迹真实角速度在 -2 rad/s 到 2 rad/s 之间变化。磁力计观测按 h(x) 生成后叠加高斯白噪声噪声标准差设成 0.15这个水平在真实板子上并不夸张——低成本磁力计静止时的噪声就能到这个量级。评价指标我用三条估计误差的均方根误差RMSE、达到稳态的时间、单步平均耗时。5.2 三组数据对比以下是同一批 2000 步仿真数据下的统计结果指标EKFUKF角度 RMSE (rad)0.0720.061角速度 RMSE (rad/s)0.240.19收敛时间从初值偏差 30° 开始约 0.6 s约 0.35 s单步平均耗时0.09 ms0.21 ms看到这个表我一开始也有点意外UKF 在每个指标上都比 EKF 表现好一点尤其是在初始误差较大、运动出现突变的片段里UKF 的角度误差峰值明显更低——最坏情况大约能压低 40%。原因其实不难理解。EKF 在每次更新时只利用真值附近的一阶导数信息而 UKF 用 5 个 Sigma 点覆盖了一个局部范围对这个非线性观测函数做了一个更接近真实的积分近似。磁针系统的观测函数是圆上的三角函数局部曲率变化又不是恒定值UKF 这种多点采样的思路天然占优。但代价也很明显UKF 单步耗时约是 EKF 的 2 到 3 倍。在 100 Hz 的控制循环里两者都远远够用差距无关痛痒。可如果你要在 MCU 上跑更高频率、或者状态维度扩展到 5 维以上Sigma 点数会从 2n1 快速增长UKF 的计算开销就需要认真掂量了。5.3 结果说明了什么我在多次调整噪声参数之后发现一个规律当观测噪声很大标准差 0.2或者系统有较强的外扰时EKF 和 UKF 的差距会更明显。因为此时滤波器的增益往往被噪声拖得偏小状态预测对观测更新的依赖变强EKF 线性化误差就容易被放大而 UKF 的 Sigma 点分布本身就考虑了状态的不确定性规模近似能力在不确定性大的情况下反而更稳。反过来如果观测噪声很小、运动又非常温和你可以把 R 压得很低此时滤波器主要靠观测修正两条算法的差距会被压缩到几乎看不出来。所以如果你只是做一个静止或者缓慢转动的演示EKF 完全够用但要做的是一个经常变速、转向、受扰的真实运动系统我更推荐直接从 UKF 起步。6. 真正会翻车的那些细节初值、噪声矩阵与磁干扰6.1 初值给错了会怎样仿真里 x0 总是设在真值附近实际跑的时候根本没有这种待遇。我试过把初始 θ 故意设到与真实航向相反的方向——例如真实角度 30°初值给 150°。EKF 在这种起步条件下会有一段明显的收敛爬坡而且因为观测函数是 cos/sin航向相反时 mx、my 的符号整体翻转Kalman 增益一开始还会把状态往错误方向拽。UKF 的情况好一些因为 5 个 Sigma 点覆盖了更大的范围对非线性函数的感知更宽但从 150° 拉回 30° 也需要一定时间。我的建议是如果系统上电时有可靠的角度基准就用那个基准没有的话就先把 P0 对角线加大到 10 甚至 100 的量级强迫滤波器在刚开始的几拍里保持开放状态不要过早收敛到某个错误方向。这个过程类似于把初值不确定性放得很大让观测在前期占主导地位。6.2 Q和R矩阵的调参手感Q 和 R 的比例关系决定了滤波器的行为骨架。两个我重复过很多次的规律固定 R、放大 Q相对值状态估计会变得更跟手但输出噪声变大固定 Q、放大 R相对值状态估计会变得更平滑但对真实运动的跟踪滞后明显。一个实用的操作路径是先把 R 按传感器静止标定值固定住再在仿真里逐步增大 Q 直到 RMSE 不再明显下降为止。如果估计轨迹还有明显拖动感说明 Q 偏小如果输出噪声已经快要和原始量测一样大了说明 Q 给得过高。很多论文里给出了一大套调参理论但工程上就是这样一个二分搜索的过程没有必要一开始就求什么全局最优。另外Q 和 R 不一定要设成对角相等。磁力计两个轴如果标定后有残余增益差R 就不该设成相同值分别估计才是正解。6.3 磁干扰下的行为与扩展状态这是所有做磁针系统的人都躲不掉的环节。电机启动、金属外壳靠近、附近有大电流导线时磁场叠加了一个几乎恒定的额外分量相当于观测方程里多了一个偏置向量 bz_k [A·cos(θ_k) bx; A·sin(θ_k) by] v_k如果你不知道这个 b 的存在滤波器会把这个偏置强行解释成角度偏差导致航向估计出现一个固定的偏移角。这是我在实板测试上踩得最惨的一坑仿真里滤波器漂亮的 RMSE一装到带电机的小车上就直接偏了 15°。解决办法有两个方向。轻量做法是每次开机后做五分钟静止标定估计出当前环境下的 b 并在观测前扣除更彻底的做法是把 b 扩进状态向量变成x_k [θ_k, ω_k, bx_k, by_k]^T这样滤波器就会在航向估计的同时在线辨识磁场偏置即使环境中慢慢出现新的干扰源它也能缓慢追踪。代价是状态维度从 2 涨到 4Kalman 增益矩阵的计算量上一个台阶但对 UKF 来说 5 维以内的 Sigma 点开销仍然可以接受。我在一次做室内小车导航时用的就是 4 维扩展状态UKF 在偏置慢慢变化例如驱动电流逐步加大的情况下能自动把航向误差拉回 3° 以内而普通 EKF 不加偏置状态的表现是直接偏着走。所以如果你的工作环境里磁场干扰不可控强烈建议直接用扩展状态方案。从我自己跑了这么多轮的体会来说状态估计最终比的不只是算法公式而是建模时有没有把系统的实际不确定性纳进来。磁针系统低维、非线性、噪声凶、干扰多恰好是理解 EKF 和 UKF 区别的一个非常典型的试验台。无论你先选哪一个把模型矩阵写清楚、把噪声标定做好、把干扰当回事这些功夫永远不会白费。
阅读完成 · 觉得有帮助？
