1 files changed, 143 insertions, 0 deletions
diff --git a/firmware/os/algos/calibration/common/sphere_fit_calibration.h b/firmware/os/algos/calibration/common/sphere_fit_calibration.h
new file mode 100644
index 00000000..e49f225a
--- /dev/null
+++ b/firmware/os/algos/calibration/common/sphere_fit_calibration.h
@@ -0,0 +1,143 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+/////////////////////////////////////////////////////////////////////////////
+/*
+ * This module contains an algorithm for performing a sphere fit calibration.
+ * A sphere fit calibration solves the following non-linear least squares
+ * problem:
+ *
+ *   arg min || ||M(x - b)|| - exp_norm ||
+ *      M,b
+ *
+ * where:
+ *  x is a 3xN matrix containing N 3-dimensional uncalibrated data points,
+ *  M is a 3x3 lower diagonal scaling matrix
+ *  b is a 3x1 offset vector.
+ *  exp_norm is the expected norm of an individual calibration data point.
+ * M and b are solved such that the norm of the calibrated data (M(x - b)) is
+ * near exp_norm.
+ *
+ * This module uses a Levenberg-Marquardt nonlinear least squares solver to find
+ * M and b.  M is assumed to be a lower diagonal, consisting of 6 parameters.
+ *
+ */
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_SPHERE_FIT_CALIBRATION_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_SPHERE_FIT_CALIBRATION_H_
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "calibration/common/calibration_data.h"
+#include "common/math/levenberg_marquardt.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define MIN_NUM_SPHERE_FIT_POINTS (14)
+
+// Enum defining the meaning of the state parameters.  The 9-parameter
+// sphere fit calibration computes a lower-diagonal scaling matrix (M) and
+// an offset such that:
+//    x_corrected = M * (x_impaired - offset)
+enum SphereFitParams {
+  eParamScaleMatrix11 = 0,
+  eParamScaleMatrix21,
+  eParamScaleMatrix22,
+  eParamScaleMatrix31,
+  eParamScaleMatrix32,
+  eParamScaleMatrix33,
+  eParamOffset1,
+  eParamOffset2,
+  eParamOffset3,
+  SF_STATE_DIM
+};
+
+// Structure containing the data to be used for the sphere fit calibration.
+struct SphereFitData {
+  // Data for fit (assumed to be a matrix of size num_fit_points x SF_DATA_DIM)
+  const float *fit_data;
+
+  // Pointer to standard deviations of the fit data, used to weight individual
+  // data points.  Assumed to point to a matrix of dimensions
+  // num_fit_points x THREE_AXIS_DIM.
+  // If NULL, data will all be used with equal weighting in the fit.
+  const float *fit_data_std;
+
+  // Number of fit points.
+  size_t num_fit_points;
+
+  // Expected data norm.
+  float expected_norm;
+};
+
+// Structure for a sphere fit calibration, including a non-linear least squares
+// solver and the latest state estimate.
+struct SphereFitCal {
+  // Levenberg-Marquardt solver.
+  struct LmSolver lm_solver;
+
+  // Minimum number of points for computing a calibration.
+  size_t min_points_for_cal;
+
+  // State estimate.
+  float x[SF_STATE_DIM];
+  uint64_t estimate_time_nanos;
+
+  // Initial state for solver.
+  float x0[SF_STATE_DIM];
+};
+
+// Initialize sphere fit calibration structure with solver and fit params.
+void sphereFitInit(struct SphereFitCal *sphere_cal,
+                   const struct LmParams *lm_params,
+                   const size_t min_num_points_for_cal);
+
+// Clears state estimate and initial state.
+void sphereFitReset(struct SphereFitCal *sphere_cal);
+
+// Sets data pointer for single solve of the Levenberg-Marquardt solver.
+// Must be called before calling sphereFitRunCal().
+void sphereFitSetSolverData(struct SphereFitCal *sphere_cal,
+                            struct LmData *lm_data);
+
+// Sends in a set of calibration data and attempts to run calibration.
+// Returns true if a calibration was successfully triggered with this data.
+bool sphereFitRunCal(struct SphereFitCal *sphere_cal,
+                     const struct SphereFitData *data,
+                     uint64_t timestamp_nanos);
+
+// Set an initial condition for the bias state.
+void sphereFitSetInitialBias(struct SphereFitCal *sphere_cal,
+                             const float initial_bias[THREE_AXIS_DIM]);
+
+// Returns the latest calibration data in a ThreeAxisCalData structure.
+void sphereFitGetLatestCal(const struct SphereFitCal *sphere_cal,
+                           struct ThreeAxisCalData *cal_data);
+
+/////////////////  TEST UTILITIES ///////////////////////////////////////////
+// The following functions are exposed in the header for testing only.
+
+// The ResidualAndJacobianFunction for sphere calibration in the
+// Levenberg-Marquardt solver.
+void sphereFitResidAndJacobianFunc(const float *state, const void *f_data,
+                                   float *residual, float *jacobian);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  //  LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_SPHERE_FIT_CALIBRATION_H_