mirror of https://github.com/ArduPilot/ardupilot
AP_Compass: add comments to calibrator
This commit is contained in:
Randy Mackay
Andrew Tridgell
parent
09046aec66
commit
feb0f141fa
|
|
@ -81,6 +81,15 @@ void CompassCalibrator::clear()
|
|||
set_status(COMPASS_CAL_NOT_STARTED);
|
||||
}
|
||||
|
||||
void CompassCalibrator::set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation)
|
||||
{
|
||||
_check_orientation = true;
|
||||
_orientation = orientation;
|
||||
_orig_orientation = orientation;
|
||||
_is_external = is_external;
|
||||
_fix_orientation = fix_orientation;
|
||||
}
|
||||
|
||||
void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx)
|
||||
{
|
||||
if (running()) {
|
||||
|
|
@ -127,6 +136,8 @@ float CompassCalibrator::get_completion_percent() const
|
|||
};
|
||||
}
|
||||
|
||||
// update completion mask based on latest sample
|
||||
// used to ensure we have collected samples in all directions
|
||||
void CompassCalibrator::update_completion_mask(const Vector3f& v)
|
||||
{
|
||||
Matrix3f softiron {
|
||||
|
|
@ -142,6 +153,7 @@ void CompassCalibrator::update_completion_mask(const Vector3f& v)
|
|||
_completion_mask[section / 8] |= 1 << (section % 8);
|
||||
}
|
||||
|
||||
// reset and updated the completion mask using all samples in the sample buffer
|
||||
void CompassCalibrator::update_completion_mask()
|
||||
{
|
||||
memset(_completion_mask, 0, sizeof(_completion_mask));
|
||||
|
|
@ -181,6 +193,7 @@ void CompassCalibrator::update(bool &failure)
|
|||
{
|
||||
failure = false;
|
||||
|
||||
// collect the minimum number of samples
|
||||
if (!fitting()) {
|
||||
return;
|
||||
}
|
||||
|
|
@ -230,17 +243,17 @@ bool CompassCalibrator::fitting() const
|
|||
return running() && (_samples_collected == COMPASS_CAL_NUM_SAMPLES);
|
||||
}
|
||||
|
||||
// initialize fitness before starting a fit
|
||||
void CompassCalibrator::initialize_fit()
|
||||
{
|
||||
//initialize _fitness before starting a fit
|
||||
if (_samples_collected != 0) {
|
||||
_fitness = calc_mean_squared_residuals(_params);
|
||||
} else {
|
||||
_fitness = 1.0e30f;
|
||||
}
|
||||
_ellipsoid_lambda = 1.0f;
|
||||
_sphere_lambda = 1.0f;
|
||||
_initial_fitness = _fitness;
|
||||
_sphere_lambda = 1.0f;
|
||||
_ellipsoid_lambda = 1.0f;
|
||||
_fit_step = 0;
|
||||
}
|
||||
|
||||
|
|
@ -284,6 +297,7 @@ bool CompassCalibrator::set_status(compass_cal_status_t status)
|
|||
return false;
|
||||
}
|
||||
|
||||
// on first attempt delay start if requested by caller
|
||||
if (_attempt == 1 && (AP_HAL::millis()-_start_time_ms)*1.0e-3f < _delay_start_sec) {
|
||||
return false;
|
||||
}
|
||||
|
|
@ -452,6 +466,7 @@ float CompassCalibrator::calc_mean_squared_residuals() const
|
|||
return calc_mean_squared_residuals(_params);
|
||||
}
|
||||
|
||||
// calc the fitness given a set of parameters (offsets, diagonals, off diagonals)
|
||||
float CompassCalibrator::calc_mean_squared_residuals(const param_t& params) const
|
||||
{
|
||||
if (_sample_buffer == nullptr || _samples_collected == 0) {
|
||||
|
|
@ -467,6 +482,17 @@ float CompassCalibrator::calc_mean_squared_residuals(const param_t& params) cons
|
|||
return sum;
|
||||
}
|
||||
|
||||
// calculate initial offsets by simply taking the average values of the samples
|
||||
void CompassCalibrator::calc_initial_offset()
|
||||
{
|
||||
// Set initial offset to the average value of the samples
|
||||
_params.offset.zero();
|
||||
for (uint16_t k = 0; k < _samples_collected; k++) {
|
||||
_params.offset -= _sample_buffer[k].get();
|
||||
}
|
||||
_params.offset /= _samples_collected;
|
||||
}
|
||||
|
||||
void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const
|
||||
{
|
||||
const Vector3f &offset = params.offset;
|
||||
|
|
@ -491,16 +517,7 @@ void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const param_t&
|
|||
ret[3] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length);
|
||||
}
|
||||
|
||||
void CompassCalibrator::calc_initial_offset()
|
||||
{
|
||||
// Set initial offset to the average value of the samples
|
||||
_params.offset.zero();
|
||||
for (uint16_t k = 0; k<_samples_collected; k++) {
|
||||
_params.offset -= _sample_buffer[k].get();
|
||||
}
|
||||
_params.offset /= _samples_collected;
|
||||
}
|
||||
|
||||
// run sphere fit to calculate diagonals and offdiagonals
|
||||
void CompassCalibrator::run_sphere_fit()
|
||||
{
|
||||
if (_sample_buffer == nullptr) {
|
||||
|
|
@ -509,6 +526,7 @@ void CompassCalibrator::run_sphere_fit()
|
|||
|
||||
const float lma_damping = 10.0f;
|
||||
|
||||
// take backup of fitness and parameters so we can determine later if this fit has improved the calibration
|
||||
float fitness = _fitness;
|
||||
float fit1, fit2;
|
||||
param_t fit1_params, fit2_params;
|
||||
|
|
@ -538,7 +556,7 @@ void CompassCalibrator::run_sphere_fit()
|
|||
}
|
||||
|
||||
//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
|
||||
//refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
|
||||
// refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
|
||||
for (uint8_t i = 0; i < COMPASS_CAL_NUM_SPHERE_PARAMS; i++) {
|
||||
JTJ[i*COMPASS_CAL_NUM_SPHERE_PARAMS+i] += _sphere_lambda;
|
||||
JTJ2[i*COMPASS_CAL_NUM_SPHERE_PARAMS+i] += _sphere_lambda/lma_damping;
|
||||
|
|
@ -552,6 +570,7 @@ void CompassCalibrator::run_sphere_fit()
|
|||
return;
|
||||
}
|
||||
|
||||
// extract radius, offset, diagonals and offdiagonal parameters
|
||||
for (uint8_t row=0; row < COMPASS_CAL_NUM_SPHERE_PARAMS; row++) {
|
||||
for (uint8_t col=0; col < COMPASS_CAL_NUM_SPHERE_PARAMS; col++) {
|
||||
fit1_params.get_sphere_params()[row] -= JTFI[col] * JTJ[row*COMPASS_CAL_NUM_SPHERE_PARAMS+col];
|
||||
|
|
@ -559,12 +578,16 @@ void CompassCalibrator::run_sphere_fit()
|
|||
}
|
||||
}
|
||||
|
||||
// calculate fitness of two possible sets of parameters
|
||||
fit1 = calc_mean_squared_residuals(fit1_params);
|
||||
fit2 = calc_mean_squared_residuals(fit2_params);
|
||||
|
||||
// decide which of the two sets of parameters is best and store in fit1_params
|
||||
if (fit1 > _fitness && fit2 > _fitness) {
|
||||
// if neither set of parameters provided better results, increase lambda
|
||||
_sphere_lambda *= lma_damping;
|
||||
} else if (fit2 < _fitness && fit2 < fit1) {
|
||||
// if fit2 was better we will use it. decrease lambda
|
||||
_sphere_lambda /= lma_damping;
|
||||
fit1_params = fit2_params;
|
||||
fitness = fit2;
|
||||
|
|
@ -573,6 +596,7 @@ void CompassCalibrator::run_sphere_fit()
|
|||
}
|
||||
//--------------------Levenberg-Marquardt-part-ends-here--------------------------------//
|
||||
|
||||
// store new parameters and update fitness
|
||||
if (!isnan(fitness) && fitness < _fitness) {
|
||||
_fitness = fitness;
|
||||
_params = fit1_params;
|
||||
|
|
@ -618,6 +642,7 @@ void CompassCalibrator::run_ellipsoid_fit()
|
|||
|
||||
const float lma_damping = 10.0f;
|
||||
|
||||
// take backup of fitness and parameters so we can determine later if this fit has improved the calibration
|
||||
float fitness = _fitness;
|
||||
float fit1, fit2;
|
||||
param_t fit1_params, fit2_params;
|
||||
|
|
@ -661,6 +686,7 @@ void CompassCalibrator::run_ellipsoid_fit()
|
|||
return;
|
||||
}
|
||||
|
||||
// extract radius, offset, diagonals and offdiagonal parameters
|
||||
for (uint8_t row=0; row < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; row++) {
|
||||
for (uint8_t col=0; col < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; col++) {
|
||||
fit1_params.get_ellipsoid_params()[row] -= JTFI[col] * JTJ[row*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+col];
|
||||
|
|
@ -668,12 +694,16 @@ void CompassCalibrator::run_ellipsoid_fit()
|
|||
}
|
||||
}
|
||||
|
||||
// calculate fitness of two possible sets of parameters
|
||||
fit1 = calc_mean_squared_residuals(fit1_params);
|
||||
fit2 = calc_mean_squared_residuals(fit2_params);
|
||||
|
||||
// decide which of the two sets of parameters is best and store in fit1_params
|
||||
if (fit1 > _fitness && fit2 > _fitness) {
|
||||
// if neither set of parameters provided better results, increase lambda
|
||||
_ellipsoid_lambda *= lma_damping;
|
||||
} else if (fit2 < _fitness && fit2 < fit1) {
|
||||
// if fit2 was better we will use it. decrease lambda
|
||||
_ellipsoid_lambda /= lma_damping;
|
||||
fit1_params = fit2_params;
|
||||
fitness = fit2;
|
||||
|
|
@ -682,6 +712,7 @@ void CompassCalibrator::run_ellipsoid_fit()
|
|||
}
|
||||
//--------------------Levenberg-part-ends-here--------------------------------//
|
||||
|
||||
// store new parameters and update fitness
|
||||
if (fitness < _fitness) {
|
||||
_fitness = fitness;
|
||||
_params = fit1_params;
|
||||
|
|
|
|||
|
|
@ -2,13 +2,12 @@
|
|||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
#define COMPASS_CAL_NUM_SPHERE_PARAMS 4
|
||||
#define COMPASS_CAL_NUM_ELLIPSOID_PARAMS 9
|
||||
#define COMPASS_CAL_NUM_SAMPLES 300
|
||||
|
||||
//RMS tolerance
|
||||
#define COMPASS_CAL_DEFAULT_TOLERANCE 5.0f
|
||||
#define COMPASS_CAL_NUM_SPHERE_PARAMS 4
|
||||
#define COMPASS_CAL_NUM_ELLIPSOID_PARAMS 9
|
||||
#define COMPASS_CAL_NUM_SAMPLES 300 // number of samples required before fitting begins
|
||||
#define COMPASS_CAL_DEFAULT_TOLERANCE 5.0f // default RMS tolerance
|
||||
|
||||
// compass calibration states
|
||||
enum compass_cal_status_t {
|
||||
COMPASS_CAL_NOT_STARTED = 0,
|
||||
COMPASS_CAL_WAITING_TO_START = 1,
|
||||
|
|
@ -21,42 +20,52 @@ enum compass_cal_status_t {
|
|||
|
||||
class CompassCalibrator {
|
||||
public:
|
||||
typedef uint8_t completion_mask_t[10];
|
||||
|
||||
CompassCalibrator();
|
||||
|
||||
// set tolerance of calibration (aka fitness)
|
||||
void set_tolerance(float tolerance) { _tolerance = tolerance; }
|
||||
|
||||
// set compass's initial orientation and whether it should be automatically fixed (if required)
|
||||
void set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation);
|
||||
|
||||
// start or stop the calibration
|
||||
void start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx);
|
||||
void clear();
|
||||
|
||||
// update the state machine and calculate offsets, diagonals and offdiagonals
|
||||
void update(bool &failure);
|
||||
void new_sample(const Vector3f &sample);
|
||||
|
||||
bool check_for_timeout();
|
||||
|
||||
// running is true if actively calculating offsets, diagonals or offdiagonals
|
||||
bool running() const;
|
||||
|
||||
void set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation) {
|
||||
_check_orientation = true;
|
||||
_orientation = orientation;
|
||||
_orig_orientation = orientation;
|
||||
_is_external = is_external;
|
||||
_fix_orientation = fix_orientation;
|
||||
}
|
||||
|
||||
void set_tolerance(float tolerance) { _tolerance = tolerance; }
|
||||
// get status of calibrations progress
|
||||
enum compass_cal_status_t get_status() const { return _status; }
|
||||
|
||||
// get calibration outputs (offsets, diagonals, offdiagonals) and fitness
|
||||
void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals);
|
||||
float get_fitness() const { return sqrtf(_fitness); }
|
||||
|
||||
// get corrected (and original) orientation
|
||||
enum Rotation get_orientation() const { return _orientation; }
|
||||
enum Rotation get_original_orientation() const { return _orig_orientation; }
|
||||
|
||||
float get_completion_percent() const;
|
||||
const completion_mask_t& get_completion_mask() const { return _completion_mask; }
|
||||
enum compass_cal_status_t get_status() const { return _status; }
|
||||
float get_fitness() const { return sqrtf(_fitness); }
|
||||
float get_orientation_confidence() const { return _orientation_confidence; }
|
||||
|
||||
// get completion percentage (0 to 100) for reporting to GCS
|
||||
float get_completion_percent() const;
|
||||
|
||||
// get how many attempts have been made to calibrate for reporting to GCS
|
||||
uint8_t get_attempt() const { return _attempt; }
|
||||
|
||||
// get completion mask for mavlink reporting (a bitmask of faces/directions for which we have compass samples)
|
||||
typedef uint8_t completion_mask_t[10];
|
||||
const completion_mask_t& get_completion_mask() const { return _completion_mask; }
|
||||
|
||||
private:
|
||||
|
||||
// results
|
||||
class param_t {
|
||||
public:
|
||||
float* get_sphere_params() {
|
||||
|
|
@ -67,10 +76,10 @@ private:
|
|||
return &offset.x;
|
||||
}
|
||||
|
||||
float radius;
|
||||
Vector3f offset;
|
||||
Vector3f diag;
|
||||
Vector3f offdiag;
|
||||
float radius; // magnetic field strength calculated from samples
|
||||
Vector3f offset; // offsets
|
||||
Vector3f diag; // diagonal scaling
|
||||
Vector3f offdiag; // off diagonal scaling
|
||||
};
|
||||
|
||||
// compact class for approximate attitude, to save memory
|
||||
|
|
@ -84,6 +93,7 @@ private:
|
|||
int8_t yaw;
|
||||
};
|
||||
|
||||
// compact class to hold compass samples, to save memory
|
||||
class CompassSample {
|
||||
public:
|
||||
Vector3f get() const;
|
||||
|
|
@ -95,40 +105,7 @@ private:
|
|||
int16_t z;
|
||||
};
|
||||
|
||||
enum Rotation _orientation;
|
||||
enum Rotation _orig_orientation;
|
||||
bool _is_external;
|
||||
bool _check_orientation;
|
||||
bool _fix_orientation;
|
||||
uint8_t _compass_idx;
|
||||
|
||||
enum compass_cal_status_t _status;
|
||||
|
||||
// timeout watchdog state
|
||||
uint32_t _last_sample_ms;
|
||||
|
||||
// behavioral state
|
||||
float _delay_start_sec;
|
||||
uint32_t _start_time_ms;
|
||||
bool _retry;
|
||||
float _tolerance;
|
||||
uint8_t _attempt;
|
||||
uint16_t _offset_max;
|
||||
|
||||
completion_mask_t _completion_mask;
|
||||
|
||||
//fit state
|
||||
class param_t _params;
|
||||
uint16_t _fit_step;
|
||||
CompassSample *_sample_buffer;
|
||||
float _fitness; // mean squared residuals
|
||||
float _initial_fitness;
|
||||
float _sphere_lambda;
|
||||
float _ellipsoid_lambda;
|
||||
uint16_t _samples_collected;
|
||||
uint16_t _samples_thinned;
|
||||
float _orientation_confidence;
|
||||
|
||||
// set status including any required initialisation
|
||||
bool set_status(compass_cal_status_t status);
|
||||
|
||||
// returns true if sample should be added to buffer
|
||||
|
|
@ -138,37 +115,78 @@ private:
|
|||
// returns true if fit is acceptable
|
||||
bool fit_acceptable();
|
||||
|
||||
// clear sample buffer and reset offsets and scaling to their defaults
|
||||
void reset_state();
|
||||
|
||||
// initialize fitness before starting a fit
|
||||
void initialize_fit();
|
||||
|
||||
// true if enough samples have been collected and fitting has begun (aka runniong())
|
||||
bool fitting() const;
|
||||
|
||||
// thins out samples between step one and step two
|
||||
void thin_samples();
|
||||
|
||||
// calc the fitness of a single sample vs a set of parameters (offsets, diagonals, off diagonals)
|
||||
float calc_residual(const Vector3f& sample, const param_t& params) const;
|
||||
|
||||
// calc the fitness of the parameters (offsets, diagonals, off diagonals) vs all the samples collected
|
||||
// returns 1.0e30f if the sample buffer is empty
|
||||
float calc_mean_squared_residuals(const param_t& params) const;
|
||||
float calc_mean_squared_residuals() const;
|
||||
|
||||
// calculate initial offsets by simply taking the average values of the samples
|
||||
void calc_initial_offset();
|
||||
|
||||
// run sphere fit to calculate diagonals and offdiagonals
|
||||
void calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const;
|
||||
void run_sphere_fit();
|
||||
|
||||
// run ellipsoid fit to calculate diagonals and offdiagonals
|
||||
void calc_ellipsoid_jacob(const Vector3f& sample, const param_t& params, float* ret) const;
|
||||
void run_ellipsoid_fit();
|
||||
|
||||
/**
|
||||
* Update #_completion_mask for the geodesic section of \p v. Corrections
|
||||
* are applied to \p v with #_params.
|
||||
*
|
||||
* @param v[in] A vector representing one calibration sample.
|
||||
*/
|
||||
void update_completion_mask(const Vector3f& v);
|
||||
/**
|
||||
* Reset and update #_completion_mask with the current samples.
|
||||
*/
|
||||
// update the completion mask based on a single sample
|
||||
void update_completion_mask(const Vector3f& sample);
|
||||
|
||||
// reset and updated the completion mask using all samples in the sample buffer
|
||||
void update_completion_mask();
|
||||
|
||||
// calculate compass orientation
|
||||
Vector3f calculate_earth_field(CompassSample &sample, enum Rotation r);
|
||||
bool calculate_orientation();
|
||||
|
||||
uint8_t _compass_idx; // index of the compass providing data
|
||||
enum compass_cal_status_t _status; // current state of calibrator
|
||||
uint32_t _last_sample_ms; // system time of last sample received for timeout
|
||||
|
||||
// values provided by caller
|
||||
float _delay_start_sec; // seconds to delay start of calibration (provided by caller)
|
||||
bool _retry; // true if calibration should be restarted on failured (provided by caller)
|
||||
float _tolerance; // worst acceptable tolerance (aka fitness). see set_tolerance()
|
||||
uint16_t _offset_max; // maximum acceptable offsets (provided by caller)
|
||||
|
||||
// behavioral state
|
||||
uint32_t _start_time_ms; // system time start() function was last called
|
||||
uint8_t _attempt; // number of attempts have been made to calibrate
|
||||
completion_mask_t _completion_mask; // bitmask of directions in which we have samples
|
||||
CompassSample *_sample_buffer; // buffer of sensor values
|
||||
uint16_t _samples_collected; // number of samples in buffer
|
||||
uint16_t _samples_thinned; // number of samples removed by the thin_samples() call (called before step 2 begins)
|
||||
|
||||
// fit state
|
||||
class param_t _params; // latest calibration outputs
|
||||
uint16_t _fit_step; // step during RUNNING_STEP_ONE/TWO which performs sphere fit and ellipsoid fit
|
||||
float _fitness; // fitness (mean squared residuals) of current parameters
|
||||
float _initial_fitness; // fitness before latest "fit" was attempted (used to determine if fit was an improvement)
|
||||
float _sphere_lambda; // sphere fit's lambda
|
||||
float _ellipsoid_lambda; // ellipsoid fit's lambda
|
||||
|
||||
// variables for orientation checking
|
||||
enum Rotation _orientation; // latest detected orientation
|
||||
enum Rotation _orig_orientation; // original orientation provided by caller
|
||||
bool _is_external; // true if compass is external (provided by caller)
|
||||
bool _check_orientation; // true if orientation should be automatically checked
|
||||
bool _fix_orientation; // true if orientation should be fixed if necessary
|
||||
float _orientation_confidence; // measure of confidence in automatic orientation detection
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue