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
 {
@@ -183,7 +186,7 @@ void  UpdateStrapdownEquationsNED()
     static float lastVelocity[3];
     const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS};
 
-    // Remove sensor bias errors
+// Remove sensor bias errors
     correctedDelAng.x = dAngIMU.x - states[10];
     correctedDelAng.y = dAngIMU.y - states[11];
     correctedDelAng.z = dAngIMU.z - states[12];
@@ -191,14 +194,14 @@ void  UpdateStrapdownEquationsNED()
     dVelIMU.y = dVelIMU.y;
     dVelIMU.z = dVelIMU.z;
 
-    // Save current measurements
+// Save current measurements
     prevDelAng = correctedDelAng;
 
-    // Apply corrections for earths rotation rate and coning errors
-    // * and + operators have been overloaded
+// Apply corrections for earths rotation rate and coning errors
+// * and + operators have been overloaded
     correctedDelAng   = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
 
-    // Convert the rotation vector to its equivalent quaternion
+// Convert the rotation vector to its equivalent quaternion
     rotationMag = correctedDelAng.length();
     if (rotationMag < 1e-12f)
     {
@@ -216,14 +219,14 @@ void  UpdateStrapdownEquationsNED()
         deltaQuat[3] = correctedDelAng.z*rotScaler;
     }
 
-    // Update the quaternions by rotating from the previous attitude through
-    // the delta angle rotation quaternion
+// Update the quaternions by rotating from the previous attitude through
+// the delta angle rotation quaternion
     qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3];
     qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2];
     qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3];
     qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
 
-    // Normalise the quaternions and update the quaternion states
+// Normalise the quaternions and update the quaternion states
     quatMag = sqrt(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
     if (quatMag > 1e-16f)
     {
@@ -234,7 +237,7 @@ void  UpdateStrapdownEquationsNED()
         states[3] = quatMagInv*qUpdated[3];
     }
 
-    // Calculate the body to nav cosine matrix
+// Calculate the body to nav cosine matrix
     q00 = sq(states[0]);
     q11 = sq(states[1]);
     q22 = sq(states[2]);
@@ -258,28 +261,28 @@ void  UpdateStrapdownEquationsNED()
 
     Tnb = Tbn.transpose();
 
-    // transform body delta velocities to delta velocities in the nav frame
-    // * and + operators have been overloaded
+// transform body delta velocities to delta velocities in the nav frame
+// * and + operators have been overloaded
     //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU;
     delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU;
     delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU;
     delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU;
 
-    // calculate the magnitude of the nav acceleration (required for GPS
-    // variance estimation)
+// calculate the magnitude of the nav acceleration (required for GPS
+// variance estimation)
     accNavMag = delVelNav.length()/dtIMU;
 
-    // If calculating position save previous velocity
+// If calculating position save previous velocity
     lastVelocity[0] = states[4];
     lastVelocity[1] = states[5];
     lastVelocity[2] = states[6];
 
-    // Sum delta velocities to get velocity
+// Sum delta velocities to get velocity
     states[4] = states[4] + delVelNav.x;
     states[5] = states[5] + delVelNav.y;
     states[6] = states[6] + delVelNav.z;
 
-    // If calculating postions, do a trapezoidal integration for position
+// If calculating postions, do a trapezoidal integration for position
     states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU;
     states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU;
     states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU;
@@ -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=1; i< n_states; i++)
+    for (uint8_t i=0; i<=20; i++) P[i][i] = nextP[i][i];
+    for (uint8_t i=1; i<=20; i++)
     {
         for (uint8_t j=0; j<=i-1; j++)
         {
@@ -901,7 +904,7 @@ void CovariancePrediction()
 void FuseVelposNED()
 {
 
-    // declare variables used by fault isolation logic
+// declare variables used by fault isolation logic
     uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure
     uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available
     uint32_t hgtRetryTime = 5000; // height measurement retry time
@@ -916,18 +919,18 @@ void FuseVelposNED()
     bool posTimeout;
     bool hgtTimeout;
 
-    // declare variables used to check measurement errors
+// declare variables used to check measurement errors
     float velInnov[3] = {0.0,0.0,0.0};
     float posInnov[2] = {0.0,0.0};
     float hgtInnov = 0.0;
 
-    // declare variables used to control access to arrays
+// declare variables used to control access to arrays
     bool fuseData[6] = {false,false,false,false,false,false};
     uint8_t stateIndex;
-    unsigned obsIndex;
-    unsigned indexLimit;
+    uint8_t obsIndex;
+    uint8_t indexLimit;
 
-    // declare variables used by state and covariance update calculations
+// declare variables used by state and covariance update calculations
     float velErr;
     float posErr;
     float R_OBS[6];
@@ -935,12 +938,12 @@ void FuseVelposNED()
     float SK;
     float quatMag;
 
-    // Perform sequential fusion of GPS measurements. This assumes that the
-    // errors in the different velocity and position components are
-    // uncorrelated which is not true, however in the absence of covariance
-    // data from the GPS receiver it is the only assumption we can make
-    // so we might as well take advantage of the computational efficiencies
-    // associated with sequential fusion
+// Perform sequential fusion of GPS measurements. This assumes that the
+// errors in the different velocity and position components are
+// uncorrelated which is not true, however in the absence of covariance
+// data from the GPS receiver it is the only assumption we can make
+// so we might as well take advantage of the computational efficiencies
+// associated with sequential fusion
     if (fuseVelData || fusePosData || fuseHgtData)
     {
         // set the GPS data timeout depending on whether airspeed data is present
@@ -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,17 +1151,17 @@ 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];
 
-    // Perform sequential fusion of Magnetometer measurements.
-    // This assumes that the errors in the different components are
-    // uncorrelated which is not true, however in the absence of covariance
-    // data fit is the only assumption we can make
-    // so we might as well take advantage of the computational efficiencies
-    // associated with sequential fusion
+// Perform sequential fusion of Magnetometer measurements.
+// This assumes that the errors in the different components are
+// uncorrelated which is not true, however in the absence of covariance
+// 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))
     {
         // Limit range of states modified when on ground
@@ -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 Kfusion[n_states];
+    float H_TAS[21];
+    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