AP_Airspeed: added auto-calibration support

This uses a Kalman filter to calculate the right ARSPD_RATIO at runtime

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>

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

libraries/AP_Airspeed/AP_Airspeed.h

@@ -7,11 +7,35 @@
 #include <AP_HAL.h>
 #include <AP_Param.h>
 
+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__
+

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 <AP_HAL.h>
+#include <AP_Math.h>
+#include <AP_Common.h>
+#include <AP_Airspeed.h>
+
+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++;
+    }
+}

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;
+