From 5242af84af0033f39709e3d42f0d0c84761c2b52 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 4 Mar 2016 17:58:11 +1100 Subject: [PATCH] EKF: Standardise covariance update and use static arrays for large matrices --- EKF/ekf.h | 2 ++ EKF/mag_fusion.cpp | 65 +++++++++++++++++++++++++--------------------- 2 files changed, 38 insertions(+), 29 deletions(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index 38b0cfe716..a44ac191c3 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -127,6 +127,8 @@ private: matrix::Dcm _R_prev; // transformation matrix from earth frame to body frame of previous ekf step float P[_k_num_states][_k_num_states]; // state covariance matrix + float KH[_k_num_states][_k_num_states]; // intermediate variable for the covariance update + float KHP[_k_num_states][_k_num_states]; // intermediate variable for the covariance update float _vel_pos_innov[6]; // innovations: 0-2 vel, 3-5 pos float _mag_innov[3]; // earth magnetic field innovations diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index e9ece63384..fc08f3817f 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -443,21 +443,17 @@ void Ekf::fuseMag() // 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] = {}; - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < 3; column++) { + for (unsigned column = 0; column <= 2; column++) { KH[row][column] = Kfusion[row] * H_MAG[index][column]; } - for (unsigned column = 16; column < 22; column++) { + for (unsigned column = 16; column <= 21; 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]; @@ -723,18 +719,27 @@ void Ekf::fuseHeading() _state.quat_nominal = dq * _state.quat_nominal; _state.quat_nominal.normalize(); - // update the covariance matrix taking advantage of the reduced size of H_YAW - float HP[_k_num_states] = {}; - - for (unsigned column = 0; column < _k_num_states; column++) { - for (unsigned row = 1; row <= 2; row++) { - HP[column] += H_YAW[row] * P[row][column]; + // apply covariance correction via P_new = (I -K*H)*P + // first calculate expression for KHP + // then calculate P - KHP + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column <= 2; column++) { + KH[row][column] = Kfusion[row] * H_YAW[column]; } } for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned column = 0; column < _k_num_states; column++) { - P[row][column] -= Kfusion[row] * HP[column]; + float tmp = KH[row][0] * P[0][column]; + tmp += KH[row][1] * P[1][column]; + tmp += KH[row][2] * P[2][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]; } } @@ -849,16 +854,12 @@ void Ekf::fuseDeclination() // first calculate expression for KHP // then calculate P - KHP // take advantage of the empty columns in KH to reduce the number of operations - float KH[_k_num_states][_k_num_states] = {}; - for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned column = 16; column <= 17; column++) { KH[row][column] = Kfusion[row] * H_DECL[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][16] * P[16][column]; @@ -1047,21 +1048,27 @@ void Ekf::fuseMag2D() _state.quat_nominal.normalize(); _state.ang_error.setZero(); - // correct the covariance using P = P - K*H*P taking advantage of the fact that only the first 3 elements in H are non zero - // and we only need the first 16 states - float HP[16]; - - for (uint8_t col = 0; col < 16; col++) { - HP[col] = 0.0f; - - for (uint8_t row = 0; row <= 2; row++) { - HP[col] += H_DECL[row] * P[row][col]; + // apply covariance correction via P_new = (I -K*H)*P + // first calculate expression for KHP + // then calculate P - KHP + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column <= 2; column++) { + KH[row][column] = Kfusion[row] * H_DECL[column]; } } - for (uint8_t row = 0; row < 16; row++) { - for (uint8_t col = 0; col < 16; col++) { - P[row][col] -= Kfusion[row] * HP[col]; + 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]; + 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]; } }