diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-18 21:23:59 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-18 21:23:59 +0200 |
commit | cfcb76f627d164b3244e9b81b2fe2fe2e54ee045 (patch) | |
tree | 32c0910fdd95f85b65b8b0626dea57a1aa614c85 /src/modules/ekf_att_pos_estimator/estimator_23states.cpp | |
parent | 1596de25d7271fb2d44731d39bf6aa639b64dfee (diff) | |
download | px4-firmware-cfcb76f627d164b3244e9b81b2fe2fe2e54ee045.tar.gz px4-firmware-cfcb76f627d164b3244e9b81b2fe2fe2e54ee045.tar.bz2 px4-firmware-cfcb76f627d164b3244e9b81b2fe2fe2e54ee045.zip |
Add long-term stable wind estimate
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/estimator_23states.cpp')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 21 |
1 files changed, 21 insertions, 0 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index d1349535d..d176ccee2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -71,6 +71,9 @@ AttPosEKF::AttPosEKF() : dtVelPosFilt(0.01f), dtHgtFilt(0.01f), dtGpsFilt(0.1f), + windSpdFiltNorth(0.0f), + windSpdFiltEast(0.0f), + windSpdFiltAltitude(0.0f), fusionModeGPS(0), innovVelPos{}, varInnovVelPos{}, @@ -1657,6 +1660,24 @@ void AttPosEKF::FuseAirspeed() // Perform fusion of True Airspeed measurement if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) { + // Lowpass the output of the wind estimate - we want a long-term + // stable estimate, but not start to load into the overall dynamics + // of the system (which adjusting covariances would do) + + // Nominally damp to 0.02% of the noise, but reduce the damping for strong altitude variations + // assuming equal wind speeds on the same altitude and varying wind speeds on + // different altitudes + float windFiltCoeff = 0.0002f; + + float altDiff = fabsf(windSpdFiltAltitude - hgtMea); + + // Change filter coefficient based on altitude + windFiltCoeff += ConstrainFloat(0.00001f * altDiff, windFiltCoeff, 0.1f); + + windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn); + windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe); + windSpdFiltAltitude = hgtMea; + // Calculate observation jacobians SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; |