/*
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 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;
}
}
}
