AP_NavEKF: use vectorN to make indexes safe

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -1,4 +1,7 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#define MATH_CHECK_INDEXES 1
+
 #include <AP_HAL.h>
 #include "AP_NavEKF.h"
 #include <stdio.h>
@@ -52,7 +55,7 @@ static void write_floats(float *v, uint8_t n)
 void NavEKF::InitialiseFilter(void)
 {
     // Calculate initial filter quaternion states from ahrs solution
-    float initQuat[4];
+    Quaternion initQuat;
     ahrsEul[0] = _ahrs.roll_sensor*0.01f*DEG_TO_RAD;
     ahrsEul[1] = _ahrs.pitch_sensor*0.01f*DEG_TO_RAD;
     ahrsEul[2] = _ahrs.yaw_sensor*0.01f*DEG_TO_RAD;
@@ -265,10 +268,10 @@ void NavEKF::UpdateStrapdownEquationsNED()
     float q23;
     float rotationMag;
     float rotScaler;
-    float qUpdated[4];
+    Quaternion qUpdated;
     float quatMag;
     float quatMagInv;
-    float deltaQuat[4];
+    Quaternion deltaQuat;
     const Vector3f gravityNED(0, 0, GRAVITY_MSS);
 
     // Remove sensor bias errors
@@ -410,12 +413,12 @@ void NavEKF::CovariancePrediction()
     float dvz_b;
 
     // arrays
-    float processNoise[24];
+    Vector24 processNoise;
-    float SF[21];
+    VectorN<float,21> SF;
-    float SG[8];
+    VectorN<float,8> SG;
-    float SQ[11];
+    VectorN<float,11> SQ;
-    float SPP[13];
+    VectorN<float,13> SPP;
-    float nextP[24][24];
+    Matrix24 nextP;
 
     // calculate covariance prediction process noise
     windVelSigma  = dt * 0.1f;
@@ -1172,17 +1175,17 @@ void NavEKF::FuseVelPosNED()
     uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available
     uint32_t hgtRetryTime = 5000; // height measurement retry time
     uint32_t horizRetryTime = 0;
-    bool velHealth;
+    bool velHealth = false;
-    bool posHealth;
+    bool posHealth = false;
     bool hgtHealth;
     bool velTimeout;
     bool posTimeout;
     bool hgtTimeout;
-    float Kfusion[24];
+    Vector24 Kfusion;
 
 // declare variables used to check measurement errors
-    float velInnov[3] = {0,0,0};
+    Vector3f velInnov;
-    float posInnov[2] = {0,0};
+    VectorN<float,2> posInnov;
     float hgtInnov = 0;
 
 // declare variables used to control access to arrays
@@ -1193,8 +1196,8 @@ void NavEKF::FuseVelPosNED()
 // declare variables used by state and covariance update calculations
     float velErr;
     float posErr;
-    float R_OBS[6];
+    Vector6 R_OBS;
-    float observation[6];
+    Vector6 observation;
     float SK;
     float quatMag;
 
@@ -1408,11 +1411,11 @@ void NavEKF::FuseMagnetometer()
     Vector3f &MagPred = mag_state.MagPred;
     float &R_MAG = mag_state.R_MAG;
     float *SH_MAG = &mag_state.SH_MAG[0];
-    float H_MAG[24];
+    Vector24 H_MAG;
-    float SK_MX[6];
+    Vector6 SK_MX;
-    float SK_MY[5];
+    Vector6 SK_MY;
-    float SK_MZ[6];
+    Vector6 SK_MZ;
-    float Kfusion[24];
+    Vector24 Kfusion;
 
     // Perform sequential fusion of Magnetometer measurements.
     // This assumes that the errors in the different components are
@@ -1691,10 +1694,10 @@ void NavEKF::FuseAirspeed()
     float vwn;
     float vwe;
     const float R_TAS = 2.0f;
-    float SH_TAS[3];
+    Vector3f SH_TAS;
     float SK_TAS;
-    float H_TAS[24];
+    Vector24 H_TAS;
-    float Kfusion[24];
+    Vector24 Kfusion;
     float VtasPred;
     float quatMag;
 
@@ -1812,7 +1815,7 @@ void NavEKF::FuseAirspeed()
     }
 }
 
-void NavEKF::zeroRows(float covMat[24][24], uint8_t first, uint8_t last)
+void NavEKF::zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last)
 {
     uint8_t row;
     uint8_t col;
@@ -1825,7 +1828,7 @@ void NavEKF::zeroRows(float covMat[24][24], uint8_t first, uint8_t last)
     }
 }
 
-void NavEKF::zeroCols(float covMat[24][24], uint8_t first, uint8_t last)
+void NavEKF::zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last)
 {
     uint8_t row;
     uint8_t col;
@@ -1848,7 +1851,7 @@ void NavEKF::StoreStates()
 }
 
 // Output the state vector stored at the time that best matches that specified by msec
-void NavEKF::RecallStates(float statesForFusion[24], uint32_t msec)
+void NavEKF::RecallStates(Vector24 &statesForFusion, uint32_t msec)
 {
     uint32_t timeDelta;
     uint32_t bestTimeDelta = 200;
@@ -1877,7 +1880,7 @@ void NavEKF::RecallStates(float statesForFusion[24], uint32_t msec)
     }
 }
 
-void NavEKF::quat2Tnb(Matrix3f &Tnb, float quat[4])
+void NavEKF::quat2Tnb(Matrix3f &Tnb, const Quaternion &quat)
 {
     // Calculate the nav to body cosine matrix
     float q00 = sq(quat[0]);
@@ -1902,7 +1905,7 @@ void NavEKF::quat2Tnb(Matrix3f &Tnb, float quat[4])
     Tnb.b.z = 2*(q23 + q01);
 }
 
-void NavEKF::quat2Tbn(Matrix3f &Tbn, float quat[4])
+void NavEKF::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat)
 {
     // Calculate the body to nav cosine matrix
     float q00 = sq(quat[0]);
@@ -1927,7 +1930,7 @@ void NavEKF::quat2Tbn(Matrix3f &Tbn, float quat[4])
     Tbn.c.y = 2*(q23 + q01);
 }
 
-void NavEKF::eul2quat(float quat[4], float eul[3])
+void NavEKF::eul2quat(Quaternion &quat, const Vector3f &eul)
 {
     float u1 = cosf(0.5f*eul[0]);
     float u2 = cosf(0.5f*eul[1]);
@@ -1941,7 +1944,7 @@ void NavEKF::eul2quat(float quat[4], float eul[3])
     quat[3] = u1*u2*u6-u4*u5*u3;
 }
 
-void NavEKF::quat2eul(Vector3f &y, float u[4])
+void NavEKF::quat2eul(Vector3f &y, const Quaternion &u)
 {
     y[0] = atan2f((2*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
     y[1] = -asinf(2*(u[1]*u[3]-u[0]*u[2]));
@@ -1950,7 +1953,7 @@ void NavEKF::quat2eul(Vector3f &y, float u[4])
 
 void NavEKF::getEulerAngles(Vector3f &euler)
 {
-    float q[4] = { states[0], states[1], states[2], states[3] };
+    Quaternion q(states[0], states[1], states[2], states[3]);
     quat2eul(euler, q);
 }
 
@@ -2007,8 +2010,6 @@ void NavEKF::readIMUData()
     uint32_t IMUusec;
     Vector3f angRate;     // angular rate vector in XYZ body axes measured by the IMU (rad/s)
     Vector3f accel;       // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
-    static Vector3f lastAngRate;
-    static Vector3f lastAccel;
 
     IMUusec     = hal.scheduler->micros();
     IMUmsec     = IMUusec/1000;

libraries/AP_NavEKF/AP_NavEKF.h

@@ -28,11 +28,19 @@
 #include <AP_AHRS.h>
 #include <AP_Airspeed.h>
 #include <AP_Compass.h>
+#include <vectorN.h>
 
 class NavEKF
 {
 public:
 
+    typedef VectorN<float,2> Vector2;
+    typedef VectorN<float,6> Vector6;
+    typedef VectorN<float,8> Vector8;
+    typedef VectorN<float,24> Vector24;
+    typedef VectorN<VectorN<float,24>,24> Matrix24;
+    typedef VectorN<VectorN<float,50>,24> Matrix24_50;
+
     // Constructor 
     NavEKF(const AP_AHRS &ahrs, AP_Baro &baro);
  
@@ -81,29 +89,29 @@ private:
     
     void FuseAirspeed();
 
-    void zeroRows(float covMat[24][24], uint8_t first, uint8_t last);
+    void zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last);
 
-    void zeroCols(float covMat[24][24], uint8_t first, uint8_t last);
+    void zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last);
 
-    void quatNorm(float quatOut[4], float quatIn[4]);
+    void quatNorm(Quaternion &quatOut, const Quaternion &quatIn);
 
     // store states along with system time stamp in msces
     void StoreStates(void);
 
     // recall state vector stored at closest time to the one specified by msec
-    void RecallStates(float statesForFusion[24], uint32_t msec);
+    void RecallStates(Vector24 &statesForFusion, uint32_t msec);
 
-    void quat2Tnb(Matrix3f &Tnb, float quat[4]);
+    void quat2Tnb(Matrix3f &Tnb, const Quaternion &quat);
 
-    void quat2Tbn(Matrix3f &Tbn, float quat[4]);
+    void quat2Tbn(Matrix3f &Tbn, const Quaternion &quat);
 
     void calcEarthRateNED(Vector3f &omega, float latitude);
 
-    void eul2quat(float quat[4], float eul[3]);
+    void eul2quat(Quaternion &quat, const Vector3f &eul);
 
-    void quat2eul(Vector3f &eul, float quat[4]);
+    void quat2eul(Vector3f &eul, const Quaternion &quat);
 
-    void calcvelNED(float velNED[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
+    void calcvelNED(Vector3f &velNED, float gpsCourse, float gpsGndSpd, float gpsVelD);
 
     void calcposNE(float lat, float lon);
 
@@ -133,12 +141,12 @@ private:
 
     bool statesInitialised;
 
-    float KH[24][24]; //  intermediate result used for covariance updates
+    Matrix24 KH; //  intermediate result used for covariance updates
-    float KHP[24][24]; // intermediate result used for covariance updates
+    Matrix24 KHP; // intermediate result used for covariance updates
-    float P[24][24]; // covariance matrix
+    Matrix24 P; // covariance matrix
-    float states[24]; // state matrix - 4 x quaternions, 3 x Vel, 3 x Pos, 3 x gyro bias, 3 x accel bias, 2 x wind vel, 3 x earth mag field, 3 x body mag field
+    Vector24 states; // state matrix - 4 x quaternions, 3 x Vel, 3 x Pos, 3 x gyro bias, 3 x accel bias, 2 x wind vel, 3 x earth mag field, 3 x body mag field
-    float storedStates[24][50]; // state vectors stored for the last 50 time steps
+    Matrix24_50 storedStates; // state vectors stored for the last 50 time steps
-    uint32_t statetimeStamp[50]; // time stamp for each state vector stored
+    VectorN<uint32_t,50> statetimeStamp; // time stamp for each state vector stored
     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)
     Vector3f summedDelAng; // corrected & summed delta angles about the xyz body axes (rad)
@@ -155,34 +163,34 @@ private:
     const bool useAirspeed; // boolean true if airspeed data is being used
     const bool useCompass; // boolean true if magnetometer data is being used
     const uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
-    float innovVelPos[6]; // innovation output for a group of measurements
+    Vector6 innovVelPos; // innovation output for a group of measurements
-    float varInnovVelPos[6]; // innovation variance output for a group of measurements
+    Vector6 varInnovVelPos; // innovation variance output for a group of measurements
     bool fuseVelData; // this boolean causes the velNED measurements to be fused
     bool fusePosData; // this boolean causes the posNE measurements to be fused
     bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused
-    float velNED[3]; // North, East, Down velocity measurements (m/s)
+    Vector3f velNED; // North, East, Down velocity measurements (m/s)
-    float posNE[2]; // North, East position measurements (m)
+    Vector2 posNE; // North, East position measurements (m)
     float hgtMea; //  height measurement relative to reference point  (m)
-    float posNED[3]; // North, East Down position relative to reference point (m)
+    Vector3f posNED; // North, East Down position relative to reference point (m)
-    float statesAtVelTime[24]; // States at the effective time of velNED measurements
+    Vector24 statesAtVelTime; // States at the effective time of velNED measurements
-    float statesAtPosTime[24]; // States at the effective time of posNE measurements
+    Vector24 statesAtPosTime; // States at the effective time of posNE measurements
-    float statesAtHgtTime[24]; // States at the effective time of hgtMea measurement
+    Vector24 statesAtHgtTime; // States at the effective time of hgtMea measurement
-    float innovMag[3]; // innovation output from fusion of X,Y,Z compass measurements
+    Vector3f innovMag; // innovation output from fusion of X,Y,Z compass measurements
-    float varInnovMag[3]; // innovation variance output from fusion of X,Y,Z compass measurements
+    Vector3f varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
     bool fuseMagData; // boolean true when magnetometer data is to be fused
     Vector3f magData; // magnetometer flux readings in X,Y,Z body axes
-    float statesAtMagMeasTime[24]; // filter states at the effective time of compass measurements
+    Vector24 statesAtMagMeasTime; // filter states at the effective time of compass measurements
     float innovVtas; // innovation output from fusion of airspeed measurements
     float varInnovVtas; // innovation variance output from fusion of airspeed measurements
     bool fuseVtasData; // boolean true when airspeed data is to be fused
     float VtasMeas; // true airspeed measurement (m/s)
-    float statesAtVtasMeasTime[24]; // filter states at the effective measurement time
+    Vector24 statesAtVtasMeasTime; // filter states at the effective measurement time
     float latRef; // WGS-84 latitude of reference point (rad)
     float lonRef; // WGS-84 longitude of reference point (rad)
     float hgtRef; // WGS-84 height of reference point (m)
     Vector3f magBias; // magnetometer bias vector in XYZ body axes
-    float eulerEst[3]; // Euler angles calculated from filter states
+    Vector3f eulerEst; // Euler angles calculated from filter states
-    float eulerDif[3]; // difference between Euler angle estimated by EKF and the AHRS solution
+    Vector3f eulerDif; // difference between Euler angle estimated by EKF and the AHRS solution
     const float covTimeStepMax; // maximum time allowed between covariance predictions
     const float covDelAngMax; // maximum delta angle between covariance predictions
     bool covPredStep; // boolean set to true when a covariance prediction step has been performed
@@ -205,7 +213,7 @@ private:
 
     // IMU input data variables
     float imuIn;
-    float tempImu[8];
+    Vector8 tempImu;
     uint32_t IMUmsec;
 
     // GPS input data variables
@@ -218,8 +226,8 @@ private:
 
     // Magnetometer input data variables
     float magIn;
-    float tempMag[8];
+    Vector8 tempMag;
-    float tempMagPrev[8];
+    Vector8 tempMagPrev;
     uint32_t MAGframe;
     uint32_t MAGtime;
     uint32_t lastMAGtime;
@@ -231,7 +239,7 @@ private:
     float VtasMeasPrev;
 
     // AHRS input data variables
-    float ahrsEul[3];
+    Vector3f ahrsEul;
 
     // Time stamp when vel, pos or height measurements last failed checks
 	uint32_t velFailTime;
@@ -268,6 +276,8 @@ private:
     // time of alst GPS fix used to determine if new data has arrived
 	uint32_t lastFixTime;
 
+    Vector3f lastAngRate;
+    Vector3f lastAccel;
 };
 #endif // AP_NavEKF