From d79e12dfa10aa97531c87155e2677769c0bb5e66 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 31 Jan 2016 00:01:44 -0800 Subject: [PATCH] EKF: fix code style --- EKF/RingBuffer.h | 2 +- EKF/common.h | 291 +++++++------- EKF/control.cpp | 144 +++---- EKF/covariance.cpp | 7 +- EKF/ekf.cpp | 81 ++-- EKF/ekf.h | 74 ++-- EKF/ekf_helper.cpp | 8 +- EKF/estimator_interface.cpp | 39 +- EKF/estimator_interface.h | 40 +- EKF/gps_checks.cpp | 252 ++++++------ EKF/mag_fusion.cpp | 753 ++++++++++++++++++------------------ 11 files changed, 864 insertions(+), 827 deletions(-) diff --git a/EKF/RingBuffer.h b/EKF/RingBuffer.h index e7a500ddf6..1704f5d56b 100644 --- a/EKF/RingBuffer.h +++ b/EKF/RingBuffer.h @@ -130,7 +130,7 @@ public: if (timestamp >= _buffer[index].time_us && timestamp - _buffer[index].time_us < 100000) { // TODO Re-evaluate the static cast and usage patterns - memcpy(static_cast(sample), static_cast(&_buffer[index]), sizeof(*sample)); + memcpy(static_cast(sample), static_cast(&_buffer[index]), sizeof(*sample)); // Now we can set the tail to the item which comes after the one we removed // since we don't want to have any older data in the buffer diff --git a/EKF/common.h b/EKF/common.h index 235ca8a51f..41e8372bbf 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -39,168 +39,169 @@ * @author Siddharth Bharat Purohit * */ -namespace estimator { - struct gps_message { - uint64_t time_usec; - int32_t lat; // Latitude in 1E-7 degrees - int32_t lon; // Longitude in 1E-7 degrees - int32_t alt; // Altitude in 1E-3 meters (millimeters) above MSL - uint8_t fix_type; // 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time - float eph; // GPS horizontal position accuracy in m - float epv; // GPS vertical position accuracy in m - float sacc; // GPS speed accuracy in m/s - uint64_t time_usec_vel; // Timestamp for velocity informations - float vel_m_s; // GPS ground speed (m/s) - float vel_ned[3]; // GPS ground speed NED - bool vel_ned_valid; // GPS ground speed is valid - uint8_t nsats; // number of satellites used - float gdop; // geometric dilution of precision - }; +namespace estimator +{ +struct gps_message { + uint64_t time_usec; + int32_t lat; // Latitude in 1E-7 degrees + int32_t lon; // Longitude in 1E-7 degrees + int32_t alt; // Altitude in 1E-3 meters (millimeters) above MSL + uint8_t fix_type; // 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time + float eph; // GPS horizontal position accuracy in m + float epv; // GPS vertical position accuracy in m + float sacc; // GPS speed accuracy in m/s + uint64_t time_usec_vel; // Timestamp for velocity informations + float vel_m_s; // GPS ground speed (m/s) + float vel_ned[3]; // GPS ground speed NED + bool vel_ned_valid; // GPS ground speed is valid + uint8_t nsats; // number of satellites used + float gdop; // geometric dilution of precision +}; - typedef matrix::Vector Vector2f; - typedef matrix::Vector Vector3f; - typedef matrix::Quaternion Quaternion; - typedef matrix::Matrix Matrix3f; +typedef matrix::Vector Vector2f; +typedef matrix::Vector Vector3f; +typedef matrix::Quaternion Quaternion; +typedef matrix::Matrix Matrix3f; - struct outputSample { - Quaternion quat_nominal; - Vector3f vel; - Vector3f pos; - uint64_t time_us; - }; +struct outputSample { + Quaternion quat_nominal; + Vector3f vel; + Vector3f pos; + uint64_t time_us; +}; - struct imuSample { - Vector3f delta_ang; - Vector3f delta_vel; - float delta_ang_dt; - float delta_vel_dt; - uint64_t time_us; - }; +struct imuSample { + Vector3f delta_ang; + Vector3f delta_vel; + float delta_ang_dt; + float delta_vel_dt; + uint64_t time_us; +}; - struct gpsSample { - Vector2f pos; - float hgt; - Vector3f vel; - uint64_t time_us; - }; +struct gpsSample { + Vector2f pos; + float hgt; + Vector3f vel; + uint64_t time_us; +}; - struct magSample { - Vector3f mag; - uint64_t time_us; - }; +struct magSample { + Vector3f mag; + uint64_t time_us; +}; - struct baroSample { - float hgt; - uint64_t time_us; - }; +struct baroSample { + float hgt; + uint64_t time_us; +}; - struct rangeSample { - float rng; - uint64_t time_us; - }; +struct rangeSample { + float rng; + uint64_t time_us; +}; - struct airspeedSample { - float airspeed; - uint64_t time_us; - }; +struct airspeedSample { + float airspeed; + uint64_t time_us; +}; - struct flowSample { - Vector2f flowRadXY; - Vector2f flowRadXYcomp; - uint64_t time_us; - }; +struct flowSample { + Vector2f flowRadXY; + Vector2f flowRadXYcomp; + uint64_t time_us; +}; - struct parameters { - float mag_delay_ms = 0.0f; - float baro_delay_ms = 0.0f; - float gps_delay_ms = 200.0f; - float airspeed_delay_ms = 200.0f; +struct parameters { + float mag_delay_ms = 0.0f; + float baro_delay_ms = 0.0f; + float gps_delay_ms = 200.0f; + float airspeed_delay_ms = 200.0f; - // input noise - float gyro_noise = 0.001f; - float accel_noise = 0.1f; + // input noise + float gyro_noise = 0.001f; + float accel_noise = 0.1f; - // process noise - float gyro_bias_p_noise = 1e-5f; - float accel_bias_p_noise = 1e-3f; - float gyro_scale_p_noise = 1e-4f; - float mag_p_noise = 1e-2f; - float wind_vel_p_noise = 0.05f; + // process noise + float gyro_bias_p_noise = 1e-5f; + float accel_bias_p_noise = 1e-3f; + float gyro_scale_p_noise = 1e-4f; + float mag_p_noise = 1e-2f; + float wind_vel_p_noise = 0.05f; - float gps_vel_noise = 0.05f; - float gps_pos_noise = 1.0f; - float baro_noise = 0.1f; - float baro_innov_gate = 5.0f; // barometric height innovation consistency gate size in standard deviations - float posNE_innov_gate = 5.0f; // GPS horizontal position innovation consistency gate size in standard deviations - float vel_innov_gate = 3.0f; // GPS velocity innovation consistency gate size in standard deviations + float gps_vel_noise = 0.05f; + float gps_pos_noise = 1.0f; + float baro_noise = 0.1f; + float baro_innov_gate = 5.0f; // barometric height innovation consistency gate size in standard deviations + float posNE_innov_gate = 5.0f; // GPS horizontal position innovation consistency gate size in standard deviations + float vel_innov_gate = 3.0f; // GPS velocity innovation consistency gate size in standard deviations - float mag_heading_noise = 3e-2f; // measurement noise used for simple heading fusion - 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 + float mag_heading_noise = 3e-2f; // measurement noise used for simple heading fusion + 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 - // these parameters control the strictness of GPS quality checks used to determine uf the GPS is - // good enough to set a local origin and commence aiding - int gps_check_mask = 21; // bitmask used to control which GPS quality checks are used - float req_hacc = 5.0f; // maximum acceptable horizontal position error - float req_vacc = 8.0f; // maximum acceptable vertical position error - float req_sacc = 1.0f; // maximum acceptable speed error - int req_nsats = 6; // minimum acceptable satellite count - float req_gdop = 2.0f; // maximum acceptable geometric dilution of precision - float req_hdrift = 0.3f; // maximum acceptable horizontal drift speed - float req_vdrift = 0.5f; // maximum acceptable vertical drift speed - }; + // these parameters control the strictness of GPS quality checks used to determine uf the GPS is + // good enough to set a local origin and commence aiding + int gps_check_mask = 21; // bitmask used to control which GPS quality checks are used + float req_hacc = 5.0f; // maximum acceptable horizontal position error + float req_vacc = 8.0f; // maximum acceptable vertical position error + float req_sacc = 1.0f; // maximum acceptable speed error + int req_nsats = 6; // minimum acceptable satellite count + float req_gdop = 2.0f; // maximum acceptable geometric dilution of precision + float req_hdrift = 0.3f; // maximum acceptable horizontal drift speed + float req_vdrift = 0.5f; // maximum acceptable vertical drift speed +}; - struct stateSample { - Vector3f ang_error; - Vector3f vel; - Vector3f pos; - Vector3f gyro_bias; - Vector3f gyro_scale; - float accel_z_bias; - Vector3f mag_I; - Vector3f mag_B; - Vector2f wind_vel; - Quaternion quat_nominal; - }; +struct stateSample { + Vector3f ang_error; + Vector3f vel; + Vector3f pos; + Vector3f gyro_bias; + Vector3f gyro_scale; + float accel_z_bias; + Vector3f mag_I; + Vector3f mag_B; + Vector2f wind_vel; + Quaternion quat_nominal; +}; - struct fault_status_t { - bool bad_mag_x: 1; - bool bad_mag_y: 1; - bool bad_mag_z: 1; - bool bad_airspeed: 1; - bool bad_sideslip: 1; - }; +struct fault_status_t { + bool bad_mag_x: 1; + bool bad_mag_y: 1; + bool bad_mag_z: 1; + bool bad_airspeed: 1; + bool bad_sideslip: 1; +}; - // publish the status of various GPS quality checks - union gps_check_fail_status_u { - struct { - uint16_t fix : 1; // 0 - true if the fix type is insufficient (no 3D solution) - uint16_t nsats : 1; // 1 - true if number of satellites used is insufficient - uint16_t gdop : 1; // 2 - true if geometric dilution of precision is insufficient - uint16_t hacc : 1; // 3 - true if reported horizontal accuracy is insufficient - uint16_t vacc : 1; // 4 - true if reported vertical accuracy is insufficient - uint16_t sacc : 1; // 5 - true if reported speed accuracy is insufficient - uint16_t hdrift : 1; // 6 - true if horizontal drift is excessive (can only be used when stationary on ground) - uint16_t vdrift : 1; // 7 - true if vertical drift is excessive (can only be used when stationary on ground) - uint16_t hspeed : 1; // 8 - true if horizontal speed is excessive (can only be used when stationary on ground) - uint16_t vspeed : 1; // 9 - true if vertical speed error is excessive - } flags; - uint16_t value; - }; +// publish the status of various GPS quality checks +union gps_check_fail_status_u { + struct { + uint16_t fix : 1; // 0 - true if the fix type is insufficient (no 3D solution) + uint16_t nsats : 1; // 1 - true if number of satellites used is insufficient + uint16_t gdop : 1; // 2 - true if geometric dilution of precision is insufficient + uint16_t hacc : 1; // 3 - true if reported horizontal accuracy is insufficient + uint16_t vacc : 1; // 4 - true if reported vertical accuracy is insufficient + uint16_t sacc : 1; // 5 - true if reported speed accuracy is insufficient + uint16_t hdrift : 1; // 6 - true if horizontal drift is excessive (can only be used when stationary on ground) + uint16_t vdrift : 1; // 7 - true if vertical drift is excessive (can only be used when stationary on ground) + uint16_t hspeed : 1; // 8 - true if horizontal speed is excessive (can only be used when stationary on ground) + uint16_t vspeed : 1; // 9 - true if vertical speed error is excessive + } flags; + uint16_t value; +}; - // bitmask containing filter control status - union filter_control_status_u { - struct { - uint8_t angle_align : 1; // 0 - true if the filter angular alignment is complete - uint8_t gps : 1; // 1 - true if GPS measurements are being fused - uint8_t opt_flow : 1; // 2 - true if optical flow measurements are being fused - uint8_t mag_hdg : 1; // 3 - true if a simple magnetic heading is being fused - uint8_t mag_3D : 1; // 4 - true if 3-axis magnetometer measurement are being fused - uint8_t mag_dec : 1; // 5 - true if synthetic magnetic declination measurements are being fused - uint8_t in_air : 1; // 6 - true when the vehicle is airborne - uint8_t armed : 1; // 7 - true when the vehicle motors are armed - } flags; - uint16_t value; - }; +// bitmask containing filter control status +union filter_control_status_u { + struct { + uint8_t angle_align : 1; // 0 - true if the filter angular alignment is complete + uint8_t gps : 1; // 1 - true if GPS measurements are being fused + uint8_t opt_flow : 1; // 2 - true if optical flow measurements are being fused + uint8_t mag_hdg : 1; // 3 - true if a simple magnetic heading is being fused + uint8_t mag_3D : 1; // 4 - true if 3-axis magnetometer measurement are being fused + uint8_t mag_dec : 1; // 5 - true if synthetic magnetic declination measurements are being fused + uint8_t in_air : 1; // 6 - true when the vehicle is airborne + uint8_t armed : 1; // 7 - true when the vehicle motors are armed + } flags; + uint16_t value; +}; } diff --git a/EKF/control.cpp b/EKF/control.cpp index 1a1a23dca9..64ca3820e1 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -43,87 +43,91 @@ void Ekf::controlFusionModes() { - // Determine the vehicle status - calculateVehicleStatus(); + // Determine the vehicle status + calculateVehicleStatus(); - // optical flow fusion mode selection logic - _control_status.flags.opt_flow = false; + // optical flow fusion mode selection logic + _control_status.flags.opt_flow = false; - // GPS fusion mode selection logic - // To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data - if (!_control_status.flags.gps) { - if (_control_status.flags.angle_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised && (_time_last_imu - _last_gps_fail_us > 5e6)) { - _control_status.flags.gps = true; - resetPosition(); - resetVelocity(); - } - } + // GPS fusion mode selection logic + // To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data + if (!_control_status.flags.gps) { + if (_control_status.flags.angle_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised + && (_time_last_imu - _last_gps_fail_us > 5e6)) { + _control_status.flags.gps = true; + resetPosition(); + resetVelocity(); + } + } - // decide when to start using optical flow data - if (!_control_status.flags.opt_flow) { - // TODO optical flow start logic - } + // decide when to start using optical flow data + if (!_control_status.flags.opt_flow) { + // TODO optical flow start logic + } - // handle the case when we are relying on GPS fusion and lose it - if (_control_status.flags.gps && !_control_status.flags.opt_flow) { - // We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something - if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) { - if (_time_last_imu - _time_last_gps > 5e5) { - // if we don't have gps then we need to switch to the non-aiding mode, zero the veloity states - // and set the synthetic GPS position to the current estimate - _control_status.flags.gps = false; - _last_known_posNE(0) = _state.pos(0); - _last_known_posNE(1) = _state.pos(1); - _state.vel.setZero(); - } else { - // Reset states to the last GPS measurement - resetPosition(); - resetVelocity(); - } - } - } + // handle the case when we are relying on GPS fusion and lose it + if (_control_status.flags.gps && !_control_status.flags.opt_flow) { + // We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something + if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) { + if (_time_last_imu - _time_last_gps > 5e5) { + // if we don't have gps then we need to switch to the non-aiding mode, zero the veloity states + // and set the synthetic GPS position to the current estimate + _control_status.flags.gps = false; + _last_known_posNE(0) = _state.pos(0); + _last_known_posNE(1) = _state.pos(1); + _state.vel.setZero(); - // handle the case when we are relying on optical flow fusion and lose it - if (_control_status.flags.opt_flow && !_control_status.flags.gps) { - // TODO - } + } else { + // Reset states to the last GPS measurement + resetPosition(); + resetVelocity(); + } + } + } - // Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances - // or the more accurate 3-axis fusion - if (!_control_status.flags.armed) { - // always use simple mag fusion for initial startup - _control_status.flags.mag_hdg = true; - _control_status.flags.mag_3D = false; - } else { - if (_control_status.flags.in_air) { - // always use 3-axis mag fusion when airborne - _control_status.flags.mag_hdg = false; - _control_status.flags.mag_3D = true; - } else { - // always use simple heading fusion when on the ground - _control_status.flags.mag_hdg = true; - _control_status.flags.mag_3D = false; - } - } + // handle the case when we are relying on optical flow fusion and lose it + if (_control_status.flags.opt_flow && !_control_status.flags.gps) { + // TODO + } + + // Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances + // or the more accurate 3-axis fusion + if (!_control_status.flags.armed) { + // always use simple mag fusion for initial startup + _control_status.flags.mag_hdg = true; + _control_status.flags.mag_3D = false; + + } else { + if (_control_status.flags.in_air) { + // always use 3-axis mag fusion when airborne + _control_status.flags.mag_hdg = false; + _control_status.flags.mag_3D = true; + + } else { + // always use simple heading fusion when on the ground + _control_status.flags.mag_hdg = true; + _control_status.flags.mag_3D = false; + } + } } void Ekf::calculateVehicleStatus() { - // determine if the vehicle is armed - _control_status.flags.armed = _vehicle_armed; + // determine if the vehicle is armed + _control_status.flags.armed = _vehicle_armed; - // record vertical position whilst disarmed to use as a height change reference - if (!_control_status.flags.armed) { - _last_disarmed_posD = _state.pos(2); - } + // record vertical position whilst disarmed to use as a height change reference + if (!_control_status.flags.armed) { + _last_disarmed_posD = _state.pos(2); + } - // Transition to in-air occurs when armed and when altitude has increased sufficiently from the altitude at arming - if (!_control_status.flags.in_air && _control_status.flags.armed && (_state.pos(2) - _last_disarmed_posD) < -1.0f) { - _control_status.flags.in_air = true; - } + // Transition to in-air occurs when armed and when altitude has increased sufficiently from the altitude at arming + if (!_control_status.flags.in_air && _control_status.flags.armed && (_state.pos(2) - _last_disarmed_posD) < -1.0f) { + _control_status.flags.in_air = true; + } - // Transition to on-ground occurs when disarmed. - if (_control_status.flags.in_air && !_control_status.flags.armed) { - _control_status.flags.in_air = false; - } + // Transition to on-ground occurs when disarmed. + if (_control_status.flags.in_air && !_control_status.flags.armed) { + _control_status.flags.in_air = false; + } } diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 4de91b39fa..01b4c2f225 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -42,7 +42,7 @@ #include "ekf.h" #include - #include +#include #define sq(_arg) powf(_arg, 2.0f) @@ -140,20 +140,25 @@ void Ekf::predictCovariance() for (unsigned i = 0; i < 9; i++) { process_noise[i] = 0.0f; } + for (unsigned i = 9; i < 12; i++) { process_noise[i] = sq(d_ang_bias_sig); } + for (unsigned i = 12; i < 15; i++) { process_noise[i] = sq(d_ang_scale_sig); } + process_noise[15] = sq(d_vel_bias_sig); for (unsigned i = 16; i < 19; i++) { process_noise[i] = sq(mag_I_sig); } + for (unsigned i = 19; i < 22; i++) { process_noise[i] = sq(mag_B_sig); } + for (unsigned i = 22; i < 24; i++) { process_noise[i] = sq(wind_vel_sig); } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 5ee2e2fead..8a7a63b581 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -43,31 +43,31 @@ #include Ekf::Ekf(): - _control_status{}, - _filter_initialised(false), + _control_status{}, + _filter_initialised(false), _earth_rate_initialised(false), _fuse_height(false), _fuse_pos(false), _fuse_hor_vel(false), _fuse_vert_vel(false), - _time_last_fake_gps(0), - _time_last_pos_fuse(0), - _time_last_vel_fuse(0), - _time_last_hgt_fuse(0), - _time_last_of_fuse(0), - _vel_pos_innov{}, + _time_last_fake_gps(0), + _time_last_pos_fuse(0), + _time_last_vel_fuse(0), + _time_last_hgt_fuse(0), + _time_last_of_fuse(0), + _vel_pos_innov{}, _mag_innov{}, _heading_innov{}, _vel_pos_innov_var{}, _mag_innov_var{}, - _heading_innov_var{} + _heading_innov_var{} { _earth_rate_NED.setZero(); _R_prev = matrix::Dcm(); _delta_angle_corr.setZero(); _delta_vel_corr.setZero(); _vel_corr.setZero(); - _last_known_posNE.setZero(); + _last_known_posNE.setZero(); } @@ -127,6 +127,7 @@ bool Ekf::update() return false; } } + //printStates(); //printStatesFast(); // prediction @@ -136,41 +137,44 @@ bool Ekf::update() predictCovariance(); } - // control logic - controlFusionModes(); + // control logic + controlFusionModes(); - // measurement updates + // measurement updates - // Fuse magnetometer data using the selected fuson method and only if angular alignment is complete + // Fuse magnetometer data using the selected fuson method and only if angular alignment is complete if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { - if (_control_status.flags.mag_3D && _control_status.flags.angle_align) { - fuseMag(); - if (_control_status.flags.mag_dec) { - // TODO need to fuse synthetic declination measurements if there is no GPS or equivalent aiding - // otherwise heading will slowly drift - } - } else if (_control_status.flags.mag_hdg && _control_status.flags.angle_align) { - fuseHeading(); - } + if (_control_status.flags.mag_3D && _control_status.flags.angle_align) { + fuseMag(); + + if (_control_status.flags.mag_dec) { + // TODO need to fuse synthetic declination measurements if there is no GPS or equivalent aiding + // otherwise heading will slowly drift + } + } else if (_control_status.flags.mag_hdg && _control_status.flags.angle_align) { + fuseHeading(); + } } if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { _fuse_height = true; } - // If we are using GPs aiding and data has fallen behind the fusion time horizon then fuse it - // if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift - // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz - if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed) && _control_status.flags.gps) { + // If we are using GPs aiding and data has fallen behind the fusion time horizon then fuse it + // if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift + // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz + if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed) && _control_status.flags.gps) { _fuse_pos = true; _fuse_vert_vel = true; _fuse_hor_vel = true; - } else if (!_control_status.flags.gps && !_control_status.flags.opt_flow && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { - _fuse_pos = true; - _gps_sample_delayed.pos(0) = _last_known_posNE(0); - _gps_sample_delayed.pos(1) = _last_known_posNE(1); - _time_last_fake_gps = _time_last_imu; - } + + } else if (!_control_status.flags.gps && !_control_status.flags.opt_flow + && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { + _fuse_pos = true; + _gps_sample_delayed.pos(0) = _last_known_posNE(0); + _gps_sample_delayed.pos(1) = _last_known_posNE(1); + _time_last_fake_gps = _time_last_imu; + } if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) { fuseVelPosHeight(); @@ -236,8 +240,8 @@ bool Ekf::initialiseFilter(void) _state.quat_nominal = Quaternion(euler_init); _output_new.quat_nominal = _state.quat_nominal; - // TODO replace this with a conditional test based on fitered angle error states. - _control_status.flags.angle_align = true; + // TODO replace this with a conditional test based on fitered angle error states. + _control_status.flags.angle_align = true; // calculate initial earth magnetic field states matrix::Dcm R_to_earth(euler_init); @@ -264,8 +268,8 @@ bool Ekf::initialiseFilter(void) void Ekf::predictState() { if (!_earth_rate_initialised) { - if (_NED_origin_initialised) { - calcEarthRateNED(_earth_rate_NED, _pos_ref.lat_rad); + if (_NED_origin_initialised) { + calcEarthRateNED(_earth_rate_NED, _pos_ref.lat_rad); _earth_rate_initialised = true; } } @@ -341,8 +345,9 @@ bool Ekf::collect_imu(imuSample &imu) _imu_down_sampled.delta_vel_dt = 0.0f; _q_down_sampled(0) = 1.0f; _q_down_sampled(1) = _q_down_sampled(2) = _q_down_sampled(3) = 0.0f; - return true; + return true; } + return false; } diff --git a/EKF/ekf.h b/EKF/ekf.h index b3ec9c579d..3529f8d13b 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -51,7 +51,7 @@ public: Ekf(); ~Ekf(); - bool init(uint64_t timestamp); + bool init(uint64_t timestamp); bool update(); // gets the innovations of velocity and position measurements @@ -80,21 +80,21 @@ public: // get the diagonal elements of the covariance matrix void get_covariances(float *covariances); - // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined - bool collect_gps(uint64_t time_usec, struct gps_message *gps); - bool collect_imu(imuSample &imu); + // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined + bool collect_gps(uint64_t time_usec, struct gps_message *gps); + bool collect_imu(imuSample &imu); - filter_control_status_u _control_status={}; + filter_control_status_u _control_status = {}; - // get the ekf WGS-84 origin positoin 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); + // get the ekf WGS-84 origin positoin 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); private: static const uint8_t _k_num_states = 24; static constexpr float _k_earth_rate = 0.000072921f; - stateSample _state; + stateSample _state; bool _filter_initialised; bool _earth_rate_initialised; @@ -104,14 +104,14 @@ private: bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused bool _fuse_vert_vel; // gps vertical velocity measurement should be fused - uint64_t _time_last_fake_gps; + uint64_t _time_last_fake_gps; - uint64_t _time_last_pos_fuse; // time the last fusion of horizotal position measurements was performed (usec) - uint64_t _time_last_vel_fuse; // time the last fusion of velocity measurements was performed (usec) - uint64_t _time_last_hgt_fuse; // time the last fusion of height measurements was performed (usec) - uint64_t _time_last_of_fuse; // time the last fusion of optical flow measurements were performed (usec) - Vector2f _last_known_posNE; // last known local NE position vector (m) - float _last_disarmed_posD; // vertical position recorded at arming (m) + uint64_t _time_last_pos_fuse; // time the last fusion of horizotal position measurements was performed (usec) + uint64_t _time_last_vel_fuse; // time the last fusion of velocity measurements was performed (usec) + uint64_t _time_last_hgt_fuse; // time the last fusion of height measurements was performed (usec) + uint64_t _time_last_of_fuse; // time the last fusion of optical flow measurements were performed (usec) + Vector2f _last_known_posNE; // last known local NE position vector (m) + float _last_disarmed_posD; // vertical position recorded at arming (m) Vector3f _earth_rate_NED; @@ -131,26 +131,26 @@ private: Vector3f _delta_angle_corr; Vector3f _delta_vel_corr; Vector3f _vel_corr; - imuSample _imu_down_sampled; - Quaternion _q_down_sampled; + imuSample _imu_down_sampled; + Quaternion _q_down_sampled; - // variables used for the GPS quality checks - float _gpsDriftVelN = 0.0f; // GPS north position derivative (m/s) - float _gpsDriftVelE = 0.0f; // GPS east position derivative (m/s) - float _gps_drift_velD = 0.0f; // GPS down position derivative (m/s) - float _gps_velD_diff_filt = 0.0f; // GPS filtered Down velocity (m/s) - float _gps_velN_filt = 0.0f; // GPS filtered North velocity (m/s) - float _gps_velE_filt = 0.0f; // GPS filtered East velocity (m/s) - uint64_t _last_gps_fail_us = 0; // last system time in usec that the GPS failed it's checks + // variables used for the GPS quality checks + float _gpsDriftVelN = 0.0f; // GPS north position derivative (m/s) + float _gpsDriftVelE = 0.0f; // GPS east position derivative (m/s) + float _gps_drift_velD = 0.0f; // GPS down position derivative (m/s) + float _gps_velD_diff_filt = 0.0f; // GPS filtered Down velocity (m/s) + float _gps_velN_filt = 0.0f; // GPS filtered North velocity (m/s) + float _gps_velE_filt = 0.0f; // GPS filtered East velocity (m/s) + uint64_t _last_gps_fail_us = 0; // last system time in usec that the GPS failed it's checks - // Variables used to publish the WGS-84 location of the EKF local NED origin - uint64_t _last_gps_origin_time_us = 0; // time the origin was last set (uSec) - float _gps_alt_ref = 0.0f; // WGS-84 height (m) + // Variables used to publish the WGS-84 location of the EKF local NED origin + uint64_t _last_gps_origin_time_us = 0; // time the origin was last set (uSec) + float _gps_alt_ref = 0.0f; // WGS-84 height (m) - gps_check_fail_status_u _gps_check_fail_status; + gps_check_fail_status_u _gps_check_fail_status; - void calculateOutputStates(); + void calculateOutputStates(); bool initialiseFilter(void); @@ -160,7 +160,7 @@ private: void predictCovariance(); - void fuseMag(); + void fuseMag(); void fuseHeading(); @@ -194,12 +194,12 @@ private: void calcEarthRateNED(Vector3f &omega, double lat_rad) const; - // return true id the GPS quality is good enough to set an origin and start aiding - bool gps_is_good(struct gps_message *gps); + // return true id the GPS quality is good enough to set an origin and start aiding + bool gps_is_good(struct gps_message *gps); - // Control the filter fusion modes - void controlFusionModes(); + // Control the filter fusion modes + void controlFusionModes(); - // Determine if we are airborne or motors are armed - void calculateVehicleStatus(); + // Determine if we are airborne or motors are armed + void calculateVehicleStatus(); }; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 7f0c6251f1..7ab35abeb3 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -158,7 +158,7 @@ void Ekf::constrainStates() _state.mag_B(i) = math::constrain(_state.mag_B(i), -0.5f, 0.5f); } - for (int i = 0; i < 2; i++) { + for (int i = 0; i < 2; i++) { _state.wind_vel(i) = math::constrain(_state.wind_vel(i), -100.0f, 100.0f); } } @@ -258,7 +258,7 @@ void Ekf::get_covariances(float *covariances) // get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set void Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) { - memcpy(origin_time, &_last_gps_origin_time_us, sizeof(uint64_t)); - memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s)); - memcpy(origin_alt, &_gps_alt_ref, sizeof(float)); + memcpy(origin_time, &_last_gps_origin_time_us, sizeof(uint64_t)); + memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s)); + memcpy(origin_alt, &_gps_alt_ref, sizeof(float)); } diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index d640db3711..1c891cf337 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -57,7 +57,8 @@ EstimatorInterface::~EstimatorInterface() } // Accumulate imu data and store to buffer at desired rate -void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel) +void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, + float *delta_vel) { if (!_initialised) { init(time_usec); @@ -119,12 +120,12 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float *data) void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) { - if(!collect_gps(time_usec, gps) || !_initialised) { + if (!collect_gps(time_usec, gps) || !_initialised) { return; } - // Only use GPS data if we have a 3D fix and limit the GPS data rate to a maximum of 14Hz - if (time_usec - _time_last_gps > 70000 && gps->fix_type >= 3) { + // Only use GPS data if we have a 3D fix and limit the GPS data rate to a maximum of 14Hz + if (time_usec - _time_last_gps > 70000 && gps->fix_type >= 3) { gpsSample gps_sample_new = {}; gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000; @@ -139,7 +140,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) float lpos_x = 0.0f; float lpos_y = 0.0f; - map_projection_project(&_pos_ref, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y); + map_projection_project(&_pos_ref, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y); gps_sample_new.pos(0) = lpos_x; gps_sample_new.pos(1) = lpos_y; gps_sample_new.hgt = gps->alt / 1e3f; @@ -150,9 +151,10 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) void EstimatorInterface::setBaroData(uint64_t time_usec, float *data) { - if(!collect_baro(time_usec, data) || !_initialised) { + if (!collect_baro(time_usec, data) || !_initialised) { return; } + if (time_usec - _time_last_baro > 70000) { baroSample baro_sample_new; @@ -170,9 +172,10 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float *data) void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *data) { - if(!collect_airspeed(time_usec, data) || !_initialised) { + if (!collect_airspeed(time_usec, data) || !_initialised) { return; } + if (time_usec > _time_last_airspeed) { airspeedSample airspeed_sample_new; airspeed_sample_new.airspeed = *data; @@ -188,7 +191,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *data) // set range data void EstimatorInterface::setRangeData(uint64_t time_usec, float *data) { - if(!collect_range(time_usec, data) || !_initialised) { + if (!collect_range(time_usec, data) || !_initialised) { return; } } @@ -196,7 +199,7 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float *data) // set optical flow data void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, float *data) { - if(!collect_opticalflow(time_usec, data) || !_initialised) { + if (!collect_opticalflow(time_usec, data) || !_initialised) { return; } } @@ -204,14 +207,14 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, float *data) bool EstimatorInterface::initialise_interface(uint64_t timestamp) { - if(!(_imu_buffer.allocate(IMU_BUFFER_LENGTH) && - _gps_buffer.allocate(OBS_BUFFER_LENGTH) && - _mag_buffer.allocate(OBS_BUFFER_LENGTH) && - _baro_buffer.allocate(OBS_BUFFER_LENGTH) && - _range_buffer.allocate(OBS_BUFFER_LENGTH) && - _airspeed_buffer.allocate(OBS_BUFFER_LENGTH) && - _flow_buffer.allocate(OBS_BUFFER_LENGTH) && - _output_buffer.allocate(IMU_BUFFER_LENGTH))) { + if (!(_imu_buffer.allocate(IMU_BUFFER_LENGTH) && + _gps_buffer.allocate(OBS_BUFFER_LENGTH) && + _mag_buffer.allocate(OBS_BUFFER_LENGTH) && + _baro_buffer.allocate(OBS_BUFFER_LENGTH) && + _range_buffer.allocate(OBS_BUFFER_LENGTH) && + _airspeed_buffer.allocate(OBS_BUFFER_LENGTH) && + _flow_buffer.allocate(OBS_BUFFER_LENGTH) && + _output_buffer.allocate(IMU_BUFFER_LENGTH))) { PX4_WARN("Estimator Buffer Allocation failed!"); unallocate_buffers(); return false; @@ -259,7 +262,7 @@ bool EstimatorInterface::position_is_valid() { // return true if the position estimate is valid // TOTO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status - return _NED_origin_initialised && (_time_last_imu - _time_last_gps) < 5e6; + return _NED_origin_initialised && (_time_last_imu - _time_last_gps) < 5e6; } void EstimatorInterface::printStoredIMU() diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index a13bf8e8bf..dd6252175b 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -81,24 +81,24 @@ public: virtual void get_covariances(float *covariances) = 0; - // get the ekf WGS-84 origin positoin and height and the system time it was last set - virtual void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0; + // get the ekf WGS-84 origin positoin and height and the system time it was last set + virtual void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0; - // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined - virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; } + // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined + virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; } - virtual bool collect_imu(imuSample &imu) { return true; } + virtual bool collect_imu(imuSample &imu) { return true; } - virtual bool collect_mag(uint64_t time_usec, float *data) { return true; } + virtual bool collect_mag(uint64_t time_usec, float *data) { return true; } - virtual bool collect_baro(uint64_t time_usec, float *data) { return true; } + virtual bool collect_baro(uint64_t time_usec, float *data) { return true; } - virtual bool collect_airspeed(uint64_t time_usec, float *data) { return true; } + virtual bool collect_airspeed(uint64_t time_usec, float *data) { return true; } - virtual bool collect_range(uint64_t time_usec, float *data) { return true; } + virtual bool collect_range(uint64_t time_usec, float *data) { return true; } - virtual bool collect_opticalflow(uint64_t time_usec, float *data) { return true; } + virtual bool collect_opticalflow(uint64_t time_usec, float *data) { return true; } // set delta angle imu data void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel); @@ -125,8 +125,8 @@ public: // in order to give access to the application parameters *getParamHandle() {return &_params;} - // set vehicle arm status data - void set_arm_status(bool data){ _vehicle_armed = data; } + // set vehicle arm status data + void set_arm_status(bool data) { _vehicle_armed = data; } void printIMU(struct imuSample *data); void printStoredIMU(); @@ -189,21 +189,21 @@ protected: outputSample _output_new; imuSample _imu_sample_new; - uint64_t _imu_ticks; + uint64_t _imu_ticks; bool _imu_updated = false; bool _initialised = false; - bool _vehicle_armed = false; // vehicle arm status used to turn off functionality used on the ground + bool _vehicle_armed = false; // vehicle arm status used to turn off functionality used on the ground - bool _NED_origin_initialised = false; - bool _gps_speed_valid = false; - struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians) + bool _NED_origin_initialised = false; + bool _gps_speed_valid = false; + struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians) bool _mag_healthy = false; // computed by mag innovation test - float _yaw_test_ratio; // yaw innovation consistency check ratio - float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios + float _yaw_test_ratio; // yaw innovation consistency check ratio + float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios - float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios + float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios RingBuffer _imu_buffer; RingBuffer _gps_buffer; diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index d2d6f9f842..924495f4a7 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -54,28 +54,30 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) { - // run gps checks if we have not yet set the NED origin or have not started using GPS - if(!_NED_origin_initialised || !_control_status.flags.gps) { - // if we have good GPS data and the NED origin is not set, set to the last GPS fix - if (gps_is_good(gps) && !_NED_origin_initialised) { - printf("gps is good - setting EKF origin\n"); - // Initialise projection - double lat = gps->lat / 1.0e7; - double lon = gps->lon / 1.0e7; - map_projection_init(&_pos_ref, lat, lon); - _gps_alt_ref = gps->alt / 1e3f; - _NED_origin_initialised = true; - _last_gps_origin_time_us = _time_last_imu; - } - } + // run gps checks if we have not yet set the NED origin or have not started using GPS + if (!_NED_origin_initialised || !_control_status.flags.gps) { + // if we have good GPS data and the NED origin is not set, set to the last GPS fix + if (gps_is_good(gps) && !_NED_origin_initialised) { + printf("gps is good - setting EKF origin\n"); + // Initialise projection + double lat = gps->lat / 1.0e7; + double lon = gps->lon / 1.0e7; + map_projection_init(&_pos_ref, lat, lon); + _gps_alt_ref = gps->alt / 1e3f; + _NED_origin_initialised = true; + _last_gps_origin_time_us = _time_last_imu; + } + } - // start collecting GPS if there is a 3D fix and the NED origin has been set - if (_NED_origin_initialised && gps->fix_type >= 3) { - return true; - } else { - return false; - } - return false; + // start collecting GPS if there is a 3D fix and the NED origin has been set + if (_NED_origin_initialised && gps->fix_type >= 3) { + return true; + + } else { + return false; + } + + return false; } /* @@ -87,126 +89,132 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) */ bool Ekf::gps_is_good(struct gps_message *gps) { - // Check the fix type - _gps_check_fail_status.flags.fix = (gps->fix_type < 3); + // Check the fix type + _gps_check_fail_status.flags.fix = (gps->fix_type < 3); - // Check the number of satellites - _gps_check_fail_status.flags.nsats = (gps->nsats < _params.req_nsats); + // Check the number of satellites + _gps_check_fail_status.flags.nsats = (gps->nsats < _params.req_nsats); - // Check the geometric dilution of precision - _gps_check_fail_status.flags.gdop = (gps->gdop > _params.req_gdop); + // Check the geometric dilution of precision + _gps_check_fail_status.flags.gdop = (gps->gdop > _params.req_gdop); - // Check the reported horizontal position accuracy - _gps_check_fail_status.flags.hacc = (gps->eph > _params.req_hacc); + // Check the reported horizontal position accuracy + _gps_check_fail_status.flags.hacc = (gps->eph > _params.req_hacc); - // Check the reported vertical position accuracy - _gps_check_fail_status.flags.vacc = (gps->epv > _params.req_vacc); + // Check the reported vertical position accuracy + _gps_check_fail_status.flags.vacc = (gps->epv > _params.req_vacc); - // Check the reported speed accuracy - _gps_check_fail_status.flags.sacc = (gps->sacc > _params.req_sacc); + // Check the reported speed accuracy + _gps_check_fail_status.flags.sacc = (gps->sacc > _params.req_sacc); - // Calculate position movement since last measurement - float delta_posN = 0.0f; - float delta_PosE = 0.0f; - double lat = gps->lat * 1.0e-7; - double lon = gps->lon * 1.0e-7; - if (_pos_ref.init_done) { - map_projection_project(&_pos_ref, lat, lon, &delta_posN, &delta_PosE); - } else { - map_projection_init(&_pos_ref, lat, lon); - _gps_alt_ref = gps->alt * 1e-3f; - } + // Calculate position movement since last measurement + float delta_posN = 0.0f; + float delta_PosE = 0.0f; + double lat = gps->lat * 1.0e-7; + double lon = gps->lon * 1.0e-7; - // Calculate time lapsed since last update, limit to prevent numerical errors and calculate the lowpass filter coefficient - const float filt_time_const = 10.0f; - float dt = fminf(fmaxf(float(_time_last_imu - _last_gps_origin_time_us)*1e-6f,0.001f),filt_time_const); - float filter_coef = dt/filt_time_const; + if (_pos_ref.init_done) { + map_projection_project(&_pos_ref, lat, lon, &delta_posN, &delta_PosE); - // Calculate the horizontal drift velocity components and limit to 10x the threshold - float vel_limit = 10.0f * _params.req_hdrift; - float velN = fminf(fmaxf(delta_posN / dt, -vel_limit), vel_limit); - float velE = fminf(fmaxf(delta_PosE / dt, -vel_limit), vel_limit); + } else { + map_projection_init(&_pos_ref, lat, lon); + _gps_alt_ref = gps->alt * 1e-3f; + } - // Apply a low pass filter - _gpsDriftVelN = velN * filter_coef + _gpsDriftVelN * (1.0f - filter_coef); - _gpsDriftVelE = velE * filter_coef + _gpsDriftVelE * (1.0f - filter_coef); + // Calculate time lapsed since last update, limit to prevent numerical errors and calculate the lowpass filter coefficient + const float filt_time_const = 10.0f; + float dt = fminf(fmaxf(float(_time_last_imu - _last_gps_origin_time_us) * 1e-6f, 0.001f), filt_time_const); + float filter_coef = dt / filt_time_const; - // Calculate the horizontal drift speed and fail if too high - // This check can only be used if the vehicle is stationary during alignment - if(!_control_status.flags.armed) { - float drift_speed = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE); - _gps_check_fail_status.flags.hdrift = (drift_speed > _params.req_hdrift); - } else { - _gps_check_fail_status.flags.hdrift = false; - } + // Calculate the horizontal drift velocity components and limit to 10x the threshold + float vel_limit = 10.0f * _params.req_hdrift; + float velN = fminf(fmaxf(delta_posN / dt, -vel_limit), vel_limit); + float velE = fminf(fmaxf(delta_PosE / dt, -vel_limit), vel_limit); - // Save current position as the reference for next time - map_projection_init(&_pos_ref, lat, lon); - _last_gps_origin_time_us = _time_last_imu; + // Apply a low pass filter + _gpsDriftVelN = velN * filter_coef + _gpsDriftVelN * (1.0f - filter_coef); + _gpsDriftVelE = velE * filter_coef + _gpsDriftVelE * (1.0f - filter_coef); - // Calculate the vertical drift velocity and limit to 10x the threshold - vel_limit = 10.0f * _params.req_vdrift; - float velD = fminf(fmaxf((_gps_alt_ref - gps->alt * 1e-3f) / dt, -vel_limit), vel_limit); + // Calculate the horizontal drift speed and fail if too high + // This check can only be used if the vehicle is stationary during alignment + if (!_control_status.flags.armed) { + float drift_speed = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE); + _gps_check_fail_status.flags.hdrift = (drift_speed > _params.req_hdrift); - // Save the current height as the reference for next time - _gps_alt_ref = gps->alt * 1e-3f; + } else { + _gps_check_fail_status.flags.hdrift = false; + } - // Apply a low pass filter to the vertical velocity - _gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef); + // Save current position as the reference for next time + map_projection_init(&_pos_ref, lat, lon); + _last_gps_origin_time_us = _time_last_imu; - // Fail if the vertical drift speed is too high - // This check can only be used if the vehicle is stationary during alignment - if(!_control_status.flags.armed) { - _gps_check_fail_status.flags.vdrift = (fabsf(_gps_drift_velD) > _params.req_vdrift); - } else { - _gps_check_fail_status.flags.vdrift = false; - } + // Calculate the vertical drift velocity and limit to 10x the threshold + vel_limit = 10.0f * _params.req_vdrift; + float velD = fminf(fmaxf((_gps_alt_ref - gps->alt * 1e-3f) / dt, -vel_limit), vel_limit); - // Check the magnitude of the filtered horizontal GPS velocity - // This check can only be used if the vehicle is stationary during alignment - if (!_control_status.flags.armed) { - vel_limit = 10.0f * _params.req_hdrift; - float velN = fminf(fmaxf(gps->vel_ned[0],-vel_limit),vel_limit); - float velE = fminf(fmaxf(gps->vel_ned[1],-vel_limit),vel_limit); - _gps_velN_filt = velN * filter_coef + _gps_velN_filt * (1.0f - filter_coef); - _gps_velE_filt = velE * filter_coef + _gps_velE_filt * (1.0f - filter_coef); - float horiz_speed = sqrtf(_gps_velN_filt * _gps_velN_filt + _gps_velE_filt * _gps_velE_filt); - _gps_check_fail_status.flags.hspeed = (horiz_speed > _params.req_hdrift); - } else { - _gps_check_fail_status.flags.hspeed = false; - } + // Save the current height as the reference for next time + _gps_alt_ref = gps->alt * 1e-3f; - // Check the filtered difference between GPS and EKF vertical velocity - vel_limit = 10.0f * _params.req_vdrift; - float vertVel = fminf(fmaxf((gps->vel_ned[2] - _state.vel(2)), -vel_limit), vel_limit); - _gps_velD_diff_filt = vertVel * filter_coef + _gps_velD_diff_filt * (1.0f - filter_coef); - _gps_check_fail_status.flags.vspeed = (fabsf(_gps_velD_diff_filt) > _params.req_vdrift); + // Apply a low pass filter to the vertical velocity + _gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef); - // assume failed first time through - if (_last_gps_fail_us == 0) { - _last_gps_fail_us = _time_last_imu; - } + // Fail if the vertical drift speed is too high + // This check can only be used if the vehicle is stationary during alignment + if (!_control_status.flags.armed) { + _gps_check_fail_status.flags.vdrift = (fabsf(_gps_drift_velD) > _params.req_vdrift); - // if any user selected checks have failed, record the fail time - if ( - _gps_check_fail_status.flags.fix || - (_gps_check_fail_status.flags.nsats && (_params.gps_check_mask & MASK_GPS_NSATS)) || - (_gps_check_fail_status.flags.gdop && (_params.gps_check_mask & MASK_GPS_GDOP)) || - (_gps_check_fail_status.flags.hacc && (_params.gps_check_mask & MASK_GPS_HACC)) || - (_gps_check_fail_status.flags.vacc && (_params.gps_check_mask & MASK_GPS_VACC)) || - (_gps_check_fail_status.flags.sacc && (_params.gps_check_mask & MASK_GPS_SACC)) || - (_gps_check_fail_status.flags.hdrift && (_params.gps_check_mask & MASK_GPS_HDRIFT)) || - (_gps_check_fail_status.flags.vdrift && (_params.gps_check_mask & MASK_GPS_VDRIFT)) || - (_gps_check_fail_status.flags.hspeed && (_params.gps_check_mask & MASK_GPS_HSPD)) || - (_gps_check_fail_status.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD)) - ) { - _last_gps_fail_us = _time_last_imu; - } + } else { + _gps_check_fail_status.flags.vdrift = false; + } - // continuous period without fail of 10 seconds required to return a healthy status - if (_time_last_imu - _last_gps_fail_us > 1e7) { - return true; - } - return false; + // Check the magnitude of the filtered horizontal GPS velocity + // This check can only be used if the vehicle is stationary during alignment + if (!_control_status.flags.armed) { + vel_limit = 10.0f * _params.req_hdrift; + float velN = fminf(fmaxf(gps->vel_ned[0], -vel_limit), vel_limit); + float velE = fminf(fmaxf(gps->vel_ned[1], -vel_limit), vel_limit); + _gps_velN_filt = velN * filter_coef + _gps_velN_filt * (1.0f - filter_coef); + _gps_velE_filt = velE * filter_coef + _gps_velE_filt * (1.0f - filter_coef); + float horiz_speed = sqrtf(_gps_velN_filt * _gps_velN_filt + _gps_velE_filt * _gps_velE_filt); + _gps_check_fail_status.flags.hspeed = (horiz_speed > _params.req_hdrift); + + } else { + _gps_check_fail_status.flags.hspeed = false; + } + + // Check the filtered difference between GPS and EKF vertical velocity + vel_limit = 10.0f * _params.req_vdrift; + float vertVel = fminf(fmaxf((gps->vel_ned[2] - _state.vel(2)), -vel_limit), vel_limit); + _gps_velD_diff_filt = vertVel * filter_coef + _gps_velD_diff_filt * (1.0f - filter_coef); + _gps_check_fail_status.flags.vspeed = (fabsf(_gps_velD_diff_filt) > _params.req_vdrift); + + // assume failed first time through + if (_last_gps_fail_us == 0) { + _last_gps_fail_us = _time_last_imu; + } + + // if any user selected checks have failed, record the fail time + if ( + _gps_check_fail_status.flags.fix || + (_gps_check_fail_status.flags.nsats && (_params.gps_check_mask & MASK_GPS_NSATS)) || + (_gps_check_fail_status.flags.gdop && (_params.gps_check_mask & MASK_GPS_GDOP)) || + (_gps_check_fail_status.flags.hacc && (_params.gps_check_mask & MASK_GPS_HACC)) || + (_gps_check_fail_status.flags.vacc && (_params.gps_check_mask & MASK_GPS_VACC)) || + (_gps_check_fail_status.flags.sacc && (_params.gps_check_mask & MASK_GPS_SACC)) || + (_gps_check_fail_status.flags.hdrift && (_params.gps_check_mask & MASK_GPS_HDRIFT)) || + (_gps_check_fail_status.flags.vdrift && (_params.gps_check_mask & MASK_GPS_VDRIFT)) || + (_gps_check_fail_status.flags.hspeed && (_params.gps_check_mask & MASK_GPS_HSPD)) || + (_gps_check_fail_status.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD)) + ) { + _last_gps_fail_us = _time_last_imu; + } + + // continuous period without fail of 10 seconds required to return a healthy status + if (_time_last_imu - _last_gps_fail_us > 1e7) { + return true; + } + + return false; } diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 1078ebe324..fd7590827f 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -44,408 +44,419 @@ void Ekf::fuseMag() { - // assign intermediate 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); + // assign intermediate 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 magN = _state.mag_I(0); - float magE = _state.mag_I(1); - float magD = _state.mag_I(2); + float magN = _state.mag_I(0); + float magE = _state.mag_I(1); + float magD = _state.mag_I(2); - // XYZ Measurement uncertainty. Need to consider timing errors for fast rotations - float R_MAG = 1e-3f; + // XYZ Measurement uncertainty. Need to consider timing errors for fast rotations + float R_MAG = 1e-3f; - // intermediate variables from algebraic optimisation - float SH_MAG[9]; - SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); - SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3); - SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3); - SH_MAG[3] = 2 * q0 * q1 + 2 * q2 * q3; - SH_MAG[4] = 2 * q0 * q3 + 2 * q1 * q2; - SH_MAG[5] = 2 * q0 * q2 + 2 * q1 * q3; - SH_MAG[6] = magE * (2 * q0 * q1 - 2 * q2 * q3); - SH_MAG[7] = 2 * q1 * q3 - 2 * q0 * q2; - SH_MAG[8] = 2 * q0 * q3; + // intermediate variables from algebraic optimisation + float SH_MAG[9]; + SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); + SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3); + SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SH_MAG[3] = 2 * q0 * q1 + 2 * q2 * q3; + SH_MAG[4] = 2 * q0 * q3 + 2 * q1 * q2; + SH_MAG[5] = 2 * q0 * q2 + 2 * q1 * q3; + SH_MAG[6] = magE * (2 * q0 * q1 - 2 * q2 * q3); + SH_MAG[7] = 2 * q1 * q3 - 2 * q0 * q2; + SH_MAG[8] = 2 * q0 * q3; - // rotate magnetometer earth field state into body frame - matrix::Dcm R_to_body(_state.quat_nominal); - R_to_body = R_to_body.transpose(); + // rotate magnetometer earth field state into body frame + matrix::Dcm R_to_body(_state.quat_nominal); + R_to_body = R_to_body.transpose(); - Vector3f mag_I_rot = R_to_body * _state.mag_I; + Vector3f mag_I_rot = R_to_body * _state.mag_I; - // 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); + // 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); - // Note that although the observation jacobians and kalman gains are decalred as arrays - // sequential fusion of the X,Y and Z components is used. - float H_MAG[3][24] = {}; - float Kfusion[24] = {}; + // Note that although the observation jacobians and kalman gains are decalred as arrays + // sequential fusion of the X,Y and Z components is used. + float H_MAG[3][24] = {}; + float Kfusion[24] = {}; - // Calculate observation Jacobians and kalman gains for each magentoemter axis - // X Axis - H_MAG[0][1] = SH_MAG[6] - magD * SH_MAG[2] - magN * SH_MAG[5]; - H_MAG[0][2] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); - H_MAG[0][16] = SH_MAG[1]; - H_MAG[0][17] = SH_MAG[4]; - H_MAG[0][18] = SH_MAG[7]; - H_MAG[0][19] = 1; + // Calculate observation Jacobians and kalman gains for each magentoemter axis + // X Axis + H_MAG[0][1] = SH_MAG[6] - magD * SH_MAG[2] - magN * SH_MAG[5]; + H_MAG[0][2] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); + H_MAG[0][16] = SH_MAG[1]; + H_MAG[0][17] = SH_MAG[4]; + H_MAG[0][18] = SH_MAG[7]; + H_MAG[0][19] = 1; - // intermediate variables - float SK_MX[4] = {}; - // innovation variance - _mag_innov_var[0] = (P[19][19] + R_MAG - P[1][19] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][19] * SH_MAG[1] - + P[17][19] * SH_MAG[4] + P[18][19] * SH_MAG[7] + P[2][19] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)) - (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) * (P[19][1] - P[1][1] * - (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][1] * SH_MAG[1] + P[17][1] * SH_MAG[4] + P[18][1] * SH_MAG[7] + - P[2][1] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[1] * - (P[19][16] - P[1][16] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][16] * SH_MAG[1] + P[17][16] * - SH_MAG[4] + P[18][16] * SH_MAG[7] + P[2][16] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[4] * (P[19][17] - P[1][17] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + - P[16][17] * SH_MAG[1] + P[17][17] * SH_MAG[4] + P[18][17] * SH_MAG[7] + P[2][17] * (magE * SH_MAG[0] + magD * SH_MAG[3] - - magN * (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[7] * (P[19][18] - P[1][18] * (magD * SH_MAG[2] - SH_MAG[6] + magN * - SH_MAG[5]) + P[16][18] * SH_MAG[1] + P[17][18] * SH_MAG[4] + P[18][18] * SH_MAG[7] + P[2][18] * - (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2))) + (magE * SH_MAG[0] + magD * SH_MAG[3] - - magN * (SH_MAG[8] - 2 * q1 * q2)) * (P[19][2] - P[1][2] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][2] * - SH_MAG[1] + P[17][2] * SH_MAG[4] + P[18][2] * SH_MAG[7] + P[2][2] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)))); + // intermediate variables + float SK_MX[4] = {}; + // innovation variance + _mag_innov_var[0] = (P[19][19] + R_MAG - P[1][19] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][19] * + SH_MAG[1] + + P[17][19] * SH_MAG[4] + P[18][19] * SH_MAG[7] + P[2][19] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)) - (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) * (P[19][1] - P[1][1] * + (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][1] * SH_MAG[1] + P[17][1] * SH_MAG[4] + P[18][1] * SH_MAG[7] + + P[2][1] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[1] * + (P[19][16] - P[1][16] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][16] * SH_MAG[1] + P[17][16] * + SH_MAG[4] + P[18][16] * SH_MAG[7] + P[2][16] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[4] * (P[19][17] - P[1][17] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + + P[16][17] * SH_MAG[1] + P[17][17] * SH_MAG[4] + P[18][17] * SH_MAG[7] + P[2][17] * (magE * SH_MAG[0] + magD * SH_MAG[3] + - magN * (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[7] * (P[19][18] - P[1][18] * (magD * SH_MAG[2] - SH_MAG[6] + magN * + SH_MAG[5]) + P[16][18] * SH_MAG[1] + P[17][18] * SH_MAG[4] + P[18][18] * SH_MAG[7] + P[2][18] * + (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2))) + (magE * SH_MAG[0] + magD * SH_MAG[3] - + magN * (SH_MAG[8] - 2 * q1 * q2)) * (P[19][2] - P[1][2] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][2] * + SH_MAG[1] + P[17][2] * SH_MAG[4] + P[18][2] * SH_MAG[7] + P[2][2] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)))); - // check for a badly conditioned covariance matrix - if (_mag_innov_var[0] >= R_MAG) { - // the innovation variance contribution from the state covariances is non-negative - no fault - _fault_status.bad_mag_x = false; - } else { - // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned - _fault_status.bad_mag_x = true; - // we need to reinitialise the covariance matrix and abort this fusion step - initialiseCovariance(); - return; - } + // check for a badly conditioned covariance matrix + if (_mag_innov_var[0] >= R_MAG) { + // the innovation variance contribution from the state covariances is non-negative - no fault + _fault_status.bad_mag_x = false; + + } else { + // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned + _fault_status.bad_mag_x = true; + // we need to reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + return; + } - // Y axis + // Y axis - H_MAG[1][0] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; - H_MAG[1][2] = - magE * SH_MAG[4] - magD * SH_MAG[7] - magN * SH_MAG[1]; - H_MAG[1][16] = 2 * q1 * q2 - SH_MAG[8]; - H_MAG[1][17] = SH_MAG[0]; - H_MAG[1][18] = SH_MAG[3]; - H_MAG[1][20] = 1; + H_MAG[1][0] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; + H_MAG[1][2] = - magE * SH_MAG[4] - magD * SH_MAG[7] - magN * SH_MAG[1]; + H_MAG[1][16] = 2 * q1 * q2 - SH_MAG[8]; + H_MAG[1][17] = SH_MAG[0]; + H_MAG[1][18] = SH_MAG[3]; + H_MAG[1][20] = 1; - // intermediate variables - note SK_MY[0] is 1/(innovation variance) - float SK_MY[4]; - _mag_innov_var[1] = (P[20][20] + R_MAG + P[0][20] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][20] * SH_MAG[0] - + P[18][20] * SH_MAG[3] - (SH_MAG[8] - 2 * q1 * q2) * (P[20][16] + P[0][16] * (magD * SH_MAG[2] - SH_MAG[6] + magN * - SH_MAG[5]) + P[17][16] * SH_MAG[0] + P[18][16] * SH_MAG[3] - P[2][16] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * - SH_MAG[1]) - P[16][16] * (SH_MAG[8] - 2 * q1 * q2)) - P[2][20] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * - SH_MAG[1]) + (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) * (P[20][0] + P[0][0] * - (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][0] * SH_MAG[0] + P[18][0] * SH_MAG[3] - P[2][0] * - (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][0] * (SH_MAG[8] - 2 * q1 * q2)) + SH_MAG[0] * - (P[20][17] + P[0][17] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][17] * SH_MAG[0] + P[18][17] * - SH_MAG[3] - P[2][17] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][17] * - (SH_MAG[8] - 2 * q1 * q2)) + SH_MAG[3] * (P[20][18] + P[0][18] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + - P[17][18] * SH_MAG[0] + P[18][18] * SH_MAG[3] - P[2][18] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - - P[16][18] * (SH_MAG[8] - 2 * q1 * q2)) - P[16][20] * (SH_MAG[8] - 2 * q1 * q2) - (magE * SH_MAG[4] + magD * SH_MAG[7] + - magN * SH_MAG[1]) * (P[20][2] + P[0][2] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][2] * SH_MAG[0] + - P[18][2] * SH_MAG[3] - P[2][2] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][2] * - (SH_MAG[8] - 2 * q1 * q2))); + // intermediate variables - note SK_MY[0] is 1/(innovation variance) + float SK_MY[4]; + _mag_innov_var[1] = (P[20][20] + R_MAG + P[0][20] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][20] * + SH_MAG[0] + + P[18][20] * SH_MAG[3] - (SH_MAG[8] - 2 * q1 * q2) * (P[20][16] + P[0][16] * (magD * SH_MAG[2] - SH_MAG[6] + magN * + SH_MAG[5]) + P[17][16] * SH_MAG[0] + P[18][16] * SH_MAG[3] - P[2][16] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * + SH_MAG[1]) - P[16][16] * (SH_MAG[8] - 2 * q1 * q2)) - P[2][20] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * + SH_MAG[1]) + (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) * (P[20][0] + P[0][0] * + (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][0] * SH_MAG[0] + P[18][0] * SH_MAG[3] - P[2][0] * + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][0] * (SH_MAG[8] - 2 * q1 * q2)) + SH_MAG[0] * + (P[20][17] + P[0][17] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][17] * SH_MAG[0] + P[18][17] * + SH_MAG[3] - P[2][17] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][17] * + (SH_MAG[8] - 2 * q1 * q2)) + SH_MAG[3] * (P[20][18] + P[0][18] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + + P[17][18] * SH_MAG[0] + P[18][18] * SH_MAG[3] - P[2][18] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - + P[16][18] * (SH_MAG[8] - 2 * q1 * q2)) - P[16][20] * (SH_MAG[8] - 2 * q1 * q2) - (magE * SH_MAG[4] + magD * SH_MAG[7] + + magN * SH_MAG[1]) * (P[20][2] + P[0][2] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][2] * SH_MAG[0] + + P[18][2] * SH_MAG[3] - P[2][2] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][2] * + (SH_MAG[8] - 2 * q1 * q2))); - // check for a badly conditioned covariance matrix - if (_mag_innov_var[1] >= R_MAG) { - // the innovation variance contribution from the state covariances is non-negative - no fault - _fault_status.bad_mag_y = false; - } else { - // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned - _fault_status.bad_mag_y = true; - // we need to reinitialise the covariance matrix and abort this fusion step - initialiseCovariance(); - return; - } + // check for a badly conditioned covariance matrix + if (_mag_innov_var[1] >= R_MAG) { + // the innovation variance contribution from the state covariances is non-negative - no fault + _fault_status.bad_mag_y = false; + + } else { + // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned + _fault_status.bad_mag_y = true; + // we need to reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + return; + } - // Z axis + // Z axis - H_MAG[2][0] = magN * (SH_MAG[8] - 2 * q1 * q2) - magD * SH_MAG[3] - magE * SH_MAG[0]; - H_MAG[2][1] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; - H_MAG[2][16] = SH_MAG[5]; - H_MAG[2][17] = 2 * q2 * q3 - 2 * q0 * q1; - H_MAG[2][18] = SH_MAG[2]; - H_MAG[2][21] = 1; + H_MAG[2][0] = magN * (SH_MAG[8] - 2 * q1 * q2) - magD * SH_MAG[3] - magE * SH_MAG[0]; + H_MAG[2][1] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; + H_MAG[2][16] = SH_MAG[5]; + H_MAG[2][17] = 2 * q2 * q3 - 2 * q0 * q1; + H_MAG[2][18] = SH_MAG[2]; + H_MAG[2][21] = 1; - // intermediate variables - float SK_MZ[4]; - _mag_innov_var[2] = (P[21][21] + R_MAG + P[16][21] * SH_MAG[5] + P[18][21] * SH_MAG[2] - (2 * q0 * q1 - 2 * q2 * q3) * - (P[21][17] + P[16][17] * SH_MAG[5] + P[18][17] * SH_MAG[2] - P[0][17] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)) + P[1][17] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][17] * - (2 * q0 * q1 - 2 * q2 * q3)) - P[0][21] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)) + P[1][21] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) + SH_MAG[5] * - (P[21][16] + P[16][16] * SH_MAG[5] + P[18][16] * SH_MAG[2] - P[0][16] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)) + P[1][16] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][16] * - (2 * q0 * q1 - 2 * q2 * q3)) + SH_MAG[2] * (P[21][18] + P[16][18] * SH_MAG[5] + P[18][18] * SH_MAG[2] - P[0][18] * - (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) + P[1][18] * - (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][18] * (2 * q0 * q1 - 2 * q2 * q3)) - - (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) * (P[21][0] + P[16][0] * SH_MAG[5] + P[18][0] * - SH_MAG[2] - P[0][0] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) + P[1][0] * - (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][0] * (2 * q0 * q1 - 2 * q2 * q3)) - P[17][21] * - (2 * q0 * q1 - 2 * q2 * q3) + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) * - (P[21][1] + P[16][1] * SH_MAG[5] + P[18][1] * SH_MAG[2] - P[0][1] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)) + P[1][1] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][1] * - (2 * q0 * q1 - 2 * q2 * q3))); + // intermediate variables + float SK_MZ[4]; + _mag_innov_var[2] = (P[21][21] + R_MAG + P[16][21] * SH_MAG[5] + P[18][21] * SH_MAG[2] - (2 * q0 * q1 - 2 * q2 * q3) * + (P[21][17] + P[16][17] * SH_MAG[5] + P[18][17] * SH_MAG[2] - P[0][17] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)) + P[1][17] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][17] * + (2 * q0 * q1 - 2 * q2 * q3)) - P[0][21] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)) + P[1][21] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) + SH_MAG[5] * + (P[21][16] + P[16][16] * SH_MAG[5] + P[18][16] * SH_MAG[2] - P[0][16] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)) + P[1][16] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][16] * + (2 * q0 * q1 - 2 * q2 * q3)) + SH_MAG[2] * (P[21][18] + P[16][18] * SH_MAG[5] + P[18][18] * SH_MAG[2] - P[0][18] * + (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) + P[1][18] * + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][18] * (2 * q0 * q1 - 2 * q2 * q3)) - + (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) * (P[21][0] + P[16][0] * SH_MAG[5] + P[18][0] * + SH_MAG[2] - P[0][0] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) + P[1][0] * + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][0] * (2 * q0 * q1 - 2 * q2 * q3)) - P[17][21] * + (2 * q0 * q1 - 2 * q2 * q3) + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) * + (P[21][1] + P[16][1] * SH_MAG[5] + P[18][1] * SH_MAG[2] - P[0][1] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)) + P[1][1] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][1] * + (2 * q0 * q1 - 2 * q2 * q3))); - // check for a badly conditioned covariance matrix - if (_mag_innov_var[2] >= R_MAG) { - // the innovation variance contribution from the state covariances is non-negative - no fault - _fault_status.bad_mag_z = false; - } else { - // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned - _fault_status.bad_mag_z = true; - // we need to reinitialise the covariance matrix and abort this fusion step - initialiseCovariance(); - return; - } + // check for a badly conditioned covariance matrix + if (_mag_innov_var[2] >= R_MAG) { + // the innovation variance contribution from the state covariances is non-negative - no fault + _fault_status.bad_mag_z = false; + + } else { + // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned + _fault_status.bad_mag_z = true; + // we need to reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + return; + } - // Perform an innovation consistency check on each measurement and if one axis fails - // do not fuse any data from the sensor because the most common errors affect multiple axes. - _mag_healthy = true; - for (uint8_t index=0; index<=2; index++) { - _mag_test_ratio[index] = sq(_mag_innov[index]) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var[index]); - if (_mag_test_ratio[index] > 1.0f) { - _mag_healthy = false; - } - } - if (!_mag_healthy) { - return; - } + // Perform an innovation consistency check on each measurement and if one axis fails + // do not fuse any data from the sensor because the most common errors affect multiple axes. + _mag_healthy = true; - // update the states and covariance usinng sequential fusion of the magnetometer components - for (uint8_t index=0; index<=2; index++) { - // Calculate Kalman gains - if (index == 0) { - // Calculate X axis Kalman gains - SK_MX[0] = 1.0f / _mag_innov_var[0]; - SK_MX[1] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); - SK_MX[2] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; - SK_MX[3] = SH_MAG[7]; + for (uint8_t index = 0; index <= 2; index++) { + _mag_test_ratio[index] = sq(_mag_innov[index]) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var[index]); - Kfusion[0] = SK_MX[0] * (P[0][19] + P[0][16] * SH_MAG[1] + P[0][17] * SH_MAG[4] - P[0][1] * SK_MX[2] + P[0][2] * - SK_MX[1] + P[0][18] * SK_MX[3]); - Kfusion[1] = SK_MX[0] * (P[1][19] + P[1][16] * SH_MAG[1] + P[1][17] * SH_MAG[4] - P[1][1] * SK_MX[2] + P[1][2] * - SK_MX[1] + P[1][18] * SK_MX[3]); - Kfusion[2] = SK_MX[0] * (P[2][19] + P[2][16] * SH_MAG[1] + P[2][17] * SH_MAG[4] - P[2][1] * SK_MX[2] + P[2][2] * - SK_MX[1] + P[2][18] * SK_MX[3]); - Kfusion[3] = SK_MX[0] * (P[3][19] + P[3][16] * SH_MAG[1] + P[3][17] * SH_MAG[4] - P[3][1] * SK_MX[2] + P[3][2] * - SK_MX[1] + P[3][18] * SK_MX[3]); - Kfusion[4] = SK_MX[0] * (P[4][19] + P[4][16] * SH_MAG[1] + P[4][17] * SH_MAG[4] - P[4][1] * SK_MX[2] + P[4][2] * - SK_MX[1] + P[4][18] * SK_MX[3]); - Kfusion[5]= SK_MX[0] * (P[5][19] + P[5][16] * SH_MAG[1] + P[5][17] * SH_MAG[4] - P[5][1] * SK_MX[2] + P[5][2] * - SK_MX[1] + P[5][18] * SK_MX[3]); - Kfusion[6] = SK_MX[0] * (P[6][19] + P[6][16] * SH_MAG[1] + P[6][17] * SH_MAG[4] - P[6][1] * SK_MX[2] + P[6][2] * - SK_MX[1] + P[6][18] * SK_MX[3]); - Kfusion[7] = SK_MX[0] * (P[7][19] + P[7][16] * SH_MAG[1] + P[7][17] * SH_MAG[4] - P[7][1] * SK_MX[2] + P[7][2] * - SK_MX[1] + P[7][18] * SK_MX[3]); - Kfusion[8] = SK_MX[0] * (P[8][19] + P[8][16] * SH_MAG[1] + P[8][17] * SH_MAG[4] - P[8][1] * SK_MX[2] + P[8][2] * - SK_MX[1] + P[8][18] * SK_MX[3]); - Kfusion[9] = SK_MX[0] * (P[9][19] + P[9][16] * SH_MAG[1] + P[9][17] * SH_MAG[4] - P[9][1] * SK_MX[2] + P[9][2] * - SK_MX[1] + P[9][18] * SK_MX[3]); - Kfusion[10] = SK_MX[0] * (P[10][19] + P[10][16] * SH_MAG[1] + P[10][17] * SH_MAG[4] - P[10][1] * SK_MX[2] + P[10][2] * - SK_MX[1] + P[10][18] * SK_MX[3]); - Kfusion[11] = SK_MX[0] * (P[11][19] + P[11][16] * SH_MAG[1] + P[11][17] * SH_MAG[4] - P[11][1] * SK_MX[2] + P[11][2] * - SK_MX[1] + P[11][18] * SK_MX[3]); - Kfusion[12] = SK_MX[0] * (P[12][19] + P[12][16] * SH_MAG[1] + P[12][17] * SH_MAG[4] - P[12][1] * SK_MX[2] + P[12][2] * - SK_MX[1] + P[12][18] * SK_MX[3]); - Kfusion[13] = SK_MX[0] * (P[13][19] + P[13][16] * SH_MAG[1] + P[13][17] * SH_MAG[4] - P[13][1] * SK_MX[2] + P[13][2] * - SK_MX[1] + P[13][18] * SK_MX[3]); - Kfusion[14] = SK_MX[0] * (P[14][19] + P[14][16] * SH_MAG[1] + P[14][17] * SH_MAG[4] - P[14][1] * SK_MX[2] + P[14][2] * - SK_MX[1] + P[14][18] * SK_MX[3]); - Kfusion[15] = SK_MX[0] * (P[15][19] + P[15][16] * SH_MAG[1] + P[15][17] * SH_MAG[4] - P[15][1] * SK_MX[2] + P[15][2] * - SK_MX[1] + P[15][18] * SK_MX[3]); - Kfusion[16] = SK_MX[0] * (P[16][19] + P[16][16] * SH_MAG[1] + P[16][17] * SH_MAG[4] - P[16][1] * SK_MX[2] + P[16][2] * - SK_MX[1] + P[16][18] * SK_MX[3]); - Kfusion[17] = SK_MX[0] * (P[17][19] + P[17][16] * SH_MAG[1] + P[17][17] * SH_MAG[4] - P[17][1] * SK_MX[2] + P[17][2] * - SK_MX[1] + P[17][18] * SK_MX[3]); - Kfusion[18] = SK_MX[0] * (P[18][19] + P[18][16] * SH_MAG[1] + P[18][17] * SH_MAG[4] - P[18][1] * SK_MX[2] + P[18][2] * - SK_MX[1] + P[18][18] * SK_MX[3]); - Kfusion[19] = SK_MX[0] * (P[19][19] + P[19][16] * SH_MAG[1] + P[19][17] * SH_MAG[4] - P[19][1] * SK_MX[2] + P[19][2] * - SK_MX[1] + P[19][18] * SK_MX[3]); - Kfusion[20] = SK_MX[0] * (P[20][19] + P[20][16] * SH_MAG[1] + P[20][17] * SH_MAG[4] - P[20][1] * SK_MX[2] + P[20][2] * - 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]); - } else if (index == 1) { - // Calculate Y axis Kalman gains - SK_MY[0] = 1.0f / _mag_innov_var[1]; - SK_MY[1] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; - SK_MY[2] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; - SK_MY[3] = SH_MAG[8] - 2 * q1 * q2; + if (_mag_test_ratio[index] > 1.0f) { + _mag_healthy = false; + } + } - Kfusion[0] = SK_MY[0] * (P[0][20] + P[0][17] * SH_MAG[0] + P[0][18] * SH_MAG[3] + P[0][0] * SK_MY[2] - P[0][2] * - SK_MY[1] - P[0][16] * SK_MY[3]); - Kfusion[1] = SK_MY[0] * (P[1][20] + P[1][17] * SH_MAG[0] + P[1][18] * SH_MAG[3] + P[1][0] * SK_MY[2] - P[1][2] * - SK_MY[1] - P[1][16] * SK_MY[3]); - Kfusion[2] = SK_MY[0] * (P[2][20] + P[2][17] * SH_MAG[0] + P[2][18] * SH_MAG[3] + P[2][0] * SK_MY[2] - P[2][2] * - SK_MY[1] - P[2][16] * SK_MY[3]); - Kfusion[3] = SK_MY[0] * (P[3][20] + P[3][17] * SH_MAG[0] + P[3][18] * SH_MAG[3] + P[3][0] * SK_MY[2] - P[3][2] * - SK_MY[1] - P[3][16] * SK_MY[3]); - Kfusion[4] = SK_MY[0] * (P[4][20] + P[4][17] * SH_MAG[0] + P[4][18] * SH_MAG[3] + P[4][0] * SK_MY[2] - P[4][2] * - SK_MY[1] - P[4][16] * SK_MY[3]); - Kfusion[5] = SK_MY[0] * (P[5][20] + P[5][17] * SH_MAG[0] + P[5][18] * SH_MAG[3] + P[5][0] * SK_MY[2] - P[5][2] * - SK_MY[1] - P[5][16] * SK_MY[3]); - Kfusion[6] = SK_MY[0] * (P[6][20] + P[6][17] * SH_MAG[0] + P[6][18] * SH_MAG[3] + P[6][0] * SK_MY[2] - P[6][2] * - SK_MY[1] - P[6][16] * SK_MY[3]); - Kfusion[7] = SK_MY[0] * (P[7][20] + P[7][17] * SH_MAG[0] + P[7][18] * SH_MAG[3] + P[7][0] * SK_MY[2] - P[7][2] * - SK_MY[1] - P[7][16] * SK_MY[3]); - Kfusion[8] = SK_MY[0] * (P[8][20] + P[8][17] * SH_MAG[0] + P[8][18] * SH_MAG[3] + P[8][0] * SK_MY[2] - P[8][2] * - SK_MY[1] - P[8][16] * SK_MY[3]); - Kfusion[9] = SK_MY[0] * (P[9][20] + P[9][17] * SH_MAG[0] + P[9][18] * SH_MAG[3] + P[9][0] * SK_MY[2] - P[9][2] * - SK_MY[1] - P[9][16] * SK_MY[3]); - Kfusion[10] = SK_MY[0] * (P[10][20] + P[10][17] * SH_MAG[0] + P[10][18] * SH_MAG[3] + P[10][0] * SK_MY[2] - P[10][2] * - SK_MY[1] - P[10][16] * SK_MY[3]); - Kfusion[11] = SK_MY[0] * (P[11][20] + P[11][17] * SH_MAG[0] + P[11][18] * SH_MAG[3] + P[11][0] * SK_MY[2] - P[11][2] * - SK_MY[1] - P[11][16] * SK_MY[3]); - Kfusion[12] = SK_MY[0] * (P[12][20] + P[12][17] * SH_MAG[0] + P[12][18] * SH_MAG[3] + P[12][0] * SK_MY[2] - P[12][2] * - SK_MY[1] - P[12][16] * SK_MY[3]); - Kfusion[13] = SK_MY[0] * (P[13][20] + P[13][17] * SH_MAG[0] + P[13][18] * SH_MAG[3] + P[13][0] * SK_MY[2] - P[13][2] * - SK_MY[1] - P[13][16] * SK_MY[3]); - Kfusion[14] = SK_MY[0] * (P[14][20] + P[14][17] * SH_MAG[0] + P[14][18] * SH_MAG[3] + P[14][0] * SK_MY[2] - P[14][2] * - SK_MY[1] - P[14][16] * SK_MY[3]); - Kfusion[15] = SK_MY[0] * (P[15][20] + P[15][17] * SH_MAG[0] + P[15][18] * SH_MAG[3] + P[15][0] * SK_MY[2] - P[15][2] * - SK_MY[1] - P[15][16] * SK_MY[3]); - Kfusion[16] = SK_MY[0] * (P[16][20] + P[16][17] * SH_MAG[0] + P[16][18] * SH_MAG[3] + P[16][0] * SK_MY[2] - P[16][2] * - SK_MY[1] - P[16][16] * SK_MY[3]); - Kfusion[17] = SK_MY[0] * (P[17][20] + P[17][17] * SH_MAG[0] + P[17][18] * SH_MAG[3] + P[17][0] * SK_MY[2] - P[17][2] * - SK_MY[1] - P[17][16] * SK_MY[3]); - Kfusion[18] = SK_MY[0] * (P[18][20] + P[18][17] * SH_MAG[0] + P[18][18] * SH_MAG[3] + P[18][0] * SK_MY[2] - P[18][2] * - SK_MY[1] - P[18][16] * SK_MY[3]); - Kfusion[19] = SK_MY[0] * (P[19][20] + P[19][17] * SH_MAG[0] + P[19][18] * SH_MAG[3] + P[19][0] * SK_MY[2] - P[19][2] * - SK_MY[1] - P[19][16] * SK_MY[3]); - Kfusion[20] = SK_MY[0] * (P[20][20] + P[20][17] * SH_MAG[0] + P[20][18] * SH_MAG[3] + P[20][0] * SK_MY[2] - P[20][2] * - 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]); - } else if (index == 2) { - // Calculate Z axis Kalman gains - SK_MZ[0] = 1.0f / _mag_innov_var[2]; - SK_MZ[1] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); - SK_MZ[2] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; - SK_MZ[3] = 2 * q0 * q1 - 2 * q2 * q3; + if (!_mag_healthy) { + return; + } - Kfusion[0] = SK_MZ[0] * (P[0][21] + P[0][18] * SH_MAG[2] + P[0][16] * SH_MAG[5] - P[0][0] * SK_MZ[1] + P[0][1] * - SK_MZ[2] - P[0][17] * SK_MZ[3]); - Kfusion[1] = SK_MZ[0] * (P[1][21] + P[1][18] * SH_MAG[2] + P[1][16] * SH_MAG[5] - P[1][0] * SK_MZ[1] + P[1][1] * - SK_MZ[2] - P[1][17] * SK_MZ[3]); - Kfusion[2] = SK_MZ[0] * (P[2][21] + P[2][18] * SH_MAG[2] + P[2][16] * SH_MAG[5] - P[2][0] * SK_MZ[1] + P[2][1] * - SK_MZ[2] - P[2][17] * SK_MZ[3]); - Kfusion[3] = SK_MZ[0] * (P[3][21] + P[3][18] * SH_MAG[2] + P[3][16] * SH_MAG[5] - P[3][0] * SK_MZ[1] + P[3][1] * - SK_MZ[2] - P[3][17] * SK_MZ[3]); - Kfusion[4] = SK_MZ[0] * (P[4][21] + P[4][18] * SH_MAG[2] + P[4][16] * SH_MAG[5] - P[4][0] * SK_MZ[1] + P[4][1] * - SK_MZ[2] - P[4][17] * SK_MZ[3]); - Kfusion[5] = SK_MZ[0] * (P[5][21] + P[5][18] * SH_MAG[2] + P[5][16] * SH_MAG[5] - P[5][0] * SK_MZ[1] + P[5][1] * - SK_MZ[2] - P[5][17] * SK_MZ[3]); - Kfusion[6] = SK_MZ[0] * (P[6][21] + P[6][18] * SH_MAG[2] + P[6][16] * SH_MAG[5] - P[6][0] * SK_MZ[1] + P[6][1] * - SK_MZ[2] - P[6][17] * SK_MZ[3]); - Kfusion[7] = SK_MZ[0] * (P[7][21] + P[7][18] * SH_MAG[2] + P[7][16] * SH_MAG[5] - P[7][0] * SK_MZ[1] + P[7][1] * - SK_MZ[2] - P[7][17] * SK_MZ[3]); - Kfusion[8] = SK_MZ[0] * (P[8][21] + P[8][18] * SH_MAG[2] + P[8][16] * SH_MAG[5] - P[8][0] * SK_MZ[1] + P[8][1] * - SK_MZ[2] - P[8][17] * SK_MZ[3]); - Kfusion[9] = SK_MZ[0] * (P[9][21] + P[9][18] * SH_MAG[2] + P[9][16] * SH_MAG[5] - P[9][0] * SK_MZ[1] + P[9][1] * - SK_MZ[2] - P[9][17] * SK_MZ[3]); - Kfusion[10] = SK_MZ[0] * (P[10][21] + P[10][18] * SH_MAG[2] + P[10][16] * SH_MAG[5] - P[10][0] * SK_MZ[1] + P[10][1] * - SK_MZ[2] - P[10][17] * SK_MZ[3]); - Kfusion[11] = SK_MZ[0] * (P[11][21] + P[11][18] * SH_MAG[2] + P[11][16] * SH_MAG[5] - P[11][0] * SK_MZ[1] + P[11][1] * - SK_MZ[2] - P[11][17] * SK_MZ[3]); - Kfusion[12] = SK_MZ[0] * (P[12][21] + P[12][18] * SH_MAG[2] + P[12][16] * SH_MAG[5] - P[12][0] * SK_MZ[1] + P[12][1] * - SK_MZ[2] - P[12][17] * SK_MZ[3]); - Kfusion[13] = SK_MZ[0] * (P[13][21] + P[13][18] * SH_MAG[2] + P[13][16] * SH_MAG[5] - P[13][0] * SK_MZ[1] + P[13][1] * - SK_MZ[2] - P[13][17] * SK_MZ[3]); - Kfusion[14] = SK_MZ[0] * (P[14][21] + P[14][18] * SH_MAG[2] + P[14][16] * SH_MAG[5] - P[14][0] * SK_MZ[1] + P[14][1] * - SK_MZ[2] - P[14][17] * SK_MZ[3]); - Kfusion[15] = SK_MZ[0] * (P[15][21] + P[15][18] * SH_MAG[2] + P[15][16] * SH_MAG[5] - P[15][0] * SK_MZ[1] + P[15][1] * - SK_MZ[2] - P[15][17] * SK_MZ[3]); - Kfusion[16] = SK_MZ[0] * (P[16][21] + P[16][18] * SH_MAG[2] + P[16][16] * SH_MAG[5] - P[16][0] * SK_MZ[1] + P[16][1] * - SK_MZ[2] - P[16][17] * SK_MZ[3]); - Kfusion[17] = SK_MZ[0] * (P[17][21] + P[17][18] * SH_MAG[2] + P[17][16] * SH_MAG[5] - P[17][0] * SK_MZ[1] + P[17][1] * - SK_MZ[2] - P[17][17] * SK_MZ[3]); - Kfusion[18] = SK_MZ[0] * (P[18][21] + P[18][18] * SH_MAG[2] + P[18][16] * SH_MAG[5] - P[18][0] * SK_MZ[1] + P[18][1] * - SK_MZ[2] - P[18][17] * SK_MZ[3]); - Kfusion[19] = SK_MZ[0] * (P[19][21] + P[19][18] * SH_MAG[2] + P[19][16] * SH_MAG[5] - P[19][0] * SK_MZ[1] + P[19][1] * - SK_MZ[2] - P[19][17] * SK_MZ[3]); - Kfusion[20] = SK_MZ[0] * (P[20][21] + P[20][18] * SH_MAG[2] + P[20][16] * SH_MAG[5] - P[20][0] * SK_MZ[1] + P[20][1] * - 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]); - } else { - return; - } + // update the states and covariance usinng sequential fusion of the magnetometer components + for (uint8_t index = 0; index <= 2; index++) { + // Calculate Kalman gains + if (index == 0) { + // Calculate X axis Kalman gains + SK_MX[0] = 1.0f / _mag_innov_var[0]; + SK_MX[1] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); + SK_MX[2] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; + SK_MX[3] = SH_MAG[7]; - // by definition our error state is zero at the time of fusion - _state.ang_error.setZero(); + Kfusion[0] = SK_MX[0] * (P[0][19] + P[0][16] * SH_MAG[1] + P[0][17] * SH_MAG[4] - P[0][1] * SK_MX[2] + P[0][2] * + SK_MX[1] + P[0][18] * SK_MX[3]); + Kfusion[1] = SK_MX[0] * (P[1][19] + P[1][16] * SH_MAG[1] + P[1][17] * SH_MAG[4] - P[1][1] * SK_MX[2] + P[1][2] * + SK_MX[1] + P[1][18] * SK_MX[3]); + Kfusion[2] = SK_MX[0] * (P[2][19] + P[2][16] * SH_MAG[1] + P[2][17] * SH_MAG[4] - P[2][1] * SK_MX[2] + P[2][2] * + SK_MX[1] + P[2][18] * SK_MX[3]); + Kfusion[3] = SK_MX[0] * (P[3][19] + P[3][16] * SH_MAG[1] + P[3][17] * SH_MAG[4] - P[3][1] * SK_MX[2] + P[3][2] * + SK_MX[1] + P[3][18] * SK_MX[3]); + Kfusion[4] = SK_MX[0] * (P[4][19] + P[4][16] * SH_MAG[1] + P[4][17] * SH_MAG[4] - P[4][1] * SK_MX[2] + P[4][2] * + SK_MX[1] + P[4][18] * SK_MX[3]); + Kfusion[5] = SK_MX[0] * (P[5][19] + P[5][16] * SH_MAG[1] + P[5][17] * SH_MAG[4] - P[5][1] * SK_MX[2] + P[5][2] * + SK_MX[1] + P[5][18] * SK_MX[3]); + Kfusion[6] = SK_MX[0] * (P[6][19] + P[6][16] * SH_MAG[1] + P[6][17] * SH_MAG[4] - P[6][1] * SK_MX[2] + P[6][2] * + SK_MX[1] + P[6][18] * SK_MX[3]); + Kfusion[7] = SK_MX[0] * (P[7][19] + P[7][16] * SH_MAG[1] + P[7][17] * SH_MAG[4] - P[7][1] * SK_MX[2] + P[7][2] * + SK_MX[1] + P[7][18] * SK_MX[3]); + Kfusion[8] = SK_MX[0] * (P[8][19] + P[8][16] * SH_MAG[1] + P[8][17] * SH_MAG[4] - P[8][1] * SK_MX[2] + P[8][2] * + SK_MX[1] + P[8][18] * SK_MX[3]); + Kfusion[9] = SK_MX[0] * (P[9][19] + P[9][16] * SH_MAG[1] + P[9][17] * SH_MAG[4] - P[9][1] * SK_MX[2] + P[9][2] * + SK_MX[1] + P[9][18] * SK_MX[3]); + Kfusion[10] = SK_MX[0] * (P[10][19] + P[10][16] * SH_MAG[1] + P[10][17] * SH_MAG[4] - P[10][1] * SK_MX[2] + P[10][2] * + SK_MX[1] + P[10][18] * SK_MX[3]); + Kfusion[11] = SK_MX[0] * (P[11][19] + P[11][16] * SH_MAG[1] + P[11][17] * SH_MAG[4] - P[11][1] * SK_MX[2] + P[11][2] * + SK_MX[1] + P[11][18] * SK_MX[3]); + Kfusion[12] = SK_MX[0] * (P[12][19] + P[12][16] * SH_MAG[1] + P[12][17] * SH_MAG[4] - P[12][1] * SK_MX[2] + P[12][2] * + SK_MX[1] + P[12][18] * SK_MX[3]); + Kfusion[13] = SK_MX[0] * (P[13][19] + P[13][16] * SH_MAG[1] + P[13][17] * SH_MAG[4] - P[13][1] * SK_MX[2] + P[13][2] * + SK_MX[1] + P[13][18] * SK_MX[3]); + Kfusion[14] = SK_MX[0] * (P[14][19] + P[14][16] * SH_MAG[1] + P[14][17] * SH_MAG[4] - P[14][1] * SK_MX[2] + P[14][2] * + SK_MX[1] + P[14][18] * SK_MX[3]); + Kfusion[15] = SK_MX[0] * (P[15][19] + P[15][16] * SH_MAG[1] + P[15][17] * SH_MAG[4] - P[15][1] * SK_MX[2] + P[15][2] * + SK_MX[1] + P[15][18] * SK_MX[3]); + Kfusion[16] = SK_MX[0] * (P[16][19] + P[16][16] * SH_MAG[1] + P[16][17] * SH_MAG[4] - P[16][1] * SK_MX[2] + P[16][2] * + SK_MX[1] + P[16][18] * SK_MX[3]); + Kfusion[17] = SK_MX[0] * (P[17][19] + P[17][16] * SH_MAG[1] + P[17][17] * SH_MAG[4] - P[17][1] * SK_MX[2] + P[17][2] * + SK_MX[1] + P[17][18] * SK_MX[3]); + Kfusion[18] = SK_MX[0] * (P[18][19] + P[18][16] * SH_MAG[1] + P[18][17] * SH_MAG[4] - P[18][1] * SK_MX[2] + P[18][2] * + SK_MX[1] + P[18][18] * SK_MX[3]); + Kfusion[19] = SK_MX[0] * (P[19][19] + P[19][16] * SH_MAG[1] + P[19][17] * SH_MAG[4] - P[19][1] * SK_MX[2] + P[19][2] * + SK_MX[1] + P[19][18] * SK_MX[3]); + Kfusion[20] = SK_MX[0] * (P[20][19] + P[20][16] * SH_MAG[1] + P[20][17] * SH_MAG[4] - P[20][1] * SK_MX[2] + P[20][2] * + 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]); - fuse(Kfusion, _mag_innov[index]); + } else if (index == 1) { + // Calculate Y axis Kalman gains + SK_MY[0] = 1.0f / _mag_innov_var[1]; + SK_MY[1] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; + SK_MY[2] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; + SK_MY[3] = SH_MAG[8] - 2 * q1 * q2; - 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(); + Kfusion[0] = SK_MY[0] * (P[0][20] + P[0][17] * SH_MAG[0] + P[0][18] * SH_MAG[3] + P[0][0] * SK_MY[2] - P[0][2] * + SK_MY[1] - P[0][16] * SK_MY[3]); + Kfusion[1] = SK_MY[0] * (P[1][20] + P[1][17] * SH_MAG[0] + P[1][18] * SH_MAG[3] + P[1][0] * SK_MY[2] - P[1][2] * + SK_MY[1] - P[1][16] * SK_MY[3]); + Kfusion[2] = SK_MY[0] * (P[2][20] + P[2][17] * SH_MAG[0] + P[2][18] * SH_MAG[3] + P[2][0] * SK_MY[2] - P[2][2] * + SK_MY[1] - P[2][16] * SK_MY[3]); + Kfusion[3] = SK_MY[0] * (P[3][20] + P[3][17] * SH_MAG[0] + P[3][18] * SH_MAG[3] + P[3][0] * SK_MY[2] - P[3][2] * + SK_MY[1] - P[3][16] * SK_MY[3]); + Kfusion[4] = SK_MY[0] * (P[4][20] + P[4][17] * SH_MAG[0] + P[4][18] * SH_MAG[3] + P[4][0] * SK_MY[2] - P[4][2] * + SK_MY[1] - P[4][16] * SK_MY[3]); + Kfusion[5] = SK_MY[0] * (P[5][20] + P[5][17] * SH_MAG[0] + P[5][18] * SH_MAG[3] + P[5][0] * SK_MY[2] - P[5][2] * + SK_MY[1] - P[5][16] * SK_MY[3]); + Kfusion[6] = SK_MY[0] * (P[6][20] + P[6][17] * SH_MAG[0] + P[6][18] * SH_MAG[3] + P[6][0] * SK_MY[2] - P[6][2] * + SK_MY[1] - P[6][16] * SK_MY[3]); + Kfusion[7] = SK_MY[0] * (P[7][20] + P[7][17] * SH_MAG[0] + P[7][18] * SH_MAG[3] + P[7][0] * SK_MY[2] - P[7][2] * + SK_MY[1] - P[7][16] * SK_MY[3]); + Kfusion[8] = SK_MY[0] * (P[8][20] + P[8][17] * SH_MAG[0] + P[8][18] * SH_MAG[3] + P[8][0] * SK_MY[2] - P[8][2] * + SK_MY[1] - P[8][16] * SK_MY[3]); + Kfusion[9] = SK_MY[0] * (P[9][20] + P[9][17] * SH_MAG[0] + P[9][18] * SH_MAG[3] + P[9][0] * SK_MY[2] - P[9][2] * + SK_MY[1] - P[9][16] * SK_MY[3]); + Kfusion[10] = SK_MY[0] * (P[10][20] + P[10][17] * SH_MAG[0] + P[10][18] * SH_MAG[3] + P[10][0] * SK_MY[2] - P[10][2] * + SK_MY[1] - P[10][16] * SK_MY[3]); + Kfusion[11] = SK_MY[0] * (P[11][20] + P[11][17] * SH_MAG[0] + P[11][18] * SH_MAG[3] + P[11][0] * SK_MY[2] - P[11][2] * + SK_MY[1] - P[11][16] * SK_MY[3]); + Kfusion[12] = SK_MY[0] * (P[12][20] + P[12][17] * SH_MAG[0] + P[12][18] * SH_MAG[3] + P[12][0] * SK_MY[2] - P[12][2] * + SK_MY[1] - P[12][16] * SK_MY[3]); + Kfusion[13] = SK_MY[0] * (P[13][20] + P[13][17] * SH_MAG[0] + P[13][18] * SH_MAG[3] + P[13][0] * SK_MY[2] - P[13][2] * + SK_MY[1] - P[13][16] * SK_MY[3]); + Kfusion[14] = SK_MY[0] * (P[14][20] + P[14][17] * SH_MAG[0] + P[14][18] * SH_MAG[3] + P[14][0] * SK_MY[2] - P[14][2] * + SK_MY[1] - P[14][16] * SK_MY[3]); + Kfusion[15] = SK_MY[0] * (P[15][20] + P[15][17] * SH_MAG[0] + P[15][18] * SH_MAG[3] + P[15][0] * SK_MY[2] - P[15][2] * + SK_MY[1] - P[15][16] * SK_MY[3]); + Kfusion[16] = SK_MY[0] * (P[16][20] + P[16][17] * SH_MAG[0] + P[16][18] * SH_MAG[3] + P[16][0] * SK_MY[2] - P[16][2] * + SK_MY[1] - P[16][16] * SK_MY[3]); + Kfusion[17] = SK_MY[0] * (P[17][20] + P[17][17] * SH_MAG[0] + P[17][18] * SH_MAG[3] + P[17][0] * SK_MY[2] - P[17][2] * + SK_MY[1] - P[17][16] * SK_MY[3]); + Kfusion[18] = SK_MY[0] * (P[18][20] + P[18][17] * SH_MAG[0] + P[18][18] * SH_MAG[3] + P[18][0] * SK_MY[2] - P[18][2] * + SK_MY[1] - P[18][16] * SK_MY[3]); + Kfusion[19] = SK_MY[0] * (P[19][20] + P[19][17] * SH_MAG[0] + P[19][18] * SH_MAG[3] + P[19][0] * SK_MY[2] - P[19][2] * + SK_MY[1] - P[19][16] * SK_MY[3]); + Kfusion[20] = SK_MY[0] * (P[20][20] + P[20][17] * SH_MAG[0] + P[20][18] * SH_MAG[3] + P[20][0] * SK_MY[2] - P[20][2] * + 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]); - // apply covariance correction via P_new = (I -K*H)*P - // first calculate expression for KHP - // then calculate P - KHP - float KH[_k_num_states][_k_num_states] = {}; + } else if (index == 2) { + // Calculate Z axis Kalman gains + SK_MZ[0] = 1.0f / _mag_innov_var[2]; + SK_MZ[1] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); + SK_MZ[2] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; + SK_MZ[3] = 2 * q0 * q1 - 2 * q2 * q3; - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < 3; column++) { - KH[row][column] = Kfusion[row] * H_MAG[index][column]; - } + Kfusion[0] = SK_MZ[0] * (P[0][21] + P[0][18] * SH_MAG[2] + P[0][16] * SH_MAG[5] - P[0][0] * SK_MZ[1] + P[0][1] * + SK_MZ[2] - P[0][17] * SK_MZ[3]); + Kfusion[1] = SK_MZ[0] * (P[1][21] + P[1][18] * SH_MAG[2] + P[1][16] * SH_MAG[5] - P[1][0] * SK_MZ[1] + P[1][1] * + SK_MZ[2] - P[1][17] * SK_MZ[3]); + Kfusion[2] = SK_MZ[0] * (P[2][21] + P[2][18] * SH_MAG[2] + P[2][16] * SH_MAG[5] - P[2][0] * SK_MZ[1] + P[2][1] * + SK_MZ[2] - P[2][17] * SK_MZ[3]); + Kfusion[3] = SK_MZ[0] * (P[3][21] + P[3][18] * SH_MAG[2] + P[3][16] * SH_MAG[5] - P[3][0] * SK_MZ[1] + P[3][1] * + SK_MZ[2] - P[3][17] * SK_MZ[3]); + Kfusion[4] = SK_MZ[0] * (P[4][21] + P[4][18] * SH_MAG[2] + P[4][16] * SH_MAG[5] - P[4][0] * SK_MZ[1] + P[4][1] * + SK_MZ[2] - P[4][17] * SK_MZ[3]); + Kfusion[5] = SK_MZ[0] * (P[5][21] + P[5][18] * SH_MAG[2] + P[5][16] * SH_MAG[5] - P[5][0] * SK_MZ[1] + P[5][1] * + SK_MZ[2] - P[5][17] * SK_MZ[3]); + Kfusion[6] = SK_MZ[0] * (P[6][21] + P[6][18] * SH_MAG[2] + P[6][16] * SH_MAG[5] - P[6][0] * SK_MZ[1] + P[6][1] * + SK_MZ[2] - P[6][17] * SK_MZ[3]); + Kfusion[7] = SK_MZ[0] * (P[7][21] + P[7][18] * SH_MAG[2] + P[7][16] * SH_MAG[5] - P[7][0] * SK_MZ[1] + P[7][1] * + SK_MZ[2] - P[7][17] * SK_MZ[3]); + Kfusion[8] = SK_MZ[0] * (P[8][21] + P[8][18] * SH_MAG[2] + P[8][16] * SH_MAG[5] - P[8][0] * SK_MZ[1] + P[8][1] * + SK_MZ[2] - P[8][17] * SK_MZ[3]); + Kfusion[9] = SK_MZ[0] * (P[9][21] + P[9][18] * SH_MAG[2] + P[9][16] * SH_MAG[5] - P[9][0] * SK_MZ[1] + P[9][1] * + SK_MZ[2] - P[9][17] * SK_MZ[3]); + Kfusion[10] = SK_MZ[0] * (P[10][21] + P[10][18] * SH_MAG[2] + P[10][16] * SH_MAG[5] - P[10][0] * SK_MZ[1] + P[10][1] * + SK_MZ[2] - P[10][17] * SK_MZ[3]); + Kfusion[11] = SK_MZ[0] * (P[11][21] + P[11][18] * SH_MAG[2] + P[11][16] * SH_MAG[5] - P[11][0] * SK_MZ[1] + P[11][1] * + SK_MZ[2] - P[11][17] * SK_MZ[3]); + Kfusion[12] = SK_MZ[0] * (P[12][21] + P[12][18] * SH_MAG[2] + P[12][16] * SH_MAG[5] - P[12][0] * SK_MZ[1] + P[12][1] * + SK_MZ[2] - P[12][17] * SK_MZ[3]); + Kfusion[13] = SK_MZ[0] * (P[13][21] + P[13][18] * SH_MAG[2] + P[13][16] * SH_MAG[5] - P[13][0] * SK_MZ[1] + P[13][1] * + SK_MZ[2] - P[13][17] * SK_MZ[3]); + Kfusion[14] = SK_MZ[0] * (P[14][21] + P[14][18] * SH_MAG[2] + P[14][16] * SH_MAG[5] - P[14][0] * SK_MZ[1] + P[14][1] * + SK_MZ[2] - P[14][17] * SK_MZ[3]); + Kfusion[15] = SK_MZ[0] * (P[15][21] + P[15][18] * SH_MAG[2] + P[15][16] * SH_MAG[5] - P[15][0] * SK_MZ[1] + P[15][1] * + SK_MZ[2] - P[15][17] * SK_MZ[3]); + Kfusion[16] = SK_MZ[0] * (P[16][21] + P[16][18] * SH_MAG[2] + P[16][16] * SH_MAG[5] - P[16][0] * SK_MZ[1] + P[16][1] * + SK_MZ[2] - P[16][17] * SK_MZ[3]); + Kfusion[17] = SK_MZ[0] * (P[17][21] + P[17][18] * SH_MAG[2] + P[17][16] * SH_MAG[5] - P[17][0] * SK_MZ[1] + P[17][1] * + SK_MZ[2] - P[17][17] * SK_MZ[3]); + Kfusion[18] = SK_MZ[0] * (P[18][21] + P[18][18] * SH_MAG[2] + P[18][16] * SH_MAG[5] - P[18][0] * SK_MZ[1] + P[18][1] * + SK_MZ[2] - P[18][17] * SK_MZ[3]); + Kfusion[19] = SK_MZ[0] * (P[19][21] + P[19][18] * SH_MAG[2] + P[19][16] * SH_MAG[5] - P[19][0] * SK_MZ[1] + P[19][1] * + SK_MZ[2] - P[19][17] * SK_MZ[3]); + Kfusion[20] = SK_MZ[0] * (P[20][21] + P[20][18] * SH_MAG[2] + P[20][16] * SH_MAG[5] - P[20][0] * SK_MZ[1] + P[20][1] * + 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]); - for (unsigned column = 16; column < 22; column++) { - KH[row][column] = Kfusion[row] * H_MAG[index][column]; - } + } else { + return; + } - } + // by definition our error state is zero at the time of fusion + _state.ang_error.setZero(); - float KHP[_k_num_states][_k_num_states] = {}; + fuse(Kfusion, _mag_innov[index]); - 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][1] * P[1][column]; - tmp += KH[row][2] * P[2][column]; - tmp += KH[row][16] * P[16][column]; - tmp += KH[row][17] * P[17][column]; - tmp += KH[row][18] * P[18][column]; - tmp += KH[row][19] * P[19][column]; - tmp += KH[row][20] * P[20][column]; - tmp += KH[row][21] * P[21][column]; - KHP[row][column] = tmp; - } - } + 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(); - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P[row][column] -= KHP[row][column]; - } - } + // apply covariance correction via P_new = (I -K*H)*P + // first calculate expression for KHP + // then calculate P - KHP + float KH[_k_num_states][_k_num_states] = {}; - makeSymmetrical(); - limitCov(); - } + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < 3; column++) { + KH[row][column] = Kfusion[row] * H_MAG[index][column]; + } + + for (unsigned column = 16; column < 22; column++) { + KH[row][column] = Kfusion[row] * H_MAG[index][column]; + } + + } + + float KHP[_k_num_states][_k_num_states] = {}; + + 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][1] * P[1][column]; + tmp += KH[row][2] * P[2][column]; + tmp += KH[row][16] * P[16][column]; + tmp += KH[row][17] * P[17][column]; + tmp += KH[row][18] * P[18][column]; + tmp += KH[row][19] * P[19][column]; + tmp += KH[row][20] * P[20][column]; + tmp += KH[row][21] * P[21][column]; + KHP[row][column] = tmp; + } + } + + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + P[row][column] -= KHP[row][column]; + } + } + + makeSymmetrical(); + limitCov(); + } } void Ekf::fuseHeading() @@ -522,14 +533,14 @@ void Ekf::fuseHeading() } if (innovation_var >= R_mag) { - // the innovation variance contribution from the state covariances is not negative, no fault + // the innovation variance contribution from the state covariances is not negative, no fault _fault_status.bad_mag_x = false; _fault_status.bad_mag_y = false; _fault_status.bad_mag_z = false; } else { - // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned - _fault_status.bad_mag_x = true; + // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned + _fault_status.bad_mag_x = true; _fault_status.bad_mag_y = true; _fault_status.bad_mag_z = true; @@ -552,16 +563,16 @@ void Ekf::fuseHeading() } // innovation test ratio - _yaw_test_ratio = sq(innovation) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var); + _yaw_test_ratio = sq(innovation) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var); // set the magnetometer unhealthy if the test fails - if (_yaw_test_ratio > 1.0f) { + 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) { + if (_control_status.flags.in_air) { return; }