From 893d2da6f603808c00b59347811d2652fe15cc31 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 20 Jul 2013 08:14:58 +1000 Subject: [PATCH] AP_Airspeed: added auto-calibration support This uses a Kalman filter to calculate the right ARSPD_RATIO at runtime Pair-Programmed-With: Paul Riseborough --- libraries/AP_Airspeed/AP_Airspeed.cpp | 10 ++ libraries/AP_Airspeed/AP_Airspeed.h | 42 +++++- .../AP_Airspeed/Airspeed_Calibration.cpp | 130 ++++++++++++++++++ libraries/AP_Airspeed/models/ADS_cal_EKF.m | 122 ++++++++++++++++ 4 files changed, 302 insertions(+), 2 deletions(-) create mode 100644 libraries/AP_Airspeed/Airspeed_Calibration.cpp create mode 100644 libraries/AP_Airspeed/models/ADS_cal_EKF.m diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index a623c1be40..55333651f4 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -70,6 +70,12 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] PROGMEM = { // @User: Advanced AP_GROUPINFO("PIN", 4, AP_Airspeed, _pin, ARSPD_DEFAULT_PIN), + // @Param: AUTOCAL + // @DisplayName: Automatic airspeed ratio calibration + // @Description: If this is enabled then the APM will automatically adjust the ARSPD_RATIO during flight, based upon an estimation filter using ground speed and true airspeed. The automatic calibration will save the new ratio to EEPROM every 2 minutes if it changes by more than 5% + // @User: Advanced + AP_GROUPINFO("AUTOCAL", 5, AP_Airspeed, _autocal, 0), + AP_GROUPEND }; @@ -105,6 +111,10 @@ void AP_Airspeed::init() } #endif _source = hal.analogin->channel(_pin); + + _calibration.init(_ratio); + _last_saved_ratio = _ratio; + _counter = 0; } // read the airspeed sensor diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index e3cc5efa94..7b39b13617 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -7,11 +7,35 @@ #include #include +class Airspeed_Calibration { +public: + // constructor + Airspeed_Calibration(void); + + // initialise the calibration + void init(float initial_ratio); + + // take current airspeed in m/s and ground speed vector and return + // new scaling factor + float update(float airspeed, const Vector3f &vg); + +private: + // state of kalman filter for airspeed ratio estimation + Matrix3f P; // covarience matrix + const float Q0; // process noise matrix top left and middle element + const float Q1; // process noise matrix bottom right element + Vector3f state; // state vector + const float DT; // time delta + +}; + class AP_Airspeed { public: // constructor - AP_Airspeed() : _ets_fd(-1) { + AP_Airspeed() : + _ets_fd(-1) + { AP_Param::setup_object_defaults(this, var_info); }; @@ -69,6 +93,15 @@ public: _airspeed = airspeed; } + // return the differential pressure in Pascal for the last + // airspeed reading. Used by the calibration code + float get_differential_pressure(void) const { + return max(_last_pressure - _offset, 0); + } + + // update airspeed ratio calibration + void update_calibration(Vector3f vground, float EAS2TAS); + static const struct AP_Param::GroupInfo var_info[]; private: @@ -78,14 +111,19 @@ private: AP_Int8 _use; AP_Int8 _enable; AP_Int8 _pin; + AP_Int8 _autocal; float _raw_airspeed; float _airspeed; int _ets_fd; float _last_pressure; + Airspeed_Calibration _calibration; + float _last_saved_ratio; + uint8_t _counter; + // return raw differential pressure in Pascal float get_pressure(void); }; - #endif // __AP_AIRSPEED_H__ + diff --git a/libraries/AP_Airspeed/Airspeed_Calibration.cpp b/libraries/AP_Airspeed/Airspeed_Calibration.cpp new file mode 100644 index 0000000000..cced38fe96 --- /dev/null +++ b/libraries/AP_Airspeed/Airspeed_Calibration.cpp @@ -0,0 +1,130 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + * auto_calibration.cpp - airspeed auto calibration + + * Algorithm by Paul Riseborough + * + */ + +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +// constructor - fill in all the initial values +Airspeed_Calibration::Airspeed_Calibration() : + P(100, 0, 0, + 0, 100, 0, + 0, 0, 0.000001f), + Q0(0.01f), + Q1(0.000001f), + state(0, 0, 0), + DT(1) +{ +} + +/* + initialise the ratio + */ +void Airspeed_Calibration::init(float initial_ratio) +{ + state.z = 1.0 / sqrtf(initial_ratio); +} + +/* + update the state of the airspeed calibration - needs to be called + once a second + + On an AVR2560 this costs 1.9 milliseconds per call + */ +float Airspeed_Calibration::update(float airspeed, const Vector3f &vg) +{ + // Perform the covariance prediction + // Q is a diagonal matrix so only need to add three terms in + // C code implementation + // P = P + Q; + P.a.x += Q0; + P.b.y += Q0; + P.c.z += Q1; + + // Perform the predicted measurement using the current state estimates + // No state prediction required because states are assumed to be time + // invariant plus process noise + // Ignore vertical wind component + float TAS_pred = state.z * sqrtf(sq(vg.x - state.x) + sq(vg.y - state.y) + sq(vg.z)); + float TAS_mea = airspeed; + + // Calculate the observation Jacobian H_TAS + float SH1 = sq(vg.y - state.y) + sq(vg.x - state.x); + if (SH1 < 0.000001f) { + // avoid division by a small number + return state.z; + } + float SH2 = 1/sqrt(SH1); + + // observation Jacobian + Vector3f H_TAS( + -(state.z*SH2*(2*vg.x - 2*state.x))/2, + -(state.z*SH2*(2*vg.y - 2*state.y))/2, + 1/SH2); + + // Calculate the fusion innovaton covariance assuming a TAS measurement + // noise of 1.0 m/s + // S = H_TAS*P*H_TAS' + 1.0; % [1 x 3] * [3 x 3] * [3 x 1] + [1 x 1] + Vector3f PH = P * H_TAS; + float S = H_TAS * PH + 1.0f; + + // Calculate the Kalman gain + // [3 x 3] * [3 x 1] / [1 x 1] + Vector3f KG = PH / S; + + // Update the states + state += KG*(TAS_mea - TAS_pred); // [3 x 1] + [3 x 1] * [1 x 1] + + // Update the covariance matrix + Vector3f HP2 = H_TAS * P; + P -= KG.mul_rowcol(HP2); + + // force symmetry on the covariance matrix - necessary due to rounding + // errors + // Implementation will also need a further check to prevent diagonal + // terms becoming negative due to rounding errors + // This step can be made more efficient by excluding diagonal terms + // (would reduce processing by 1/3) + P = (P + P.transpose()) * 0.5f; // [1 x 1] * ( [3 x 3] + [3 x 3]) + + return state.z; +} + + +/* + called once a second to do calibration update + */ +void AP_Airspeed::update_calibration(Vector3f vground, float EAS2TAS) +{ + if (!_autocal) { + // auto-calibration not enabled + return; + } + // calculate true airspeed, assuming a ratio of 1.0 + float airspeed = sqrtf(get_differential_pressure()) * EAS2TAS; + float ratio = _calibration.update(airspeed, vground); + if (isnan(ratio) || isinf(ratio)) { + return; + } + // this constrains the resulting ratio to between 1.5 and 3 + ratio = constrain_float(ratio, 0.577f, 0.816f); + _ratio.set(1/sq(ratio)); + if (_counter > 60) { + if (_last_saved_ratio < 1.05f*_ratio || + _last_saved_ratio < 0.95f*_ratio) { + _ratio.save(); + _last_saved_ratio = _ratio; + _counter = 0; + } + } else { + _counter++; + } +} diff --git a/libraries/AP_Airspeed/models/ADS_cal_EKF.m b/libraries/AP_Airspeed/models/ADS_cal_EKF.m new file mode 100644 index 0000000000..0c5def09dd --- /dev/null +++ b/libraries/AP_Airspeed/models/ADS_cal_EKF.m @@ -0,0 +1,122 @@ +% Implementation of a simple 3-state EKF that can identify the scale +% factor that needs to be applied to a true airspeed measurement +% Paul Riseborough 27 June 2013 + +% Inputs: +% Measured true airsped (m/s) + +clear all; + +% Define wind speed used for truth model +vwn_truth = 4.0; +vwe_truth = 3.0; +vwd_truth = -0.5; % convection can produce values of up to 1.5 m/s, however + % average will zero over longer periods at lower altitudes + % Slope lift will be persistent + +% Define airspeed scale factor used for truth model +K_truth = 1.2; + +% Use a 1 second time step +DT = 1.0; + +% Define the initial state error covariance matrix +% Assume initial wind uncertainty of 10 m/s and scale factor uncertainty of +% 0.2 +P = diag([10^2 10^2 0.001^2]); + +% Define state error growth matrix assuming wind changes at a rate of 0.1 +% m/s/s and scale factor drifts at a rate of 0.001 per second +Q = diag([0.1^2 0.1^2 0.001^2])*DT^2; + +% Define the initial state matrix assuming zero wind and a scale factor of +% 1.0 +x = [0;0;1.0]; + +for i = 1:1000 + + %% Calculate truth values + % calculate ground velocity by simulating a wind relative + % circular path of of 60m radius and 16 m/s airspeed + time = i*DT; + radius = 60; + TAS_truth = 16; + vwnrel_truth = TAS_truth*cos(TAS_truth*time/radius); + vwerel_truth = TAS_truth*sin(TAS_truth*time/radius); + vwdrel_truth = 0.0; + vgn_truth = vwnrel_truth + vwn_truth; + vge_truth = vwerel_truth + vwe_truth; + vgd_truth = vwdrel_truth + vwd_truth; + + % calculate measured ground velocity and airspeed, adding some noise and + % adding a scale factor to the airspeed measurement. + vgn_mea = vgn_truth + 0.1*rand; + vge_mea = vge_truth + 0.1*rand; + vgd_mea = vgd_truth + 0.1*rand; + TAS_mea = K_truth * TAS_truth + 0.5*rand; + + %% Perform filter processing + % This benefits from a matrix library that can handle up to 3x3 + % matrices + + % Perform the covariance prediction + % Q is a diagonal matrix so only need to add three terms in + % C code implementation + P = P + Q; + + % Perform the predicted measurement using the current state estimates + % No state prediction required because states are assumed to be time + % invariant plus process noise + % Ignore vertical wind component + TAS_pred = x(3) * sqrt((vgn_mea - x(1))^2 + (vge_mea - x(2))^2 + vgd_mea^2); + + % Calculate the observation Jacobian H_TAS + SH1 = (vge_mea - x(2))^2 + (vgn_mea - x(1))^2; + SH2 = 1/sqrt(SH1); + H_TAS = zeros(1,3); + H_TAS(1,1) = -(x(3)*SH2*(2*vgn_mea - 2*x(1)))/2; + H_TAS(1,2) = -(x(3)*SH2*(2*vge_mea - 2*x(2)))/2; + H_TAS(1,3) = 1/SH2; + + % Calculate the fusion innovaton covariance assuming a TAS measurement + % noise of 1.0 m/s + S = H_TAS*P*H_TAS' + 1.0; % [1 x 3] * [3 x 3] * [3 x 1] + [1 x 1] + + % Calculate the Kalman gain + KG = P*H_TAS'/S; % [3 x 3] * [3 x 1] / [1 x 1] + + % Update the states + x = x + KG*(TAS_mea - TAS_pred); % [3 x 1] + [3 x 1] * [1 x 1] + + % Update the covariance matrix + P = P - KG*H_TAS*P; % [3 x 3] * + + % force symmetry on the covariance matrix - necessary due to rounding + % errors + % Implementation will also need a further check to prevent diagonal + % terms becoming negative due to rounding errors + % This step can be made more efficient by excluding diagonal terms + % (would reduce processing by 1/3) + P = 0.5*(P + P'); % [1 x 1] * ( [3 x 3] + [3 x 3]) + + %% Store results + output(i,:) = [time,x(1),x(2),x(3),vwn_truth,vwe_truth,K_truth]; + +end + +%% Plot output +figure; +subplot(3,1,1);plot(output(:,1),[output(:,2),output(:,5)]); +ylabel('Wind Vel North (m/s)'); +xlabel('time (sec)'); +grid on; +subplot(3,1,2);plot(output(:,1),[output(:,3),output(:,6)]); +ylabel('Wind Vel East (m/s)'); +xlabel('time (sec)'); +grid on; +subplot(3,1,3);plot(output(:,1),[output(:,4),output(:,7)]); +ylim([0 1.5]); +ylabel('Airspeed scale factor correction'); +xlabel('time (sec)'); +grid on; +