From 0b71618f4f553a78c92ae867571776ed5341c351 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 30 Dec 2013 13:25:02 +1100 Subject: [PATCH] AP_NavEKF: use vectorN to make indexes safe --- libraries/AP_NavEKF/AP_NavEKF.cpp | 69 ++++++++++++++-------------- libraries/AP_NavEKF/AP_NavEKF.h | 76 +++++++++++++++++-------------- 2 files changed, 78 insertions(+), 67 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 494a2081f3..420b71b9de 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/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 #include "AP_NavEKF.h" #include @@ -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]; - float SF[21]; - float SG[8]; - float SQ[11]; - float SPP[13]; - float nextP[24][24]; + Vector24 processNoise; + VectorN SF; + VectorN SG; + VectorN SQ; + VectorN SPP; + 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 posHealth; + bool velHealth = false; + 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}; - float posInnov[2] = {0,0}; + Vector3f velInnov; + VectorN 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]; - float observation[6]; + Vector6 R_OBS; + 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]; - float SK_MX[6]; - float SK_MY[5]; - float SK_MZ[6]; - float Kfusion[24]; + Vector24 H_MAG; + Vector6 SK_MX; + Vector6 SK_MY; + Vector6 SK_MZ; + 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]; - float Kfusion[24]; + Vector24 H_TAS; + 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; diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 30662f744f..77abd6a86c 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -28,11 +28,19 @@ #include #include #include +#include class NavEKF { public: + typedef VectorN Vector2; + typedef VectorN Vector6; + typedef VectorN Vector8; + typedef VectorN Vector24; + typedef VectorN,24> Matrix24; + typedef VectorN,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 - float KHP[24][24]; // intermediate result used for covariance updates - float P[24][24]; // 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 - float storedStates[24][50]; // state vectors stored for the last 50 time steps - uint32_t statetimeStamp[50]; // time stamp for each state vector stored + Matrix24 KH; // intermediate result used for covariance updates + Matrix24 KHP; // intermediate result used for covariance updates + Matrix24 P; // covariance matrix + 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 + Matrix24_50 storedStates; // state vectors stored for the last 50 time steps + VectorN 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 - float varInnovVelPos[6]; // innovation variance output for a group of measurements + Vector6 innovVelPos; // innovation 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) - float posNE[2]; // North, East position measurements (m) + Vector3f velNED; // North, East, Down velocity measurements (m/s) + 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) - float statesAtVelTime[24]; // States at the effective time of velNED measurements - float statesAtPosTime[24]; // States at the effective time of posNE measurements - float statesAtHgtTime[24]; // States at the effective time of hgtMea measurement - float innovMag[3]; // 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 posNED; // North, East Down position relative to reference point (m) + Vector24 statesAtVelTime; // States at the effective time of velNED measurements + Vector24 statesAtPosTime; // States at the effective time of posNE measurements + Vector24 statesAtHgtTime; // States at the effective time of hgtMea measurement + Vector3f innovMag; // innovation 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 - float eulerDif[3]; // difference between Euler angle estimated by EKF and the AHRS solution + Vector3f eulerEst; // Euler angles calculated from filter states + 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]; - float tempMagPrev[8]; + Vector8 tempMag; + 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