Euler angle solution for EKF attitude estimation in Pixhawk flight control

The master does not indicate whether it is the original firmware or APM version of the firmware, the original firmware has been canceled EKF attitude solution, the official explanation is because the ram is too large. And think you're asking for the original firmware. EKF Attitude solving algorithm, simple introduction, do not understand, and related theories are not clear to see the introduction of inertial navigation books can, should not have many people need to really study the algorithm bar ~
The first is modeling, setting state variables,

Where w is the system angular velocity, WA is the system angular acceleration, ze is the gravity vector, and mu is the magnetic field vector. If you use the inertia matrix to correct the current angular acceleration, then:

The angular velocity of the machine system is:

The rotational angular velocity transformation matrix based on the relative geographic coordinate system of the body coordinate system:

If the first-order Taylor approximation with exponential functions is:

Set

Then the state transfer a matrix is:

The process noise matrix is:

The process noise of gyro angular velocity, angular acceleration process noise, acceleration process noise and magnetometer process noise are respectively represented.

Covariance matrices are:

Measuring the Noise matrix:

The observation matrix H is:

The measurement equation is:

Introduce measurement noise:

EKF's Kalman Gain matrix:

State estimation:

To update the covariance matrix:

Extracting status information:

Finally, the transformation matrix of reference system and machine is obtained:

Convert into Euler angle:

The EKF method is computationally large, the first-order approximation is used in the calculation of the state transition matrix and the basic matrices, and the approximate error is large, and if the computational amount is allowed, 2-3 orders can be used.

Native firmware about EKF solution, embedded code is automatically generated by MATLAB, do not directly see C code ....

Euler angle solution for EKF attitude estimation in Pixhawk flight control