低端 MCU 上做传感器融合,我的建议是:主循环用定点,标定和离线参数用浮点,两边分工。
没有 FPU 的 Cortex-M0/M3 上,单精度浮点是软件仿真,一次乘加可能要几十个周期,卡尔曼或互补滤波每周期跑几十次浮点运算,采样率一高就吃不消。改成定点后,核心是选好 Q 格式:陀螺仪角速度、加速度先归一化再定标,比如 Q12/Q14,乘法后及时右移防溢出,累加器用 32 位,中间结果用 64 位暂存。互补滤波这种一阶结构用定点几乎没有精度损失,实测姿态角误差和浮点版差不到 0.1 度。
卡尔曼的协方差更新对动态范围要求高,纯定点容易溢出或下溢,折中办法是状态更新用定点,协方差和增益用查表或分段近似,或者把卡尔曼降采样到 50-100Hz 跑,内层姿态积分仍用定点高频跑。还有一点容易被忽略:定点之前先把传感器零偏和温漂标定好,零偏没扣干净时,定点的量化误差会被积分放大,表现为姿态慢慢漂移,这时换浮点也救不回来。先确认主控有没有 FPU、采样率和滤波阶数,再定 Q 格式,比直接二选一更稳妥。