From b9a3fa60bc06b31d4862919398c1161e6a35f360 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Jun 2014 13:03:11 +0200 Subject: Add support for 21 and 23 state estimators. Promoto a number of small delta variables to double --- .../ekf_att_pos_estimator/estimator_utilities.h | 79 ++++++++++++++++++++++ 1 file changed, 79 insertions(+) create mode 100644 src/modules/ekf_att_pos_estimator/estimator_utilities.h (limited to 'src/modules/ekf_att_pos_estimator/estimator_utilities.h') diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h new file mode 100644 index 000000000..7dddee468 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -0,0 +1,79 @@ +#include +#include + +#pragma once + +#define GRAVITY_MSS 9.80665f +#define deg2rad 0.017453292f +#define rad2deg 57.295780f +#define pi 3.141592657f +#define earthRate 0.000072921f +#define earthRadius 6378145.0f +#define earthRadiusInv 1.5678540e-7f + +class Vector3f +{ +private: +public: + float x; + float y; + float z; + + float length(void) const; + void zero(void); +}; + +class Mat3f +{ +private: +public: + Vector3f x; + Vector3f y; + Vector3f z; + + Mat3f(); + + void identity(); + Mat3f transpose(void) const; +}; + +Vector3f operator*(float sclIn1, Vector3f vecIn1); +Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator*( Mat3f matIn, Vector3f vecIn); +Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator*(Vector3f vecIn1, float sclIn1); + +void swap_var(float &d1, float &d2); + +const unsigned int n_states = 21; +const unsigned int data_buffer_size = 50; + +enum GPS_FIX { + GPS_FIX_NOFIX = 0, + GPS_FIX_2D = 2, + GPS_FIX_3D = 3 +}; + +struct ekf_status_report { + bool velHealth; + bool posHealth; + bool hgtHealth; + bool velTimeout; + bool posTimeout; + bool hgtTimeout; + uint32_t velFailTime; + uint32_t posFailTime; + uint32_t hgtFailTime; + float states[n_states]; + bool angNaN; + bool summedDelVelNaN; + bool KHNaN; + bool KHPNaN; + bool PNaN; + bool covarianceNaN; + bool kalmanGainsNaN; + bool statesNaN; +}; + +void ekf_debug(const char *fmt, ...); \ No newline at end of file -- cgit v1.2.3 From f4075b5623bb9c92dd46e92b97bc363a91498ff6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Jun 2014 16:06:18 +0200 Subject: Switching back to 23 states, fixed mag update logic --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 15 +++++++++++---- src/modules/ekf_att_pos_estimator/estimator_21states.cpp | 6 +++--- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 11 +++-------- src/modules/ekf_att_pos_estimator/estimator_23states.h | 2 +- src/modules/ekf_att_pos_estimator/estimator_utilities.h | 2 +- src/modules/ekf_att_pos_estimator/module.mk | 2 +- 6 files changed, 20 insertions(+), 18 deletions(-) (limited to 'src/modules/ekf_att_pos_estimator/estimator_utilities.h') 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 86f7eb99f..9cff9fab0 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 @@ -83,7 +83,7 @@ #include #include -#include "estimator_21states.h" +#include "estimator_23states.h" @@ -451,6 +451,8 @@ FixedwingEstimator::~FixedwingEstimator() } while (_estimator_task != -1); } + delete _ekf; + estimator::g_estimator = nullptr; } @@ -564,7 +566,7 @@ FixedwingEstimator::task_main() #else _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_sensor_combined_sub, 4); + orb_set_interval(_sensor_combined_sub, 9); #endif /* sets also parameters in the EKF object */ @@ -809,6 +811,8 @@ FixedwingEstimator::task_main() #endif + //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); + bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); @@ -1247,12 +1251,15 @@ FixedwingEstimator::task_main() _ekf->fuseMagData = true; _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data + _ekf->magstate.obsIndex = 0; + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); + } else { _ekf->fuseMagData = false; } - if (_ekf->statesInitialised) _ekf->FuseMagnetometer(); - // Fuse Airspeed Measurements if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { _ekf->fuseVtasData = true; diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp index 5a60ac1c0..0f5a17a33 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp @@ -61,8 +61,8 @@ void AttPosEKF::UpdateStrapdownEquationsNED() // Apply corrections for earths rotation rate and coning errors // * and + operators have been overloaded - correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); - + correctedDelAng = dAngIMU;//correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); + correctedDelAng.z = 0; // Convert the rotation vector to its equivalent quaternion rotationMag = correctedDelAng.length(); if (rotationMag < 1e-12f) @@ -151,7 +151,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; // Constrain states (to protect against filter divergence) - ConstrainStates(); + //ConstrainStates(); } void AttPosEKF::CovariancePrediction(float dt) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index ca9db9685..89e6d3948 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1,4 +1,4 @@ -#include "estimator.h" +#include "estimator_23states.h" #include #include @@ -1140,7 +1140,7 @@ void AttPosEKF::FuseMagnetometer() // data fit is the only assumption we can make // so we might as well take advantage of the computational efficiencies // associated with sequential fusion - if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) + if (useCompass && fuseMagData && (obsIndex < 3)) { // Limit range of states modified when on ground if(!onGround) @@ -1156,7 +1156,7 @@ void AttPosEKF::FuseMagnetometer() // three prediction time steps. // Calculate observation jacobians and Kalman gains - if (fuseMagData) + if (obsIndex == 0) { // Copy required states to local variable names q0 = statesAtMagMeasTime[0]; @@ -1251,11 +1251,6 @@ void AttPosEKF::FuseMagnetometer() Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); varInnovMag[0] = 1.0f/SK_MX[0]; innovMag[0] = MagPred[0] - magData.x; - - // reset the observation index to 0 (we start by fusing the X - // measurement) - obsIndex = 0; - fuseMagData = false; } else if (obsIndex == 1) // we are now fusing the Y measurement { diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 6d3b89108..851e46371 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -229,7 +229,7 @@ static void quat2eul(float (&eul)[3], const float (&quat)[4]); static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); -static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); +void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 7dddee468..714dfe623 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -46,7 +46,7 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1); void swap_var(float &d1, float &d2); -const unsigned int n_states = 21; +const unsigned int n_states = 23; const unsigned int data_buffer_size = 50; enum GPS_FIX { diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 0058b3fd1..dc5220bf0 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -39,5 +39,5 @@ MODULE_COMMAND = ekf_att_pos_estimator SRCS = ekf_att_pos_estimator_main.cpp \ ekf_att_pos_estimator_params.c \ - estimator_21states.cpp \ + estimator_23states.cpp \ estimator_utilities.cpp -- cgit v1.2.3 From 7983e105bf1aa6b8cf13ed49dac36c4f1b3a034f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Jun 2014 15:28:21 +0200 Subject: Much more aggressive reset logic bounding the filter effectively --- .../ekf_att_pos_estimator/estimator_23states.cpp | 64 +++++++++++++++++++--- .../ekf_att_pos_estimator/estimator_23states.h | 8 +++ .../ekf_att_pos_estimator/estimator_utilities.h | 8 +-- 3 files changed, 69 insertions(+), 11 deletions(-) (limited to 'src/modules/ekf_att_pos_estimator/estimator_utilities.h') diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 29b5391ba..de3c9d60e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1,5 +1,6 @@ #include "estimator_23states.h" #include +#include #include #define EKF_COVARIANCE_DIVERGED 1.0e8f @@ -33,6 +34,8 @@ AttPosEKF::AttPosEKF() magDeclination = 0.0f; dAngIMU.zero(); dVelIMU.zero(); + ekfDiverged = false; + delAngTotal.zero(); memset(&last_ekf_error, 0, sizeof(last_ekf_error)); ZeroVariables(); @@ -70,6 +73,10 @@ void AttPosEKF::UpdateStrapdownEquationsNED() dVelIMU.y = dVelIMU.y; dVelIMU.z = dVelIMU.z - states[13]; + delAngTotal.x += correctedDelAng.x; + delAngTotal.y += correctedDelAng.y; + delAngTotal.z += correctedDelAng.z; + // Save current measurements Vector3f prevDelAng = correctedDelAng; @@ -201,7 +208,8 @@ void AttPosEKF::CovariancePrediction(float dt) float nextP[n_states][n_states]; // calculate covariance prediction process noise - for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i= 0; i<4; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i= 4; i<10; i++) processNoise[i] = 1.0e-9f; for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; // scale gyro bias noise when on ground to allow for faster bias estimation for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; @@ -2081,7 +2089,7 @@ void AttPosEKF::ForceSymmetry() if ((fabsf(P[i][j]) > EKF_COVARIANCE_DIVERGED) || (fabsf(P[j][i]) > EKF_COVARIANCE_DIVERGED)) { - // XXX divergence report error + last_ekf_error.covariancesExcessive = true; InitializeDynamic(velNED, magDeclination); return; } @@ -2117,6 +2125,24 @@ bool AttPosEKF::GyroOffsetsDiverged() return diverged; } +bool AttPosEKF::VelNEDDiverged() +{ + Vector3f current_vel; + current_vel.x = states[4]; + current_vel.y = states[5]; + current_vel.z = states[6]; + + Vector3f gps_vel; + gps_vel.x = velNED[0]; + gps_vel.y = velNED[1]; + gps_vel.z = velNED[2]; + + Vector3f delta = current_vel - gps_vel; + float delta_len = delta.length(); + + return (delta_len > 8.0f); +} + bool AttPosEKF::FilterHealthy() { if (!statesInitialised) { @@ -2188,6 +2214,7 @@ void AttPosEKF::FillErrorReport(struct ekf_status_report *err) { err->states[i] = states[i]; } + err->n_states = n_states; err->velHealth = current_ekf_state.velHealth; err->posHealth = current_ekf_state.posHealth; @@ -2290,13 +2317,19 @@ int AttPosEKF::CheckAndBound() // Store the old filter state bool currStaticMode = staticMode; + if (ekfDiverged) { + ekfDiverged = false; + } + + int ret = 0; + // Reset the filter if the states went NaN if (StatesNaN(&last_ekf_error)) { ekf_debug("re-initializing dynamic"); InitializeDynamic(velNED, magDeclination); - return 1; + ret = 1; } // Reset the filter if the IMU data is too old @@ -2308,7 +2341,7 @@ int AttPosEKF::CheckAndBound() ResetStoredStates(); // that's all we can do here, return - return 2; + ret = 2; } // Check if we're on ground - this also sets static mode. @@ -2322,7 +2355,7 @@ int AttPosEKF::CheckAndBound() ResetHeight(); ResetStoredStates(); - return 3; + ret = 3; } // Reset the filter if gyro offsets are excessive @@ -2331,10 +2364,23 @@ int AttPosEKF::CheckAndBound() InitializeDynamic(velNED, magDeclination); // that's all we can do here, return - return 4; + ret = 4; } - return 0; + // Reset the filter if it diverges too far from GPS + if (VelNEDDiverged()) { + FillErrorReport(&last_ekf_error); + InitializeDynamic(velNED, magDeclination); + + // that's all we can do here, return + ret = 5; + } + + if (ret) { + ekfDiverged = true; + } + + return ret; } void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) @@ -2386,6 +2432,8 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) { + ZeroVariables(); + // Fill variables with valid data velNED[0] = initvelNED[0]; velNED[1] = initvelNED[1]; @@ -2469,6 +2517,7 @@ void AttPosEKF::ZeroVariables() { // Initialize on-init initialized variables + storeIndex = 0; // Do the data structure init @@ -2486,6 +2535,7 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); + lastGyroOffset.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 32c514004..1bf1312b0 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -2,6 +2,9 @@ #include "estimator_utilities.h" +const unsigned int n_states = 23; +const unsigned int data_buffer_size = 50; + class AttPosEKF { public: @@ -121,6 +124,7 @@ public: Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) Vector3f lastGyroOffset; // Last gyro offset + Vector3f delAngTotal; Mat3f Tbn; // transformation matrix from body to NED coordinates Mat3f Tnb; // transformation amtrix from NED to body coordinates @@ -180,6 +184,8 @@ public: bool useCompass; ///< boolean true if magnetometer data is being used bool useRangeFinder; ///< true when rangefinder is being used + bool ekfDiverged; + struct ekf_status_report current_ekf_state; struct ekf_status_report last_ekf_error; @@ -281,6 +287,8 @@ bool FilterHealthy(); bool GyroOffsetsDiverged(); +bool VelNEDDiverged(); + void ResetHeight(void); void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 714dfe623..e5f76d7cd 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -46,9 +46,6 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1); void swap_var(float &d1, float &d2); -const unsigned int n_states = 23; -const unsigned int data_buffer_size = 50; - enum GPS_FIX { GPS_FIX_NOFIX = 0, GPS_FIX_2D = 2, @@ -65,7 +62,8 @@ struct ekf_status_report { uint32_t velFailTime; uint32_t posFailTime; uint32_t hgtFailTime; - float states[n_states]; + float states[32]; + unsigned n_states; bool angNaN; bool summedDelVelNaN; bool KHNaN; @@ -74,6 +72,8 @@ struct ekf_status_report { bool covarianceNaN; bool kalmanGainsNaN; bool statesNaN; + bool gyroOffsetsExcessive; + bool covariancesExcessive; }; void ekf_debug(const char *fmt, ...); \ No newline at end of file -- cgit v1.2.3 From 6951e1c95e3b58c35a031aff92b228b3f651a94c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Jun 2014 15:00:48 +0200 Subject: estimator lib: Improve error reporting --- .../ekf_att_pos_estimator/estimator_23states.cpp | 117 ++++++++++++++------- .../ekf_att_pos_estimator/estimator_23states.h | 5 +- .../ekf_att_pos_estimator/estimator_utilities.h | 3 + 3 files changed, 82 insertions(+), 43 deletions(-) (limited to 'src/modules/ekf_att_pos_estimator/estimator_utilities.h') diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index c122b3e51..4c503b5d4 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -46,6 +46,7 @@ AttPosEKF::AttPosEKF() lastReset = 0; memset(&last_ekf_error, 0, sizeof(last_ekf_error)); + memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); ZeroVariables(); InitialiseParameters(); } @@ -2097,7 +2098,8 @@ void AttPosEKF::ForceSymmetry() if ((fabsf(P[i][j]) > EKF_COVARIANCE_DIVERGED) || (fabsf(P[j][i]) > EKF_COVARIANCE_DIVERGED)) { - last_ekf_error.covariancesExcessive = true; + current_ekf_state.covariancesExcessive = true; + current_ekf_state.error |= true; InitializeDynamic(velNED, magDeclination); return; } @@ -2129,6 +2131,8 @@ bool AttPosEKF::GyroOffsetsDiverged() bool diverged = (delta_len_scaled > 1.0f); lastGyroOffset = current_bias; + current_ekf_state.error |= diverged; + current_ekf_state.gyroOffsetsExcessive = diverged; return diverged; } @@ -2148,7 +2152,12 @@ bool AttPosEKF::VelNEDDiverged() Vector3f delta = current_vel - gps_vel; float delta_len = delta.length(); - return (delta_len > 20.0f); + bool excessive = (delta_len > 20.0f); + + current_ekf_state.error |= excessive; + current_ekf_state.velOffsetExcessive = excessive; + + return excessive; } bool AttPosEKF::FilterHealthy() @@ -2215,43 +2224,26 @@ void AttPosEKF::ResetVelocity(void) } } - -void AttPosEKF::FillErrorReport(struct ekf_status_report *err) -{ - for (unsigned i = 0; i < n_states; i++) - { - err->states[i] = states[i]; - } - err->n_states = n_states; - - err->velHealth = current_ekf_state.velHealth; - err->posHealth = current_ekf_state.posHealth; - err->hgtHealth = current_ekf_state.hgtHealth; - err->velTimeout = current_ekf_state.velTimeout; - err->posTimeout = current_ekf_state.posTimeout; - err->hgtTimeout = current_ekf_state.hgtTimeout; -} - -bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { +bool AttPosEKF::StatesNaN() { bool err = false; // check all integrators if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { - err_report->angNaN = true; + current_ekf_state.angNaN = true; ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z); err = true; goto out; } // delta angles if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { - err_report->angNaN = true; + current_ekf_state.angNaN = true; ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); err = true; goto out; } // delta angles if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { - err_report->summedDelVelNaN = true; + current_ekf_state.summedDelVelNaN = true; ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); err = true; goto out; @@ -2262,7 +2254,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { for (unsigned j = 0; j < n_states; j++) { if (!isfinite(KH[i][j])) { - err_report->KHNaN = true; + current_ekf_state.KHNaN = true; err = true; ekf_debug("KH NaN"); goto out; @@ -2270,7 +2262,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(KHP[i][j])) { - err_report->KHPNaN = true; + current_ekf_state.KHPNaN = true; err = true; ekf_debug("KHP NaN"); goto out; @@ -2278,7 +2270,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(P[i][j])) { - err_report->covarianceNaN = true; + current_ekf_state.covarianceNaN = true; err = true; ekf_debug("P NaN"); } // covariance matrix @@ -2286,7 +2278,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(Kfusion[i])) { - err_report->kalmanGainsNaN = true; + current_ekf_state.kalmanGainsNaN = true; ekf_debug("Kfusion NaN"); err = true; goto out; @@ -2294,7 +2286,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(states[i])) { - err_report->statesNaN = true; + current_ekf_state.statesNaN = true; ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]); err = true; goto out; @@ -2303,7 +2295,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { out: if (err) { - FillErrorReport(err_report); + current_ekf_state.error |= true; } return err; @@ -2319,7 +2311,7 @@ out: * updated, but before any of the fusion steps are * executed. */ -int AttPosEKF::CheckAndBound() +int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) { // Store the old filter state @@ -2341,9 +2333,10 @@ int AttPosEKF::CheckAndBound() OnGroundCheck(); // Reset the filter if the states went NaN - if (StatesNaN(&last_ekf_error)) { + if (StatesNaN()) { ekf_debug("re-initializing dynamic"); + // Reset and fill error report InitializeDynamic(velNED, magDeclination); ret = 1; @@ -2351,19 +2344,29 @@ int AttPosEKF::CheckAndBound() // Reset the filter if the IMU data is too old if (dtIMU > 0.3f) { - FillErrorReport(&last_ekf_error); + + current_ekf_state.imuTimeout = true; + + // Fill error report + GetFilterState(&last_ekf_error); + ResetVelocity(); ResetPosition(); ResetHeight(); ResetStoredStates(); + // Timeout cleared with this reset + current_ekf_state.imuTimeout = false; + // that's all we can do here, return ret = 2; } // Check if we switched between states if (currStaticMode != staticMode) { - FillErrorReport(&last_ekf_error); + // Fill error report, but not setting error flag + GetFilterState(&last_ekf_error); + ResetVelocity(); ResetPosition(); ResetHeight(); @@ -2374,7 +2377,8 @@ int AttPosEKF::CheckAndBound() // Reset the filter if gyro offsets are excessive if (GyroOffsetsDiverged()) { - FillErrorReport(&last_ekf_error); + + // Reset and fill error report InitializeDynamic(velNED, magDeclination); // that's all we can do here, return @@ -2383,7 +2387,8 @@ int AttPosEKF::CheckAndBound() // Reset the filter if it diverges too far from GPS if (VelNEDDiverged()) { - FillErrorReport(&last_ekf_error); + + // Reset and fill error report InitializeDynamic(velNED, magDeclination); // that's all we can do here, return @@ -2392,15 +2397,16 @@ int AttPosEKF::CheckAndBound() // The excessive covariance detection already // reset the filter. Just need to report here. - if (current_ekf_state.covariancesExcessive) { - FillErrorReport(&last_ekf_error); - current_ekf_state.covariancesExcessive = false; + if (last_ekf_error.covariancesExcessive) { ret = 6; } if (ret) { ekfDiverged = true; lastReset = millis(); + + // This reads the last error and clears it + GetLastErrorState(last_error); } return ret; @@ -2454,8 +2460,31 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) { + if (current_ekf_state.error) { + GetFilterState(&last_ekf_error); + } + ZeroVariables(); + // Reset error states + current_ekf_state.error = false; + current_ekf_state.angNaN = false; + current_ekf_state.summedDelVelNaN = false; + current_ekf_state.KHNaN = false; + current_ekf_state.KHPNaN = false; + current_ekf_state.PNaN = false; + current_ekf_state.covarianceNaN = false; + current_ekf_state.kalmanGainsNaN = false; + current_ekf_state.statesNaN = false; + + current_ekf_state.velHealth = true; + //current_ekf_state.posHealth = ?; + //current_ekf_state.hgtHealth = ?; + + current_ekf_state.velTimeout = false; + //current_ekf_state.posTimeout = ?; + //current_ekf_state.hgtTimeout = ?; + // Fill variables with valid data velNED[0] = initvelNED[0]; velNED[1] = initvelNED[1]; @@ -2582,7 +2611,7 @@ void AttPosEKF::ZeroVariables() } -void AttPosEKF::GetFilterState(struct ekf_status_report *state) +void AttPosEKF::GetFilterState(struct ekf_status_report *err) { // Copy states @@ -2591,10 +2620,18 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *state) } current_ekf_state.n_states = n_states; - memcpy(state, ¤t_ekf_state, sizeof(*state)); + memcpy(err, ¤t_ekf_state, sizeof(*err)); + + // err->velHealth = current_ekf_state.velHealth; + // err->posHealth = current_ekf_state.posHealth; + // err->hgtHealth = current_ekf_state.hgtHealth; + // err->velTimeout = current_ekf_state.velTimeout; + // err->posTimeout = current_ekf_state.posTimeout; + // err->hgtTimeout = current_ekf_state.hgtTimeout; } void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) { memcpy(last_error, &last_ekf_error, sizeof(*last_error)); + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); } diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 10a646025..5a1f5125a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -266,7 +266,7 @@ void ConstrainStates(); void ForceSymmetry(); -int CheckAndBound(); +int CheckAndBound(struct ekf_status_report *last_error); void ResetPosition(); @@ -278,8 +278,7 @@ void GetFilterState(struct ekf_status_report *state); void GetLastErrorState(struct ekf_status_report *last_error); -bool StatesNaN(struct ekf_status_report *err_report); -void FillErrorReport(struct ekf_status_report *err); +bool StatesNaN(); void InitializeDynamic(float (&initvelNED)[3], float declination); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index e5f76d7cd..97f22b453 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -53,12 +53,14 @@ enum GPS_FIX { }; struct ekf_status_report { + bool error; bool velHealth; bool posHealth; bool hgtHealth; bool velTimeout; bool posTimeout; bool hgtTimeout; + bool imuTimeout; uint32_t velFailTime; uint32_t posFailTime; uint32_t hgtFailTime; @@ -74,6 +76,7 @@ struct ekf_status_report { bool statesNaN; bool gyroOffsetsExcessive; bool covariancesExcessive; + bool velOffsetExcessive; }; void ekf_debug(const char *fmt, ...); \ No newline at end of file -- cgit v1.2.3 From 91bba668f670db7cec916966d3c53d3b25ac7061 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 11:27:43 +0200 Subject: Define earth radius as double, as our calculations relying on it need double precision. --- src/modules/ekf_att_pos_estimator/estimator_utilities.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/ekf_att_pos_estimator/estimator_utilities.h') diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 97f22b453..d47568b62 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -8,8 +8,8 @@ #define rad2deg 57.295780f #define pi 3.141592657f #define earthRate 0.000072921f -#define earthRadius 6378145.0f -#define earthRadiusInv 1.5678540e-7f +#define earthRadius 6378145.0 +#define earthRadiusInv 1.5678540e-7 class Vector3f { -- cgit v1.2.3