咱们玩无人机或者看手机屏幕自动旋转时，背后都藏着IMU的姿态解算。今天用Matlab手撕一套四元数姿态解算方案，直接上硬核代码！（文末附完整工程）

37.基于matlab的IMU姿态解算,姿态类型为四元数角速度和线加速度的类型为三维向量。 IMU全称是惯性导航系统主要元件有陀螺仪、加速度计和磁力计。 其中陀螺仪可以得到各个轴的加速度而加速度计能得到xyz方向的加速度而磁力计能获得周围磁场的信息。 主要的工作便是将三个传感器的数据融合得到较为准确的姿态信息。 程序已调通可直接运行。先搞明白IMU传感器的三板斧陀螺仪给角速度加速度计给线加速度磁力计测磁场。不过咱们这次先不用磁力计纯靠陀螺仪和加速度计玩姿态融合。上主菜——核心算法流程% 初始化四元数躺着开机的情况 q [1 0 0 0]; % 主循环 for k 2:length(t) % 读取陀螺仪数据 (rad/s) gyro [wx(k), wy(k), wz(k)]; % 四元数更新陀螺仪积分 q quatmultiply(q, quat_exp(gyro * dt)); % 加速度计校正 acc [ax(k), ay(k), az(k)]; if norm(acc) ~ 0 acc acc / norm(acc); v quatrotate(q, [0 0 1]); % 理论重力方向 error cross(v, acc); q quatmultiply(q, quat_exp(alpha * error * dt)); end % 存储四元数 q_hist(k,:) q; end这段代码藏着两个关键点用陀螺仪做姿态预测用加速度计做校正。quat_exp函数实现四元数微分方程的积分这里用了最简化的处理function q quat_exp(w) delta_theta norm(w); if delta_theta 0 q [1 0 0 0]; else axis w / delta_theta; q [cos(delta_theta/2), axis*sin(delta_theta/2)]; end end加速度计校正环节才是精髓所在。当设备静止时加速度计测量的应该是重力方向。咱们通过计算当前姿态的理论重力方向与实际测量的夹角用叉乘得到旋转误差v quatrotate(q, [0 0 1]); % 理论重力方向 error cross(v, acc); % 叉乘得到旋转轴这个error向量直接对应到陀螺仪的角速度补偿上相当于在陀螺仪数据里混入修正量。参数alpha控制着校正力度一般在0.01到0.1之间调参。37.基于matlab的IMU姿态解算,姿态类型为四元数角速度和线加速度的类型为三维向量。 IMU全称是惯性导航系统主要元件有陀螺仪、加速度计和磁力计。 其中陀螺仪可以得到各个轴的加速度而加速度计能得到xyz方向的加速度而磁力计能获得周围磁场的信息。 主要的工作便是将三个传感器的数据融合得到较为准确的姿态信息。 程序已调通可直接运行。实际跑起来要注意几个坑陀螺仪数据必须转弧度制加速度计数据需要做低通滤波别让高频振动干扰采样时间dt要精确测量用互补滤波融合时调试参数就像调鸡尾酒——alpha太大校正过猛会引入加速度计噪声太小又抑制不了陀螺仪漂移。建议先用仿真数据试参数% 生成仿真数据绕X轴匀速旋转 t 0:0.01:10; wx 0.5 * ones(size(t)); % 0.5 rad/s wy zeros(size(t)); wz zeros(size(t));跑完算法用四元数转欧拉角看看曲线正常应该得到一条漂亮的斜直线。如果出现发散或者震荡检查四元数归一化有没有做。完整工程里还藏着几个实用技巧启动时用加速度计做初始对准四元数每次更新后强制归一化动态调整alpha参数运动剧烈时降低校正权重实测这套方案在STM32上能跑到500Hz更新率足够平衡车这类应用。想进阶的话可以上卡尔曼滤波不过对于多数应用场景这个简版算法已经能打十个了。