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 | |
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')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 21 | ||||
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_23states.h | 3 |
2 files changed, 24 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; diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 12cbc53b8..6349b03f0 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -149,6 +149,9 @@ public: float dtVelPosFilt; // average time between position / velocity fusion steps float dtHgtFilt; // average time between height measurement updates float dtGpsFilt; // average time between gps measurement updates + float windSpdFiltNorth; // average wind speed north component + float windSpdFiltEast; // average wind speed east component + float windSpdFiltAltitude; // the last altitude used to filter wind speed uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity float innovVelPos[6]; // innovation output float varInnovVelPos[6]; // innovation variance output |