1 /*
2 * Copyright (c) 2021 Marti Riba Pons
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <stdio.h>
8 #include <math.h>
9 #include <errno.h>
10 #include <zsl/orientation/fusion/fusion.h>
11 #include <zsl/orientation/fusion/kalman.h>
12
13 static uint32_t zsl_fus_kalm_freq;
14 static uint32_t zsl_fus_kalm_initialised;
15
zsl_fus_kalman(struct zsl_vec * g,struct zsl_vec * a,struct zsl_vec * m,zsl_real_t * var_g,zsl_real_t * var_a,zsl_real_t * var_m,zsl_real_t * incl,struct zsl_mtx * P,struct zsl_quat * q)16 static int zsl_fus_kalman(struct zsl_vec *g, struct zsl_vec *a,
17 struct zsl_vec *m, zsl_real_t *var_g, zsl_real_t *var_a,
18 zsl_real_t *var_m, zsl_real_t *incl, struct zsl_mtx *P,
19 struct zsl_quat *q)
20 {
21 int rc = 0;
22
23 #if CONFIG_ZSL_BOUNDS_CHECKS
24 if (a == NULL || a->sz != 3 || m == NULL || m->sz != 3 || g == NULL ||
25 g->sz != 3) {
26 rc = -EINVAL;
27 goto err;
28 }
29 #endif
30
31 /* PREDICTION STEP. */
32
33 /* Useful constant to reduce the code. */
34 zsl_real_t if2 = 1.0 / (2.0 * zsl_fus_kalm_freq);
35
36 /* Calculate the matrix F and its transpose. */
37 ZSL_MATRIX_DEF(F, 4, 4);
38 ZSL_MATRIX_DEF(Ft, 4, 4);
39
40 zsl_real_t F_data[16] = {
41 1.0, -if2 * g->data[0], -if2 * g->data[1], -if2 * g->data[2],
42 if2 * g->data[0], 1.0, if2 * g->data[2], -if2 * g->data[1],
43 if2 * g->data[1], -if2 * g->data[2], 1.0, if2 * g->data[0],
44 if2 * g->data[2], if2 * g->data[1], -if2 * g->data[0], 1.0,
45 };
46
47 zsl_mtx_from_arr(&F, F_data);
48 zsl_mtx_trans(&F, &Ft);
49
50 /* Calculate the matrix W and its transpose. */
51 ZSL_MATRIX_DEF(W, 4, 3);
52 ZSL_MATRIX_DEF(Wt, 3, 4);
53
54 zsl_real_t W_data[12] = {
55 -q->i, -q->j, -q->k,
56 q->r, -q->k, q->j,
57 q->k, q->r, -q->i,
58 -q->j, q->i, q->r
59 };
60
61 zsl_mtx_from_arr(&W, W_data);
62 zsl_mtx_trans(&W, &Wt);
63
64 /* Calculate the matrix Q. */
65 ZSL_MATRIX_DEF(Q, 4, 4);
66 zsl_mtx_mult(&W, &Wt, &Q);
67 zsl_mtx_scalar_mult_d(&Q, *var_g * if2 * if2);
68
69 /* Calculate the matrix P. */
70 ZSL_MATRIX_DEF(FP, 4, 4);
71 zsl_mtx_mult(&F, P, &FP);
72 zsl_mtx_mult(&FP, &Ft, P);
73 zsl_mtx_add_d(P, &Q);
74
75 /* Calculate an estimation of the orientation using only the data of the
76 * gyroscope and quaternion integration. */
77 zsl_quat_from_ang_vel(g, q, 1.0 / zsl_fus_kalm_freq, q);
78
79 /* CORRECTION STEP. */
80
81 /* Normalize the magnetometer and accelerometer data. */
82 zsl_vec_to_unit(a);
83 zsl_vec_to_unit(m);
84
85 /* Define the measurement vector 'z'. */
86 ZSL_MATRIX_DEF(z, 6, 1);
87 z.data[0] = a->data[0];
88 z.data[1] = a->data[1];
89 z.data[2] = a->data[2];
90 z.data[3] = m->data[0];
91 z.data[4] = m->data[1];
92 z.data[5] = m->data[2];
93
94 /* Turn the data of the magnetometer into a pure quaterion. */
95 struct zsl_quat qm = {
96 .r = 0.0,
97 .i = m->data[0],
98 .j = m->data[1],
99 .k = m->data[2]
100 };
101
102 /* Define the normalized gravity and magnetic field in the global NED
103 * frame. */
104 struct zsl_quat gv = { .r = 0.0, .i = 0.0, .j = 0.0, .k = -1.0 };
105 struct zsl_quat mg = { .r = 0.0, .i = 0.0, .j = 0.0, .k = 0.0 };
106 if (incl == NULL) {
107 struct zsl_quat h;
108 zsl_quat_rot(q, &qm, &h);
109 mg.i = ZSL_SQRT(h.i * h.i + h.j * h.j);
110 mg.k = h.k;
111 } else {
112 mg.i = ZSL_COS(*incl * ZSL_PI / 180.0);
113 mg.k = ZSL_SIN(*incl * ZSL_PI / 180.0);
114 }
115
116 zsl_quat_to_unit_d(&mg);
117
118 /* Calculate the measurement model, h. To do this, use the predicted
119 * orientation 'q' to rotate the quaternions 'grav' and 'magn'. */
120 ZSL_MATRIX_DEF(h, 6, 1);
121 struct zsl_quat a2;
122 struct zsl_quat m2;
123
124 zsl_quat_rot(q, &gv, &a2);
125 zsl_quat_rot(q, &mg, &m2);
126
127 h.data[0] = 2.0 * a2.i;
128 h.data[1] = 2.0 * a2.j;
129 h.data[2] = 2.0 * a2.k;
130 h.data[3] = 2.0 * m2.i;
131 h.data[4] = 2.0 * m2.j;
132 h.data[5] = 2.0 * m2.k;
133
134 /* Calculate H, which is the Jacobian matrix of h. Calculate its
135 * transpose too. */
136 ZSL_MATRIX_DEF(H, 6, 4);
137 ZSL_MATRIX_DEF(Ht, 4, 6);
138
139 zsl_real_t H_data[24] = {
140 /* Fisrt 3 rows of the H matrix. */
141 q->j, -q->k, q->r, -q->i,
142 -q->i, -q->r, -q->k, -q->j,
143 0.0, 2.0 * q->i, 2.0 * q->j, 0.0,
144
145 /* Fourth row of the H matrix. */
146 -mg.k * q->j, mg.k * q->k, -2.0 * mg.i * q->j - mg.k * q->r,
147 -2.0 * mg.i * q->k + mg.k * q->i,
148
149 /* Fifth row of the H matrix. */
150 -mg.i * q->k + mg.k * q->i, mg.i * q->j + mg.k * q->r,
151 mg.i * q->i + mg.k * q->k, -mg.i * q->r + mg.k * q->j,
152
153 /* Sixth row of the H matrix. */
154 mg.i * q->j, mg.i * q->k - 2.0 * mg.k * q->i,
155 mg.i * q->r - 2.0 * mg.k * q->j, mg.i * q->i
156 };
157
158 zsl_mtx_from_arr(&H, H_data);
159 zsl_mtx_scalar_mult_d(&H, 2.0);
160 zsl_mtx_trans(&H, &Ht);
161
162 /* Define the R matrix with the variance of the accelerometer
163 * and magnetometer. */
164 ZSL_MATRIX_DEF(R, 6, 6);
165 zsl_mtx_init(&R, zsl_mtx_entry_fn_identity);
166 zsl_mtx_set(&R, 0, 0, *var_a);
167 zsl_mtx_set(&R, 1, 1, *var_a);
168 zsl_mtx_set(&R, 2, 2, *var_a);
169 zsl_mtx_set(&R, 3, 3, *var_m);
170 zsl_mtx_set(&R, 4, 4, *var_m);
171 zsl_mtx_set(&R, 5, 5, *var_m);
172
173 /* Define the vector v, the matrix S and the matrix K. */
174 ZSL_MATRIX_DEF(v, 6, 1);
175 ZSL_MATRIX_DEF(S, 6, 6);
176 ZSL_MATRIX_DEF(K, 4, 6);
177
178 /* Calculation of v. */
179 zsl_mtx_sub(&z, &h, &v);
180
181 /* Calculation of S. */
182 ZSL_MATRIX_DEF(HP, 6, 4);
183 ZSL_MATRIX_DEF(HPHt, 6, 6);
184 zsl_mtx_mult(&H, P, &HP);
185 zsl_mtx_mult(&HP, &Ht, &HPHt);
186 zsl_mtx_add(&HPHt, &R, &S);
187
188 /* Calculation of K. */
189 ZSL_MATRIX_DEF(S_inv, 6, 6);
190 ZSL_MATRIX_DEF(PHt, 4, 6);
191 zsl_mtx_inv(&S, &S_inv);
192 zsl_mtx_mult(P, &Ht, &PHt);
193 zsl_mtx_mult(&PHt, &S_inv, &K);
194
195 /* Calculate the corrected matrix P. */
196 ZSL_MATRIX_DEF(idx, 4, 4);
197 ZSL_MATRIX_DEF(KH, 4, 4);
198 zsl_mtx_init(&idx, zsl_mtx_entry_fn_identity);
199 zsl_mtx_mult(&K, &H, &KH);
200 zsl_mtx_sub_d(&idx, &KH);
201 zsl_mtx_mult(&idx, P, P);
202
203 /* Calculate the corrected orientation quaternion q. */
204 ZSL_MATRIX_DEF(Kv, 4, 1);
205 zsl_mtx_mult(&K, &v, &Kv);
206
207 q->r += Kv.data[0];
208 q->i += Kv.data[1];
209 q->j += Kv.data[2];
210 q->k += Kv.data[3];
211
212 /* Normalize the output quaternion. */
213 zsl_quat_to_unit_d(q);
214
215 err:
216 return rc;
217 }
218
zsl_fus_kalm_quat_init(struct zsl_vec * a,struct zsl_vec * m,struct zsl_quat * q)219 static int zsl_fus_kalm_quat_init(struct zsl_vec *a, struct zsl_vec *m,
220 struct zsl_quat *q)
221 {
222 int rc = 0;
223
224 #if CONFIG_ZSL_BOUNDS_CHECKS
225 /* Make sure that the input vectors are tridimensional. */
226 if (a == NULL || a->sz != 3 || m == NULL || m->sz != 3) {
227 rc = -EINVAL;
228 goto err;
229 }
230 #endif
231
232 /* Define the rotation matrix and two helper vectors. */
233 ZSL_MATRIX_DEF(mtx, 3, 3);
234 ZSL_VECTOR_DEF(v, 3);
235 ZSL_VECTOR_DEF(w, 3);
236 ZSL_VECTOR_DEF(u, 3);
237
238 /* Set the first column of the rotation matrix as (a x m) x a (cross
239 * product), the second column as (a x m) and the third one as 'a'. */
240 zsl_vec_cross(a, m, &w);
241 zsl_vec_cross(&w, a, &v);
242 zsl_vec_copy(&u, a);
243
244 zsl_vec_to_unit(&v);
245 zsl_vec_to_unit(&w);
246 zsl_vec_to_unit(&u);
247
248 zsl_mtx_set_col(&mtx, 0, v.data);
249 zsl_mtx_set_col(&mtx, 1, w.data);
250 zsl_mtx_set_col(&mtx, 2, u.data);
251
252 /* The initial quaternion is obtained converting this rotation matrix into
253 * a unit quaternion. */
254 zsl_quat_from_rot_mtx(&mtx, q);
255 zsl_quat_to_unit_d(q);
256
257 /* Inidicate that we have already called this function. */
258 zsl_fus_kalm_initialised = 1;
259
260 err:
261 return rc;
262 }
263
zsl_fus_kalm_P_init(struct zsl_mtx * P)264 static int zsl_fus_kalm_P_init(struct zsl_mtx *P)
265 {
266 int rc = 0;
267
268 #if CONFIG_ZSL_BOUNDS_CHECKS
269 /* Make sure that the input matrix P has dimension 4x4. */
270 if (P->sz_rows != 4 || P->sz_cols != 4) {
271 rc = -EINVAL;
272 goto err;
273 }
274 #endif
275
276 zsl_mtx_init(P, zsl_mtx_entry_fn_identity);
277
278 /* Inidicate that we have already called this function. */
279 zsl_fus_kalm_initialised = 1;
280
281 err:
282 return rc;
283 }
284
zsl_fus_kalm_init(uint32_t freq,void * cfg)285 int zsl_fus_kalm_init(uint32_t freq, void *cfg)
286 {
287 int rc = 0;
288
289 struct zsl_fus_kalm_cfg *mcfg = cfg;
290
291 (void)mcfg;
292
293 #if CONFIG_ZSL_BOUNDS_CHECKS
294 /* Make sure that the sample frequency is positive. */
295 if (freq <= 0.0) {
296 rc = -EINVAL;
297 goto err;
298 }
299 #endif
300
301 zsl_fus_kalm_freq = freq;
302
303 err:
304 return rc;
305 }
306
zsl_fus_kalm_feed(struct zsl_vec * a,struct zsl_vec * m,struct zsl_vec * g,zsl_real_t * incl,struct zsl_quat * q,void * cfg)307 int zsl_fus_kalm_feed(struct zsl_vec *a, struct zsl_vec *m, struct zsl_vec *g,
308 zsl_real_t *incl, struct zsl_quat *q, void *cfg)
309 {
310 struct zsl_fus_kalm_cfg *mcfg = cfg;
311
312 if (mcfg->var_g < 0.0 || mcfg->var_a < 0.0 || mcfg->var_m < 0.0) {
313 return -EINVAL;
314 }
315
316 /* This functions should only be called once. */
317 if (!zsl_fus_kalm_initialised) {
318 zsl_fus_kalm_quat_init(a, m, q);
319 zsl_fus_kalm_P_init(&(mcfg->P));
320 }
321
322 return zsl_fus_kalman(g, a, m, &(mcfg->var_g), &(mcfg->var_a),
323 &(mcfg->var_m), incl, &(mcfg->P), q);
324 }
325
zsl_fus_kalm_error(int error)326 void zsl_fus_kalm_error(int error)
327 {
328 /* ToDo: Log error in default handler. */
329 }
330
