From b63d0969b76fe42612289771795f57b1d3e8220f Mon Sep 17 00:00:00 2001 From: priseborough Date: Fri, 29 Mar 2013 20:42:26 +1100 Subject: [PATCH] AP_AHRS: Addition of a first order complementary filter to AP_AHRS::groundspeed_vector Addition of a complementary filter to estimation of the ground velocity vector for use by the L1-nav --- libraries/AP_AHRS/AP_AHRS.cpp | 77 +++++++++++++++++++++++++++-------- libraries/AP_AHRS/AP_AHRS.h | 13 +++++- 2 files changed, 70 insertions(+), 20 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index feddd9dd56..7814620349 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -12,7 +12,7 @@ extern const AP_HAL::HAL& hal; // table of user settable parameters const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = { - // index 0 and 1 are for old parameters that are no longer used + // index 0 and 1 are for old parameters that are no longer not used // @Param: GPS_GAIN // @DisplayName: AHRS GPS gain @@ -82,6 +82,13 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = { // @User: Advanced AP_GROUPINFO("ORIENTATION", 9, AP_AHRS, _board_orientation, 0), + // @Param: COMP_BETA + // @DisplayName: AHRS Velocity Complmentary Filter Beta Coefficient + // @Description: This controls the time constant for the cross-over frequency used to fuse AHRS (airspeed and heading) and GPS data to estimate ground velocity. Time constant is 0.1/beta. A larger time constant will use GPS data less and a small time constant will use air data less. + // @Range: 0.001 0.5 + // @Increment: .01 + AP_GROUPINFO("COMP_BETA", 10, AP_AHRS, beta, 0.1f), + AP_GROUPEND }; @@ -166,21 +173,55 @@ void AP_AHRS::wind_correct_bearing(int32_t &nav_bearing_cd) // return a ground speed estimate in m/s Vector2f AP_AHRS::groundspeed_vector(void) { - if (_gps && _gps->status() >= GPS::GPS_OK_FIX_2D) { - Vector2f v; - float cog = radians(_gps->ground_course*0.01f); - return Vector2f(cos(cog), sin(cog)) * _gps->ground_speed * 0.01f; - } - // we don't have ground speed - see if we can estimate from - // airspeed and the wind estimate - float airspeed; - if (!airspeed_estimate(&airspeed)) { - // we have no idea what our ground vector is! - return Vector2f(0.0f, 0.0f); - } - Vector3f wind = wind_estimate(); - Vector2f wind2d = Vector2f(wind.x, wind.y); - Vector2f airspeed_vector = Vector2f(cos(yaw), sin(yaw)) * airspeed; - Vector2f adjusted = airspeed_vector - wind2d; - return adjusted; + // Generate estimate of ground speed vector using air data system + Vector2f gndVelADS; + Vector2f gndVelGPS; + float airspeed; + bool gotAirspeed = airspeed_estimate(&airspeed); + bool gotGPS = (_gps && _gps->status() >= GPS::GPS_OK_FIX_2D); + if (gotAirspeed) { + Vector3f wind = wind_estimate(); + Vector2f wind2d = Vector2f(wind.x, wind.y); + Vector2f airspeed_vector = Vector2f(cos(yaw), sin(yaw)) * airspeed; + gndVelADS = airspeed_vector - wind2d; + } + + // Generate estimate of ground speed vector using GPS + if (gotGPS) { + Vector2f v; + float cog = radians(_gps->ground_course*0.01f); + gndVelGPS = Vector2f(cos(cog), sin(cog)) * _gps->ground_speed * 0.01f; + } + // If both ADS and GPS data is available, apply a complementary filter + if (gotAirspeed && gotGPS) { + // The LPF is applied to the GPS and the HPF is applied to the air data estimate + // before the two are summed + //Define filter coefficients + // alpha and beta must sum to one + // beta = dt/Tau, where + // dt = filter time step (0.1 sec if called by nav loop) + // Tau = cross-over time constant (nominal 2 seconds) + // More lag on GPS requires Tau to be bigger, less lag allows it to be smaller + // To-Do - set Tau as a function of GPS lag. + const float alpha = 1.0f - beta; + // Run LP filters + _xlp = beta*gndVelGPS.x + alpha*_xlp; + _ylp = beta*gndVelGPS.y + alpha*_ylp; + // Run HP filters + _xhp = gndVelADS.x - _lastGndVelADS.x + alpha*_xhp; + _yhp = gndVelADS.y - _lastGndVelADS.y + alpha*_yhp; + // Save the current ADS ground vector for the next time step + _lastGndVelADS = gndVelADS; + // Sum the HP and LP filter outputs + return Vector2f(_xhp + _xlp, _yhp + _ylp); + } + // Only ADS data is available return ADS estimate + if (gotAirspeed && !gotGPS) { + return gndVelADS; + } + // Only GPS data is available so return GPS estimate + if (!gotAirspeed && gotGPS) { + return gndVelGPS; + } + return Vector2f(0.0f, 0.0f); } diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index bb4bda8e89..cb995add82 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -166,7 +166,8 @@ public: virtual void add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom = true); // settable parameters - AP_Float _kp_yaw; + AP_Float beta; + AP_Float _kp_yaw; AP_Float _kp; AP_Float gps_gain; AP_Int8 _gps_use; @@ -214,7 +215,15 @@ protected: // accelerometer values in the earth frame in m/s/s Vector3f _accel_ef; -}; + // Declare filter states for HPF and LPF used by complementary + // filter in AP_AHRS::groundspeed_vector + float _xlp; // x component low-pass filter + float _ylp; // y component low-pass filter + float _xhp; // x component high-pass filter + float _yhp; // y component high-pass filter + Vector2f _lastGndVelADS; // previous HPF input + + }; #include #include