企业官网建设流程全解析

简介：面向惯性导航、机器人及无人机姿态解算学习者的MATLAB实践资料包，侧重九轴传感器（陀螺仪、加速度计、磁力计）的数据校准与多传感器融合。内容围绕卡尔曼、Mahony、互补滤波及梯度下降等姿态估计思路展开，并配有传感器校准数据与演示脚本，便于理解欧拉角、四元数等姿态输出的实现脉络。压缩包共12个文件，以m函数与脚本为主体，另含mat校准数据和asv备份文件，整体约555KB，结构紧凑、模块划分便于按滤波与校准环节查阅。已有5071人学习，适合具备MATLAB基础、希望从仿真代码层面掌握惯导姿态解算流程的读者参考，也可作为课程设计或项目原型的对照材料。

1. 九轴姿态解算：陀螺仪单独积分为什么撑不过半分钟

某四旋翼平台悬停到第二十秒，航向开始缓慢偏；三十秒后偏了七八度，爬升加速的一瞬间俯仰角还被拽偏三四度。传感器没坏，是陀螺仪单独积分的必然结果：零偏看着只有 0.05°/s，一分钟就能攒出 3° 误差。九轴姿态解算要干的活，就是把加速度计测到的重力方向和磁力计测到的水平分量当作两个绝对参考，实时把积分漂移拽回来。这份 MATLAB 资源围绕惯导定位里的姿态解算环节，把数据读入、传感器标定、互补滤波与 Mahony / Madgwick 融合、误差评估拆成能单独运行的脚本，跑完能直接看到姿态曲线和误差统计。适合手上有九轴模块、想先把离线数据跑通再移植到嵌入式的人，也适合刚接触惯导定位、需要一份能对着改的参考实现的人。

2. 数据链路与坐标约定：九轴原始 LSB 怎么变成物理量

拿到一份九轴日志，最先出问题的往往不是滤波算法，而是喂进去的数据。上位机导出的 CSV 里，加速度是 LSB、陀螺是 dps、磁力计是 mT 还是 µT，三种单位混在一起；时间戳是毫秒计数，中间还夹着丢包造成的重复值。这一层不洗干净，后面无论 Mahony 还是 Madgwick，调参都只是玄学。这一章把从原始字节到统一物理量的链路走一遍，顺带把坐标系和转序固定下来，后面所有公式才有共同的基准。

2.1 量程、灵敏度与单位统一

九轴模块出厂时，每个通道都有自己的量程和灵敏度。以常见的 16 位 IMU 配三轴磁力计为例，加速度计设 ±8 g，灵敏度约 4096 LSB/g；陀螺仪设 ±500 dps，约 65.5 LSB/dps；磁力计按手册填，常见在 0.15 µT/LSB 这个量级。这三个数字必须从器件手册和实际配置的寄存器值里取，不能照抄别人的代码，一旦量程改过而换算系数没跟着改，姿态角会以肉眼可见的速度发散。

单位上建议只留一套内部标准：加速度用 g（而不是 m/s²），角速度用 rad/s，磁场用 µT。原因很实际，重力参考向量的模长在 g 单位下就是 1，后面归一化和阈值判断都省事；而角速度必须是 rad/s，四元数微分方程里的 ω 是弧度制，拿 dps 直接代进去会得到一个大 57 倍的结果，姿态转得飞快，很多人第一反应是“传感器坏了”，其实只是单位没换。

% unit_convert.m —— 三行换算，写错任何一行后面全废 acc_g = acc_raw / cfg.accLSB; % LSB -> g，cfg.accLSB = 4096 gyr_rps = gyr_raw / cfg.gyrLSB * pi/180; % LSB -> dps -> rad/s mag_uT = mag_raw * cfg.magLSB; % LSB -> uT，按手册填 % 自检：静止时加速度模长应该贴近 1 g，偏差超过 5% 说明量程或系数不对 fprintf('静止段 |a| 均值 = %.3f g\n', mean(vecnorm(acc_g, 2, 2)));

cfg建议单独存成一个结构体或者.mat，和这块板子绑定。同一批次的模块灵敏度会有零点几个百分点的离散，磁力计更明显，部分器件出厂时还带一个需要读 Fuse ROM 才能拿到的调整系数，不做这一步，磁场模长会随朝向变化，航向角跟着摆。换算完先看静止段的|a|和|m|是不是稳定，|a|应该在 1 g 附近，|m|应该接近当地地磁总场强度（国内大部分地区在 45～55 µT 之间），对不上就别往下走。

2.2 机体系、导航系与 321 转序

姿态解算的本质是求两个坐标系之间的旋转关系，所以先把这两套坐标系说死。机体坐标系跟着板子走，常见约定是 x 轴指向正前方、y 轴指向右、z 轴指向下，三轴构成右手系；导航坐标系取北-东-地（NED），x 北、y 东、z 地。z 轴向下这个约定很关键，它决定了静止时加速度计读数在机体系下是[0, 0, +1]而不是[0, 0, -1]，符号搞反的话滤波会往错误方向收敛，表现为倾角越修越大。

旋转的表示用四元数，方向约定统一成“机体系到导航系”，也就是把机体系下的一个向量左乘旋转矩阵后，得到它在导航系下的表达。这个方向一旦定下来，四元数更新、误差叉乘、欧拉角提取三处的公式就都固定了；反过来如果某一段用了共轭，现象是姿态能跟住但整体镜像，或者俯仰正常、横滚反向，靠调参数是调不回来的。

欧拉角采用航空上通用的 321 转序，也就是先绕 z 轴转偏航 ψ、再绕 y 轴转俯仰 θ、最后绕 x 轴转横滚 φ，对应旋转矩阵R = Rz(ψ)·Ry(θ)·Rx(φ)。这个转序在 θ = ±90° 时会出现万向锁，两个轴退化成同一个自由度，所以内部一律用四元数或旋转矩阵递推，只在最后输出给人看的时候才转欧拉角。NED 与 ENU 两种导航系的差别主要在 z 轴方向和偏航角的零点，用 ENU 的话静止加速度是[0, 0, -1]，偏航角以正东为零，混用会导致航向整体差 90°。

提示：先在一张纸上写清机体系三个轴指向、导航系是 NED 还是 ENU、旋转方向是机体系到导航系还是反过来，三句话贴在显示器边上，比调半天参数有用。

2.3 日志读入、时间戳去重与重采样

真实日志几乎不可能完美。上位机可能按毫秒计数打时间戳，中途丢过包，某些行的时间戳重复甚至倒退；磁力计的采样率和 IMU 不一致，有的模块磁力计只有 100 Hz 而陀螺仪 1000 Hz。直接按采样率倒数当dt递推，遇到丢包就会积出一个错误的转角，时间越长错得越多。

处理顺序是：先读入并统一单位，再去重保证时间戳单调，最后按固定采样率重采样成等间隔序列。插值方式上，加速度和角速度用线性插值就够，磁力计带宽低，插值前最好先过一遍低通，避免把噪声插成假的细节。

function imu = load_nineaxis(csvFile, cfg) % 读九轴日志：统一单位、去重、返回结构体 raw = readmatrix(csvFile, 'NumHeaderLines', 1); if size(raw, 2) < 10 error('列数不足 10 列，检查导出格式是否为 时间戳 + acc(3) + gyr(3) + mag(3)'); end t = (raw(:,1) - raw(1,1)) * 1e-3; % 毫秒 -> 从 0 开始的秒 acc = raw(:,2:4) / cfg.accLSB; % g gyr = raw(:,5:7) / cfg.gyrLSB * pi/180; % rad/s mag = raw(:,8:10) * cfg.magLSB; % uT % 去重并保持原顺序，丢掉时间戳重复/倒退的行 [t, iu] = unique(t, 'stable'); imu.t = t; imu.acc = acc(iu,:); imu.gyr = gyr(iu,:); imu.mag = mag(iu,:); end function imu = resample_imu(imu, fs) % 统一重采样到 fs，保证解算循环里 dt 恒定 tq = (imu.t(1) : 1/fs : imu.t(end))'; imu.acc = interp1(imu.t, imu.acc, tq, 'linear', 'extrap'); imu.gyr = interp1(imu.t, imu.gyr, tq, 'linear', 'extrap'); imu.mag = interp1(imu.t, imu.mag, tq, 'linear', 'extrap'); imu.t = tq; imu.dt = 1/fs; end

unique的'stable'参数不要省，默认排序会打乱原始顺序，姿态曲线会出现莫名其妙的跳变。interp1的'extrap'是为了让首尾那几个点不至于变成 NaN，但如果日志两端本身缺数据，更稳妥的做法是直接裁掉。重采样之后建议再检查一次diff(imu.t)是不是全部等于1/fs，这一步能挡掉后面一半的诡异问题。

3. 标定环节：陀螺零偏、加计六面与磁力计椭球拟合

同一份算法代码，有人跑出来静态漂移 0.2°/min，有人跑出来 5°/min，差别常常全在标定。九轴模块便宜，出厂校准最多做到零偏量级，磁力计更是几乎没有硬磁补偿，板子焊在机架上、旁边还有电机和电源线，磁场早就被拉成了椭球。这一章按陀螺、加速度计、磁力计三块拆开，每一步都给可执行的函数和判据。

3.1 陀螺零偏：静止取样与噪声底

陀螺零偏是最容易做也最容易漏的一步。做法很简单：板子静止放在水平桌面上，采集 30 秒以上，取三轴均值作为零偏，标准差作为噪声水平。零偏直接减掉，噪声标准差留着调滤波增益——噪声大的模块，比例增益就得压小，否则姿态会被噪声推着抖。

function [bias, sigma] = gyro_calib_static(gyr, fs, sec) % 静止段估计陀螺零偏，sec 建议 30 s 以上 n = min(size(gyr,1), round(sec*fs)); seg = gyr(1:n, :); bias = mean(seg, 1); % rad/s，直接减掉 sigma = std(seg, 0, 1); % rad/s，用于确定滤波带宽 fprintf('零偏 %.5f %.5f %.5f rad/s，噪声 %.5f rad/s\n', bias, sigma); end

判断零偏有没有取对，看减完之后静止段的均值是不是落在 ±0.002 rad/s 以内，以及噪声标准差是不是和器件手册的噪声密度量级对得上。如果静止段本身就在缓慢变化，说明采集时桌面在震或者温漂明显，这时候取均值意义不大，需要先恒温静置，或者干脆把零偏做成开机后持续更新的在线估计，交给第 4 章的积分项去跟。

3.2 加速度计六面校准：解 12 个参数

加速度计的误差模型取线性形式a_meas = M · a_true + b，M 是 3×3 的尺度与轴间耦合矩阵，b 是三轴零偏，一共 12 个未知数。六面法就是让板子的 ±x、±y、±z 六个方向分别朝下静止，每个姿态下a_true是已知的g乘以单位方向，把六个姿态的测量值堆起来做最小二乘就能解出来。

function [M, b] = accel_calib_sixface(acc, segs, g) % segs: 6x2，依次为 +x -x +y -y +z -z 六个静止段的起止采样点 dirs = [1 0 0; -1 0 0; 0 1 0; 0 -1 0; 0 0 1; 0 0 -1]; A = zeros(18, 12); y = zeros(18, 1); for k = 1:6 a = mean(acc(segs(k,1):segs(k,2), :), 1)'; % 3x1 测量值 at = g * dirs(k,:)'; % 3x1 真值 for ax = 1:3 r = (k-1)*3 + ax; % 每个采样姿态贡献 3 行 A(r, (ax-1)*4 + (1:3)) = at'; A(r, (ax-1)*4 + 4) = 1; y(r) = a(ax); end end p = A \ y; % 12x1 最小二乘解 M = [p(1:3)'; p(5:7)'; p(9:11)']; % 尺度+耦合 b = p([4 8 12]); % 零偏 end

校正时用a_true = M \ (a_meas - b)反解。segs的六段要各取稳定的一两秒，判据是段内标准差小于 0.01 g。解完之后检查 M 的对角元素是不是在 1 附近（0.95～1.05），非对角元素是不是接近 0，偏得离谱说明六面没摆正，或者某一段采到了碰撞。

3.3 磁力计椭球拟合与磁偏角扣除

磁力计的原始数据在三维空间里不是一个球，而是一个被拉偏的椭球：硬磁干扰把球心推离原点，软磁干扰把球压成椭球。校准的目标是求一个变换，把椭球拉回以原点为心、半径等于地磁总场强度的球。工程上常用二次型拟合，把方程写成m'Am + 2d'm + f = 0，归一化后线性最小二乘解 9 个参数，再反解中心 b 和形状矩阵 W。

function [b, W] = mag_calib_ellipsoid(m) % m: N x 3 原始磁测（uT），采集时要尽量转遍各个朝向 mx = m(:,1); my = m(:,2); mz = m(:,3); 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(m,1), 1); % 约束 f = -1，消掉平凡解 A = [p(1) p(4) p(5); p(4) p(2) p(6); p(5) p(6) p(3)]; d = p(7:9); b = -A \ d; % 椭球中心，即硬磁偏置 k = b' * A * b + 1; % 代入归一化常数 W = A / k; % (m-b)'*W*(m-b) = 1 end

校正时m_cal = sqrtm(W) * (m - b)，再乘上当地地磁总场强度还原成 µT。sqrtm要求 W 正定，如果解出来的特征值有负的，说明采集时朝向覆盖不够，尤其是没转到板子正下方，回去补数据比重解参数更有效。

磁力计校准完还有一步容易被忘：磁偏角。磁力计测的是地磁北，真北和磁北之间差一个磁偏角，做航向输出要么把偏角加回去，要么在评估时统一按磁北比较，两种都行，混着用就会出现“静止时航向差好几度”的假故障。

3.4 把标定系数回填成一次函数调用

标定算出来的 M、b、W 都是矩阵，散在脚本里迟早会丢。做法是统一存成一个calib.mat，里面按板子序列号分字段，解算入口只调一个函数完成全部校正，以后换板子只换文件不改代码。

function s = apply_calib(s, cal) % s 含 acc/gyr/mag 原始物理量，cal 为标定文件里的结构体 s.acc = (cal.M_acc \ (s.acc' - cal.b_acc))'; % Nx3，逐行处理 s.gyr = s.gyr - cal.bias_gyr; % 零偏直接减 s.mag = (sqrtm(cal.W_mag) * (s.mag' - cal.b_mag))' * cal.magNorm; end

magNorm是当地地磁总场强度，用来把归一化的球还原成 µT。校验方法是把标定前后的磁场模长画出来，标定前随朝向摆动可能超过 ±10 µT，标定后应该收在 ±1 µT 以内；加速度计同理，校正后六个面的模长都应该贴近 1 g。

4. 姿态解算核心：四元数互补滤波到 Madgwick 的逐段实现

前面的数据洗干净、标定系数回填之后，才轮到算法。这一章按递进顺序拆开：先用陀螺单独积分看漂移有多大，再引入加速度计和磁力计做互补校正，然后分别给出 Mahony 的 PI 结构和 Madgwick 的梯度下降结构，最后用 MATLAB 自带滤波器做交叉验证。每个解算器都写成单步函数，外面套一个循环，方便单步调试。

4.1 四元数微分方程与离散更新

四元数递推的核心就一个公式：q_dot = 0.5 · q ⊗ [0, ω]，其中 ω 是机体系角速度，⊗ 是四元数乘法。离散化用一阶欧拉就够，跑完归一化。写的时候注意四元数的分量顺序，本文统一用标量在前[w x y z]，和 MATLAB 自带quaternion类的顺序一致，避免和某些 C 库的[x y z w]混用。

function q = quat_integrate(q, w, dt) % q: [w x y z]，机体系->导航系；w: 机体系角速度 rad/s dq = 0.5 * quatmul(q, [0, w]); q = q + dq * dt; q = q / norm(q); % 不归一化，几十秒后必然发散 end function r = quatmul(a, b) % 四元数乘法，标量在前 r = [a(1)*b(1) - a(2)*b(2) - a(3)*b(3) - a(4)*b(4), ... a(1)*b(2) + a(2)*b(1) + a(3)*b(4) - a(4)*b(3), ... a(1)*b(3) - a(2)*b(4) + a(3)*b(1) + a(4)*b(2), ... a(1)*b(4) + a(2)*b(3) - a(3)*b(2) + a(4)*b(1)]; end

dt就是重采样后的固定步长，别用相邻时间戳现算，日志里一个跳变就能让这一帧转角翻倍。一阶积分的误差在低转速下可以忽略，角速度超过 300°/s 时建议换二阶或四阶，MATLAB 离线跑不在乎这点算力，但移植到 MCU 上要重新权衡。

4.2 Mahony 互补滤波：PI 校正与在线零偏估计

Mahony 的思路是把两个参考向量在机体系下的期望方向算出来，和实测方向做叉乘得到误差，误差乘比例增益直接修正角速度，再乘积分增益累加成零偏估计。重力参考是[0 0 1]，磁场参考用当前姿态估计出的水平分量，这样磁力计只参与航向修正，不会把倾角带偏。

function [q, bias] = mahony_step(q, bias, gyr, acc, mag, dt, Kp, Ki) % 输入均为机体系；q 为机体系->导航系四元数 acc = acc(:)' / norm(acc); mag = mag(:)' / norm(mag); R = q2R(q); v_acc = (R' * [0; 0; 1])'; % 重力在机体系的期望方向 mn = R * mag'; % 磁测转到导航系 v_mag = (R' * [norm(mn(1:2)); 0; mn(3)])'; % 只保留水平分量作参考 v_mag = v_mag / norm(v_mag); e = cross(acc, v_acc) + cross(mag, v_mag); % 两个叉乘误差相加，rad 量级 bias = bias - Ki * e * dt; % 积分项：在线估计残余零偏 w = gyr - bias + Kp * e; % 比例项：直接拉回角速度 q = quat_integrate(q, w, dt); end function R = q2R(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

Kp决定收敛速度，静止起步时给 1～5 收敛快，但噪声大的模块会抖，实际取值通常在 0.5～2 之间；Ki一般取Kp的百分之一量级，用来慢慢吃掉零偏，给太大反而会引起低频振荡。调参的判据是：静态下姿态标准差小于 0.5°，手动翻转后 2 秒内收敛到新姿态且不过冲。叉乘的顺序也要注意，cross(acc, v_acc)和反过来写差一个负号，结果是姿态往反方向修正，现象是倾角越来越大直到翻过去。

4.3 Madgwick 梯度下降：残差、雅可比与 beta 选择

Madgwick 把姿态估计写成一个最优化问题：找一个四元数，让“按当前姿态预测出的重力方向和磁场方向”与实测方向的残差最小，用梯度下降一步步逼近。相比 Mahony 的叉乘近似，它在初始误差大时收敛更稳，代价是计算量高一些。

function [q, bias] = madgwick_step(q, bias, gyr, acc, mag, dt, beta) acc = acc(:)' / norm(acc); mag = mag(:)' / norm(mag); % 由当前姿态估计参考方向（导航系） R = q2R(q); mn = R * mag'; v_m = [norm(mn(1:2)), 0, mn(3)]; v_m = v_m / norm(v_m); v_g = [0 0 1]; % 残差函数：把参考向量转回机体系，与实测相减 resid = @(qq) [ (q2R(qq)' * v_g')' - acc, (q2R(qq)' * v_m')' - mag ]'; f = resid(q)'; % 6x1 J = zeros(6, 4); % 数值雅可比，易读版 h = 1e-6; for i = 1:4 dq = zeros(1,4); dq(i) = h; J(:,i) = (resid(q + dq)' - f) / h; end grad = J' * f; % 梯度方向 if norm(grad) > 1e-9 gdir = grad / norm(grad); w = gyr - bias - beta * gdir'; % 梯度当作角速度修正项 else w = gyr - bias; end q = quat_integrate(q, w, dt); end

数值雅可比的好处是公式不容易写错、改参考向量时不用重推；代价是每次迭代多算四次旋转矩阵，实时性差。移植到 MCU 时一般换成解析雅可比，把 f 对四个四元数分量求导写死，能省五到十倍时间，离线评估阶段用数值版完全够。beta是梯度下降的步长，等效于 Mahony 里的Kp，取值通常在 0.03～0.1 之间，取大了姿态抖，取小了初始收敛慢，判断标准和上一节一样。

4.4 输出欧拉角与万向锁处理

内部全程用四元数，只在绘图和评估时转欧拉角。按 321 转序，从旋转矩阵元素反解出俯仰、横滚、偏航，俯仰接近 ±90° 时单独处理，约定横滚为 0，让偏航和横滚的组合唯一。

function [yaw, pitch, roll] = quat2eul_zyx(q) % 返回弧度，转序 ZYX（航空 321） R = q2R(q); pitch = asin(max(-1, min(1, -R(3,1)))); % 夹紧防止浮点越界 if abs(R(3,1)) > 0.99999 % 万向锁 roll = 0; yaw = atan2(-R(1,2), R(2,2)); else roll = atan2(R(3,2), R(3,3)); yaw = atan2(R(2,1), R(1,1)); end end

asin外面套max/min不是多余动作，浮点误差会让-R(3,1)略微超过 1，直接asin返回复数，后面全变 NaN。万向锁附近俯仰角的分辨率急剧下降，评估时最好把这一段单独标出来，别把量化误差算到算法头上。航向角输出前记得处理 ±π 跳变，画图时用unwrap展开，否则曲线会出现垂直的长竖线。

4.5 用 ahrsfilter 做交叉验证

自己写的解算器跑出曲线之后，最省事的验证方式是拿 MATLAB 自带的ahrsfilter跑同一份数据做对比。它是内置的互补滤波实现，参数调好之后两条曲线的趋势应该高度重合，差得多的那段往往就是自己代码里符号或者顺序出了问题。

% acc 单位必须是 m/s^2，gyr 是 rad/s，mag 是 uT fuse = ahrsfilter('SampleRate', fs, 'ReferenceFrame', 'NED', ... 'OrientationFormat', 'quaternion'); q_ref = fuse(acc * 9.80665, gyr, mag); % 注意 g 换成 m/s^2 eul_ref = euler(q_ref, 'ZYX', 'frame'); % 自带函数，转序参数别写错 err = rad2deg(wrapToPi(eul_mine - eul_ref)); fprintf('与内置滤波器的 RMS 偏差: %.2f deg\n', rms(err));

单位是这个对比里最容易翻车的点，ahrsfilter的加速度输入要求 m/s²，喂 g 进去它照样能跑，但内部的重力常数对不上，收敛出来的倾角会差一个比例。转序参数'frame'和'point'的含义不同，前者是坐标系旋转，后者是向量旋转，两者差一个转置，选错的话偏航角会整体反号。对比时只看稳态段，起始那几百毫秒是收敛过程，差异大属于正常。

5. 避坑与排查：五个让人怀疑传感器坏了的现场

调这块代码的头两年，我有一半时间花在怀疑硬件上，最后发现全是自己挖的坑。下面五条是按出现频率排的，每条都给出当时的现场、根因和改法，对着排查能省掉不少无意义的换板子。

第一条：姿态转得飞快或者几乎不动。现象是给板子转 90°，解算出来的角度只走了几度，或者反过来转一点点就冲出去好几圈。根因基本都是陀螺仪单位，dps 没转 rad/s，四元数微分方程里 ω 大 57 倍或者小 57 倍。解决办法是在数据读入后打印一段已知转动的积分角度，和实际转角比对，误差在 5% 以内才算单位对。

第二条：跑几十秒后角度变成 NaN。现象是前几秒曲线正常，之后突然崩掉，图上出现空白。根因是四元数没有归一化，模长在递推中缓慢漂移，或者某个时刻dt异常大导致一步积分过头，再被asin放大成复数。解决办法是每次递推后强制q = q/norm(q)，并且在重采样阶段就保证dt恒定，asin的输入做饱和截断。

第三条：静止时航向缓慢自转，倾角正常。现象是俯仰横滚都稳，只有偏航一直往一个方向爬。根因通常是磁力计没做椭球校准，硬磁偏置被当成真实磁场，或者电机通电后磁场被干扰。解决办法是先离线校准，装到机架上之后再采一圈数据看磁场模长波动，波动超过 2 µT 就要考虑做现场重新校准，或者把磁力计的权重降下来。

第四条：坐标系或转序约定前后不一致，姿态镜像。现象是能跟住运动，但左右反了，或者俯仰正常、横滚反向。根因是某一段代码里四元数取了共轭，或者机体系定义成了 z 轴向上而参考向量还是[0 0 1]。解决办法是拿一个确定的姿态做单点验证：板子绕 x 轴正向转 30°，横滚角应该输出 +30°，符号不对就回去查旋转矩阵的定义方向，别靠改增益硬掰。

第五条：动态机动时倾角被“带跑”。

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