From d1ecfad4cd51d18d6f2b51382a07707ea4ea0f59 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 5 Feb 2015 13:06:37 +0100 Subject: EKFAttPos: Enforce type safety --- .../ekf_att_pos_estimator_main.cpp | 48 +++--- .../ekf_att_pos_estimator/estimator_22states.cpp | 170 ++++++++++----------- .../ekf_att_pos_estimator/estimator_22states.h | 41 ++--- 3 files changed, 130 insertions(+), 129 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 62965976d..eabef2798 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -113,22 +113,22 @@ uint64_t getMicros() return IMUusec; } -class FixedwingEstimator +class AttitudePositionEstimatorEKF { public: /** * Constructor */ - FixedwingEstimator(); + AttitudePositionEstimatorEKF(); /* we do not want people ever copying this class */ - FixedwingEstimator(const FixedwingEstimator& that) = delete; - FixedwingEstimator operator=(const FixedwingEstimator&) = delete; + AttitudePositionEstimatorEKF(const AttitudePositionEstimatorEKF& that) = delete; + AttitudePositionEstimatorEKF operator=(const AttitudePositionEstimatorEKF&) = delete; /** * Destructor, also kills the sensors task. */ - ~FixedwingEstimator(); + ~AttitudePositionEstimatorEKF(); /** * Start the sensors task. @@ -338,10 +338,10 @@ namespace estimator #endif static const int ERROR = -1; -FixedwingEstimator *g_estimator = nullptr; +AttitudePositionEstimatorEKF *g_estimator = nullptr; } -FixedwingEstimator::FixedwingEstimator() : +AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _task_should_exit(false), _task_running(false), @@ -498,7 +498,7 @@ FixedwingEstimator::FixedwingEstimator() : } } -FixedwingEstimator::~FixedwingEstimator() +AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() { if (_estimator_task != -1) { @@ -526,7 +526,7 @@ FixedwingEstimator::~FixedwingEstimator() } int -FixedwingEstimator::enable_logging(bool logging) +AttitudePositionEstimatorEKF::enable_logging(bool logging) { _ekf_logging = logging; @@ -534,7 +534,7 @@ FixedwingEstimator::enable_logging(bool logging) } int -FixedwingEstimator::parameters_update() +AttitudePositionEstimatorEKF::parameters_update() { param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms)); @@ -579,7 +579,7 @@ FixedwingEstimator::parameters_update() } void -FixedwingEstimator::vehicle_status_poll() +AttitudePositionEstimatorEKF::vehicle_status_poll() { bool vstatus_updated; @@ -593,7 +593,7 @@ FixedwingEstimator::vehicle_status_poll() } int -FixedwingEstimator::check_filter_state() +AttitudePositionEstimatorEKF::check_filter_state() { /* * CHECK IF THE INPUT DATA IS SANE @@ -687,15 +687,15 @@ FixedwingEstimator::check_filter_state() } // Copy all states or at least all that we can fit - unsigned ekf_n_states = ekf_report.n_states; - unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); + size_t ekf_n_states = ekf_report.n_states; + size_t max_states = (sizeof(rep.states) / sizeof(rep.states[0])); rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; - for (unsigned i = 0; i < rep.n_states; i++) { + for (size_t i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; } - for (unsigned i = 0; i < rep.n_states; i++) { + for (size_t i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; } @@ -710,13 +710,13 @@ FixedwingEstimator::check_filter_state() } void -FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) +AttitudePositionEstimatorEKF::task_main_trampoline(int argc, char *argv[]) { estimator::g_estimator->task_main(); } void -FixedwingEstimator::task_main() +AttitudePositionEstimatorEKF::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -1638,7 +1638,7 @@ FixedwingEstimator::task_main() } int -FixedwingEstimator::start() +AttitudePositionEstimatorEKF::start() { ASSERT(_estimator_task == -1); @@ -1647,7 +1647,7 @@ FixedwingEstimator::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 7500, - (main_t)&FixedwingEstimator::task_main_trampoline, + (main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr); if (_estimator_task < 0) { @@ -1659,7 +1659,7 @@ FixedwingEstimator::start() } void -FixedwingEstimator::print_status() +AttitudePositionEstimatorEKF::print_status() { math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); math::Matrix<3, 3> R = q.to_dcm(); @@ -1688,7 +1688,7 @@ FixedwingEstimator::print_status() printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); - if (n_states == 23) { + if (EKF_STATE_ESTIMATES == 23) { printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]); printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); @@ -1713,7 +1713,7 @@ FixedwingEstimator::print_status() (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); } -int FixedwingEstimator::trip_nan() { +int AttitudePositionEstimatorEKF::trip_nan() { int ret = 0; @@ -1772,7 +1772,7 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) if (estimator::g_estimator != nullptr) errx(1, "already running"); - estimator::g_estimator = new FixedwingEstimator; + estimator::g_estimator = new AttitudePositionEstimatorEKF(); if (estimator::g_estimator == nullptr) errx(1, "alloc failed"); diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 15d018ab6..6e654f496 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -322,12 +322,12 @@ void AttPosEKF::CovariancePrediction(float dt) float dvz_b; // arrays - float processNoise[n_states]; + float processNoise[EKF_STATE_ESTIMATES]; float SF[15]; float SG[8]; float SQ[11]; float SPP[8] = {0}; - float nextP[n_states][n_states]; + float nextP[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // calculate covariance prediction process noise for (uint8_t i= 0; i<4; i++) processNoise[i] = 1.0e-9f; @@ -343,13 +343,13 @@ void AttPosEKF::CovariancePrediction(float dt) } if (!inhibitMagStates) { for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; - for (uint8_t i=19; i < n_states; i++) processNoise[i] = dt * magBodySigma; + for (uint8_t i=19; i < EKF_STATE_ESTIMATES; i++) processNoise[i] = dt * magBodySigma; } else { - for (uint8_t i=16; i < n_states; i++) processNoise[i] = 0; + for (uint8_t i=16; i < EKF_STATE_ESTIMATES; i++) processNoise[i] = 0; } // square all sigmas - for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]); + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) processNoise[i] = sq(processNoise[i]); // set variables used to calculate covariance growth dvx = summedDelVel.x; @@ -908,7 +908,7 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[21][20] = P[21][20]; nextP[21][21] = P[21][21]; - for (unsigned i = 0; i < n_states; i++) + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { nextP[i][i] = nextP[i][i] + processNoise[i]; } @@ -921,7 +921,7 @@ void AttPosEKF::CovariancePrediction(float dt) { for (uint8_t i=7; i<=8; i++) { - for (unsigned j = 0; j < n_states; j++) + for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) { nextP[i][j] = P[i][j]; nextP[j][i] = P[j][i]; @@ -930,12 +930,12 @@ void AttPosEKF::CovariancePrediction(float dt) } // Copy covariance - for (unsigned i = 0; i < n_states; i++) { + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { P[i][i] = nextP[i][i]; } // force symmetry for observable states - for (unsigned i = 1; i < n_states; i++) + for (size_t i = 1; i < EKF_STATE_ESTIMATES; i++) { for (uint8_t j = 0; j < i; j++) { @@ -1179,7 +1179,7 @@ void AttPosEKF::FuseVelposNED() } // Don't update magnetic field states if inhibited if (inhibitMagStates) { - for (uint8_t i = 16; i < n_states; i++) + for (uint8_t i = 16; i < EKF_STATE_ESTIMATES; i++) { Kfusion[i] = 0; } @@ -1246,8 +1246,8 @@ void AttPosEKF::FuseMagnetometer() float SK_MX[6]; float SK_MY[5]; float SK_MZ[6]; - float H_MAG[n_states]; - for (uint8_t i = 0; i < n_states; i++) { + float H_MAG[EKF_STATE_ESTIMATES]; + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { H_MAG[i] = 0.0f; } @@ -1303,7 +1303,7 @@ void AttPosEKF::FuseMagnetometer() SH_MAG[7] = 2*magN*q0; SH_MAG[8] = 2*magE*q3; - for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0; + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_MAG[i] = 0; H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; H_MAG[1] = SH_MAG[0]; H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2; @@ -1360,7 +1360,7 @@ void AttPosEKF::FuseMagnetometer() Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); } else { - for (uint8_t i=16; i < n_states; i++) { + for (uint8_t i=16; i < EKF_STATE_ESTIMATES; i++) { Kfusion[i] = 0; } } @@ -1370,7 +1370,7 @@ void AttPosEKF::FuseMagnetometer() else if (obsIndex == 1) // we are now fusing the Y measurement { // Calculate observation jacobians - for (unsigned int i = 0; i < n_states; i++) H_MAG[i] = 0; + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_MAG[i] = 0; H_MAG[0] = SH_MAG[2]; H_MAG[1] = SH_MAG[1]; H_MAG[2] = SH_MAG[0]; @@ -1439,7 +1439,7 @@ void AttPosEKF::FuseMagnetometer() else if (obsIndex == 2) // we are now fusing the Z measurement { // Calculate observation jacobians - for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0; + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_MAG[i] = 0; H_MAG[0] = SH_MAG[1]; H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1; H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; @@ -1512,7 +1512,7 @@ void AttPosEKF::FuseMagnetometer() if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f) { // correct the state vector - for (uint8_t j= 0; j < n_states; j++) + for (uint8_t j= 0; j < EKF_STATE_ESTIMATES; j++) { states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; } @@ -1529,7 +1529,7 @@ void AttPosEKF::FuseMagnetometer() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the // number of operations - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { for (uint8_t j = 0; j <= 3; j++) { @@ -1538,22 +1538,22 @@ void AttPosEKF::FuseMagnetometer() for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f; if (!onGround) { - for (uint8_t j = 16; j < n_states; j++) + for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++) { KH[i][j] = Kfusion[i] * H_MAG[j]; } } else { - for (uint8_t j = 16; j < n_states; j++) + for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++) { KH[i][j] = 0.0f; } } } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { KHP[i][j] = 0.0f; for (uint8_t k = 0; k <= 3; k++) @@ -1562,7 +1562,7 @@ void AttPosEKF::FuseMagnetometer() } if (!onGround) { - for (uint8_t k = 16; k < n_states; k++) + for (uint8_t k = 16; k < EKF_STATE_ESTIMATES; k++) { KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; } @@ -1570,9 +1570,9 @@ void AttPosEKF::FuseMagnetometer() } } } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1614,8 +1614,8 @@ void AttPosEKF::FuseAirspeed() SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; - float H_TAS[n_states]; - for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f; + float H_TAS[EKF_STATE_ESTIMATES]; + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_TAS[i] = 0.0f; H_TAS[4] = SH_TAS[2]; H_TAS[5] = SH_TAS[1]; H_TAS[6] = vd*SH_TAS[0]; @@ -1664,7 +1664,7 @@ void AttPosEKF::FuseAirspeed() Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); } else { - for (uint8_t i=16; i < n_states; i++) { + for (uint8_t i=16; i < EKF_STATE_ESTIMATES; i++) { Kfusion[i] = 0; } } @@ -1676,7 +1676,7 @@ void AttPosEKF::FuseAirspeed() if ((innovVtas*innovVtas*SK_TAS) < 25.0f) { // correct the state vector - for (uint8_t j=0; j < n_states; j++) + for (uint8_t j=0; j < EKF_STATE_ESTIMATES; j++) { states[j] = states[j] - Kfusion[j] * innovVtas; } @@ -1693,7 +1693,7 @@ void AttPosEKF::FuseAirspeed() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in H to reduce the // number of operations - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { for (uint8_t j = 0; j <= 3; j++) KH[i][j] = 0.0; for (uint8_t j = 4; j <= 6; j++) @@ -1705,11 +1705,11 @@ void AttPosEKF::FuseAirspeed() { KH[i][j] = Kfusion[i] * H_TAS[j]; } - for (uint8_t j = 16; j < n_states; j++) KH[i][j] = 0.0; + for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++) KH[i][j] = 0.0; } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { KHP[i][j] = 0.0; for (uint8_t k = 4; k <= 6; k++) @@ -1722,9 +1722,9 @@ void AttPosEKF::FuseAirspeed() } } } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1736,13 +1736,13 @@ void AttPosEKF::FuseAirspeed() ConstrainVariances(); } -void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +void AttPosEKF::zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last) { uint8_t row; uint8_t col; for (row=first; row<=last; row++) { - for (col=0; col 5Sigma if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 25.0f) { // correct the state vector - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { states[j] = states[j] - K_LOS[obsIndex][j] * innovOptFlow[obsIndex]; } @@ -1972,7 +1972,7 @@ void AttPosEKF::FuseOptFlow() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the // number of operations - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { for (uint8_t j = 0; j <= 6; j++) { @@ -1983,14 +1983,14 @@ void AttPosEKF::FuseOptFlow() KH[i][j] = 0.0f; } KH[i][9] = K_LOS[obsIndex][i] * H_LOS[obsIndex][9]; - for (uint8_t j = 10; j < n_states; j++) + for (uint8_t j = 10; j < EKF_STATE_ESTIMATES; j++) { KH[i][j] = 0.0f; } } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { KHP[i][j] = 0.0f; for (uint8_t k = 0; k <= 6; k++) @@ -2000,9 +2000,9 @@ void AttPosEKF::FuseOptFlow() KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; } } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -2239,13 +2239,13 @@ void AttPosEKF::OpticalFlowEKF() } -void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +void AttPosEKF::zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last) { uint8_t row; uint8_t col; for (col=first; col<=last; col++) { - for (row=0; row < n_states; row++) + for (row=0; row < EKF_STATE_ESTIMATES; row++) { covMat[row][col] = 0.0; } @@ -2278,14 +2278,14 @@ float AttPosEKF::min(float valIn1, float valIn2) // Store states in a history array along with time stamp void AttPosEKF::StoreStates(uint64_t timestamp_ms) { - for (unsigned i=0; i 0) { - for (unsigned i=0; i < 3; i++) { + for (size_t i=0; i < 3; i++) { omegaForFusion[i] += storedOmega[i][storeIndexLocal]; } sumIndex += 1; } } if (sumIndex >= 1) { - for (unsigned i=0; i < 3; i++) { + for (size_t i=0; i < 3; i++) { omegaForFusion[i] = omegaForFusion[i] / float(sumIndex); } } else { @@ -2596,22 +2596,22 @@ void AttPosEKF::ConstrainVariances() // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) // Constrain quaternion variances - for (unsigned i = 0; i <= 3; i++) { + for (size_t i = 0; i <= 3; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } // Constrain velocity variances - for (unsigned i = 4; i <= 6; i++) { + for (size_t i = 4; i <= 6; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); } // Constrain position variances - for (unsigned i = 7; i <= 9; i++) { + for (size_t i = 7; i <= 9; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); } // Constrain delta angle bias variances - for (unsigned i = 10; i <= 12; i++) { + for (size_t i = 10; i <= 12; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.12f * dtIMU)); } @@ -2619,17 +2619,17 @@ void AttPosEKF::ConstrainVariances() P[13][13] = ConstrainFloat(P[13][13], 0.0f, sq(1.0f * dtIMU)); // Wind velocity variances - for (unsigned i = 14; i <= 15; i++) { + for (size_t i = 14; i <= 15; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); } // Earth magnetic field variances - for (unsigned i = 16; i <= 18; i++) { + for (size_t i = 16; i <= 18; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } // Body magnetic field variances - for (unsigned i = 19; i <= 21; i++) { + for (size_t i = 19; i <= 21; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } @@ -2652,17 +2652,17 @@ void AttPosEKF::ConstrainStates() // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) // Constrain quaternion - for (unsigned i = 0; i <= 3; i++) { + for (size_t i = 0; i <= 3; i++) { states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); } // Constrain velocities to what GPS can do for us - for (unsigned i = 4; i <= 6; i++) { + for (size_t i = 4; i <= 6; i++) { states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); } // Constrain position to a reasonable vehicle range (in meters) - for (unsigned i = 7; i <= 8; i++) { + for (size_t i = 7; i <= 8; i++) { states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); } @@ -2671,7 +2671,7 @@ void AttPosEKF::ConstrainStates() states[9] = ConstrainFloat(states[9], -4.0e4f, 4.0e4f); // Angle bias limit - set to 8 degrees / sec - for (unsigned i = 10; i <= 12; i++) { + for (size_t i = 10; i <= 12; i++) { states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); } @@ -2679,18 +2679,18 @@ void AttPosEKF::ConstrainStates() states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); // Wind velocity limits - assume 120 m/s max velocity - for (unsigned i = 14; i <= 15; i++) { + for (size_t i = 14; i <= 15; i++) { states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); } // Earth magnetic field limits (in Gauss) - for (unsigned i = 16; i <= 18; i++) { + for (size_t i = 16; i <= 18; i++) { states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); } // Body magnetic field variances (in Gauss). // the max offset should be in this range. - for (unsigned i = 19; i <= 21; i++) { + for (size_t i = 19; i <= 21; i++) { states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); } @@ -2704,7 +2704,7 @@ void AttPosEKF::ForceSymmetry() // Force symmetry on the covariance matrix to prevent ill-conditioning // of the matrix which would cause the filter to blow-up - for (unsigned i = 1; i < n_states; i++) + for (size_t i = 1; i < EKF_STATE_ESTIMATES; i++) { for (uint8_t j = 0; j < i; j++) { @@ -2865,8 +2865,8 @@ bool AttPosEKF::StatesNaN() { } // delta velocities // check all states and covariance matrices - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { + for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) { if (!isfinite(KH[i][j])) { current_ekf_state.KHNaN = true; @@ -3206,8 +3206,8 @@ void AttPosEKF::ZeroVariables() lastVelPosFusion = millis(); // Do the data structure init - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { + for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) { KH[i][j] = 0.0f; // intermediate result used for covariance updates KHP[i][j] = 0.0f; // intermediate result used for covariance updates P[i][j] = 0.0f; // covariance matrix @@ -3231,9 +3231,9 @@ void AttPosEKF::ZeroVariables() flowStates[0] = 1.0f; flowStates[1] = 0.0f; - for (unsigned i = 0; i < data_buffer_size; i++) { + for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; i++) { - for (unsigned j = 0; j < n_states; j++) { + for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) { storedStates[j][i] = 0.0f; } @@ -3252,10 +3252,10 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err) { // Copy states - for (unsigned i = 0; i < n_states; i++) { + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { current_ekf_state.states[i] = states[i]; } - current_ekf_state.n_states = n_states; + current_ekf_state.n_states = EKF_STATE_ESTIMATES; current_ekf_state.onGround = onGround; current_ekf_state.staticMode = staticMode; current_ekf_state.useCompass = useCompass; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index b1d71bd74..f8cd743cc 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -1,9 +1,10 @@ #pragma once #include "estimator_utilities.h" +#include -const unsigned int n_states = 22; -const unsigned int data_buffer_size = 50; +constexpr size_t EKF_STATE_ESTIMATES = 22; +constexpr size_t EKF_DATA_BUFFER_SIZE = 50; class AttPosEKF { @@ -108,25 +109,25 @@ public: // Global variables - float KH[n_states][n_states]; // intermediate result used for covariance updates - float KHP[n_states][n_states]; // intermediate result used for covariance updates - float P[n_states][n_states]; // covariance matrix - float Kfusion[n_states]; // Kalman gains - float states[n_states]; // state matrix - float resetStates[n_states]; - float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps - uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored + float KH[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // intermediate result used for covariance updates + float KHP[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // intermediate result used for covariance updates + float P[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // covariance matrix + float Kfusion[EKF_STATE_ESTIMATES]; // Kalman gains + float states[EKF_STATE_ESTIMATES]; // state matrix + float resetStates[EKF_STATE_ESTIMATES]; + float storedStates[EKF_STATE_ESTIMATES][EKF_DATA_BUFFER_SIZE]; // state vectors stored for the last 50 time steps + uint32_t statetimeStamp[EKF_DATA_BUFFER_SIZE]; // time stamp for each state vector stored // Times uint64_t lastVelPosFusion; // the time of the last velocity fusion, in the standard time unit of the filter - float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements - float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements - float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement - float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time - float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time - float statesAtRngTime[n_states]; // filter states at the effective measurement time - float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time + float statesAtVelTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for posNE and velNED measurements + float statesAtPosTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for posNE and velNED measurements + float statesAtHgtTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for the hgtMea measurement + float statesAtMagMeasTime[EKF_STATE_ESTIMATES]; // filter satates at the effective measurement time + float statesAtVtasMeasTime[EKF_STATE_ESTIMATES]; // filter states at the effective measurement time + float statesAtRngTime[EKF_STATE_ESTIMATES]; // filter states at the effective measurement time + float statesAtFlowTime[EKF_STATE_ESTIMATES]; // States at the effective optical flow measurement time float omegaAcrossFlowTime[3]; // angular rates at the effective optical flow measurement time Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) @@ -227,7 +228,7 @@ public: unsigned storeIndex; // Optical Flow error estimation - float storedOmega[3][data_buffer_size]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators + float storedOmega[3][EKF_DATA_BUFFER_SIZE]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators // Two state EKF used to estimate focal length scale factor and terrain position float Popt[2][2]; // state covariance matrix @@ -265,9 +266,9 @@ void FuseOptFlow(); void OpticalFlowEKF(); -void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); +void zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last); -void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); +void zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last); void quatNorm(float (&quatOut)[4], const float quatIn[4]); -- cgit v1.2.3