From ac319a724090d34c53f10fa867794f0c06dcd0ff Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 22 Jun 2014 16:51:48 +0200
Subject: [PATCH] 23 state estimator: Prepare GPS accel init (not yet active),
 clean up init calls

---
 .../estimator_23states.cpp                    | 34 ++++++++++++-------
 .../estimator_23states.h                      |  2 ++
 2 files changed, 23 insertions(+), 13 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
index 673865dd47..9be48cfeab 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -38,8 +38,12 @@ AttPosEKF::AttPosEKF()
     velNED[0] = 0.0f;
     velNED[1] = 0.0f;
     velNED[2] = 0.0f;
+    accelGPSNED[0] = 0.0f;
+    accelGPSNED[1] = 0.0f;
+    accelGPSNED[2] = 0.0f;
     delAngTotal.zero();
     ekfDiverged = false;
+    lastReset = 0;
 
     memset(&last_ekf_error, 0, sizeof(last_ekf_error));
     ZeroVariables();
@@ -2321,12 +2325,21 @@ int AttPosEKF::CheckAndBound()
     // Store the old filter state
     bool currStaticMode = staticMode;
 
+    // Limit reset rate to 5 Hz to allow the filter
+    // to settle
+    if (millis() - lastReset < 200) {
+        return 0;
+    }
+
     if (ekfDiverged) {
         ekfDiverged = false;
     }
 
     int ret = 0;
 
+    // Check if we're on ground - this also sets static mode.
+    OnGroundCheck();
+
     // Reset the filter if the states went NaN
     if (StatesNaN(&last_ekf_error)) {
         ekf_debug("re-initializing dynamic");
@@ -2348,9 +2361,6 @@ int AttPosEKF::CheckAndBound()
         ret = 2;
     }
 
-    // Check if we're on ground - this also sets static mode.
-    OnGroundCheck();
-
     // Check if we switched between states
     if (currStaticMode != staticMode) {
         FillErrorReport(&last_ekf_error);
@@ -2388,6 +2398,7 @@ int AttPosEKF::CheckAndBound()
 
     if (ret) {
         ekfDiverged = true;
+        lastReset = millis();
     }
 
     return ret;
@@ -2441,7 +2452,6 @@ 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
@@ -2478,17 +2488,13 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
     magstate.R_MAG = sq(magMeasurementSigma);
     magstate.DCM = DCM;
 
-    // Calculate position from gps measurement
-    float posNEDInit[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
-    calcposNED(posNEDInit, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
-
     // write to state vector
     for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
     for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities
     // positions:
-    states[7] = posNEDInit[0];
-    states[8] = posNEDInit[1];
-    states[9] = posNEDInit[2];
+    states[7] = posNE[0];
+    states[8] = posNE[1];
+    states[9] = -hgtMea;
     for (uint8_t j=10; j<=15; j++) states[j] = 0.0f; // dAngBias, dVelBias, windVel
     states[16] = initMagNED.x; // Magnetic Field North
     states[17] = initMagNED.y; // Magnetic Field East
@@ -2520,11 +2526,13 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do
     hgtRef = referenceHgt;
     refSet = true;
 
-    // we are at reference altitude, so measurement must be zero
-    hgtMea = 0.0f;
+    // we are at reference position, so measurement must be zero
     posNE[0] = 0.0f;
     posNE[1] = 0.0f;
 
+    // we are at an unknown, possibly non-zero altitude - so altitude
+    // is not reset (hgtMea)
+
     // the baro offset must be this difference now
     baroHgtOffset = baroHgt - referenceHgt;
 
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h
index 1bf1312b00..10a6460251 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h
@@ -138,6 +138,7 @@ public:
     float varInnovVelPos[6]; // innovation variance output
 
     float velNED[3]; // North, East, Down velocity obs (m/s)
+    float accelGPSNED[3];   // Acceleration predicted by GPS in earth frame
     float posNE[2]; // North, East position obs (m)
     float hgtMea; //  measured height (m)
     float baroHgtOffset;        ///< the baro (weather) offset from normalized altitude
@@ -185,6 +186,7 @@ public:
     bool useRangeFinder;     ///< true when rangefinder is being used
 
     bool ekfDiverged;
+    uint64_t lastReset;
 
     struct ekf_status_report current_ekf_state;
     struct ekf_status_report last_ekf_error;