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
moc_eigen_test(struct KasaFit * kasa)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
magKasaFit(struct KasaFit * kasa,struct Vec3 * bias,float * radius)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
magKasaReset(struct KasaFit * kasa)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
magCalReset(struct MagCal * moc)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
moc_batch_complete(struct MagCal * moc,uint64_t sample_time_us)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
initKasa(struct KasaFit * kasa)161 void initKasa(struct KasaFit *kasa) {
162 magKasaReset(kasa);
163 }
164
initMagCal(struct MagCal * moc,float x_bias,float y_bias,float z_bias,float c00,float c01,float c02,float c10,float c11,float c12,float c20,float c21,float c22,size_t min_num_diverse_vectors,size_t max_num_max_distance,float var_threshold,float max_min_threshold,float local_field,float threshold_tuning_param,float max_distance_tuning_param)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
magCalDestroy(struct MagCal * moc)209 void magCalDestroy(struct MagCal *moc) { (void)moc; }
210
magCalUpdate(struct MagCal * moc,uint64_t sample_time_us,float x,float y,float z)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
magCalGetBias(struct MagCal * moc,float * x,float * y,float * z)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
magCalAddBias(struct MagCal * moc,float x,float y,float z)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
magCalRemoveBias(struct MagCal * moc,float xi,float yi,float zi,float * xo,float * yo,float * zo)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
magCalSetSoftiron(struct MagCal * moc,float c00,float c01,float c02,float c10,float c11,float c12,float c20,float c21,float c22)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
magCalRemoveSoftiron(struct MagCal * moc,float xi,float yi,float zi,float * xo,float * yo,float * zo)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