1.目标

• 地理坐标系的角速度

2.已知量

• 机体坐标系的角速度 gyro_x, gyro_y,gyro_z;
• 欧拉角，pitch，roll，yaw，参考我的上一章节姿态解算知识点1——四元数互补滤波

​​​​​​​3.算法总框图

 

4.数学模型和公式推导

这里坐标系取东北天 --- 右前上

由以上求得地理坐标系的角速度为：

5.代码实现

 

6.数据曲线

• 地理坐标系角度速率曲线效果图

红色曲线-pitch_rate

绿色曲线- roll_rate

棕色曲线- yaw_rate

 

7.参考文献

• 惯性导航——秦永元
• 惯性导航基本原理——刘保中