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