From 1b59a89a18f23a9cc4a8eb9fba76db644f9a2e81 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 2 Nov 2016 17:47:18 +1100 Subject: [PATCH] EKF: save some stack memory --- EKF/airspeed_fusion.cpp | 1 + EKF/ekf.h | 1 - EKF/mag_fusion.cpp | 3 +++ EKF/optflow_fusion.cpp | 1 + 4 files changed, 5 insertions(+), 1 deletion(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index c6e8432a1e..24aa6fbc5a 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -161,6 +161,7 @@ void Ekf::fuseAirspeed() // 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++) { KH[row][4] = Kfusion[row] * H_TAS[4]; KH[row][5] = Kfusion[row] * H_TAS[5]; diff --git a/EKF/ekf.h b/EKF/ekf.h index 19a7c31e24..f254ee1cec 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -238,7 +238,6 @@ private: matrix::Dcm _R_to_earth; // transformation matrix from body frame to earth frame from last EKF predition 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 _vel_pos_innov[6]; // innovations: 0-2 vel, 3-5 pos float _vel_pos_innov_var[6]; // innovation variances: 0-2 vel, 3-5 pos diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 9d6035d113..f413dbe36d 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -309,6 +309,7 @@ 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++) { KH[row][column] = Kfusion[row] * H_MAG[column]; @@ -638,6 +639,7 @@ void Ekf::fuseHeading() // 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++) { KH[row][column] = Kfusion[row] * H_YAW[column]; @@ -775,6 +777,7 @@ 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]; diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 4e5b263289..dead4dd7ae 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -433,6 +433,7 @@ void Ekf::fuseOptFlow() // 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 <= 6; column++) { KH[row][column] = gain[row] * H_LOS[obs_index][column];