Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: simplify variable handling in EKF

Browse Source 
use named state variables instead of states[] array where possible.
This commit is contained in:
Andrew Tridgell 2014-10-01 12:50:53 +10:00
parent 8223a0d193
commit a1f5be5b9e
1 changed files with 56 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

145
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -401,15 +401,15 @@ bool NavEKF::PositionDrifting(void) const
void NavEKF::ResetPosition(void) void NavEKF::ResetPosition(void)
{ {
if (staticMode) { if (staticMode) {
states[7] = 0; state.position.x = 0;
states[8] = 0; state.position.y = 0;
} else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) { } else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
// read the GPS // read the GPS
readGpsData(); readGpsData();
// write to state vector and compensate for GPS latency // write to state vector and compensate for GPS latency
states[7] = gpsPosNE.x + gpsPosGlitchOffsetNE.x + 0.001f*velNED.x*float(_msecPosDelay); state.position.x = gpsPosNE.x + gpsPosGlitchOffsetNE.x + 0.001f*velNED.x*float(_msecPosDelay);
states[8] = gpsPosNE.y + gpsPosGlitchOffsetNE.y + 0.001f*velNED.y*float(_msecPosDelay); state.position.y = gpsPosNE.y + gpsPosGlitchOffsetNE.y + 0.001f*velNED.y*float(_msecPosDelay);
} }
// reset the glitch ofset correction states // reset the glitch ofset correction states
gpsPosGlitchOffsetNE.zero(); gpsPosGlitchOffsetNE.zero();
@ -435,20 +435,12 @@ void NavEKF::ResetVelocity(void)
velNED[2] = 0.0f; velNED[2] = 0.0f;
} }
// reset filter velocity states // reset filter velocity states
states[4] = velNED[0]; // north velocity from blended accel data state.velocity = velNED;
states[5] = velNED[1]; // east velocity from blended accel data state.vel1 = velNED;
states[23] = velNED[0]; // north velocity from IMU1 accel data state.vel2 = velNED;
states[24] = velNED[1]; // east velocity from IMU1 accel data
states[27] = velNED[0]; // north velocity from IMU2 accel data
states[28] = velNED[1]; // east velocity from IMU2 accel data
states[6] = velNED[2]; // down velocity from blended accel data
states[25] = velNED[2]; // down velocity from IMU1 accel data
states[29] = velNED[2]; // down velocity from IMU2 accel data
// reset stored velocity states to prevent subsequent GPS measurements from being rejected // reset stored velocity states to prevent subsequent GPS measurements from being rejected
for (uint8_t i=0; i<=49; i++){ for (uint8_t i=0; i<=49; i++){
storedStates[i].velocity[0] = velNED[0]; storedStates[i].velocity = velNED;
storedStates[i].velocity[1] = velNED[1];
storedStates[i].velocity[2] = velNED[2];
} }
} }
} }
@ -459,12 +451,12 @@ void NavEKF::ResetHeight(void)
// read the altimeter // read the altimeter
readHgtData(); readHgtData();
// write to the state vector // write to the state vector
states[9] = -hgtMea; // down position from blended accel data state.position.z = -hgtMea; // down position from blended accel data
state.posD1 = -hgtMea; // down position from IMU1 accel data state.posD1 = -hgtMea; // down position from IMU1 accel data
state.posD2 = -hgtMea; // down position from IMU2 accel data state.posD2 = -hgtMea; // down position from IMU2 accel data
// reset stored vertical position states to prevent subsequent GPS measurements from being rejected // reset stored vertical position states to prevent subsequent GPS measurements from being rejected
for (uint8_t i=0; i<=49; i++){ for (uint8_t i=0; i<=49; i++){
storedStates[i].position[2] = -hgtMea; storedStates[i].position.z = -hgtMea;
} }
} }
@ -491,11 +483,9 @@ void NavEKF::InitialiseFilterDynamic(void)
magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv); magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv);
// calculate initial orientation and earth magnetic field states // calculate initial orientation and earth magnetic field states
Quaternion initQuat; state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
initQuat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
// write to state vector // write to state vector
state.quat = initQuat;
state.gyro_bias.zero(); state.gyro_bias.zero();
state.accel_zbias1 = 0; state.accel_zbias1 = 0;
state.accel_zbias2 = 0; state.accel_zbias2 = 0;
@ -912,9 +902,8 @@ void NavEKF::UpdateStrapdownEquationsNED()
state.quat = qUpdated; state.quat = qUpdated;
// calculate the body to nav cosine matrix // calculate the body to nav cosine matrix
Quaternion q(states[0],states[1],states[2],states[3]);
Matrix3f Tbn_temp; Matrix3f Tbn_temp;
q.rotation_matrix(Tbn_temp); state.quat.rotation_matrix(Tbn_temp);
prevTnb = Tbn_temp.transposed(); prevTnb = Tbn_temp.transposed();
// transform body delta velocities to delta velocities in the nav frame // transform body delta velocities to delta velocities in the nav frame
@ -990,7 +979,7 @@ void NavEKF::CovariancePrediction()
// this allows for wind gradient effects. // this allows for wind gradient effects.
// filter height rate using a 10 second time constant filter // filter height rate using a 10 second time constant filter
float alpha = 0.1f * dt; float alpha = 0.1f * dt;
hgtRate = hgtRate * (1.0f - alpha) - states[6] * alpha; hgtRate = hgtRate * (1.0f - alpha) - state.velocity.z * alpha;
// use filtered height rate to increase wind process noise when climbing or descending // use filtered height rate to increase wind process noise when climbing or descending
// this allows for wind gradient effects. // this allows for wind gradient effects.
@ -1030,14 +1019,14 @@ void NavEKF::CovariancePrediction()
dax = summedDelAng.x; dax = summedDelAng.x;
day = summedDelAng.y; day = summedDelAng.y;
daz = summedDelAng.z; daz = summedDelAng.z;
q0 = states[0]; q0 = state.quat[0];
q1 = states[1]; q1 = state.quat[1];
q2 = states[2]; q2 = state.quat[2];
q3 = states[3]; q3 = state.quat[3];
dax_b = states[10]; dax_b = state.gyro_bias.x;
day_b = states[11]; day_b = state.gyro_bias.y;
daz_b = states[12]; daz_b = state.gyro_bias.z;
dvz_b = IMU1_weighting * states[13] + (1.0f - IMU1_weighting) * states[22]; dvz_b = IMU1_weighting * state.accel_zbias1 + (1.0f - IMU1_weighting) * state.accel_zbias2;
_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);
@ -1917,8 +1906,8 @@ void NavEKF::FuseVelPosNED()
float hgtInnov2 = statesAtHgtTime.posD2 - observation[obsIndex]; float hgtInnov2 = statesAtHgtTime.posD2 - observation[obsIndex];
// Correct single IMU prediction states using height measurement, limiting rate of change of bias to 0.02 m/s3 // Correct single IMU prediction states using height measurement, limiting rate of change of bias to 0.02 m/s3
float correctionLimit = 0.02f * dtIMU *dtVelPos; float correctionLimit = 0.02f * dtIMU *dtVelPos;
states[13] = states[13] - constrain_float(Kfusion[13] * hgtInnov1, -correctionLimit, correctionLimit); // IMU1 Z accel bias state.accel_zbias1 -= constrain_float(Kfusion[13] * hgtInnov1, -correctionLimit, correctionLimit); // IMU1 Z accel bias
states[22] = states[22] - constrain_float(Kfusion[22] * hgtInnov2, -correctionLimit, correctionLimit); // IMU2 Z accel bias state.accel_zbias2 -= constrain_float(Kfusion[22] * hgtInnov2, -correctionLimit, correctionLimit); // IMU2 Z accel bias
for (uint8_t i = 23; i<=26; i++) { for (uint8_t i = 23; i<=26; i++) {
states[i] = states[i] - Kfusion[i] * hgtInnov1; // IMU1 velNED,posD states[i] = states[i] - Kfusion[i] * hgtInnov1; // IMU1 velNED,posD
} }
@ -2475,11 +2464,8 @@ void NavEKF::FuseAirspeed()
states[j] = states[j] - Kfusion[j] * innovVtas; states[j] = states[j] - Kfusion[j] * innovVtas;
} }
Quaternion q(states[0], states[1], states[2], states[3]); state.quat.normalize();
q.normalize();
for (uint8_t i = 0; i<=3; i++) {
states[i] = q[i];
}
// correct the covariance P = (I - K*H)*P // correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in H to reduce the number of operations // take advantage of the empty columns in H to reduce the number of operations
for (uint8_t i = 0; i<=21; i++) for (uint8_t i = 0; i<=21; i++)
@ -2553,15 +2539,15 @@ void NavEKF::FuseSideslip()
float innovBeta; float innovBeta;
// copy required states to local variable names // copy required states to local variable names
q0 = states[0]; q0 = state.quat[0];
q1 = states[1]; q1 = state.quat[1];
q2 = states[2]; q2 = state.quat[2];
q3 = states[3]; q3 = state.quat[3];
vn = states[4]; vn = state.velocity.x;
ve = states[5]; ve = state.velocity.y;
vd = states[6]; vd = state.velocity.z;
vwn = states[14]; vwn = state.wind_vel.x;
vwe = states[15]; vwe = state.wind_vel.y;
// calculate predicted wind relative velocity in NED // calculate predicted wind relative velocity in NED
vel_rel_wind.x = vn - vwn; vel_rel_wind.x = vn - vwn;
@ -2669,11 +2655,8 @@ void NavEKF::FuseSideslip()
states[j] = states[j] - Kfusion[j] * innovBeta; states[j] = states[j] - Kfusion[j] * innovBeta;
} }
Quaternion q(states[0], states[1], states[2], states[3]); state.quat.normalize();
q.normalize();
for (uint8_t i = 0; i<=3; i++) {
states[i] = q[i];
}
// correct the covariance P = (I - K*H)*P // correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in H to reduce the // take advantage of the empty columns in H to reduce the
// number of operations // number of operations
@ -2805,26 +2788,21 @@ void NavEKF::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const
// return the Euler roll, pitch and yaw angle in radians // return the Euler roll, pitch and yaw angle in radians
void NavEKF::getEulerAngles(Vector3f &euler) const void NavEKF::getEulerAngles(Vector3f &euler) const
{ {
Quaternion q(states[0], states[1], states[2], states[3]); state.quat.to_euler(&euler.x, &euler.y, &euler.z);
q.to_euler(&euler.x, &euler.y, &euler.z);
euler = euler - _ahrs->get_trim(); euler = euler - _ahrs->get_trim();
} }
// return NED velocity in m/s // return NED velocity in m/s
void NavEKF::getVelNED(Vector3f &vel) const void NavEKF::getVelNED(Vector3f &vel) const
{ {
vel.x = states[4]; vel = state.velocity;
vel.y = states[5];
vel.z = states[6];
} }
// return the last calculated NED position relative to the reference point (m). // return the last calculated NED position relative to the reference point (m).
// return false if no position is available // return false if no position is available
bool NavEKF::getPosNED(Vector3f &pos) const bool NavEKF::getPosNED(Vector3f &pos) const
{ {
pos.x = states[7]; pos = state.position;
pos.y = states[8];
pos.z = states[9];
return true; return true;
} }
@ -2835,9 +2813,7 @@ void NavEKF::getGyroBias(Vector3f &gyroBias) const
gyroBias.zero(); gyroBias.zero();
return; return;
} }
gyroBias.x = states[10] / dtIMU; gyroBias = state.gyro_bias / dtIMU;
gyroBias.y = states[11] / dtIMU;
gyroBias.z = states[12] / dtIMU;
} }
// return weighting of first IMU in blending function and the individual Z-accel bias estimates in m/s^2 // return weighting of first IMU in blending function and the individual Z-accel bias estimates in m/s^2
@ -2848,33 +2824,29 @@ void NavEKF::getAccelBias(Vector3f &accelBias) const
accelBias.y = 0; accelBias.y = 0;
accelBias.z = 0; accelBias.z = 0;
} else { } else {
accelBias.y = states[22] / dtIMU; accelBias.y = state.accel_zbias2 / dtIMU;
accelBias.z = states[13] / dtIMU; accelBias.z = state.accel_zbias1 / dtIMU;
} }
} }
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
void NavEKF::getWind(Vector3f &wind) const void NavEKF::getWind(Vector3f &wind) const
{ {
wind.x = states[14]; wind.x = state.wind_vel.x;
wind.y = states[15]; wind.y = state.wind_vel.y;
wind.z = 0.0f; // currently don't estimate this wind.z = 0.0f; // currently don't estimate this
} }
// return earth magnetic field estimates in measurement units / 1000 // return earth magnetic field estimates in measurement units / 1000
void NavEKF::getMagNED(Vector3f &magNED) const void NavEKF::getMagNED(Vector3f &magNED) const
{ {
magNED.x = states[16]*1000.0f; magNED = state.earth_magfield * 1000.0f;
magNED.y = states[17]*1000.0f;
magNED.z = states[18]*1000.0f;
} }
// return body magnetic field estimates in measurement units / 1000 // return body magnetic field estimates in measurement units / 1000
void NavEKF::getMagXYZ(Vector3f &magXYZ) const void NavEKF::getMagXYZ(Vector3f &magXYZ) const
{ {
magXYZ.x = states[19]*1000.0f; magXYZ = state.body_magfield*1000.0f;
magXYZ.y = states[20]*1000.0f;
magXYZ.z = states[21]*1000.0f;
} }
// return the last calculated latitude, longitude and height // return the last calculated latitude, longitude and height
@ -2882,10 +2854,10 @@ bool NavEKF::getLLH(struct Location &loc) const
{ {
loc.lat = _ahrs->get_home().lat; loc.lat = _ahrs->get_home().lat;
loc.lng = _ahrs->get_home().lng; loc.lng = _ahrs->get_home().lng;
loc.alt = _ahrs->get_home().alt - states[9]*100; loc.alt = _ahrs->get_home().alt - state.position.z*100;
loc.flags.relative_alt = 0; loc.flags.relative_alt = 0;
loc.flags.terrain_alt = 0; loc.flags.terrain_alt = 0;
location_offset(loc, states[7], states[8]); location_offset(loc, state.position.x, state.position.y);
return true; return true;
} }
@ -3281,24 +3253,20 @@ void NavEKF::alignYawGPS()
float newYaw; float newYaw;
float yawErr; float yawErr;
// get quaternion from existing filter states and calculate roll, pitch and yaw angles // get quaternion from existing filter states and calculate roll, pitch and yaw angles
Quaternion initQuat; state.quat.to_euler(&roll, &pitch, &oldYaw);
Quaternion newQuat;
for (uint8_t i=0; i<=3; i++) initQuat[i] = states[i];
initQuat.to_euler(&roll, &pitch, &oldYaw);
// calculate yaw angle from GPS velocity // calculate yaw angle from GPS velocity
newYaw = atan2f(velNED[1],velNED[0]); newYaw = atan2f(velNED[1],velNED[0]);
// modify yaw angle using GPS ground course if more than 45 degrees away or if not previously aligned // modify yaw angle using GPS ground course if more than 45 degrees away or if not previously aligned
yawErr = fabsf(newYaw - oldYaw); yawErr = fabsf(newYaw - oldYaw);
if (((yawErr > 0.7854f) && (yawErr < 5.4978f)) || !yawAligned) { if (((yawErr > 0.7854f) && (yawErr < 5.4978f)) || !yawAligned) {
// calculate new filter quaternion states from Euler angles // calculate new filter quaternion states from Euler angles
newQuat.from_euler(roll, pitch, newYaw); state.quat.from_euler(roll, pitch, newYaw);
for (uint8_t i=0; i<=3; i++) states[i] = newQuat[i];
// the yaw angle is now aligned so update its status // the yaw angle is now aligned so update its status
yawAligned = true; yawAligned = true;
// set the velocity states // set the velocity states
if (_fusionModeGPS < 2) { if (_fusionModeGPS < 2) {
states[4] = velNED[0]; state.velocity.x = velNED.x;
states[5] = velNED[1]; state.velocity.y = velNED.y;
} }
// reinitialise the quaternion, velocity and position covariances // reinitialise the quaternion, velocity and position covariances
// zero the matrix entries // zero the matrix entries
@ -3329,12 +3297,12 @@ void NavEKF::alignYawGPS()
// representative of typical launch wind // representative of typical launch wind
void NavEKF::setWindVelStates() void NavEKF::setWindVelStates()
{ {
float gndSpd = sqrtf(sq(states[4]) + sq(states[5])); float gndSpd = pythagorous2(state.velocity.x, state.velocity.y);
if (gndSpd > 4.0f) { if (gndSpd > 4.0f) {
// set the wind states to be the reciprocal of the velocity and scale // set the wind states to be the reciprocal of the velocity and scale
float scaleFactor = STARTUP_WIND_SPEED / gndSpd; float scaleFactor = STARTUP_WIND_SPEED / gndSpd;
states[14] = - states[4] * scaleFactor; state.wind_vel.x = - state.velocity.x * scaleFactor;
states[15] = - states[5] * scaleFactor; state.wind_vel.y = - state.velocity.y * scaleFactor;
// reinitialise the wind state covariances // reinitialise the wind state covariances
zeroRows(P,14,15); zeroRows(P,14,15);
zeroCols(P,14,15); zeroCols(P,14,15);
@ -3347,8 +3315,7 @@ void NavEKF::setWindVelStates()
void NavEKF::getRotationBodyToNED(Matrix3f &mat) const void NavEKF::getRotationBodyToNED(Matrix3f &mat) const
{ {
Vector3f trim = _ahrs->get_trim(); Vector3f trim = _ahrs->get_trim();
Quaternion q(states[0], states[1], states[2], states[3]); state.quat.rotation_matrix(mat);
q.rotation_matrix(mat);
mat.rotateXYinv(trim); mat.rotateXYinv(trim);
} }