文章目录 原始代码 全局变量定义 逆平方根函数 四元数解算函数 理论解释 四元数 加速度计数据归一化 计算方向余弦矩阵的第三行 计算误差 计算并应用积分反馈 应用比例反馈 积分陀螺仪数据,更新四元数 归一化四元数 更新姿态数据 整体流程 原始代码 #define SAMPLING_FREQ 20.0f // 采样频率 /************************************** Function: 平方根倒数 求四元数用到 ***************************************/ float InvSqrt(float number) { volatile long i; volatile