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_mean_x, acc_mean_y, acc_mean_z; 24 float acc_x, acc_y, acc_z, acc_w; 25 float acc_xx, acc_xy, acc_xz, acc_xw; 26 float acc_yy, acc_yz, acc_yw, acc_zz, acc_zw; 27 size_t nsamples; 28 }; 29 30 // Resets the KasaFit data structure (sets all variables to zero). 31 void kasaReset(struct KasaFit *kasa); 32 33 // Initializes the KasaFit data structure. 34 void kasaInit(struct KasaFit *kasa); 35 36 // Accumulates the Kasa acc_** variables with the input vector [x, y, z], and 37 // updates the number of samples. 38 void kasaAccumulate(struct KasaFit *kasa, float x, float y, float z); 39 40 // Normalizes the Kasa acc_** variables. Returns 'false' if the number of 41 // samples is zero, otherwise 'true'. 42 bool kasaNormalize(struct KasaFit *kasa); 43 44 // Uses the Kasa sphere-fit method to extract a 'bias' estimate (centroid) for 45 // the best-fit sphere using the normal equations, and the sphere's 'radius'. 46 // Returns '1' if the radius of the fit sphere is within the bounds 47 // (min_fit, max_fit), otherwise '0'. 48 int kasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius, 49 float max_fit, float min_fit); 50 51 #ifdef __cplusplus 52 } 53 #endif 54 55 #endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_KASA_H_ 56