aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Fix bug for start of wind speed estimation

Browse Source 
The commencement of airspeed fusion could cause pitch errors due to small variances and large innovations. This issue is addressed by the following changes:
1) The airspeed measurement is used to set wind states to a value that reduces initial innovations.
2) The wind state variances are set to values that better reflect the wind speed uncertainty
This commit is contained in:
Paul Riseborough 2016-05-24 11:49:42 +10:00 committed by Andrew Tridgell
parent ac329ec31c
commit 722eb0d706
1 changed files with 26 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -46,7 +46,32 @@ void NavEKF2_core::controlFilterModes()
void NavEKF2_core::setWindMagStateLearningMode()
{
// If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE));
bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE);
if (!inhibitWindStates && setWindInhibit) {
inhibitWindStates = true;
} else if (inhibitWindStates && !setWindInhibit) {
inhibitWindStates = false;
// set states and variances
if (yawAlignComplete && useAirspeed()) {
// if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading
// which assumes the vehicle has launched into the wind
Vector3f tempEuler;
stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
float windSpeed = sqrtf(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas;
stateStruct.wind_vel.x = windSpeed * cosf(tempEuler.z);
stateStruct.wind_vel.y = windSpeed * sinf(tempEuler.z);
// set the wind sate variances to the measurement uncertainty
for (uint8_t index=22; index<=23; index++) {
P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(_ahrs->get_EAS2TAS(), 0.9f, 10.0f));
}
} else {
// set the variances using a typical wind speed
for (uint8_t index=22; index<=23; index++) {
P[index][index] = sq(5.0f);
}
}
}
// determine if the vehicle is manoevring
if (accNavMagHoriz > 0.5f) {