姿态估计（互补滤波）

参考材料：
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} \frac{(α_{2}, β_{1})}{(β_{1}, β_{1})} β_{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)

四元数 $Q$ 与旋转矩阵 $R$ 的转换关系

${\begin{matrix} q_{0} = \frac{1}{2} \sqrt{(1 + r_{11} + r_{22} + r_{33})} \\ q_{1} = \frac{1}{4 q_{0}} (r_{32} r_{23}) \\ q_{2} = \frac{1}{4 q_{0}} (r_{31} r_{13}) \\ q_{3} = \frac{1}{4 q_{0}} (r_{21} r_{12}) \end{matrix}}$

3、根据磁偏角矫正后，将四元数归一化，完成初始化。

// Compensate for magnetic declination
Quaternion decl_rotation;
decl_rotation.from_yaw(_mag_decl);
_q = decl_rotation * _q;

_q.normalize();

update();更新四元数

1、执行一次初始化函数，得到第一个四元数

    if (!_inited) {

        if (!_data_good) {
            return false;
        }

        return init();
    }

2、px4中校准的手段有视觉，动作捕捉，磁力计；其中前两者属于外部航向信息不做讨论，只做磁力计校准。

    if (_ext_hdg_mode == 0 || !_ext_hdg_good) {
        // 磁力计校准
        // 从体坐标系转换到导航坐标系
        Vector<3> mag_earth = _q.conjugate(_mag);    

        float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);//与自动获取的磁偏角做差
        float gainMult = 1.0f;
        const float fifty_dps = 0.873f;

        if (spinRate > fifty_dps) {
            gainMult = math::min(spinRate / fifty_dps, 10.0f);
        }

        // Project magnetometer correction to body frame
        corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag * gainMult;
    }

    _q.normalize();

首先将磁力计读数转换到导航坐标系上：

m a g_e a r t h = R_{b}^{n}_m a g

磁力计的输出为当前机体与地磁场的夹角。测量原理与指南针相似。低频特性较好，但易受周围磁场干扰。

px4中用GPS信息进行校准:
将磁力计的读数由向量换算到角度; 该角度减去由GPS得到的磁偏_mag_decl ;得到磁场偏差mag_err;

float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);

*arctan和arcsin的结果是 $[- \frac{π}{2}, \frac{π}{2}]$ ，这并不能覆盖所有朝向(但对于 $θ$ 角的取值范围已经满足)，因此需要用atan2f来代替arctan，取值范围变成 $[- π, π]$ 。
**_wrap_pi是将 $[0, 2 π]$ 映射到 $[- π, π]$ 上。

将mag_err转换到机体坐标系上，并乘以权重

R_{n}^{b} [\begin{matrix} 0 \\ 0 \\ m a g_e r r \end{matrix}]_w_m a g

_q.normalize();四元数归一化，这里的归一化用于校正update_mag_declination后的偏差。

3、加速度计校准

    Vector<3> k(
        2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
        2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
        (_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
    );

$k$ 表示重力向量在机体坐标系的向量:

k = [\begin{matrix} k_{x} \\ k_{y} \\ k_{z} \end{matrix}] = R_{n}^{b} [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] = [\begin{matrix} 2 (q_{1} q_{3} q_{0} q_{2}) \\ 2 (q_{2} q_{3} + q_{0} q_{1}) \\ q_{0}^{2} q_{1}^{2} q_{2}^{2} + q_{3}^{2} \end{matrix}]

$e_{a c c}$ 是由 $Δ q_{a c c}$ 和 $k$ 叉乘得到； $Δ q_{a c c}$ 是加速度计通过低通滤波得到的。

corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;

_accel 是加速度计的读数;
_pos_acc 是由位置估计(GPS) 微分得到的加速度;
_accel - _pos_acc 表示飞行器加速度向量减去其水平分量;
k 与 (_accel - _pos_acc)叉乘，得到偏差;

程序到现在为止， $c o r r = e_{a c c} + e_{m a g}$

4、修正陀螺仪

    // Gyro bias estimation
    if (spinRate < 0.175f) {
        _gyro_bias += corr * (_w_gyro_bias * dt);//此处修正值为 mahony 滤波的 pi 控制器的 积分部分; 

        //陀螺仪偏差上限
        for (int i = 0; i < 3; i++) {
            _gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
        }

    }

5、_gyro_bias随时间不断累积，不归零

_g y r o_b i a s = (e_{a c c} + e_{m a g}) w_{g y r o_b i a s} d t

_rates = _gyro + _gyro_bias;

ω = ω_{g y r o} + δ

corr += _rates;

PI控制的P项， $c o r r =_r a t e s + c o r r + k_{i} \int c o r r d t$
此时的 $c o r r$ 为最终校准得到的角速度

_q += _q.derivative(corr) * dt;

6、一阶龙格库塔求导：

\dot{q} = f (q, ω)

$ω = c o r r$

q (t + T) = q (t) + T f (q, ω)

所以得到四元数更新矩阵：

q (t + T) = [\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix}] + \frac{T}{2} [\begin{matrix} 0 & ω_{x} & ω_{y} & ω_{z} \\ ω_{x} & 0 & ω_{z} & ω_{y} \\ ω_{y} & ω_{z} & 0 & ω_{x} \\ ω_{z} & ω_{y} & ω_{x} & 0 \end{matrix}] [\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix}]

    //检查四元数是否发散，若果发散则使用上一次更新的q

    if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
          PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {

        _q = q_last;
        _rates.zero();
        _gyro_bias.zero();
        return false;
    }

姿态估计（互补滤波）

四元数姿态解算

AttitudeEstimatorQ::init()

update();更新四元数

浏览过的版块