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