简介：面向嵌入式、机器人与无人机方向的姿态解算学习者，这份MATLAB工程围绕九轴传感器（三轴陀螺仪、加速度计与磁力计）的数据融合展开，覆盖陀螺仪角速度、线性加速度和地磁观测的校准、滤波与姿态输出，适合已具备一定MATLAB基础、希望把互补滤波、Mahony、扩展卡尔曼滤波等方法落实到代码中的读者。压缩包共12个文件，以10个.m脚本/函数为主体，另有1个.mat数据文件和1个.asv自动备份文件，整体约555KB，便于直接运行、阅读与二次修改。已有5071人学习，说明其在惯导定位与姿态解算入门实践中具有一定参考价值。资源按核心算法、传感器校准、数据处理与演示脚本组织，包含低通/高通滤波、磁力计到加速度坐标系转换、欧拉角与四元数结果查看，可帮助读者理解从原始IMU数据到俯仰、偏航、翻滚角输出的完整链路，并据此搭建自己的实验流程。

1. 九轴姿态解算的真实起点：静止十分钟，航向漂了三十度

前阵子帮人看一台田间小车的日志，机器停在原地十分钟，串口打印的航向角从 32° 一路爬到 61°，一动起来 roll 和 pitch 又开始抽。传感器没坏，问题出在从原始采样到姿态四元数这条链路上：标定没做干净、磁力计没做倾角补偿、陀螺零偏被当成真值一路积分。这份基于 MATLAB 的九轴传感器姿态解算工程，把这套链路完整摊开——加速度计、陀螺仪、磁力计三路数据怎么对齐，零偏和椭球误差怎么标，四元数怎么积分，Mahony 和 Madgwick 两种互补滤波怎么选，姿态稳定后怎么接惯导定位推速度和位置。适合正在调 IMU 模块、做云台/小车/机械臂末端姿态，或者准备把 MATLAB 算法往嵌入式搬的开发者。

2. 工程骨架与数据流：从原始采样到统一时间轴

拿到一个姿态解算工程，先别急着看滤波公式。九轴数据最容易翻车的地方不在算法本身，而在“三路传感器的时间戳对不上、坐标轴定义各说各话”。这份工程把 IO、标定、AHRS、INS、评估拆成五层，每一层只做一件事，改起来不会牵一发动全身。

2.1 目录结构与脚本职责

工程按功能分层，而不是按“第几章第几节”平铺，这样二次开发时替换某一层不会影响其他模块：

nine_axis_ahrs/ ├── data/ │ ├── calib_static.mat % 静止采集，用于陀螺零偏 │ ├── calib_six_face.mat % 六面采集，用于加速度计标定 │ ├── calib_mag_rotate.mat % 绕三轴慢速旋转，用于磁椭球拟合 │ └── run_dynamic.mat % 动态跑车/摇摆数据 ├── src/ │ ├── io/ │ │ └── load_dataset.m % 统一读入、单位换算、时间对齐 │ ├── calib/ │ │ ├── gyro_bias_calib.m │ │ ├── acc_six_face_calib.m │ │ └── mag_ellipsoid_calib.m │ ├── ahrs/ │ │ ├── quat_multiply.m │ │ ├── quat_to_dcm.m │ │ ├── dcm_to_euler.m │ │ ├── mahony_update.m │ │ └── madgwick_update.m │ ├── ins/ │ │ ├── ins_mechanize.m % 比力变换、重力补偿、积分 │ │ └── zupt_detect.m % 零速检测 │ └── eval/ │ ├── plot_attitude.m │ └── plot_trajectory.m ├── run_all.m % 一键跑通：标定→解算→画图 └── README.md

data里放的是采集好的.mat，不依赖硬件就能复现；calib下的三个脚本各自独立，可以单独拿出来给别的项目用；ahrs只负责“给定角速度和比力，输出四元数”，不碰文件读写；ins拿四元数做机械编排；eval只画图，不改数据。这种切法的好处是，你要换 Madgwick 为 EKF，只动ahrs一个目录，run_all.m的调用方式不变。

run_all.m我一般写成显式流水线，而不是一堆函数互相调用，方便打断点：

% run_all.m % 1) 读数据 D = load_dataset('data/run_dynamic.mat'); % 2) 标定参数（首次运行会从 calib_*.mat 里算出来并缓存） P = load('data/calib_params.mat'); % 3) 应用标定 gyro = (D.gyro - P.gyro_bias') * diag(P.gyro_scale); acc = (D.acc - P.acc_bias') * diag(P.acc_scale); mag = (D.mag - P.mag_center) * P.mag_W; % 4) 逐采样递推 q = [1;0;0;0]; for k = 2:numel(D.t) dt = D.t(k) - D.t(k-1); q = mahony_update(q, gyro(k,:)', acc(k,:)', mag(k,:)', dt, 0.5, 0.02); eul(k,:) = dcm_to_euler(quat_to_dcm(q))'; end % 5) 画图 plot_attitude(D.t, eul);

这里的mahony_update入参顺序是(q, gyro, acc, mag, dt, Kp, Ki)，Kp取 0.5、Ki取 0.02 是常见起步值，后面第 4 章会解释这两个数怎么调。dt不要用固定 1/fs，用相邻时间戳相减，采集卡丢包时能自动吸收误差。

2.2 数据格式与时间对齐

九轴模块常见的输出是：陀螺和加速度计 100~1000 Hz，磁力计 10~100 Hz。如果直接把三路按行号一一对应，等于假设它们同时采样，磁力计那一行会整体滞后几十毫秒，航向在快速转动时明显拖尾。

工程里约定每个数据集至少包含以下字段：

对齐用interp1把磁力计插到陀螺时间轴上，注意外推标志：

function D = align_streams(D) % 以陀螺时间轴为主轴，把磁力计线性插值过来 if isfield(D, 't_mag') && numel(D.t_mag) ~= numel(D.t_gyro) D.mag = interp1(D.t_mag, D.mag, D.t_gyro, 'linear', 'extrap'); end D.t = D.t_gyro; % 去掉首尾各 1% 的数据，避免插值边界抖动 n = numel(D.t); idx = round(0.01*n)+1 : n-round(0.01*n); D.t = D.t(idx); D.gyro = D.gyro(idx,:); D.acc = D.acc(idx,:); D.mag = D.mag(idx,:); end

'linear'对磁力计足够，因为磁场变化本来就慢；'extrap'是为了防止t_mag比t_gyro短一截时首尾出现NaN，但外推出来的点不可信，所以后面裁掉首尾 1%。如果磁力计采样率低于 10 Hz，线性插值的误差会变大，这时改用零阶保持'previous'更稳，代价是航向更新有台阶。

还有一个容易忽略的点：单位。不同模块陀螺输出可能是deg/s，磁力计可能是gauss，读入时必须统一到rad/s和μT，否则Kp调到头也压不住漂移。load_dataset.m里我习惯加一段显式检查：

assert(max(abs(D.gyro(:))) < 20, '陀螺单位疑似 deg/s，请先转 rad/s'); assert(mean(vecnorm(D.mag,2,2)) > 5 && mean(vecnorm(D.mag,2,2)) < 200, ... '磁力计模值异常，检查单位是否为 uT');

2.3 坐标系约定与旋转顺序

姿态解算里一半的“玄学”来自坐标系没对齐。工程统一采用航空常用的约定，并在 README 第一行写死：

在这个约定下，静止时加速度计输出约[0 0 -9.81]（比力方向向上，与 z 轴反向），磁力计在水平放置时北向分量为正。如果拿到的模块是“前右下”之外的轴向，比如 x 向右、y 向前，必须在load_dataset里做一次轴交换，而不是等到滤波里再补，否则后面所有叉积的符号都是错的。

欧拉角用 ZYX 顺序，反解时pitch在 ±90° 附近会出现万向锁，atan2的分母趋近于零，角度会跳变。这不是算法错误，是欧拉角表示本身的奇点，第 5 章会讲怎么在画图和输出时绕开它。四元数全程参与运算，只在最后展示时转欧拉角，这样积分过程不受奇点影响。

3. 标定与预处理：零偏、六面校准与磁椭球拟合

很多人拿到模块直接跑滤波，看到曲线“大概跟着动”就认为成了，结果航向一小时漂几十度。九轴姿态解算的精度上限由标定决定，滤波只负责把剩下的误差压平。这一章三个标定各管一段：陀螺零偏管短期漂移，加速度计六面管倾角基准，磁椭球管航向绝对参考。

3.1 陀螺零偏与温度漂移

陀螺静止时输出不是零，而是一个几十到几百deg/h的偏置，直接积分就是航向漂移。最直接的做法是开机静止采一段求均值：

function [bias, sigma] = gyro_bias_calib(gyro, fs, sec) % gyro: N×3 rad/s % sec : 静止采集时长，建议 30~60 s n = min(round(sec*fs), size(gyro,1)); seg = gyro(1:n, :); bias = mean(seg, 1)'; sigma = std(seg, 0, 1)'; % 用 3σ 剔除野点后重算，避免开机瞬间的冲击 keep = all(abs(seg - bias') < 3*sigma', 2); bias = mean(seg(keep,:), 1)'; fprintf('gyro bias (rad/s): %.5f %.5f %.5f\n', bias); fprintf('gyro noise std : %.5f %.5f %.5f\n', sigma); end

sec取 30 秒是经验值：太短均值不稳，太长用户等不及。sigma不是拿来好看的，它是后面 Mahony 的Ki和 ZUPT 阈值的参考——噪声标准差乘以采样间隔，就是单步积分的不确定度。如果sigma大于 0.01 rad/s，先检查是不是采集时桌子在抖，或者模块没固定。

零偏会随温度变，冷机开机和跑十分钟后的零偏能差 20% 以上。工程里没有做完整温补，但留了一个线性模型接口：

% 若数据里带温度字段 D.temp（摄氏度），可以做一阶拟合 p = polyfit(D.temp(static_idx), gyro_static(:,1), 1); bias_x = polyval(p, D.temp(k));

我一般会在run_all.m里把冷机静止段和热机静止段各存一份，对比零偏差异，超过 30% 就在 README 里注明“建议预热 5 分钟”。这是血泪经验：不写这句话，下一个用工程的人一定会在冷机状态下调参数，然后怀疑算法有问题。

3.2 加速度计六面标定

加速度计的误差模型是raw = S * a_true + b，S是 3×3 的尺度加轴间耦合，b是零偏。完整解需要十二面甚至更多姿态，但工程里用六面法已经能消掉主要误差：把模块分别朝 +x、-x、+y、-y、+z、-z 六个方向静止放置，每个方向采几秒取均值。

function [bias, scale] = acc_six_face_calib(acc_faces, g) % acc_faces: 6×3，行顺序为 +x,-x,+y,-y,+z,-z 的平均读数 % g: 当地重力加速度，9.81 bias = zeros(3,1); scale = zeros(3,1); for i = 1:3 pos = acc_faces(2*i-1, i); % 该轴指向下时的读数 neg = acc_faces(2*i, i); % 该轴指向上时的读数 % 理想情况 pos = +g, neg = -g scale(i) = 2*g / (pos - neg); bias(i) = (pos + neg) / 2; end % 校正：a_cal = scale .* (raw - bias) end

这里假设六个面放置时敏感轴严格对准重力方向，实际用手摆会有几度偏差，scale会有 1%~2% 的误差。对姿态解算来说够用，因为加速度计只用来修正 roll 和 pitch 的长期漂移，不参与高频积分。bias的单位是 m/s²，scale无量纲，校正时逐轴相乘。

如果模块焊在板子上、板子又装在结构件里，六个面很难摆正，可以退而求其次：只做零偏校正，把模块水平静置，取bias = mean(acc) - [0 0 -g]。代价是倾角在±30° 以外会有几度误差，适合对精度要求不高的场景。

3.3 磁力计椭球拟合与硬磁/软磁补偿

磁力计是航向的唯一绝对参考，但它也是最容易被干扰的一路。硬磁干扰（喇叭磁铁、电机永磁体）让球心偏移，软磁干扰（铁质结构件）让球变成椭球。未标定的磁力计转一圈，模值可能从 20 μT 变到 80 μT，航向自然乱跳。

工程里用九参数椭球拟合。把磁力计绕三个轴各转几圈，采到的点满足一般二次曲面：

function [center, W] = mag_ellipsoid_calib(mag) % mag: N×3，单位 μT，需覆盖各个朝向 mx = mag(:,1); my = mag(:,2); mz = mag(:,3); % 设计矩阵：Ax²+By²+Cz²+2Dxy+2Exz+2Fyz+2Gx+2Hy+2Iz = 1 D = [mx.^2, my.^2, mz.^2, ... 2*mx.*my, 2*mx.*mz, 2*my.*mz, ... 2*mx, 2*my, 2*mz]; p = D \ ones(size(mx)); % 还原二次型矩阵与一次项 M = [p(1) p(4) p(5); p(4) p(2) p(6); p(5) p(6) p(3)]; d = p(7:9); center = -M \ d; % 椭球球心，即硬磁偏移 % 软磁：把 M 归一化到单位球 [V, E] = eig(M); s = sqrt(diag(E) / min(diag(E))); W = V * diag(1./s) * V'; % 3×3 软磁补偿矩阵 end

p是九维系数，M是二次型矩阵，d是一次项。完成配方后球心是-M\d，也就是硬磁偏移；W把椭球拉回球。应用时：

mag_cal = (mag - center') * W; % 检查标定效果：模值标准差应小于均值的 5% fprintf('mag norm mean=%.2f, std=%.2f\n', ... mean(vecnorm(mag_cal,2,2)), std(vecnorm(mag_cal,2,2)));

参数说明：D的列顺序必须和p的物理含义一一对应，改顺序会得到完全错误的球心；eig分解要求M正定，如果采集点分布太集中（比如只在一个平面转），M会接近奇异，W里出现巨大数值。判断方法是看cond(M)，大于 1e6 就重新采数据，让模块绕三个轴都转到。

标定后的磁力计还要做倾角补偿才能算航向。简单做法是把磁场旋到水平面：

function yaw = mag_yaw(mag_b, roll, pitch) mx = mag_b(1); my = mag_b(2); mz = mag_b(3); hx = mx*cos(pitch) + my*sin(roll)*sin(pitch) + mz*cos(roll)*sin(pitch); hy = my*cos(roll) - mz*sin(roll); yaw = atan2(-hy, hx); % NED 下北向为 0，顺时针为正 end

这段只在静止或低动态时可信，动态下更稳的做法是把磁力计观测直接送进 Mahony 或 Madgwick，让滤波器自己处理权重，第 4 章会给出对应代码。

4. 姿态解算与惯导定位：四元数积分、Mahony、重力补偿与 ZUPT

标定把系统性误差压下去之后，剩下的就是滤波器的活。这一章从四元数微分方程讲到 Mahony 和 Madgwick 两种互补滤波，再往下接惯导机械编排。代码都按“能直接抄进自己的工程”来写，参数含义和调法跟在每段后面。

4.1 四元数更新与陀螺积分

四元数q = [w;x;y;z]表示 b 系到 n 系的旋转，微分方程是q_dot = 0.5 * q ⊗ [0; ω]，其中ω是机体角速度。离散化后用一阶欧拉积分，再归一化：

function q = quat_integrate(q, omega, dt) % omega: 3×1 rad/s，已扣除零偏并做滤波修正 wx = omega(1); wy = omega(2); wz = omega(3); % 四元数乘法矩阵形式，q_dot = 0.5 * Omega * q Omega = [0 -wx -wy -wz; wx 0 wz -wy; wy -wz 0 wx; wz wy -wx 0 ]; q = q + 0.5 * dt * (Omega * q); q = q / norm(q); % 必须归一化，否则模值漂移会污染旋转矩阵 end

dt用相邻时间戳之差，不用固定值。一阶积分在dt小于 10 ms 时误差可忽略；如果采样率只有 50 Hz，快速转动下会有明显锥误差，这时换二阶龙格库塔或圆锥补偿。归一化那行不能省，浮点累积几十万步之后模值会偏离 1，旋转矩阵就不再正交，重力方向会慢慢歪掉。

四元数转旋转矩阵用标准公式，注意这里是 b 系到 n 系：

function R = quat_to_dcm(q) q = q / norm(q); w = q(1); x = q(2); y = q(3); z = q(4); R = [1-2*(y^2+z^2), 2*(x*y-w*z), 2*(x*z+w*y); 2*(x*y+w*z), 1-2*(x^2+z^2), 2*(y*z-w*x); 2*(x*z-w*y), 2*(y*z+w*x), 1-2*(x^2+y^2)]; end

R的第三行[2(xz-wy), 2(yz+wx), 1-2(x²+y²)]正好是 n 系重力单位向量[0;0;1]在 b 系的投影，Mahony 的重力修正直接用它，不用再单独推公式。

4.2 Mahony 互补滤波的 PI 修正

Mahony 的思路是：陀螺积分 trustworthy 在短时间，加速度计和磁力计 trustworthy 在长时间。用加速度计观测到的重力方向与四元数预测的重力方向做叉积，得到误差向量，PI 控制后反馈到角速度上。

function [q, b] = mahony_update(q, b, gyro, acc, mag, dt, Kp, Ki) % q : 4×1 四元数 % b : 3×1 积分反馈项（陀螺零偏的在线估计） % acc : 3×1 比力，静止时约 [0 0 -g] % mag : 3×1 磁场，可为空 R = quat_to_dcm(q); % 加速度计观测：取反归一化，得到 b 系下的“重力方向” a = -acc(:) / norm(acc); % 四元数预测的重力方向（R 的第三行） v = R(3, :)'; e = cross(a, v); % 磁力计修正：把磁场转到 n 系，去掉垂直分量，只保留水平北向 if nargin >= 5 && ~isempty(mag) m = mag(:) / norm(mag); h = R * m; h(3) = 0; if norm(h) > 1e-6 h = h / norm(h); v_m = R' * h; % 期望磁矢量在 b 系的方向 e = e + cross(m, v_m); end end % PI 反馈：Kp 压高频误差，Ki 估零偏 if Ki > 0 b = b - Ki * e * dt; end omega = gyro(:) + Kp * e + b; q = quat_integrate(q, omega, dt); end

Kp决定收敛速度，取 0.5~2.0；Ki决定零偏跟踪速度，取 0.01~0.05。Kp太大，姿态会跟着加速度计的振动抖；Ki太大，航向会在磁干扰下缓慢跑偏。调参顺序是先把Ki置零，调Kp到静态倾角稳定、动态不抖，再加Ki让长时间静止的航向不漂。

b的初值给[0;0;0]，它会在运行中慢慢收敛到残余零偏。注意b的符号：b = b - Ki*e*dt，对应 Mahony 原文的b_dot = -Ki*e，如果写成加号，误差会发散而不是收敛。

4.3 Madgwick 梯度下降与 β 参数

Madgwick 把姿态解算写成优化问题：找一个四元数，让预测的重力方向和磁场方向与观测的误差最小，用梯度下降迭代，再和陀螺积分融合。它的计算量比 Mahony 略大，但收敛更稳，尤其是在磁干扰间歇出现的场景。

下面是加速度计部分的梯度，磁力计部分结构相同，源码里给了完整雅可比：

function q = madgwick_update(q, gyro, acc, mag, dt, beta) q = q / norm(q); a = -acc(:) / norm(acc); % 与 Mahony 一致，取反后 az≈+1 ax = a(1); ay = a(2); az = a(3); w = q(1); x = q(2); y = q(3); z = q(4); % 目标函数 f：预测重力方向 - 观测重力方向 f = [2*(x*z - w*y) - ax; 2*(w*x + y*z) - ay; 2*(0.5 - x^2 - y^2) - az]; % 雅可比 J = df/dq J = [-2*y, 2*z, -2*w, 2*x; 2*x, 2*w, 2*z, 2*y; 0, -4*x, -4*y, 0 ]; grad = J' * f; if norm(grad) > 1e-9 grad = grad / norm(grad); end % 陀螺积分项 - β * 梯度修正项 q_dot = 0.5 * quat_multiply(q, [0; gyro(:)]) - beta * grad; q = q + q_dot * dt; q = q / norm(q); end

beta是梯度下降步长，也是滤波器的“信任分配”旋钮：beta大，更信加速度计，收敛快但动态噪声大；beta小，更信陀螺，平滑但倾角恢复慢。常用范围 0.02~0.1，对应约 1~5 秒的收敛时间。如果采样率是 100 Hz，beta取 0.05 左右比较稳；采样率降到 50 Hz，beta要相应减小，否则每一步修正过冲。

quat_multiply就是标准四元数乘法，输入两个 4×1 列向量，输出也是 4×1：

function q = quat_multiply(p, r) pw=p(1); px=p(2); py=p(3); pz=p(4); rw=r(1); rx=r(2); ry=r(3); rz=r(4); q = [pw*rw - px*rx - py*ry - pz*rz; pw*rx + px*rw + py*rz - pz*ry; pw*ry - px*rz + py*rw + pz*rx; pw*rz + px*ry - py*rx + pz*rw]; end

实际使用时不必自己手写 Madgwick 的完整雅可比，源码里madgwick_update.m已经把加速度计和磁力计两部分拼好，调用方式和 Mahony 一致，方便在同一份数据上对比两条曲线。

4.4 姿态转欧拉角与万向锁

四元数转欧拉角只在展示和评估时做，ZYX 顺序：

function eul = dcm_to_euler(R) % 返回 [roll; pitch; yaw]，单位 rad roll = atan2(R(3,2), R(3,3)); pitch = -asin(max(-1, min(1, R(3,1)))); % 限幅防 NaN yaw = atan2(R(2,1), R(1,1)); eul = [roll; pitch; yaw]; end

asin外面套max/min是防止数值误差让R(3,1)略超 ±1 导致NaN。pitch接近 ±90° 时，roll和yaw的分母同时趋近零，两个角会互相“抢”，画出来就是跳变。这不是解算错了，是欧拉角在这个姿态下不唯一。评估时我一般把pitch限制在 ±85° 以内做测试，或者在图上同时画四元数分量，避免被欧拉角跳变误导。

4.5 比力变换、重力补偿与 ZUPT

姿态稳了之后，惯导定位的链路是：把机体比力转到 n 系，加回重力，积分得速度，再积分得位置：

function [vn, pn] = ins_mechanize(q, acc, vn, pn, dt, g) % q : 当前姿态四元数 % acc: 3×1 比力，单位 m/s² % g : 9.81 R = quat_to_dcm(q); f_n = R * acc(:); % 比力转到 n 系 a_n = f_n + [0; 0; g]; % NED 下重力沿 +z vn = vn + a_n * dt; pn = pn + vn * dt; end

静止时 `

本文还有配套的精品资源，点击获取