Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : fixed bug in velocity rate of change filter 

This bug meant the velocity rate of change used to scale the GPS measurement variances was noisy and too small
This commit is contained in:
Paul Riseborough 2014-02-23 18:21:42 +11:00 committed by priseborough
parent 01c84c3f47
commit 5db9a87d31
2 changed files with 7 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -224,9 +224,9 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
{
AP_Param::setup_object_defaults(this, var_info);
// Tuning parameters
_gpsNEVelVarAccScale = 0.1f; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
_gpsDVelVarAccScale = 0.15f; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
_gpsPosVarAccScale = 0.1f; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
_gpsNEVelVarAccScale = 0.05f; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
_gpsDVelVarAccScale = 0.07f; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
_gpsPosVarAccScale = 0.05f; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
_msecHgtDelay = 60; // Height measurement delay (msec)
_msecMagDelay = 40; // Magnetometer measurement delay (msec)
_msecTasDelay = 240; // Airspeed measurement delay (msec)
@ -724,12 +724,12 @@ void NavEKF::UpdateStrapdownEquationsNED()
// Calculate the rate of change of velocity (used for launch detect and other functions)
velDotNED = delVelNav / dtIMU ;
// Calculate a filtered
velDotNEDfilt = velDotNED * 0.05f + lastVelDotNED * 0.95f;
// Apply a first order lowpass filter
velDotNEDfilt = velDotNED * 0.05f + velDotNEDfilt * 0.95f;
// calculate a magnitude of the filtered nav acceleration (required for GPS
// variance estimation)
accNavMag = velDotNED.length();
accNavMag = velDotNEDfilt.length();
// If calculating position save previous velocity
Vector3f lastVelocity = state.velocity;
@ -2629,7 +2629,7 @@ void NavEKF::ZeroVariables()
prevDelAng.zero();
lastAngRate.zero();
lastAccel.zero();
lastVelDotNED.zero();
velDotNEDfilt.zero();
lastAngRate.zero();
lastAccel.zero();
summedDelAng.zero();

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -386,7 +386,6 @@ private:
uint32_t lastMagUpdate; // last time compass was updated
Vector3f velDotNED; // rate of change of velocity in NED frame
Vector3f velDotNEDfilt; // low pass filtered velDotNED
Vector3f lastVelDotNED; // velDotNED filter state
uint32_t lastAirspeedUpdate; // last time airspeed was updated
uint32_t IMUmsec; // time that the last IMU value was taken
ftype gpsCourse; // GPS ground course angle(rad)