ardupilot 及PX4姿态误差计算算法对比分析

目录

摘要

本节主要记录ardupilot 及PX4姿态误差计算算法差异对比过程，欢迎批评指正。

备注：
1.创作不易，有问题急时反馈
2.需要理解四元物理含义、叉乘及点乘含义、方向余弦矩阵含义、四元数乘法物理含义、四元数“差”的物理含义、四元数连乘法则及物理意义、三个坐标轴对应的四元数的含义、四元数和旋转矩阵的关系、四元数轴角方程、四元数和旋转矢量转换关系等
3.注意叉乘的顺序，物理含义的理解
4.PX4算法的本质难点关键是理解四元数乘法的本质含义：按照旋转矩阵去理解

1.APM姿态误差计算算法

2.PX4姿态误差计算算法

3.结论

APM过程：
1.根据实际姿态及目标姿态相关计算倾斜误差四元数；
2.把倾斜误差四元数转换到机体坐标系下；
3.通过轴角方程得到机体坐标系下倾斜姿态误差矢量（横滚+俯仰姿态误差矢量）；
4.根据四元数“差”相关理论，计算“总的姿态误差四