【IMU】6轴数据校准算法
算法概述
本算法用于惯性测量单元的自动校准，通过多次采样和优化选择，确定传感器的最佳偏移量。
算法步骤
- 初始化
- converged = false（加速度计收敛标志）
- new_offset = {0,0,0}（最优偏移量）
- last_average = {0,0,0}（上次平均值）
-
主循环（重复CALIB_CYCLES次）
FOR i = 1 to CALIB_CYCLES:步骤1.1: 采集50次传感器数据，计算平均值this_avg步骤1.2: 计算与上次平均值的差异this_diff = norm(last_avg - this_avg)步骤1.3: 判断收敛，和最佳值更新步骤2.4: 更新last_average = this_avg步骤1.3: 判断收敛，和最佳值更新:IF this_diff < 1.0:- 更新滑动平均：last_average = 0.5 × this_avg + 0.5 × last_average- 如果首次收敛或新值更优，更新 new_offset = last_average- 设置 converged = trueelse if (this_diff < best_diff): // 未收敛但更优- 更新最优平均值：best_avg = 0.5×acc_avg + 0.5×acc_last_average- 更新最优差值：best_diff = this_diff如果收敛则取new_offset作为最后的offset值，否则使用best_avg作为offset值