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 <algos/mag_cal.h>
18 #include <algos/mat.h>
19
20 #include <errno.h>
21 #include <nanohub_math.h>
22 #include <string.h>
23
24 #define MAX_EIGEN_RATIO 25.0f
25 #define MAX_EIGEN_MAG 80.0f // uT
26 #define MIN_EIGEN_MAG 10.0f // uT
27
28 #define MAX_FIT_MAG 80.0f
29 #define MIN_FIT_MAG 10.0f
30
31 #define MIN_BATCH_WINDOW 1000000UL // 1 sec
32 #define MAX_BATCH_WINDOW 15000000UL // 15 sec
33 #define MIN_BATCH_SIZE 25 // samples
34
35 // eigen value magnitude and ratio test
moc_eigen_test(struct MagCal * moc)36 static int moc_eigen_test(struct MagCal *moc)
37 {
38 // covariance matrix
39 struct Mat33 S;
40 S.elem[0][0] = moc->acc_xx - moc->acc_x * moc->acc_x;
41 S.elem[0][1] = S.elem[1][0] = moc->acc_xy - moc->acc_x * moc->acc_y;
42 S.elem[0][2] = S.elem[2][0] = moc->acc_xz - moc->acc_x * moc->acc_z;
43 S.elem[1][1] = moc->acc_yy - moc->acc_y * moc->acc_y;
44 S.elem[1][2] = S.elem[2][1] = moc->acc_yz - moc->acc_y * moc->acc_z;
45 S.elem[2][2] = moc->acc_zz - moc->acc_z * moc->acc_z;
46
47 struct Vec3 eigenvals;
48 struct Mat33 eigenvecs;
49 mat33GetEigenbasis(&S, &eigenvals, &eigenvecs);
50
51 float evmax = (eigenvals.x > eigenvals.y) ? eigenvals.x : eigenvals.y;
52 evmax = (eigenvals.z > evmax) ? eigenvals.z : evmax;
53
54 float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y;
55 evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin;
56
57 float evmag = sqrtf(eigenvals.x + eigenvals.y + eigenvals.z);
58
59 int eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax)
60 && (evmag > MIN_EIGEN_MAG)
61 && (evmag < MAX_EIGEN_MAG);
62
63 return eigen_pass;
64 }
65
66 //Kasa sphere fitting with normal equation
moc_fit(struct MagCal * moc,struct Vec3 * bias,float * radius)67 static int moc_fit(struct MagCal *moc, struct Vec3 *bias, float *radius)
68 {
69 // A * out = b
70 // (4 x 4) (4 x 1) (4 x 1)
71 struct Mat44 A;
72 A.elem[0][0] = moc->acc_xx; A.elem[0][1] = moc->acc_xy;
73 A.elem[0][2] = moc->acc_xz; A.elem[0][3] = moc->acc_x;
74 A.elem[1][0] = moc->acc_xy; A.elem[1][1] = moc->acc_yy;
75 A.elem[1][2] = moc->acc_yz; A.elem[1][3] = moc->acc_y;
76 A.elem[2][0] = moc->acc_xz; A.elem[2][1] = moc->acc_yz;
77 A.elem[2][2] = moc->acc_zz; A.elem[2][3] = moc->acc_z;
78 A.elem[3][0] = moc->acc_x; A.elem[3][1] = moc->acc_y;
79 A.elem[3][2] = moc->acc_z; A.elem[3][3] = 1.0f;
80
81 struct Vec4 b;
82 initVec4(&b, -moc->acc_xw, -moc->acc_yw, -moc->acc_zw, -moc->acc_w);
83
84 struct Size4 pivot;
85 mat44DecomposeLup(&A, &pivot);
86
87 struct Vec4 out;
88 mat44Solve(&A, &out, &b, &pivot);
89
90 // sphere: (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2
91 //
92 // xc = -out[0] / 2, yc = -out[1] / 2, zc = -out[2] / 2
93 // r = sqrt(xc^2 + yc^2 + zc^2 - out[3])
94
95 struct Vec3 v;
96 initVec3(&v, out.x, out.y, out.z);
97 vec3ScalarMul(&v, -0.5f);
98
99 float r = sqrtf(vec3Dot(&v, &v) - out.w);
100
101 initVec3(bias, v.x, v.y, v.z);
102 *radius = r;
103
104 int success = 0;
105 if (r > MIN_FIT_MAG && r < MAX_FIT_MAG) {
106 success = 1;
107 }
108
109 return success;
110 }
111
moc_reset(struct MagCal * moc)112 static void moc_reset(struct MagCal *moc)
113 {
114 moc->acc_x = moc->acc_y = moc->acc_z = moc->acc_w = 0.0f;
115 moc->acc_xx = moc->acc_xy = moc->acc_xz = moc->acc_xw = 0.0f;
116 moc->acc_yy = moc->acc_yz = moc->acc_yw = 0.0f;
117 moc->acc_zz = moc->acc_zw = 0.0f;
118
119 moc->nsamples = 0;
120 moc->start_time = 0;
121 }
122
moc_batch_complete(struct MagCal * moc,uint64_t sample_time_us)123 static int moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us)
124 {
125 int complete = 0;
126
127 if ((sample_time_us - moc->start_time > MIN_BATCH_WINDOW)
128 && (moc->nsamples > MIN_BATCH_SIZE)) {
129
130 complete = 1;
131
132 } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) {
133 // not enough samples collected in MAX_BATCH_WINDOW
134 moc_reset(moc);
135 }
136
137 return complete;
138 }
139
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)140 void initMagCal(struct MagCal *moc,
141 float x_bias, float y_bias, float z_bias,
142 float c00, float c01, float c02,
143 float c10, float c11, float c12,
144 float c20, float c21, float c22)
145 {
146 moc_reset(moc);
147 moc->update_time = 0;
148 moc->radius = 0.0f;
149
150 moc->x_bias = x_bias;
151 moc->y_bias = y_bias;
152 moc->z_bias = z_bias;
153
154 moc->c00 = c00; moc->c01 = c01; moc->c02 = c02;
155 moc->c10 = c10; moc->c11 = c11; moc->c12 = c12;
156 moc->c20 = c20; moc->c21 = c21; moc->c22 = c22;
157 }
158
destroy_mag_cal(struct MagCal * moc)159 void destroy_mag_cal(struct MagCal *moc)
160 {
161 (void)moc;
162 }
163
magCalUpdate(struct MagCal * moc,uint64_t sample_time_us,float x,float y,float z)164 bool magCalUpdate(struct MagCal *moc, uint64_t sample_time_us,
165 float x, float y, float z)
166 {
167 bool new_bias = false;
168
169 // 1. run accumulators
170 float w = x * x + y * y + z * z;
171
172 moc->acc_x += x;
173 moc->acc_y += y;
174 moc->acc_z += z;
175 moc->acc_w += w;
176
177 moc->acc_xx += x * x;
178 moc->acc_xy += x * y;
179 moc->acc_xz += x * z;
180 moc->acc_xw += x * w;
181
182 moc->acc_yy += y * y;
183 moc->acc_yz += y * z;
184 moc->acc_yw += y * w;
185
186 moc->acc_zz += z * z;
187 moc->acc_zw += z * w;
188
189 if (++moc->nsamples == 1) {
190 moc->start_time = sample_time_us;
191 }
192
193 // 2. batch has enough samples?
194 if (moc_batch_complete(moc, sample_time_us)) {
195
196 float inv = 1.0f / moc->nsamples;
197
198 moc->acc_x *= inv;
199 moc->acc_y *= inv;
200 moc->acc_z *= inv;
201 moc->acc_w *= inv;
202
203 moc->acc_xx *= inv;
204 moc->acc_xy *= inv;
205 moc->acc_xz *= inv;
206 moc->acc_xw *= inv;
207
208 moc->acc_yy *= inv;
209 moc->acc_yz *= inv;
210 moc->acc_yw *= inv;
211
212 moc->acc_zz *= inv;
213 moc->acc_zw *= inv;
214
215 // 3. eigen test
216 if (moc_eigen_test(moc)) {
217
218 struct Vec3 bias;
219 float radius;
220
221 // 4. Kasa sphere fitting
222 if (moc_fit(moc, &bias, &radius)) {
223
224 moc->x_bias = bias.x;
225 moc->y_bias = bias.y;
226 moc->z_bias = bias.z;
227
228 moc->radius = radius;
229 moc->update_time = sample_time_us;
230
231 new_bias = true;
232 }
233 }
234
235 // 5. reset for next batch
236 moc_reset(moc);
237 }
238
239 return new_bias;
240 }
241
magCalGetBias(struct MagCal * moc,float * x,float * y,float * z)242 void magCalGetBias(struct MagCal *moc, float *x, float *y, float *z)
243 {
244 *x = moc->x_bias;
245 *y = moc->y_bias;
246 *z = moc->z_bias;
247 }
248
magCalAddBias(struct MagCal * moc,float x,float y,float z)249 void magCalAddBias(struct MagCal *moc, float x, float y, float z)
250 {
251 moc->x_bias += x;
252 moc->y_bias += y;
253 moc->z_bias += z;
254 }
255
magCalRemoveBias(struct MagCal * moc,float xi,float yi,float zi,float * xo,float * yo,float * zo)256 void magCalRemoveBias(struct MagCal *moc, float xi, float yi, float zi,
257 float *xo, float *yo, float *zo)
258 {
259 *xo = xi - moc->x_bias;
260 *yo = yi - moc->y_bias;
261 *zo = zi - moc->z_bias;
262 }
263
magCalSetSoftiron(struct MagCal * moc,float c00,float c01,float c02,float c10,float c11,float c12,float c20,float c21,float c22)264 void magCalSetSoftiron(struct MagCal *moc,
265 float c00, float c01, float c02,
266 float c10, float c11, float c12,
267 float c20, float c21, float c22)
268 {
269 moc->c00 = c00; moc->c01 = c01; moc->c02 = c02;
270 moc->c10 = c10; moc->c11 = c11; moc->c12 = c12;
271 moc->c20 = c20; moc->c21 = c21; moc->c22 = c22;
272 }
273
magCalRemoveSoftiron(struct MagCal * moc,float xi,float yi,float zi,float * xo,float * yo,float * zo)274 void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi,
275 float *xo, float *yo, float *zo)
276 {
277 *xo = moc->c00 * xi + moc->c01 * yi + moc->c02 * zi;
278 *yo = moc->c10 * xi + moc->c11 * yi + moc->c12 * zi;
279 *zo = moc->c20 * xi + moc->c21 * yi + moc->c22 * zi;
280 }
281
282