咱们玩无人机或者看手机屏幕自动旋转时，背后都藏着IMU的姿态解算。今天用Matlab手撕一套四元数姿态解算方案，直接上硬核代码！（文末附完整工程）
37.基于matlab的IMU姿态解算,姿态类型为四元数；角速度和线加速度的类型为三维向量。 IMU全称是惯性导航系统，主要元件有陀螺仪、加速度计和磁力计。 其中陀螺仪可以得到各个轴的加速度，而加速度计能得到x，y，z方向的加速度，而磁力计能获得周围磁场的信息。 主要的工作便是将三个传感器的数据融合得到较为准确的姿态信息。 程序已调通，可直接运行。
先搞明白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全称是惯性导航系统，主要元件有陀螺仪、加速度计和磁力计。 其中陀螺仪可以得到各个轴的加速度，而加速度计能得到x，y，z方向的加速度，而磁力计能获得周围磁场的信息。 主要的工作便是将三个传感器的数据融合得到较为准确的姿态信息。 程序已调通，可直接运行。
实际跑起来要注意几个坑：
- 陀螺仪数据必须转弧度制
- 加速度计数据需要做低通滤波（别让高频振动干扰）
- 采样时间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更新率，足够平衡车这类应用。想进阶的话可以上卡尔曼滤波，不过对于多数应用场景，这个简版算法已经能打十个了。