From 4e946d5bcb8bd0e475b0f39c1b925108f024fc9e Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Wed, 4 Sep 2019 19:51:39 +0200 Subject: [PATCH] implemented synthesized magnetometer Z measurement - calculate a theoretical value based on the knowledge of the direction and strength of the magnetic field vector and X/Y sensor measurements - needs knowledge about location on earth to work --- EKF/common.h | 4 ++++ EKF/control.cpp | 12 ++++++++++++ EKF/ekf.h | 5 +++++ EKF/mag_fusion.cpp | 25 ++++++++++++++++++++++++- 4 files changed, 45 insertions(+), 1 deletion(-) diff --git a/EKF/common.h b/EKF/common.h index c5ecaf81ea..c36ac6d216 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -357,6 +357,9 @@ struct parameters { // control of on-ground movement check float is_moving_scaler{1.0f}; ///< gain scaler used to adjust the threshold for the on-ground movement detection. Larger values make the test less sensitive. + + // compute synthetic magnetomter Z value if possible + int32_t synthesize_mag_z{0}; }; struct stateSample { @@ -458,6 +461,7 @@ union filter_control_status_u { uint32_t gps_yaw : 1; ///< 22 - true when yaw (not ground course) data from a GPS receiver is being fused uint32_t mag_align_complete : 1; ///< 23 - true when the in-flight mag field alignment has been completed uint32_t ev_vel : 1; ///< 24 - true when local earth frame velocity data from external vision measurements are being fused + uint32_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component } flags; uint32_t value; }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 6db7dc1dc2..32d282683f 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -90,6 +90,18 @@ void Ekf::controlFusionModes() _gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed); _mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed); + if (_mag_data_ready) { + // if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value. + // this is useful if there is a lot of interference on the sensor measurement. + if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) &&_NED_origin_initialised) { + Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0); + _mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred); + _control_status.flags.synthetic_mag_z = true; + } else { + _control_status.flags.synthetic_mag_z = false; + } + } + _delta_time_baro_us = _baro_sample_delayed.time_us; _baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed); diff --git a/EKF/ekf.h b/EKF/ekf.h index 26b8487ec1..9f90e61e14 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -362,6 +362,7 @@ private: bool _vehicle_at_rest_prev{false}; ///< true when the vehicle was at rest the previous time the status was checked bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetometer data has been requested bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event. + bool _synthetic_mag_z_active{false}; ///< true if we are generating synthetic magnetometer Z measurements float P[_k_num_states][_k_num_states] {}; ///< state covariance matrix @@ -719,4 +720,8 @@ private: // Ref: https://en.wikipedia.org/wiki/Kahan_summation_algorithm float kahanSummation(float sum_previous, float input, float &accumulator) const; + // calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter + // sensor measurement + float calculate_synthetic_mag_z_measurement(Vector3f mag_meas, Vector3f mag_earth_predicted); + }; diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 355384b398..585884ee21 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -81,8 +81,13 @@ void Ekf::fuseMag() // compute magnetometer innovations _mag_innov[0] = (mag_I_rot(0) + _state.mag_B(0)) - _mag_sample_delayed.mag(0); _mag_innov[1] = (mag_I_rot(1) + _state.mag_B(1)) - _mag_sample_delayed.mag(1); - _mag_innov[2] = (mag_I_rot(2) + _state.mag_B(2)) - _mag_sample_delayed.mag(2); + // do not use the synthesized measurement for the magnetomter Z component for 3D fusion + if (_control_status.flags.synthetic_mag_z) { + _mag_innov[2] = 0.0f; + } else { + _mag_innov[2] = (mag_I_rot(2) + _state.mag_B(2)) - _mag_sample_delayed.mag(2); + } // Observation jacobian and Kalman gain vectors float H_MAG[24]; float Kfusion[24]; @@ -284,6 +289,12 @@ void Ekf::fuseMag() Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); } else if (index == 2) { + + // we do not fuse synthesized magnetomter measurements when doing 3D fusion + if (_control_status.flags.synthetic_mag_z) { + continue; + } + // calculate Z axis observation jacobians memset(H_MAG, 0, sizeof(H_MAG)); H_MAG[0] = SH_MAG[1]; @@ -983,3 +994,15 @@ void Ekf::limitDeclination() _state.mag_I(1) = h_field * sinf(decl_min); } } + +float Ekf::calculate_synthetic_mag_z_measurement(Vector3f mag_meas, Vector3f mag_earth_predicted) +{ + // theoretical magnitude of the magnetometer Z component value given X and Y sensor measurement and our knowledge + // of the earth magnetic field vector at the current location + float mag_z_abs = sqrtf(math::max(sq(mag_earth_predicted.length()) - sq(mag_meas(0)) - sq(mag_meas(1)), 0.0f)); + + // calculate sign of synthetic magnetomter Z component based on the sign of the predicted magnetomer Z component + float mag_z_body_pred = _R_to_earth(0,2) * mag_earth_predicted(0) + _R_to_earth(1,2) * mag_earth_predicted(1) + _R_to_earth(2,2) * mag_earth_predicted(2); + + return mag_z_body_pred < 0 ? -mag_z_abs : mag_z_abs; +}