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