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/calibration.h>
12 #include <zsl/statistics.h>
13
14 #ifndef CONFIG_ZSL_SINGLE_PRECISION
zsl_fus_cal_magn_f_shp(struct zsl_vec * H,struct zsl_vec * b)15 static zsl_real_t zsl_fus_cal_magn_f_shp(struct zsl_vec *H, struct zsl_vec *b)
16 {
17 ZSL_VECTOR_DEF(Hb, 3);
18 zsl_vec_add(H, b, &Hb);
19
20 return zsl_vec_norm(&Hb);
21 }
22 #endif
23
24 #ifndef CONFIG_ZSL_SINGLE_PRECISION
zsl_fus_cal_magn_f_elli(struct zsl_vec * H,struct zsl_mtx * g)25 static zsl_real_t zsl_fus_cal_magn_f_elli(struct zsl_vec *H, struct zsl_mtx *g)
26 {
27 ZSL_MATRIX_DEF(K, 3, 3);
28 K.data[0] = g->data[0];
29 K.data[1] = K.data[3] = g->data[1];
30 K.data[2] = K.data[6] = g->data[2];
31 K.data[4] = g->data[3];
32 K.data[5] = K.data[7] = g->data[4];
33 K.data[8] = g->data[5];
34
35 ZSL_MATRIX_DEF(Hb, 3, 1);
36 Hb.data[0] = H->data[0] + g->data[6];
37 Hb.data[1] = H->data[1] + g->data[7];
38 Hb.data[2] = H->data[2] + g->data[8];
39
40 ZSL_MATRIX_DEF(KHb, 3, 1);
41 zsl_mtx_mult(&K, &Hb, &KHb);
42
43 ZSL_VECTOR_DEF(Hd, 3);
44 zsl_vec_from_arr(&Hd, KHb.data);
45
46 return zsl_vec_norm(&Hd);
47 }
48 #endif
49
zsl_fus_cal_rot_mtx(struct zsl_vec * v,struct zsl_mtx * m,struct zsl_vec * v_rot)50 int zsl_fus_cal_rot_mtx(struct zsl_vec *v, struct zsl_mtx *m,
51 struct zsl_vec *v_rot)
52 {
53 int rc = 0;
54
55 #if CONFIG_ZSL_BOUNDS_CHECKS
56 if (v->sz != 3 || v_rot->sz != 3 || m->sz_rows != 3 || m->sz_cols != 3) {
57 rc = -EINVAL;
58 goto err;
59 }
60 #endif
61
62 ZSL_MATRIX_DEF(vm, 3, 1);
63 zsl_mtx_from_arr(&vm, v->data);
64
65 ZSL_MATRIX_DEF(mv, 3, 1);
66 zsl_mtx_mult(m, &vm, &mv);
67
68 zsl_vec_from_arr(v_rot, mv.data);
69
70 err:
71 return rc;
72 }
73
zsl_fus_cal_rot_axis_angle(struct zsl_vec * v,struct zsl_vec * a,zsl_real_t * b,struct zsl_vec * v_rot)74 int zsl_fus_cal_rot_axis_angle(struct zsl_vec *v, struct zsl_vec *a,
75 zsl_real_t *b, struct zsl_vec *v_rot)
76 {
77 int rc = 0;
78
79 #if CONFIG_ZSL_BOUNDS_CHECKS
80 if (v->sz != 3 || v_rot->sz != 3 || a->sz != 3) {
81 rc = -EINVAL;
82 goto err;
83 }
84 #endif
85
86 struct zsl_quat q;
87 ZSL_MATRIX_DEF(m, 3, 3);
88 zsl_quat_from_axis_angle(a, b, &q);
89 zsl_quat_to_rot_mtx(&q, &m);
90
91 zsl_fus_cal_rot_mtx(v, &m, v_rot);
92
93 err:
94 return rc;
95 }
96
97 #ifndef CONFIG_ZSL_SINGLE_PRECISION
zsl_fus_cal_magn(struct zsl_mtx * m,zsl_real_t * l,zsl_real_t * mu,struct zsl_mtx * K,struct zsl_vec * b)98 int zsl_fus_cal_magn(struct zsl_mtx *m, zsl_real_t *l, zsl_real_t *mu,
99 struct zsl_mtx *K, struct zsl_vec *b)
100 {
101 #if CONFIG_ZSL_BOUNDS_CHECKS
102 /* Make sure that the input magnetometer data matrix has dimension N x 3.
103 * 'K' must be a 3 x 3 matrix and 'b' a tridimensional vector. The
104 * 'mu' parameter must be greater than one. */
105 if (m->sz_cols != 3 || K->sz_rows != 3 || K->sz_cols != 3 ||
106 b->sz != 3 || *mu < 1.0) {
107 return -EINVAL;
108 }
109 #endif
110
111 /* Make a copy of l. */
112 zsl_real_t l_copy = *l;
113
114
115 /* SPHERE FITTING. */
116
117 /* Calculate the initial value of beta (b) as the mean value of (x, y, z)
118 * max and (x, y, z) min. */
119 ZSL_VECTOR_DEF(v, m->sz_rows);
120 ZSL_VECTOR_DEF(w, m->sz_rows);
121
122 zsl_real_t mean = 0.0;
123
124 for (size_t i = 0; i < 3; i++) {
125 zsl_mtx_get_col(m, i, v.data);
126 zsl_vec_sort(&v, &w);
127 b->data[i] = -(w.data[m->sz_rows - 1] + w.data[0]) / 2.0;
128 mean += (w.data[m->sz_rows - 1] - w.data[0]) / 2.0;
129 }
130
131 /* R is a first estimation of the radius for the sphere fitting. */
132 zsl_real_t R = mean / 3.0;
133
134 /* Define needed variables. */
135 ZSL_VECTOR_DEF(H, 3);
136 ZSL_MATRIX_DEF(J, 1, 4);
137 ZSL_MATRIX_DEF(Jt, 4, 1);
138 ZSL_MATRIX_DEF(JtJ, 4, 4);
139 ZSL_MATRIX_DEF(idx, 4, 4);
140 ZSL_MATRIX_DEF(t, 4, 1);
141 zsl_real_t f, S = 0.0, S2;
142
143 /* Initialize the identity matrix. */
144 zsl_mtx_init(&idx, zsl_mtx_entry_fn_identity);
145
146 for (size_t j = 0; j < m->sz_rows; j++) {
147
148 /* Calculate the Jacobian matrix J. */
149 zsl_mtx_init(&J, NULL);
150 for (size_t i = 0; i < m->sz_rows; i++) {
151 zsl_mtx_get_row(m, i, H.data);
152 f = zsl_fus_cal_magn_f_shp(&H, b);
153 J.data[0] += 1.0;
154 J.data[1] += -(H.data[0] - b->data[0]) / f;
155 J.data[2] += -(H.data[1] - b->data[1]) / f;
156 J.data[3] += -(H.data[2] - b->data[2]) / f;
157 }
158
159 /* Compute the transpose of J. */
160 zsl_mtx_trans(&J, &Jt);
161
162 /* Calculate the value of tau ('t'). */
163 zsl_mtx_get_row(m, j, H.data);
164 f = zsl_fus_cal_magn_f_shp(&H, b);
165 zsl_mtx_mult(&Jt, &J, &JtJ);
166 idx.data[0] = *l * JtJ.data[0];
167 idx.data[5] = *l * JtJ.data[5];
168 idx.data[10] = *l * JtJ.data[10];
169 idx.data[15] = *l * JtJ.data[15];
170 zsl_mtx_add_d(&JtJ, &idx);
171 zsl_mtx_inv(&JtJ, &idx);
172 zsl_mtx_mult(&idx, &Jt, &t);
173 zsl_mtx_scalar_mult_d(&t, -(R - f));
174
175 /* Calculate the new pair (R, b) by adding tau to the old values of R
176 * and b. */
177 R += t.data[0];
178 b->data[0] += t.data[1];
179 b->data[1] += t.data[2];
180 b->data[2] += t.data[3];
181
182 if (j < (m->sz_rows - 1)) {
183
184 /* Calculate the squared residual sum of all samples. */
185 zsl_mtx_get_row(m, j + 1, H.data);
186 f = zsl_fus_cal_magn_f_shp(&H, b);
187 S2 = (S * j + (R - f) * (R - f)) / (zsl_real_t) (j + 1);
188
189 /* Update the value of lambda (l) with the information from the
190 * squared residual sum. */
191 if (S2 < S) {
192 *l /= *mu;
193 } else {
194 *l *= *mu;
195 }
196
197 S = S2;
198 }
199 }
200
201 /* ELLIPSOID FITTING. */
202
203 /* Initialize the matrix K as an identity matrix. */
204 zsl_mtx_init(K, zsl_mtx_entry_fn_identity);
205
206 /* Reset the value of lambda ('l') and the value of the sum (S). */
207 *l = l_copy;
208 S = 0.0;
209
210 /* Define gamma (g) and initialize it with the calculated values of
211 * beta in the sphere fitting. */
212 ZSL_MATRIX_DEF(g, 9, 1);
213 g.data[0] = 1.0;
214 g.data[1] = 0.0;
215 g.data[2] = 0.0;
216 g.data[3] = 1.0;
217 g.data[4] = 0.0;
218 g.data[5] = 1.0;
219 g.data[6] = b->data[0];
220 g.data[7] = b->data[1];
221 g.data[8] = b->data[2];
222
223 /* Define needed variables. */
224 ZSL_MATRIX_DEF(N, 1, 9);
225 ZSL_MATRIX_DEF(Nt, 9, 1);
226 ZSL_MATRIX_DEF(NtN, 9, 9);
227 ZSL_MATRIX_DEF(idxN, 9, 9);
228 ZSL_MATRIX_DEF(tN, 9, 1);
229 zsl_real_t A, B, C;
230
231 /* Initialize the identity matrix. */
232 zsl_mtx_init(&idxN, zsl_mtx_entry_fn_identity);
233
234 /* Start the ellipsoid fitting algorithm. */
235 for (size_t j = 0; j < m->sz_rows; j++) {
236 zsl_mtx_init(&N, NULL);
237
238 for (size_t i = 0; i < m->sz_rows; i++) {
239 zsl_mtx_get_row(m, i, H.data);
240 f = zsl_fus_cal_magn_f_elli(&H, &g);
241
242 /* Define variables for repeated calculations. */
243 A = g.data[0] * (H.data[0] + b->data[0]) +
244 g.data[1] * (H.data[1] + b->data[1]) +
245 g.data[2] * (H.data[2] + b->data[2]);
246 B = g.data[1] * (H.data[0] + b->data[0]) +
247 g.data[3] * (H.data[1] + b->data[1]) +
248 g.data[4] * (H.data[2] + b->data[2]);
249 C = g.data[2] * (H.data[0] + b->data[0]) +
250 g.data[4] * (H.data[1] + b->data[1]) +
251 g.data[5] * (H.data[2] + b->data[2]);
252
253 /* Calculate the Jacobian matrix N. */
254 N.data[0] += -(H.data[0] + b->data[0]) * A / f;
255 N.data[1] += -(H.data[1] + b->data[1]) * B / f;
256 N.data[2] += -(H.data[2] + b->data[2]) * C / f;
257 N.data[3] += -((H.data[1] + b->data[1]) * A +
258 (H.data[0] + b->data[0]) * B) / f;
259 N.data[4] += -((H.data[2] + b->data[2]) * A +
260 (H.data[0] + b->data[0]) * C) / f;
261 N.data[5] += -((H.data[2] + b->data[2]) * B +
262 (H.data[1] + b->data[1]) * C) / f;
263 N.data[6] += -A / f;
264 N.data[7] += -B / f;
265 N.data[8] += -C / f;
266 }
267
268 /* Calculate the transpose of N. */
269 zsl_mtx_trans(&N, &Nt);
270
271 /* Calculate the value of tau (tN). */
272 zsl_mtx_get_row(m, j, H.data);
273 f = zsl_fus_cal_magn_f_elli(&H, &g);
274 zsl_mtx_mult(&Nt, &N, &NtN);
275 idxN.data[0] = *l * NtN.data[0];
276 idxN.data[10] = *l * NtN.data[10];
277 idxN.data[20] = *l * NtN.data[20];
278 idxN.data[30] = *l * NtN.data[30];
279 idxN.data[40] = *l * NtN.data[40];
280 idxN.data[50] = *l * NtN.data[50];
281 idxN.data[60] = *l * NtN.data[60];
282 idxN.data[70] = *l * NtN.data[70];
283 idxN.data[80] = *l * NtN.data[80];
284 zsl_mtx_add_d(&NtN, &idxN);
285 zsl_mtx_inv(&NtN, &idxN);
286 zsl_mtx_mult(&idxN, &Nt, &tN);
287 zsl_mtx_scalar_mult_d(&tN, -(R - f));
288
289 /* Calculate the new vector gamma. */
290 g.data[0] += tN.data[0];
291 g.data[1] += tN.data[1];
292 g.data[2] += tN.data[2];
293 g.data[3] += tN.data[3];
294 g.data[4] += tN.data[4];
295 g.data[5] += tN.data[5];
296 g.data[6] += tN.data[6];
297 g.data[7] += tN.data[7];
298 g.data[8] += tN.data[8];
299
300 if (j < (m->sz_rows - 1)) {
301
302 /* Calculate the squared residual sum of all samples. */
303 zsl_mtx_get_row(m, j + 1, H.data);
304 f = zsl_fus_cal_magn_f_elli(&H, &g);
305 S2 = (S * j + (R - f) * (R - f)) / (zsl_real_t) (j + 1);
306
307 /* Update the value of lambda (l) with the information from the
308 * squared residual sum. */
309 if (S2 < S) {
310 *l /= *mu;
311 } else {
312 *l *= *mu;
313 }
314
315 S = S2;
316 }
317 }
318
319 /* Calculate the matrix K. */
320 K->data[0] = g.data[0];
321 K->data[1] = K->data[3] = g.data[1];
322 K->data[2] = K->data[6] = g.data[2];
323 K->data[4] = g.data[3];
324 K->data[5] = K->data[7] = g.data[4];
325 K->data[8] = g.data[5];
326
327 /* Calculate the vector beta ('b'). */
328 b->data[0] = g.data[6];
329 b->data[1] = g.data[7];
330 b->data[2] = g.data[8];
331
332 return 0;
333 }
334 #endif
335
336 #ifndef CONFIG_ZSL_SINGLE_PRECISION
zsl_fus_cal_magn_fast(struct zsl_mtx * m,zsl_real_t * me,struct zsl_mtx * K,struct zsl_vec * b)337 int zsl_fus_cal_magn_fast(struct zsl_mtx *m, zsl_real_t *me, struct zsl_mtx *K,
338 struct zsl_vec *b)
339 {
340
341 zsl_real_t approx_me = 50.0;
342
343 /* Use an approximation for me if no value provided. */
344 if (me == NULL) {
345 /* Locations vary fromo 25-65 uT, but most are close to 50 uT. */
346 me = &approx_me;
347 }
348
349 #if CONFIG_ZSL_BOUNDS_CHECKS
350 /* Make sure that the input magnetometer data matrix has dimension N x 3.
351 * 'K' must be a 3 x 3 matrix and 'b' a tridimensional vector. Also 'me'
352 * should be positive. */
353 if (m->sz_cols != 3 || K->sz_rows != 3 || K->sz_cols != 3 ||
354 b->sz != 3 || *me < 0.0) {
355 return -EINVAL;
356 }
357 #endif
358
359 ZSL_VECTOR_DEF(coef, 9);
360 zsl_sta_quad_fit(m, &coef);
361
362 ZSL_MATRIX_DEF(A, 3, 3);
363 ZSL_MATRIX_DEF(v, 3, 1);
364 A.data[0] = coef.data[0];
365 A.data[1] = A.data[3] = coef.data[3];
366 A.data[2] = A.data[6] = coef.data[4];
367 A.data[4] = coef.data[1];
368 A.data[5] = A.data[7] = coef.data[5];
369 A.data[8] = coef.data[2];
370
371 v.data[0] = coef.data[6];
372 v.data[1] = coef.data[7];
373 v.data[2] = coef.data[8];
374
375 ZSL_MATRIX_DEF(X0, 3, 1);
376 ZSL_MATRIX_DEF(L, 3, 3);
377 ZSL_MATRIX_DEF(G, 3, 3);
378 ZSL_MATRIX_DEF(Ai, 3, 3);
379
380 zsl_mtx_cholesky(&A, &L);
381
382 zsl_mtx_trans(&L, &G);
383 zsl_mtx_scalar_mult_d(&G, *me);
384 zsl_mtx_copy(K, &G);
385
386 zsl_mtx_inv(&A, &Ai);
387 zsl_mtx_mult(&Ai, &v, &X0);
388
389 b->data[0] = X0.data[0];
390 b->data[1] = X0.data[1];
391 b->data[2] = X0.data[2];
392
393 return 0;
394 }
395 #endif
396
zsl_fus_cal_corr_scalar(zsl_real_t * d,zsl_real_t * k,zsl_real_t * b,zsl_real_t * d_out)397 int zsl_fus_cal_corr_scalar(zsl_real_t *d, zsl_real_t *k, zsl_real_t *b,
398 zsl_real_t *d_out)
399 {
400 int rc = 0;
401
402 *d_out = *k * (*d + *b);
403
404 return rc;
405 }
406
zsl_fus_cal_corr_vec(struct zsl_vec * v,struct zsl_mtx * K,struct zsl_vec * b,struct zsl_vec * v_out)407 int zsl_fus_cal_corr_vec(struct zsl_vec *v, struct zsl_mtx *K,
408 struct zsl_vec *b, struct zsl_vec *v_out)
409 {
410 #if CONFIG_ZSL_BOUNDS_CHECKS
411 /* Make sure that the input vectors and matrices have dimension the
412 * same dimensions. */
413 if (v->sz != v_out->sz || v->sz != K->sz_rows || v->sz != K->sz_cols ||
414 v->sz != b->sz) {
415 return -EINVAL;
416 }
417 #endif
418
419 ZSL_VECTOR_DEF(vb, v->sz);
420 ZSL_MATRIX_DEF(VB, v->sz, 1);
421 ZSL_MATRIX_DEF(KVB, v->sz, 1);
422
423 zsl_vec_add(v, b, &vb);
424 zsl_mtx_from_arr(&VB, vb.data);
425 zsl_mtx_mult(K, &VB, &KVB);
426 zsl_vec_from_arr(v_out, KVB.data);
427
428 return 0;
429 }
430
zsl_fus_cal_madg(struct zsl_mtx * g,struct zsl_mtx * a,struct zsl_mtx * m,zsl_real_t sampleFreq,zsl_real_t * incl,zsl_real_t * beta)431 int zsl_fus_cal_madg(struct zsl_mtx *g, struct zsl_mtx *a,
432 struct zsl_mtx *m, zsl_real_t sampleFreq, zsl_real_t *incl,
433 zsl_real_t *beta)
434 {
435 int rc = 0;
436
437 #if CONFIG_ZSL_BOUNDS_CHECKS
438 /* Make sure that the input vectors have dimension 3, and that the
439 * frequency is positive. */
440 if (g->sz_cols != 3 || a->sz_cols != 3 || m->sz_cols != 3 ||
441 sampleFreq < 0.0 || g->sz_rows != a->sz_rows ||
442 g->sz_rows != m->sz_rows) {
443 rc = -EINVAL;
444 goto err;
445 }
446 #endif
447
448 struct zsl_fus_madg_cfg madg_cfg;
449
450 struct zsl_fus_drv madg_drv = {
451 .init_handler = zsl_fus_madg_init,
452 .feed_handler = zsl_fus_madg_feed,
453 .error_handler = zsl_fus_madg_error,
454 .config = &madg_cfg,
455 };
456
457 struct zsl_quat q;
458 struct zsl_euler e = { 0 };
459 struct zsl_attitude att;
460
461 ZSL_VECTOR_DEF(gv, 3);
462 ZSL_VECTOR_DEF(av, 3);
463 ZSL_VECTOR_DEF(mv, 3);
464
465 zsl_real_t Sx, Sy, St, Smin;
466
467 madg_drv.init_handler(sampleFreq, madg_drv.config);
468
469 for (size_t i = 0; i < 1000; i++) {
470 zsl_quat_init(&q, ZSL_QUAT_TYPE_IDENTITY);
471 madg_cfg.beta = (zsl_real_t) i / 1000.0;
472 Sx = 0.0;
473 Sy = 0.0;
474
475 for (size_t j = 0; j < a->sz_rows; j++) {
476 zsl_mtx_get_row(g, j, gv.data);
477 zsl_mtx_get_row(a, j, av.data);
478 zsl_mtx_get_row(m, j, mv.data);
479
480 madg_drv.feed_handler(&av, &mv, &gv, incl, &q, madg_drv.config);
481
482 zsl_quat_to_euler(&q, &e);
483 e.x *= 180. / ZSL_PI;
484 e.y *= 180. / ZSL_PI;
485 e.z *= 180. / ZSL_PI;
486
487 zsl_att_from_accel(&av, &att);
488
489 Sx += (e.x - att.roll) * (e.x - att.roll);
490 Sy += (e.y - att.pitch) * (e.y - att.pitch);
491 }
492 Sx = ZSL_SQRT(Sx / a->sz_rows);
493 Sy = ZSL_SQRT(Sy / a->sz_rows);
494
495 St = (Sx + Sy) / 2.0;
496
497 Smin = 0;
498 if (i == 0) {
499 Smin = St;
500 *beta = madg_cfg.beta;
501 } else if (St < Smin) {
502 Smin = St;
503 *beta = madg_cfg.beta;
504 }
505 }
506
507 err:
508 return rc;
509 }
510
zsl_fus_cal_mahn(struct zsl_mtx * g,struct zsl_mtx * a,struct zsl_mtx * m,zsl_real_t sampleFreq,zsl_real_t * incl,zsl_real_t * kp)511 int zsl_fus_cal_mahn(struct zsl_mtx *g, struct zsl_mtx *a,
512 struct zsl_mtx *m, zsl_real_t sampleFreq, zsl_real_t *incl,
513 zsl_real_t *kp)
514 {
515 int rc = 0;
516
517 #if CONFIG_ZSL_BOUNDS_CHECKS
518 /* Make sure that the input vectors have dimension 3, and that the
519 * frequency is positive. */
520 if (g->sz_cols != 3 || a->sz_cols != 3 || m->sz_cols != 3 ||
521 sampleFreq < 0.0 || g->sz_rows != a->sz_rows ||
522 g->sz_rows != m->sz_rows) {
523 rc = -EINVAL;
524 goto err;
525 }
526 #endif
527
528 zsl_real_t _mahn_intfb[3] = { 0.0, 0.0, 0.0 };
529 struct zsl_fus_mahn_cfg mahn_cfg = {
530 .ki = 0.02,
531 .intfb = {
532 .sz = 3,
533 .data = _mahn_intfb,
534 },
535 };
536
537 struct zsl_fus_drv mahn_drv = {
538 .init_handler = zsl_fus_mahn_init,
539 .feed_handler = zsl_fus_mahn_feed,
540 .error_handler = zsl_fus_mahn_error,
541 .config = &mahn_cfg,
542 };
543
544 struct zsl_quat q;
545 struct zsl_euler e = { 0 };
546 struct zsl_attitude att;
547
548 ZSL_VECTOR_DEF(gv, 3);
549 ZSL_VECTOR_DEF(av, 3);
550 ZSL_VECTOR_DEF(mv, 3);
551
552 zsl_real_t Sx, Sy, St, Smin;
553
554 mahn_drv.init_handler(sampleFreq, mahn_drv.config);
555
556 for (size_t i = 0; i < 1000; i++) {
557 zsl_quat_init(&q, ZSL_QUAT_TYPE_IDENTITY);
558 mahn_cfg.kp = (zsl_real_t) i / 1000.0;
559 Sx = 0.0;
560 Sy = 0.0;
561
562 for (size_t j = 0; j < a->sz_rows; j++) {
563 zsl_mtx_get_row(g, j, gv.data);
564 zsl_mtx_get_row(a, j, av.data);
565 zsl_mtx_get_row(m, j, mv.data);
566
567 mahn_drv.feed_handler(&av, &mv, &gv, incl, &q, mahn_drv.config);
568
569 zsl_quat_to_euler(&q, &e);
570 e.x *= 180. / ZSL_PI;
571 e.y *= 180. / ZSL_PI;
572 e.z *= 180. / ZSL_PI;
573
574 zsl_att_from_accel(&av, &att);
575
576 Sx += (e.x - att.roll) * (e.x - att.roll);
577 Sy += (e.y - att.pitch) * (e.y - att.pitch);
578 }
579 Sx = ZSL_SQRT(Sx / a->sz_rows);
580 Sy = ZSL_SQRT(Sy / a->sz_rows);
581
582 St = (Sx + Sy) / 2.0;
583
584 Smin = 0;
585 if (i == 0) {
586 Smin = St;
587 *kp = mahn_cfg.kp;
588 } else if (St < Smin) {
589 Smin = St;
590 *kp = mahn_cfg.kp;
591 }
592 }
593
594 err:
595 return rc;
596 }
597
