mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-23 09:08:30 -04:00
AP_NavEKF: use vectorN to make indexes safe
This commit is contained in:
Andrew Tridgell
parent
e972acbc9f
commit
0b71618f4f
@ -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];
|
||||
float SF[21];
|
||||
float SG[8];
|
||||
float SQ[11];
|
||||
float SPP[13];
|
||||
float nextP[24][24];
|
||||
Vector24 processNoise;
|
||||
VectorN<float,21> SF;
|
||||
VectorN<float,8> SG;
|
||||
VectorN<float,11> SQ;
|
||||
VectorN<float,13> 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<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];
|
||||
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;
|
||||
|
||||
@ -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
|
||||
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<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
|
||||
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
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user