mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
Merge pull request #55 from PX4/pr-FixYawFusionBug
Fix bug in heading fusion and add new method for ground/indoor use
This commit is contained in:
Paul Riseborough
commit
35865048a2
16
EKF/common.h
16
EKF/common.h
@ -163,8 +163,9 @@ struct parameters {
|
||||
|
||||
// Integer definitions for mag_fusion_type
|
||||
#define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic
|
||||
#define MAG_FUSE_TYPE_HEADING 1 // Magnetic heading fusion will alays be used. This is less accurate, but less affected by earth field distortions
|
||||
#define MAG_FUSE_TYPE_HEADING 1 // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
|
||||
#define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
|
||||
#define MAG_FUSE_TYPE_2D 3 // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by earth field distortions.
|
||||
|
||||
struct stateSample {
|
||||
Vector3f ang_error; // attitude axis angle error (error state formulation)
|
||||
@ -215,12 +216,13 @@ union filter_control_status_u {
|
||||
uint8_t yaw_align : 1; // 1 - true if the filter yaw alignment is complete
|
||||
uint8_t gps : 1; // 2 - true if GPS measurements are being fused
|
||||
uint8_t opt_flow : 1; // 3 - true if optical flow measurements are being fused
|
||||
uint8_t mag_hdg : 1; // 4 - true if a simple magnetic heading is being fused
|
||||
uint8_t mag_3D : 1; // 5 - true if 3-axis magnetometer measurement are being fused
|
||||
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
|
||||
uint8_t mag_hdg : 1; // 4 - true if a simple magnetic yaw heading is being fused
|
||||
uint8_t mag_2D : 1; // 5 - true if the horizontal projection of magnetometer data is being fused
|
||||
uint8_t mag_3D : 1; // 6 - true if 3-axis magnetometer measurement are being fused
|
||||
uint8_t mag_dec : 1; // 7 - true if synthetic magnetic declination measurements are being fused
|
||||
uint8_t in_air : 1; // 8 - true when the vehicle is airborne
|
||||
uint8_t armed : 1; // 9 - true when the vehicle motors are armed
|
||||
uint8_t wind : 1; // 10 - true when wind velocity is being estimated
|
||||
} flags;
|
||||
uint16_t value;
|
||||
};
|
||||
|
||||
@ -103,36 +103,55 @@ void Ekf::controlFusionModes()
|
||||
// or the more accurate 3-axis fusion
|
||||
if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
|
||||
if (!_control_status.flags.armed) {
|
||||
// always use simple mag fusion for initial startup
|
||||
_control_status.flags.mag_hdg = true;
|
||||
// always use 2D mag fusion for initial startup
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_2D = true;
|
||||
_control_status.flags.mag_3D = false;
|
||||
|
||||
} else {
|
||||
if (_control_status.flags.in_air) {
|
||||
// always use 3-axis mag fusion when airborne
|
||||
// always use 3D mag fusion when airborne
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_2D = false;
|
||||
_control_status.flags.mag_3D = true;
|
||||
|
||||
} else {
|
||||
// always use simple heading fusion when on the ground
|
||||
_control_status.flags.mag_hdg = true;
|
||||
// always use 2D mag fusion when on the ground
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_2D = true;
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
|
||||
// always use simple heading fusion
|
||||
_control_status.flags.mag_hdg = true;
|
||||
// always use yaw fusion unless tilt is over 45 deg, otherwise use 2D fusion
|
||||
if (_R_prev(2, 2) > 0.7071f) {
|
||||
_control_status.flags.mag_hdg = true;
|
||||
_control_status.flags.mag_2D = false;
|
||||
|
||||
} else {
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_2D = true;
|
||||
}
|
||||
|
||||
_control_status.flags.mag_3D = false;
|
||||
|
||||
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_2D) {
|
||||
// always use 2D mag fusion
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_2D = true;
|
||||
_control_status.flags.mag_3D = false;
|
||||
|
||||
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
|
||||
// always use 3-axis mag fusion
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_2D = false;
|
||||
_control_status.flags.mag_3D = true;
|
||||
|
||||
} else {
|
||||
// do no magnetometer fusion at all
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_2D = false;
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
|
||||
|
||||
@ -150,6 +150,9 @@ bool Ekf::update()
|
||||
fuseDeclination();
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.mag_2D && _control_status.flags.yaw_align) {
|
||||
fuseMag2D();
|
||||
|
||||
} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
|
||||
fuseHeading();
|
||||
|
||||
|
||||
@ -183,9 +183,12 @@ private:
|
||||
// ekf sequential fusion of magnetometer measurements
|
||||
void fuseMag();
|
||||
|
||||
// fuse magnetometer heading measurement
|
||||
// fuse heading measurement (currently uses estimate from magnetometer)
|
||||
void fuseHeading();
|
||||
|
||||
// fuse projecton of magnetometer onto horizontal plane
|
||||
void fuseMag2D();
|
||||
|
||||
// fuse magnetometer declination measurement
|
||||
void fuseDeclination();
|
||||
|
||||
|
||||
@ -495,7 +495,7 @@ void Ekf::fuseHeading()
|
||||
float R_YAW = fmaxf(_params.mag_heading_noise, 1.0e-2f);
|
||||
R_YAW = R_YAW * R_YAW;
|
||||
|
||||
// calculate intermediate variables for observation jacobian
|
||||
// calculate intermediate variables for observation jacobians
|
||||
float t2 = q0 * q0;
|
||||
float t3 = q1 * q1;
|
||||
float t4 = q2 * q2;
|
||||
@ -509,7 +509,7 @@ void Ekf::fuseHeading()
|
||||
if (fabsf(t6) > 1e-6f) {
|
||||
t10 = 1.0f / (t6 * t6);
|
||||
|
||||
} else {
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
@ -527,36 +527,39 @@ void Ekf::fuseHeading()
|
||||
|
||||
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));
|
||||
H_YAW[2] = t14 * (t15 * (t2 - t3 + t4 - t5) + t9 * t10 * (t7 - t8)); // calculate observation jacobian
|
||||
|
||||
// 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;
|
||||
// calculate intermediate expressions for Kalman gains
|
||||
float t16 = q0 * q1 * 2.0f;
|
||||
float t29 = q2 * q3 * 2.0f;
|
||||
float t17 = t16 - t29;
|
||||
float t18 = t15 * t17;
|
||||
float t19 = q0 * q2 * 2.0f;
|
||||
float t20 = q1 * q3 * 2.0f;
|
||||
float t21 = t19 + t20;
|
||||
float t22 = t9 * t10 * t21;
|
||||
float t23 = t18 + t22;
|
||||
float t40 = t14 * t23;
|
||||
float t24 = t2 - t3 + t4 - t5;
|
||||
float t25 = t15 * t24;
|
||||
float t26 = t7 - t8;
|
||||
float t27 = t9 * t10 * t26;
|
||||
float t28 = t25 + t27;
|
||||
float t41 = t14 * t28;
|
||||
float t30 = P[1][1] * t40;
|
||||
float t31 = P[1][2] * t40;
|
||||
float t32 = P[2][2] * t41;
|
||||
float t33 = t31 + t32;
|
||||
float t34 = t41 * t33;
|
||||
float t35 = P[2][1] * t41;
|
||||
float t36 = t30 + t35;
|
||||
float t37 = t40 * t36;
|
||||
float t38 = R_YAW + t34 + t37; // Innovation variance
|
||||
_heading_innov_var = t38;
|
||||
|
||||
// wrap the innovation to the interval between +-pi
|
||||
innovation = matrix::wrap_pi(innovation);
|
||||
_heading_innov = innovation;
|
||||
|
||||
// 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_YAW[column];
|
||||
}
|
||||
|
||||
innovation_var += H_YAW[row] * PH[row];
|
||||
}
|
||||
|
||||
if (innovation_var >= R_YAW) {
|
||||
if (t38 >= R_YAW) {
|
||||
// the innovation variance contribution from the state covariances is not negative, no fault
|
||||
_fault_status.bad_mag_hdg = false;
|
||||
|
||||
@ -569,44 +572,65 @@ void Ekf::fuseHeading()
|
||||
return;
|
||||
}
|
||||
|
||||
float innovation_var_inv = 1.0f / innovation_var;
|
||||
float t39 = 1.0f / t38;
|
||||
|
||||
// calculate the kalman gains taking dvantage of the reduce size of H_YAW
|
||||
float Kfusion[_k_num_states] = {};
|
||||
// calculate Kalman gains
|
||||
float Kfusion[24] = {};
|
||||
Kfusion[0] = t39 * (P[0][1] * t40 + P[0][2] * t41);
|
||||
Kfusion[1] = t39 * (t30 + P[1][2] * t41);
|
||||
Kfusion[2] = t39 * (t32 + P[2][1] * t40);
|
||||
Kfusion[3] = t39 * (P[3][1] * t40 + P[3][2] * t41);
|
||||
Kfusion[4] = t39 * (P[4][1] * t40 + P[4][2] * t41);
|
||||
Kfusion[5] = t39 * (P[5][1] * t40 + P[5][2] * t41);
|
||||
Kfusion[6] = t39 * (P[6][1] * t40 + P[6][2] * t41);
|
||||
Kfusion[7] = t39 * (P[7][1] * t40 + P[7][2] * t41);
|
||||
Kfusion[8] = t39 * (P[8][1] * t40 + P[8][2] * t41);
|
||||
Kfusion[9] = t39 * (P[9][1] * t40 + P[9][2] * t41);
|
||||
Kfusion[10] = t39 * (P[10][1] * t40 + P[10][2] * t41);
|
||||
Kfusion[11] = t39 * (P[11][1] * t40 + P[11][2] * t41);
|
||||
Kfusion[12] = t39 * (P[12][1] * t40 + P[12][2] * t41);
|
||||
Kfusion[13] = t39 * (P[13][1] * t40 + P[13][2] * t41);
|
||||
Kfusion[14] = t39 * (P[14][1] * t40 + P[14][2] * t41);
|
||||
Kfusion[15] = t39 * (P[15][1] * t40 + P[15][2] * t41);
|
||||
|
||||
// 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_YAW[column];
|
||||
}
|
||||
/* we won't be using these states because we are doing heading fusion
|
||||
Kfusion[16] = t39*(P[16][1]*t40+P[16][2]*t41);
|
||||
Kfusion[17] = t39*(P[17][1]*t40+P[17][2]*t41);
|
||||
Kfusion[18] = t39*(P[18][1]*t40+P[18][2]*t41);
|
||||
Kfusion[19] = t39*(P[19][1]*t40+P[19][2]*t41);
|
||||
Kfusion[20] = t39*(P[20][1]*t40+P[20][2]*t41);
|
||||
Kfusion[21] = t39*(P[21][1]*t40+P[21][2]*t41);
|
||||
*/
|
||||
|
||||
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
|
||||
// don't adjust these states if we are not using them
|
||||
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;
|
||||
}
|
||||
Kfusion[22] = t39 * (P[22][1] * t40 + P[22][2] * t41);
|
||||
Kfusion[23] = t39 * (P[23][1] * t40 + P[23][2] * t41);
|
||||
}
|
||||
|
||||
// TODO - enable use of an off-board heading measurement
|
||||
|
||||
// rotate the magnetometer measurement into earth frame using an assumed zero yaw angle
|
||||
matrix::Euler<float> euler(_state.quat_nominal);
|
||||
float predicted_hdg = euler(2); // we will need the predicted heading to calculate the innovation
|
||||
euler(2) = 0.0f;
|
||||
matrix::Dcm<float> R_to_earth(euler);
|
||||
matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
|
||||
|
||||
// Use the difference between the horizontal projection and declination to give the measured heading
|
||||
float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
|
||||
|
||||
// wrap the heading to the interval between +-pi
|
||||
measured_hdg = matrix::wrap_pi(measured_hdg);
|
||||
|
||||
// calculate the innovation
|
||||
_heading_innov = predicted_hdg - measured_hdg;
|
||||
|
||||
// wrap the innovation to the interval between +-pi
|
||||
_heading_innov = matrix::wrap_pi(_heading_innov);
|
||||
|
||||
// innovation test ratio
|
||||
_yaw_test_ratio = sq(innovation) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var);
|
||||
_yaw_test_ratio = sq(_heading_innov) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var);
|
||||
|
||||
// set the magnetometer unhealthy if the test fails
|
||||
if (_yaw_test_ratio > 1.0f) {
|
||||
@ -620,8 +644,8 @@ void Ekf::fuseHeading()
|
||||
|
||||
} 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);
|
||||
float gate_limit = sqrtf((sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var));
|
||||
_heading_innov = math::constrain(_heading_innov, -gate_limit, gate_limit);
|
||||
}
|
||||
|
||||
} else {
|
||||
@ -630,7 +654,7 @@ void Ekf::fuseHeading()
|
||||
|
||||
// zero the attitude error states and use the kalman gain vector and innovation to update the states
|
||||
_state.ang_error.setZero();
|
||||
fuse(Kfusion, innovation);
|
||||
fuse(Kfusion, _heading_innov);
|
||||
|
||||
// correct the nominal quaternion
|
||||
Quaternion dq;
|
||||
@ -642,7 +666,7 @@ void Ekf::fuseHeading()
|
||||
float HP[_k_num_states] = {};
|
||||
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
for (unsigned row = 0; row < 3; row++) {
|
||||
for (unsigned row = 1; row <= 2; row++) {
|
||||
HP[column] += H_YAW[row] * P[row][column];
|
||||
}
|
||||
}
|
||||
@ -776,8 +800,7 @@ void Ekf::fuseDeclination()
|
||||
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
float tmp = KH[row][0] * P[0][column];
|
||||
tmp += KH[row][16] * P[16][column];
|
||||
float tmp = KH[row][16] * P[16][column];
|
||||
tmp += KH[row][17] * P[17][column];
|
||||
KHP[row][column] = tmp;
|
||||
}
|
||||
@ -793,3 +816,194 @@ void Ekf::fuseDeclination()
|
||||
makeSymmetrical();
|
||||
limitCov();
|
||||
}
|
||||
|
||||
void Ekf::fuseMag2D()
|
||||
{
|
||||
// assign intermediate state variables
|
||||
float q0 = _state.quat_nominal(0);
|
||||
float q1 = _state.quat_nominal(1);
|
||||
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_DECL = fmaxf(_params.mag_heading_noise, 1.0e-2f);
|
||||
R_DECL = R_DECL * R_DECL;
|
||||
|
||||
// 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 * q3 * 2.0f;
|
||||
float t8 = t2 - t3 + t4 - t5;
|
||||
float t9 = q0 * q1 * 2.0f;
|
||||
float t10 = q2 * q3 * 2.0f;
|
||||
float t11 = t9 - t10;
|
||||
float t14 = q1 * q2 * 2.0f;
|
||||
float t21 = magY * t8;
|
||||
float t22 = t6 + t14;
|
||||
float t23 = magX * t22;
|
||||
float t24 = magZ * t11;
|
||||
float t7 = t21 + t23 - t24;
|
||||
float t12 = t2 + t3 - t4 - t5;
|
||||
float t13 = magX * t12;
|
||||
float t15 = q0 * q2 * 2.0f;
|
||||
float t16 = q1 * q3 * 2.0f;
|
||||
float t17 = t15 + t16;
|
||||
float t18 = magZ * t17;
|
||||
float t19 = t6 - t14;
|
||||
float t25 = magY * t19;
|
||||
float t20 = t13 + t18 - t25;
|
||||
|
||||
if (fabsf(t20) < 1e-6f) {
|
||||
return;
|
||||
}
|
||||
|
||||
float t26 = 1.0f / (t20 * t20);
|
||||
float t27 = t7 * t7;
|
||||
float t28 = t26 * t27;
|
||||
float t29 = t28 + 1.0f;
|
||||
|
||||
if (fabsf(t29) < 1e-12f) {
|
||||
return;
|
||||
}
|
||||
|
||||
float t30 = 1.0f / t29;
|
||||
|
||||
if (fabsf(t20) < 1e-12f) {
|
||||
return;
|
||||
}
|
||||
|
||||
float t31 = 1.0f / t20;
|
||||
|
||||
// calculate observation jacobian
|
||||
float H_DECL[3] = {};
|
||||
H_DECL[0] = -t30 * (t31 * (magZ * t8 + magY * t11) + t7 * t26 * (magY * t17 + magZ * t19));
|
||||
H_DECL[1] = t30 * (t31 * (magX * t11 + magZ * t22) - t7 * t26 * (magZ * t12 - magX * t17));
|
||||
H_DECL[2] = t30 * (t31 * (magX * t8 - magY * t22) + t7 * t26 * (magY * t12 + magX * t19));
|
||||
|
||||
// rotate the magnetometer measurement into earth frame
|
||||
matrix::Dcm<float> R_to_earth(_state.quat_nominal);
|
||||
matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
|
||||
|
||||
// check if there is enough magnetic field length to use and exit if too small
|
||||
float magLength2 = sq(mag_earth_pred(0) + mag_earth_pred(1));
|
||||
|
||||
if (magLength2 < sq(_params.mag_noise)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Adjust the measurement variance upwards if thehorizontal strength to magnetometer noise ratio make the value unrealistic
|
||||
R_DECL = fmaxf(R_DECL, sq(_params.mag_noise) / magLength2);
|
||||
|
||||
// Calculate the innovation, using the declination angle of the projection onto the horizontal as the measurement
|
||||
_heading_innov = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination;
|
||||
|
||||
// wrap the innovation to the interval between +-pi
|
||||
_heading_innov = matrix::wrap_pi(_heading_innov);
|
||||
|
||||
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
|
||||
float PH[3];
|
||||
_heading_innov_var = R_DECL;
|
||||
|
||||
for (unsigned row = 0; row <= 2; row++) {
|
||||
PH[row] = 0.0f;
|
||||
|
||||
for (unsigned col = 0; col <= 2; col++) {
|
||||
PH[row] += P[row][col] * H_DECL[col];
|
||||
}
|
||||
|
||||
_heading_innov_var += H_DECL[row] * PH[row];
|
||||
}
|
||||
|
||||
float varInnovInv;
|
||||
|
||||
if (_heading_innov_var >= R_DECL) {
|
||||
// the innovation variance contribution from the state covariances is not negative, no fault
|
||||
_fault_status.bad_mag_hdg = false;
|
||||
|
||||
} else {
|
||||
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
|
||||
_fault_status.bad_mag_hdg = true;
|
||||
|
||||
// we reinitialise the covariance matrix and abort this fusion step
|
||||
initialiseCovariance();
|
||||
return;
|
||||
}
|
||||
|
||||
// innovation test ratio
|
||||
_yaw_test_ratio = sq(_heading_innov) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var);
|
||||
|
||||
// set the magnetometer unhealthy if the test fails
|
||||
if (_yaw_test_ratio > 1.0f) {
|
||||
_mag_healthy = false;
|
||||
|
||||
// if we are in air we don't want to fuse the measurement
|
||||
// we allow to use it when on the ground because the large innovation could be caused
|
||||
// by interference or a large initial gyro bias
|
||||
if (_control_status.flags.in_air) {
|
||||
printf("return 5\n");
|
||||
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)) * _heading_innov_var));
|
||||
_heading_innov = math::constrain(_heading_innov, -gate_limit, gate_limit);
|
||||
}
|
||||
|
||||
} else {
|
||||
_mag_healthy = true;
|
||||
}
|
||||
|
||||
varInnovInv = 1.0f / _heading_innov_var;
|
||||
|
||||
// calculate the Kalman gains
|
||||
float Kfusion[24] = {};
|
||||
|
||||
for (unsigned row = 0; row < 16; row++) {
|
||||
Kfusion[row] = 0.0f;
|
||||
|
||||
for (unsigned col = 0; col <= 2; col++) {
|
||||
Kfusion[row] += P[row][col] * H_DECL[col];
|
||||
}
|
||||
|
||||
Kfusion[row] *= varInnovInv;
|
||||
}
|
||||
|
||||
// by definition our error state is zero at the time of fusion
|
||||
_state.ang_error.setZero();
|
||||
|
||||
// correct the states
|
||||
fuse(Kfusion, _heading_innov);
|
||||
|
||||
// correct the quaternon using the attitude error estimate
|
||||
Quaternion q_correction;
|
||||
q_correction.from_axis_angle(_state.ang_error);
|
||||
_state.quat_nominal = q_correction * _state.quat_nominal;
|
||||
_state.quat_nominal.normalize();
|
||||
_state.ang_error.setZero();
|
||||
|
||||
// correct the covariance using P = P - K*H*P taking advantage of the fact that only the first 3 elements in H are non zero
|
||||
// and we only need the first 16 states
|
||||
float HP[16];
|
||||
|
||||
for (uint8_t col = 0; col < 16; col++) {
|
||||
HP[col] = 0.0f;
|
||||
|
||||
for (uint8_t row = 0; row <= 2; row++) {
|
||||
HP[col] += H_DECL[row] * P[row][col];
|
||||
}
|
||||
}
|
||||
|
||||
for (uint8_t row = 0; row < 16; row++) {
|
||||
for (uint8_t col = 0; col < 16; col++) {
|
||||
P[row][col] -= Kfusion[row] * HP[col];
|
||||
}
|
||||
}
|
||||
|
||||
makeSymmetrical();
|
||||
limitCov();
|
||||
}
|
||||
|
||||
@ -1,32 +1,57 @@
|
||||
/*
|
||||
Autocode for fusion of a yaw error measurement where the innovation is given by:
|
||||
Observation jacobian for fusion of the horizontal components of magnetometer measurements.
|
||||
|
||||
innovation = atan2f(magMeasEarthFrameEast,magMeasEarthFrameNorth) - declinationAngle;
|
||||
innovation = atan2(magE/magN) - declination, where magN and magE are the North and East components obtained
|
||||
by rotating the measured magnetometer readings from body into earth axes.
|
||||
|
||||
magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to eath frame.
|
||||
declinationAngle is the estimated declination as that location
|
||||
This method of fusion reduces roll and pitch errors due to external field disturbances and is suitable for initial alignment and ground / indoor use
|
||||
|
||||
Protection against /0 and covariance matrix errors will need to be added.
|
||||
Divide by zero protection hs been added
|
||||
*/
|
||||
|
||||
// intermediate variables
|
||||
// calculate intermediate variables for observation jacobian
|
||||
float t2 = q0*q0;
|
||||
float t3 = q1*q1;
|
||||
float t4 = q2*q2;
|
||||
float t5 = q3*q3;
|
||||
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;
|
||||
float t6 = q0*q3*2.0f;
|
||||
float t8 = t2-t3+t4-t5;
|
||||
float t9 = q0*q1*2.0f;
|
||||
float t10 = q2*q3*2.0f;
|
||||
float t11 = t9-t10;
|
||||
float t14 = q1*q2*2.0f;
|
||||
float t21 = magY*t8;
|
||||
float t22 = t6+t14;
|
||||
float t23 = magX*t22;
|
||||
float t24 = magZ*t11;
|
||||
float t7 = t21+t23-t24;
|
||||
float t12 = t2+t3-t4-t5;
|
||||
float t13 = magX*t12;
|
||||
float t15 = q0*q2*2.0f;
|
||||
float t16 = q1*q3*2.0f;
|
||||
float t17 = t15+t16;
|
||||
float t18 = magZ*t17;
|
||||
float t19 = t6-t14;
|
||||
float t25 = magY*t19;
|
||||
float t20 = t13+t18-t25;
|
||||
if (fabsf(t20) < 1e-6f) {
|
||||
return;
|
||||
}
|
||||
float t26 = 1.0f/(t20*t20);
|
||||
float t27 = t7*t7;
|
||||
float t28 = t26*t27;
|
||||
float t29 = t28+1.0;
|
||||
if (fabsf(t29) < 1e-12f) {
|
||||
return;
|
||||
}
|
||||
float t30 = 1.0f/t29;
|
||||
if (fabsf(t20) < 1e-12f) {
|
||||
return;
|
||||
}
|
||||
float t31 = 1.0f/t20;
|
||||
|
||||
// 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_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 observation jacobian
|
||||
float H_DECL[3] = {};
|
||||
H_DECL[0] = -t30*(t31*(magZ*t8+magY*t11)+t7*t26*(magY*t17+magZ*t19));
|
||||
H_DECL[1] = t30*(t31*(magX*t11+magZ*t22)-t7*t26*(magZ*t12-magX*t17));
|
||||
H_DECL[2] = t30*(t31*(magX*t8-magY*t22)+t7*t26*(magY*t12+magX*t19));
|
||||
|
||||
27
matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt
Normal file
27
matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt
Normal file
@ -0,0 +1,27 @@
|
||||
/*
|
||||
Autocode for fusion of an Euler yaw measurement.
|
||||
|
||||
Protection against /0 and covariance matrix errors will need to be added.
|
||||
*/
|
||||
|
||||
// intermediate variables
|
||||
float t2 = q0*q0;
|
||||
float t3 = q1*q1;
|
||||
float t4 = q2*q2;
|
||||
float t5 = q3*q3;
|
||||
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_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));
|
||||
@ -1,6 +1,6 @@
|
||||
% IMPORTANT - This script requires the Matlab symbolic toolbox and takes ~3 hours to run
|
||||
|
||||
% Derivation of Navigation EKF using a local NED earth Tangent Frame and
|
||||
% Derivation of Navigation EKF using a local NED earth Tangent Frame and
|
||||
% XYZ body fixed frame
|
||||
% Sequential fusion of velocity and position measurements
|
||||
% Fusion of true airspeed
|
||||
@ -15,8 +15,8 @@
|
||||
% Based on use of a rotation vector for attitude estimation as described
|
||||
% here:
|
||||
|
||||
% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation",
|
||||
% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003),
|
||||
% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation",
|
||||
% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003),
|
||||
% pp. 855-860.
|
||||
|
||||
% State vector:
|
||||
@ -68,8 +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_YAW real; % variance for magnetic deviation measurement
|
||||
syms R_DECL real; % variance of supplied declination
|
||||
syms R_DECL R_YAW real; % variance of declination or yaw angle observation
|
||||
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)
|
||||
syms R_ACC real % variance of accelerometer measurements (m/s^2)^2
|
||||
@ -102,7 +101,7 @@ truthQuat = QuatMult(estQuat, errQuat);
|
||||
Tbn = Quat2Tbn(truthQuat);
|
||||
|
||||
% define the truth delta angle
|
||||
% ignore coning compensation as these effects are negligible in terms of
|
||||
% ignore coning compensation as these effects are negligible in terms of
|
||||
% covariance growth for our application and grade of sensor
|
||||
dAngTruth = dAngMeas.*dAngScale - dAngBias - [daxNoise;dayNoise;dazNoise];
|
||||
|
||||
@ -189,10 +188,10 @@ F = jacobian(newStateVector, stateVector);
|
||||
F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
[F,SF]=OptimiseAlgebra(F,'SF');
|
||||
|
||||
% define a symbolic covariance matrix using strings to represent
|
||||
% define a symbolic covariance matrix using strings to represent
|
||||
% '_l_' to represent '( '
|
||||
% '_c_' to represent ,
|
||||
% '_r_' to represent ')'
|
||||
% '_r_' to represent ')'
|
||||
% these can be substituted later to create executable code
|
||||
for rowIndex = 1:nStates
|
||||
for colIndex = 1:nStates
|
||||
@ -264,12 +263,33 @@ K_LOS = [K_LOSX,K_LOSY];
|
||||
simplify(K_LOS);
|
||||
[K_LOS,SK_LOS]=OptimiseAlgebra(K_LOS,'SK_LOS');
|
||||
|
||||
% Use matlab c code converter for an alternate method of
|
||||
% Use matlab c code converter for an alternate method of
|
||||
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 direct yaw measurement
|
||||
%% derive equations for simple fusion of 2-D magnetic heading measurements
|
||||
|
||||
% rotate magnetic field into earth axes
|
||||
magMeasNED = Tbn*[magX;magY;magZ];
|
||||
% the predicted measurement is the angle wrt true north of the horizontal
|
||||
% component of the measured field
|
||||
angMeas = atan(magMeasNED(2)/magMeasNED(1));
|
||||
simpleStateVector = [errRotVec;vn;ve;vd;pn;pe;pd;dax_b;day_b;daz_b;dax_s;day_s;daz_s;dvz_b];
|
||||
Psimple = P(1:16,1:16);
|
||||
H_MAGS = jacobian(angMeas,simpleStateVector); % 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_MAGS,SH_MAGS]=OptimiseAlgebra(H_MAGS,'SH_MAGS');
|
||||
ccode(H_MAGS,'file','calcH_MAGS.c');
|
||||
% Calculate Kalman gain vector
|
||||
K_MAGS = (Psimple*transpose(H_MAGS))/(H_MAGS*Psimple*transpose(H_MAGS) + R_DECL);
|
||||
%K_MAGS = simplify(K_MAGS);
|
||||
%[K_MAGS,SK_MAGS]=OptimiseAlgebra(K_MAGS,'SK_MAGS');
|
||||
ccode(K_MAGS,'file','calcK_MAGS.c');
|
||||
|
||||
%% derive equations for fusion of yaw measurements
|
||||
|
||||
% rotate X body axis into earth axes
|
||||
yawVec = Tbn*[1;0;0];
|
||||
@ -298,7 +318,7 @@ H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
|
||||
H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
H_MAGD = simplify(H_MAGD);
|
||||
%[H_MAGD,SH_MAGD]=OptimiseAlgebra(H_MAGD,'SH_MAGD');
|
||||
%ccode(H_MAGD,'file','calcH_MAGD.c');
|
||||
ccode(H_MAGD,'file','calcH_MAGD.c');
|
||||
% Calculate Kalman gain vector
|
||||
K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL);
|
||||
ccode([H_MAGD',K_MAGD],'file','calcMAGD.c');
|
||||
@ -319,7 +339,7 @@ vrel = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd]; % predicted wind relative velocity
|
||||
% accYpred = -0.5*rho*vrel(2)*vrel(2)*BCYinv; % predicted acceleration measured along Y body axis
|
||||
|
||||
% Use a simple viscous drag model for the linear estimator equations
|
||||
% Use the the derivative from speed to acceleration averaged across the
|
||||
% Use the the derivative from speed to acceleration averaged across the
|
||||
% speed range
|
||||
% The nonlinear equation will be used to calculate the predicted
|
||||
% measurement in implementation
|
||||
@ -339,14 +359,12 @@ H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian
|
||||
H_ACCY = subs(H_ACCY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
[H_ACCY,SH_ACCY]=OptimiseAlgebra(H_ACCY,'SH_ACCY'); % optimise processing
|
||||
K_ACCY = (P*transpose(H_ACCY))/(H_ACCY*P*transpose(H_ACCY) + R_ACC);
|
||||
ccode([H_ACCY',K_ACCY],'file','calcACCY.c');
|
||||
[K_ACCY,SK_ACCY]=OptimiseAlgebra(K_ACCY,'SK_ACCY'); % Kalman gain vector
|
||||
|
||||
% generate c code for jacobians using the matlab code generator
|
||||
ccode([H_ACCX;H_ACCY],'file','calcH_ACCXY.c');
|
||||
|
||||
%% Save output and convert to m and c code fragments
|
||||
fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
|
||||
save(fileName);
|
||||
SaveScriptCode(nStates);
|
||||
ConvertToM(nStates);
|
||||
ConvertToC(nStates);
|
||||
ConvertToC(nStates);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user