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