compass-calibration.c

   1 /*
   2  * Copyright (C) 2014 Intel Corporation.
   3  */
   4
   5 #include <math.h>
   6 #include <hardware/sensors.h>
   7 #include <stdio.h>
   8 #include <utils/Log.h>
   9 #include "calibration.h"
  10 #include "matrix-ops.h"
  11 #include "description.h"
  12
  13
  14 /* Compass defines */
  15 #define COMPASS_CALIBRATION_PATH    "/data/compass.conf"
  16 #define EPSILON                     0.000000001
  17
  18 #define MAGNETIC_LOW                960 /* 31 micro tesla squared */
  19 #define CAL_STEPS                   5
  20
  21 /* We'll have multiple calibration levels so that we can provide an estimation as fast as possible */
  22 static const float          min_diffs       [CAL_STEPS] = {0.2,  0.25, 0.4, 0.6, 1.0};
  23 static const float          max_sqr_errs    [CAL_STEPS] = {10.0, 10.0, 8.0, 5.0, 3.5};
  24 static const unsigned int   lookback_counts [CAL_STEPS] = {2,    3,    4,   5,   6  };
  25
  26
  27 /* Reset calibration algorithm */
  28 static void reset_sample (compass_cal_t* data)
  29 {
  30     int i,j;
  31     data->sample_count = 0;
  32     for (i = 0; i < MAGN_DS_SIZE; i++)
  33         for (j=0; j < 3; j++)
  34             data->sample[i][j] = 0;
  35
  36     data->average[0] = data->average[1] = data->average[2] = 0;
  37 }
  38
  39
  40 static double calc_square_err (compass_cal_t* data)
  41 {
  42     double err = 0;
  43     double raw[3][1], result[3][1], mat_diff[3][1];
  44     int i;
  45     float stdev[3] = {0,0,0};
  46     double diff;
  47
  48     for (i = 0; i < MAGN_DS_SIZE; i++) {
  49         raw[0][0] = data->sample[i][0];
  50         raw[1][0] = data->sample[i][1];
  51         raw[2][0] = data->sample[i][2];
  52
  53         stdev[0] += (raw[0][0] - data->average[0]) * (raw[0][0] - data->average[0]);
  54         stdev[1] += (raw[1][0] - data->average[1]) * (raw[1][0] - data->average[1]);
  55         stdev[2] += (raw[2][0] - data->average[2]) * (raw[2][0] - data->average[2]);
  56
  57         substract (3, 1, raw, data->offset, mat_diff);
  58         multiply(3, 3, 1, data->w_invert, mat_diff, result);
  59
  60         diff = sqrt(result[0][0] * result[0][0] + result[1][0] * result[1][0] + result[2][0] * result[2][0]) - data->bfield;
  61
  62         err += diff * diff;
  63     }
  64
  65     stdev[0] = sqrt(stdev[0] / MAGN_DS_SIZE);
  66     stdev[1] = sqrt(stdev[1] / MAGN_DS_SIZE);
  67     stdev[2] = sqrt(stdev[2] / MAGN_DS_SIZE);
  68
  69     /* A sanity check - if we have too little variation for an axis it's best to reject the calibration than risking a wrong calibration */
  70     if (stdev[0] <= 1 || stdev[1] <= 1 || stdev[2] <= 1)
  71         return max_sqr_errs[0];
  72
  73     err /= MAGN_DS_SIZE;
  74     return err;
  75 }
  76
  77
  78 /* Given an real symmetric 3x3 matrix A, compute the eigenvalues */
  79 static void compute_eigenvalues (double mat[3][3], double* eig1, double* eig2, double* eig3)
  80 {
  81     double p = mat[0][1] * mat[0][1] + mat[0][2] * mat[0][2] + mat[1][2] * mat[1][2];
  82
  83     if (p < EPSILON) {
  84         *eig1 = mat[0][0];
  85         *eig2 = mat[1][1];
  86         *eig3 = mat[2][2];
  87         return;
  88     }
  89
  90     double q = (mat[0][0] + mat[1][1] + mat[2][2]) / 3;
  91     double temp1 = mat[0][0] - q;
  92     double temp2 = mat[1][1] - q;
  93     double temp3 = mat[2][2] - q;
  94
  95     p = temp1 * temp1 + temp2 * temp2 + temp3 * temp3 + 2 * p;
  96     p = sqrt(p / 6);
  97
  98     double mat2[3][3];
  99     assign(3, 3, mat, mat2);
 100     mat2[0][0] -= q;
 101     mat2[1][1] -= q;
 102     mat2[2][2] -= q;
 103     multiply_scalar_inplace(3, 3, mat2, 1/p);
 104
 105     double r = (mat2[0][0] * mat2[1][1] * mat2[2][2] + mat2[0][1] * mat2[1][2] * mat2[2][0]
 106         + mat2[0][2] * mat2[1][0] * mat2[2][1] - mat2[0][2] * mat2[1][1] * mat2[2][0]
 107         - mat2[0][0] * mat2[1][2] * mat2[2][1] - mat2[0][1] * mat2[1][0] * mat2[2][2]) / 2;
 108
 109     double phi;
 110     if (r <= -1.0)
 111         phi = M_PI/3;
 112     else if (r >= 1.0)
 113             phi = 0;
 114         else
 115             phi = acos(r) / 3;
 116
 117     *eig3 = q + 2 * p * cos(phi);
 118     *eig1 = q + 2 * p * cos(phi + 2 * M_PI / 3);
 119     *eig2 = 3 * q - *eig1 - *eig3;
 120 }
 121
 122
 123 static void calc_evector (double mat[3][3], double eig, double vec[3][1])
 124 {
 125     double h[3][3];
 126     double x_tmp[2][2];
 127     assign(3, 3, mat, h);
 128     h[0][0] -= eig;
 129     h[1][1] -= eig;
 130     h[2][2] -= eig;
 131
 132     double x[2][2];
 133     x[0][0] = h[1][1];
 134     x[0][1] = h[1][2];
 135     x[1][0] = h[2][1];
 136     x[1][1] = h[2][2];
 137     invert(2, x, x_tmp);
 138     assign(2, 2, x_tmp, x);
 139
 140     double temp1 = x[0][0] * (-h[1][0]) + x[0][1] * (-h[2][0]);
 141     double temp2 = x[1][0] * (-h[1][0]) + x[1][1] * (-h[2][0]);
 142     double norm = sqrt(1 + temp1 * temp1 + temp2 * temp2);
 143
 144     vec[0][0] = 1.0 / norm;
 145     vec[1][0] = temp1 / norm;
 146     vec[2][0] = temp2 / norm;
 147 }
 148
 149
 150 static int ellipsoid_fit (mat_input_t m, double offset[3][1], double w_invert[3][3], double* bfield)
 151 {
 152     int i;
 153     double h[MAGN_DS_SIZE][9];
 154     double w[MAGN_DS_SIZE][1];
 155     double h_trans[9][MAGN_DS_SIZE];
 156     double p_temp1[9][9];
 157     double p_temp2[9][MAGN_DS_SIZE];
 158     double temp1[3][3], temp[3][3];
 159     double temp1_inv[3][3];
 160     double temp2[3][1];
 161     double result[9][9];
 162     double p[9][1];
 163     double a[3][3], sqrt_evals[3][3], evecs[3][3], evecs_trans[3][3];
 164     double evec1[3][1], evec2[3][1], evec3[3][1];
 165
 166     for (i = 0; i < MAGN_DS_SIZE; i++) {
 167         w[i][0] = m[i][0] * m[i][0];
 168         h[i][0] = m[i][0];
 169         h[i][1] = m[i][1];
 170         h[i][2] = m[i][2];
 171         h[i][3] = -1 * m[i][0] * m[i][1];
 172         h[i][4] = -1 * m[i][0] * m[i][2];
 173         h[i][5] = -1 * m[i][1] * m[i][2];
 174         h[i][6] = -1 * m[i][1] * m[i][1];
 175         h[i][7] = -1 * m[i][2] * m[i][2];
 176         h[i][8] = 1;
 177     }
 178
 179     transpose (MAGN_DS_SIZE, 9, h, h_trans);
 180     multiply (9, MAGN_DS_SIZE, 9, h_trans, h, result);
 181     invert (9, result, p_temp1);
 182     multiply (9, 9, MAGN_DS_SIZE, p_temp1, h_trans, p_temp2);
 183     multiply (9, MAGN_DS_SIZE, 1, p_temp2, w, p);
 184
 185     temp1[0][0] = 2;
 186     temp1[0][1] = p[3][0];
 187     temp1[0][2] = p[4][0];
 188     temp1[1][0] = p[3][0];
 189     temp1[1][1] = 2 * p[6][0];
 190     temp1[1][2] = p[5][0];
 191     temp1[2][0] = p[4][0];
 192     temp1[2][1] = p[5][0];
 193     temp1[2][2] = 2 * p[7][0];
 194
 195     temp2[0][0] = p[0][0];
 196     temp2[1][0] = p[1][0];
 197     temp2[2][0] = p[2][0];
 198
 199     invert(3, temp1, temp1_inv);
 200     multiply(3, 3, 1, temp1_inv, temp2, offset);
 201     double off_x = offset[0][0];
 202     double off_y = offset[1][0];
 203     double off_z = offset[2][0];
 204
 205     a[0][0] = 1.0 / (p[8][0] + off_x * off_x + p[6][0] * off_y * off_y
 206             + p[7][0] * off_z * off_z + p[3][0] * off_x * off_y
 207             + p[4][0] * off_x * off_z + p[5][0] * off_y * off_z);
 208
 209     a[0][1] = p[3][0] * a[0][0] / 2;
 210     a[0][2] = p[4][0] * a[0][0] / 2;
 211     a[1][2] = p[5][0] * a[0][0] / 2;
 212     a[1][1] = p[6][0] * a[0][0];
 213     a[2][2] = p[7][0] * a[0][0];
 214     a[2][1] = a[1][2];
 215     a[1][0] = a[0][1];
 216     a[2][0] = a[0][2];
 217
 218     double eig1 = 0, eig2 = 0, eig3 = 0;
 219     compute_eigenvalues(a, &eig1, &eig2, &eig3);
 220
 221     if (eig1 <=0 || eig2 <= 0 || eig3 <= 0)
 222         return 0;
 223
 224     sqrt_evals[0][0] = sqrt(eig1);
 225     sqrt_evals[1][0] = 0;
 226     sqrt_evals[2][0] = 0;
 227     sqrt_evals[0][1] = 0;
 228     sqrt_evals[1][1] = sqrt(eig2);
 229     sqrt_evals[2][1] = 0;
 230     sqrt_evals[0][2] = 0;
 231     sqrt_evals[1][2] = 0;
 232     sqrt_evals[2][2] = sqrt(eig3);
 233
 234     calc_evector(a, eig1, evec1);
 235     calc_evector(a, eig2, evec2);
 236     calc_evector(a, eig3, evec3);
 237
 238     evecs[0][0] = evec1[0][0];
 239     evecs[1][0] = evec1[1][0];
 240     evecs[2][0] = evec1[2][0];
 241     evecs[0][1] = evec2[0][0];
 242     evecs[1][1] = evec2[1][0];
 243     evecs[2][1] = evec2[2][0];
 244     evecs[0][2] = evec3[0][0];
 245     evecs[1][2] = evec3[1][0];
 246     evecs[2][2] = evec3[2][0];
 247
 248     multiply (3, 3, 3, evecs, sqrt_evals, temp1);
 249     transpose(3, 3, evecs, evecs_trans);
 250     multiply (3, 3, 3, temp1, evecs_trans, temp);
 251     transpose (3, 3, temp, w_invert);
 252
 253     *bfield = pow(sqrt(1/eig1) * sqrt(1/eig2) * sqrt(1/eig3), 1.0/3.0);
 254
 255     if (*bfield < 0)
 256         return 0;
 257
 258     multiply_scalar_inplace(3, 3, w_invert, *bfield);
 259
 260     return 1;
 261 }
 262
 263
 264 static void compass_cal_init (FILE* data_file, sensor_info_t* info)
 265 {
 266     compass_cal_t* cal_data = (compass_cal_t*) info->cal_data;
 267     int cal_steps = (info->max_cal_level && info->max_cal_level <= CAL_STEPS) ? info->max_cal_level : CAL_STEPS;
 268     if (cal_data == NULL)
 269         return;
 270
 271     int data_count = 14;
 272     reset_sample(cal_data);
 273
 274     if (!info->cal_level && data_file != NULL) {
 275        int ret = fscanf(data_file, "%d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
 276             &info->cal_level, &cal_data->offset[0][0], &cal_data->offset[1][0], &cal_data->offset[2][0],
 277             &cal_data->w_invert[0][0], &cal_data->w_invert[0][1], &cal_data->w_invert[0][2],
 278             &cal_data->w_invert[1][0], &cal_data->w_invert[1][1], &cal_data->w_invert[1][2],
 279             &cal_data->w_invert[2][0], &cal_data->w_invert[2][1], &cal_data->w_invert[2][2],
 280             &cal_data->bfield);
 281
 282         if (ret != data_count || info->cal_level >= cal_steps)
 283             info->cal_level = 0;
 284     }
 285
 286     if (info->cal_level) {
 287         ALOGV("CompassCalibration: load old data, caldata: %f %f %f %f %f %f %f %f %f %f %f %f %f",
 288             cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0],
 289             cal_data->w_invert[0][0], cal_data->w_invert[0][1], cal_data->w_invert[0][2], cal_data->w_invert[1][0],
 290             cal_data->w_invert[1][1], cal_data->w_invert[1][2], cal_data->w_invert[2][0], cal_data->w_invert[2][1],
 291             cal_data->w_invert[2][2], cal_data->bfield);
 292     } else {
 293         cal_data->offset[0][0] = 0;
 294         cal_data->offset[1][0] = 0;
 295         cal_data->offset[2][0] = 0;
 296
 297         cal_data->w_invert[0][0] = 1;
 298         cal_data->w_invert[1][0] = 0;
 299         cal_data->w_invert[2][0] = 0;
 300         cal_data->w_invert[0][1] = 0;
 301         cal_data->w_invert[1][1] = 1;
 302         cal_data->w_invert[2][1] = 0;
 303         cal_data->w_invert[0][2] = 0;
 304         cal_data->w_invert[1][2] = 0;
 305         cal_data->w_invert[2][2] = 1;
 306
 307         cal_data->bfield = 0;
 308     }
 309 }
 310
 311
 312 static void compass_store_result (FILE* data_file, sensor_info_t* info)
 313 {
 314     compass_cal_t* cal_data = (compass_cal_t*) info->cal_data;
 315
 316     if (data_file == NULL || cal_data == NULL)
 317         return;
 318
 319     int ret = fprintf(data_file, "%d %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
 320         info->cal_level, cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0],
 321         cal_data->w_invert[0][0], cal_data->w_invert[0][1], cal_data->w_invert[0][2],
 322         cal_data->w_invert[1][0], cal_data->w_invert[1][1], cal_data->w_invert[1][2],
 323         cal_data->w_invert[2][0], cal_data->w_invert[2][1], cal_data->w_invert[2][2],
 324         cal_data->bfield);
 325
 326     if (ret < 0)
 327         ALOGE ("Compass calibration - store data failed!");
 328 }
 329
 330
 331 static int compass_collect (sensors_event_t* event, sensor_info_t* info)
 332 {
 333     float data[3] = {event->magnetic.x, event->magnetic.y, event->magnetic.z};
 334     unsigned int index,j;
 335     unsigned int lookback_count;
 336     float min_diff;
 337
 338     compass_cal_t* cal_data = (compass_cal_t*) info->cal_data;
 339
 340     if (cal_data == NULL)
 341         return -1;
 342
 343     /* Discard the point if not valid */
 344     if (data[0] == 0 || data[1] == 0 || data[2] == 0)
 345         return -1;
 346
 347     lookback_count = lookback_counts[info->cal_level];
 348     min_diff = min_diffs[info->cal_level];
 349
 350     /* For the current point to be accepted, each x/y/z value must be different enough to the last several collected points */
 351     if (cal_data->sample_count > 0 && cal_data->sample_count < MAGN_DS_SIZE) {
 352         unsigned int lookback = lookback_count < cal_data->sample_count ? lookback_count : cal_data->sample_count;
 353         for (index = 0; index < lookback; index++)
 354             for (j = 0; j < 3; j++)
 355                 if (fabsf(data[j] - cal_data->sample[cal_data->sample_count-1-index][j]) < min_diff) {
 356                     ALOGV("CompassCalibration:point reject: [%f,%f,%f], selected_count=%d", data[0], data[1], data[2], cal_data->sample_count);
 357                     return 0;
 358                 }
 359     }
 360
 361     if (cal_data->sample_count < MAGN_DS_SIZE) {
 362         memcpy(cal_data->sample[cal_data->sample_count], data, sizeof(float) * 3);
 363         cal_data->sample_count++;
 364         cal_data->average[0] += data[0];
 365         cal_data->average[1] += data[1];
 366         cal_data->average[2] += data[2];
 367         ALOGV("CompassCalibration:point collected [%f,%f,%f], selected_count=%d", (double)data[0], (double)data[1], (double)data[2], cal_data->sample_count);
 368     }
 369     return 1;
 370 }
 371
 372
 373 static void scale_event (sensors_event_t* event)
 374 {
 375     float sqr_norm = 0;
 376     float sanity_norm = 0;
 377     float scale = 1;
 378
 379     sqr_norm = (event->magnetic.x * event->magnetic.x +
 380                 event->magnetic.y * event->magnetic.y +
 381                 event->magnetic.z * event->magnetic.z);
 382
 383     if (sqr_norm < MAGNETIC_LOW)
 384         sanity_norm = MAGNETIC_LOW;
 385
 386     if (sanity_norm && sqr_norm) {
 387         scale = sanity_norm / sqr_norm;
 388         scale = sqrt(scale);
 389         event->magnetic.x = event->magnetic.x * scale;
 390         event->magnetic.y = event->magnetic.y * scale;
 391         event->magnetic.z = event->magnetic.z * scale;
 392     }
 393 }
 394
 395
 396 static void compass_compute_cal (sensors_event_t* event, sensor_info_t* info)
 397 {
 398     compass_cal_t* cal_data = (compass_cal_t*) info->cal_data;
 399     double result[3][1], raw[3][1], diff[3][1];
 400
 401     if (!info->cal_level || cal_data == NULL)
 402         return;
 403
 404     raw[0][0] = event->magnetic.x;
 405     raw[1][0] = event->magnetic.y;
 406     raw[2][0] = event->magnetic.z;
 407
 408     substract(3, 1, raw, cal_data->offset, diff);
 409     multiply (3, 3, 1, cal_data->w_invert, diff, result);
 410
 411     event->magnetic.x = event->data[0] = result[0][0];
 412     event->magnetic.y = event->data[1] = result[1][0];
 413     event->magnetic.z = event->data[2] = result[2][0];
 414
 415     scale_event(event);
 416 }
 417
 418
 419 static int compass_ready (sensor_info_t* info)
 420 {
 421     mat_input_t mat;
 422     int i;
 423     float max_sqr_err;
 424
 425     compass_cal_t* cal_data = (compass_cal_t*) info->cal_data;
 426     compass_cal_t new_cal_data;
 427
 428     /*
 429      * Some sensors take unrealistically long to calibrate at higher levels. We'll use a max_cal_level if we have such a property setup,
 430      * or go with the default settings if not.
 431      */
 432     int cal_steps = (info->max_cal_level && info->max_cal_level <= CAL_STEPS) ? info->max_cal_level : CAL_STEPS;
 433
 434     if (cal_data->sample_count < MAGN_DS_SIZE)
 435         return info->cal_level;
 436
 437     max_sqr_err = max_sqr_errs[info->cal_level];
 438
 439     /* Enough points have been collected, do the ellipsoid calibration */
 440
 441     /* Compute average per axis */
 442     cal_data->average[0] /= MAGN_DS_SIZE;
 443     cal_data->average[1] /= MAGN_DS_SIZE;
 444     cal_data->average[2] /= MAGN_DS_SIZE;
 445
 446     for (i = 0; i < MAGN_DS_SIZE; i++) {
 447        mat[i][0] = cal_data->sample[i][0];
 448        mat[i][1] = cal_data->sample[i][1];
 449        mat[i][2] = cal_data->sample[i][2];
 450     }
 451
 452     /* Check if result is good. The sample data must remain the same */
 453     new_cal_data = *cal_data;
 454
 455     if (ellipsoid_fit(mat, new_cal_data.offset, new_cal_data.w_invert, &new_cal_data.bfield)) {
 456         double new_err = calc_square_err (&new_cal_data);
 457         ALOGI("new err is %f, max sqr err id %f", new_err,max_sqr_err);
 458         if (new_err < max_sqr_err) {
 459             double err = calc_square_err(cal_data);
 460             if (new_err < err) {
 461                 /* New cal data is better, so we switch to the new */
 462                 memcpy(cal_data->offset, new_cal_data.offset, sizeof(cal_data->offset));
 463                 memcpy(cal_data->w_invert, new_cal_data.w_invert, sizeof(cal_data->w_invert));
 464                 cal_data->bfield = new_cal_data.bfield;
 465                 if (info->cal_level < (cal_steps - 1))
 466                     info->cal_level++;
 467                 ALOGV("CompassCalibration: ready check success, caldata: %f %f %f %f %f %f %f %f %f %f %f %f %f, err %f",
 468                     cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0], cal_data->w_invert[0][0],
 469                     cal_data->w_invert[0][1], cal_data->w_invert[0][2], cal_data->w_invert[1][0], cal_data->w_invert[1][1],
 470                     cal_data->w_invert[1][2], cal_data->w_invert[2][0], cal_data->w_invert[2][1], cal_data->w_invert[2][2],
 471                     cal_data->bfield, new_err);
 472             }
 473         }
 474     }
 475     reset_sample(cal_data);
 476     return info->cal_level;
 477 }
 478
 479
 480 void calibrate_compass (sensors_event_t* event, sensor_info_t* info)
 481 {
 482     int cal_level;
 483
 484     /* Calibration is continuous */
 485     compass_collect (event, info);
 486
 487     cal_level = compass_ready(info);
 488
 489     switch (cal_level) {
 490         case 0:
 491             scale_event(event);
 492             event->magnetic.status = SENSOR_STATUS_UNRELIABLE;
 493             break;
 494
 495         case 1:
 496             compass_compute_cal (event, info);
 497             event->magnetic.status = SENSOR_STATUS_ACCURACY_LOW;
 498             break;
 499
 500         case 2:
 501             compass_compute_cal (event, info);
 502             event->magnetic.status = SENSOR_STATUS_ACCURACY_MEDIUM;
 503             break;
 504
 505         default:
 506             compass_compute_cal (event, info);
 507             event->magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;
 508             break;
 509     }
 510 }
 511
 512 void compass_read_data (sensor_info_t* info)
 513 {
 514     FILE* data_file = fopen (COMPASS_CALIBRATION_PATH, "r");
 515
 516     compass_cal_init(data_file, info);
 517
 518     if (data_file)
 519         fclose(data_file);
 520 }
 521
 522
 523 void compass_store_data (sensor_info_t* info)
 524 {
 525     FILE* data_file = fopen (COMPASS_CALIBRATION_PATH, "w");
 526
 527     compass_store_result(data_file, info);
 528
 529     if (data_file)
 530         fclose(data_file);
 531 }