Attitude estimation (complementary filtering)

Reference material ：
A Complementary Filter for Attitude Estimation of a Fixed-Wing UAV
mahony Complementary filter
px4 Source code v1.7.3

Quaternion attitude solution

While looking at the program, talk about the principle ：

AttitudeEstimatorQ::init()

The process of getting the initial quaternion ：
1、 First built on NED Rotation matrix under system
$\vec{k}$ by z Axis , Direction is D towards , Vertical down

    // 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}$ yes NED In a coordinate system x Axis , From Schmidt orthogonalization ：

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

Schmidt orthogonalization ：

$$\beta_2 =\alpha_2-\frac{(\alpha_2, \beta_1)}{(\beta_1,\beta_1)}\cdot \beta_1$$
$\vec{k}$ It has been normalized, so the denominator is 1

$\vec{j}$ yes NED In a coordinate system y Axis , from $\vec{i}$,$\vec{k}$ Cross multiply to get the vertical vector

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

2、 Fill the rotation matrix , And convert to quaternion ：

    // 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)

Four yuan number $Q$ And rotation matrix $R$ The transformation of

$$\begin{Bmatrix} q_0=\frac{1}{2}\sqrt{(1+r_{11}+r_{22}+r_{33})}\\ q_1=\frac{1}{4q_0}(r_{32}-r_{23})\\ q_2=\frac{1}{4q_0}(r_{31}-r_{13})\\ q_3=\frac{1}{4q_0}(r_{21}-r_{12}) \end{Bmatrix}$$

3、 After correction according to magnetic declination , Normalize quaternions , Initialization complete .

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

_q.normalize();

update(); Update quaternion

1、 Execute the initialization function once , Get the first quaternion

    if (!_inited) {

        if (!_data_good) {
            return false;
        }

        return init();
    }

2、px4 The means of calibration in includes vision , Motion capture , Magnetometer ; The first two belong to external heading information and will not be discussed , Only calibrate the magnetometer .

    if (_ext_hdg_mode == 0 || !_ext_hdg_good) {
        //  Magnetometer calibration 
        //  Convert from volume coordinate system to navigation coordinate system 
        Vector<3> mag_earth = _q.conjugate(_mag);    

        float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);// Make a difference with the automatically obtained magnetic declination 
        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();

First, convert the magnetometer reading to the navigation coordinate system ：

$$mag\_earth=R^n_b*\_mag$$

The output of the magnetometer is the angle between the current body and the geomagnetic field . The measuring principle is similar to that of a compass . Good low frequency characteristics , But it is easily disturbed by the surrounding magnetic field .

px4 of use GPS Information for calibration :
Convert the reading of magnetometer from vector to angle ; This angle is subtracted by GPS The magnetic bias obtained _mag_decl ; Get the magnetic field deviation mag_err;

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

*arctan and arcsin The result is $[-\frac\pi2,\frac\pi2]$, This does not cover all orientations ( But for the $\theta$ The value range of angle has met ), So we need to use atan2f Instead of arctan, The value range becomes $[-\pi,\pi]$.
**_wrap_pi Yes, it will $[0,2\pi]$ Mapping to $[-\pi,\pi]$ On .

take mag_err Convert to the body coordinate system , And multiply by the weight

$$R^b_n*\begin{bmatrix} 0\\0\\-mag\_err \end{bmatrix}*\_w\_mag$$
_q.normalize(); Quaternion normalization , The normalization here is used for correction update_mag_declination Deviation after .

3、 Accelerometer calibration

    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$ Represents the vector of gravity vector in the body coordinate system :

$$k=\begin{bmatrix}k_x\\k_y\\k_z\end{bmatrix}=R^b_n*\begin{bmatrix}0\\0\\1\end{bmatrix}\\ =\begin{bmatrix}2(q_1q_3-q_0q_2)\\2(q_2q_3+q_0q_1)\\q_0^2-q_1^2-q_2^2+q_3^2\end{bmatrix}$$
$e_{acc}$ By $\Delta {q_{acc}}$ and $k$ Cross multiply to get ; $\Delta {q_{acc}}$ It is obtained by the accelerometer through low-pass filtering .

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

_accel Is the accelerometer reading ;
_pos_acc Is estimated by location (GPS) The acceleration obtained by differentiation ;
_accel - _pos_acc Represents the acceleration vector of the aircraft minus its horizontal component ;
k And (_accel - _pos_acc) Cross riding , Get deviation ;

Program so far ,$corr=e_{acc}+e_{mag}$

4、 Correct the gyroscope

    // Gyro bias estimation
    if (spinRate < 0.175f) {
        _gyro_bias += corr * (_w_gyro_bias * dt);// The correction value here is  mahony  Filtered  pi  Controller   Integral part ; 

        // Gyro deviation upper limit 
        for (int i = 0; i < 3; i++) {
            _gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
        }

    }

5、_gyro_bias Over time , Non return to zero

$$\_gyro\_bias=(e_{acc}+e_{mag})*w_{gyro\_bias}*dt$$

_rates = _gyro + _gyro_bias;

$$\omega=\omega_{gyro}+\delta$$

corr += _rates;

PI The control of the P term ,$corr=\_rates+corr+k_i \int corr dt$
At this time $corr$ Angular velocity obtained for final calibration

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

6、 First order Runge Kutta derivation ：

$$\dot{q}=f(q,\omega)$$

$\omega= corr$

$$q(t+T)=q(t)+T\cdot f(q,\omega)$$

So we get quaternion update matrix ：

$$q(t+T)=\begin{bmatrix}q_0\\q_1\\q_2\\q_3\end{bmatrix}+ \frac T2 \begin{bmatrix} 0&-\omega_x&-\omega_y&-\omega_z \\ \omega_x&0&\omega_z&-\omega_y \\ \omega_y&-\omega_z&0&\omega_x\\ \omega_z&\omega_y&-\omega_x&0 \end{bmatrix}\begin{bmatrix}q_0\\q_1\\q_2\\q_3\end{bmatrix}$$

    // Check whether quaternions diverge , If it is divergent, use the last updated 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;
    }