Updated estimator, not using optical flow for now until proven on the bench

1 files changed, 5 insertions, 18 deletions

diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h
index 6effe062d..3a8f45d9a 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.h
+++ b/src/modules/ekf_att_pos_estimator/estimator.h
@@ -20,7 +20,7 @@ public:
     float z;
 
     float length(void) const;
-    Vector3f zero(void) const;
+    void zero(void);
 };
 
 class Mat3f
@@ -93,9 +93,6 @@ public:
     float covTimeStepMax; // maximum time allowed between covariance predictions
     float covDelAngMax; // maximum delta angle between covariance predictions
     float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
-    float a1; // optical flow sensor misalgnment angle about X axis (rad)
-    float a2; // optical flow sensor misalgnment angle about Y axis (rad)
-    float a3; // optical flow sensor misalgnment angle about Z axis (rad)
 
     float yawVarScale;
     float windVelSigma;
@@ -122,10 +119,6 @@ public:
         covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
         covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
         rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
-        a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad)
-        a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad)
-        a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad)
-
         EAS2TAS = 1.0f;
 
         yawVarScale = 1.0f;
@@ -165,7 +158,6 @@ public:
     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 statesAtLosMeasTime[n_states]; // filter states at the effective measurement time
 
     Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
     Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
@@ -192,14 +184,11 @@ public:
     float rngMea; // Ground distance
     float posNED[3]; // North, East Down position (m)
 
-    float innovMag[3]; // innovation output for magnetometer measurements
-    float varInnovMag[3]; // innovation variance output for magnetometer measurements
-    float varInnovLOS[2]; // innovation variance output for optical flow measurements
+    float innovMag[3]; // innovation output
+    float varInnovMag[3]; // innovation variance output
     Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
-    float innovVtas; // innovation output for true airspeed measurements
-    float innovRng; ///< Range finder innovation for rnge finder measurements
-    float innovLOS[2]; // Innovations for optical flow LOS rate measurements
-    float losData[2]; // Optical flow LOS rate measurements
+    float innovVtas; // innovation output
+    float innovRng; ///< Range finder innovation
     float varInnovVtas; // innovation variance output
     float VtasMeas; // true airspeed measurement (m/s)
     double latRef; // WGS-84 latitude of reference point (rad)
@@ -227,14 +216,12 @@ public:
     bool fuseMagData; // boolean true when magnetometer data is to be fused
     bool fuseVtasData; // boolean true when airspeed data is to be fused
     bool fuseRngData;   ///< true when range data is fused
-    bool fuseOptData;   // true when optical flow data is fused
 
     bool onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
     bool staticMode;    ///< boolean true if no position feedback is fused
     bool useAirspeed;    ///< boolean true if airspeed data is being used
     bool useCompass;    ///< boolean true if magnetometer data is being used
     bool useRangeFinder;     ///< true when rangefinder is being used
-    bool useOpticalFlow; // true when optical flow data is being used
 
     struct ekf_status_report current_ekf_state;
     struct ekf_status_report last_ekf_error;