简介面向嵌入式、机器人与无人机方向的姿态解算学习者这份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.mddata里放的是采集好的.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。如果直接把三路按行号一一对应等于假设它们同时采样磁力计那一行会整体滞后几十毫秒航向在快速转动时明显拖尾。工程里约定每个数据集至少包含以下字段字段维度单位说明t_gyroN×1s陀螺时间戳作为主时间轴gyroN×3rad/s机体坐标系角速度accN×3m/s²比力含重力t_magM×1s磁力计时间戳magM×3μT机体坐标系磁场fs标量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,:); endlinear对磁力计足够因为磁场变化本来就慢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 第一行写死符号含义约定b 系机体坐标系x 向前y 向右z 向下FRDn 系导航坐标系北-东-地NEDq姿态四元数[w;x;y;z]表示 b 系到 n 系的旋转欧拉角ZYX 顺序先绕 z 转 yaw再绕 y 转 pitch最后绕 x 转 rollg重力加速度9.81 m/s²NED 下沿 z在这个约定下静止时加速度计输出约[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); endsec取 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 bS是 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²2Dxy2Exz2Fyz2Gx2Hy2Iz 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 软磁补偿矩阵 endp是九维系数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); % 必须归一化否则模值漂移会污染旋转矩阵 enddt用相邻时间戳之差不用固定值。一阶积分在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^2z^2), 2*(x*y-w*z), 2*(x*zw*y); 2*(x*yw*z), 1-2*(x^2z^2), 2*(y*z-w*x); 2*(x*z-w*y), 2*(y*zw*x), 1-2*(x^2y^2)]; endR的第三行[2(xz-wy), 2(yzwx), 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); endKp决定收敛速度取 0.5~2.0Ki决定零偏跟踪速度取 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); endbeta是梯度下降步长也是滤波器的“信任分配”旋钮beta大更信加速度计收敛快但动态噪声大beta小更信陀螺平滑但倾角恢复慢。常用范围 0.02~0.1对应约 1~5 秒的收敛时间。如果采样率是 100 Hzbeta取 0.05 左右比较稳采样率降到 50 Hzbeta要相应减小否则每一步修正过冲。quat_multiply就是标准四元数乘法输入两个 4×1 列向量输出也是 4×1function q quat_multiply(p, r) pwp(1); pxp(2); pyp(3); pzp(4); rwr(1); rxr(2); ryr(3); rzr(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]; endasin外面套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静止时 本文还有配套的精品资源点击获取 MATLAB九轴IMU姿态解算：标定、四元数与互补滤波
简介面向嵌入式、机器人与无人机方向的姿态解算学习者这份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.mddata里放的是采集好的.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。如果直接把三路按行号一一对应等于假设它们同时采样磁力计那一行会整体滞后几十毫秒航向在快速转动时明显拖尾。工程里约定每个数据集至少包含以下字段字段维度单位说明t_gyroN×1s陀螺时间戳作为主时间轴gyroN×3rad/s机体坐标系角速度accN×3m/s²比力含重力t_magM×1s磁力计时间戳magM×3μT机体坐标系磁场fs标量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,:); endlinear对磁力计足够因为磁场变化本来就慢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 第一行写死符号含义约定b 系机体坐标系x 向前y 向右z 向下FRDn 系导航坐标系北-东-地NEDq姿态四元数[w;x;y;z]表示 b 系到 n 系的旋转欧拉角ZYX 顺序先绕 z 转 yaw再绕 y 转 pitch最后绕 x 转 rollg重力加速度9.81 m/s²NED 下沿 z在这个约定下静止时加速度计输出约[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); endsec取 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 bS是 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²2Dxy2Exz2Fyz2Gx2Hy2Iz 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 软磁补偿矩阵 endp是九维系数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); % 必须归一化否则模值漂移会污染旋转矩阵 enddt用相邻时间戳之差不用固定值。一阶积分在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^2z^2), 2*(x*y-w*z), 2*(x*zw*y); 2*(x*yw*z), 1-2*(x^2z^2), 2*(y*z-w*x); 2*(x*z-w*y), 2*(y*zw*x), 1-2*(x^2y^2)]; endR的第三行[2(xz-wy), 2(yzwx), 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); endKp决定收敛速度取 0.5~2.0Ki决定零偏跟踪速度取 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); endbeta是梯度下降步长也是滤波器的“信任分配”旋钮beta大更信加速度计收敛快但动态噪声大beta小更信陀螺平滑但倾角恢复慢。常用范围 0.02~0.1对应约 1~5 秒的收敛时间。如果采样率是 100 Hzbeta取 0.05 左右比较稳采样率降到 50 Hzbeta要相应减小否则每一步修正过冲。quat_multiply就是标准四元数乘法输入两个 4×1 列向量输出也是 4×1function q quat_multiply(p, r) pwp(1); pxp(2); pyp(3); pzp(4); rwr(1); rxr(2); ryr(3); rzr(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]; endasin外面套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静止时 本文还有配套的精品资源点击获取