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 /*
18  * This module contains an algorithm for performing a sphere fit calibration.
19  * A sphere fit calibration solves the following non-linear least squares
20  * problem:
21  *
22  *   arg min || ||M(x - b)|| - exp_norm ||
23  *      M,b
24  *
25  * where:
26  *  x is a 3xN matrix containing N 3-dimensional uncalibrated data points,
27  *  M is a 3x3 lower diagonal scaling matrix
28  *  b is a 3x1 offset vector.
29  *  exp_norm is the expected norm of an individual calibration data point.
30  * M and b are solved such that the norm of the calibrated data (M(x - b)) is
31  * near exp_norm.
32  *
33  * This module uses a Levenberg-Marquardt nonlinear least squares solver to find
34  * M and b.  M is assumed to be a lower diagonal, consisting of 6 parameters.
35  *
36  */
37 
38 #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_
39 #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_
40 
41 #include <stdbool.h>
42 #include <stdint.h>
43 
44 #include "calibration/sphere_fit/calibration_data.h"
45 #include "common/math/levenberg_marquardt.h"
46 
47 #ifdef __cplusplus
48 extern "C" {
49 #endif
50 
51 #define MIN_NUM_SPHERE_FIT_POINTS (14)
52 
53 // Enum defining the meaning of the state parameters.  The 9-parameter
54 // sphere fit calibration computes a lower-diagonal scaling matrix (M) and
55 // an offset such that:
56 //    x_corrected = M * (x_impaired - offset)
57 enum SphereFitParams {
58   eParamScaleMatrix11 = 0,
59   eParamScaleMatrix21,
60   eParamScaleMatrix22,
61   eParamScaleMatrix31,
62   eParamScaleMatrix32,
63   eParamScaleMatrix33,
64   eParamOffset1,
65   eParamOffset2,
66   eParamOffset3,
67   SF_STATE_DIM
68 };
69 
70 // Structure containing the data to be used for the sphere fit calibration.
71 struct SphereFitData {
72   // Data for fit (assumed to be a matrix of size num_fit_points x SF_DATA_DIM)
73   const float *fit_data;
74 
75   // Pointer to standard deviations of the fit data, used to weight individual
76   // data points.  Assumed to point to a matrix of dimensions
77   // num_fit_points x THREE_AXIS_DIM.
78   // If NULL, data will all be used with equal weighting in the fit.
79   const float *fit_data_std;
80 
81   // Number of fit points.
82   size_t num_fit_points;
83 
84   // Expected data norm.
85   float expected_norm;
86 };
87 
88 // Structure for a sphere fit calibration, including a non-linear least squares
89 // solver and the latest state estimate.
90 struct SphereFitCal {
91   // Levenberg-Marquardt solver.
92   struct LmSolver lm_solver;
93 
94   // Minimum number of points for computing a calibration.
95   size_t min_points_for_cal;
96 
97   // State estimate.
98   float x[SF_STATE_DIM];
99   uint64_t estimate_time_nanos;
100 
101   // Initial state for solver.
102   float x0[SF_STATE_DIM];
103 };
104 
105 // Initialize sphere fit calibration structure with solver and fit params.
106 void sphereFitInit(struct SphereFitCal *sphere_cal,
107                    const struct LmParams *lm_params,
108                    const size_t min_num_points_for_cal);
109 
110 // Clears state estimate and initial state.
111 void sphereFitReset(struct SphereFitCal *sphere_cal);
112 
113 // Sets data pointer for single solve of the Levenberg-Marquardt solver.
114 // Must be called before calling sphereFitRunCal().
115 void sphereFitSetSolverData(struct SphereFitCal *sphere_cal,
116                             struct LmData *lm_data);
117 
118 // Sends in a set of calibration data and attempts to run calibration.
119 // Returns true if a calibration was successfully triggered with this data.
120 bool sphereFitRunCal(struct SphereFitCal *sphere_cal,
121                      const struct SphereFitData *data,
122                      uint64_t timestamp_nanos);
123 
124 // Set an initial condition for the bias state.
125 void sphereFitSetInitialBias(struct SphereFitCal *sphere_cal,
126                              const float initial_bias[THREE_AXIS_DIM]);
127 
128 // Returns the latest calibration data in a ThreeAxisCalData structure.
129 void sphereFitGetLatestCal(const struct SphereFitCal *sphere_cal,
130                            struct ThreeAxisCalData *cal_data);
131 
132 /////////////////  TEST UTILITIES ///////////////////////////////////////////
133 // The following functions are exposed in the header for testing only.
134 
135 // The ResidualAndJacobianFunction for sphere calibration in the
136 // Levenberg-Marquardt solver.
137 void sphereFitResidAndJacobianFunc(const float *state, const void *f_data,
138                                    float *residual, float *jacobian);
139 
140 #ifdef __cplusplus
141 }
142 #endif
143 
144 #endif  //  LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_
145