Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: add comments to calibrator

Browse Source
This commit is contained in:
Randy Mackay 2019-11-19 15:02:58 +09:00 committed by Andrew Tridgell
parent 015eed7159
commit 233e3bae61
2 changed files with 133 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
libraries/AP_Compass/CompassCalibrator.cpp
View File

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

158
libraries/AP_Compass/CompassCalibrator.h
View File

@ -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
};