Updated to latest estimator version

src/modules/fw_att_pos_estimator/estimator.cpp

@@ -20,16 +20,9 @@ Vector3f dVelIMU;
 Vector3f dAngIMU;
 float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
 float dt; // time lapsed since last covariance prediction
-bool onGround = true; // boolean true when the flight vehicle is on the ground (not flying)
-bool useAirspeed = true; // boolean true if airspeed data is being used
-bool useCompass = true; // boolean true if magnetometer data is being used
 uint8_t fusionModeGPS = 0; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
 float innovVelPos[6]; // innovation output
 float varInnovVelPos[6]; // innovation variance output
-bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused
-bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused
-bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused
-bool fuseMagData = false; // boolean true when magnetometer data is to be fused
 
 float velNED[3]; // North, East, Down velocity obs (m/s)
 float posNE[2]; // North, East position obs (m)
@@ -45,7 +38,6 @@ Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
 float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
 float innovVtas; // innovation output
 float varInnovVtas; // innovation variance output
-bool fuseVtasData = false; // boolean true when airspeed data is to be fused
 float VtasMeas; // true airspeed measurement (m/s)
 float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
 float latRef; // WGS-84 latitude of reference point (rad)
@@ -64,9 +56,20 @@ float gpsLon;
 float gpsHgt;
 uint8_t GPSstatus;
 
+// Baro input
+float baroHgt;
 
 bool statesInitialised = false;
 
+bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused
+bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused
+bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused
+bool fuseMagData = false; // boolean true when magnetometer data is to be fused
+bool fuseVtasData = false; // boolean true when airspeed data is to be fused
+
+bool onGround = true; // boolean true when the flight vehicle is on the ground (not flying)
+bool useAirspeed = true; // boolean true if airspeed data is being used
+bool useCompass = true; // boolean true if magnetometer data is being used
 
 float Vector3f::length(void) const
 {
@@ -315,12 +318,12 @@ void CovariancePrediction()
     float daz_b;
 
     // arrays
-    float processNoise[n_states];
+    float processNoise[21];
     float SF[14];
     float SG[8];
     float SQ[11];
     float SPP[13];
-    float nextP[n_states][n_states];
+    float nextP[21][21];
 
     // calculate covariance prediction process noise
     const float yawVarScale = 1.0f;
@@ -846,7 +849,7 @@ void CovariancePrediction()
     nextP[20][19] = P[20][19];
     nextP[20][20] = P[20][20];
 
-    for (uint8_t i=0; i< n_states; i++)
+    for (uint8_t i=0; i<= 20; i++)
     {
         nextP[i][i] = nextP[i][i] + processNoise[i];
     }
@@ -885,8 +888,8 @@ void CovariancePrediction()
 
     // Force symmetry on the covariance matrix to prevent ill-conditioning
     // of the matrix which would cause the filter to blow-up
-    for (uint8_t i=0; i< n_states; i++) P[i][i] = nextP[i][i];
+    for (uint8_t i=0; i<=20; i++) P[i][i] = nextP[i][i];
-    for (uint8_t i=1; i< n_states; i++)
+    for (uint8_t i=1; i<=20; i++)
     {
         for (uint8_t j=0; j<=i-1; j++)
         {
@@ -924,8 +927,8 @@ void FuseVelposNED()
 // declare variables used to control access to arrays
     bool fuseData[6] = {false,false,false,false,false,false};
     uint8_t stateIndex;
-    unsigned obsIndex;
+    uint8_t obsIndex;
-    unsigned indexLimit;
+    uint8_t indexLimit;
 
 // declare variables used by state and covariance update calculations
     float velErr;
@@ -1069,7 +1072,7 @@ void FuseVelposNED()
                 stateIndex = 4 + obsIndex;
                 // Calculate the measurement innovation, using states from a
                 // different time coordinate if fusing height data
-                if (obsIndex <= 2)
+                if (obsIndex >= 0 && obsIndex <= 2)
                 {
                     innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex];
                 }
@@ -1137,7 +1140,7 @@ void FuseMagnetometer()
     static float magXbias = 0.0;
     static float magYbias = 0.0;
     static float magZbias = 0.0;
-    static unsigned obsIndex;
+    static uint8_t obsIndex;
     uint8_t indexLimit;
     float DCM[3][3] =
     {
@@ -1148,7 +1151,7 @@ void FuseMagnetometer()
     static float MagPred[3] = {0.0,0.0,0.0};
     static float R_MAG;
     static float SH_MAG[9] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
-    float H_MAG[n_states];
+    float H_MAG[21];
     float SK_MX[6];
     float SK_MY[5];
     float SK_MZ[6];
@@ -1434,8 +1437,8 @@ void FuseAirspeed()
     const float R_TAS = 2.0f;
     float SH_TAS[3];
     float SK_TAS;
-    float H_TAS[n_states];
+    float H_TAS[21];
-    float Kfusion[n_states];
+    float Kfusion[21];
     float VtasPred;
     float quatMag;
 
@@ -1550,6 +1553,7 @@ void FuseAirspeed()
         }
     }
 }
+
 void zeroRows(float covMat[n_states][n_states], uint8_t first, uint8_t last)
 {
     uint8_t row;
@@ -1598,7 +1602,7 @@ void RecallStates(float statesForFusion[n_states], uint32_t msec)
     long int bestTimeDelta = 200;
     uint8_t storeIndex;
     uint8_t bestStoreIndex = 0;
-    for (storeIndex=0; storeIndex < n_states; storeIndex++)
+    for (storeIndex=0; storeIndex < data_buffer_size; storeIndex++)
     {
         timeDelta = msec - statetimeStamp[storeIndex];
         if (timeDelta < 0) timeDelta = -timeDelta;

src/modules/fw_att_pos_estimator/estimator.h

@@ -117,6 +117,9 @@ extern float gpsLon;
 extern float gpsHgt;
 extern uint8_t GPSstatus;
 
+// Baro input
+extern float baroHgt;
+
 extern bool statesInitialised;
 
 const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions