![](https://csdnimg.cn/release/download_crawler_static/10474464/bg1.jpg)
PX4 四元数姿态估计算法
代码位置 PX4/Firmware/src/…/Attitude_estimate_q_main.cpp)
1. int AttitudeEstimatorQ::start( ) 程序启动函数
2. void AttitudeEstimatorQ::task_main( ) 进程入口
3. float gyro[3] 获取传感器数据,DataValidatorGroup,并检验数据可靠度
4. 通过 uORB 模型获取 vision 和 mocap 的数据
5. 位置加速度获取(_pos_acc)
6. 对四元数向量_q 进行初始化赋值与更新 update
7. 将_q 转换成欧拉角 euler 并发布 _q.to_euler
对两个坐标系进行实时的标定和修正。因为坐标系有三个轴,偏航 yaw 修正由电子罗
盘(基于载体)、地磁(基于地理)对比修正误差补偿得到。俯仰 pitch 和横滚 roll 上的修正
由加速度计(基于载体)、重力(基于地理)对比修正误差得到。
Vector<3> k = -_accel;
k.normalize();
Vector<3> i = (_mag - k * (_mag * k));
i.normalize();
四元数初始化:
仅用加速度计和磁力计生成姿态四元数矩阵 。
加速度计测量的是目标向量与 b 系三轴的夹角,假设加
速度计输出为
,z 轴向上;加速度计能感应重力,
重力向量指向地心,由于定义的坐标系为 NED,z 轴向
下;k 为加速度计测量到的加速度方向向量,
归一化:
=
磁力计可以感应地理的北极。假设磁力计的输出为
。
为防止磁力计向量与加速度向量 k 不垂直,计算△
k=k·(_mag·k)=
为_mag 在 k 轴上
的投影,
通过 i = _mag - △k
=
,
T
可以得到一个与 k 轴垂直的向量。并进行正交化后得到
指向北的单位向量。i==