ardupilot/libraries/AP_Airspeed/Airspeed_Calibration.cpp

170 lines
5.0 KiB
C++

/// -*- 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/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_Common/AP_Common.h>
#include "AP_Airspeed.h"
extern const AP_HAL::HAL& hal;
// constructor - fill in all the initial values
Airspeed_Calibration::Airspeed_Calibration(const AP_Vehicle::FixedWing &parms) :
P(100, 0, 0,
0, 100, 0,
0, 0, 0.000001f),
Q0(0.01f),
Q1(0.0000005f),
state(0, 0, 0),
DT(1),
aparm(parms)
{
}
/*
initialise the ratio
*/
void Airspeed_Calibration::init(float initial_ratio)
{
state.z = 1.0f / sqrtf(initial_ratio);
}
/*
update the state of the airspeed calibration - needs to be called
once a second
*/
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 * pythagorous3(vg.x - state.x, vg.y - state.y, 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/sqrtf(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
float P12 = 0.5f * (P.a.y + P.b.x);
float P13 = 0.5f * (P.a.z + P.c.x);
float P23 = 0.5f * (P.b.z + P.c.y);
P.a.y = P.b.x = P12;
P.a.z = P.c.x = P13;
P.b.z = P.c.y = P23;
// Constrain diagonals to be non-negative - protects against rounding errors
P.a.x = MAX(P.a.x, 0.0f);
P.b.y = MAX(P.b.y, 0.0f);
P.c.z = MAX(P.c.z, 0.0f);
state.x = constrain_float(state.x, -aparm.airspeed_max, aparm.airspeed_max);
state.y = constrain_float(state.y, -aparm.airspeed_max, aparm.airspeed_max);
state.z = constrain_float(state.z, 0.5f, 1.0f);
return state.z;
}
/*
called once a second to do calibration update
*/
void AP_Airspeed::update_calibration(const Vector3f &vground)
{
if (!_autocal) {
// auto-calibration not enabled
return;
}
// set state.z based on current ratio, this allows the operator to
// override the current ratio in flight with autocal, which is
// very useful both for testing and to force a reasonable value.
float ratio = constrain_float(_ratio, 1.0f, 4.0f);
_calibration.state.z = 1.0f / sqrtf(ratio);
// calculate true airspeed, assuming a airspeed ratio of 1.0
float dpress = get_differential_pressure();
float true_airspeed = sqrtf(dpress) * _EAS2TAS;
float zratio = _calibration.update(true_airspeed, vground);
if (isnan(zratio) || isinf(zratio)) {
return;
}
// this constrains the resulting ratio to between 1.0 and 4.0
zratio = constrain_float(zratio, 0.5f, 1.0f);
_ratio.set(1/sq(zratio));
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++;
}
}
// log airspeed calibration data to MAVLink
void AP_Airspeed::log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground)
{
mavlink_msg_airspeed_autocal_send(chan,
vground.x,
vground.y,
vground.z,
get_differential_pressure(),
_EAS2TAS,
_ratio.get(),
_calibration.state.x,
_calibration.state.y,
_calibration.state.z,
_calibration.P.a.x,
_calibration.P.b.y,
_calibration.P.c.z);
}