Skip to content

Commit

Permalink
AP_AccelCal: fix bug preventing accel cal fit to run more than one it…
Browse files Browse the repository at this point in the history
…eration

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.
  • Loading branch information
bugobliterator authored and lucasdemarchi committed Nov 18, 2016
1 parent 11229d2 commit 451c2f1
Showing 1 changed file with 0 additions and 5 deletions.
5 changes: 0 additions & 5 deletions libraries/AP_AccelCal/AccelCalibrator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -379,9 +377,6 @@ void AccelCalibrator::run_fit(uint8_t max_iterations, float& fitness)
}

num_iterations++;
if (fitness - last_fitness < 1.0e-9f) {
break;
}
}
}

Expand Down

0 comments on commit 451c2f1

Please sign in to comment.