【自动驾驶】通过下位机发送的加速度、角速度计算机器人在世界坐标系中的姿态
文章目录
- 原始代码
- 全局变量定义
- 逆平方根函数
- 四元数解算函数
- 理论解释
- 四元数
- 加速度计数据归一化
- 计算方向余弦矩阵的第三行
- 计算误差
- 计算并应用积分反馈
- 应用比例反馈
- 积分陀螺仪数据，更新四元数
- 归一化四元数
- 更新姿态数据
- 整体流程
原始代码
#define SAMPLING_FREQ 20.0f // 采样频率
/**************************************
Function: 平方根倒数 求四元数用到
***************************************/
float InvSqrt(float number)
{volatile long i;volatile