企业官网建设流程全解析

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

其中 θ 是航向角，ω 是角速度。角速度不是传感器直接给出来的，而是状态模型里运动规律自带的隐变量。有了 ω，即使磁力计连续若干拍没有有效更新，滤波器也能靠运动方程把角度的先验估计推下去。

状态转移方程取常见的匀速旋转离散模型：

θ_{k+1} = θ_k + ω_k · Δt + w_1 ω_{k+1} = ω_k + w_2

写成矩阵就是：

x_{k+1} = F · x_k + w_k

F = [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 的思路很简单粗暴：既然系统是非线性的，那就把非线性函数在当前估计点做一阶泰勒展开，得到雅可比矩阵，然后完全照搬标准卡尔曼滤波的"预测 + 更新"骨架。

每一轮递推其实就做四件事：

1. 用 F 对状态做一步预测；
2. 用 F 对协方差 P 做一步传播；
3. 计算观测函数 h(x) 在预测点的雅可比 H_k；
4. 用 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]; end

3.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点与权重设置

面对二维状态向量 x，UKF 一共需要 2n+1 = 5 个 Sigma 点。生成规则是：

χ_0 = x̄ χ_i = x̄ + sqrt((n + λ) · P) 的第 i 列，i = 1, ..., n χ_{n+i} = 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*n+1); X_sp(:, 1) = x; for i = 1:n X_sp(:, i+1) = x + sqrt(n + lambda) * P_sqrt(:, i); X_sp(:, n+i+1) = x - sqrt(n + lambda) * P_sqrt(:, i); end wm = zeros(1, 2*n+1); wc = zeros(1, 2*n+1); wm(1) = lambda / (n + lambda); wc(1) = lambda / (n + lambda) + (1 - alpha^2 + beta); for i = 2:2*n+1 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*2+1); 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 步仿真数据下的统计结果：

看到这个表我一开始也有点意外：UKF 在每个指标上都比 EKF 表现好一点，尤其是在初始误差较大、运动出现突变的片段里，UKF 的角度误差峰值明显更低——最坏情况大约能压低 40%。

原因其实不难理解。EKF 在每次更新时只利用真值附近的一阶导数信息，而 UKF 用 5 个 Sigma 点覆盖了一个局部范围，对这个非线性观测函数做了一个更接近真实的积分近似。磁针系统的观测函数是圆上的三角函数，局部曲率变化又不是恒定值，UKF 这种"多点采样"的思路天然占优。

但代价也很明显：UKF 单步耗时约是 EKF 的 2 到 3 倍。在 100 Hz 的控制循环里，两者都远远够用，差距无关痛痒。可如果你要在 MCU 上跑更高频率、或者状态维度扩展到 5 维以上，Sigma 点数会从 2n+1 快速增长，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 磁干扰下的行为与扩展状态

这是所有做磁针系统的人都躲不掉的环节。电机启动、金属外壳靠近、附近有大电流导线时，磁场叠加了一个几乎恒定的额外分量，相当于观测方程里多了一个偏置向量 b：

z_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 涨到 4，Kalman 增益矩阵的计算量上一个台阶，但对 UKF 来说 5 维以内的 Sigma 点开销仍然可以接受。

我在一次做室内小车导航时用的就是 4 维扩展状态，UKF 在偏置慢慢变化（例如驱动电流逐步加大）的情况下，能自动把航向误差拉回 3° 以内，而普通 EKF 不加偏置状态的表现是直接偏着走。所以如果你的工作环境里磁场干扰不可控，强烈建议直接用扩展状态方案。

从我自己跑了这么多轮的体会来说，状态估计最终比的不只是算法公式，而是建模时有没有把系统的实际不确定性纳进来。磁针系统低维、非线性、噪声凶、干扰多，恰好是理解 EKF 和 UKF 区别的一个非常典型的试验台。无论你先选哪一个，把模型矩阵写清楚、把噪声标定做好、把干扰当回事，这些功夫永远不会白费。