rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-31 16:52:10 +01:00
Code Issues Releases Activity

LP-443 - EKF14 - Apply "2D" mag handling for EKF13 as well

Browse Source
This commit is contained in:
Alessio Morale 2016-07-22 00:41:13 +02:00
parent 34202a670e
commit 8e484dd862
1 changed files with 22 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
flight/libraries/insgps13state.c
View File

@ -402,9 +402,28 @@ void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[
if (SensorsUsed & MAG_SENSORS) {
Z[6] = mag_data[0];
Z[7] = mag_data[1];
Z[8] = mag_data[2];
// magnetometer data in any units (use unit vector) and in body frame
float Rbe_a[3][3];
float q0 = ekf.X[6];
float q1 = ekf.X[7];
float q2 = ekf.X[8];
float q3 = ekf.X[9];
float k1 = 1.0f / sqrtf(powf(q0 * q1 * 2.0f + q2 * q3 * 2.0f, 2.0f) + powf(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3, 2.0f));
float k2 = sqrtf(-powf(q0 * q2 * 2.0f - q1 * q3 * 2.0f, 2.0f) + 1.0f);
Rbe_a[0][0] = k2;
Rbe_a[0][1] = 0.0f;
Rbe_a[0][2] = q0 * q2 * -2.0f + q1 * q3 * 2.0f;
Rbe_a[1][0] = k1 * (q0 * q1 * 2.0f + q2 * q3 * 2.0f) * (q0 * q2 * 2.0f - q1 * q3 * 2.0f);
Rbe_a[1][1] = k1 * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3);
Rbe_a[1][2] = k1 * sqrtf(-powf(q0 * q2 * 2.0f - q1 * q3 * 2.0f, 2.0f) + 1.0f) * (q0 * q1 * 2.0f + q2 * q3 * 2.0f);
Rbe_a[2][0] = k1 * (q0 * q2 * 2.0f - q1 * q3 * 2.0f) * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3);
Rbe_a[2][1] = -k1 * (q0 * q1 * 2.0f + q2 * q3 * 2.0f);
Rbe_a[2][2] = k1 * k2 * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3);
Z[6] = Rbe_a[0][0] * mag_data[0] + Rbe_a[1][0] * mag_data[1] + Rbe_a[2][0] * mag_data[2];
Z[7] = Rbe_a[0][1] * mag_data[0] + Rbe_a[1][1] * mag_data[1] + Rbe_a[2][1] * mag_data[2];
Z[8] = Rbe_a[0][2] * mag_data[0] + Rbe_a[1][2] * mag_data[1] + Rbe_a[2][2] * mag_data[2];
}
// barometric altimeter in meters and in local NED frame