mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 09:58:28 -04:00
AP_NavEKF : Add accel aliasing protection
This commit is contained in:
Paul Riseborough
Andrew Tridgell
parent
f2c79ad639
commit
bd152d332c
@ -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 = {
|
||||
|
||||
// @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) :
|
||||
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)
|
||||
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)
|
||||
// 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)
|
||||
// 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)
|
||||
// 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()
|
||||
|
||||
// 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()
|
||||
void NavEKF::UpdateStrapdownEquationsNED()
|
||||
{
|
||||
Vector3f delVelNav;
|
||||
Vector3f delVelNav1;
|
||||
Vector3f delVelNav2;
|
||||
float rotationMag;
|
||||
float rotScaler;
|
||||
Quaternion qUpdated;
|
||||
@ -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()
|
||||
|
||||
// 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()
|
||||
|
||||
// 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()
|
||||
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()
|
||||
|
||||
// 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()
|
||||
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()
|
||||
{
|
||||
// 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()
|
||||
{
|
||||
indexLimit = 13;
|
||||
}
|
||||
|
||||
// Fuse measurements sequentially
|
||||
for (obsIndex=0; obsIndex<=5; obsIndex++)
|
||||
{
|
||||
@ -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) {
|
||||
Kfusion[13] = 0.0f;
|
||||
// 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
|
||||
}
|
||||
// Calculate state corrections and re-normalise the quaternions
|
||||
// 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 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)
|
||||
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()
|
||||
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)
|
||||
}
|
||||
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
|
||||
|
||||
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()
|
||||
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()
|
||||
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()
|
||||
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
|
||||
/*
|
||||
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();
|
||||
}
|
||||
|
||||
@ -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:
|
||||
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:
|
||||
|
||||
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:
|
||||
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:
|
||||
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:
|
||||
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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user