简介：这份资源是《传感器信息融合——MATLAB程序实现》的随书程序包，面向学习多传感器数据融合的本科生、研究生及工程技术人员，帮助读者把卡尔曼滤波、粒子滤波、贝叶斯估计等理论落到可运行的代码上。压缩包共40个文件，以34个m脚本为主体，覆盖数据预处理、传感器建模、融合算法实现与结果评估等环节，另含jpg、tif、bmp图像素材，用于图像融合实验的输入与对比展示，整体约443KB，轻量便于本地调试。目前已有3458人学习下载，说明其在教学与自学场景中具备一定参考价值。读者可借助这些脚本理解Sobel边缘检测、小波分解、区域方差匹配等具体实现思路，对照随书章节复现实验流程，并在此基础上调整参数、替换数据集，迁移到自动驾驶、无人机导航或工业自动化等实际项目中。
1. 从一堆跳变的读数说起：这套 MATLAB 传感器融合程序到底能干什么
做过嵌入式或者导航算法的朋友大概都遇到过这种场景：加速度计单独看还行，陀螺仪积分一会儿就飘，磁力计在电机旁边直接发疯，GPS 在隧道里丢星。你手里有三四个传感器，每个都在输出数据，但每个都不可信。这时候真正要解决的问题不是「怎么再买一个更贵的传感器」，而是「怎么把这几路互相矛盾的数据揉成一个能用的估计值」。传感器信息融合要干的就是这件事，而这套 MATLAB 程序实现，就是把这套理论从公式变成能跑、能改、能看结果的代码。
它适合三类人：正在做毕设或课程设计、需要一套能跑通的融合框架的学生；刚转做导航、姿态估计、组合导航，想先拿仿真把卡尔曼滤波吃透的工程师；以及手上有真实传感器数据、想先用 MATLAB 验证算法再移植到嵌入式的开发者。整套程序围绕 MATLAB 展开，核心是滤波与状态估计，不依赖特定硬件，你拿自己的数据也能替换进去跑。
2. 融合的骨架：状态方程、观测方程与卡尔曼滤波的落地写法
2.1 为什么选卡尔曼滤波而不是直接加权平均
很多人第一反应是「加权平均不就完了」，给陀螺仪 0.7、加速度计 0.3，凑一个输出。这种做法在静态、线性、噪声稳定的场景下确实能糊弄过去，但只要传感器特性一变、或者系统本身有动态过程，权重就没法自适应了。卡尔曼滤波的价值在于它把「系统怎么演化」和「传感器怎么观测」分开建模，用协方差矩阵动态地决定每一时刻该信谁多一点。陀螺仪短期准、长期飘，加速度计长期稳、短期抖，卡尔曼滤波恰好能利用这个互补特性：短期信陀螺，长期用加速度计把漂移拉回来。
这套程序里，状态量通常取姿态角（或四元数）加陀螺零偏，观测来自加速度计解算的倾角和磁力计解算的航向。状态方程描述的是「上一时刻姿态 + 陀螺积分 - 零偏影响」，观测方程描述的是「当前姿态投影到重力/地磁方向应该得到什么读数」。理解这两条方程，比背卡尔曼五个公式重要得多。
2.2 状态方程与观测方程的代码骨架
下面这段是典型的离散卡尔曼预测与更新骨架，姿态用四元数表示，状态向量取 7 维（四元数 4 + 零偏 3）。实际程序里矩阵维度会按你的传感器数量调整，但结构一致。
% 状态: x = [q0 q1 q2 q3 bx by bz]' 四元数 + 陀螺零偏 % 预测步: 用陀螺角速度推进四元数 function x_pred = predict_state(x, omega, dt) q = x(1:4); b = x(5:7); w = omega - b; % 去掉零偏后的真实角速度 % 四元数微分: q_dot = 0.5 * q ⊗ [0 w] Omega = [0, -w'; w, -skew(w)]; q_dot = 0.5 * Omega * q; q_pred = q + q_dot * dt; q_pred = q_pred / norm(q_pred); % 必须归一化，否则会漂 x_pred = [q_pred; b]; % 零偏按随机游走建模，预测不变 end % 更新步: 用加速度计观测重力方向做校正 function [x_upd, P_upd] = update_acc(x_pred, P_pred, acc, R) g_ref = [0; 0; 1]; % 参考重力方向(导航系) q = x_pred(1:4); g_pred = quat2dcm(q)' * g_ref; % 预测的机体系重力方向 y = acc / norm(acc) - g_pred; % 观测残差 H = jacobian_acc(x_pred); % 观测矩阵，对四元数求偏导 S = H * P_pred * H' + R; K = P_pred * H' / S; % 卡尔曼增益 x_upd = x_pred + K * y; x_upd(1:4) = x_upd(1:4) / norm(x_upd(1:4)); P_upd = (eye(7) - K * H) * P_pred; end逻辑上分两步：predict_state负责用陀螺把状态往前推，update_acc负责用加速度计把预测拉回真实。参数里dt是采样周期，必须和你的传感器实际输出频率一致，写错这个值结果会整体偏慢或偏快；R是观测噪声协方差，加速度计越抖，R对角线就该给越大，滤波才会更信陀螺。skew(w)是反对称矩阵函数，quat2dcm是四元数转方向余弦矩阵，MATLAB 的 Aerospace Toolbox 里有现成的，没有的话自己写十几行也能实现。
2.3 观测噪声 R 与过程噪声 Q 的整定方法
新手最容易翻车的地方就是 Q 和 R 拍脑袋填。我的习惯是先做静态实验：传感器静止放十分钟，采集加速度计和陀螺的输出，算各自的方差。加速度计方差直接填进 R 的对角线，陀螺的随机游走方差填进 Q 里零偏对应的位置。这样起步至少不会离谱。
然后做动态调参：如果融合后的姿态在静止时还在缓慢画圈，说明陀螺零偏没被观测约束住，把 Q 里零偏项调小、或者确认磁力计/加速度计观测真的在起作用；如果动态时响应明显滞后，说明 R 给太大、太信陀螺了，适当减小 R。这个过程没有解析解，就是反复跑数据看曲线，属于典型的玄学调参，但有了静态方差做起点，能少走很多弯路。
3. 多传感器接入：加速度计、陀螺、磁力计的互补与冲突处理
3.1 三路传感器的角色分工
单靠陀螺只能得到相对姿态，时间一长必然漂；加速度计能提供绝对的水平参考（重力方向），但一有运动加速度就被污染；磁力计能提供绝对的航向参考，但周围有铁磁物质或电流就会偏。三者融合的本质是：用加速度计和磁力计提供长期绝对基准，用陀螺提供短期高动态响应。
程序里通常把加速度计和磁力计分别做观测更新，而不是合成一个观测。这样做的好处是某一路失效时可以单独关掉，比如电机干扰大时先禁用磁力计，只靠加速度计维持水平姿态，航向暂时交给陀螺短时维持。
3.2 磁力计椭球校正与航向解算
磁力计原始输出往往不是标准球面分布，而是被硬铁和软铁干扰拉成椭球。直接拿原始值算航向，误差能到几十度。常见做法是先做椭球拟合校正：
% 采集多组姿态下的磁力计数据 mag (N x 3) % 拟合椭球: (m - c)' * A * (m - c) = 1 % 简化做法: 用最小二乘拟合偏移和缩放 function [mag_cal] = calib_mag(mag_raw) c = mean(mag_raw); % 粗略取均值作为硬铁偏移 m_centered = mag_raw - c; scale = max(abs(m_centered)); % 各轴缩放因子 mag_cal = m_centered ./ scale; end % 用校正后的磁力计和加速度计算航向 function yaw = compute_yaw(mag_cal, acc) % 先用加速度计把磁力计投影到水平面 roll = atan2(acc(2), acc(3)); pitch = atan2(-acc(1), sqrt(acc(2)^2 + acc(3)^2)); mx = mag_cal(1)*cos(pitch) + mag_cal(2)*sin(roll)*sin(pitch) ... + mag_cal(3)*cos(roll)*sin(pitch); my = mag_cal(2)*cos(roll) - mag_cal(3)*sin(roll); yaw = atan2(-my, mx); endcalib_mag里用均值代替硬铁偏移是最省事的近似，精度要求高时应该用椭球拟合求偏移和变换矩阵。compute_yaw先把磁力计投影到水平面再算航向，这一步不能省，否则倾斜时航向会跟着姿态一起变。参数上，scale取各轴最大绝对值是为了把椭球拉回球面，如果你的磁力计三轴灵敏度差异大，这里要换成逐轴缩放。
3.3 观测失效时的降级策略
真实系统里传感器会掉线、会饱和、会被干扰。程序里应该给每路观测加一个有效性判断：加速度计模值偏离 1g 太多就认为有运动加速度，暂时不更新；磁力计模值突变就认为受干扰，跳过这次航向校正。降级后系统退化成纯陀螺积分，短时间还能用，等观测恢复再重新校正。这个逻辑不写，融合结果在干扰出现时会直接被带偏，而且很难查。
4. 避坑与排查：融合结果不对时先看这几处
4.1 姿态缓慢画圈或持续漂移
现象：静止放置时，融合输出的航向或俯仰角以固定速率缓慢变化，几分钟就偏出十几度。原因通常是陀螺零偏没有被有效观测，或者观测更新根本没生效。解决：先确认加速度计和磁力计的更新函数确实被调用，再检查 Q 矩阵里零偏对应的方差是不是给得太小，导致滤波器认为零偏不会变、不去估计它。把零偏项方差调大一个量级，通常就能把漂移压住。
4.2 动态响应明显滞后
现象：快速转动传感器时，融合输出跟不上真实姿态，松手后还要过一会儿才慢慢归位。原因是 R 给得太大，滤波器过度信任陀螺的预测、不敢用观测去修正。解决：减小加速度计和磁力计的观测噪声 R，或者检查观测残差y是不是被错误地做了限幅。限幅本意是抗野值，但阈值设太小会把正常动态也砍掉。
4.3 四元数不归一化导致数值发散
现象：跑一段时间后姿态输出突然变成 NaN 或者乱跳。原因是四元数在迭代中累积了数值误差，模长偏离 1，再继续运算就会放大。解决：每次预测和更新之后都强制归一化，就是代码里那句q = q / norm(q)。这一步看着不起眼，但漏掉它，长时间跑必翻车。
4.4 采样率不一致导致 dt 用错
现象：融合结果整体偏快或偏慢，和真实运动对不上。原因是陀螺、加速度计、磁力计的更新频率不同，但代码里用了同一个dt。解决：给每路观测单独记录时间戳，预测步用陀螺的dt，观测更新时用「距上次更新的实际时间差」。如果嫌麻烦，至少保证所有传感器同步采样，或者把高频数据降采样到统一频率再进滤波器。
4.5 坐标系定义混乱
现象：某个轴的方向反了，或者姿态整体旋转了 90 度。原因是导航系和机体系的定义、传感器安装方向、四元数旋转方向三者没对齐。解决：拿一个已知姿态做验证，比如水平静止时加速度计应该输出[0 0 1]（或[0 0 -1]，取决于你的约定），磁力计指向北时航向应该为 0。把这三个基准点对齐，坐标系问题基本就定位了。
5. 从仿真到实测：用真实数据验证融合效果的一个笨办法
程序跑通仿真只是第一步，真正检验它的是你自己的数据。我一般会用一个很笨但很有效的办法：把传感器固定在一个能手动复现的动作上，比如先静止三十秒，再绕某个轴匀速转九十度，停住，再转回来。全程录数据，然后拿融合输出和这个已知动作对比。
具体操作上，先把录到的数据存成 MAT 文件，字段名和程序里读取的变量对齐：
% 假设录到的数据: t(时间), gyr(陀螺 rad/s), acc(g), mag(uT) load('my_sensor_log.mat'); dt = mean(diff(t)); % 实际平均采样周期 N = length(t); x_est = zeros(7, N); % 存融合结果 x_est(:,1) = [1;0;0;0;0;0;0]; % 初始四元数 P = eye(7) * 0.1; % 初始协方差 for k = 2:N x_pred = predict_state(x_est(:,k-1), gyr(k,:)', dt); P_pred = P + Q * dt; % 简化过程噪声传播 if norm(acc(k,:)) > 0.8 && norm(acc(k,:)) < 1.2 [x_pred, P_pred] = update_acc(x_pred, P_pred, acc(k,:)', R_acc); end if norm(mag(k,:)) > 20 && norm(mag(k,:)) < 80 [x_pred, P_pred] = update_mag(x_pred, P_pred, mag(k,:)', R_mag); end x_est(:,k) = x_pred; P = P_pred; end % 把四元数转成欧拉角看曲线 eul = quat2eul(x_est(1:4,:)', 'ZYX') * 180/pi; plot(t, eul); legend('Yaw','Pitch','Roll');这段代码的关键在有效性判断：加速度计模值在 0.8g 到 1.2g 之间才更新，磁力计模值在合理区间才更新，超出范围就跳过。参数Q和R_acc、R_mag用前面静态实验整定出来的值。跑完把欧拉角曲线和你的已知动作对照，如果静止段平直、转动段跟得上、回位后不漂，说明融合基本可用；如果静止段还在缓慢变化，回到第 4 章排查零偏和观测。
验证通过之后，再把这套逻辑往嵌入式移植就心里有底了。MATLAB 里能跑通不代表 C 里能跑通，定点数、浮点精度、实时性都是新问题，但至少算法层面你已经确认它是对的。从那以后我每次拿到新的传感器数据，都强制先跑一遍这个「静止—转动—回位」的验证流程，不看到曲线对得上，绝不往下做。希望这套程序能帮你把融合这件事从公式变成手上能跑的东西，帮到你。
本文还有配套的精品资源，点击获取
