mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-14 05:53:33 +08:00
Merge pull request #49 from PX4/pr-fixMagCovariance
EKF: Fix magnetometer fusion and covariance handling
This commit is contained in:
Paul Riseborough
+2
-1
@@ -141,7 +141,7 @@ struct parameters {
|
||||
float mag_declination_deg = 0.0f; // magnetic declination in degrees
|
||||
float heading_innov_gate = 3.0f; // heading fusion innovation consistency gate size in standard deviations
|
||||
float mag_innov_gate = 3.0f; // magnetometer fusion innovation consistency gate size in standard deviations
|
||||
int mag_declination_source = 7; // bitmask used to control the handling of declination data
|
||||
int mag_declination_source = 3; // bitmask used to control the handling of declination data
|
||||
int mag_fusion_type = 0; // integer used to specify the type of magnetometer fusion used
|
||||
|
||||
// these parameters control the strictness of GPS quality checks used to determine uf the GPS is
|
||||
@@ -220,6 +220,7 @@ union filter_control_status_u {
|
||||
uint8_t mag_dec : 1; // 6 - true if synthetic magnetic declination measurements are being fused
|
||||
uint8_t in_air : 1; // 7 - true when the vehicle is airborne
|
||||
uint8_t armed : 1; // 8 - true when the vehicle motors are armed
|
||||
uint8_t wind : 1; // 9 - true when wind velocity is being estimated
|
||||
} flags;
|
||||
uint16_t value;
|
||||
};
|
||||
|
||||
@@ -145,6 +145,14 @@ void Ekf::controlFusionModes()
|
||||
_control_status.flags.mag_dec = false;
|
||||
}
|
||||
|
||||
// Placeholder for control of wind velocity states estimation
|
||||
// TODO add methods for true airspeed and/or sidelsip fusion or some type of drag force measurement
|
||||
if (false) {
|
||||
_control_status.flags.wind = false;
|
||||
}
|
||||
|
||||
// Store the status to enable change detection
|
||||
_control_status_prev.value = _control_status.value;
|
||||
}
|
||||
|
||||
void Ekf::calculateVehicleStatus()
|
||||
|
||||
+237
-184
@@ -81,19 +81,22 @@ void Ekf::initialiseCovariance()
|
||||
// accel z bias
|
||||
P[15][15] = 0.0001f;
|
||||
|
||||
// variances for optional states
|
||||
// these state variances are set to zero until the states are required, then they must be initialised
|
||||
|
||||
// earth magnetic field
|
||||
P[16][16] = 0.0001f;
|
||||
P[17][17] = 0.0001f;
|
||||
P[18][18] = 0.0001f;
|
||||
P[16][16] = 0.0f;
|
||||
P[17][17] = 0.0f;
|
||||
P[18][18] = 0.0f;
|
||||
|
||||
// body magnetic field
|
||||
P[19][19] = 0.0001f;
|
||||
P[20][20] = 0.0001f;
|
||||
P[21][21] = 0.0001f;
|
||||
P[19][19] = 0.0f;
|
||||
P[20][20] = 0.0f;
|
||||
P[21][21] = 0.0f;
|
||||
|
||||
// wind
|
||||
P[22][22] = 0.01f;
|
||||
P[23][23] = 0.01f;
|
||||
P[22][22] = 0.0f;
|
||||
P[23][23] = 0.0f;
|
||||
|
||||
}
|
||||
|
||||
@@ -131,9 +134,33 @@ void Ekf::predictCovariance()
|
||||
float d_ang_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1e-4f);
|
||||
float d_vel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1e-2f);
|
||||
float d_ang_scale_sig = dt * math::constrain(_params.gyro_scale_p_noise, 0.0f, 1e-2f);
|
||||
float mag_I_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f);
|
||||
float mag_B_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f);
|
||||
float wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f);
|
||||
float mag_I_sig, mag_B_sig;
|
||||
|
||||
// Don't continue to grow the body field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
|
||||
if (_control_status.flags.mag_3D && (P[16][16] + P[17][17] + P[18][8]) < 0.1f) {
|
||||
mag_I_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f);
|
||||
|
||||
} else {
|
||||
mag_I_sig = 0.0f;
|
||||
}
|
||||
|
||||
// Don't continue to grow the earth field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
|
||||
if (_control_status.flags.mag_3D && (P[19][19] + P[20][20] + P[21][21]) < 0.1f) {
|
||||
mag_B_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f);
|
||||
|
||||
} else {
|
||||
mag_B_sig = 0.0f;
|
||||
}
|
||||
|
||||
float wind_vel_sig;
|
||||
|
||||
// Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned
|
||||
if (_control_status.flags.wind && (P[22][22] + P[22][22]) < 1000.0f) {
|
||||
wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f);
|
||||
|
||||
} else {
|
||||
wind_vel_sig = 0.0f;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < 9; i++) {
|
||||
process_noise[i] = 0.0f;
|
||||
@@ -493,170 +520,200 @@ void Ekf::predictCovariance()
|
||||
nextP[13][15] = P[13][15];
|
||||
nextP[14][15] = P[14][15];
|
||||
nextP[15][15] = P[15][15];
|
||||
nextP[0][16] = P[0][16] * SPP[5] - P[1][16] * SPP[4] + P[2][16] * SPP[7] + P[9][16] * SPP[22] + P[12][16] * SPP[18];
|
||||
nextP[1][16] = P[1][16] * SPP[6] - P[0][16] * SPP[2] - P[2][16] * SPP[8] + P[10][16] * SPP[22] + P[13][16] * SPP[17];
|
||||
nextP[2][16] = P[0][16] * SPP[14] - P[1][16] * SPP[3] + P[2][16] * SPP[13] + P[11][16] * SPP[22] + P[14][16] * SPP[16];
|
||||
nextP[3][16] = P[3][16] + P[0][16] * SPP[1] + P[1][16] * SPP[19] + P[2][16] * SPP[15] - P[15][16] * SPP[21];
|
||||
nextP[4][16] = P[4][16] + P[15][16] * SF[22] + P[0][16] * SPP[20] + P[1][16] * SPP[12] + P[2][16] * SPP[11];
|
||||
nextP[5][16] = P[5][16] + P[15][16] * SF[20] - P[0][16] * SPP[9] + P[1][16] * SPP[10] + P[2][16] * SPP[0];
|
||||
nextP[6][16] = P[6][16] + P[3][16] * dt;
|
||||
nextP[7][16] = P[7][16] + P[4][16] * dt;
|
||||
nextP[8][16] = P[8][16] + P[5][16] * dt;
|
||||
nextP[9][16] = P[9][16];
|
||||
nextP[10][16] = P[10][16];
|
||||
nextP[11][16] = P[11][16];
|
||||
nextP[12][16] = P[12][16];
|
||||
nextP[13][16] = P[13][16];
|
||||
nextP[14][16] = P[14][16];
|
||||
nextP[15][16] = P[15][16];
|
||||
nextP[16][16] = P[16][16];
|
||||
nextP[0][17] = P[0][17] * SPP[5] - P[1][17] * SPP[4] + P[2][17] * SPP[7] + P[9][17] * SPP[22] + P[12][17] * SPP[18];
|
||||
nextP[1][17] = P[1][17] * SPP[6] - P[0][17] * SPP[2] - P[2][17] * SPP[8] + P[10][17] * SPP[22] + P[13][17] * SPP[17];
|
||||
nextP[2][17] = P[0][17] * SPP[14] - P[1][17] * SPP[3] + P[2][17] * SPP[13] + P[11][17] * SPP[22] + P[14][17] * SPP[16];
|
||||
nextP[3][17] = P[3][17] + P[0][17] * SPP[1] + P[1][17] * SPP[19] + P[2][17] * SPP[15] - P[15][17] * SPP[21];
|
||||
nextP[4][17] = P[4][17] + P[15][17] * SF[22] + P[0][17] * SPP[20] + P[1][17] * SPP[12] + P[2][17] * SPP[11];
|
||||
nextP[5][17] = P[5][17] + P[15][17] * SF[20] - P[0][17] * SPP[9] + P[1][17] * SPP[10] + P[2][17] * SPP[0];
|
||||
nextP[6][17] = P[6][17] + P[3][17] * dt;
|
||||
nextP[7][17] = P[7][17] + P[4][17] * dt;
|
||||
nextP[8][17] = P[8][17] + P[5][17] * dt;
|
||||
nextP[9][17] = P[9][17];
|
||||
nextP[10][17] = P[10][17];
|
||||
nextP[11][17] = P[11][17];
|
||||
nextP[12][17] = P[12][17];
|
||||
nextP[13][17] = P[13][17];
|
||||
nextP[14][17] = P[14][17];
|
||||
nextP[15][17] = P[15][17];
|
||||
nextP[16][17] = P[16][17];
|
||||
nextP[17][17] = P[17][17];
|
||||
nextP[0][18] = P[0][18] * SPP[5] - P[1][18] * SPP[4] + P[2][18] * SPP[7] + P[9][18] * SPP[22] + P[12][18] * SPP[18];
|
||||
nextP[1][18] = P[1][18] * SPP[6] - P[0][18] * SPP[2] - P[2][18] * SPP[8] + P[10][18] * SPP[22] + P[13][18] * SPP[17];
|
||||
nextP[2][18] = P[0][18] * SPP[14] - P[1][18] * SPP[3] + P[2][18] * SPP[13] + P[11][18] * SPP[22] + P[14][18] * SPP[16];
|
||||
nextP[3][18] = P[3][18] + P[0][18] * SPP[1] + P[1][18] * SPP[19] + P[2][18] * SPP[15] - P[15][18] * SPP[21];
|
||||
nextP[4][18] = P[4][18] + P[15][18] * SF[22] + P[0][18] * SPP[20] + P[1][18] * SPP[12] + P[2][18] * SPP[11];
|
||||
nextP[5][18] = P[5][18] + P[15][18] * SF[20] - P[0][18] * SPP[9] + P[1][18] * SPP[10] + P[2][18] * SPP[0];
|
||||
nextP[6][18] = P[6][18] + P[3][18] * dt;
|
||||
nextP[7][18] = P[7][18] + P[4][18] * dt;
|
||||
nextP[8][18] = P[8][18] + P[5][18] * dt;
|
||||
nextP[9][18] = P[9][18];
|
||||
nextP[10][18] = P[10][18];
|
||||
nextP[11][18] = P[11][18];
|
||||
nextP[12][18] = P[12][18];
|
||||
nextP[13][18] = P[13][18];
|
||||
nextP[14][18] = P[14][18];
|
||||
nextP[15][18] = P[15][18];
|
||||
nextP[16][18] = P[16][18];
|
||||
nextP[17][18] = P[17][18];
|
||||
nextP[18][18] = P[18][18];
|
||||
nextP[0][19] = P[0][19] * SPP[5] - P[1][19] * SPP[4] + P[2][19] * SPP[7] + P[9][19] * SPP[22] + P[12][19] * SPP[18];
|
||||
nextP[1][19] = P[1][19] * SPP[6] - P[0][19] * SPP[2] - P[2][19] * SPP[8] + P[10][19] * SPP[22] + P[13][19] * SPP[17];
|
||||
nextP[2][19] = P[0][19] * SPP[14] - P[1][19] * SPP[3] + P[2][19] * SPP[13] + P[11][19] * SPP[22] + P[14][19] * SPP[16];
|
||||
nextP[3][19] = P[3][19] + P[0][19] * SPP[1] + P[1][19] * SPP[19] + P[2][19] * SPP[15] - P[15][19] * SPP[21];
|
||||
nextP[4][19] = P[4][19] + P[15][19] * SF[22] + P[0][19] * SPP[20] + P[1][19] * SPP[12] + P[2][19] * SPP[11];
|
||||
nextP[5][19] = P[5][19] + P[15][19] * SF[20] - P[0][19] * SPP[9] + P[1][19] * SPP[10] + P[2][19] * SPP[0];
|
||||
nextP[6][19] = P[6][19] + P[3][19] * dt;
|
||||
nextP[7][19] = P[7][19] + P[4][19] * dt;
|
||||
nextP[8][19] = P[8][19] + P[5][19] * dt;
|
||||
nextP[9][19] = P[9][19];
|
||||
nextP[10][19] = P[10][19];
|
||||
nextP[11][19] = P[11][19];
|
||||
nextP[12][19] = P[12][19];
|
||||
nextP[13][19] = P[13][19];
|
||||
nextP[14][19] = P[14][19];
|
||||
nextP[15][19] = P[15][19];
|
||||
nextP[16][19] = P[16][19];
|
||||
nextP[17][19] = P[17][19];
|
||||
nextP[18][19] = P[18][19];
|
||||
nextP[19][19] = P[19][19];
|
||||
nextP[0][20] = P[0][20] * SPP[5] - P[1][20] * SPP[4] + P[2][20] * SPP[7] + P[9][20] * SPP[22] + P[12][20] * SPP[18];
|
||||
nextP[1][20] = P[1][20] * SPP[6] - P[0][20] * SPP[2] - P[2][20] * SPP[8] + P[10][20] * SPP[22] + P[13][20] * SPP[17];
|
||||
nextP[2][20] = P[0][20] * SPP[14] - P[1][20] * SPP[3] + P[2][20] * SPP[13] + P[11][20] * SPP[22] + P[14][20] * SPP[16];
|
||||
nextP[3][20] = P[3][20] + P[0][20] * SPP[1] + P[1][20] * SPP[19] + P[2][20] * SPP[15] - P[15][20] * SPP[21];
|
||||
nextP[4][20] = P[4][20] + P[15][20] * SF[22] + P[0][20] * SPP[20] + P[1][20] * SPP[12] + P[2][20] * SPP[11];
|
||||
nextP[5][20] = P[5][20] + P[15][20] * SF[20] - P[0][20] * SPP[9] + P[1][20] * SPP[10] + P[2][20] * SPP[0];
|
||||
nextP[6][20] = P[6][20] + P[3][20] * dt;
|
||||
nextP[7][20] = P[7][20] + P[4][20] * dt;
|
||||
nextP[8][20] = P[8][20] + P[5][20] * dt;
|
||||
nextP[9][20] = P[9][20];
|
||||
nextP[10][20] = P[10][20];
|
||||
nextP[11][20] = P[11][20];
|
||||
nextP[12][20] = P[12][20];
|
||||
nextP[13][20] = P[13][20];
|
||||
nextP[14][20] = P[14][20];
|
||||
nextP[15][20] = P[15][20];
|
||||
nextP[16][20] = P[16][20];
|
||||
nextP[17][20] = P[17][20];
|
||||
nextP[18][20] = P[18][20];
|
||||
nextP[19][20] = P[19][20];
|
||||
nextP[20][20] = P[20][20];
|
||||
nextP[0][21] = P[0][21] * SPP[5] - P[1][21] * SPP[4] + P[2][21] * SPP[7] + P[9][21] * SPP[22] + P[12][21] * SPP[18];
|
||||
nextP[1][21] = P[1][21] * SPP[6] - P[0][21] * SPP[2] - P[2][21] * SPP[8] + P[10][21] * SPP[22] + P[13][21] * SPP[17];
|
||||
nextP[2][21] = P[0][21] * SPP[14] - P[1][21] * SPP[3] + P[2][21] * SPP[13] + P[11][21] * SPP[22] + P[14][21] * SPP[16];
|
||||
nextP[3][21] = P[3][21] + P[0][21] * SPP[1] + P[1][21] * SPP[19] + P[2][21] * SPP[15] - P[15][21] * SPP[21];
|
||||
nextP[4][21] = P[4][21] + P[15][21] * SF[22] + P[0][21] * SPP[20] + P[1][21] * SPP[12] + P[2][21] * SPP[11];
|
||||
nextP[5][21] = P[5][21] + P[15][21] * SF[20] - P[0][21] * SPP[9] + P[1][21] * SPP[10] + P[2][21] * SPP[0];
|
||||
nextP[6][21] = P[6][21] + P[3][21] * dt;
|
||||
nextP[7][21] = P[7][21] + P[4][21] * dt;
|
||||
nextP[8][21] = P[8][21] + P[5][21] * dt;
|
||||
nextP[9][21] = P[9][21];
|
||||
nextP[10][21] = P[10][21];
|
||||
nextP[11][21] = P[11][21];
|
||||
nextP[12][21] = P[12][21];
|
||||
nextP[13][21] = P[13][21];
|
||||
nextP[14][21] = P[14][21];
|
||||
nextP[15][21] = P[15][21];
|
||||
nextP[16][21] = P[16][21];
|
||||
nextP[17][21] = P[17][21];
|
||||
nextP[18][21] = P[18][21];
|
||||
nextP[19][21] = P[19][21];
|
||||
nextP[20][21] = P[20][21];
|
||||
nextP[21][21] = P[21][21];
|
||||
nextP[0][22] = P[0][22] * SPP[5] - P[1][22] * SPP[4] + P[2][22] * SPP[7] + P[9][22] * SPP[22] + P[12][22] * SPP[18];
|
||||
nextP[1][22] = P[1][22] * SPP[6] - P[0][22] * SPP[2] - P[2][22] * SPP[8] + P[10][22] * SPP[22] + P[13][22] * SPP[17];
|
||||
nextP[2][22] = P[0][22] * SPP[14] - P[1][22] * SPP[3] + P[2][22] * SPP[13] + P[11][22] * SPP[22] + P[14][22] * SPP[16];
|
||||
nextP[3][22] = P[3][22] + P[0][22] * SPP[1] + P[1][22] * SPP[19] + P[2][22] * SPP[15] - P[15][22] * SPP[21];
|
||||
nextP[4][22] = P[4][22] + P[15][22] * SF[22] + P[0][22] * SPP[20] + P[1][22] * SPP[12] + P[2][22] * SPP[11];
|
||||
nextP[5][22] = P[5][22] + P[15][22] * SF[20] - P[0][22] * SPP[9] + P[1][22] * SPP[10] + P[2][22] * SPP[0];
|
||||
nextP[6][22] = P[6][22] + P[3][22] * dt;
|
||||
nextP[7][22] = P[7][22] + P[4][22] * dt;
|
||||
nextP[8][22] = P[8][22] + P[5][22] * dt;
|
||||
nextP[9][22] = P[9][22];
|
||||
nextP[10][22] = P[10][22];
|
||||
nextP[11][22] = P[11][22];
|
||||
nextP[12][22] = P[12][22];
|
||||
nextP[13][22] = P[13][22];
|
||||
nextP[14][22] = P[14][22];
|
||||
nextP[15][22] = P[15][22];
|
||||
nextP[16][22] = P[16][22];
|
||||
nextP[17][22] = P[17][22];
|
||||
nextP[18][22] = P[18][22];
|
||||
nextP[19][22] = P[19][22];
|
||||
nextP[20][22] = P[20][22];
|
||||
nextP[21][22] = P[21][22];
|
||||
nextP[22][22] = P[22][22];
|
||||
nextP[0][23] = P[0][23] * SPP[5] - P[1][23] * SPP[4] + P[2][23] * SPP[7] + P[9][23] * SPP[22] + P[12][23] * SPP[18];
|
||||
nextP[1][23] = P[1][23] * SPP[6] - P[0][23] * SPP[2] - P[2][23] * SPP[8] + P[10][23] * SPP[22] + P[13][23] * SPP[17];
|
||||
nextP[2][23] = P[0][23] * SPP[14] - P[1][23] * SPP[3] + P[2][23] * SPP[13] + P[11][23] * SPP[22] + P[14][23] * SPP[16];
|
||||
nextP[3][23] = P[3][23] + P[0][23] * SPP[1] + P[1][23] * SPP[19] + P[2][23] * SPP[15] - P[15][23] * SPP[21];
|
||||
nextP[4][23] = P[4][23] + P[15][23] * SF[22] + P[0][23] * SPP[20] + P[1][23] * SPP[12] + P[2][23] * SPP[11];
|
||||
nextP[5][23] = P[5][23] + P[15][23] * SF[20] - P[0][23] * SPP[9] + P[1][23] * SPP[10] + P[2][23] * SPP[0];
|
||||
nextP[6][23] = P[6][23] + P[3][23] * dt;
|
||||
nextP[7][23] = P[7][23] + P[4][23] * dt;
|
||||
nextP[8][23] = P[8][23] + P[5][23] * dt;
|
||||
nextP[9][23] = P[9][23];
|
||||
nextP[10][23] = P[10][23];
|
||||
nextP[11][23] = P[11][23];
|
||||
nextP[12][23] = P[12][23];
|
||||
nextP[13][23] = P[13][23];
|
||||
nextP[14][23] = P[14][23];
|
||||
nextP[15][23] = P[15][23];
|
||||
nextP[16][23] = P[16][23];
|
||||
nextP[17][23] = P[17][23];
|
||||
nextP[18][23] = P[18][23];
|
||||
nextP[19][23] = P[19][23];
|
||||
nextP[20][23] = P[20][23];
|
||||
nextP[21][23] = P[21][23];
|
||||
nextP[22][23] = P[22][23];
|
||||
nextP[23][23] = P[23][23];
|
||||
|
||||
// Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion
|
||||
if (_control_status.flags.mag_3D) {
|
||||
// Check if we have just transitioned into 3-axis fusion and set the state variances
|
||||
if (!_control_status_prev.flags.mag_3D) {
|
||||
for (uint8_t index = 16; index <= 21; index++) {
|
||||
P[index][index] = sq(fmaxf(_params.mag_noise, 0.001f));
|
||||
}
|
||||
}
|
||||
|
||||
nextP[0][16] = P[0][16] * SPP[5] - P[1][16] * SPP[4] + P[2][16] * SPP[7] + P[9][16] * SPP[22] + P[12][16] * SPP[18];
|
||||
nextP[1][16] = P[1][16] * SPP[6] - P[0][16] * SPP[2] - P[2][16] * SPP[8] + P[10][16] * SPP[22] + P[13][16] * SPP[17];
|
||||
nextP[2][16] = P[0][16] * SPP[14] - P[1][16] * SPP[3] + P[2][16] * SPP[13] + P[11][16] * SPP[22] + P[14][16] * SPP[16];
|
||||
nextP[3][16] = P[3][16] + P[0][16] * SPP[1] + P[1][16] * SPP[19] + P[2][16] * SPP[15] - P[15][16] * SPP[21];
|
||||
nextP[4][16] = P[4][16] + P[15][16] * SF[22] + P[0][16] * SPP[20] + P[1][16] * SPP[12] + P[2][16] * SPP[11];
|
||||
nextP[5][16] = P[5][16] + P[15][16] * SF[20] - P[0][16] * SPP[9] + P[1][16] * SPP[10] + P[2][16] * SPP[0];
|
||||
nextP[6][16] = P[6][16] + P[3][16] * dt;
|
||||
nextP[7][16] = P[7][16] + P[4][16] * dt;
|
||||
nextP[8][16] = P[8][16] + P[5][16] * dt;
|
||||
nextP[9][16] = P[9][16];
|
||||
nextP[10][16] = P[10][16];
|
||||
nextP[11][16] = P[11][16];
|
||||
nextP[12][16] = P[12][16];
|
||||
nextP[13][16] = P[13][16];
|
||||
nextP[14][16] = P[14][16];
|
||||
nextP[15][16] = P[15][16];
|
||||
nextP[16][16] = P[16][16];
|
||||
|
||||
nextP[0][17] = P[0][17] * SPP[5] - P[1][17] * SPP[4] + P[2][17] * SPP[7] + P[9][17] * SPP[22] + P[12][17] * SPP[18];
|
||||
nextP[1][17] = P[1][17] * SPP[6] - P[0][17] * SPP[2] - P[2][17] * SPP[8] + P[10][17] * SPP[22] + P[13][17] * SPP[17];
|
||||
nextP[2][17] = P[0][17] * SPP[14] - P[1][17] * SPP[3] + P[2][17] * SPP[13] + P[11][17] * SPP[22] + P[14][17] * SPP[16];
|
||||
nextP[3][17] = P[3][17] + P[0][17] * SPP[1] + P[1][17] * SPP[19] + P[2][17] * SPP[15] - P[15][17] * SPP[21];
|
||||
nextP[4][17] = P[4][17] + P[15][17] * SF[22] + P[0][17] * SPP[20] + P[1][17] * SPP[12] + P[2][17] * SPP[11];
|
||||
nextP[5][17] = P[5][17] + P[15][17] * SF[20] - P[0][17] * SPP[9] + P[1][17] * SPP[10] + P[2][17] * SPP[0];
|
||||
nextP[6][17] = P[6][17] + P[3][17] * dt;
|
||||
nextP[7][17] = P[7][17] + P[4][17] * dt;
|
||||
nextP[8][17] = P[8][17] + P[5][17] * dt;
|
||||
nextP[9][17] = P[9][17];
|
||||
nextP[10][17] = P[10][17];
|
||||
nextP[11][17] = P[11][17];
|
||||
nextP[12][17] = P[12][17];
|
||||
nextP[13][17] = P[13][17];
|
||||
nextP[14][17] = P[14][17];
|
||||
nextP[15][17] = P[15][17];
|
||||
nextP[16][17] = P[16][17];
|
||||
nextP[17][17] = P[17][17];
|
||||
|
||||
nextP[0][18] = P[0][18] * SPP[5] - P[1][18] * SPP[4] + P[2][18] * SPP[7] + P[9][18] * SPP[22] + P[12][18] * SPP[18];
|
||||
nextP[1][18] = P[1][18] * SPP[6] - P[0][18] * SPP[2] - P[2][18] * SPP[8] + P[10][18] * SPP[22] + P[13][18] * SPP[17];
|
||||
nextP[2][18] = P[0][18] * SPP[14] - P[1][18] * SPP[3] + P[2][18] * SPP[13] + P[11][18] * SPP[22] + P[14][18] * SPP[16];
|
||||
nextP[3][18] = P[3][18] + P[0][18] * SPP[1] + P[1][18] * SPP[19] + P[2][18] * SPP[15] - P[15][18] * SPP[21];
|
||||
nextP[4][18] = P[4][18] + P[15][18] * SF[22] + P[0][18] * SPP[20] + P[1][18] * SPP[12] + P[2][18] * SPP[11];
|
||||
nextP[5][18] = P[5][18] + P[15][18] * SF[20] - P[0][18] * SPP[9] + P[1][18] * SPP[10] + P[2][18] * SPP[0];
|
||||
nextP[6][18] = P[6][18] + P[3][18] * dt;
|
||||
nextP[7][18] = P[7][18] + P[4][18] * dt;
|
||||
nextP[8][18] = P[8][18] + P[5][18] * dt;
|
||||
nextP[9][18] = P[9][18];
|
||||
nextP[10][18] = P[10][18];
|
||||
nextP[11][18] = P[11][18];
|
||||
nextP[12][18] = P[12][18];
|
||||
nextP[13][18] = P[13][18];
|
||||
nextP[14][18] = P[14][18];
|
||||
nextP[15][18] = P[15][18];
|
||||
nextP[16][18] = P[16][18];
|
||||
nextP[17][18] = P[17][18];
|
||||
nextP[18][18] = P[18][18];
|
||||
|
||||
nextP[0][19] = P[0][19] * SPP[5] - P[1][19] * SPP[4] + P[2][19] * SPP[7] + P[9][19] * SPP[22] + P[12][19] * SPP[18];
|
||||
nextP[1][19] = P[1][19] * SPP[6] - P[0][19] * SPP[2] - P[2][19] * SPP[8] + P[10][19] * SPP[22] + P[13][19] * SPP[17];
|
||||
nextP[2][19] = P[0][19] * SPP[14] - P[1][19] * SPP[3] + P[2][19] * SPP[13] + P[11][19] * SPP[22] + P[14][19] * SPP[16];
|
||||
nextP[3][19] = P[3][19] + P[0][19] * SPP[1] + P[1][19] * SPP[19] + P[2][19] * SPP[15] - P[15][19] * SPP[21];
|
||||
nextP[4][19] = P[4][19] + P[15][19] * SF[22] + P[0][19] * SPP[20] + P[1][19] * SPP[12] + P[2][19] * SPP[11];
|
||||
nextP[5][19] = P[5][19] + P[15][19] * SF[20] - P[0][19] * SPP[9] + P[1][19] * SPP[10] + P[2][19] * SPP[0];
|
||||
nextP[6][19] = P[6][19] + P[3][19] * dt;
|
||||
nextP[7][19] = P[7][19] + P[4][19] * dt;
|
||||
nextP[8][19] = P[8][19] + P[5][19] * dt;
|
||||
nextP[9][19] = P[9][19];
|
||||
nextP[10][19] = P[10][19];
|
||||
nextP[11][19] = P[11][19];
|
||||
nextP[12][19] = P[12][19];
|
||||
nextP[13][19] = P[13][19];
|
||||
nextP[14][19] = P[14][19];
|
||||
nextP[15][19] = P[15][19];
|
||||
nextP[16][19] = P[16][19];
|
||||
nextP[17][19] = P[17][19];
|
||||
nextP[18][19] = P[18][19];
|
||||
nextP[19][19] = P[19][19];
|
||||
|
||||
nextP[0][20] = P[0][20] * SPP[5] - P[1][20] * SPP[4] + P[2][20] * SPP[7] + P[9][20] * SPP[22] + P[12][20] * SPP[18];
|
||||
nextP[1][20] = P[1][20] * SPP[6] - P[0][20] * SPP[2] - P[2][20] * SPP[8] + P[10][20] * SPP[22] + P[13][20] * SPP[17];
|
||||
nextP[2][20] = P[0][20] * SPP[14] - P[1][20] * SPP[3] + P[2][20] * SPP[13] + P[11][20] * SPP[22] + P[14][20] * SPP[16];
|
||||
nextP[3][20] = P[3][20] + P[0][20] * SPP[1] + P[1][20] * SPP[19] + P[2][20] * SPP[15] - P[15][20] * SPP[21];
|
||||
nextP[4][20] = P[4][20] + P[15][20] * SF[22] + P[0][20] * SPP[20] + P[1][20] * SPP[12] + P[2][20] * SPP[11];
|
||||
nextP[5][20] = P[5][20] + P[15][20] * SF[20] - P[0][20] * SPP[9] + P[1][20] * SPP[10] + P[2][20] * SPP[0];
|
||||
nextP[6][20] = P[6][20] + P[3][20] * dt;
|
||||
nextP[7][20] = P[7][20] + P[4][20] * dt;
|
||||
nextP[8][20] = P[8][20] + P[5][20] * dt;
|
||||
nextP[9][20] = P[9][20];
|
||||
nextP[10][20] = P[10][20];
|
||||
nextP[11][20] = P[11][20];
|
||||
nextP[12][20] = P[12][20];
|
||||
nextP[13][20] = P[13][20];
|
||||
nextP[14][20] = P[14][20];
|
||||
nextP[15][20] = P[15][20];
|
||||
nextP[16][20] = P[16][20];
|
||||
nextP[17][20] = P[17][20];
|
||||
nextP[18][20] = P[18][20];
|
||||
nextP[19][20] = P[19][20];
|
||||
nextP[20][20] = P[20][20];
|
||||
|
||||
nextP[0][21] = P[0][21] * SPP[5] - P[1][21] * SPP[4] + P[2][21] * SPP[7] + P[9][21] * SPP[22] + P[12][21] * SPP[18];
|
||||
nextP[1][21] = P[1][21] * SPP[6] - P[0][21] * SPP[2] - P[2][21] * SPP[8] + P[10][21] * SPP[22] + P[13][21] * SPP[17];
|
||||
nextP[2][21] = P[0][21] * SPP[14] - P[1][21] * SPP[3] + P[2][21] * SPP[13] + P[11][21] * SPP[22] + P[14][21] * SPP[16];
|
||||
nextP[3][21] = P[3][21] + P[0][21] * SPP[1] + P[1][21] * SPP[19] + P[2][21] * SPP[15] - P[15][21] * SPP[21];
|
||||
nextP[4][21] = P[4][21] + P[15][21] * SF[22] + P[0][21] * SPP[20] + P[1][21] * SPP[12] + P[2][21] * SPP[11];
|
||||
nextP[5][21] = P[5][21] + P[15][21] * SF[20] - P[0][21] * SPP[9] + P[1][21] * SPP[10] + P[2][21] * SPP[0];
|
||||
nextP[6][21] = P[6][21] + P[3][21] * dt;
|
||||
nextP[7][21] = P[7][21] + P[4][21] * dt;
|
||||
nextP[8][21] = P[8][21] + P[5][21] * dt;
|
||||
nextP[9][21] = P[9][21];
|
||||
nextP[10][21] = P[10][21];
|
||||
nextP[11][21] = P[11][21];
|
||||
nextP[12][21] = P[12][21];
|
||||
nextP[13][21] = P[13][21];
|
||||
nextP[14][21] = P[14][21];
|
||||
nextP[15][21] = P[15][21];
|
||||
nextP[16][21] = P[16][21];
|
||||
nextP[17][21] = P[17][21];
|
||||
nextP[18][21] = P[18][21];
|
||||
nextP[19][21] = P[19][21];
|
||||
nextP[20][21] = P[20][21];
|
||||
nextP[21][21] = P[21][21];
|
||||
}
|
||||
|
||||
// Don't do covariance prediction on wind states unless we are using them
|
||||
if (_control_status.flags.wind) {
|
||||
// Check if we have jsut transitioned to using wind states and set the variances accordingly
|
||||
if (!_control_status_prev.flags.mag_3D) {
|
||||
for (uint8_t index = 22; index <= 23; index++) {
|
||||
// TODO initialise wind states using ground speed and airspeed and set initial variance using sum of ground speed and airspeed variances
|
||||
P[index][index] = sq(5.0f);
|
||||
}
|
||||
}
|
||||
|
||||
nextP[0][22] = P[0][22] * SPP[5] - P[1][22] * SPP[4] + P[2][22] * SPP[7] + P[9][22] * SPP[22] + P[12][22] * SPP[18];
|
||||
nextP[1][22] = P[1][22] * SPP[6] - P[0][22] * SPP[2] - P[2][22] * SPP[8] + P[10][22] * SPP[22] + P[13][22] * SPP[17];
|
||||
nextP[2][22] = P[0][22] * SPP[14] - P[1][22] * SPP[3] + P[2][22] * SPP[13] + P[11][22] * SPP[22] + P[14][22] * SPP[16];
|
||||
nextP[3][22] = P[3][22] + P[0][22] * SPP[1] + P[1][22] * SPP[19] + P[2][22] * SPP[15] - P[15][22] * SPP[21];
|
||||
nextP[4][22] = P[4][22] + P[15][22] * SF[22] + P[0][22] * SPP[20] + P[1][22] * SPP[12] + P[2][22] * SPP[11];
|
||||
nextP[5][22] = P[5][22] + P[15][22] * SF[20] - P[0][22] * SPP[9] + P[1][22] * SPP[10] + P[2][22] * SPP[0];
|
||||
nextP[6][22] = P[6][22] + P[3][22] * dt;
|
||||
nextP[7][22] = P[7][22] + P[4][22] * dt;
|
||||
nextP[8][22] = P[8][22] + P[5][22] * dt;
|
||||
nextP[9][22] = P[9][22];
|
||||
nextP[10][22] = P[10][22];
|
||||
nextP[11][22] = P[11][22];
|
||||
nextP[12][22] = P[12][22];
|
||||
nextP[13][22] = P[13][22];
|
||||
nextP[14][22] = P[14][22];
|
||||
nextP[15][22] = P[15][22];
|
||||
nextP[16][22] = P[16][22];
|
||||
nextP[17][22] = P[17][22];
|
||||
nextP[18][22] = P[18][22];
|
||||
nextP[19][22] = P[19][22];
|
||||
nextP[20][22] = P[20][22];
|
||||
nextP[21][22] = P[21][22];
|
||||
nextP[22][22] = P[22][22];
|
||||
|
||||
nextP[0][23] = P[0][23] * SPP[5] - P[1][23] * SPP[4] + P[2][23] * SPP[7] + P[9][23] * SPP[22] + P[12][23] * SPP[18];
|
||||
nextP[1][23] = P[1][23] * SPP[6] - P[0][23] * SPP[2] - P[2][23] * SPP[8] + P[10][23] * SPP[22] + P[13][23] * SPP[17];
|
||||
nextP[2][23] = P[0][23] * SPP[14] - P[1][23] * SPP[3] + P[2][23] * SPP[13] + P[11][23] * SPP[22] + P[14][23] * SPP[16];
|
||||
nextP[3][23] = P[3][23] + P[0][23] * SPP[1] + P[1][23] * SPP[19] + P[2][23] * SPP[15] - P[15][23] * SPP[21];
|
||||
nextP[4][23] = P[4][23] + P[15][23] * SF[22] + P[0][23] * SPP[20] + P[1][23] * SPP[12] + P[2][23] * SPP[11];
|
||||
nextP[5][23] = P[5][23] + P[15][23] * SF[20] - P[0][23] * SPP[9] + P[1][23] * SPP[10] + P[2][23] * SPP[0];
|
||||
nextP[6][23] = P[6][23] + P[3][23] * dt;
|
||||
nextP[7][23] = P[7][23] + P[4][23] * dt;
|
||||
nextP[8][23] = P[8][23] + P[5][23] * dt;
|
||||
nextP[9][23] = P[9][23];
|
||||
nextP[10][23] = P[10][23];
|
||||
nextP[11][23] = P[11][23];
|
||||
nextP[12][23] = P[12][23];
|
||||
nextP[13][23] = P[13][23];
|
||||
nextP[14][23] = P[14][23];
|
||||
nextP[15][23] = P[15][23];
|
||||
nextP[16][23] = P[16][23];
|
||||
nextP[17][23] = P[17][23];
|
||||
nextP[18][23] = P[18][23];
|
||||
nextP[19][23] = P[19][23];
|
||||
nextP[20][23] = P[20][23];
|
||||
nextP[21][23] = P[21][23];
|
||||
nextP[22][23] = P[22][23];
|
||||
nextP[23][23] = P[23][23];
|
||||
|
||||
}
|
||||
|
||||
// add process noise
|
||||
for (unsigned i = 0; i < _k_num_states; i++) {
|
||||
@@ -664,7 +721,7 @@ void Ekf::predictCovariance()
|
||||
}
|
||||
|
||||
// stop position covariance growth if our total position variance reaches 100m
|
||||
// this can happen if we loose gps for some time
|
||||
// this can happen if we lose gps for some time
|
||||
if ((P[6][6] + P[7][7]) > 1e4f) {
|
||||
for (uint8_t i = 6; i < 8; i++) {
|
||||
for (uint8_t j = 0; j < _k_num_states; j++) {
|
||||
@@ -681,10 +738,12 @@ void Ekf::predictCovariance()
|
||||
}
|
||||
}
|
||||
|
||||
// copy variances (diagonals)
|
||||
for (unsigned i = 0; i < _k_num_states; i++) {
|
||||
P[i][i] = nextP[i][i];
|
||||
}
|
||||
|
||||
// force symmetry
|
||||
for (unsigned row = 1; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < row; column++) {
|
||||
P[row][column] = 0.5f * (nextP[row][column] + nextP[column][row]);
|
||||
@@ -706,8 +765,8 @@ void Ekf::limitCov()
|
||||
P_lim[3] = 0.001f; // gyro bias max var
|
||||
P_lim[4] = 0.01f; // gyro scale max var
|
||||
P_lim[5] = 0.1f; // delta velocity z bias max var
|
||||
P_lim[6] = 0.01f; // earth mag field max var
|
||||
P_lim[7] = 0.01f; // body mag field max var
|
||||
P_lim[6] = 0.1f; // earth mag field max var
|
||||
P_lim[7] = 0.1f; // body mag field max var
|
||||
P_lim[8] = 1000.0f; // wind max var
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
@@ -742,20 +801,14 @@ void Ekf::limitCov()
|
||||
math::constrain(P[15][15], 0.0f, P_lim[5]);
|
||||
|
||||
for (int i = 16; i < 19; i++) {
|
||||
|
||||
math::constrain(P[i][i], 0.0f, P_lim[6]);
|
||||
}
|
||||
|
||||
for (int i = 19; i < 22; i++) {
|
||||
|
||||
|
||||
math::constrain(P[i][i], 0.0f, P_lim[7]);
|
||||
}
|
||||
|
||||
for (int i = 22; i < 24; i++) {
|
||||
|
||||
math::constrain(P[i][i], 0.0f, P_lim[8]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -85,8 +85,12 @@ public:
|
||||
bool collect_gps(uint64_t time_usec, struct gps_message *gps);
|
||||
bool collect_imu(imuSample &imu);
|
||||
|
||||
// this is the current status of the filter control modes
|
||||
filter_control_status_u _control_status = {};
|
||||
|
||||
// this is the previous status of the filter control modes - used to detect mode transitions
|
||||
filter_control_status_u _control_status_prev = {};
|
||||
|
||||
// get the ekf WGS-84 origin position and height and the system time it was last set
|
||||
void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt);
|
||||
|
||||
|
||||
+168
-115
@@ -284,10 +284,18 @@ void Ekf::fuseMag()
|
||||
SK_MX[1] + P[20][18] * SK_MX[3]);
|
||||
Kfusion[21] = SK_MX[0] * (P[21][19] + P[21][16] * SH_MAG[1] + P[21][17] * SH_MAG[4] - P[21][1] * SK_MX[2] + P[21][2] *
|
||||
SK_MX[1] + P[21][18] * SK_MX[3]);
|
||||
Kfusion[22] = SK_MX[0] * (P[22][19] + P[22][16] * SH_MAG[1] + P[22][17] * SH_MAG[4] - P[22][1] * SK_MX[2] + P[22][2] *
|
||||
SK_MX[1] + P[22][18] * SK_MX[3]);
|
||||
Kfusion[23] = SK_MX[0] * (P[23][19] + P[23][16] * SH_MAG[1] + P[23][17] * SH_MAG[4] - P[23][1] * SK_MX[2] + P[23][2] *
|
||||
SK_MX[1] + P[23][18] * SK_MX[3]);
|
||||
|
||||
// Don't update wind states unless we are doing wind estimation
|
||||
if (_control_status.flags.wind) {
|
||||
Kfusion[22] = SK_MX[0] * (P[22][19] + P[22][16] * SH_MAG[1] + P[22][17] * SH_MAG[4] - P[22][1] * SK_MX[2] + P[22][2] *
|
||||
SK_MX[1] + P[22][18] * SK_MX[3]);
|
||||
Kfusion[23] = SK_MX[0] * (P[23][19] + P[23][16] * SH_MAG[1] + P[23][17] * SH_MAG[4] - P[23][1] * SK_MX[2] + P[23][2] *
|
||||
SK_MX[1] + P[23][18] * SK_MX[3]);
|
||||
|
||||
} else {
|
||||
Kfusion[22] = 0.0f;
|
||||
Kfusion[23] = 0.0f;
|
||||
}
|
||||
|
||||
} else if (index == 1) {
|
||||
// Calculate Y axis Kalman gains
|
||||
@@ -340,10 +348,18 @@ void Ekf::fuseMag()
|
||||
SK_MY[1] - P[20][16] * SK_MY[3]);
|
||||
Kfusion[21] = SK_MY[0] * (P[21][20] + P[21][17] * SH_MAG[0] + P[21][18] * SH_MAG[3] + P[21][0] * SK_MY[2] - P[21][2] *
|
||||
SK_MY[1] - P[21][16] * SK_MY[3]);
|
||||
Kfusion[22] = SK_MY[0] * (P[22][20] + P[22][17] * SH_MAG[0] + P[22][18] * SH_MAG[3] + P[22][0] * SK_MY[2] - P[22][2] *
|
||||
SK_MY[1] - P[22][16] * SK_MY[3]);
|
||||
Kfusion[23] = SK_MY[0] * (P[23][20] + P[23][17] * SH_MAG[0] + P[23][18] * SH_MAG[3] + P[23][0] * SK_MY[2] - P[23][2] *
|
||||
SK_MY[1] - P[23][16] * SK_MY[3]);
|
||||
|
||||
// Don't update wind states unless we are doing wind estimation
|
||||
if (_control_status.flags.wind) {
|
||||
Kfusion[22] = SK_MY[0] * (P[22][20] + P[22][17] * SH_MAG[0] + P[22][18] * SH_MAG[3] + P[22][0] * SK_MY[2] - P[22][2] *
|
||||
SK_MY[1] - P[22][16] * SK_MY[3]);
|
||||
Kfusion[23] = SK_MY[0] * (P[23][20] + P[23][17] * SH_MAG[0] + P[23][18] * SH_MAG[3] + P[23][0] * SK_MY[2] - P[23][2] *
|
||||
SK_MY[1] - P[23][16] * SK_MY[3]);
|
||||
|
||||
} else {
|
||||
Kfusion[22] = 0.0f;
|
||||
Kfusion[23] = 0.0f;
|
||||
}
|
||||
|
||||
} else if (index == 2) {
|
||||
// Calculate Z axis Kalman gains
|
||||
@@ -396,10 +412,18 @@ void Ekf::fuseMag()
|
||||
SK_MZ[2] - P[20][17] * SK_MZ[3]);
|
||||
Kfusion[21] = SK_MZ[0] * (P[21][21] + P[21][18] * SH_MAG[2] + P[21][16] * SH_MAG[5] - P[21][0] * SK_MZ[1] + P[21][1] *
|
||||
SK_MZ[2] - P[21][17] * SK_MZ[3]);
|
||||
Kfusion[22] = SK_MZ[0] * (P[22][21] + P[22][18] * SH_MAG[2] + P[22][16] * SH_MAG[5] - P[22][0] * SK_MZ[1] + P[22][1] *
|
||||
SK_MZ[2] - P[22][17] * SK_MZ[3]);
|
||||
Kfusion[23] = SK_MZ[0] * (P[23][21] + P[23][18] * SH_MAG[2] + P[23][16] * SH_MAG[5] - P[23][0] * SK_MZ[1] + P[23][1] *
|
||||
SK_MZ[2] - P[23][17] * SK_MZ[3]);
|
||||
|
||||
// Don't update wind states unless we are doing wind estimation
|
||||
if (_control_status.flags.wind) {
|
||||
Kfusion[22] = SK_MZ[0] * (P[22][21] + P[22][18] * SH_MAG[2] + P[22][16] * SH_MAG[5] - P[22][0] * SK_MZ[1] + P[22][1] *
|
||||
SK_MZ[2] - P[22][17] * SK_MZ[3]);
|
||||
Kfusion[23] = SK_MZ[0] * (P[23][21] + P[23][18] * SH_MAG[2] + P[23][16] * SH_MAG[5] - P[23][0] * SK_MZ[1] + P[23][1] *
|
||||
SK_MZ[2] - P[23][17] * SK_MZ[3]);
|
||||
|
||||
} else {
|
||||
Kfusion[22] = 0.0f;
|
||||
Kfusion[23] = 0.0f;
|
||||
}
|
||||
|
||||
} else {
|
||||
return;
|
||||
@@ -468,73 +492,71 @@ void Ekf::fuseHeading()
|
||||
float q2 = _state.quat_nominal(2);
|
||||
float q3 = _state.quat_nominal(3);
|
||||
|
||||
float magX = _mag_sample_delayed.mag(0);
|
||||
float magY = _mag_sample_delayed.mag(1);
|
||||
float magZ = _mag_sample_delayed.mag(2);
|
||||
|
||||
float R_mag = fmaxf(_params.mag_heading_noise, 1.0e-2f);
|
||||
R_mag = R_mag * R_mag;
|
||||
float R_YAW = fmaxf(_params.mag_heading_noise, 1.0e-2f);
|
||||
R_YAW = R_YAW * R_YAW;
|
||||
|
||||
// calculate intermediate variables for observation jacobian
|
||||
float t2 = q0 * q0;
|
||||
float t3 = q1 * q1;
|
||||
float t4 = q2 * q2;
|
||||
float t5 = q3 * q3;
|
||||
float t6 = q0 * q2 * 2.0f;
|
||||
float t7 = q1 * q3 * 2.0f;
|
||||
float t8 = t6 + t7;
|
||||
float t9 = q0 * q3 * 2.0f;
|
||||
float t13 = q1 * q2 * 2.0f;
|
||||
float t10 = t9 - t13;
|
||||
float t11 = t2 + t3 - t4 - t5;
|
||||
float t12 = magX * t11;
|
||||
float t14 = magZ * t8;
|
||||
float t19 = magY * t10;
|
||||
float t15 = t12 + t14 - t19;
|
||||
float t16 = t2 - t3 + t4 - t5;
|
||||
float t17 = q0 * q1 * 2.0f;
|
||||
float t24 = q2 * q3 * 2.0f;
|
||||
float t18 = t17 - t24;
|
||||
float t20 = 1.0f / t15;
|
||||
float t21 = magY * t16;
|
||||
float t22 = t9 + t13;
|
||||
float t23 = magX * t22;
|
||||
float t28 = magZ * t18;
|
||||
float t25 = t21 + t23 - t28;
|
||||
float t29 = t20 * t25;
|
||||
float t26 = tan(t29);
|
||||
float t27 = 1.0f / (t15 * t15);
|
||||
float t30 = t26 * t26;
|
||||
float t31 = t30 + 1.0f;
|
||||
float t6 = t2 + t3 - t4 - t5;
|
||||
float t7 = q0 * q3 * 2.0f;
|
||||
float t8 = q1 * q2 * 2.0f;
|
||||
float t9 = t7 + t8;
|
||||
float t10;
|
||||
|
||||
float H_HDG[3] = {};
|
||||
H_HDG[0] = -t31 * (t20 * (magZ * t16 + magY * t18) + t25 * t27 * (magY * t8 + magZ * t10));
|
||||
H_HDG[1] = t31 * (t20 * (magX * t18 + magZ * t22) + t25 * t27 * (magX * t8 - magZ * t11));
|
||||
H_HDG[2] = t31 * (t20 * (magX * t16 - magY * t22) + t25 * t27 * (magX * t10 + magY * t11));
|
||||
if (fabsf(t6) > 1e-6f) {
|
||||
t10 = 1.0f / (t6 * t6);
|
||||
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
float t11 = t9 * t9;
|
||||
float t12 = t10 * t11;
|
||||
float t13 = t12 + 1.0f;
|
||||
float t14;
|
||||
|
||||
if (fabsf(t13) > 1e-6f) {
|
||||
t14 = 1.0f / t13;
|
||||
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
float t15 = 1.0f / t6;
|
||||
|
||||
// calculate observation jacobian
|
||||
float H_YAW[3] = {};
|
||||
H_YAW[1] = t14 * (t15 * (q0 * q1 * 2.0f - q2 * q3 * 2.0f) + t9 * t10 * (q0 * q2 * 2.0f + q1 * q3 * 2.0f));
|
||||
H_YAW[2] = t14 * (t15 * (t2 - t3 + t4 - t5) + t9 * t10 * (t7 - t8));
|
||||
|
||||
// calculate innovation
|
||||
// rotate body field magnetic field measurement into earth frame and compare to published declination to get heading measurement
|
||||
// TODO - enable use of an off-board heading measurement
|
||||
matrix::Dcm<float> R_to_earth(_state.quat_nominal);
|
||||
matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
|
||||
|
||||
float innovation = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination;
|
||||
|
||||
innovation = math::constrain(innovation, -0.5f, 0.5f);
|
||||
// wrap the innovation to the interval between +-pi
|
||||
innovation = matrix::wrap_pi(innovation);
|
||||
_heading_innov = innovation;
|
||||
|
||||
float innovation_var = R_mag;
|
||||
_heading_innov_var = innovation_var;
|
||||
|
||||
// calculate innovation variance
|
||||
float innovation_var = R_YAW;
|
||||
_heading_innov_var = innovation_var;
|
||||
float PH[3] = {};
|
||||
|
||||
for (unsigned row = 0; row < 3; row++) {
|
||||
for (unsigned column = 0; column < 3; column++) {
|
||||
PH[row] += P[row][column] * H_HDG[column];
|
||||
PH[row] += P[row][column] * H_YAW[column];
|
||||
}
|
||||
|
||||
innovation_var += H_HDG[row] * PH[row];
|
||||
innovation_var += H_YAW[row] * PH[row];
|
||||
}
|
||||
|
||||
if (innovation_var >= R_mag) {
|
||||
if (innovation_var >= R_YAW) {
|
||||
// the innovation variance contribution from the state covariances is not negative, no fault
|
||||
_fault_status.bad_mag_hdg = false;
|
||||
|
||||
@@ -547,19 +569,42 @@ void Ekf::fuseHeading()
|
||||
return;
|
||||
}
|
||||
|
||||
float innovation_var_inv = 1 / innovation_var;
|
||||
float innovation_var_inv = 1.0f / innovation_var;
|
||||
|
||||
// calculate kalman gain
|
||||
// calculate the kalman gains taking dvantage of the reduce size of H_YAW
|
||||
float Kfusion[_k_num_states] = {};
|
||||
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
// gains for states that are always used
|
||||
for (unsigned row = 0; row <= 15; row++) {
|
||||
for (unsigned column = 0; column < 3; column++) {
|
||||
Kfusion[row] += P[row][column] * H_HDG[column];
|
||||
Kfusion[row] += P[row][column] * H_YAW[column];
|
||||
}
|
||||
|
||||
Kfusion[row] *= innovation_var_inv;
|
||||
}
|
||||
|
||||
// only calculate gains for magnetic field states if they are being used
|
||||
if (_control_status.flags.mag_3D) {
|
||||
for (unsigned row = 16; row <= 21; row++) {
|
||||
for (unsigned column = 0; column < 3; column++) {
|
||||
Kfusion[row] += P[row][column] * H_YAW[column];
|
||||
}
|
||||
|
||||
Kfusion[row] *= innovation_var_inv;
|
||||
}
|
||||
}
|
||||
|
||||
// only calculate gains for wind states if they are being used
|
||||
if (_control_status.flags.wind) {
|
||||
for (unsigned row = 22; row <= 23; row++) {
|
||||
for (unsigned column = 0; column < 3; column++) {
|
||||
Kfusion[row] += P[row][column] * H_YAW[column];
|
||||
}
|
||||
|
||||
Kfusion[row] *= innovation_var_inv;
|
||||
}
|
||||
}
|
||||
|
||||
// innovation test ratio
|
||||
_yaw_test_ratio = sq(innovation) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var);
|
||||
|
||||
@@ -572,12 +617,18 @@ void Ekf::fuseHeading()
|
||||
// by interference or a large initial gyro bias
|
||||
if (_control_status.flags.in_air) {
|
||||
return;
|
||||
|
||||
} else {
|
||||
// constrain the innovation to the maximum set by the gate
|
||||
float gate_limit = sqrtf((sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var));
|
||||
innovation = math::constrain(innovation, -gate_limit, gate_limit);
|
||||
}
|
||||
|
||||
} else {
|
||||
_mag_healthy = true;
|
||||
}
|
||||
|
||||
// zero the attitude error states and use the kalman gain vector and innovation to update the states
|
||||
_state.ang_error.setZero();
|
||||
fuse(Kfusion, innovation);
|
||||
|
||||
@@ -587,11 +638,12 @@ void Ekf::fuseHeading()
|
||||
_state.quat_nominal = dq * _state.quat_nominal;
|
||||
_state.quat_nominal.normalize();
|
||||
|
||||
// update the covariance matrix taking advantage of the reduced size of H_YAW
|
||||
float HP[_k_num_states] = {};
|
||||
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
for (unsigned row = 0; row < 3; row++) {
|
||||
HP[column] += H_HDG[row] * P[row][column];
|
||||
HP[column] += H_YAW[row] * P[row][column];
|
||||
}
|
||||
}
|
||||
|
||||
@@ -615,30 +667,28 @@ void Ekf::fuseDeclination()
|
||||
|
||||
// Calculate intermediate variables
|
||||
// if the horizontal magnetic field is too small, this calculation will be badly conditioned
|
||||
if (fabsf(magN) < 0.001f) {
|
||||
if (magN < 0.001f) {
|
||||
return;
|
||||
}
|
||||
|
||||
float t2 = 1.0f / magN;
|
||||
float t4 = magE * t2;
|
||||
float t3 = tanf(t4);
|
||||
float t5 = t3 * t3;
|
||||
float t6 = t5 + 1.0f;
|
||||
float t25 = t2 * t6;
|
||||
float t7 = 1.0f / (magN * magN);
|
||||
float t26 = magE * t6 * t7;
|
||||
float t8 = P[17][17] * t25;
|
||||
float t15 = P[16][17] * t26;
|
||||
float t9 = t8 - t15;
|
||||
float t10 = t25 * t9;
|
||||
float t11 = P[17][16] * t25;
|
||||
float t16 = P[16][16] * t26;
|
||||
float t12 = t11 - t16;
|
||||
float t17 = t26 * t12;
|
||||
float t13 = R_DECL + t10 - t17; // innovation variance
|
||||
float t2 = magE * magE;
|
||||
float t3 = magN * magN;
|
||||
float t4 = t2 + t3;
|
||||
float t5 = 1.0f / t4;
|
||||
float t22 = magE * t5;
|
||||
float t23 = magN * t5;
|
||||
float t6 = P[16][16] * t22;
|
||||
float t13 = P[17][16] * t23;
|
||||
float t7 = t6 - t13;
|
||||
float t8 = t22 * t7;
|
||||
float t9 = P[16][17] * t22;
|
||||
float t14 = P[17][17] * t23;
|
||||
float t10 = t9 - t14;
|
||||
float t15 = t23 * t10;
|
||||
float t11 = R_DECL + t8 - t15; // innovation variance
|
||||
|
||||
// check the innovation variance calculation for a badly conditioned covariance matrix
|
||||
if (t13 >= R_DECL) {
|
||||
if (t11 >= R_DECL) {
|
||||
// the innovation variance contribution from the state covariances is not negative, no fault
|
||||
_fault_status.bad_mag_decl = false;
|
||||
|
||||
@@ -651,50 +701,53 @@ void Ekf::fuseDeclination()
|
||||
return;
|
||||
}
|
||||
|
||||
float t14 = 1.0f / t13;
|
||||
float t18 = magE;
|
||||
float t19 = magN;
|
||||
float t21 = 1.0f / t19;
|
||||
float t22 = t18 * t21;
|
||||
float t20 = tanf(t22);
|
||||
float t23 = t20 * t20;
|
||||
float t24 = t23 + 1.0f;
|
||||
float t12 = 1.0f / t11;
|
||||
|
||||
// Calculate the observation Jacobian
|
||||
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
|
||||
float H_DECL[24] = {};
|
||||
H_DECL[16] = -t18 * 1.0f / (t19 * t19) * t24;
|
||||
H_DECL[17] = t21 * t24;
|
||||
H_DECL[16] = -magE * t5;
|
||||
H_DECL[17] = magN * t5;
|
||||
|
||||
// Calculate the Kalman gains
|
||||
float Kfusion[_k_num_states] = {};
|
||||
Kfusion[0] = t14 * (P[0][17] * t25 - P[0][16] * t26);
|
||||
Kfusion[1] = t14 * (P[1][17] * t25 - P[1][16] * t26);
|
||||
Kfusion[2] = t14 * (P[2][17] * t25 - P[2][16] * t26);
|
||||
Kfusion[3] = t14 * (P[3][17] * t25 - P[3][16] * t26);
|
||||
Kfusion[4] = t14 * (P[4][17] * t25 - P[4][16] * t26);
|
||||
Kfusion[5] = t14 * (P[5][17] * t25 - P[5][16] * t26);
|
||||
Kfusion[6] = t14 * (P[6][17] * t25 - P[6][16] * t26);
|
||||
Kfusion[7] = t14 * (P[7][17] * t25 - P[7][16] * t26);
|
||||
Kfusion[8] = t14 * (P[8][17] * t25 - P[8][16] * t26);
|
||||
Kfusion[9] = t14 * (P[9][17] * t25 - P[9][16] * t26);
|
||||
Kfusion[10] = t14 * (P[10][17] * t25 - P[10][16] * t26);
|
||||
Kfusion[11] = t14 * (P[11][17] * t25 - P[11][16] * t26);
|
||||
Kfusion[12] = t14 * (P[12][17] * t25 - P[12][16] * t26);
|
||||
Kfusion[13] = t14 * (P[13][17] * t25 - P[13][16] * t26);
|
||||
Kfusion[14] = t14 * (P[14][17] * t25 - P[14][16] * t26);
|
||||
Kfusion[15] = t14 * (P[15][17] * t25 - P[15][16] * t26);
|
||||
Kfusion[16] = -t14 * (t16 - P[16][17] * t25);
|
||||
Kfusion[17] = t14 * (t8 - P[17][16] * t26);
|
||||
Kfusion[18] = t14 * (P[18][17] * t25 - P[18][16] * t26);
|
||||
Kfusion[19] = t14 * (P[19][17] * t25 - P[19][16] * t26);
|
||||
Kfusion[20] = t14 * (P[20][17] * t25 - P[20][16] * t26);
|
||||
Kfusion[21] = t14 * (P[21][17] * t25 - P[21][16] * t26);
|
||||
Kfusion[22] = t14 * (P[22][17] * t25 - P[22][16] * t26);
|
||||
Kfusion[23] = t14 * (P[23][17] * t25 - P[23][16] * t26);
|
||||
Kfusion[0] = -t12 * (P[0][16] * t22 - P[0][17] * t23);
|
||||
Kfusion[1] = -t12 * (P[1][16] * t22 - P[1][17] * t23);
|
||||
Kfusion[2] = -t12 * (P[2][16] * t22 - P[2][17] * t23);
|
||||
Kfusion[3] = -t12 * (P[3][16] * t22 - P[3][17] * t23);
|
||||
Kfusion[4] = -t12 * (P[4][16] * t22 - P[4][17] * t23);
|
||||
Kfusion[5] = -t12 * (P[5][16] * t22 - P[5][17] * t23);
|
||||
Kfusion[6] = -t12 * (P[6][16] * t22 - P[6][17] * t23);
|
||||
Kfusion[7] = -t12 * (P[7][16] * t22 - P[7][17] * t23);
|
||||
Kfusion[8] = -t12 * (P[8][16] * t22 - P[8][17] * t23);
|
||||
Kfusion[9] = -t12 * (P[9][16] * t22 - P[9][17] * t23);
|
||||
Kfusion[10] = -t12 * (P[10][16] * t22 - P[10][17] * t23);
|
||||
Kfusion[11] = -t12 * (P[11][16] * t22 - P[11][17] * t23);
|
||||
Kfusion[12] = -t12 * (P[12][16] * t22 - P[12][17] * t23);
|
||||
Kfusion[13] = -t12 * (P[13][16] * t22 - P[13][17] * t23);
|
||||
Kfusion[14] = -t12 * (P[14][16] * t22 - P[14][17] * t23);
|
||||
Kfusion[15] = -t12 * (P[15][16] * t22 - P[15][17] * t23);
|
||||
|
||||
// We only do declination fusion when we are using all the field states, so no logic required here
|
||||
Kfusion[16] = -t12 * (t6 - P[16][17] * t23);
|
||||
Kfusion[17] = t12 * (t14 - P[17][16] * t22);
|
||||
Kfusion[18] = -t12 * (P[18][16] * t22 - P[18][17] * t23);
|
||||
Kfusion[19] = -t12 * (P[19][16] * t22 - P[19][17] * t23);
|
||||
Kfusion[20] = -t12 * (P[20][16] * t22 - P[20][17] * t23);
|
||||
Kfusion[21] = -t12 * (P[21][16] * t22 - P[21][17] * t23);
|
||||
|
||||
// Don't update wind states unless we are doing wind estimation
|
||||
if (_control_status.flags.wind) {
|
||||
Kfusion[22] = -t12 * (P[22][16] * t22 - P[22][17] * t23);
|
||||
Kfusion[23] = -t12 * (P[23][16] * t22 - P[23][17] * t23);
|
||||
|
||||
} else {
|
||||
Kfusion[22] = 0.0f;
|
||||
Kfusion[23] = 0.0f;
|
||||
}
|
||||
|
||||
// calculate innovation and constrain
|
||||
float innovation = atanf(t4) - _mag_declination;
|
||||
float innovation = atanf(magE / magN) - _mag_declination;
|
||||
innovation = math::constrain(innovation, -0.5f, 0.5f);
|
||||
|
||||
// zero attitude error states and perform the state correction
|
||||
|
||||
+22
-2
@@ -153,9 +153,29 @@ void Ekf::fuseVelPosHeight()
|
||||
unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state
|
||||
|
||||
// calculate kalman gain K = PHS, where S = 1/innovation variance
|
||||
for (int row = 0; row < 24; row++) {
|
||||
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
|
||||
for (int row = 0; row <= 15; row++) {
|
||||
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
|
||||
}
|
||||
// only update magnetic field states if we are fusing 3-axis observations
|
||||
if (_control_status.flags.mag_3D) {
|
||||
for (int row = 16; row <= 21; row++) {
|
||||
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
|
||||
}
|
||||
} else {
|
||||
for (int row = 16; row <= 21; row++) {
|
||||
Kfusion[row] = 0.0f;
|
||||
}
|
||||
}
|
||||
// only update wind states if we are doing wind estimation
|
||||
if (_control_status.flags.wind) {
|
||||
for (int row = 22; row <= 23; row++) {
|
||||
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
|
||||
}
|
||||
} else {
|
||||
for (int row = 22; row <= 23; row++) {
|
||||
Kfusion[row] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
// by definition the angle error state is zero at the fusion time
|
||||
_state.ang_error.setZero();
|
||||
|
||||
@@ -9,62 +9,56 @@ declinationAngle is the estimated declination as that location
|
||||
This fusion method is used to constrain the rotation of the earth field vector when there are no earth relative measurements
|
||||
(e.g. using optical flow without GPS, or when the vehicle is stationary) to provide an absolute yaw reference. In this situation the presence of yaw gyro errors
|
||||
can cause the magnetic declination of the earth field estimates to slowly rotate.
|
||||
|
||||
Divide by zero protection and protection against a badly conditioned covariance matrix must be included.
|
||||
*/
|
||||
|
||||
// Calculate intermediate variable
|
||||
float t2 = 1.0f/magN;
|
||||
float t4 = magE*t2;
|
||||
float t3 = tanf(t4);
|
||||
float t5 = t3*t3;
|
||||
float t6 = t5+1.0f;
|
||||
float t25 = t2*t6;
|
||||
float t7 = 1.0f/(magN*magN);
|
||||
float t26 = magE*t6*t7;
|
||||
float t8 = P[17][17]*t25;
|
||||
float t15 = P[16][17]*t26;
|
||||
float t9 = t8-t15;
|
||||
float t10 = t25*t9;
|
||||
float t11 = P[17][16]*t25;
|
||||
float t16 = P[16][16]*t26;
|
||||
float t12 = t11-t16;
|
||||
float t17 = t26*t12;
|
||||
float t13 = R_DECL+t10-t17;
|
||||
float t14 = 1.0f/t13;
|
||||
float t18 = magE;
|
||||
float t19 = magN;
|
||||
float t21 = 1.0f/t19;
|
||||
float t22 = t18*t21;
|
||||
float t20 = tanf(t22);
|
||||
float t23 = t20*t20;
|
||||
float t24 = t23+1.0f;
|
||||
float t2 = magE*magE;
|
||||
float t3 = magN*magN;
|
||||
float t4 = t2+t3;
|
||||
float t5 = 1.0f/t4;
|
||||
float t22 = magE*t5;
|
||||
float t23 = magN*t5;
|
||||
float t6 = P[16][16]*t22;
|
||||
float t13 = P[17][16]*t23;
|
||||
float t7 = t6-t13;
|
||||
float t8 = t22*t7;
|
||||
float t9 = P[16][17]*t22;
|
||||
float t14 = P[17][17]*t23;
|
||||
float t10 = t9-t14;
|
||||
float t15 = t23*t10;
|
||||
float t11 = R_DECL+t8-t15; // innovation variance
|
||||
float t12 = 1.0f/t11;
|
||||
|
||||
// Calculate the observation Jacobian
|
||||
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
|
||||
H_MAG[16] = -t18*1.0f/(t19*t19)*t24;
|
||||
H_MAG[17] = t21*t24;
|
||||
H_DECL[16] = -magE*t5;
|
||||
H_DECL[17] = magN*t5;
|
||||
|
||||
// Calculate the Kalman gains
|
||||
Kfusion[0] = t14*(P[0][17]*t25-P[0][16]*t26);
|
||||
Kfusion[1] = t14*(P[1][17]*t25-P[1][16]*t26);
|
||||
Kfusion[2] = t14*(P[2][17]*t25-P[2][16]*t26);
|
||||
Kfusion[3] = t14*(P[3][17]*t25-P[3][16]*t26);
|
||||
Kfusion[4] = t14*(P[4][17]*t25-P[4][16]*t26);
|
||||
Kfusion[5] = t14*(P[5][17]*t25-P[5][16]*t26);
|
||||
Kfusion[6] = t14*(P[6][17]*t25-P[6][16]*t26);
|
||||
Kfusion[7] = t14*(P[7][17]*t25-P[7][16]*t26);
|
||||
Kfusion[8] = t14*(P[8][17]*t25-P[8][16]*t26);
|
||||
Kfusion[9] = t14*(P[9][17]*t25-P[9][16]*t26);
|
||||
Kfusion[10] = t14*(P[10][17]*t25-P[10][16]*t26);
|
||||
Kfusion[11] = t14*(P[11][17]*t25-P[11][16]*t26);
|
||||
Kfusion[12] = t14*(P[12][17]*t25-P[12][16]*t26);
|
||||
Kfusion[13] = t14*(P[13][17]*t25-P[13][16]*t26);
|
||||
Kfusion[14] = t14*(P[14][17]*t25-P[14][16]*t26);
|
||||
Kfusion[15] = t14*(P[15][17]*t25-P[15][16]*t26);
|
||||
Kfusion[16] = -t14*(t16-P[16][17]*t25);
|
||||
Kfusion[17] = t14*(t8-P[17][16]*t26);
|
||||
Kfusion[18] = t14*(P[18][17]*t25-P[18][16]*t26);
|
||||
Kfusion[19] = t14*(P[19][17]*t25-P[19][16]*t26);
|
||||
Kfusion[20] = t14*(P[20][17]*t25-P[20][16]*t26);
|
||||
Kfusion[21] = t14*(P[21][17]*t25-P[21][16]*t26);
|
||||
Kfusion[22] = t14*(P[22][17]*t25-P[22][16]*t26);
|
||||
Kfusion[23] = t14*(P[23][17]*t25-P[23][16]*t26);
|
||||
Kfusion[0] = -t12*(P[0][16]*t22-P[0][17]*t23);
|
||||
Kfusion[1] = -t12*(P[1][16]*t22-P[1][17]*t23);
|
||||
Kfusion[2] = -t12*(P[2][16]*t22-P[2][17]*t23);
|
||||
Kfusion[3] = -t12*(P[3][16]*t22-P[3][17]*t23);
|
||||
Kfusion[4] = -t12*(P[4][16]*t22-P[4][17]*t23);
|
||||
Kfusion[5] = -t12*(P[5][16]*t22-P[5][17]*t23);
|
||||
Kfusion[6] = -t12*(P[6][16]*t22-P[6][17]*t23);
|
||||
Kfusion[7] = -t12*(P[7][16]*t22-P[7][17]*t23);
|
||||
Kfusion[8] = -t12*(P[8][16]*t22-P[8][17]*t23);
|
||||
Kfusion[9] = -t12*(P[9][16]*t22-P[9][17]*t23);
|
||||
Kfusion[10] = -t12*(P[10][16]*t22-P[10][17]*t23);
|
||||
Kfusion[11] = -t12*(P[11][16]*t22-P[11][17]*t23);
|
||||
Kfusion[12] = -t12*(P[12][16]*t22-P[12][17]*t23);
|
||||
Kfusion[13] = -t12*(P[13][16]*t22-P[13][17]*t23);
|
||||
Kfusion[14] = -t12*(P[14][16]*t22-P[14][17]*t23);
|
||||
Kfusion[15] = -t12*(P[15][16]*t22-P[15][17]*t23);
|
||||
Kfusion[16] = -t12*(t6-P[16][17]*t23);
|
||||
Kfusion[17] = t12*(t14-P[17][16]*t22);
|
||||
Kfusion[18] = -t12*(P[18][16]*t22-P[18][17]*t23);
|
||||
Kfusion[19] = -t12*(P[19][16]*t22-P[19][17]*t23);
|
||||
Kfusion[20] = -t12*(P[20][16]*t22-P[20][17]*t23);
|
||||
Kfusion[21] = -t12*(P[21][16]*t22-P[21][17]*t23);
|
||||
Kfusion[22] = -t12*(P[22][16]*t22-P[22][17]*t23);
|
||||
Kfusion[23] = -t12*(P[23][16]*t22-P[23][17]*t23);
|
||||
|
||||
|
||||
@@ -5,6 +5,8 @@ innovation = atan2f(magMeasEarthFrameEast,magMeasEarthFrameNorth) - declinationA
|
||||
|
||||
magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to eath frame.
|
||||
declinationAngle is the estimated declination as that location
|
||||
|
||||
Protection against /0 and covariance matrix errors will need to be added.
|
||||
*/
|
||||
|
||||
// intermediate variables
|
||||
@@ -12,36 +14,19 @@ float t2 = q0*q0;
|
||||
float t3 = q1*q1;
|
||||
float t4 = q2*q2;
|
||||
float t5 = q3*q3;
|
||||
float t6 = q0*q2*2.0f;
|
||||
float t7 = q1*q3*2.0f;
|
||||
float t8 = t6+t7;
|
||||
float t9 = q0*q3*2.0f;
|
||||
float t13 = q1*q2*2.0f;
|
||||
float t10 = t9-t13;
|
||||
float t11 = t2+t3-t4-t5;
|
||||
float t12 = magX*t11;
|
||||
float t14 = magZ*t8;
|
||||
float t19 = magY*t10;
|
||||
float t15 = t12+t14-t19;
|
||||
float t16 = t2-t3+t4-t5;
|
||||
float t17 = q0*q1*2.0f;
|
||||
float t24 = q2*q3*2.0f;
|
||||
float t18 = t17-t24;
|
||||
float t20 = 1.0f/t15;
|
||||
float t21 = magY*t16;
|
||||
float t22 = t9+t13;
|
||||
float t23 = magX*t22;
|
||||
float t28 = magZ*t18;
|
||||
float t25 = t21+t23-t28;
|
||||
float t29 = t20*t25;
|
||||
float t26 = tan(t29);
|
||||
float t27 = 1.0f/(t15*t15);
|
||||
float t30 = t26*t26;
|
||||
float t31 = t30+1.0f;
|
||||
float t6 = t2+t3-t4-t5;
|
||||
float t7 = q0*q3*2.0f;
|
||||
float t8 = q1*q2*2.0f;
|
||||
float t9 = t7+t8;
|
||||
float t10 = 1.0f/(t6*t6);
|
||||
float t11 = t9*t9;
|
||||
float t12 = t10*t11;
|
||||
float t13 = t12+1.0f;
|
||||
float t14 = 1.0f/t13;
|
||||
float t15 = 1.0f/t6;
|
||||
|
||||
// Calculate the observation jacobian for the innovation derivative wrt the attitude error states only
|
||||
// Use the reduced order to optimise the calculation of the Kalman gain matrix and covariance update
|
||||
float H_MAG[3];
|
||||
H_MAG[0] = -t31*(t20*(magZ*t16+magY*t18)+t25*t27*(magY*t8+magZ*t10));
|
||||
H_MAG[1] = t31*(t20*(magX*t18+magZ*t22)+t25*t27*(magX*t8-magZ*t11));
|
||||
H_MAG[2] = t31*(t20*(magX*t16-magY*t22)+t25*t27*(magX*t10+magY*t11));
|
||||
H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
|
||||
H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
|
||||
@@ -68,7 +68,7 @@ syms R_LOS real % variance of LOS angular rate mesurements (rad/sec)^2
|
||||
syms ptd real % location of terrain in D axis
|
||||
syms rotErrX rotErrY rotErrZ real; % error rotation vector in body frame
|
||||
syms decl real; % earth magnetic field declination from true north
|
||||
syms R_MAGS real; % variance for magnetic deviation measurement
|
||||
syms R_YAW real; % variance for magnetic deviation measurement
|
||||
syms R_DECL real; % variance of supplied declination
|
||||
syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y body axes
|
||||
syms rho real % air density (kg/m^3)
|
||||
@@ -269,19 +269,22 @@ ccode(H_LOS,'file','H_LOS.txt');
|
||||
ccode(K_LOSX,'file','K_LOSX.txt');
|
||||
ccode(K_LOSY,'file','K_LOSY.txt');
|
||||
|
||||
%% derive equations for fusion of magnetic heading measurement
|
||||
%% derive equations for fusion of direct yaw measurement
|
||||
|
||||
% rotate magnetic field into earth axes
|
||||
magMeasNED = Tbn*[magX;magY;magZ];
|
||||
% the predicted measurement is the angle wrt magnetic north of the horizontal
|
||||
% component of the measured field
|
||||
angMeas = tan(magMeasNED(2)/magMeasNED(1)) - decl;
|
||||
H_MAGS = jacobian(angMeas,errRotVec); % measurement Jacobian
|
||||
% rotate X body axis into earth axes
|
||||
yawVec = Tbn*[1;0;0];
|
||||
% Calculate the yaw angle of the projection
|
||||
angMeas = atan(yawVec(2)/yawVec(1));
|
||||
H_YAW = jacobian(angMeas,stateVector); % measurement Jacobian
|
||||
%H_MAGS = H_MAGS(1:3);
|
||||
H_MAGS = subs(H_MAGS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
H_MAGS = simplify(H_MAGS);
|
||||
H_YAW = subs(H_YAW, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
H_YAW = simplify(H_YAW);
|
||||
%[H_MAGS,SH_MAGS]=OptimiseAlgebra(H_MAGS,'SH_MAGS');
|
||||
ccode(H_MAGS,'file','calcH_MAGS.c');
|
||||
ccode(H_YAW,'file','calcH_YAW.c');
|
||||
% Calculate Kalman gain vector
|
||||
K_YAW = (P*transpose(H_YAW))/(H_YAW*P*transpose(H_YAW) + R_YAW);
|
||||
%K_MAGS = simplify(K_MAGS);
|
||||
ccode(K_YAW,'file','calcK_YAW.c');
|
||||
|
||||
%% derive equations for fusion of synthetic deviation measurement
|
||||
% used to keep correct heading when operating without absolute position or
|
||||
@@ -290,7 +293,7 @@ ccode(H_MAGS,'file','calcH_MAGS.c');
|
||||
magMeasNED = [magN;magE;magD];
|
||||
% the predicted measurement is the angle wrt magnetic north of the horizontal
|
||||
% component of the measured field
|
||||
angMeas = tan(magMeasNED(2)/magMeasNED(1));
|
||||
angMeas = atan(magMeasNED(2)/magMeasNED(1));
|
||||
H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
|
||||
H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
H_MAGD = simplify(H_MAGD);
|
||||
|
||||
Reference in New Issue
Block a user