Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 : This program is distributed in the hope that it will be useful,
7 : but WITHOUT ANY WARRANTY; without even the implied warranty of
8 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
9 : GNU General Public License for more details.
10 : You should have received a copy of the GNU General Public License
11 : along with this program. If not, see <http://www.gnu.org/licenses/>.
12 : */
13 :
14 : // Authored by Jonathan Challinger, 3D Robotics Inc.
15 :
16 : #include "AccelCalibrator.h"
17 : #include <stdio.h>
18 : #include <AP_HAL/AP_HAL.h>
19 :
20 : const extern AP_HAL::HAL& hal;
21 : /*
22 : * TODO
23 : * - time out when not receiving samples
24 : */
25 :
26 : ////////////////////////////////////////////////////////////
27 : ///////////////////// PUBLIC INTERFACE /////////////////////
28 : ////////////////////////////////////////////////////////////
29 :
30 7 : AccelCalibrator::AccelCalibrator() :
31 7 : _conf_tolerance(ACCEL_CAL_TOLERANCE),
32 7 : _sample_buffer(nullptr)
33 : {
34 7 : clear();
35 7 : }
36 : /*
37 : Select options, initialise variables and initiate accel calibration
38 : Options:
39 : Fit Type: Will assume that if accelerometer static samples around all possible orientation
40 : are spread in space will cover a surface of AXIS_ALIGNED_ELLIPSOID or any general
41 : ELLIPSOID as chosen
42 :
43 : Num Samples: Number of samples user should will be gathering, please note that with type of surface
44 : chosen the minimum number of samples required will vary, for instance in the case of AXIS
45 : ALIGNED ELLIPSOID atleast 6 distinct samples are required while for generic ELLIPSOIDAL fit
46 : which will include calculation of offdiagonal parameters too requires atleast 8 parameters
47 : to form a distinct ELLIPSOID
48 :
49 : Sample Time: Time over which the samples will be gathered and averaged to add to a single sample for least
50 : square regression
51 :
52 : offset,diag,offdiag: initial parameter values for LSQ calculation
53 : */
54 4 : void AccelCalibrator::start(enum accel_cal_fit_type_t fit_type, uint8_t num_samples, float sample_time) {
55 4 : start(fit_type, num_samples, sample_time, Vector3f(0,0,0), Vector3f(1,1,1), Vector3f(0,0,0));
56 4 : }
57 :
58 4 : void AccelCalibrator::start(enum accel_cal_fit_type_t fit_type, uint8_t num_samples, float sample_time, Vector3f offset, Vector3f diag, Vector3f offdiag) {
59 4 : if (_status == ACCEL_CAL_FAILED || _status == ACCEL_CAL_SUCCESS) {
60 0 : clear();
61 : }
62 4 : if (_status != ACCEL_CAL_NOT_STARTED) {
63 0 : return;
64 : }
65 :
66 4 : _conf_num_samples = num_samples;
67 4 : _conf_sample_time = sample_time;
68 4 : _conf_fit_type = fit_type;
69 :
70 4 : const uint16_t faces = 2*_conf_num_samples-4;
71 4 : const float a = (4.0f * M_PI / (3.0f * faces)) + M_PI / 3.0f;
72 4 : const float theta = 0.5f * acosf(cosf(a) / (1.0f - cosf(a)));
73 4 : _min_sample_dist = GRAVITY_MSS * 2*sinf(theta/2);
74 :
75 4 : _param.s.offset = offset;
76 4 : _param.s.diag = diag;
77 4 : _param.s.offdiag = offdiag;
78 :
79 4 : switch (_conf_fit_type) {
80 4 : case ACCEL_CAL_AXIS_ALIGNED_ELLIPSOID:
81 4 : if (_conf_num_samples < 6) {
82 0 : set_status(ACCEL_CAL_FAILED);
83 0 : return;
84 : }
85 4 : break;
86 0 : case ACCEL_CAL_ELLIPSOID:
87 0 : if (_conf_num_samples < 8) {
88 0 : set_status(ACCEL_CAL_FAILED);
89 0 : return;
90 : }
91 0 : break;
92 : }
93 :
94 4 : set_status(ACCEL_CAL_WAITING_FOR_ORIENTATION);
95 : }
96 :
97 :
98 : // set Accel calibrator status to make itself ready for future accel cals
99 13 : void AccelCalibrator::clear() {
100 13 : set_status(ACCEL_CAL_NOT_STARTED);
101 13 : }
102 :
103 : // returns true if accel calibrator is running
104 14 : bool AccelCalibrator::running() {
105 14 : return _status == ACCEL_CAL_WAITING_FOR_ORIENTATION || _status == ACCEL_CAL_COLLECTING_SAMPLE;
106 : }
107 :
108 : // set Accel calibrator to start collecting samples in the next cycle
109 12 : void AccelCalibrator::collect_sample() {
110 12 : set_status(ACCEL_CAL_COLLECTING_SAMPLE);
111 12 : }
112 :
113 : // collect and avg sample to be passed onto LSQ estimator after all requisite orientations are done
114 311 : void AccelCalibrator::new_sample(const Vector3f& delta_velocity, float dt) {
115 311 : if (_status != ACCEL_CAL_COLLECTING_SAMPLE) {
116 0 : return;
117 : }
118 :
119 311 : if (_samples_collected >= _conf_num_samples) {
120 0 : set_status(ACCEL_CAL_FAILED);
121 0 : return;
122 : }
123 :
124 311 : _sample_buffer[_samples_collected].delta_velocity += delta_velocity;
125 311 : _sample_buffer[_samples_collected].delta_time += dt;
126 :
127 311 : _last_samp_frag_collected_ms = AP_HAL::millis();
128 :
129 311 : if (_sample_buffer[_samples_collected].delta_time > _conf_sample_time) {
130 12 : Vector3f sample = _sample_buffer[_samples_collected].delta_velocity/_sample_buffer[_samples_collected].delta_time;
131 12 : if (!accept_sample(sample)) {
132 0 : set_status(ACCEL_CAL_FAILED);
133 0 : return;
134 : }
135 :
136 12 : _samples_collected++;
137 :
138 12 : if (_samples_collected >= _conf_num_samples) {
139 2 : run_fit(MAX_ITERATIONS, _fitness);
140 :
141 2 : if (_fitness < _conf_tolerance && accept_result()) {
142 2 : set_status(ACCEL_CAL_SUCCESS);
143 : } else {
144 0 : set_status(ACCEL_CAL_FAILED);
145 : }
146 : } else {
147 10 : set_status(ACCEL_CAL_WAITING_FOR_ORIENTATION);
148 : }
149 : }
150 : }
151 :
152 : // determines if the result is acceptable
153 2 : bool AccelCalibrator::accept_result() const {
154 2 : if (fabsf(_param.s.offset.x) > GRAVITY_MSS ||
155 2 : fabsf(_param.s.offset.y) > GRAVITY_MSS ||
156 2 : fabsf(_param.s.offset.z) > GRAVITY_MSS ||
157 2 : _param.s.diag.x < 0.8f || _param.s.diag.x > 1.2f ||
158 2 : _param.s.diag.y < 0.8f || _param.s.diag.y > 1.2f ||
159 2 : _param.s.diag.z < 0.8f || _param.s.diag.z > 1.2f) {
160 0 : return false;
161 : } else {
162 2 : return true;
163 : }
164 : }
165 :
166 : // interface for LSq estimator to read sample buffer sent after conversion from delta velocity
167 : // to averaged acc over time
168 1244 : bool AccelCalibrator::get_sample(uint8_t i, Vector3f& s) const {
169 1244 : if (i >= _samples_collected) {
170 0 : return false;
171 : }
172 1244 : s = _sample_buffer[i].delta_velocity / _sample_buffer[i].delta_time;
173 1244 : return true;
174 : }
175 :
176 : // returns true and sample corrected with diag offdiag parameters as calculated by LSq estimation procedure
177 : // returns false if no correct parameter exists to be applied along with existing sample without corrections
178 2 : bool AccelCalibrator::get_sample_corrected(uint8_t i, Vector3f& s) const {
179 2 : if (_status != ACCEL_CAL_SUCCESS || !get_sample(i,s)) {
180 0 : return false;
181 : }
182 :
183 : Matrix3f M (
184 2 : _param.s.diag.x , _param.s.offdiag.x , _param.s.offdiag.y,
185 2 : _param.s.offdiag.x , _param.s.diag.y , _param.s.offdiag.z,
186 2 : _param.s.offdiag.y , _param.s.offdiag.z , _param.s.diag.z
187 2 : );
188 :
189 2 : s = M*(s+_param.s.offset);
190 :
191 2 : return true;
192 : }
193 :
194 : // checks if no new sample has been received for considerable amount of time
195 72 : void AccelCalibrator::check_for_timeout() {
196 72 : const uint32_t timeout = _conf_sample_time*2*1000 + 500;
197 72 : if (_status == ACCEL_CAL_COLLECTING_SAMPLE && AP_HAL::millis() - _last_samp_frag_collected_ms > timeout) {
198 0 : set_status(ACCEL_CAL_FAILED);
199 : }
200 72 : }
201 :
202 : // returns spherical fit parameters
203 0 : void AccelCalibrator::get_calibration(Vector3f& offset) const {
204 0 : offset = -_param.s.offset;
205 0 : }
206 :
207 : // returns axis aligned ellipsoidal fit parameters
208 2 : void AccelCalibrator::get_calibration(Vector3f& offset, Vector3f& diag) const {
209 2 : offset = -_param.s.offset;
210 2 : diag = _param.s.diag;
211 2 : }
212 :
213 : // returns generic ellipsoidal fit parameters
214 0 : void AccelCalibrator::get_calibration(Vector3f& offset, Vector3f& diag, Vector3f& offdiag) const {
215 0 : offset = -_param.s.offset;
216 0 : diag = _param.s.diag;
217 0 : offdiag = _param.s.offdiag;
218 0 : }
219 :
220 : /////////////////////////////////////////////////////////////
221 : ////////////////////// PRIVATE METHODS //////////////////////
222 : /////////////////////////////////////////////////////////////
223 :
224 : /*
225 : The sample acceptance distance is determined as follows:
226 : For any regular polyhedron with triangular faces, the angle theta subtended
227 : by two closest points is defined as
228 :
229 : theta = arccos(cos(A)/(1-cos(A)))
230 :
231 : Where:
232 : A = (4pi/F + pi)/3
233 : and
234 : F = 2V - 4 is the number of faces for the polyhedron in consideration,
235 : which depends on the number of vertices V
236 :
237 : The above equation was proved after solving for spherical triangular excess
238 : and related equations.
239 : */
240 12 : bool AccelCalibrator::accept_sample(const Vector3f& sample)
241 : {
242 12 : if (_sample_buffer == nullptr) {
243 0 : return false;
244 : }
245 :
246 42 : for(uint8_t i=0; i < _samples_collected; i++) {
247 30 : Vector3f other_sample;
248 30 : get_sample(i, other_sample);
249 :
250 30 : if ((other_sample - sample).length() < _min_sample_dist){
251 0 : return false;
252 : }
253 : }
254 12 : return true;
255 : }
256 :
257 : // sets status of calibrator and takes appropriate actions
258 41 : void AccelCalibrator::set_status(enum accel_cal_status_t status) {
259 41 : switch (status) {
260 13 : case ACCEL_CAL_NOT_STARTED:
261 : //Calibrator not started
262 13 : _status = ACCEL_CAL_NOT_STARTED;
263 :
264 13 : _samples_collected = 0;
265 13 : if (_sample_buffer != nullptr) {
266 2 : free(_sample_buffer);
267 2 : _sample_buffer = nullptr;
268 : }
269 :
270 13 : break;
271 :
272 14 : case ACCEL_CAL_WAITING_FOR_ORIENTATION:
273 : //Callibrator has been started and is waiting for user to ack after orientation setting
274 14 : if (!running()) {
275 4 : _samples_collected = 0;
276 4 : if (_sample_buffer == nullptr) {
277 4 : _sample_buffer = (struct AccelSample*)calloc(_conf_num_samples,sizeof(struct AccelSample));
278 4 : if (_sample_buffer == nullptr) {
279 0 : set_status(ACCEL_CAL_FAILED);
280 0 : break;
281 : }
282 : }
283 : }
284 14 : if (_samples_collected >= _conf_num_samples) {
285 0 : break;
286 : }
287 14 : _status = ACCEL_CAL_WAITING_FOR_ORIENTATION;
288 14 : break;
289 :
290 12 : case ACCEL_CAL_COLLECTING_SAMPLE:
291 : // Calibrator is waiting on collecting samples from accelerometer for amount of
292 : // time as requested by user/GCS
293 12 : if (_status != ACCEL_CAL_WAITING_FOR_ORIENTATION) {
294 0 : break;
295 : }
296 12 : _last_samp_frag_collected_ms = AP_HAL::millis();
297 12 : _status = ACCEL_CAL_COLLECTING_SAMPLE;
298 12 : break;
299 :
300 2 : case ACCEL_CAL_SUCCESS:
301 : // Calibrator has successfully fitted the samples to user requested surface model
302 : // and has passed tolerance test
303 2 : if (_status != ACCEL_CAL_COLLECTING_SAMPLE) {
304 0 : break;
305 : }
306 :
307 2 : _status = ACCEL_CAL_SUCCESS;
308 2 : break;
309 :
310 0 : case ACCEL_CAL_FAILED:
311 : // Calibration has failed with reasons that can range from
312 : // bad sample data leading to failure in tolerance test to lack of distinct samples
313 0 : if (_status == ACCEL_CAL_NOT_STARTED) {
314 0 : break;
315 : }
316 :
317 0 : _status = ACCEL_CAL_FAILED;
318 0 : break;
319 : };
320 41 : }
321 :
322 : /*
323 : Run Gauss Newton fitting algorithm over the sample space and come up with offsets, diagonal/scale factors
324 : and crosstalk/offdiagonal parameters
325 : */
326 2 : void AccelCalibrator::run_fit(uint8_t max_iterations, float& fitness)
327 : {
328 2 : if (_sample_buffer == nullptr) {
329 0 : return;
330 : }
331 2 : fitness = calc_mean_squared_residuals(_param.s);
332 2 : float min_fitness = fitness;
333 2 : union param_u fit_param = _param;
334 2 : uint8_t num_iterations = 0;
335 :
336 102 : while(num_iterations < max_iterations) {
337 100 : float JTJ[ACCEL_CAL_MAX_NUM_PARAMS*ACCEL_CAL_MAX_NUM_PARAMS] {};
338 100 : VectorP JTFI;
339 :
340 700 : for(uint16_t k = 0; k<_samples_collected; k++) {
341 600 : Vector3f sample;
342 600 : get_sample(k, sample);
343 :
344 600 : VectorN<float,ACCEL_CAL_MAX_NUM_PARAMS> jacob;
345 :
346 600 : calc_jacob(sample, fit_param.s, jacob);
347 :
348 4200 : for(uint8_t i = 0; i < get_num_params(); i++) {
349 : // compute JTJ
350 25200 : for(uint8_t j = 0; j < get_num_params(); j++) {
351 21600 : JTJ[i*get_num_params()+j] += jacob[i] * jacob[j];
352 : }
353 : // compute JTFI
354 3600 : JTFI[i] += jacob[i] * calc_residual(sample, fit_param.s);
355 : }
356 : }
357 :
358 100 : if (!mat_inverse(JTJ, JTJ, get_num_params())) {
359 0 : return;
360 : }
361 :
362 700 : for(uint8_t row=0; row < get_num_params(); row++) {
363 4200 : for(uint8_t col=0; col < get_num_params(); col++) {
364 3600 : fit_param.a[row] -= JTFI[col] * JTJ[row*get_num_params()+col];
365 : }
366 : }
367 :
368 100 : fitness = calc_mean_squared_residuals(fit_param.s);
369 :
370 100 : if (isnan(fitness) || isinf(fitness)) {
371 0 : return;
372 : }
373 :
374 100 : if (fitness < min_fitness) {
375 10 : min_fitness = fitness;
376 10 : _param = fit_param;
377 : }
378 :
379 100 : num_iterations++;
380 : }
381 : }
382 :
383 : // calculates residual from samples(corrected using supplied parameter) to gravity
384 : // used to create Fitness column matrix
385 4212 : float AccelCalibrator::calc_residual(const Vector3f& sample, const struct param_t& params) const {
386 : Matrix3f M (
387 4212 : params.diag.x , params.offdiag.x , params.offdiag.y,
388 4212 : params.offdiag.x , params.diag.y , params.offdiag.z,
389 4212 : params.offdiag.y , params.offdiag.z , params.diag.z
390 4212 : );
391 4212 : return GRAVITY_MSS - (M*(sample+params.offset)).length();
392 : }
393 :
394 : // calculated the total mean squared fitness of all the collected samples using parameters
395 : // supplied
396 102 : float AccelCalibrator::calc_mean_squared_residuals(const struct param_t& params) const
397 : {
398 102 : if (_sample_buffer == nullptr || _samples_collected == 0) {
399 0 : return 1.0e30f;
400 : }
401 102 : float sum = 0.0f;
402 714 : for(uint16_t i=0; i < _samples_collected; i++){
403 612 : Vector3f sample;
404 612 : get_sample(i, sample);
405 612 : float resid = calc_residual(sample, params);
406 612 : sum += sq(resid);
407 : }
408 102 : sum /= _samples_collected;
409 102 : return sum;
410 : }
411 :
412 : // calculate jacobian, a matrix that defines relation to variation in fitness with variation in each of the parameters
413 : // this is used in LSq estimator to adjust variation in parameter to be used for next iteration of LSq
414 600 : void AccelCalibrator::calc_jacob(const Vector3f& sample, const struct param_t& params, VectorP &ret) const {
415 600 : switch (_conf_fit_type) {
416 600 : case ACCEL_CAL_AXIS_ALIGNED_ELLIPSOID:
417 : case ACCEL_CAL_ELLIPSOID: {
418 600 : const Vector3f &offset = params.offset;
419 600 : const Vector3f &diag = params.diag;
420 600 : const Vector3f &offdiag = params.offdiag;
421 : Matrix3f M(
422 600 : diag.x , offdiag.x , offdiag.y,
423 600 : offdiag.x , diag.y , offdiag.z,
424 600 : offdiag.y , offdiag.z , diag.z
425 600 : );
426 :
427 600 : float A = (diag.x * (sample.x + offset.x)) + (offdiag.x * (sample.y + offset.y)) + (offdiag.y * (sample.z + offset.z));
428 600 : float B = (offdiag.x * (sample.x + offset.x)) + (diag.y * (sample.y + offset.y)) + (offdiag.z * (sample.z + offset.z));
429 600 : float C = (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) + (diag.z * (sample.z + offset.z));
430 600 : float length = (M*(sample+offset)).length();
431 :
432 : // 0-2: offsets
433 600 : ret[0] = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length);
434 600 : ret[1] = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length);
435 600 : ret[2] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length);
436 : // 3-5: diagonals
437 600 : ret[3] = -1.0f * ((sample.x + offset.x) * A)/length;
438 600 : ret[4] = -1.0f * ((sample.y + offset.y) * B)/length;
439 600 : ret[5] = -1.0f * ((sample.z + offset.z) * C)/length;
440 : // 6-8: off-diagonals
441 600 : ret[6] = -1.0f * (((sample.y + offset.y) * A) + ((sample.x + offset.x) * B))/length;
442 600 : ret[7] = -1.0f * (((sample.z + offset.z) * A) + ((sample.x + offset.x) * C))/length;
443 600 : ret[8] = -1.0f * (((sample.z + offset.z) * B) + ((sample.y + offset.y) * C))/length;
444 600 : return;
445 : }
446 : }
447 : }
448 :
449 : // returns number of parameters are required for selected Fit type
450 59600 : uint8_t AccelCalibrator::get_num_params() const {
451 59600 : switch (_conf_fit_type) {
452 59600 : case ACCEL_CAL_AXIS_ALIGNED_ELLIPSOID:
453 59600 : return 6;
454 0 : case ACCEL_CAL_ELLIPSOID:
455 0 : return 9;
456 0 : default:
457 0 : return 6;
458 : }
459 : }
|
