/* 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++; } }
END_TEST START_TEST (test_get_num_params) { ck_assert(check_enter_method("program", parser_data) != NULL); ck_assert(check_enter_method("fun", parser_data) != NULL); set_method_param_count(100, parser_data); ck_assert_int_eq(100, get_num_params("fun", parser_data)); }
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; } } }