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

55
libraries/AP_NavEKF/AP_NavEKF.h
Unescape View File

@ -66,9 +66,10 @@ public:
typedef ftype Vector14[14]; typedef ftype Vector14[14];
typedef ftype Vector15[15]; typedef ftype Vector15[15];
typedef ftype Vector22[22]; typedef ftype Vector22[22];
typedef ftype Vector31[31];
typedef ftype Matrix3[3][3]; typedef ftype Matrix3[3][3];
typedef ftype Matrix22[22][22]; typedef ftype Matrix22[22][22];
typedef ftype Matrix22_50[22][50]; typedef ftype Matrix31_50[31][50];
#endif #endif
// Constructor // Constructor
@ -193,7 +194,7 @@ private:
void StoreStatesReset(void); void StoreStatesReset(void);
// recall state vector stored at closest time to the one specified by msec // 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 // calculate nav to body quaternions from body to nav rotation matrix
void quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const; void quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const;
@ -260,19 +261,24 @@ private:
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 // broken down as individual elements. Both are equivalent (same
// memory) // memory)
Vector22 states; Vector31 states;
struct state_elements { struct state_elements {
Quaternion quat; // 0..3 Quaternion quat; // 0..3
Vector3f velocity; // 4..6 Vector3f velocity; // 4..6
Vector3f position; // 7..9 Vector3f position; // 7..9
Vector3f gyro_bias; // 10..12 Vector3f gyro_bias; // 10..12
float accel_zbias; // 13 float accel_zbias1; // 13
Vector2f wind_vel; // 14..15 Vector2f wind_vel; // 14..15
Vector3f earth_magfield; // 16..18 Vector3f earth_magfield; // 16..18
Vector3f body_magfield; // 19..21 Vector3f body_magfield; // 19..21
float accel_zbias2; // 22
Vector3f vel1; // 23 .. 25
float posD1; // 26
Vector3f vel2; // 27 .. 29
float posD2; // 30
} &state; } &state;
// EKF Mavlink Tuneable Parameters // EKF Mavlink Tuneable Parameters
@ -326,21 +332,24 @@ private:
bool posTimeout; // boolean true if position measurements have failed innovation consistency check and timed out 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 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 KH; // intermediate result used for covariance updates
Matrix22 KHP; // intermediate result used for covariance updates Matrix22 KHP; // intermediate result used for covariance updates
Matrix22 P; // covariance matrix 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 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 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 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 summedDelVel; // corrected & summed delta velocities along the XYZ body axes (m/s)
Vector3f prevDelAng; // previous delta angle use for INS coning error compensation Vector3f prevDelAng; // previous delta angle use for INS coning error compensation
Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation 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) 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 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) 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 dtIMU; // time lapsed since the last IMU measurement (sec)
ftype dt; // time lapsed since the last covariance prediction (sec) ftype dt; // time lapsed since the last covariance prediction (sec)
@ -355,19 +364,19 @@ private:
Vector3f velNED; // North, East, Down velocity measurements (m/s) Vector3f velNED; // North, East, Down velocity measurements (m/s)
Vector2 posNE; // North, East position measurements (m) Vector2 posNE; // North, East position measurements (m)
ftype hgtMea; // height measurement relative to reference point (m) ftype hgtMea; // height measurement relative to reference point (m)
Vector22 statesAtVelTime; // States at the effective time of velNED measurements Vector31 statesAtVelTime; // States at the effective time of velNED measurements
Vector22 statesAtPosTime; // States at the effective time of posNE measurements Vector31 statesAtPosTime; // States at the effective time of posNE measurements
Vector22 statesAtHgtTime; // States at the effective time of hgtMea measurement Vector31 statesAtHgtTime; // States at the effective time of hgtMea measurement
Vector3f innovMag; // innovation output from fusion of X,Y,Z compass measurements 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 Vector3f varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
bool fuseMagData; // boolean true when magnetometer data is to be fused bool fuseMagData; // boolean true when magnetometer data is to be fused
Vector3f magData; // magnetometer flux readings in X,Y,Z body axes 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 innovVtas; // innovation output from fusion of airspeed measurements
ftype varInnovVtas; // innovation variance 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 bool fuseVtasData; // boolean true when airspeed data is to be fused
float VtasMeas; // true airspeed measurement (m/s) 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 Vector3f magBias; // magnetometer bias vector in XYZ body axes
const ftype covTimeStepMax; // maximum time allowed between covariance predictions const ftype covTimeStepMax; // maximum time allowed between covariance predictions
const ftype covDelAngMax; // maximum delta angle between covariance predictions const ftype covDelAngMax; // maximum delta angle between covariance predictions
@ -406,13 +415,15 @@ private:
uint32_t lastFixTime_ms; // time of last GPS fix used to determine if new data has arrived 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 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 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 Matrix22 nextP; // Predicted covariance matrix before addition of process noise to diagonals
Vector22 processNoise; // process noise added to diagonals of predicted covariance matrix Vector22 processNoise; // process noise added to diagonals of predicted covariance matrix
Vector15 SF; // intermediate variables used to calculate predicted covariance matrix Vector15 SF; // intermediate variables used to calculate predicted covariance matrix
Vector8 SG; // 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 Vector11 SQ; // intermediate variables used to calculate predicted covariance matrix
Vector8 SPP; // 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 // states held by magnetomter fusion across time steps
// magnetometer X,Y,Z measurements are fused across three time steps // magnetometer X,Y,Z measurements are fused across three time steps

Write Preview
Loading…
Cancel
Save