Much more aggressive reset logic bounding the filter effectively

1 files changed, 8 insertions, 0 deletions

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);