zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF : Add accel aliasing protection

master
Paul Riseborough 11 years ago committed by Andrew Tridgell
parent
f2c79ad639
commit
bd152d332c
2 changed files with 176 additions and 76 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 197
      libraries/AP_NavEKF/AP_NavEKF.cpp
  2. 55
      libraries/AP_NavEKF/AP_NavEKF.h

197
libraries/AP_NavEKF/AP_NavEKF.cpp
Unescape View File

@ -91,7 +91,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { @@ -91,7 +91,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: GYRO_PNOISE
// @DisplayName: Rate gyro noise (rad/s)
// @Description: This noise controls the growth of estimated error due to gyro measurement errors excluding bias. Increasing it makes the flter trust the gyro measurements less and other measurements more.
// @Description: This noise controls the growth of estimated error due to gyro measurement errors excluding bias. Increasing it makes the flter trust the gyro measurements less and other measurements more.
// @Range: 0.001 - 0.05
// @Increment: 0.001
// @User: advanced
@ -99,7 +99,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { @@ -99,7 +99,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: ACC_PNOISE
// @DisplayName: Accelerometer noise (m/s^2)
// @Description: This noise controls the growth of estimated error due to accelerometer measurement errors excluding bias. Increasing it makes the flter trust the accelerometer measurements less and other measurements more.
// @Description: This noise controls the growth of estimated error due to accelerometer measurement errors excluding bias. Increasing it makes the flter trust the accelerometer measurements less and other measurements more.
// @Range: 0.05 - 1.0 AP_Float _gpsNEVelVarAccScale; // scale factor applied to NE velocity measurement variance due to Vdot
// @Increment: 0.01
// @User: advanced
@ -243,6 +243,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : @@ -243,6 +243,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
hgtRate = 0.0f;
mag_state.q0 = 1;
mag_state.DCM.identity();
IMU1_weighting = 0.5f;
}
bool NavEKF::healthy(void) const
@ -297,23 +298,25 @@ void NavEKF::ResetPosition(void) @@ -297,23 +298,25 @@ void NavEKF::ResetPosition(void)
void NavEKF::ResetVelocity(void)
{
if (staticMode) {
if (_fusionModeGPS <= 1) {
states[4] = 0;
states[5] = 0;
}
if (_fusionModeGPS <= 0) {
states[6] = 0;
}
state.velocity.zero();
state.vel1.zero();
state.vel2.zero();
} else if (_ahrs->get_gps()->status() >= GPS::GPS_OK_FIX_3D) {
// read the GPS
readGpsData();
// write to state vector
if (_fusionModeGPS <= 1) {
states[4] = velNED[0];
states[5] = velNED[1];
states[4] = velNED[0];
states[5] = velNED[1];
states[23] = velNED[0];
states[24] = velNED[1];
states[27] = velNED[0];
states[28] = velNED[1];
}
if (_fusionModeGPS <= 0) {
states[6] = velNED[2];
states[6] = velNED[2];
states[25] = velNED[2];
states[29] = velNED[2];
}
}
}
@ -323,7 +326,9 @@ void NavEKF::ResetHeight(void) @@ -323,7 +326,9 @@ void NavEKF::ResetHeight(void)
// read the altimeter
readHgtData();
// write to state vector
states[9] = -hgtMea;
states[9] = -hgtMea;
state.posD1 = -hgtMea;
state.posD2 = -hgtMea;
}
// This function is used to initialise the filter whilst moving, using the AHRS DCM solution
@ -375,7 +380,8 @@ void NavEKF::InitialiseFilterDynamic(void) @@ -375,7 +380,8 @@ void NavEKF::InitialiseFilterDynamic(void)
// write to state vector
state.quat = initQuat;
state.gyro_bias.zero();
state.accel_zbias = 0;
state.accel_zbias1 = 0;
state.accel_zbias2 = 0;
state.wind_vel.zero();
ResetVelocity();
ResetPosition();
@ -462,7 +468,8 @@ void NavEKF::InitialiseFilterBootstrap(void) @@ -462,7 +468,8 @@ void NavEKF::InitialiseFilterBootstrap(void)
// write to state vector
state.quat = initQuat;
state.gyro_bias.zero();
state.accel_zbias = 0;
state.accel_zbias1 = 0;
state.accel_zbias2 = 0;
state.wind_vel.zero();
state.earth_magfield = initMagNED;
state.body_magfield = magBias;
@ -531,7 +538,7 @@ void NavEKF::UpdateFilter() @@ -531,7 +538,7 @@ void NavEKF::UpdateFilter()
// sum delta angles and time used by covariance prediction
summedDelAng = summedDelAng + correctedDelAng;
summedDelVel = summedDelVel + correctedDelVel;
summedDelVel = summedDelVel + correctedDelVel1;
dt += dtIMU;
// perform a covariance prediction if the total delta angle has exceeded the limit
@ -666,6 +673,8 @@ void NavEKF::SelectTasFusion() @@ -666,6 +673,8 @@ void NavEKF::SelectTasFusion()
void NavEKF::UpdateStrapdownEquationsNED()
{
Vector3f delVelNav;
Vector3f delVelNav1;
Vector3f delVelNav2;
float rotationMag;
float rotScaler;
Quaternion qUpdated;
@ -676,8 +685,13 @@ void NavEKF::UpdateStrapdownEquationsNED() @@ -676,8 +685,13 @@ void NavEKF::UpdateStrapdownEquationsNED()
// Remove sensor bias errors
correctedDelAng = dAngIMU - state.gyro_bias;
correctedDelVel = dVelIMU;
correctedDelVel.z -= state.accel_zbias;
correctedDelVel1 = dVelIMU1;
correctedDelVel2 = dVelIMU2;
correctedDelVel1.z -= state.accel_zbias1;
correctedDelVel2.z -= state.accel_zbias2;
// Use weighted average of both IMU units for delta velocities
correctedDelVel12 = correctedDelVel1 * IMU1_weighting + correctedDelVel2 * (1.0f - IMU1_weighting);
// Save current measurements
prevDelAng = correctedDelAng;
@ -730,7 +744,11 @@ void NavEKF::UpdateStrapdownEquationsNED() @@ -730,7 +744,11 @@ void NavEKF::UpdateStrapdownEquationsNED()
// transform body delta velocities to delta velocities in the nav frame
// * and + operators have been overloaded
delVelNav = Tbn_temp*correctedDelVel + gravityNED*dtIMU;
// blended IMU calc
delVelNav = Tbn_temp*correctedDelVel12 + gravityNED*dtIMU;
// single IMU calcs
delVelNav1 = Tbn_temp*correctedDelVel1 + gravityNED*dtIMU;
delVelNav2 = Tbn_temp*correctedDelVel2 + gravityNED*dtIMU;
// Calculate the rate of change of velocity (used for launch detect and other functions)
velDotNED = delVelNav / dtIMU ;
@ -744,12 +762,18 @@ void NavEKF::UpdateStrapdownEquationsNED() @@ -744,12 +762,18 @@ void NavEKF::UpdateStrapdownEquationsNED()
// If calculating position save previous velocity
Vector3f lastVelocity = state.velocity;
Vector3f lastVel1 = state.vel1;
Vector3f lastVel2 = state.vel2;
// Sum delta velocities to get velocity
state.velocity += delVelNav;
state.vel1 += delVelNav1;
state.vel2 += delVelNav2;
// If calculating postions, do a trapezoidal integration for position
state.position += (state.velocity + lastVelocity) * (dtIMU*0.5f);
state.posD1 += (state.vel1.z + lastVel1.z) * (dtIMU*0.5f);
state.posD2 += (state.vel2.z + lastVel2.z) * (dtIMU*0.5f);
// Limit states to protect against divergence
ConstrainStates();
@ -828,7 +852,7 @@ void NavEKF::CovariancePrediction() @@ -828,7 +852,7 @@ void NavEKF::CovariancePrediction()
dax_b = states[10];
day_b = states[11];
daz_b = states[12];
dvz_b = states[13];
dvz_b = IMU1_weighting * states[13] + (1.0f - IMU1_weighting) * states[22];
_gyrNoise = constrain_float(_gyrNoise, 1e-3f, 5e-2f);
daxCov = sq(dt*_gyrNoise);
dayCov = sq(dt*_gyrNoise);
@ -1411,6 +1435,8 @@ void NavEKF::FuseVelPosNED() @@ -1411,6 +1435,8 @@ void NavEKF::FuseVelPosNED()
// declare variables used to check measurement errors
Vector3f velInnov;
Vector3f velInnov1;
Vector3f velInnov2;
Vector2 posInnov;
float hgtInnov = 0;
@ -1447,7 +1473,7 @@ void NavEKF::FuseVelPosNED() @@ -1447,7 +1473,7 @@ void NavEKF::FuseVelPosNED()
statesAtHgtTime[i] = states[i];
}
}
// set the GPS data timeout depending on whether airspeed data is present
uint32_t gpsRetryTime;
if (useAirspeed()) gpsRetryTime = _gpsRetryTimeUseTAS;
@ -1500,13 +1526,36 @@ void NavEKF::FuseVelPosNED() @@ -1500,13 +1526,36 @@ void NavEKF::FuseVelPosNED()
{
// test velocity measurements
uint8_t imax = 2;
if (_fusionModeGPS == 1) imax = 1;
if (_fusionModeGPS == 1) {
imax = 1;
}
float K1 = 0;
float K2 = 0;
for (uint8_t i = 0; i<=imax; i++)
{
stateIndex = i + 4;
velInnov[i] = statesAtVelTime[stateIndex] - observation[i];
// velocity states start at index 4
stateIndex = i + 4;
// calculate innovations using blended and single IMU predicted states
velInnov[i] = statesAtVelTime[stateIndex] - observation[i]; // blended
velInnov1[i] = statesAtVelTime[23 + i] - observation[i]; // IMU1
velInnov2[i] = statesAtVelTime[27 + i] - observation[i]; // IMU2
// calculate innovation variance
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
// calculate error weightings for singloe IMU velocity states using
// observation error to normalise
float R_hgt;
if (i == 2) {
R_hgt = sq(_gpsVertVelNoise);
} else {
R_hgt = sq(_gpsHorizVelNoise);
}
K1 += R_hgt / (R_hgt + sq(velInnov1[i]));
K2 += R_hgt / (R_hgt + sq(velInnov2[i]));
}
// Calculate weighting used by fuseVelPosNED to do IMU accel data blending
// This is used to detect and compensate for aliasing errors with the accelerometers
// Provision for a first order lowpass filter to reduce noise on the weighting if required
IMU1_weighting = 1.0f * (K1 / (K1 + K2)) + 0.0f * IMU1_weighting;
// apply an innovation consistency threshold test, but don't fail if bad IMU data
velHealth = !((sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) > (sq(_gpsVelInnovGate) * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2])) && !badIMUdata);
velTimeout = (hal.scheduler->millis() - velFailTime) > gpsRetryTime;
@ -1617,6 +1666,7 @@ void NavEKF::FuseVelPosNED() @@ -1617,6 +1666,7 @@ void NavEKF::FuseVelPosNED()
{
indexLimit = 13;
}
// Fuse measurements sequentially
for (obsIndex=0; obsIndex<=5; obsIndex++)
{
@ -1645,17 +1695,57 @@ void NavEKF::FuseVelPosNED() @@ -1645,17 +1695,57 @@ void NavEKF::FuseVelPosNED()
{
Kfusion[i] = P[i][stateIndex]*SK;
}
// Allow only height measurements to modify Z accel bias state
if (obsIndex != 5) {
// Set the Kalman gain values for the single IMU states
Kfusion[22] = Kfusion[13]; // IMU2 Z accel bias
Kfusion[26] = Kfusion[9]; // IMU1 posD
Kfusion[30] = Kfusion[9]; // IMU2 posD
for (uint8_t i = 0; i<=2; i++) {
Kfusion[i+23] = Kfusion[i+4]; // IMU1 velNED
Kfusion[i+27] = Kfusion[i+4]; // IMU2 velNED
}
// Correct states that have been predicted using single (not blended) IMU data
if (obsIndex == 5){
// Calculate height measurement innovations using single IMU states
float hgtInnov1 = statesAtHgtTime[26] - observation[obsIndex];
float hgtInnov2 = statesAtHgtTime[30] - observation[obsIndex];
// Correct single IMU prediction states using height measurement
states[13] = states[13] - Kfusion[13] * hgtInnov1; // IMU1 Z accel bias
states[22] = states[22] - Kfusion[22] * hgtInnov2; // IMU2 Z accel bias
for (uint8_t i = 23; i<=26; i++)
{
states[i] = states[i] - Kfusion[i] * hgtInnov1; // IMU1 velNED,posD
}
for (uint8_t i = 27; i<=30; i++)
{
states[i] = states[i] - Kfusion[i] * hgtInnov2; // IMU2 velNED,posD
}
// don't allow subsequent fusion operations to modify the Z accel bias state
Kfusion[13] = 0.0f;
Kfusion[22] = 0.0f;
} else if (obsIndex == 0 || obsIndex == 1 || obsIndex == 2){
// don't allow subsequent fusion operations to modify the Z accel bias state
Kfusion[13] = 0.0f;
Kfusion[22] = 0.0f;
// Correct single IMU prediction states using velocity measurements
for (uint8_t i = 23; i<=26; i++)
{
states[i] = states[i] - Kfusion[i] * velInnov1[obsIndex]; // IMU1 velNED,posD
}
for (uint8_t i = 27; i<=30; i++)
{
states[i] = states[i] - Kfusion[i] * velInnov2[obsIndex]; // IMU2 velNED,posD
}
} else {
// don't allow subsequent fusion operations to modify the Z accel bias state
Kfusion[13] = 0.0f;
Kfusion[22] = 0.0f;
}
// Calculate state corrections and re-normalise the quaternions
// Calculate state corrections and re-normalise the quaternions for blended IMU data predicted states
for (uint8_t i = 0; i<=indexLimit; i++)
{
states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
}
state.quat.normalize();
// Update the covariance - take advantage of direct observation of a
// single state at index = stateIndex to reduce computations
// Optimised implementation of standard equation P = (I - K*H)*P;
@ -2165,7 +2255,7 @@ void NavEKF::zeroCols(Matrix22 &covMat, uint8_t first, uint8_t last) @@ -2165,7 +2255,7 @@ void NavEKF::zeroCols(Matrix22 &covMat, uint8_t first, uint8_t last)
void NavEKF::StoreStates()
{
if (storeIndex > 49) storeIndex = 0;
for (uint8_t i=0; i<=21; i++) storedStates[i][storeIndex] = states[i];
for (uint8_t i=0; i<=30; i++) storedStates[i][storeIndex] = states[i];
statetimeStamp[storeIndex] = hal.scheduler->millis();
storeIndex = storeIndex + 1;
}
@ -2178,13 +2268,13 @@ void NavEKF::StoreStatesReset() @@ -2178,13 +2268,13 @@ void NavEKF::StoreStatesReset()
memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
// store current state vector in first column
storeIndex = 0;
for (uint8_t i=0; i<=21; i++) storedStates[i][storeIndex] = states[i];
for (uint8_t i=0; i<=30; i++) storedStates[i][storeIndex] = states[i];
statetimeStamp[storeIndex] = hal.scheduler->millis();
storeIndex = storeIndex + 1;
}
// Output the state vector stored at the time that best matches that specified by msec
void NavEKF::RecallStates(Vector22 &statesForFusion, uint32_t msec)
void NavEKF::RecallStates(Vector31 &statesForFusion, uint32_t msec)
{
uint32_t timeDelta;
uint32_t bestTimeDelta = 200;
@ -2200,13 +2290,13 @@ void NavEKF::RecallStates(Vector22 &statesForFusion, uint32_t msec) @@ -2200,13 +2290,13 @@ void NavEKF::RecallStates(Vector22 &statesForFusion, uint32_t msec)
}
if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
{
for (uint8_t i=0; i<=21; i++) {
for (uint8_t i=0; i<=30; i++) {
statesForFusion[i] = storedStates[i][bestStoreIndex];
}
}
else // otherwise output current state
{
for (uint8_t i=0; i<=21; i++) {
for (uint8_t i=0; i<=30; i++) {
statesForFusion[i] = states[i];
}
}
@ -2249,8 +2339,8 @@ void NavEKF::getGyroBias(Vector3f &gyroBias) const @@ -2249,8 +2339,8 @@ void NavEKF::getGyroBias(Vector3f &gyroBias) const
void NavEKF::getAccelBias(Vector3f &accelBias) const
{
accelBias.x = staticMode? 1.0f : 0.0f;
accelBias.y = onGround? 1.0f : 0.0f;
accelBias.x = IMU1_weighting;
accelBias.y = states[22] / dtIMU;
accelBias.z = states[13] / dtIMU;
}
@ -2444,8 +2534,9 @@ void NavEKF::ConstrainStates() @@ -2444,8 +2534,9 @@ void NavEKF::ConstrainStates()
states[9] = constrain_float(states[9],-4.0e4f,1.0e4f);
// gyro bias limit ~6 deg/sec (this needs to be set based on manufacturers specs)
for (uint8_t i=10; i<=12; i++) states[i] = constrain_float(states[i],-0.1f*dtIMU,0.1f*dtIMU);
// Z accel bias limit 0.5 m/s^2 (this neeeds to be set based on manufacturers specs)
states[13] = constrain_float(states[13],-0.5f*dtIMU,0.5f*dtIMU);
// Z accel bias limit 1.0 m/s^2 (this needs to be finalised from test data)
states[13] = constrain_float(states[13],-1.0f*dtIMU,1.0f*dtIMU);
states[22] = constrain_float(states[22],-1.0f*dtIMU,1.0f*dtIMU);
// Wind Limit 100 m/s (should be based on some multiple of max airspeed * EAS2TAS)
//TODO apply circular limit
for (uint8_t i=14; i<=15; i++) states[i] = constrain_float(states[i],-100.0f,100.0f);
@ -2458,29 +2549,28 @@ void NavEKF::ConstrainStates() @@ -2458,29 +2549,28 @@ void NavEKF::ConstrainStates()
void NavEKF::readIMUData()
{
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)
Vector3f accel1; // acceleration vector in XYZ body axes measured by IMU1 (m/s^2)
Vector3f accel2; // acceleration vector in XYZ body axes measured by IMU2 (m/s^2)
IMUmsec = hal.scheduler->millis();
// Limit IMU delta time to prevent numerical problems elsewhere
dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(), 0.001f, 1.0f);
angRate = _ahrs->get_ins().get_gyro();
// we use get_fly_forward() to detect if this is a copter. If it
// is a copter then we need to use the same accel all the time to
// prevent small altitude jumps. If it is a plane or rover then we
// are better off using the accel that DCM has set as active to
// cope with larger aliasing effects
if (_ahrs->get_fly_forward()) {
accel = _ahrs->get_ins().get_accel(_ahrs->get_active_accel_instance());
// get accels from dual sensors if healthy
if (_ahrs->get_ins().get_accel_health(0) && _ahrs->get_ins().get_accel_health(1)) {
accel1 = _ahrs->get_ins().get_accel(0);
accel2 = _ahrs->get_ins().get_accel(1);
} else {
accel = _ahrs->get_ins().get_accel();
accel1 = _ahrs->get_ins().get_accel();
accel2 = accel1;
}
// trapezoidal integration
dAngIMU = (angRate + lastAngRate) * dtIMU * 0.5f;
lastAngRate = angRate;
dVelIMU = (accel + lastAccel) * dtIMU * 0.5f;
lastAccel = accel;
dVelIMU1 = (accel1 + lastAccel1) * dtIMU * 0.5f;
lastAccel1 = accel1;
dVelIMU2 = (accel2 + lastAccel2) * dtIMU * 0.5f;
lastAccel2 = accel2;
}
void NavEKF::readGpsData()
@ -2666,10 +2756,9 @@ void NavEKF::ZeroVariables() @@ -2666,10 +2756,9 @@ void NavEKF::ZeroVariables()
storeIndex = 0;
prevDelAng.zero();
lastAngRate.zero();
lastAccel.zero();
lastAccel1.zero();
lastAccel2.zero();
velDotNEDfilt.zero();
lastAngRate.zero();
lastAccel.zero();
summedDelAng.zero();
summedDelVel.zero();
velNED.zero();
@ -2693,7 +2782,7 @@ bool NavEKF::useAirspeed(void) const @@ -2693,7 +2782,7 @@ bool NavEKF::useAirspeed(void) const
/*
see if the vehicle code has demanded static mode
*/
bool NavEKF::static_mode_demanded(void) const
bool NavEKF::static_mode_demanded(void) const
{
return !_ahrs->get_armed() || !_ahrs->get_correct_centrifugal();
}

55
libraries/AP_NavEKF/AP_NavEKF.h
Unescape View File

@ -66,9 +66,10 @@ public: @@ -66,9 +66,10 @@ public:
typedef ftype Vector14[14];
typedef ftype Vector15[15];
typedef ftype Vector22[22];
typedef ftype Vector31[31];
typedef ftype Matrix3[3][3];
typedef ftype Matrix22[22][22];
typedef ftype Matrix22_50[22][50];
typedef ftype Matrix31_50[31][50];
#endif
// Constructor
@ -193,7 +194,7 @@ private: @@ -193,7 +194,7 @@ private:
void StoreStatesReset(void);
// recall state vector stored at closest time to the one specified by msec
void RecallStates(Vector22 &statesForFusion, uint32_t msec);
void RecallStates(Vector31 &statesForFusion, uint32_t msec);
// calculate nav to body quaternions from body to nav rotation matrix
void quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const;
@ -260,19 +261,24 @@ private: @@ -260,19 +261,24 @@ private:
private:
// the states are available in two forms, either as a Vector22, or
// the states are available in two forms, either as a Vector27, or
// broken down as individual elements. Both are equivalent (same
// memory)
Vector22 states;
Vector31 states;
struct state_elements {
Quaternion quat; // 0..3
Vector3f velocity; // 4..6
Vector3f position; // 7..9
Vector3f gyro_bias; // 10..12
float accel_zbias; // 13
Vector2f wind_vel; // 14..15
Vector3f earth_magfield; // 16..18
Vector3f body_magfield; // 19..21
Quaternion quat; // 0..3
Vector3f velocity; // 4..6
Vector3f position; // 7..9
Vector3f gyro_bias; // 10..12
float accel_zbias1; // 13
Vector2f wind_vel; // 14..15
Vector3f earth_magfield; // 16..18
Vector3f body_magfield; // 19..21
float accel_zbias2; // 22
Vector3f vel1; // 23 .. 25
float posD1; // 26
Vector3f vel2; // 27 .. 29
float posD2; // 30
} &state;
// EKF Mavlink Tuneable Parameters
@ -326,21 +332,24 @@ private: @@ -326,21 +332,24 @@ private:
bool posTimeout; // boolean true if position measurements have failed innovation consistency check and timed out
bool hgtTimeout; // boolean true if height measurements have failed innovation consistency check and timed out
Vector22 Kfusion; // Kalman gain vector
Vector31 Kfusion; // Kalman gain vector
Matrix22 KH; // intermediate result used for covariance updates
Matrix22 KHP; // intermediate result used for covariance updates
Matrix22 P; // covariance matrix
Matrix22_50 storedStates; // state vectors stored for the last 50 time steps
Matrix31_50 storedStates; // state vectors stored for the last 50 time steps
uint32_t statetimeStamp[50]; // 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 correctedDelVel12; // delta velocities along the XYZ body axes for weighted average of IMU1 and IMU2 corrected for errors (m/s)
Vector3f correctedDelVel1; // delta velocities along the XYZ body axes for IMU1 corrected for errors (m/s)
Vector3f correctedDelVel2; // delta velocities along the XYZ body axes for IMU2 corrected for errors (m/s)
Vector3f summedDelAng; // corrected & summed delta angles about the xyz body axes (rad)
Vector3f summedDelVel; // corrected & summed delta velocities along the XYZ body axes (m/s)
Vector3f prevDelAng; // previous delta angle use for INS coning error compensation
Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation
ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f dVelIMU; // delta velocity vector in XYZ body axes measured by the IMU (m/s)
Vector3f dVelIMU1; // delta velocity vector in XYZ body axes measured by IMU1 (m/s)
Vector3f dVelIMU2; // delta velocity vector in XYZ body axes measured by IMU2 (m/s)
Vector3f dAngIMU; // delta angle vector in XYZ body axes measured by the IMU (rad)
ftype dtIMU; // time lapsed since the last IMU measurement (sec)
ftype dt; // time lapsed since the last covariance prediction (sec)
@ -355,19 +364,19 @@ private: @@ -355,19 +364,19 @@ private:
Vector3f velNED; // North, East, Down velocity measurements (m/s)
Vector2 posNE; // North, East position measurements (m)
ftype hgtMea; // height measurement relative to reference point (m)
Vector22 statesAtVelTime; // States at the effective time of velNED measurements
Vector22 statesAtPosTime; // States at the effective time of posNE measurements
Vector22 statesAtHgtTime; // States at the effective time of hgtMea measurement
Vector31 statesAtVelTime; // States at the effective time of velNED measurements
Vector31 statesAtPosTime; // States at the effective time of posNE measurements
Vector31 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
Vector22 statesAtMagMeasTime; // filter states at the effective time of compass measurements
Vector31 statesAtMagMeasTime; // filter states at the effective time of compass measurements
ftype innovVtas; // innovation output from fusion of airspeed measurements
ftype 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)
Vector22 statesAtVtasMeasTime; // filter states at the effective measurement time
Vector31 statesAtVtasMeasTime; // filter states at the effective measurement time
Vector3f magBias; // magnetometer bias vector in XYZ body axes
const ftype covTimeStepMax; // maximum time allowed between covariance predictions
const ftype covDelAngMax; // maximum delta angle between covariance predictions
@ -406,13 +415,15 @@ private: @@ -406,13 +415,15 @@ private:
uint32_t lastFixTime_ms; // time of last GPS fix used to determine if new data has arrived
uint32_t secondLastFixTime_ms; // time of second last GPS fix used to determine how long since last update
Vector3f lastAngRate; // angular rate from previous IMU sample used for trapezoidal integrator
Vector3f lastAccel; // acceleration from previous IMU sample used for trapezoidal integrator
Vector3f lastAccel1; // acceleration from previous IMU1 sample used for trapezoidal integrator
Vector3f lastAccel2; // acceleration from previous IMU2 sample used for trapezoidal integrator
Matrix22 nextP; // Predicted covariance matrix before addition of process noise to diagonals
Vector22 processNoise; // process noise added to diagonals of predicted covariance matrix
Vector15 SF; // intermediate variables used to calculate predicted covariance matrix
Vector8 SG; // intermediate variables used to calculate predicted covariance matrix
Vector11 SQ; // intermediate variables used to calculate predicted covariance matrix
Vector8 SPP; // intermediate variables used to calculate predicted covariance matrix
float IMU1_weighting; // Weighting applied to use of IMU1. Varies between 0 and 1.
// states held by magnetomter fusion across time steps
// magnetometer X,Y,Z measurements are fused across three time steps

Write Preview
Loading…
Cancel
Save