1 /* 2 * Copyright (C) 2016 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 #include "calibration/magnetometer/mag_cal.h" 18 #include <errno.h> 19 #include <nanohub_math.h> 20 #include <string.h> 21 22 #ifdef MAG_CAL_ORIGINAL_TUNING 23 #define MAX_EIGEN_RATIO 25.0f 24 #define MAX_EIGEN_MAG 80.0f // uT 25 #define MIN_EIGEN_MAG 10.0f // uT 26 #define MAX_FIT_MAG 80.0f 27 #define MIN_FIT_MAG 10.0f 28 #define MIN_BATCH_WINDOW 1000000UL // 1 sec 29 #define MAX_BATCH_WINDOW 15000000UL // 15 sec 30 #define MIN_BATCH_SIZE 25 // samples 31 #else 32 #define MAX_EIGEN_RATIO 15.0f 33 #define MAX_EIGEN_MAG 70.0f // uT 34 #define MIN_EIGEN_MAG 20.0f // uT 35 #define MAX_FIT_MAG 70.0f 36 #define MIN_FIT_MAG 20.0f 37 #define MIN_BATCH_WINDOW 3000000UL // 3 sec 38 #define MAX_BATCH_WINDOW 15000000UL // 15 sec 39 #define MIN_BATCH_SIZE 25 // samples 40 #endif 41 42 #ifdef DIVERSITY_CHECK_ENABLED 43 #define MAX_DISTANCE_VIOLATIONS 2 44 #endif 45 46 // eigen value magnitude and ratio test 47 static int moc_eigen_test(struct KasaFit *kasa) { 48 // covariance matrix 49 struct Mat33 S; 50 S.elem[0][0] = kasa->acc_xx - kasa->acc_x * kasa->acc_x; 51 S.elem[0][1] = S.elem[1][0] = kasa->acc_xy - kasa->acc_x * kasa->acc_y; 52 S.elem[0][2] = S.elem[2][0] = kasa->acc_xz - kasa->acc_x * kasa->acc_z; 53 S.elem[1][1] = kasa->acc_yy - kasa->acc_y * kasa->acc_y; 54 S.elem[1][2] = S.elem[2][1] = kasa->acc_yz - kasa->acc_y * kasa->acc_z; 55 S.elem[2][2] = kasa->acc_zz - kasa->acc_z * kasa->acc_z; 56 57 struct Vec3 eigenvals; 58 struct Mat33 eigenvecs; 59 mat33GetEigenbasis(&S, &eigenvals, &eigenvecs); 60 61 float evmax = (eigenvals.x > eigenvals.y) ? eigenvals.x : eigenvals.y; 62 evmax = (eigenvals.z > evmax) ? eigenvals.z : evmax; 63 64 float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y; 65 evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin; 66 67 float evmag = sqrtf(eigenvals.x + eigenvals.y + eigenvals.z); 68 69 int eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax) && 70 (evmag > MIN_EIGEN_MAG) && (evmag < MAX_EIGEN_MAG); 71 72 return eigen_pass; 73 } 74 75 // Kasa sphere fitting with normal equation 76 int magKasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius) { 77 // A * out = b 78 // (4 x 4) (4 x 1) (4 x 1) 79 struct Mat44 A; 80 A.elem[0][0] = kasa->acc_xx; 81 A.elem[0][1] = kasa->acc_xy; 82 A.elem[0][2] = kasa->acc_xz; 83 A.elem[0][3] = kasa->acc_x; 84 A.elem[1][0] = kasa->acc_xy; 85 A.elem[1][1] = kasa->acc_yy; 86 A.elem[1][2] = kasa->acc_yz; 87 A.elem[1][3] = kasa->acc_y; 88 A.elem[2][0] = kasa->acc_xz; 89 A.elem[2][1] = kasa->acc_yz; 90 A.elem[2][2] = kasa->acc_zz; 91 A.elem[2][3] = kasa->acc_z; 92 A.elem[3][0] = kasa->acc_x; 93 A.elem[3][1] = kasa->acc_y; 94 A.elem[3][2] = kasa->acc_z; 95 A.elem[3][3] = 1.0f; 96 97 struct Vec4 b; 98 initVec4(&b, -kasa->acc_xw, -kasa->acc_yw, -kasa->acc_zw, -kasa->acc_w); 99 100 struct Size4 pivot; 101 mat44DecomposeLup(&A, &pivot); 102 103 struct Vec4 out; 104 mat44Solve(&A, &out, &b, &pivot); 105 106 // sphere: (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2 107 // 108 // xc = -out[0] / 2, yc = -out[1] / 2, zc = -out[2] / 2 109 // r = sqrt(xc^2 + yc^2 + zc^2 - out[3]) 110 111 struct Vec3 v; 112 initVec3(&v, out.x, out.y, out.z); 113 vec3ScalarMul(&v, -0.5f); 114 115 float r = sqrtf(vec3Dot(&v, &v) - out.w); 116 117 initVec3(bias, v.x, v.y, v.z); 118 *radius = r; 119 120 int success = 0; 121 if (r > MIN_FIT_MAG && r < MAX_FIT_MAG) { 122 success = 1; 123 } 124 125 return success; 126 } 127 128 void magKasaReset(struct KasaFit *kasa) { 129 kasa->acc_x = kasa->acc_y = kasa->acc_z = kasa->acc_w = 0.0f; 130 kasa->acc_xx = kasa->acc_xy = kasa->acc_xz = kasa->acc_xw = 0.0f; 131 kasa->acc_yy = kasa->acc_yz = kasa->acc_yw = 0.0f; 132 kasa->acc_zz = kasa->acc_zw = 0.0f; 133 134 kasa->nsamples = 0; 135 } 136 137 void magCalReset(struct MagCal *moc) { 138 magKasaReset(&moc->kasa); 139 #ifdef DIVERSITY_CHECK_ENABLED 140 diversityCheckerReset(&moc->diversity_checker); 141 #endif 142 moc->start_time = 0; 143 } 144 145 static int moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us) { 146 int complete = 0; 147 148 if ((sample_time_us - moc->start_time > MIN_BATCH_WINDOW) && 149 (moc->kasa.nsamples > MIN_BATCH_SIZE)) { 150 complete = 1; 151 152 } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) { 153 // not enough samples collected in MAX_BATCH_WINDOW or too many 154 // maximum distance violations detected. 155 magCalReset(moc); 156 } 157 158 return complete; 159 } 160 161 void initKasa(struct KasaFit *kasa) { 162 magKasaReset(kasa); 163 } 164 165 void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias, 166 float c00, float c01, float c02, float c10, float c11, 167 float c12, float c20, float c21, float c22 168 #ifdef DIVERSITY_CHECK_ENABLED 169 ,size_t min_num_diverse_vectors 170 ,size_t max_num_max_distance 171 ,float var_threshold 172 ,float max_min_threshold 173 ,float local_field 174 ,float threshold_tuning_param 175 ,float max_distance_tuning_param 176 #endif 177 ) { 178 magCalReset(moc); 179 moc->update_time = 0; 180 moc->radius = 0.0f; 181 182 moc->x_bias = x_bias; 183 moc->y_bias = y_bias; 184 moc->z_bias = z_bias; 185 186 moc->c00 = c00; 187 moc->c01 = c01; 188 moc->c02 = c02; 189 moc->c10 = c10; 190 moc->c11 = c11; 191 moc->c12 = c12; 192 moc->c20 = c20; 193 moc->c21 = c21; 194 moc->c22 = c22; 195 196 #ifdef DIVERSITY_CHECK_ENABLED 197 // Diversity Checker Init 198 diversityCheckerInit(&moc->diversity_checker, 199 min_num_diverse_vectors, 200 max_num_max_distance, 201 var_threshold, 202 max_min_threshold, 203 local_field, 204 threshold_tuning_param, 205 max_distance_tuning_param); 206 #endif 207 } 208 209 void magCalDestroy(struct MagCal *moc) { (void)moc; } 210 211 bool magCalUpdate(struct MagCal *moc, uint64_t sample_time_us, float x, float y, 212 float z) { 213 bool new_bias = false; 214 215 #ifdef DIVERSITY_CHECK_ENABLED 216 // Diversity Checker Update. 217 diversityCheckerUpdate(&moc->diversity_checker, x, y, z); 218 #endif 219 220 // 1. run accumulators 221 float w = x * x + y * y + z * z; 222 223 moc->kasa.acc_x += x; 224 moc->kasa.acc_y += y; 225 moc->kasa.acc_z += z; 226 moc->kasa.acc_w += w; 227 228 moc->kasa.acc_xx += x * x; 229 moc->kasa.acc_xy += x * y; 230 moc->kasa.acc_xz += x * z; 231 moc->kasa.acc_xw += x * w; 232 233 moc->kasa.acc_yy += y * y; 234 moc->kasa.acc_yz += y * z; 235 moc->kasa.acc_yw += y * w; 236 237 moc->kasa.acc_zz += z * z; 238 moc->kasa.acc_zw += z * w; 239 240 if (++moc->kasa.nsamples == 1) { 241 moc->start_time = sample_time_us; 242 } 243 244 // 2. batch has enough samples? 245 if (moc_batch_complete(moc, sample_time_us)) { 246 float inv = 1.0f / moc->kasa.nsamples; 247 248 moc->kasa.acc_x *= inv; 249 moc->kasa.acc_y *= inv; 250 moc->kasa.acc_z *= inv; 251 moc->kasa.acc_w *= inv; 252 253 moc->kasa.acc_xx *= inv; 254 moc->kasa.acc_xy *= inv; 255 moc->kasa.acc_xz *= inv; 256 moc->kasa.acc_xw *= inv; 257 258 moc->kasa.acc_yy *= inv; 259 moc->kasa.acc_yz *= inv; 260 moc->kasa.acc_yw *= inv; 261 262 moc->kasa.acc_zz *= inv; 263 moc->kasa.acc_zw *= inv; 264 265 // 3. eigen test 266 if (moc_eigen_test(&moc->kasa)) { 267 struct Vec3 bias; 268 float radius; 269 // 4. Kasa sphere fitting 270 if (magKasaFit(&moc->kasa, &bias, &radius)) { 271 #ifdef DIVERSITY_CHECK_ENABLED 272 diversityCheckerLocalFieldUpdate(&moc->diversity_checker, 273 radius); 274 if (diversityCheckerNormQuality(&moc->diversity_checker, 275 bias.x, 276 bias.y, 277 bias.z) && 278 moc->diversity_checker.num_max_dist_violations 279 <= MAX_DISTANCE_VIOLATIONS) { 280 #endif 281 moc->x_bias = bias.x; 282 moc->y_bias = bias.y; 283 moc->z_bias = bias.z; 284 285 moc->radius = radius; 286 moc->update_time = sample_time_us; 287 288 new_bias = true; 289 #ifdef DIVERSITY_CHECK_ENABLED 290 } 291 #endif 292 } 293 } 294 295 // 5. reset for next batch 296 magCalReset(moc); 297 } 298 299 return new_bias; 300 } 301 302 void magCalGetBias(struct MagCal *moc, float *x, float *y, float *z) { 303 *x = moc->x_bias; 304 *y = moc->y_bias; 305 *z = moc->z_bias; 306 } 307 308 void magCalAddBias(struct MagCal *moc, float x, float y, float z) { 309 moc->x_bias += x; 310 moc->y_bias += y; 311 moc->z_bias += z; 312 } 313 314 void magCalRemoveBias(struct MagCal *moc, float xi, float yi, float zi, 315 float *xo, float *yo, float *zo) { 316 *xo = xi - moc->x_bias; 317 *yo = yi - moc->y_bias; 318 *zo = zi - moc->z_bias; 319 } 320 321 void magCalSetSoftiron(struct MagCal *moc, float c00, float c01, float c02, 322 float c10, float c11, float c12, float c20, float c21, 323 float c22) { 324 moc->c00 = c00; 325 moc->c01 = c01; 326 moc->c02 = c02; 327 moc->c10 = c10; 328 moc->c11 = c11; 329 moc->c12 = c12; 330 moc->c20 = c20; 331 moc->c21 = c21; 332 moc->c22 = c22; 333 } 334 335 void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi, 336 float *xo, float *yo, float *zo) { 337 *xo = moc->c00 * xi + moc->c01 * yi + moc->c02 * zi; 338 *yo = moc->c10 * xi + moc->c11 * yi + moc->c12 * zi; 339 *zo = moc->c20 * xi + moc->c21 * yi + moc->c22 * zi; 340 } 341
