姿态估计（互补滤波）

最新推荐文章于 2024-07-01 15:09:14 发布

r1ch4rd

最新推荐文章于 2024-07-01 15:09:14 发布

阅读量2.1k

点赞数 2

本文链接：https://blog.csdn.net/r1ch4rd/article/details/80473348

版权

本文介绍了无人机姿态估计中使用四元数和互补滤波器的方法。首先，通过施密特正交化建立NED坐标系下的旋转矩阵并转换为四元数进行初始化。接着，利用磁力计和加速度计的数据，结合GPS信息进行校准和误差修正。通过PI控制和一阶龙格库塔方法更新四元数，以实现稳定姿态估计。

摘要由CSDN通过智能技术生成

参考材料：
A Complementary Filter for Attitude Estimation of a Fixed-Wing UAV
mahony 互补滤波器
px4源码v1.7.3

四元数姿态解算

一边看程序一边讲原理：

AttitudeEstimatorQ::init()

得到初始四元数的过程：
1、首先建立在NED系下的旋转矩阵
$\vec{k}$ 为z轴，方向为D向，竖直向下

    // Rotation matrix can be easily constructed from acceleration and mag field vectors
    // 'k' is Earth Z axis (Down) unit vector in body frame

    Vector<3> k = -_accel;
    k.normalize();

$\vec{i}$ 是NED坐标系下的x轴，由施密特正交化得出：

    //'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
    Vector<3> i = (_mag - k * (_mag * k));
    i.normalize();

施密特正交化：

$β 2 = α 2 - ( α 2 , β 1 ) ( β 1 , β 1 ) \cdot β 1$ $\beta_2 =\alpha_2-\frac{(\alpha_2, \beta_1)}{(\beta_1,\beta_1)}\cdot \beta_1$
$\vec{k}$ 已经归一化所以分母为1

$\vec{j}$ 是NED坐标系下的y轴，由 $\vec{i}$ ， $\vec{k}$ 叉乘得到垂直向量

    // 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
    Vector<3> j = k % i;

2、填充旋转矩阵，并且转换为四元数：

    // Fill rotation matrix
    Matrix<3, 3> R;
    R.set_row(0, i);
    R.set_row(1, j);
    R.set_row(2, k);

    // Convert to quaternion
    _q.from_dcm(R)