mirror of https://github.com/ArduPilot/ardupilot
AP_AccelCal: fix bug preventing accel cal fit to run more than one iteration
The check for fitness being smaller than the last value should use the
absolute value, to mean the fitness isn't changing since last iteration.
It's currently always quiting the function after the first iteration.
However for Gauss Newton we anyway want to run as many iterations as we
can, because there are ups and downs along the iteration so we might get
say best result where before and after iterations were bad. The lines
above takes care that sane and the best is selected.
(cherry picked from commit 451c2f146d
)
This commit is contained in:
parent
6076bdfc02
commit
b71f766aff
|
@ -334,8 +334,6 @@ void AccelCalibrator::run_fit(uint8_t max_iterations, float& fitness)
|
|||
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] {};
|
||||
VectorP JTFI;
|
||||
|
||||
|
@ -379,9 +377,6 @@ void AccelCalibrator::run_fit(uint8_t max_iterations, float& fitness)
|
|||
}
|
||||
|
||||
num_iterations++;
|
||||
if (fitness - last_fitness < 1.0e-9f) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue