/*
Run Gauss Newton fitting algorithm over the sample space and come up with offsets, diagonal/scale factors
and crosstalk/offdiagonal parameters
*/
void AccelCalibrator::run_fit(uint8_t max_iterations, float& fitness)
{
if (_sample_buffer == nullptr) {
return;
}
fitness = calc_mean_squared_residuals(_param.s);
float min_fitness = fitness;
union param_u fit_param = _param;
uint8_t num_iterations = 0;
while(num_iterations < max_iterations) {
float JTJ[ACCEL_CAL_MAX_NUM_PARAMS*ACCEL_CAL_MAX_NUM_PARAMS] {};
VectorP JTFI;
for(uint16_t k = 0; k<_samples_collected; k++) {
Vector3f sample;
get_sample(k, sample);
VectorN<float,ACCEL_CAL_MAX_NUM_PARAMS> jacob;
calc_jacob(sample, fit_param.s, jacob);
for(uint8_t i = 0; i < get_num_params(); i++) {
// compute JTJ
for(uint8_t j = 0; j < get_num_params(); j++) {
JTJ[i*get_num_params()+j] += jacob[i] * jacob[j];
}
// compute JTFI
JTFI[i] += jacob[i] * calc_residual(sample, fit_param.s);
}
}
if (!inverse(JTJ, JTJ, get_num_params())) {
return;
}
for(uint8_t row=0; row < get_num_params(); row++) {
for(uint8_t col=0; col < get_num_params(); col++) {
fit_param.a[row] -= JTFI[col] * JTJ[row*get_num_params()+col];
}
}
fitness = calc_mean_squared_residuals(fit_param.s);
if (isnan(fitness) || isinf(fitness)) {
return;
}
if (fitness < min_fitness) {
min_fitness = fitness;
_param = fit_param;
}
num_iterations++;
}
}
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;
}
// calculated the total mean squared fitness of all the collected samples using parameters
// converged to LSq estimator so far
float AccelCalibrator::calc_mean_squared_residuals() const
{
return calc_mean_squared_residuals(_param.s);
}
void AccelCalibrator::run_fit(uint8_t max_iterations, float& fitness)
{
if(_sample_buffer == NULL) {
return;
}
fitness = calc_mean_squared_residuals(_params);
float min_fitness = fitness;
struct param_t fit_param = _params;
float* param_array = (float*)&fit_param;
uint8_t num_iterations = 0;
while(num_iterations < max_iterations) {
float last_fitness = fitness;
float JTJ[ACCEL_CAL_MAX_NUM_PARAMS*ACCEL_CAL_MAX_NUM_PARAMS];
float JTFI[ACCEL_CAL_MAX_NUM_PARAMS];
memset(&JTJ,0,sizeof(JTJ));
memset(&JTFI,0,sizeof(JTFI));
for(uint16_t k = 0; k<_samples_collected; k++) {
Vector3f sample;
get_sample(k, sample);
float jacob[ACCEL_CAL_MAX_NUM_PARAMS];
calc_jacob(sample, fit_param, jacob);
for(uint8_t i = 0; i < get_num_params(); i++) {
// compute JTJ
for(uint8_t j = 0; j < get_num_params(); j++) {
JTJ[i*get_num_params()+j] += jacob[i] * jacob[j];
}
// compute JTFI
JTFI[i] += jacob[i] * calc_residual(sample, fit_param);
}
}
if(!inverse(JTJ, JTJ, get_num_params())) {
return;
}
for(uint8_t row=0; row < get_num_params(); row++) {
for(uint8_t col=0; col < get_num_params(); col++) {
param_array[row] -= JTFI[col] * JTJ[row*get_num_params()+col];
}
}
fitness = calc_mean_squared_residuals(fit_param);
if(isnan(fitness) || isinf(fitness)) {
return;
}
if(fitness < min_fitness) {
min_fitness = fitness;
_params = fit_param;
}
num_iterations++;
if (fitness - last_fitness < 1.0e-9f) {
break;
}
}
}
void CompassCalibrator::run_ellipsoid_fit()
{
if(_sample_buffer == nullptr) {
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] = { };
// Gauss Newton Part common for all kind of extensions including LM
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-Marquardt-part-starts-here---------------------------------//
//refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
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(!inverse(JTJ, JTJ, 9)) {
return;
}
if(!inverse(JTJ2, JTJ2, 9)) {
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;
update_completion_mask();
}
}
float CompassCalibrator::calc_mean_squared_residuals() const
{
return calc_mean_squared_residuals(_params);
}
