mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 15:08:28 -04:00
1175 lines
32 KiB
C++
1175 lines
32 KiB
C++
#include "CompassCalibrator.h"
|
|
#include <AP_HAL.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
////////////////////////////////////////////////////////////
|
|
///////////////////// PUBLIC INTERFACE /////////////////////
|
|
////////////////////////////////////////////////////////////
|
|
|
|
CompassCalibrator::CompassCalibrator():
|
|
_tolerance(COMPASS_CAL_DEFAULT_TOLERANCE)
|
|
{
|
|
clear();
|
|
}
|
|
|
|
void CompassCalibrator::clear() {
|
|
set_status(COMPASS_CAL_NOT_STARTED);
|
|
}
|
|
|
|
void CompassCalibrator::start(bool retry, bool autosave, float delay) {
|
|
if(running()) {
|
|
return;
|
|
}
|
|
_autosave = autosave;
|
|
_attempt = 1;
|
|
_retry = retry;
|
|
_delay_start_sec = delay;
|
|
_start_time_ms = hal.scheduler->millis();
|
|
set_status(COMPASS_CAL_WAITING_TO_START);
|
|
}
|
|
|
|
void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals) {
|
|
if (_status != COMPASS_CAL_SUCCESS) {
|
|
return;
|
|
}
|
|
|
|
offsets = _params.offset;
|
|
diagonals = _params.diag;
|
|
offdiagonals = _params.offdiag;
|
|
}
|
|
|
|
float CompassCalibrator::get_completion_percent() const {
|
|
// first sampling step is 1/3rd of the progress bar
|
|
// never return more than 99% unless _status is COMPASS_CAL_SUCCESS
|
|
switch(_status) {
|
|
case COMPASS_CAL_NOT_STARTED:
|
|
case COMPASS_CAL_WAITING_TO_START:
|
|
return 0.0f;
|
|
case COMPASS_CAL_RUNNING_STEP_ONE:
|
|
return 33.3f * _samples_collected/COMPASS_CAL_NUM_SAMPLES;
|
|
case COMPASS_CAL_RUNNING_STEP_TWO:
|
|
return 33.3f + 65.7f*((float)(_samples_collected-_samples_thinned)/(COMPASS_CAL_NUM_SAMPLES-_samples_thinned));
|
|
case COMPASS_CAL_SUCCESS:
|
|
return 100.0f;
|
|
case COMPASS_CAL_FAILED:
|
|
default:
|
|
return 0.0f;
|
|
};
|
|
}
|
|
|
|
bool CompassCalibrator::check_for_timeout() {
|
|
uint32_t tnow = hal.scheduler->millis();
|
|
if(running() && tnow - _last_sample_ms > 1000) {
|
|
_retry = false;
|
|
set_status(COMPASS_CAL_FAILED);
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void CompassCalibrator::new_sample(const Vector3f& sample) {
|
|
_last_sample_ms = hal.scheduler->millis();
|
|
|
|
if(_status == COMPASS_CAL_WAITING_TO_START) {
|
|
set_status(COMPASS_CAL_RUNNING_STEP_ONE);
|
|
}
|
|
|
|
if(running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(sample)) {
|
|
_sample_buffer[_samples_collected].set(sample);
|
|
_samples_collected++;
|
|
}
|
|
}
|
|
|
|
void CompassCalibrator::update(bool &failure) {
|
|
failure = false;
|
|
|
|
if(!fitting()) {
|
|
return;
|
|
}
|
|
|
|
if(_status == COMPASS_CAL_RUNNING_STEP_ONE) {
|
|
if (_fit_step >= 10) {
|
|
if(_fitness == _initial_fitness || isnan(_fitness)) { //if true, means that fitness is diverging instead of converging
|
|
set_status(COMPASS_CAL_FAILED);
|
|
failure = true;
|
|
}
|
|
set_status(COMPASS_CAL_RUNNING_STEP_TWO);
|
|
} else {
|
|
run_sphere_fit();
|
|
_fit_step++;
|
|
}
|
|
} else if(_status == COMPASS_CAL_RUNNING_STEP_TWO) {
|
|
if (_fit_step >= 35) {
|
|
if(fit_acceptable()) {
|
|
set_status(COMPASS_CAL_SUCCESS);
|
|
} else {
|
|
set_status(COMPASS_CAL_FAILED);
|
|
failure = true;
|
|
}
|
|
} else if (_fit_step < 15) {
|
|
run_sphere_fit();
|
|
_fit_step++;
|
|
} else {
|
|
run_ellipsoid_fit();
|
|
_fit_step++;
|
|
}
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////
|
|
////////////////////// PRIVATE METHODS //////////////////////
|
|
/////////////////////////////////////////////////////////////
|
|
bool CompassCalibrator::running() const {
|
|
return _status == COMPASS_CAL_RUNNING_STEP_ONE || _status == COMPASS_CAL_RUNNING_STEP_TWO;
|
|
}
|
|
|
|
bool CompassCalibrator::fitting() const {
|
|
return running() && _samples_collected == COMPASS_CAL_NUM_SAMPLES;
|
|
}
|
|
|
|
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;
|
|
_fit_step = 0;
|
|
}
|
|
|
|
void CompassCalibrator::reset_state() {
|
|
_samples_collected = 0;
|
|
_samples_thinned = 0;
|
|
_params.radius = 200;
|
|
_params.offset.zero();
|
|
_params.diag = Vector3f(1.0f,1.0f,1.0f);
|
|
_params.offdiag.zero();
|
|
|
|
initialize_fit();
|
|
}
|
|
|
|
bool CompassCalibrator::set_status(compass_cal_status_t status) {
|
|
if (status != COMPASS_CAL_NOT_STARTED && _status == status) {
|
|
return true;
|
|
}
|
|
|
|
switch(status) {
|
|
case COMPASS_CAL_NOT_STARTED:
|
|
reset_state();
|
|
_status = COMPASS_CAL_NOT_STARTED;
|
|
|
|
if(_sample_buffer != NULL) {
|
|
free(_sample_buffer);
|
|
_sample_buffer = NULL;
|
|
}
|
|
return true;
|
|
|
|
case COMPASS_CAL_WAITING_TO_START:
|
|
reset_state();
|
|
_status = COMPASS_CAL_WAITING_TO_START;
|
|
|
|
set_status(COMPASS_CAL_RUNNING_STEP_ONE);
|
|
return true;
|
|
|
|
case COMPASS_CAL_RUNNING_STEP_ONE:
|
|
if(_status != COMPASS_CAL_WAITING_TO_START) {
|
|
return false;
|
|
}
|
|
|
|
if(_attempt == 1 && (hal.scheduler->millis()-_start_time_ms)*1.0e-3f < _delay_start_sec) {
|
|
return false;
|
|
}
|
|
|
|
if(_sample_buffer != NULL) {
|
|
initialize_fit();
|
|
_status = COMPASS_CAL_RUNNING_STEP_ONE;
|
|
return true;
|
|
}
|
|
|
|
_sample_buffer = (CompassSample*)malloc(sizeof(CompassSample)*COMPASS_CAL_NUM_SAMPLES);
|
|
|
|
if(_sample_buffer != NULL) {
|
|
initialize_fit();
|
|
_status = COMPASS_CAL_RUNNING_STEP_ONE;
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
|
|
case COMPASS_CAL_RUNNING_STEP_TWO:
|
|
if(_status != COMPASS_CAL_RUNNING_STEP_ONE) {
|
|
return false;
|
|
}
|
|
thin_samples();
|
|
initialize_fit();
|
|
_status = COMPASS_CAL_RUNNING_STEP_TWO;
|
|
return true;
|
|
|
|
case COMPASS_CAL_SUCCESS:
|
|
if(_status != COMPASS_CAL_RUNNING_STEP_TWO) {
|
|
return false;
|
|
}
|
|
|
|
if(_sample_buffer != NULL) {
|
|
free(_sample_buffer);
|
|
_sample_buffer = NULL;
|
|
}
|
|
|
|
_status = COMPASS_CAL_SUCCESS;
|
|
return true;
|
|
|
|
case COMPASS_CAL_FAILED:
|
|
if(_status == COMPASS_CAL_NOT_STARTED) {
|
|
return false;
|
|
}
|
|
|
|
if(_retry && set_status(COMPASS_CAL_WAITING_TO_START)) {
|
|
_attempt++;
|
|
return true;
|
|
}
|
|
|
|
if(_sample_buffer != NULL) {
|
|
free(_sample_buffer);
|
|
_sample_buffer = NULL;
|
|
}
|
|
|
|
_status = COMPASS_CAL_FAILED;
|
|
return true;
|
|
|
|
default:
|
|
return false;
|
|
};
|
|
}
|
|
|
|
bool CompassCalibrator::fit_acceptable() {
|
|
if( !isnan(_fitness) &&
|
|
_params.radius > 150 && _params.radius < 950 && //Earth's magnetic field strength range: 250-850mG
|
|
fabsf(_params.offset.x) < 1000 &&
|
|
fabsf(_params.offset.y) < 1000 &&
|
|
fabsf(_params.offset.z) < 1000 &&
|
|
_params.diag.x > 0.2f && _params.diag.x < 5.0f &&
|
|
_params.diag.y > 0.2f && _params.diag.y < 5.0f &&
|
|
_params.diag.z > 0.2f && _params.diag.z < 5.0f &&
|
|
fabsf(_params.offdiag.x) < 1.0f && //absolute of sine/cosine output cannot be greater than 1
|
|
fabsf(_params.offdiag.y) < 1.0f &&
|
|
fabsf(_params.offdiag.z) < 1.0f ){
|
|
|
|
return _fitness <= sq(_tolerance);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void CompassCalibrator::thin_samples() {
|
|
if(_sample_buffer == NULL) {
|
|
return;
|
|
}
|
|
|
|
_samples_thinned = 0;
|
|
// shuffle the samples http://en.wikipedia.org/wiki/Fisher%E2%80%93Yates_shuffle
|
|
// this is so that adjacent samples don't get sequentially eliminated
|
|
for(uint16_t i=_samples_collected-1; i>=1; i--) {
|
|
uint16_t j = get_random() % (i+1);
|
|
CompassSample temp = _sample_buffer[i];
|
|
_sample_buffer[i] = _sample_buffer[j];
|
|
_sample_buffer[j] = temp;
|
|
}
|
|
|
|
for(uint16_t i=0; i < _samples_collected; i++) {
|
|
if(!accept_sample(_sample_buffer[i])) {
|
|
_sample_buffer[i] = _sample_buffer[_samples_collected-1];
|
|
_samples_collected --;
|
|
_samples_thinned ++;
|
|
}
|
|
}
|
|
}
|
|
|
|
bool CompassCalibrator::accept_sample(const Vector3f& sample)
|
|
{
|
|
if(_sample_buffer == NULL) {
|
|
return false;
|
|
}
|
|
|
|
float max_distance = fabsf(5.38709f * _params.radius / sqrtf((float)COMPASS_CAL_NUM_SAMPLES)) / 3.0f;
|
|
|
|
for (uint16_t i = 0; i<_samples_collected; i++){
|
|
float distance = (sample - _sample_buffer[i].get()).length();
|
|
if(distance < max_distance) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool CompassCalibrator::accept_sample(const CompassSample& sample) {
|
|
return accept_sample(sample.get());
|
|
}
|
|
|
|
float CompassCalibrator::calc_residual(const Vector3f& sample, const param_t& params) const {
|
|
Matrix3f softiron(
|
|
params.diag.x , params.offdiag.x , params.offdiag.y,
|
|
params.offdiag.x , params.diag.y , params.offdiag.z,
|
|
params.offdiag.y , params.offdiag.z , params.diag.z
|
|
);
|
|
return params.radius - (softiron*(sample+params.offset)).length();
|
|
}
|
|
|
|
float CompassCalibrator::calc_mean_squared_residuals() const
|
|
{
|
|
return calc_mean_squared_residuals(_params);
|
|
}
|
|
|
|
float CompassCalibrator::calc_mean_squared_residuals(const param_t& params) const
|
|
{
|
|
if(_sample_buffer == NULL || _samples_collected == 0) {
|
|
return 1.0e30f;
|
|
}
|
|
float sum = 0.0f;
|
|
for(uint16_t i=0; i < _samples_collected; i++){
|
|
Vector3f sample = _sample_buffer[i].get();
|
|
float resid = calc_residual(sample, params);
|
|
sum += sq(resid);
|
|
}
|
|
sum /= _samples_collected;
|
|
return sum;
|
|
}
|
|
|
|
void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const{
|
|
const Vector3f &offset = params.offset;
|
|
const Vector3f &diag = params.diag;
|
|
const Vector3f &offdiag = params.offdiag;
|
|
Matrix3f softiron(
|
|
diag.x , offdiag.x , offdiag.y,
|
|
offdiag.x , diag.y , offdiag.z,
|
|
offdiag.y , offdiag.z , diag.z
|
|
);
|
|
|
|
float A = (diag.x * (sample.x + offset.x)) + (offdiag.x * (sample.y + offset.y)) + (offdiag.y * (sample.z + offset.z));
|
|
float B = (offdiag.x * (sample.x + offset.x)) + (diag.y * (sample.y + offset.y)) + (offdiag.z * (sample.z + offset.z));
|
|
float C = (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) + (diag.z * (sample.z + offset.z));
|
|
float length = (softiron*(sample+offset)).length();
|
|
|
|
// 0: radius
|
|
ret[0] = 1.0f;
|
|
// 1-3: offsets
|
|
ret[1] = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length);
|
|
ret[2] = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length);
|
|
ret[3] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length);
|
|
}
|
|
|
|
void CompassCalibrator::run_sphere_fit()
|
|
{
|
|
if(_sample_buffer == NULL) {
|
|
return;
|
|
}
|
|
|
|
const float lma_damping = 10.0f;
|
|
|
|
float fitness = _fitness;
|
|
float fit1, fit2;
|
|
param_t fit1_params, fit2_params;
|
|
fit1_params = fit2_params = _params;
|
|
|
|
float JTJ[COMPASS_CAL_NUM_SPHERE_PARAMS*COMPASS_CAL_NUM_SPHERE_PARAMS];
|
|
float JTJ2[COMPASS_CAL_NUM_SPHERE_PARAMS*COMPASS_CAL_NUM_SPHERE_PARAMS];
|
|
float JTFI[COMPASS_CAL_NUM_SPHERE_PARAMS];
|
|
|
|
memset(&JTJ,0,sizeof(JTJ));
|
|
memset(&JTJ2,0,sizeof(JTJ2));
|
|
memset(&JTFI,0,sizeof(JTFI));
|
|
|
|
for(uint16_t k = 0; k<_samples_collected; k++) {
|
|
Vector3f sample = _sample_buffer[k].get();
|
|
|
|
float sphere_jacob[COMPASS_CAL_NUM_SPHERE_PARAMS];
|
|
|
|
calc_sphere_jacob(sample, fit1_params, sphere_jacob);
|
|
|
|
for(uint8_t i = 0;i < COMPASS_CAL_NUM_SPHERE_PARAMS; i++) {
|
|
// compute JTJ
|
|
for(uint8_t j = 0; j < COMPASS_CAL_NUM_SPHERE_PARAMS; j++) {
|
|
JTJ[i*COMPASS_CAL_NUM_SPHERE_PARAMS+j] += sphere_jacob[i] * sphere_jacob[j];
|
|
JTJ2[i*COMPASS_CAL_NUM_SPHERE_PARAMS+j] += sphere_jacob[i] * sphere_jacob[j]; //a backup JTJ for LM
|
|
}
|
|
// compute JTFI
|
|
JTFI[i] += sphere_jacob[i] * calc_residual(sample, fit1_params);
|
|
}
|
|
}
|
|
|
|
|
|
//------------------------Levenberg-part-starts-here---------------------------------//
|
|
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;
|
|
}
|
|
|
|
if(!inverse4x4(JTJ, JTJ)) {
|
|
return;
|
|
}
|
|
|
|
if(!inverse4x4(JTJ2, JTJ2)) {
|
|
return;
|
|
}
|
|
|
|
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];
|
|
fit2_params.get_sphere_params()[row] -= JTFI[col] * JTJ2[row*COMPASS_CAL_NUM_SPHERE_PARAMS+col];
|
|
}
|
|
}
|
|
|
|
fit1 = calc_mean_squared_residuals(fit1_params);
|
|
fit2 = calc_mean_squared_residuals(fit2_params);
|
|
|
|
if(fit1 > _fitness && fit2 > _fitness){
|
|
_sphere_lambda *= lma_damping;
|
|
} else if(fit2 < _fitness && fit2 < fit1) {
|
|
_sphere_lambda /= lma_damping;
|
|
fit1_params = fit2_params;
|
|
fitness = fit2;
|
|
} else if(fit1 < _fitness){
|
|
fitness = fit1;
|
|
}
|
|
//--------------------Levenberg-part-ends-here--------------------------------//
|
|
|
|
if(!isnan(fitness) && fitness < _fitness) {
|
|
_fitness = fitness;
|
|
_params = fit1_params;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const param_t& params, float* ret) const{
|
|
const Vector3f &offset = params.offset;
|
|
const Vector3f &diag = params.diag;
|
|
const Vector3f &offdiag = params.offdiag;
|
|
Matrix3f softiron(
|
|
diag.x , offdiag.x , offdiag.y,
|
|
offdiag.x , diag.y , offdiag.z,
|
|
offdiag.y , offdiag.z , diag.z
|
|
);
|
|
|
|
float A = (diag.x * (sample.x + offset.x)) + (offdiag.x * (sample.y + offset.y)) + (offdiag.y * (sample.z + offset.z));
|
|
float B = (offdiag.x * (sample.x + offset.x)) + (diag.y * (sample.y + offset.y)) + (offdiag.z * (sample.z + offset.z));
|
|
float C = (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) + (diag.z * (sample.z + offset.z));
|
|
float length = (softiron*(sample+offset)).length();
|
|
|
|
// 0-2: offsets
|
|
ret[0] = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length);
|
|
ret[1] = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length);
|
|
ret[2] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length);
|
|
// 3-5: diagonals
|
|
ret[3] = -1.0f * ((sample.x + offset.x) * A)/length;
|
|
ret[4] = -1.0f * ((sample.y + offset.y) * B)/length;
|
|
ret[5] = -1.0f * ((sample.z + offset.z) * C)/length;
|
|
// 6-8: off-diagonals
|
|
ret[6] = -1.0f * (((sample.y + offset.y) * A) + ((sample.x + offset.x) * B))/length;
|
|
ret[7] = -1.0f * (((sample.z + offset.z) * A) + ((sample.x + offset.x) * C))/length;
|
|
ret[8] = -1.0f * (((sample.z + offset.z) * B) + ((sample.y + offset.y) * C))/length;
|
|
}
|
|
|
|
void CompassCalibrator::run_ellipsoid_fit()
|
|
{
|
|
if(_sample_buffer == NULL) {
|
|
return;
|
|
}
|
|
|
|
const float lma_damping = 10.0f;
|
|
|
|
|
|
float fitness = _fitness;
|
|
float fit1, fit2;
|
|
param_t fit1_params, fit2_params;
|
|
fit1_params = fit2_params = _params;
|
|
|
|
|
|
float JTJ[COMPASS_CAL_NUM_ELLIPSOID_PARAMS*COMPASS_CAL_NUM_ELLIPSOID_PARAMS];
|
|
float JTJ2[COMPASS_CAL_NUM_ELLIPSOID_PARAMS*COMPASS_CAL_NUM_ELLIPSOID_PARAMS];
|
|
float JTFI[COMPASS_CAL_NUM_ELLIPSOID_PARAMS];
|
|
|
|
memset(&JTJ,0,sizeof(JTJ));
|
|
memset(&JTJ2,0,sizeof(JTJ2));
|
|
memset(&JTFI,0,sizeof(JTFI));
|
|
|
|
for(uint16_t k = 0; k<_samples_collected; k++) {
|
|
Vector3f sample = _sample_buffer[k].get();
|
|
|
|
float ellipsoid_jacob[COMPASS_CAL_NUM_ELLIPSOID_PARAMS];
|
|
|
|
calc_ellipsoid_jacob(sample, fit1_params, ellipsoid_jacob);
|
|
|
|
for(uint8_t i = 0;i < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; i++) {
|
|
// compute JTJ
|
|
for(uint8_t j = 0; j < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; j++) {
|
|
JTJ [i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+j] += ellipsoid_jacob[i] * ellipsoid_jacob[j];
|
|
JTJ2[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+j] += ellipsoid_jacob[i] * ellipsoid_jacob[j];
|
|
}
|
|
// compute JTFI
|
|
JTFI[i] += ellipsoid_jacob[i] * calc_residual(sample, fit1_params);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
//------------------------Levenberg-part-starts-here---------------------------------//
|
|
for(uint8_t i = 0; i < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; i++) {
|
|
JTJ[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+i] += _ellipsoid_lambda;
|
|
JTJ2[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+i] += _ellipsoid_lambda/lma_damping;
|
|
}
|
|
|
|
if(!inverse9x9(JTJ, JTJ)) {
|
|
return;
|
|
}
|
|
|
|
if(!inverse9x9(JTJ2, JTJ2)) {
|
|
return;
|
|
}
|
|
|
|
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];
|
|
fit2_params.get_ellipsoid_params()[row] -= JTFI[col] * JTJ2[row*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+col];
|
|
}
|
|
}
|
|
|
|
fit1 = calc_mean_squared_residuals(fit1_params);
|
|
fit2 = calc_mean_squared_residuals(fit2_params);
|
|
|
|
if(fit1 > _fitness && fit2 > _fitness){
|
|
_ellipsoid_lambda *= lma_damping;
|
|
} else if(fit2 < _fitness && fit2 < fit1) {
|
|
_ellipsoid_lambda /= lma_damping;
|
|
fit1_params = fit2_params;
|
|
fitness = fit2;
|
|
} else if(fit1 < _fitness){
|
|
fitness = fit1;
|
|
}
|
|
//--------------------Levenberg-part-ends-here--------------------------------//
|
|
|
|
if(fitness < _fitness) {
|
|
_fitness = fitness;
|
|
_params = fit1_params;
|
|
}
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////
|
|
////////////////////// MATH HELPERS //////////////////////
|
|
//////////////////////////////////////////////////////////
|
|
bool CompassCalibrator::inverse9x9(const float x[81], float y[81])
|
|
{
|
|
if(fabsf(det9x9(x)) < 1.0e-20f) {
|
|
return false;
|
|
}
|
|
float A[81];
|
|
int32_t i0;
|
|
int8_t ipiv[9];
|
|
int32_t j;
|
|
int32_t c;
|
|
int32_t pipk;
|
|
int32_t ix;
|
|
float smax;
|
|
int32_t k;
|
|
float s;
|
|
int32_t jy;
|
|
int32_t ijA;
|
|
int8_t p[9];
|
|
for (i0 = 0; i0 < 81; i0++) {
|
|
A[i0] = x[i0];
|
|
y[i0] = 0.0;
|
|
}
|
|
|
|
for (i0 = 0; i0 < 9; i0++) {
|
|
ipiv[i0] = (int8_t)(1 + i0);
|
|
}
|
|
|
|
for (j = 0; j < 8; j++) {
|
|
c = j * 10;
|
|
pipk = 0;
|
|
ix = c;
|
|
smax = fabs(A[c]);
|
|
for (k = 2; k <= 9 - j; k++) {
|
|
ix++;
|
|
s = fabs(A[ix]);
|
|
if (s > smax) {
|
|
pipk = k - 1;
|
|
smax = s;
|
|
}
|
|
}
|
|
|
|
if (A[c + pipk] != 0.0) {
|
|
if (pipk != 0) {
|
|
ipiv[j] = (int8_t)((j + pipk) + 1);
|
|
ix = j;
|
|
pipk += j;
|
|
for (k = 0; k < 9; k++) {
|
|
smax = A[ix];
|
|
A[ix] = A[pipk];
|
|
A[pipk] = smax;
|
|
ix += 9;
|
|
pipk += 9;
|
|
}
|
|
}
|
|
|
|
i0 = (c - j) + 9;
|
|
for (jy = c + 1; jy + 1 <= i0; jy++) {
|
|
A[jy] /= A[c];
|
|
}
|
|
}
|
|
|
|
pipk = c;
|
|
jy = c + 9;
|
|
for (k = 1; k <= 8 - j; k++) {
|
|
smax = A[jy];
|
|
if (A[jy] != 0.0) {
|
|
ix = c + 1;
|
|
i0 = (pipk - j) + 18;
|
|
for (ijA = 10 + pipk; ijA + 1 <= i0; ijA++) {
|
|
A[ijA] += A[ix] * -smax;
|
|
ix++;
|
|
}
|
|
}
|
|
|
|
jy += 9;
|
|
pipk += 9;
|
|
}
|
|
}
|
|
|
|
for (i0 = 0; i0 < 9; i0++) {
|
|
p[i0] = (int8_t)(1 + i0);
|
|
}
|
|
|
|
for (k = 0; k < 8; k++) {
|
|
if (ipiv[k] > 1 + k) {
|
|
pipk = p[ipiv[k] - 1];
|
|
p[ipiv[k] - 1] = p[k];
|
|
p[k] = (int8_t)pipk;
|
|
}
|
|
}
|
|
|
|
for (k = 0; k < 9; k++) {
|
|
y[k + 9 * (p[k] - 1)] = 1.0;
|
|
for (j = k; j + 1 < 10; j++) {
|
|
if (y[j + 9 * (p[k] - 1)] != 0.0) {
|
|
for (jy = j + 1; jy + 1 < 10; jy++) {
|
|
y[jy + 9 * (p[k] - 1)] -= y[j + 9 * (p[k] - 1)] * A[jy + 9 * j];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
for (j = 0; j < 9; j++) {
|
|
c = 9 * j;
|
|
for (k = 8; k > -1; k += -1) {
|
|
pipk = 9 * k;
|
|
if (y[k + c] != 0.0) {
|
|
y[k + c] /= A[k + pipk];
|
|
for (jy = 0; jy + 1 <= k; jy++) {
|
|
y[jy + c] -= y[k + c] * A[jy + pipk];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
bool CompassCalibrator::inverse6x6(const float x[], float y[])
|
|
{
|
|
if(fabsf(det6x6(x)) < 1.0e-20f) {
|
|
return false;
|
|
}
|
|
|
|
float A[36];
|
|
int32_t i0;
|
|
int32_t ipiv[6];
|
|
int32_t j;
|
|
int32_t c;
|
|
int32_t pipk;
|
|
int32_t ix;
|
|
float smax;
|
|
int32_t k;
|
|
float s;
|
|
int32_t jy;
|
|
int32_t ijA;
|
|
int32_t p[6];
|
|
for (i0 = 0; i0 < 36; i0++) {
|
|
A[i0] = x[i0];
|
|
y[i0] = 0.0f;
|
|
}
|
|
|
|
for (i0 = 0; i0 < 6; i0++) {
|
|
ipiv[i0] = (int32_t)(1 + i0);
|
|
}
|
|
|
|
for (j = 0; j < 5; j++) {
|
|
c = j * 7;
|
|
pipk = 0;
|
|
ix = c;
|
|
smax = fabsf(A[c]);
|
|
for (k = 2; k <= 6 - j; k++) {
|
|
ix++;
|
|
s = fabsf(A[ix]);
|
|
if (s > smax) {
|
|
pipk = k - 1;
|
|
smax = s;
|
|
}
|
|
}
|
|
|
|
if (A[c + pipk] != 0.0f) {
|
|
if (pipk != 0) {
|
|
ipiv[j] = (int32_t)((j + pipk) + 1);
|
|
ix = j;
|
|
pipk += j;
|
|
for (k = 0; k < 6; k++) {
|
|
smax = A[ix];
|
|
A[ix] = A[pipk];
|
|
A[pipk] = smax;
|
|
ix += 6;
|
|
pipk += 6;
|
|
}
|
|
}
|
|
|
|
i0 = (c - j) + 6;
|
|
for (jy = c + 1; jy + 1 <= i0; jy++) {
|
|
A[jy] /= A[c];
|
|
}
|
|
}
|
|
|
|
pipk = c;
|
|
jy = c + 6;
|
|
for (k = 1; k <= 5 - j; k++) {
|
|
smax = A[jy];
|
|
if (A[jy] != 0.0f) {
|
|
ix = c + 1;
|
|
i0 = (pipk - j) + 12;
|
|
for (ijA = 7 + pipk; ijA + 1 <= i0; ijA++) {
|
|
A[ijA] += A[ix] * -smax;
|
|
ix++;
|
|
}
|
|
}
|
|
|
|
jy += 6;
|
|
pipk += 6;
|
|
}
|
|
}
|
|
|
|
for (i0 = 0; i0 < 6; i0++) {
|
|
p[i0] = (int32_t)(1 + i0);
|
|
}
|
|
|
|
for (k = 0; k < 5; k++) {
|
|
if (ipiv[k] > 1 + k) {
|
|
pipk = p[ipiv[k] - 1];
|
|
p[ipiv[k] - 1] = p[k];
|
|
p[k] = (int32_t)pipk;
|
|
}
|
|
}
|
|
|
|
for (k = 0; k < 6; k++) {
|
|
y[k + 6 * (p[k] - 1)] = 1.0;
|
|
for (j = k; j + 1 < 7; j++) {
|
|
if (y[j + 6 * (p[k] - 1)] != 0.0f) {
|
|
for (jy = j + 1; jy + 1 < 7; jy++) {
|
|
y[jy + 6 * (p[k] - 1)] -= y[j + 6 * (p[k] - 1)] * A[jy + 6 * j];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
for (j = 0; j < 6; j++) {
|
|
c = 6 * j;
|
|
for (k = 5; k > -1; k += -1) {
|
|
pipk = 6 * k;
|
|
if (y[k + c] != 0.0f) {
|
|
y[k + c] /= A[k + pipk];
|
|
for (jy = 0; jy + 1 <= k; jy++) {
|
|
y[jy + c] -= y[k + c] * A[jy + pipk];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool CompassCalibrator::inverse3x3(float m[], float invOut[])
|
|
{
|
|
float inv[9];
|
|
// computes the inverse of a matrix m
|
|
float det = m[0] * (m[4] * m[8] - m[7] * m[5]) -
|
|
m[1] * (m[3] * m[8] - m[5] * m[6]) +
|
|
m[2] * (m[3] * m[7] - m[4] * m[6]);
|
|
if(fabsf(det) < 1.0e-20f){
|
|
return false;
|
|
}
|
|
|
|
float invdet = 1 / det;
|
|
|
|
inv[0] = (m[4] * m[8] - m[7] * m[5]) * invdet;
|
|
inv[1] = (m[2] * m[7] - m[1] * m[8]) * invdet;
|
|
inv[2] = (m[1] * m[5] - m[2] * m[4]) * invdet;
|
|
inv[3] = (m[5] * m[6] - m[5] * m[8]) * invdet;
|
|
inv[4] = (m[0] * m[8] - m[2] * m[6]) * invdet;
|
|
inv[5] = (m[3] * m[2] - m[0] * m[5]) * invdet;
|
|
inv[6] = (m[3] * m[7] - m[6] * m[4]) * invdet;
|
|
inv[7] = (m[6] * m[1] - m[0] * m[7]) * invdet;
|
|
inv[8] = (m[0] * m[4] - m[3] * m[1]) * invdet;
|
|
|
|
for(uint8_t i = 0; i < 9; i++){
|
|
invOut[i] = inv[i];
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
* matrix inverse code only for 4x4 square matrix copied from
|
|
* gluInvertMatrix implementation in
|
|
* opengl for 4x4 matrices.
|
|
*
|
|
* @param m, input 4x4 matrix
|
|
* @param invOut, Output inverted 4x4 matrix
|
|
* @returns false = matrix is Singular, true = matrix inversion successful
|
|
* Known Issues/ Possible Enhancements:
|
|
* -Will need a different implementation for more number
|
|
* of parameters like in the case of addition of soft
|
|
* iron calibration
|
|
*/
|
|
bool CompassCalibrator::inverse4x4(float m[],float invOut[])
|
|
{
|
|
float inv[16], det;
|
|
uint8_t i;
|
|
|
|
inv[0] = m[5] * m[10] * m[15] -
|
|
m[5] * m[11] * m[14] -
|
|
m[9] * m[6] * m[15] +
|
|
m[9] * m[7] * m[14] +
|
|
m[13] * m[6] * m[11] -
|
|
m[13] * m[7] * m[10];
|
|
|
|
inv[4] = -m[4] * m[10] * m[15] +
|
|
m[4] * m[11] * m[14] +
|
|
m[8] * m[6] * m[15] -
|
|
m[8] * m[7] * m[14] -
|
|
m[12] * m[6] * m[11] +
|
|
m[12] * m[7] * m[10];
|
|
|
|
inv[8] = m[4] * m[9] * m[15] -
|
|
m[4] * m[11] * m[13] -
|
|
m[8] * m[5] * m[15] +
|
|
m[8] * m[7] * m[13] +
|
|
m[12] * m[5] * m[11] -
|
|
m[12] * m[7] * m[9];
|
|
|
|
inv[12] = -m[4] * m[9] * m[14] +
|
|
m[4] * m[10] * m[13] +
|
|
m[8] * m[5] * m[14] -
|
|
m[8] * m[6] * m[13] -
|
|
m[12] * m[5] * m[10] +
|
|
m[12] * m[6] * m[9];
|
|
|
|
inv[1] = -m[1] * m[10] * m[15] +
|
|
m[1] * m[11] * m[14] +
|
|
m[9] * m[2] * m[15] -
|
|
m[9] * m[3] * m[14] -
|
|
m[13] * m[2] * m[11] +
|
|
m[13] * m[3] * m[10];
|
|
|
|
inv[5] = m[0] * m[10] * m[15] -
|
|
m[0] * m[11] * m[14] -
|
|
m[8] * m[2] * m[15] +
|
|
m[8] * m[3] * m[14] +
|
|
m[12] * m[2] * m[11] -
|
|
m[12] * m[3] * m[10];
|
|
|
|
inv[9] = -m[0] * m[9] * m[15] +
|
|
m[0] * m[11] * m[13] +
|
|
m[8] * m[1] * m[15] -
|
|
m[8] * m[3] * m[13] -
|
|
m[12] * m[1] * m[11] +
|
|
m[12] * m[3] * m[9];
|
|
|
|
inv[13] = m[0] * m[9] * m[14] -
|
|
m[0] * m[10] * m[13] -
|
|
m[8] * m[1] * m[14] +
|
|
m[8] * m[2] * m[13] +
|
|
m[12] * m[1] * m[10] -
|
|
m[12] * m[2] * m[9];
|
|
|
|
inv[2] = m[1] * m[6] * m[15] -
|
|
m[1] * m[7] * m[14] -
|
|
m[5] * m[2] * m[15] +
|
|
m[5] * m[3] * m[14] +
|
|
m[13] * m[2] * m[7] -
|
|
m[13] * m[3] * m[6];
|
|
|
|
inv[6] = -m[0] * m[6] * m[15] +
|
|
m[0] * m[7] * m[14] +
|
|
m[4] * m[2] * m[15] -
|
|
m[4] * m[3] * m[14] -
|
|
m[12] * m[2] * m[7] +
|
|
m[12] * m[3] * m[6];
|
|
|
|
inv[10] = m[0] * m[5] * m[15] -
|
|
m[0] * m[7] * m[13] -
|
|
m[4] * m[1] * m[15] +
|
|
m[4] * m[3] * m[13] +
|
|
m[12] * m[1] * m[7] -
|
|
m[12] * m[3] * m[5];
|
|
|
|
inv[14] = -m[0] * m[5] * m[14] +
|
|
m[0] * m[6] * m[13] +
|
|
m[4] * m[1] * m[14] -
|
|
m[4] * m[2] * m[13] -
|
|
m[12] * m[1] * m[6] +
|
|
m[12] * m[2] * m[5];
|
|
|
|
inv[3] = -m[1] * m[6] * m[11] +
|
|
m[1] * m[7] * m[10] +
|
|
m[5] * m[2] * m[11] -
|
|
m[5] * m[3] * m[10] -
|
|
m[9] * m[2] * m[7] +
|
|
m[9] * m[3] * m[6];
|
|
|
|
inv[7] = m[0] * m[6] * m[11] -
|
|
m[0] * m[7] * m[10] -
|
|
m[4] * m[2] * m[11] +
|
|
m[4] * m[3] * m[10] +
|
|
m[8] * m[2] * m[7] -
|
|
m[8] * m[3] * m[6];
|
|
|
|
inv[11] = -m[0] * m[5] * m[11] +
|
|
m[0] * m[7] * m[9] +
|
|
m[4] * m[1] * m[11] -
|
|
m[4] * m[3] * m[9] -
|
|
m[8] * m[1] * m[7] +
|
|
m[8] * m[3] * m[5];
|
|
|
|
inv[15] = m[0] * m[5] * m[10] -
|
|
m[0] * m[6] * m[9] -
|
|
m[4] * m[1] * m[10] +
|
|
m[4] * m[2] * m[9] +
|
|
m[8] * m[1] * m[6] -
|
|
m[8] * m[2] * m[5];
|
|
|
|
det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
|
|
|
|
if(fabsf(det) < 1.0e-20f){
|
|
return false;
|
|
}
|
|
|
|
det = 1.0f / det;
|
|
|
|
for (i = 0; i < 16; i++)
|
|
invOut[i] = inv[i] * det;
|
|
return true;
|
|
}
|
|
|
|
float CompassCalibrator::det6x6(const float C[36])
|
|
{
|
|
float f;
|
|
float A[36];
|
|
int8_t ipiv[6];
|
|
int32_t i0;
|
|
int32_t j;
|
|
int32_t c;
|
|
int32_t iy;
|
|
int32_t ix;
|
|
float smax;
|
|
int32_t jy;
|
|
float s;
|
|
int32_t b_j;
|
|
int32_t ijA;
|
|
bool isodd;
|
|
memcpy(&A[0], &C[0], 36U * sizeof(float));
|
|
for (i0 = 0; i0 < 6; i0++) {
|
|
ipiv[i0] = (int8_t)(1 + i0);
|
|
}
|
|
|
|
for (j = 0; j < 5; j++) {
|
|
c = j * 7;
|
|
iy = 0;
|
|
ix = c;
|
|
smax = fabsf(A[c]);
|
|
for (jy = 2; jy <= 6 - j; jy++) {
|
|
ix++;
|
|
s = fabsf(A[ix]);
|
|
if (s > smax) {
|
|
iy = jy - 1;
|
|
smax = s;
|
|
}
|
|
}
|
|
|
|
if (A[c + iy] != 0.0f) {
|
|
if (iy != 0) {
|
|
ipiv[j] = (int8_t)((j + iy) + 1);
|
|
ix = j;
|
|
iy += j;
|
|
for (jy = 0; jy < 6; jy++) {
|
|
smax = A[ix];
|
|
A[ix] = A[iy];
|
|
A[iy] = smax;
|
|
ix += 6;
|
|
iy += 6;
|
|
}
|
|
}
|
|
|
|
i0 = (c - j) + 6;
|
|
for (iy = c + 1; iy + 1 <= i0; iy++) {
|
|
A[iy] /= A[c];
|
|
}
|
|
}
|
|
|
|
iy = c;
|
|
jy = c + 6;
|
|
for (b_j = 1; b_j <= 5 - j; b_j++) {
|
|
smax = A[jy];
|
|
if (A[jy] != 0.0f) {
|
|
ix = c + 1;
|
|
i0 = (iy - j) + 12;
|
|
for (ijA = 7 + iy; ijA + 1 <= i0; ijA++) {
|
|
A[ijA] += A[ix] * -smax;
|
|
ix++;
|
|
}
|
|
}
|
|
|
|
jy += 6;
|
|
iy += 6;
|
|
}
|
|
}
|
|
|
|
f = A[0];
|
|
isodd = false;
|
|
for (jy = 0; jy < 5; jy++) {
|
|
f *= A[(jy + 6 * (1 + jy)) + 1];
|
|
if (ipiv[jy] > 1 + jy) {
|
|
isodd = !isodd;
|
|
}
|
|
}
|
|
|
|
if (isodd) {
|
|
f = -f;
|
|
}
|
|
|
|
return f;
|
|
}
|
|
|
|
float CompassCalibrator::det9x9(const float C[81])
|
|
{
|
|
float f;
|
|
float A[81];
|
|
int8_t ipiv[9];
|
|
int32_t i0;
|
|
int32_t j;
|
|
int32_t c;
|
|
int32_t iy;
|
|
int32_t ix;
|
|
float smax;
|
|
int32_t jy;
|
|
float s;
|
|
int32_t b_j;
|
|
int32_t ijA;
|
|
bool isodd;
|
|
memcpy(&A[0], &C[0], 81U * sizeof(float));
|
|
for (i0 = 0; i0 < 9; i0++) {
|
|
ipiv[i0] = (int8_t)(1 + i0);
|
|
}
|
|
|
|
for (j = 0; j < 8; j++) {
|
|
c = j * 10;
|
|
iy = 0;
|
|
ix = c;
|
|
smax = fabs(A[c]);
|
|
for (jy = 2; jy <= 9 - j; jy++) {
|
|
ix++;
|
|
s = fabs(A[ix]);
|
|
if (s > smax) {
|
|
iy = jy - 1;
|
|
smax = s;
|
|
}
|
|
}
|
|
|
|
if (A[c + iy] != 0.0) {
|
|
if (iy != 0) {
|
|
ipiv[j] = (int8_t)((j + iy) + 1);
|
|
ix = j;
|
|
iy += j;
|
|
for (jy = 0; jy < 9; jy++) {
|
|
smax = A[ix];
|
|
A[ix] = A[iy];
|
|
A[iy] = smax;
|
|
ix += 9;
|
|
iy += 9;
|
|
}
|
|
}
|
|
|
|
i0 = (c - j) + 9;
|
|
for (iy = c + 1; iy + 1 <= i0; iy++) {
|
|
A[iy] /= A[c];
|
|
}
|
|
}
|
|
|
|
iy = c;
|
|
jy = c + 9;
|
|
for (b_j = 1; b_j <= 8 - j; b_j++) {
|
|
smax = A[jy];
|
|
if (A[jy] != 0.0) {
|
|
ix = c + 1;
|
|
i0 = (iy - j) + 18;
|
|
for (ijA = 10 + iy; ijA + 1 <= i0; ijA++) {
|
|
A[ijA] += A[ix] * -smax;
|
|
ix++;
|
|
}
|
|
}
|
|
|
|
jy += 9;
|
|
iy += 9;
|
|
}
|
|
}
|
|
|
|
f = A[0];
|
|
isodd = false;
|
|
for (jy = 0; jy < 8; jy++) {
|
|
f *= A[(jy + 9 * (1 + jy)) + 1];
|
|
if (ipiv[jy] > 1 + jy) {
|
|
isodd = !isodd;
|
|
}
|
|
}
|
|
|
|
if (isodd) {
|
|
f = -f;
|
|
}
|
|
|
|
return f;
|
|
}
|
|
|
|
uint16_t CompassCalibrator::get_random(void)
|
|
{
|
|
static uint32_t m_z = 1234;
|
|
static uint32_t m_w = 76542;
|
|
m_z = 36969 * (m_z & 65535) + (m_z >> 16);
|
|
m_w = 18000 * (m_w & 65535) + (m_w >> 16);
|
|
return ((m_z << 16) + m_w) & 0xFFFF;
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////
|
|
//////////// CompassSample public interface //////////////
|
|
//////////////////////////////////////////////////////////
|
|
|
|
Vector3f CompassCalibrator::CompassSample::get() const {
|
|
Vector3f out;
|
|
out.x = (float)x*2048.0f/32700.0f;
|
|
out.y = (float)y*2048.0f/32700.0f;
|
|
out.z = (float)z*2048.0f/32700.0f;
|
|
return out;
|
|
}
|
|
|
|
void CompassCalibrator::CompassSample::set(const Vector3f &in) {
|
|
x = (int16_t)(in.x*32700.0f/2048.0f + 0.5f);
|
|
y = (int16_t)(in.y*32700.0f/2048.0f + 0.5f);
|
|
z = (int16_t)(in.z*32700.0f/2048.0f + 0.5f);
|
|
}
|