From 99ba1c3745eb5feaebb50de894a2ff24aed7befa Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 23 Aug 2017 21:18:48 -0400 Subject: [PATCH] EKF trivial code style cleanup --- EKF/RingBuffer.h | 2 +- EKF/airspeed_fusion.cpp | 3 +- EKF/control.cpp | 28 +-- EKF/covariance.cpp | 55 +++--- EKF/drag_fusion.cpp | 24 +-- EKF/ekf.cpp | 26 +-- EKF/ekf_helper.cpp | 14 +- EKF/geo.cpp | 16 +- EKF/mag_fusion.cpp | 12 +- EKF/optflow_fusion.cpp | 2 +- EKF/sideslip_fusion.cpp | 388 ++++++++++++++++++++-------------------- ecl.h | 14 ++ 12 files changed, 295 insertions(+), 289 deletions(-) diff --git a/EKF/RingBuffer.h b/EKF/RingBuffer.h index 6d5d7650a1..f318800d1c 100644 --- a/EKF/RingBuffer.h +++ b/EKF/RingBuffer.h @@ -71,7 +71,7 @@ public: _size = size; // set the time elements to zero so that bad data is not retrieved from the buffers - for (unsigned index=0; index < _size; index++) { + for (unsigned index = 0; index < _size; index++) { _buffer[index].time_us = 0; } _first_write = true; diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 0d720f7589..aec4e40455 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -150,8 +150,7 @@ void Ekf::fuseAirspeed() if (_tas_test_ratio > 1.0f) { _innov_check_fail_status.flags.reject_airspeed = true; return; - } - else { + } else { _innov_check_fail_status.flags.reject_airspeed = false; } diff --git a/EKF/control.cpp b/EKF/control.cpp index 947febad1d..75d6dd92fb 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -195,7 +195,7 @@ void Ekf::controlExternalVisionFusion() for (unsigned i=0; i < output_length; i++) { output_states = _output_buffer.get_from_index(i); output_states.quat_nominal *= _state_reset_status.quat_change; - _output_buffer.push_to_index(i,output_states); + _output_buffer.push_to_index(i, output_states); } // apply the change in attitude quaternion to our newest quaternion estimate @@ -424,7 +424,7 @@ void Ekf::controlGpsFusion() } if (_control_status.flags.gps) { ECL_INFO("EKF commencing GPS fusion"); - _time_last_gps = _time_last_imu; + _time_last_gps = _time_last_imu; } } @@ -470,7 +470,7 @@ void Ekf::controlGpsFusion() // correct velocity for offset relative to IMU Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt); Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; - Vector3f vel_offset_body = cross_product(ang_rate,pos_offset_body); + Vector3f vel_offset_body = cross_product(ang_rate, pos_offset_body); Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; _gps_sample_delayed.vel -= vel_offset_earth; @@ -577,7 +577,7 @@ void Ekf::controlHeightSensorTimeouts() reset_height = true; ECL_WARN("EKF baro hgt timeout - reset to GPS"); - } else if (reset_to_baro){ + } else if (reset_to_baro) { // set height sensor health _baro_hgt_faulty = false; @@ -968,11 +968,13 @@ void Ekf::checkForStuckRange() _rng_stuck = false; } else { - if (_range_sample_delayed.rng > _rng_check_max_val) + if (_range_sample_delayed.rng > _rng_check_max_val) { _rng_check_max_val = _range_sample_delayed.rng; + } - if (_rng_check_min_val < 0.1f || _range_sample_delayed.rng < _rng_check_min_val) + if (_rng_check_min_val < 0.1f || _range_sample_delayed.rng < _rng_check_min_val) { _rng_check_min_val = _range_sample_delayed.rng; + } _range_data_ready = false; } @@ -984,11 +986,12 @@ void Ekf::checkForStuckRange() void Ekf::controlAirDataFusion() { - // control activation and initialisation/reset of wind states required for airspeed fusion + // control activation and initialisation/reset of wind states required for airspeed fusion // If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates - bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6; - bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6; + bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6; + bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6; + if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) { _control_status.flags.wind = false; @@ -1048,15 +1051,12 @@ void Ekf::controlBetaFusion() } fuseSideslip(); - } - - - + } } void Ekf::controlDragFusion() { - if (_params.fusion_mode & MASK_USE_DRAG ) { + if (_params.fusion_mode & MASK_USE_DRAG) { if (_control_status.flags.in_air) { if (!_control_status.flags.wind) { // reset the wind states and covariances when starting drag accel fusion diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index f85eb54361..b21a402ce7 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -95,7 +95,7 @@ void Ekf::initialiseCovariance() // earth frame and body frame magnetic field // set to observation variance - for (uint8_t index=16; index <= 21; index ++) { + for (uint8_t index = 16; index <= 21; index ++) { P[index][index] = sq(_params.mag_noise); } @@ -733,12 +733,14 @@ void Ekf::fixCovarianceErrors() const float minSafeStateVar = 1e-9f; float maxStateVar = minSafeStateVar; bool resetRequired = false; - for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) { - if (P[stateIndex][stateIndex] > maxStateVar) { - maxStateVar = P[stateIndex][stateIndex]; - } else if (P[stateIndex][stateIndex] < minSafeStateVar) { - resetRequired = true; - } + + for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) { + if (P[stateIndex][stateIndex] > maxStateVar) { + maxStateVar = P[stateIndex][stateIndex]; + + } else if (P[stateIndex][stateIndex] < minSafeStateVar) { + resetRequired = true; + } } // To ensure stability of the covariance matrix operations, the ratio of a max and min variance must @@ -746,31 +748,37 @@ void Ekf::fixCovarianceErrors() // Also limit variance to a maximum equivalent to a 1g uncertainty const float minStateVarTarget = 1E-8f; float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget); - for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) { - P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar, sq(_gravity_mss * _dt_ekf_avg)); + + for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) { + P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar, + sq(_gravity_mss * _dt_ekf_avg)); } // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero if (resetRequired) { - float delVelBiasVar[3]; - // store all delta velocity bias variances - for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) { - delVelBiasVar[stateIndex-13] = P[stateIndex][stateIndex]; - } - // reset all delta velocity bias covariances - zeroCols(P,13,15); - // restore all delta velocity bias variances - for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) { - P[stateIndex][stateIndex] = delVelBiasVar[stateIndex-13]; - } + float delVelBiasVar[3]; + + // store all delta velocity bias variances + for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) { + delVelBiasVar[stateIndex - 13] = P[stateIndex][stateIndex]; + } + + // reset all delta velocity bias covariances + zeroCols(P, 13, 15); + + // restore all delta velocity bias variances + for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) { + P[stateIndex][stateIndex] = delVelBiasVar[stateIndex - 13]; + } } // Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong // calculate accel bias term aligned with the gravity vector float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg; float down_dvel_bias = 0.0f; - for (uint8_t axis_index=0; axis_index < 3; axis_index++) { - down_dvel_bias += _state.accel_bias(axis_index) * _R_to_earth(2,axis_index); + + for (uint8_t axis_index = 0; axis_index < 3; axis_index++) { + down_dvel_bias += _state.accel_bias(axis_index) * _R_to_earth(2, axis_index); } // check that the vertical componenent of accel bias is consistent with both the vertical position and velocity innovation @@ -838,7 +846,7 @@ void Ekf::fixCovarianceErrors() } void Ekf::resetMagCovariance() -{ +{ // set the quaternion covariance terms to zero zeroRows(P,0,3); zeroCols(P,0,3); @@ -893,5 +901,4 @@ void Ekf::resetWindCovariance() P[23][23] = sq(1.0f); } - } diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index 463bfcc720..a7daf26525 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -43,7 +43,7 @@ #include "mathlib.h" void Ekf::fuseDrag() -{ +{ float SH_ACC[4] = {}; // Variable used to optimise calculations of measurement jacobian float H_ACC[24] = {}; // Observation Jacobian float SK_ACC[9] = {}; // Variable used to optimise calculations of the Kalman gain vector @@ -85,7 +85,7 @@ void Ekf::fuseDrag() rel_wind = earth_to_body * rel_wind; // perform sequential fusion of XY specific forces - for (uint8_t axis_index = 0; axis_index <2; axis_index++) { + for (uint8_t axis_index = 0; axis_index < 2; axis_index++) { // calculate observation jacobiam and Kalman gain vectors if (axis_index == 0) { // Estimate the airspeed from the measured drag force and ballistic coefficient @@ -94,7 +94,7 @@ void Ekf::fuseDrag() // Estimate the derivative of specific force wrt airspeed along the X axis // Limit lower value to prevent arithmetic exceptions - float Kacc = fmaxf(1e-1f,rho * BC_inv_x * airSpd); + float Kacc = fmaxf(1e-1f, rho * BC_inv_x * airSpd); SH_ACC[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3); SH_ACC[1] = vn - vwn; @@ -149,11 +149,13 @@ void Ekf::fuseDrag() // calculate the predicted acceleration and innovation measured along the X body axis float drag_sign; if (rel_wind(axis_index) >= 0.0f) { - drag_sign = 1.0f; + drag_sign = 1.0f; + } else { - drag_sign = -1.0f; + drag_sign = -1.0f; } - float predAccel = -BC_inv_x * 0.5f*rho*sq(rel_wind(axis_index)) * drag_sign; + + float predAccel = -BC_inv_x * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign; _drag_innov[axis_index] = predAccel - mea_acc; _drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]); @@ -164,7 +166,7 @@ void Ekf::fuseDrag() // Estimate the derivative of specific force wrt airspeed along the X axis // Limit lower value to prevent arithmetic exceptions - float Kacc = fmaxf(1e-1f,rho * BC_inv_y * airSpd); + float Kacc = fmaxf(1e-1f, rho * BC_inv_y * airSpd); SH_ACC[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); SH_ACC[1] = vn - vwn; @@ -221,11 +223,13 @@ void Ekf::fuseDrag() // calculate the predicted acceleration and innovation measured along the Y body axis float drag_sign; if (rel_wind(axis_index) >= 0.0f) { - drag_sign = 1.0f; + drag_sign = 1.0f; + } else { - drag_sign = -1.0f; + drag_sign = -1.0f; } - float predAccel = -BC_inv_y * 0.5f*rho*sq(rel_wind(axis_index)) * drag_sign; + + float predAccel = -BC_inv_y * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign; _drag_innov[axis_index] = predAccel - mea_acc; _drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]); diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index e7d070bc0c..a93cf795d7 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -43,21 +43,6 @@ #include "ekf.h" #include "mathlib.h" -#ifndef __PX4_QURT -#if defined(__cplusplus) && !defined(__PX4_NUTTX) -#include -#define ISFINITE(x) std::isfinite(x) -#else -#define ISFINITE(x) isfinite(x) -#endif -#endif - -#if defined(__PX4_QURT) -// Missing math.h defines -#define ISFINITE(x) __builtin_isfinite(x) -#endif - - bool Ekf::init(uint64_t timestamp) { bool ret = initialise_interface(timestamp); @@ -617,7 +602,8 @@ void Ekf::calculateOutputStates() unsigned index = _output_vert_buffer.get_oldest_index(); unsigned index_next; unsigned size = _output_vert_buffer.get_length(); - for (unsigned counter=0; counter < (size - 1); counter++) { + + for (unsigned counter = 0; counter < (size - 1); counter++) { index_next = (index + 1) % size; current_state = _output_vert_buffer.get_from_index(index); next_state = _output_vert_buffer.get_from_index(index_next); @@ -625,7 +611,7 @@ void Ekf::calculateOutputStates() // correct the velocity if (counter == 0) { current_state.vel_d += vel_d_correction; - _output_vert_buffer.push_to_index(index,current_state); + _output_vert_buffer.push_to_index(index, current_state); } next_state.vel_d += vel_d_correction; @@ -633,7 +619,7 @@ void Ekf::calculateOutputStates() next_state.vel_d_integ = current_state.vel_d_integ + (current_state.vel_d + next_state.vel_d) * 0.5f * next_state.dt; // push the updated data to the buffer - _output_vert_buffer.push_to_index(index_next,next_state); + _output_vert_buffer.push_to_index(index_next, next_state); // advance the index index = (index + 1) % size; @@ -664,7 +650,7 @@ void Ekf::calculateOutputStates() // loop through the output filter state history and apply the corrections to the velocity and position states outputSample output_states; unsigned max_index = _output_buffer.get_length() - 1; - for (unsigned index=0; index <= max_index; index++) { + for (unsigned index = 0; index <= max_index; index++) { output_states = _output_buffer.get_from_index(index); // a constant velocity correction is applied @@ -674,7 +660,7 @@ void Ekf::calculateOutputStates() output_states.pos += pos_correction; // push the updated data to the buffer - _output_buffer.push_to_index(index,output_states); + _output_buffer.push_to_index(index, output_states); } diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 3dc01d9e9f..34d3142de1 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -351,10 +351,10 @@ bool Ekf::realignYawGPS() bool badVelInnov = ((_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f)) && _control_status.flags.gps; // calculate GPS course over ground angle - float gpsCOG = atan2f(_gps_sample_delayed.vel(1),_gps_sample_delayed.vel(0)); + float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0)); // calculate course yaw angle - float ekfGOG = atan2f(_state.vel(1),_state.vel(0)); + float ekfGOG = atan2f(_state.vel(1), _state.vel(0)); // Check the EKF and GPS course over ground for consistency float courseYawError = gpsCOG - ekfGOG; @@ -444,10 +444,10 @@ bool Ekf::realignYawGPS() // add the reset amount to the output observer buffered data outputSample output_states; unsigned output_length = _output_buffer.get_length(); - for (unsigned i=0; i < output_length; i++) { + for (unsigned i = 0; i < output_length; i++) { output_states = _output_buffer.get_from_index(i); output_states.quat_nominal *= _state_reset_status.quat_change; - _output_buffer.push_to_index(i,output_states); + _output_buffer.push_to_index(i, output_states); } // apply the change in attitude quaternion to our newest quaternion estimate @@ -527,10 +527,10 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // add the reset amount to the output observer buffered data outputSample output_states; unsigned output_length = _output_buffer.get_length(); - for (unsigned i=0; i < output_length; i++) { + for (unsigned i = 0; i < output_length; i++) { output_states = _output_buffer.get_from_index(i); output_states.quat_nominal *= _state_reset_status.quat_change; - _output_buffer.push_to_index(i,output_states); + _output_buffer.push_to_index(i, output_states); } // apply the change in attitude quaternion to our newest quaternion estimate @@ -664,7 +664,7 @@ void Ekf::calcMagDeclination() // set source of magnetic declination for internal use if (_flt_mag_align_complete) { // Use value consistent with earth field state - _mag_declination = atan2f(_state.mag_I(1),_state.mag_I(0)); + _mag_declination = atan2f(_state.mag_I(1), _state.mag_I(0)); } else if (_params.mag_declination_source & MASK_USE_GEO_DECL) { // use parameter value until GPS is available, then use value returned by geo library if (_NED_origin_initialised) { diff --git a/EKF/geo.cpp b/EKF/geo.cpp index 1576e1f7ee..7fca38888c 100644 --- a/EKF/geo.cpp +++ b/EKF/geo.cpp @@ -42,23 +42,15 @@ * @author Anton Babushkin */ #ifdef POSIX_SHARED -//#include -//#include + +#include "ecl.h" + #include #include #include #include #include -#ifndef __PX4_QURT -#if defined(__cplusplus) && !defined(__PX4_NUTTX) -#include -#define ISFINITE(x) std::isfinite(x) -#else -#define ISFINITE(x) isfinite(x) -#endif -#endif - /**************************************************************************** * * Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved. @@ -473,7 +465,7 @@ float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_ne double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ - float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , + float theta = atan2f(sin(d_lon) * cos(lat_next_rad), cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); theta = _wrap_pi(theta); diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 6c19d63950..2294d1a5b0 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -207,7 +207,7 @@ void Ekf::fuseMag() Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]); Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]); } else { - for (uint8_t i=0; i<16; i++) { + for (uint8_t i = 0; i < 16; i++) { Kfusion[i] = 0.0f; } Kfusion[22] = 0.0f; @@ -261,7 +261,7 @@ void Ekf::fuseMag() Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]); } else { - for (uint8_t i=0; i<16; i++) { + for (uint8_t i = 0; i < 16; i++) { Kfusion[i] = 0.0f; } Kfusion[22] = 0.0f; @@ -315,7 +315,7 @@ void Ekf::fuseMag() Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]); Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]); } else { - for (uint8_t i=0; i<16; i++) { + for (uint8_t i = 0; i < 16; i++) { Kfusion[i] = 0.0f; } Kfusion[22] = 0.0f; @@ -523,9 +523,9 @@ void Ekf::fuseHeading() [ cos(pitch)*sin(yaw) + cos(yaw)*sin(pitch)*sin(roll), cos(roll)*cos(yaw), sin(pitch)*sin(yaw) - cos(pitch)*cos(yaw)*sin(roll)] [ -cos(roll)*sin(pitch), sin(roll), cos(pitch)*cos(roll)] */ - float yaw = atan2f(-_R_to_earth(0, 1) , _R_to_earth(1, 1)); // first rotation (yaw) + float yaw = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); // first rotation (yaw) float roll = asinf(_R_to_earth(2, 1)); // second rotation (roll) - float pitch = atan2f(-_R_to_earth(2, 0) , _R_to_earth(2, 2)); // third rotation (pitch) + float pitch = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch) predicted_hdg = yaw; // we will need the predicted heading to calculate the innovation @@ -812,7 +812,7 @@ void Ekf::fuseDeclination() Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN); // calculate innovation and constrain - float innovation = atan2f(magE , magN) - _mag_declination; + float innovation = atan2f(magE, magN) - _mag_declination; innovation = math::constrain(innovation, -0.5f, 0.5f); // apply covariance correction via P_new = (I -K*H)*P diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index ee01a301ec..29ff1531fb 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -76,7 +76,7 @@ void Ekf::fuseOptFlow() Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; // calculate the velocity of the sensor reelative to the imu in body frame - Vector3f vel_rel_imu_body = cross_product(_flow_sample_delayed.gyroXYZ , pos_offset_body); + Vector3f vel_rel_imu_body = cross_product(_flow_sample_delayed.gyroXYZ, pos_offset_body); // calculate the velocity of the sensor in the earth frame Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index 3604f3269a..b3f102b0fc 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -44,12 +44,12 @@ #include "mathlib.h" void Ekf::fuseSideslip() -{ +{ float SH_BETA[13] = {}; // Varialbe used to optimise calculations of measurement jacobian float H_BETA[24] = {}; // Observation Jacobian float SK_BETA[8] = {}; // Varialbe used to optimise calculations of the Kalman gain vector float Kfusion[24] = {}; // Kalman gain vector - float R_BETA = _params.beta_noise; + float R_BETA = _params.beta_noise; // get latest estimated orientation float q0 = _state.quat_nominal(0); @@ -67,214 +67,218 @@ void Ekf::fuseSideslip() float vwe = _state.wind_vel(1); // relative wind velocity in earth frame - Vector3f rel_wind; - rel_wind(0) = vn - vwn; - rel_wind(1) = ve - vwe; - rel_wind(2) = vd; + Vector3f rel_wind; + rel_wind(0) = vn - vwn; + rel_wind(1) = ve - vwe; + rel_wind(2) = vd; - Dcmf earth_to_body(_state.quat_nominal); + Dcmf earth_to_body(_state.quat_nominal); earth_to_body = earth_to_body.transpose(); //Why transpose? - // rotate into body axes - rel_wind = earth_to_body * rel_wind; + // rotate into body axes + rel_wind = earth_to_body * rel_wind; - // perform fusion of assumed sideslip = 0 - if (rel_wind(0) > 7.0f){ + // perform fusion of assumed sideslip = 0 + if (rel_wind(0) > 7.0f) { // Calculate the observation jacobians // intermediate variable from algebraic optimisation - SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2.0f*q0*q2 - 2.0f*q1*q3) + (ve - vwe)*(2.0f*q0*q3 + 2.0f*q1*q2); - if (fabsf(SH_BETA[0]) <= 1e-9f) { - return; - } - SH_BETA[1] = (ve - vwe)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2.0f*q0*q1 + 2.0f*q2*q3) - (vn - vwn)*(2.0f*q0*q3 - 2.0f*q1*q2); - SH_BETA[2] = vn - vwn; - SH_BETA[3] = ve - vwe; - SH_BETA[4] = 1.0f/sq(SH_BETA[0]); - SH_BETA[5] = 1.0f/SH_BETA[0]; - SH_BETA[6] = SH_BETA[5]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)); - SH_BETA[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3); - SH_BETA[8] = 2.0f*q0*SH_BETA[3] - 2.0f*q3*SH_BETA[2] + 2.0f*q1*vd; - SH_BETA[9] = 2.0f*q0*SH_BETA[2] + 2.0f*q3*SH_BETA[3] - 2.0f*q2*vd; - SH_BETA[10] = 2.0f*q2*SH_BETA[2] - 2.0f*q1*SH_BETA[3] + 2.0f*q0*vd; - SH_BETA[11] = 2.0f*q1*SH_BETA[2] + 2.0f*q2*SH_BETA[3] + 2.0f*q3*vd; - SH_BETA[12] = 2.0f*q0*q3; + SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2.0f*q0*q2 - 2.0f*q1*q3) + (ve - vwe)*(2.0f*q0*q3 + 2.0f*q1*q2); - H_BETA[0] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; - H_BETA[1] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; - H_BETA[2] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; - H_BETA[3] = - SH_BETA[5]*SH_BETA[9] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; - H_BETA[4] = - SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) - SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; - H_BETA[5] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2); - H_BETA[6] = SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3); - H_BETA[22] = SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; - H_BETA[23] = SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2) - SH_BETA[6]; - - for (uint8_t i=7; i<=21; i++) { - H_BETA[i] = 0.0f; - } - - // determine if we need the sideslip fusion to correct states other than wind - bool update_wind_only = ((_time_last_imu - _time_last_gps) < 1e6) || ((_time_last_imu - _time_last_ext_vision) < 1e6) || ((_time_last_imu - _time_last_optflow) < 1e6); - - // intermediate variables - note SK_BETA[0] is 1/(innovation variance) - _beta_innov_var = (R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][4]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][4]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][4]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][4]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][4]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][4]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][4]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][22]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][22]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][22]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][22]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][22]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][22]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][22]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2))*(P[22][5]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][5]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][5]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][5]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][5]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][5]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][5]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) - (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2))*(P[22][23]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][23]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][23]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][23]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][23]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][23]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][23]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9])*(P[22][0]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][0]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][0]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][0]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][0]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][0]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][0]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11])*(P[22][1]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][1]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][1]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][1]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][1]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][1]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][1]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10])*(P[22][2]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][2]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][2]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][2]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][2]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][2]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][2]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) - (SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][3]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][3]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][3]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][3]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][3]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][3]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][3]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))*(P[22][6]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][6]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][6]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][6]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][6]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][6]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][6]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3)))); - if (_beta_innov_var >= R_BETA) { - SK_BETA[0] = 1.0f / _beta_innov_var; - _fault_status.flags.bad_sideslip = false; - } else { // Reset the estimator - _fault_status.flags.bad_sideslip = true; - // if we are getting aiding from other sources, warn and reset the wind states and covariances only - if (update_wind_only) { - resetWindStates(); - resetWindCovariance(); - ECL_ERR("EKF synthetic sideslip fusion badly conditioned - wind covariance reset"); - - } else { - initialiseCovariance(); - _state.wind_vel.setZero(); - ECL_ERR("EKF synthetic sideslip fusion badly conditioned - full covariance reset"); - - } - return; - } - SK_BETA[1] = SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; - SK_BETA[2] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2); - SK_BETA[3] = SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3); - SK_BETA[4] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; - SK_BETA[5] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; - SK_BETA[6] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; - SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; - - // Calculate Kalman gains - if (update_wind_only) { - // If we are getting aiding from other sources, then don't allow the sideslip fusion to affect the non-windspeed states - for (unsigned row = 0; row <= 21; row++) { - Kfusion[row] = 0.0f; - - } - - } else { - Kfusion[0] = SK_BETA[0]*(P[0][0]*SK_BETA[5] + P[0][1]*SK_BETA[4] - P[0][4]*SK_BETA[1] + P[0][5]*SK_BETA[2] + P[0][2]*SK_BETA[6] + P[0][6]*SK_BETA[3] - P[0][3]*SK_BETA[7] + P[0][22]*SK_BETA[1] - P[0][23]*SK_BETA[2]); - Kfusion[1] = SK_BETA[0]*(P[1][0]*SK_BETA[5] + P[1][1]*SK_BETA[4] - P[1][4]*SK_BETA[1] + P[1][5]*SK_BETA[2] + P[1][2]*SK_BETA[6] + P[1][6]*SK_BETA[3] - P[1][3]*SK_BETA[7] + P[1][22]*SK_BETA[1] - P[1][23]*SK_BETA[2]); - Kfusion[2] = SK_BETA[0]*(P[2][0]*SK_BETA[5] + P[2][1]*SK_BETA[4] - P[2][4]*SK_BETA[1] + P[2][5]*SK_BETA[2] + P[2][2]*SK_BETA[6] + P[2][6]*SK_BETA[3] - P[2][3]*SK_BETA[7] + P[2][22]*SK_BETA[1] - P[2][23]*SK_BETA[2]); - Kfusion[3] = SK_BETA[0]*(P[3][0]*SK_BETA[5] + P[3][1]*SK_BETA[4] - P[3][4]*SK_BETA[1] + P[3][5]*SK_BETA[2] + P[3][2]*SK_BETA[6] + P[3][6]*SK_BETA[3] - P[3][3]*SK_BETA[7] + P[3][22]*SK_BETA[1] - P[3][23]*SK_BETA[2]); - Kfusion[4] = SK_BETA[0]*(P[4][0]*SK_BETA[5] + P[4][1]*SK_BETA[4] - P[4][4]*SK_BETA[1] + P[4][5]*SK_BETA[2] + P[4][2]*SK_BETA[6] + P[4][6]*SK_BETA[3] - P[4][3]*SK_BETA[7] + P[4][22]*SK_BETA[1] - P[4][23]*SK_BETA[2]); - Kfusion[5] = SK_BETA[0]*(P[5][0]*SK_BETA[5] + P[5][1]*SK_BETA[4] - P[5][4]*SK_BETA[1] + P[5][5]*SK_BETA[2] + P[5][2]*SK_BETA[6] + P[5][6]*SK_BETA[3] - P[5][3]*SK_BETA[7] + P[5][22]*SK_BETA[1] - P[5][23]*SK_BETA[2]); - Kfusion[6] = SK_BETA[0]*(P[6][0]*SK_BETA[5] + P[6][1]*SK_BETA[4] - P[6][4]*SK_BETA[1] + P[6][5]*SK_BETA[2] + P[6][2]*SK_BETA[6] + P[6][6]*SK_BETA[3] - P[6][3]*SK_BETA[7] + P[6][22]*SK_BETA[1] - P[6][23]*SK_BETA[2]); - Kfusion[7] = SK_BETA[0]*(P[7][0]*SK_BETA[5] + P[7][1]*SK_BETA[4] - P[7][4]*SK_BETA[1] + P[7][5]*SK_BETA[2] + P[7][2]*SK_BETA[6] + P[7][6]*SK_BETA[3] - P[7][3]*SK_BETA[7] + P[7][22]*SK_BETA[1] - P[7][23]*SK_BETA[2]); - Kfusion[8] = SK_BETA[0]*(P[8][0]*SK_BETA[5] + P[8][1]*SK_BETA[4] - P[8][4]*SK_BETA[1] + P[8][5]*SK_BETA[2] + P[8][2]*SK_BETA[6] + P[8][6]*SK_BETA[3] - P[8][3]*SK_BETA[7] + P[8][22]*SK_BETA[1] - P[8][23]*SK_BETA[2]); - Kfusion[9] = SK_BETA[0]*(P[9][0]*SK_BETA[5] + P[9][1]*SK_BETA[4] - P[9][4]*SK_BETA[1] + P[9][5]*SK_BETA[2] + P[9][2]*SK_BETA[6] + P[9][6]*SK_BETA[3] - P[9][3]*SK_BETA[7] + P[9][22]*SK_BETA[1] - P[9][23]*SK_BETA[2]); - Kfusion[10] = SK_BETA[0]*(P[10][0]*SK_BETA[5] + P[10][1]*SK_BETA[4] - P[10][4]*SK_BETA[1] + P[10][5]*SK_BETA[2] + P[10][2]*SK_BETA[6] + P[10][6]*SK_BETA[3] - P[10][3]*SK_BETA[7] + P[10][22]*SK_BETA[1] - P[10][23]*SK_BETA[2]); - Kfusion[11] = SK_BETA[0]*(P[11][0]*SK_BETA[5] + P[11][1]*SK_BETA[4] - P[11][4]*SK_BETA[1] + P[11][5]*SK_BETA[2] + P[11][2]*SK_BETA[6] + P[11][6]*SK_BETA[3] - P[11][3]*SK_BETA[7] + P[11][22]*SK_BETA[1] - P[11][23]*SK_BETA[2]); - Kfusion[12] = SK_BETA[0]*(P[12][0]*SK_BETA[5] + P[12][1]*SK_BETA[4] - P[12][4]*SK_BETA[1] + P[12][5]*SK_BETA[2] + P[12][2]*SK_BETA[6] + P[12][6]*SK_BETA[3] - P[12][3]*SK_BETA[7] + P[12][22]*SK_BETA[1] - P[12][23]*SK_BETA[2]); - Kfusion[13] = SK_BETA[0]*(P[13][0]*SK_BETA[5] + P[13][1]*SK_BETA[4] - P[13][4]*SK_BETA[1] + P[13][5]*SK_BETA[2] + P[13][2]*SK_BETA[6] + P[13][6]*SK_BETA[3] - P[13][3]*SK_BETA[7] + P[13][22]*SK_BETA[1] - P[13][23]*SK_BETA[2]); - Kfusion[14] = SK_BETA[0]*(P[14][0]*SK_BETA[5] + P[14][1]*SK_BETA[4] - P[14][4]*SK_BETA[1] + P[14][5]*SK_BETA[2] + P[14][2]*SK_BETA[6] + P[14][6]*SK_BETA[3] - P[14][3]*SK_BETA[7] + P[14][22]*SK_BETA[1] - P[14][23]*SK_BETA[2]); - Kfusion[15] = SK_BETA[0]*(P[15][0]*SK_BETA[5] + P[15][1]*SK_BETA[4] - P[15][4]*SK_BETA[1] + P[15][5]*SK_BETA[2] + P[15][2]*SK_BETA[6] + P[15][6]*SK_BETA[3] - P[15][3]*SK_BETA[7] + P[15][22]*SK_BETA[1] - P[15][23]*SK_BETA[2]); - - // Only update the magnetometer states if we are airborne and using 3D mag fusion - if (_control_status.flags.mag_3D && _control_status.flags.in_air) { - Kfusion[16] = SK_BETA[0]*(P[16][0]*SK_BETA[5] + P[16][1]*SK_BETA[4] - P[16][4]*SK_BETA[1] + P[16][5]*SK_BETA[2] + P[16][2]*SK_BETA[6] + P[16][6]*SK_BETA[3] - P[16][3]*SK_BETA[7] + P[16][22]*SK_BETA[1] - P[16][23]*SK_BETA[2]); - Kfusion[17] = SK_BETA[0]*(P[17][0]*SK_BETA[5] + P[17][1]*SK_BETA[4] - P[17][4]*SK_BETA[1] + P[17][5]*SK_BETA[2] + P[17][2]*SK_BETA[6] + P[17][6]*SK_BETA[3] - P[17][3]*SK_BETA[7] + P[17][22]*SK_BETA[1] - P[17][23]*SK_BETA[2]); - Kfusion[18] = SK_BETA[0]*(P[18][0]*SK_BETA[5] + P[18][1]*SK_BETA[4] - P[18][4]*SK_BETA[1] + P[18][5]*SK_BETA[2] + P[18][2]*SK_BETA[6] + P[18][6]*SK_BETA[3] - P[18][3]*SK_BETA[7] + P[18][22]*SK_BETA[1] - P[18][23]*SK_BETA[2]); - Kfusion[19] = SK_BETA[0]*(P[19][0]*SK_BETA[5] + P[19][1]*SK_BETA[4] - P[19][4]*SK_BETA[1] + P[19][5]*SK_BETA[2] + P[19][2]*SK_BETA[6] + P[19][6]*SK_BETA[3] - P[19][3]*SK_BETA[7] + P[19][22]*SK_BETA[1] - P[19][23]*SK_BETA[2]); - Kfusion[20] = SK_BETA[0]*(P[20][0]*SK_BETA[5] + P[20][1]*SK_BETA[4] - P[20][4]*SK_BETA[1] + P[20][5]*SK_BETA[2] + P[20][2]*SK_BETA[6] + P[20][6]*SK_BETA[3] - P[20][3]*SK_BETA[7] + P[20][22]*SK_BETA[1] - P[20][23]*SK_BETA[2]); - Kfusion[21] = SK_BETA[0]*(P[21][0]*SK_BETA[5] + P[21][1]*SK_BETA[4] - P[21][4]*SK_BETA[1] + P[21][5]*SK_BETA[2] + P[21][2]*SK_BETA[6] + P[21][6]*SK_BETA[3] - P[21][3]*SK_BETA[7] + P[21][22]*SK_BETA[1] - P[21][23]*SK_BETA[2]); - - } else { - for (int i = 16; i <= 21; i++) { - Kfusion[i] = 0.0f; - - } - } - } - Kfusion[22] = SK_BETA[0]*(P[22][0]*SK_BETA[5] + P[22][1]*SK_BETA[4] - P[22][4]*SK_BETA[1] + P[22][5]*SK_BETA[2] + P[22][2]*SK_BETA[6] + P[22][6]*SK_BETA[3] - P[22][3]*SK_BETA[7] + P[22][22]*SK_BETA[1] - P[22][23]*SK_BETA[2]); - Kfusion[23] = SK_BETA[0]*(P[23][0]*SK_BETA[5] + P[23][1]*SK_BETA[4] - P[23][4]*SK_BETA[1] + P[23][5]*SK_BETA[2] + P[23][2]*SK_BETA[6] + P[23][6]*SK_BETA[3] - P[23][3]*SK_BETA[7] + P[23][22]*SK_BETA[1] - P[23][23]*SK_BETA[2]); - - // Calculate predicted sideslip angle and innovation using small angle approximation - _beta_innov = rel_wind(1) / rel_wind(0); - - // Compute the ratio of innovation to gate size - _beta_test_ratio = sq(_beta_innov) / (sq(fmaxf(_params.beta_innov_gate, 1.0f)) * _beta_innov_var); - - // if the innovation consistency check fails then don't fuse the sample and indicate bad beta health - if (_beta_test_ratio > 1.0f) { - _innov_check_fail_status.flags.reject_sideslip = true; - return; - - } else { - _innov_check_fail_status.flags.reject_sideslip = false; - - } - - // synthetic sideslip measurement sample has passed check so record it - _time_last_beta_fuse = _time_last_imu; - - // apply covariance correction via P_new = (I -K*H)*P - // first calculate expression for KHP - // then calculate P - KHP - float KHP[_k_num_states][_k_num_states]; - float KH[9]; - for (unsigned row = 0; row < _k_num_states; row++) { - - KH[0] = Kfusion[row] * H_BETA[0]; - KH[1] = Kfusion[row] * H_BETA[1]; - KH[2] = Kfusion[row] * H_BETA[2]; - KH[3] = Kfusion[row] * H_BETA[3]; - KH[4] = Kfusion[row] * H_BETA[4]; - KH[5] = Kfusion[row] * H_BETA[5]; - KH[6] = Kfusion[row] * H_BETA[6]; - KH[7] = Kfusion[row] * H_BETA[22]; - KH[8] = Kfusion[row] * H_BETA[23]; - - for (unsigned column = 0; column < _k_num_states; column++) { - float tmp = KH[0] * P[0][column]; - tmp += KH[1] * P[1][column]; - tmp += KH[2] * P[2][column]; - tmp += KH[3] * P[3][column]; - tmp += KH[4] * P[4][column]; - tmp += KH[5] * P[5][column]; - tmp += KH[6] * P[6][column]; - tmp += KH[7] * P[22][column]; - tmp += KH[8] * P[23][column]; - KHP[row][column] = tmp; + if (fabsf(SH_BETA[0]) <= 1e-9f) { + return; } - } - // if the covariance correction will result in a negative variance, then - // the covariance marix is unhealthy and must be corrected - bool healthy = true; - _fault_status.flags.bad_sideslip = false; - for (int i = 0; i < _k_num_states; i++) { - if (P[i][i] < KHP[i][i]) { - // zero rows and columns - zeroRows(P,i,i); - zeroCols(P,i,i); + SH_BETA[1] = (ve - vwe)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2.0f*q0*q1 + 2.0f*q2*q3) - (vn - vwn)*(2.0f*q0*q3 - 2.0f*q1*q2); + SH_BETA[2] = vn - vwn; + SH_BETA[3] = ve - vwe; + SH_BETA[4] = 1.0f/sq(SH_BETA[0]); + SH_BETA[5] = 1.0f/SH_BETA[0]; + SH_BETA[6] = SH_BETA[5]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)); + SH_BETA[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3); + SH_BETA[8] = 2.0f*q0*SH_BETA[3] - 2.0f*q3*SH_BETA[2] + 2.0f*q1*vd; + SH_BETA[9] = 2.0f*q0*SH_BETA[2] + 2.0f*q3*SH_BETA[3] - 2.0f*q2*vd; + SH_BETA[10] = 2.0f*q2*SH_BETA[2] - 2.0f*q1*SH_BETA[3] + 2.0f*q0*vd; + SH_BETA[11] = 2.0f*q1*SH_BETA[2] + 2.0f*q2*SH_BETA[3] + 2.0f*q3*vd; + SH_BETA[12] = 2.0f*q0*q3; - //flag as unhealthy - healthy = false; + H_BETA[0] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; + H_BETA[1] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; + H_BETA[2] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; + H_BETA[3] = - SH_BETA[5]*SH_BETA[9] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; + H_BETA[4] = - SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) - SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; + H_BETA[5] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2); + H_BETA[6] = SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3); + H_BETA[22] = SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; + H_BETA[23] = SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2) - SH_BETA[6]; - // update individual measurement health status + for (uint8_t i = 7; i <= 21; i++) { + H_BETA[i] = 0.0f; + } + + // determine if we need the sideslip fusion to correct states other than wind + bool update_wind_only = ((_time_last_imu - _time_last_gps) < 1e6) || ((_time_last_imu - _time_last_ext_vision) < 1e6) || ((_time_last_imu - _time_last_optflow) < 1e6); + + // intermediate variables - note SK_BETA[0] is 1/(innovation variance) + _beta_innov_var = (R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][4]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][4]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][4]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][4]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][4]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][4]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][4]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][22]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][22]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][22]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][22]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][22]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][22]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][22]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2))*(P[22][5]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][5]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][5]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][5]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][5]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][5]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][5]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) - (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2))*(P[22][23]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][23]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][23]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][23]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][23]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][23]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][23]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9])*(P[22][0]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][0]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][0]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][0]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][0]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][0]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][0]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11])*(P[22][1]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][1]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][1]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][1]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][1]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][1]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][1]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10])*(P[22][2]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][2]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][2]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][2]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][2]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][2]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][2]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) - (SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][3]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][3]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][3]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][3]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][3]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][3]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][3]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))*(P[22][6]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][6]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][6]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][6]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][6]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][6]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][6]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3)))); + if (_beta_innov_var >= R_BETA) { + SK_BETA[0] = 1.0f / _beta_innov_var; + _fault_status.flags.bad_sideslip = false; + + } else { // Reset the estimator _fault_status.flags.bad_sideslip = true; - } - } + // if we are getting aiding from other sources, warn and reset the wind states and covariances only + if (update_wind_only) { + resetWindStates(); + resetWindCovariance(); + ECL_ERR("EKF synthetic sideslip fusion badly conditioned - wind covariance reset"); - // only apply covariance and state corrrections if healthy - if (healthy) { - // apply the covariance corrections - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P[row][column] = P[row][column] - KHP[row][column]; + } else { + initialiseCovariance(); + _state.wind_vel.setZero(); + ECL_ERR("EKF synthetic sideslip fusion badly conditioned - full covariance reset"); + } + + return; + } + + SK_BETA[1] = SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; + SK_BETA[2] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2); + SK_BETA[3] = SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3); + SK_BETA[4] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; + SK_BETA[5] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; + SK_BETA[6] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; + SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; + + // Calculate Kalman gains + if (update_wind_only) { + // If we are getting aiding from other sources, then don't allow the sideslip fusion to affect the non-windspeed states + for (unsigned row = 0; row <= 21; row++) { + Kfusion[row] = 0.0f; + + } + + } else { + Kfusion[0] = SK_BETA[0]*(P[0][0]*SK_BETA[5] + P[0][1]*SK_BETA[4] - P[0][4]*SK_BETA[1] + P[0][5]*SK_BETA[2] + P[0][2]*SK_BETA[6] + P[0][6]*SK_BETA[3] - P[0][3]*SK_BETA[7] + P[0][22]*SK_BETA[1] - P[0][23]*SK_BETA[2]); + Kfusion[1] = SK_BETA[0]*(P[1][0]*SK_BETA[5] + P[1][1]*SK_BETA[4] - P[1][4]*SK_BETA[1] + P[1][5]*SK_BETA[2] + P[1][2]*SK_BETA[6] + P[1][6]*SK_BETA[3] - P[1][3]*SK_BETA[7] + P[1][22]*SK_BETA[1] - P[1][23]*SK_BETA[2]); + Kfusion[2] = SK_BETA[0]*(P[2][0]*SK_BETA[5] + P[2][1]*SK_BETA[4] - P[2][4]*SK_BETA[1] + P[2][5]*SK_BETA[2] + P[2][2]*SK_BETA[6] + P[2][6]*SK_BETA[3] - P[2][3]*SK_BETA[7] + P[2][22]*SK_BETA[1] - P[2][23]*SK_BETA[2]); + Kfusion[3] = SK_BETA[0]*(P[3][0]*SK_BETA[5] + P[3][1]*SK_BETA[4] - P[3][4]*SK_BETA[1] + P[3][5]*SK_BETA[2] + P[3][2]*SK_BETA[6] + P[3][6]*SK_BETA[3] - P[3][3]*SK_BETA[7] + P[3][22]*SK_BETA[1] - P[3][23]*SK_BETA[2]); + Kfusion[4] = SK_BETA[0]*(P[4][0]*SK_BETA[5] + P[4][1]*SK_BETA[4] - P[4][4]*SK_BETA[1] + P[4][5]*SK_BETA[2] + P[4][2]*SK_BETA[6] + P[4][6]*SK_BETA[3] - P[4][3]*SK_BETA[7] + P[4][22]*SK_BETA[1] - P[4][23]*SK_BETA[2]); + Kfusion[5] = SK_BETA[0]*(P[5][0]*SK_BETA[5] + P[5][1]*SK_BETA[4] - P[5][4]*SK_BETA[1] + P[5][5]*SK_BETA[2] + P[5][2]*SK_BETA[6] + P[5][6]*SK_BETA[3] - P[5][3]*SK_BETA[7] + P[5][22]*SK_BETA[1] - P[5][23]*SK_BETA[2]); + Kfusion[6] = SK_BETA[0]*(P[6][0]*SK_BETA[5] + P[6][1]*SK_BETA[4] - P[6][4]*SK_BETA[1] + P[6][5]*SK_BETA[2] + P[6][2]*SK_BETA[6] + P[6][6]*SK_BETA[3] - P[6][3]*SK_BETA[7] + P[6][22]*SK_BETA[1] - P[6][23]*SK_BETA[2]); + Kfusion[7] = SK_BETA[0]*(P[7][0]*SK_BETA[5] + P[7][1]*SK_BETA[4] - P[7][4]*SK_BETA[1] + P[7][5]*SK_BETA[2] + P[7][2]*SK_BETA[6] + P[7][6]*SK_BETA[3] - P[7][3]*SK_BETA[7] + P[7][22]*SK_BETA[1] - P[7][23]*SK_BETA[2]); + Kfusion[8] = SK_BETA[0]*(P[8][0]*SK_BETA[5] + P[8][1]*SK_BETA[4] - P[8][4]*SK_BETA[1] + P[8][5]*SK_BETA[2] + P[8][2]*SK_BETA[6] + P[8][6]*SK_BETA[3] - P[8][3]*SK_BETA[7] + P[8][22]*SK_BETA[1] - P[8][23]*SK_BETA[2]); + Kfusion[9] = SK_BETA[0]*(P[9][0]*SK_BETA[5] + P[9][1]*SK_BETA[4] - P[9][4]*SK_BETA[1] + P[9][5]*SK_BETA[2] + P[9][2]*SK_BETA[6] + P[9][6]*SK_BETA[3] - P[9][3]*SK_BETA[7] + P[9][22]*SK_BETA[1] - P[9][23]*SK_BETA[2]); + Kfusion[10] = SK_BETA[0]*(P[10][0]*SK_BETA[5] + P[10][1]*SK_BETA[4] - P[10][4]*SK_BETA[1] + P[10][5]*SK_BETA[2] + P[10][2]*SK_BETA[6] + P[10][6]*SK_BETA[3] - P[10][3]*SK_BETA[7] + P[10][22]*SK_BETA[1] - P[10][23]*SK_BETA[2]); + Kfusion[11] = SK_BETA[0]*(P[11][0]*SK_BETA[5] + P[11][1]*SK_BETA[4] - P[11][4]*SK_BETA[1] + P[11][5]*SK_BETA[2] + P[11][2]*SK_BETA[6] + P[11][6]*SK_BETA[3] - P[11][3]*SK_BETA[7] + P[11][22]*SK_BETA[1] - P[11][23]*SK_BETA[2]); + Kfusion[12] = SK_BETA[0]*(P[12][0]*SK_BETA[5] + P[12][1]*SK_BETA[4] - P[12][4]*SK_BETA[1] + P[12][5]*SK_BETA[2] + P[12][2]*SK_BETA[6] + P[12][6]*SK_BETA[3] - P[12][3]*SK_BETA[7] + P[12][22]*SK_BETA[1] - P[12][23]*SK_BETA[2]); + Kfusion[13] = SK_BETA[0]*(P[13][0]*SK_BETA[5] + P[13][1]*SK_BETA[4] - P[13][4]*SK_BETA[1] + P[13][5]*SK_BETA[2] + P[13][2]*SK_BETA[6] + P[13][6]*SK_BETA[3] - P[13][3]*SK_BETA[7] + P[13][22]*SK_BETA[1] - P[13][23]*SK_BETA[2]); + Kfusion[14] = SK_BETA[0]*(P[14][0]*SK_BETA[5] + P[14][1]*SK_BETA[4] - P[14][4]*SK_BETA[1] + P[14][5]*SK_BETA[2] + P[14][2]*SK_BETA[6] + P[14][6]*SK_BETA[3] - P[14][3]*SK_BETA[7] + P[14][22]*SK_BETA[1] - P[14][23]*SK_BETA[2]); + Kfusion[15] = SK_BETA[0]*(P[15][0]*SK_BETA[5] + P[15][1]*SK_BETA[4] - P[15][4]*SK_BETA[1] + P[15][5]*SK_BETA[2] + P[15][2]*SK_BETA[6] + P[15][6]*SK_BETA[3] - P[15][3]*SK_BETA[7] + P[15][22]*SK_BETA[1] - P[15][23]*SK_BETA[2]); + + // Only update the magnetometer states if we are airborne and using 3D mag fusion + if (_control_status.flags.mag_3D && _control_status.flags.in_air) { + Kfusion[16] = SK_BETA[0]*(P[16][0]*SK_BETA[5] + P[16][1]*SK_BETA[4] - P[16][4]*SK_BETA[1] + P[16][5]*SK_BETA[2] + P[16][2]*SK_BETA[6] + P[16][6]*SK_BETA[3] - P[16][3]*SK_BETA[7] + P[16][22]*SK_BETA[1] - P[16][23]*SK_BETA[2]); + Kfusion[17] = SK_BETA[0]*(P[17][0]*SK_BETA[5] + P[17][1]*SK_BETA[4] - P[17][4]*SK_BETA[1] + P[17][5]*SK_BETA[2] + P[17][2]*SK_BETA[6] + P[17][6]*SK_BETA[3] - P[17][3]*SK_BETA[7] + P[17][22]*SK_BETA[1] - P[17][23]*SK_BETA[2]); + Kfusion[18] = SK_BETA[0]*(P[18][0]*SK_BETA[5] + P[18][1]*SK_BETA[4] - P[18][4]*SK_BETA[1] + P[18][5]*SK_BETA[2] + P[18][2]*SK_BETA[6] + P[18][6]*SK_BETA[3] - P[18][3]*SK_BETA[7] + P[18][22]*SK_BETA[1] - P[18][23]*SK_BETA[2]); + Kfusion[19] = SK_BETA[0]*(P[19][0]*SK_BETA[5] + P[19][1]*SK_BETA[4] - P[19][4]*SK_BETA[1] + P[19][5]*SK_BETA[2] + P[19][2]*SK_BETA[6] + P[19][6]*SK_BETA[3] - P[19][3]*SK_BETA[7] + P[19][22]*SK_BETA[1] - P[19][23]*SK_BETA[2]); + Kfusion[20] = SK_BETA[0]*(P[20][0]*SK_BETA[5] + P[20][1]*SK_BETA[4] - P[20][4]*SK_BETA[1] + P[20][5]*SK_BETA[2] + P[20][2]*SK_BETA[6] + P[20][6]*SK_BETA[3] - P[20][3]*SK_BETA[7] + P[20][22]*SK_BETA[1] - P[20][23]*SK_BETA[2]); + Kfusion[21] = SK_BETA[0]*(P[21][0]*SK_BETA[5] + P[21][1]*SK_BETA[4] - P[21][4]*SK_BETA[1] + P[21][5]*SK_BETA[2] + P[21][2]*SK_BETA[6] + P[21][6]*SK_BETA[3] - P[21][3]*SK_BETA[7] + P[21][22]*SK_BETA[1] - P[21][23]*SK_BETA[2]); + + } else { + for (int i = 16; i <= 21; i++) { + Kfusion[i] = 0.0f; + + } } } - // correct the covariance marix for gross errors - fixCovarianceErrors(); + Kfusion[22] = SK_BETA[0]*(P[22][0]*SK_BETA[5] + P[22][1]*SK_BETA[4] - P[22][4]*SK_BETA[1] + P[22][5]*SK_BETA[2] + P[22][2]*SK_BETA[6] + P[22][6]*SK_BETA[3] - P[22][3]*SK_BETA[7] + P[22][22]*SK_BETA[1] - P[22][23]*SK_BETA[2]); + Kfusion[23] = SK_BETA[0]*(P[23][0]*SK_BETA[5] + P[23][1]*SK_BETA[4] - P[23][4]*SK_BETA[1] + P[23][5]*SK_BETA[2] + P[23][2]*SK_BETA[6] + P[23][6]*SK_BETA[3] - P[23][3]*SK_BETA[7] + P[23][22]*SK_BETA[1] - P[23][23]*SK_BETA[2]); - // apply the state corrections - fuse(Kfusion, _beta_innov); + // Calculate predicted sideslip angle and innovation using small angle approximation + _beta_innov = rel_wind(1) / rel_wind(0); - } - } + // Compute the ratio of innovation to gate size + _beta_test_ratio = sq(_beta_innov) / (sq(fmaxf(_params.beta_innov_gate, 1.0f)) * _beta_innov_var); + + // if the innovation consistency check fails then don't fuse the sample and indicate bad beta health + if (_beta_test_ratio > 1.0f) { + _innov_check_fail_status.flags.reject_sideslip = true; + return; + + } else { + _innov_check_fail_status.flags.reject_sideslip = false; + } + + // synthetic sideslip measurement sample has passed check so record it + _time_last_beta_fuse = _time_last_imu; + + // apply covariance correction via P_new = (I -K*H)*P + // first calculate expression for KHP + // then calculate P - KHP + float KHP[_k_num_states][_k_num_states]; + float KH[9]; + + for (unsigned row = 0; row < _k_num_states; row++) { + KH[0] = Kfusion[row] * H_BETA[0]; + KH[1] = Kfusion[row] * H_BETA[1]; + KH[2] = Kfusion[row] * H_BETA[2]; + KH[3] = Kfusion[row] * H_BETA[3]; + KH[4] = Kfusion[row] * H_BETA[4]; + KH[5] = Kfusion[row] * H_BETA[5]; + KH[6] = Kfusion[row] * H_BETA[6]; + KH[7] = Kfusion[row] * H_BETA[22]; + KH[8] = Kfusion[row] * H_BETA[23]; + + for (unsigned column = 0; column < _k_num_states; column++) { + float tmp = KH[0] * P[0][column]; + tmp += KH[1] * P[1][column]; + tmp += KH[2] * P[2][column]; + tmp += KH[3] * P[3][column]; + tmp += KH[4] * P[4][column]; + tmp += KH[5] * P[5][column]; + tmp += KH[6] * P[6][column]; + tmp += KH[7] * P[22][column]; + tmp += KH[8] * P[23][column]; + KHP[row][column] = tmp; + } + } + + // if the covariance correction will result in a negative variance, then + // the covariance marix is unhealthy and must be corrected + bool healthy = true; + _fault_status.flags.bad_sideslip = false; + + for (int i = 0; i < _k_num_states; i++) { + if (P[i][i] < KHP[i][i]) { + // zero rows and columns + zeroRows(P, i, i); + zeroCols(P, i, i); + + //flag as unhealthy + healthy = false; + + // update individual measurement health status + _fault_status.flags.bad_sideslip = true; + } + } + + // only apply covariance and state corrrections if healthy + if (healthy) { + // apply the covariance corrections + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + P[row][column] = P[row][column] - KHP[row][column]; + } + } + + // correct the covariance marix for gross errors + fixCovarianceErrors(); + + // apply the state corrections + fuse(Kfusion, _beta_innov); + } + } } diff --git a/ecl.h b/ecl.h index bca60865e9..cf9c32572b 100644 --- a/ecl.h +++ b/ecl.h @@ -56,3 +56,17 @@ #define ECL_ERR printf #endif + +#ifndef __PX4_QURT +#if defined(__cplusplus) && !defined(__PX4_NUTTX) +#include +#define ISFINITE(x) std::isfinite(x) +#else +#define ISFINITE(x) isfinite(x) +#endif +#endif + +#if defined(__PX4_QURT) +// Missing math.h defines +#define ISFINITE(x) __builtin_isfinite(x) +#endif