From cef2ba5ab9fbc3b36c09181f7a9170d8c255fb8b Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 8 May 2019 12:10:08 +0200 Subject: [PATCH] implemented Kahan summation algorithm for adding process noise to delta angle- and delta velocity bias variance - the contribution of process noise per iteration for these states can be so small that it gets lost if using standard floating point summation Signed-off-by: Roman --- EKF/CMakeLists.txt | 2 ++ EKF/covariance.cpp | 18 ++++++++++++++++-- EKF/ekf.h | 9 +++++++++ EKF/ekf_helper.cpp | 8 ++++++++ 4 files changed, 35 insertions(+), 2 deletions(-) diff --git a/EKF/CMakeLists.txt b/EKF/CMakeLists.txt index da1b71096e..7a736db45b 100644 --- a/EKF/CMakeLists.txt +++ b/EKF/CMakeLists.txt @@ -55,6 +55,8 @@ target_link_libraries(ecl_EKF PRIVATE ecl_geo ecl_geo_lookup mathlib) set_target_properties(ecl_EKF PROPERTIES PUBLIC_HEADER "ekf.h") +target_compile_options(ecl_EKF PRIVATE -fno-associative-math) + if(EKF_PYTHON_TESTS) add_subdirectory(swig) endif() diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 63307b9583..7af7e39332 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -54,6 +54,9 @@ void Ekf::initialiseCovariance() } } + _delta_angle_bias_var_accum.setZero(); + _delta_vel_bias_var_accum.setZero(); + // calculate average prediction time step in sec float dt = FILTER_UPDATE_PERIOD_S; @@ -430,10 +433,17 @@ void Ekf::predictCovariance() nextP[12][12] = P[12][12]; // add process noise that is not from the IMU - for (unsigned i = 0; i <= 12; i++) { + for (unsigned i = 0; i <= 9; i++) { nextP[i][i] += process_noise[i]; } + // process noise contribution for delta angle states can be very small compared to + // the variances, therefore use algorithm to minimise numerical error + for (unsigned i = 10; i <=12; i++) { + const int index = i-10; + nextP[i][i] = kahanSummation(nextP[i][i], process_noise[i], _delta_angle_bias_var_accum(index)); + } + // Don't calculate these covariance terms if IMU delta velocity bias estimation is inhibited if (!(_params.fusion_mode & MASK_INHIBIT_ACC_BIAS) && !_accel_bias_inhibit) { @@ -485,14 +495,18 @@ void Ekf::predictCovariance() nextP[15][15] = P[15][15]; // add process noise that is not from the IMU + // process noise contributiton for delta velocity states can be very small compared to + // the variances, therefore use algorithm to minimise numerical error for (unsigned i = 13; i <= 15; i++) { - nextP[i][i] += process_noise[i]; + const int index = i-13; + nextP[i][i] = kahanSummation(nextP[i][i], process_noise[i], _delta_vel_bias_var_accum(index)); } } else { // Inhibit delta velocity bias learning by zeroing the covariance terms zeroRows(nextP, 13, 15); zeroCols(nextP, 13, 15); + _delta_vel_bias_var_accum.setZero(); } // Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion diff --git a/EKF/ekf.h b/EKF/ekf.h index a5c95b1a55..8815cd8dd3 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -363,6 +363,9 @@ private: float P[_k_num_states][_k_num_states] {}; ///< state covariance matrix + Vector3f _delta_vel_bias_var_accum; ///< kahan summation algorithm accumulator for delta velocity bias variance + Vector3f _delta_angle_bias_var_accum; ///< kahan summation algorithm accumulator for delta angle bias variance + float _vel_pos_innov[6] {}; ///< NED velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m) float _vel_pos_innov_var[6] {}; ///< NED velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2) float _aux_vel_innov[2] {}; ///< NE auxiliary velocity innovations: (m/sec) @@ -707,4 +710,10 @@ private: // uncorrelate quaternion states from other states void uncorrelateQuatStates(); + // Use Kahan summation algorithm to get the sum of "sum_previous" and "input". + // This function relies on the caller to be responsible for keeping a copy of + // "accumulator" and passing this value at the next iteration. + // Ref: https://en.wikipedia.org/wiki/Kahan_summation_algorithm + float kahanSummation(float sum_previous, float input, float &accumulator) const; + }; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 52d42e43ba..4313c1443e 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1722,3 +1722,11 @@ void Ekf::save_mag_cov_data() } } } + +float Ekf::kahanSummation(float sum_previous, float input, float &accumulator) const +{ + float y = input - accumulator; + float t = sum_previous + y; + accumulator = (t - sum_previous) - y; + return t; +}