1 /*
2  * This module provides a data structure, initialization, and fit
3  * routine for algorithms that use the Kasa method for determining the
4  * 3-dimensional offset vector from a set of points on a sphere.
5  *
6  * Reference: I. Kåsa, "A circle fitting procedure and its error analysis," in
7  * IEEE Transactions on Instrumentation and Measurement, vol. IM-25, no. 1, pp.
8  * 8-14, March 1976.
9  */
10 
11 #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_KASA_H_
12 #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_KASA_H_
13 
14 #include <stdbool.h>
15 
16 #include "common/math/vec.h"
17 
18 #ifdef __cplusplus
19 extern "C" {
20 #endif
21 
22 struct KasaFit {
23   float acc_x, acc_y, acc_z, acc_w;
24   float acc_xx, acc_xy, acc_xz, acc_xw;
25   float acc_yy, acc_yz, acc_yw, acc_zz, acc_zw;
26   size_t nsamples;
27 };
28 
29 // Resets the KasaFit data structure (sets all variables to zero).
30 void kasaReset(struct KasaFit *kasa);
31 
32 // Initializes the KasaFit data structure.
33 void kasaInit(struct KasaFit *kasa);
34 
35 // Accumulates the Kasa acc_** variables with the input vector [x, y, z], and
36 // updates the number of samples.
37 void kasaAccumulate(struct KasaFit *kasa, float x, float y, float z);
38 
39 // Normalizes the Kasa acc_** variables. Returns 'false' if the number of
40 // samples is zero, otherwise 'true'.
41 bool kasaNormalize(struct KasaFit *kasa);
42 
43 // Uses the Kasa sphere-fit method to extract a 'bias' estimate (centroid) for
44 // the best-fit sphere using the normal equations, and the sphere's 'radius'.
45 // Returns '1' if the radius of the fit sphere is within the bounds
46 // (min_fit, max_fit), otherwise '0'.
47 int kasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius,
48             float max_fit, float min_fit);
49 
50 #ifdef __cplusplus
51 }
52 #endif
53 
54 #endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_KASA_H_
55