Syncs google3 Calibration Code to Android.

This CL syncs the recent calibration updates made in the google3
directory: /location/lbs/contexthub/nanoapps/calibration/

Test:
 - Firmware has been built and tested for the Nexus/Pixel devices.
 - CTS tests have been run (all passed) for PixelXL (OC-release).

Tip of G3 CL: 141908053

Change-Id: Ib2f7802fa308e04ec603a942a3ac81e43a480e53

25 files changed, 4147 insertions, 744 deletions

diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c
index eea4493a..a789ce9d 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.c
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c
@@ -156,12 +156,7 @@ static void accelCalAlgoInit(struct AccelCalAlgo *acc, uint32_t fx,
                              uint32_t fxb, uint32_t fy, uint32_t fyb,
                              uint32_t fz, uint32_t fzb, uint32_t fle) {
   accelGoodDataInit(&acc->agd, fx, fxb, fy, fyb, fz, fzb, fle);
-
-  initMagCal(&acc->amoc,         // mag_cal_t struct need for accel cal
-             0.0f, 0.0f, 0.0f,   // bias x, y, z
-             1.0f, 0.0f, 0.0f,   // c00, c01, c02
-             0.0f, 1.0f, 0.0f,   // c10, c11, c12
-             0.0f, 0.0f, 1.0f);  // c20, c21, c22
+  initKasa(&acc->akf);
 }
 
 // Accel cal init.
@@ -265,28 +260,28 @@ static int accelStillnessDetection(struct AccelStillDet *asd,
 }
 
 // Accumulate data for KASA fit.
-static void accelCalUpdate(struct MagCal *amoc, struct AccelStillDet *asd) {
+static void accelCalUpdate(struct KasaFit *akf, struct AccelStillDet *asd) {
   // Run accumulators.
   float w = asd->mean_x * asd->mean_x + asd->mean_y * asd->mean_y +
             asd->mean_z * asd->mean_z;
 
-  amoc->acc_x += asd->mean_x;
-  amoc->acc_y += asd->mean_y;
-  amoc->acc_z += asd->mean_z;
-  amoc->acc_w += w;
+  akf->acc_x += asd->mean_x;
+  akf->acc_y += asd->mean_y;
+  akf->acc_z += asd->mean_z;
+  akf->acc_w += w;
 
-  amoc->acc_xx += asd->mean_x * asd->mean_x;
-  amoc->acc_xy += asd->mean_x * asd->mean_y;
-  amoc->acc_xz += asd->mean_x * asd->mean_z;
-  amoc->acc_xw += asd->mean_x * w;
+  akf->acc_xx += asd->mean_x * asd->mean_x;
+  akf->acc_xy += asd->mean_x * asd->mean_y;
+  akf->acc_xz += asd->mean_x * asd->mean_z;
+  akf->acc_xw += asd->mean_x * w;
 
-  amoc->acc_yy += asd->mean_y * asd->mean_y;
-  amoc->acc_yz += asd->mean_y * asd->mean_z;
-  amoc->acc_yw += asd->mean_y * w;
+  akf->acc_yy += asd->mean_y * asd->mean_y;
+  akf->acc_yz += asd->mean_y * asd->mean_z;
+  akf->acc_yw += asd->mean_y * w;
 
-  amoc->acc_zz += asd->mean_z * asd->mean_z;
-  amoc->acc_zw += asd->mean_z * w;
-  amoc->nsamples += 1;
+  akf->acc_zz += asd->mean_z * asd->mean_z;
+  akf->acc_zw += asd->mean_z * w;
+  akf->nsamples += 1;
 }
 
 // Good data detection, sorting and accumulate the data for Kasa.
@@ -301,42 +296,42 @@ static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1,
     ac1->agd.nx += 1;
     ac1->agd.acc_t += temp;
     ac1->agd.acc_tt += temp * temp;
-    accelCalUpdate(&ac1->amoc, asd);
+    accelCalUpdate(&ac1->akf, asd);
   }
   // Negative x bucket nxb.
   if (PHIb > asd->mean_x && ac1->agd.nxb < ac1->agd.nfxb) {
     ac1->agd.nxb += 1;
     ac1->agd.acc_t += temp;
     ac1->agd.acc_tt += temp * temp;
-    accelCalUpdate(&ac1->amoc, asd);
+    accelCalUpdate(&ac1->akf, asd);
   }
   // Y bucket ny.
   if (PHI < asd->mean_y && ac1->agd.ny < ac1->agd.nfy) {
     ac1->agd.ny += 1;
     ac1->agd.acc_t += temp;
     ac1->agd.acc_tt += temp * temp;
-    accelCalUpdate(&ac1->amoc, asd);
+    accelCalUpdate(&ac1->akf, asd);
   }
   // Negative y bucket nyb.
   if (PHIb > asd->mean_y && ac1->agd.nyb < ac1->agd.nfyb) {
     ac1->agd.nyb += 1;
     ac1->agd.acc_t += temp;
     ac1->agd.acc_tt += temp * temp;
-    accelCalUpdate(&ac1->amoc, asd);
+    accelCalUpdate(&ac1->akf, asd);
   }
   // Z bucket nz.
   if (PHIZ < asd->mean_z && ac1->agd.nz < ac1->agd.nfz) {
     ac1->agd.nz += 1;
     ac1->agd.acc_t += temp;
     ac1->agd.acc_tt += temp * temp;
-    accelCalUpdate(&ac1->amoc, asd);
+    accelCalUpdate(&ac1->akf, asd);
   }
   // Negative z bucket nzb.
   if (PHIZb > asd->mean_z && ac1->agd.nzb < ac1->agd.nfzb) {
     ac1->agd.nzb += 1;
     ac1->agd.acc_t += temp;
     ac1->agd.acc_tt += temp * temp;
-    accelCalUpdate(&ac1->amoc, asd);
+    accelCalUpdate(&ac1->akf, asd);
   }
   // The leftover bucket nle.
   if (PHI > asd->mean_x && PHIb < asd->mean_x && PHI > asd->mean_y &&
@@ -345,39 +340,39 @@ static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1,
     ac1->agd.nle += 1;
     ac1->agd.acc_t += temp;
     ac1->agd.acc_tt += temp * temp;
-    accelCalUpdate(&ac1->amoc, asd);
+    accelCalUpdate(&ac1->akf, asd);
   }
   // Checking if all buckets are full.
   if (ac1->agd.nx == ac1->agd.nfx && ac1->agd.nxb == ac1->agd.nfxb &&
       ac1->agd.ny == ac1->agd.nfy && ac1->agd.nyb == ac1->agd.nfyb &&
       ac1->agd.nz == ac1->agd.nfz && ac1->agd.nzb == ac1->agd.nfzb) {
-    //  Check if amoc->nsamples is zero.
-    if (ac1->amoc.nsamples == 0) {
+    //  Check if akf->nsamples is zero.
+    if (ac1->akf.nsamples == 0) {
       agdReset(&ac1->agd);
-      magCalReset(&ac1->amoc);
+      magKasaReset(&ac1->akf);
       complete = 0;
       return complete;
     } else {
       // Normalize the data to the sample numbers.
-      inv = 1.0f / ac1->amoc.nsamples;
+      inv = 1.0f / ac1->akf.nsamples;
     }
 
-    ac1->amoc.acc_x *= inv;
-    ac1->amoc.acc_y *= inv;
-    ac1->amoc.acc_z *= inv;
-    ac1->amoc.acc_w *= inv;
+    ac1->akf.acc_x *= inv;
+    ac1->akf.acc_y *= inv;
+    ac1->akf.acc_z *= inv;
+    ac1->akf.acc_w *= inv;
 
-    ac1->amoc.acc_xx *= inv;
-    ac1->amoc.acc_xy *= inv;
-    ac1->amoc.acc_xz *= inv;
-    ac1->amoc.acc_xw *= inv;
+    ac1->akf.acc_xx *= inv;
+    ac1->akf.acc_xy *= inv;
+    ac1->akf.acc_xz *= inv;
+    ac1->akf.acc_xw *= inv;
 
-    ac1->amoc.acc_yy *= inv;
-    ac1->amoc.acc_yz *= inv;
-    ac1->amoc.acc_yw *= inv;
+    ac1->akf.acc_yy *= inv;
+    ac1->akf.acc_yz *= inv;
+    ac1->akf.acc_yw *= inv;
 
-    ac1->amoc.acc_zz *= inv;
-    ac1->amoc.acc_zw *= inv;
+    ac1->akf.acc_zz *= inv;
+    ac1->akf.acc_zw *= inv;
 
     // Calculate the temp VAR and MEA.N
     ac1->agd.var_t =
@@ -392,7 +387,7 @@ static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1,
       ac1->agd.ny > ac1->agd.nfy || ac1->agd.nyb > ac1->agd.nfyb ||
       ac1->agd.nz > ac1->agd.nfz || ac1->agd.nzb > ac1->agd.nfzb) {
     agdReset(&ac1->agd);
-    magCalReset(&ac1->amoc);
+    magKasaReset(&ac1->akf);
     complete = 0;
     return complete;
   }
@@ -400,15 +395,15 @@ static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1,
 }
 
 // Eigen value magnitude and ratio test.
-static int mocEigenTest(struct MagCal *moc, struct AccelGoodData *agd) {
+static int accEigenTest(struct KasaFit *akf, struct AccelGoodData *agd) {
   // covariance matrix.
   struct Mat33 S;
-  S.elem[0][0] = moc->acc_xx - moc->acc_x * moc->acc_x;
-  S.elem[0][1] = S.elem[1][0] = moc->acc_xy - moc->acc_x * moc->acc_y;
-  S.elem[0][2] = S.elem[2][0] = moc->acc_xz - moc->acc_x * moc->acc_z;
-  S.elem[1][1] = moc->acc_yy - moc->acc_y * moc->acc_y;
-  S.elem[1][2] = S.elem[2][1] = moc->acc_yz - moc->acc_y * moc->acc_z;
-  S.elem[2][2] = moc->acc_zz - moc->acc_z * moc->acc_z;
+  S.elem[0][0] = akf->acc_xx - akf->acc_x * akf->acc_x;
+  S.elem[0][1] = S.elem[1][0] = akf->acc_xy - akf->acc_x * akf->acc_y;
+  S.elem[0][2] = S.elem[2][0] = akf->acc_xz - akf->acc_x * akf->acc_z;
+  S.elem[1][1] = akf->acc_yy - akf->acc_y * akf->acc_y;
+  S.elem[1][2] = S.elem[2][1] = akf->acc_yz - akf->acc_y * akf->acc_z;
+  S.elem[2][2] = akf->acc_zz - akf->acc_z * akf->acc_z;
 
   struct Vec3 eigenvals;
   struct Mat33 eigenvecs;
@@ -502,13 +497,13 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nsec, float x,
         float radius;
 
         // Grabbing the fit from the MAG cal.
-        magCalFit(&acc->ac1[temp_gate].amoc, &bias, &radius);
+        magKasaFit(&acc->ac1[temp_gate].akf, &bias, &radius);
 
         // If offset is too large don't take.
         if (fabsf(bias.x) < MAX_OFF && fabsf(bias.y) < MAX_OFF &&
             fabsf(bias.z) < MAX_OFF) {
           // Eigen Ratio Test.
-          if (mocEigenTest(&acc->ac1[temp_gate].amoc,
+          if (accEigenTest(&acc->ac1[temp_gate].akf,
                            &acc->ac1[temp_gate].agd)) {
             // Storing the new offsets.
             acc->x_bias_new = bias.x * KSCALE2;
@@ -545,7 +540,7 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nsec, float x,
 
         // Resetting the structs for a new accel cal run.
         agdReset(&acc->ac1[temp_gate].agd);
-        magCalReset(&acc->ac1[temp_gate].amoc);
+        magKasaReset(&acc->ac1[temp_gate].akf);
       }
     }
   }
@@ -730,7 +725,7 @@ void accelCalDebPrint(struct AccelCal *acc, float temp) {
         (unsigned)acc->ac1[0].agd.nx, (unsigned)acc->ac1[0].agd.nxb,
         (unsigned)acc->ac1[0].agd.ny, (unsigned)acc->ac1[0].agd.nyb,
         (unsigned)acc->ac1[0].agd.nz, (unsigned)acc->ac1[0].agd.nzb,
-        (unsigned)acc->ac1[0].agd.nle, (unsigned)acc->ac1[0].amoc.nsamples);
+        (unsigned)acc->ac1[0].agd.nle, (unsigned)acc->ac1[0].akf.nsamples);
     // Live bucket count hogher.
     CAL_DEBUG_LOG(
         "[BMI160]",
@@ -739,7 +734,7 @@ void accelCalDebPrint(struct AccelCal *acc, float temp) {
         (unsigned)acc->ac1[1].agd.nx, (unsigned)acc->ac1[1].agd.nxb,
         (unsigned)acc->ac1[1].agd.ny, (unsigned)acc->ac1[1].agd.nyb,
         (unsigned)acc->ac1[1].agd.nz, (unsigned)acc->ac1[1].agd.nzb,
-        (unsigned)acc->ac1[1].agd.nle, (unsigned)acc->ac1[1].amoc.nsamples);
+        (unsigned)acc->ac1[1].agd.nle, (unsigned)acc->ac1[1].akf.nsamples);
     // Offset used.
     CAL_DEBUG_LOG(
         "[BMI160]",
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.h b/firmware/os/algos/calibration/accelerometer/accel_cal.h
index 83a45070..03c65e92 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.h
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.h
@@ -1,6 +1,3 @@
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ACCELEROMETER_ACCEL_CAL_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ACCELEROMETER_ACCEL_CAL_H_
-
 /*
  * Copyright (C) 2016 The Android Open Source Project
  *
@@ -25,6 +22,8 @@
  * the vectors should end onto a sphere. Furthermore the offset values are
  * subtracted from the accelerometer data calibrating the sensor.
  */
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ACCELEROMETER_ACCEL_CAL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ACCELEROMETER_ACCEL_CAL_H_
 
 #include <stdint.h>
 #include <sys/types.h>
@@ -124,7 +123,8 @@ struct AccelStatsMem {
 // Struct for an accel calibration for a single temperature window.
 struct AccelCalAlgo {
   struct AccelGoodData agd;
-  struct MagCal amoc;
+  // TODO(mkramerm): Replace all abbreviations.
+  struct KasaFit akf;
 };
 
 // Complete accel calibration struct.
diff --git a/firmware/os/algos/calibration/common/calibration_data.c b/firmware/os/algos/calibration/common/calibration_data.c
new file mode 100644
index 00000000..9ae72d27
--- /dev/null
+++ b/firmware/os/algos/calibration/common/calibration_data.c
@@ -0,0 +1,36 @@
+#include "calibration/common/calibration_data.h"
+
+#include <string.h>
+
+#include "common/math/vec.h"
+
+// FUNCTION IMPLEMENTATIONS
+//////////////////////////////////////////////////////////////////////////////
+
+// Set calibration data to identity scale factors, zero skew and
+// zero bias.
+void calDataReset(struct ThreeAxisCalData *calstruct) {
+  memset(calstruct, 0, sizeof(struct ThreeAxisCalData));
+  calstruct->scale_factor_x = 1.0f;
+  calstruct->scale_factor_y = 1.0f;
+  calstruct->scale_factor_z = 1.0f;
+}
+
+void calDataCorrectData(const struct ThreeAxisCalData* calstruct,
+                        const float x_impaired[THREE_AXIS_DIM],
+                        float* x_corrected) {
+  // x_temp = (x_impaired - bias).
+  float x_temp[THREE_AXIS_DIM];
+  vecSub(x_temp, x_impaired, calstruct->bias, THREE_AXIS_DIM);
+
+  // x_corrected = scale_skew_mat * x_temp, where:
+  // scale_skew_mat = [scale_factor_x    0         0
+  //                   skew_yx    scale_factor_y   0
+  //                   skew_zx       skew_zy   scale_factor_z].
+  x_corrected[0] = calstruct->scale_factor_x * x_temp[0];
+  x_corrected[1] = calstruct->skew_yx * x_temp[0] +
+      calstruct->scale_factor_y * x_temp[1];
+  x_corrected[2] = calstruct->skew_zx * x_temp[0] +
+      calstruct->skew_zy * x_temp[1] +
+      calstruct->scale_factor_z * x_temp[2];
+}
diff --git a/firmware/os/algos/calibration/common/calibration_data.h b/firmware/os/algos/calibration/common/calibration_data.h
new file mode 100644
index 00000000..b0e6809b
--- /dev/null
+++ b/firmware/os/algos/calibration/common/calibration_data.h
@@ -0,0 +1,72 @@
+/*
+ * 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 a data structure and corresponding helper functions for
+ * a three-axis sensor calibration.  The calibration consists of a bias vector,
+ * bias, and a lower-diagonal scaling and skew matrix, scale_skew_mat.
+ *
+ * The calibration is applied to impaired sensor data as follows:
+ *
+ * corrected_data = scale_skew_mat * (impaired_data - bias).
+ */
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_CALIBRATION_DATA_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_CALIBRATION_DATA_H_
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define THREE_AXIS_DIM (3)
+
+// Calibration data structure.
+struct ThreeAxisCalData {
+  // Scale factor and skew terms.  Used to construct the following lower
+  // diagonal scale_skew_mat:
+  // scale_skew_mat = [scale_factor_x    0         0
+  //                   skew_yx    scale_factor_y   0
+  //                   skew_zx       skew_zy   scale_factor_z].
+  float scale_factor_x;
+  float scale_factor_y;
+  float scale_factor_z;
+  float skew_yx;
+  float skew_zx;
+  float skew_zy;
+
+  // Sensor bias offset.
+  float bias[THREE_AXIS_DIM];
+
+  // Calibration time.
+  uint64_t calibration_time_nanos;
+};
+
+// Set calibration data to identity scale factors, zero skew and
+// zero bias.
+void calDataReset(struct ThreeAxisCalData *calstruct);
+
+// Apply a stored calibration to correct a single sample of impaired sensor
+// data.
+void calDataCorrectData(const struct ThreeAxisCalData* calstruct,
+                        const float x_impaired[THREE_AXIS_DIM],
+                        float* x_corrected);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_CALIBRATION_DATA_H_
diff --git a/firmware/os/algos/calibration/common/diversity_checker.c b/firmware/os/algos/calibration/common/diversity_checker.c
new file mode 100644
index 00000000..8e5c3e28
--- /dev/null
+++ b/firmware/os/algos/calibration/common/diversity_checker.c
@@ -0,0 +1,165 @@
+/*
+ * 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.
+ */
+
+#include "calibration/common/diversity_checker.h"
+
+#include <errno.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <string.h>
+#include "common/math/vec.h"
+
+// Struct initialization.
+void diversityCheckerInit(
+    struct DiversityChecker* diverse_data,
+    float threshold,
+    float max_distance,
+    size_t min_num_diverse_vectors,
+    size_t max_num_max_distance,
+    float var_threshold,
+    float max_min_threshold) {
+  ASSERT_NOT_NULL(diverse_data);
+  // Initialize parameters.
+  diverse_data->threshold = threshold;
+  diverse_data->max_distance = max_distance;
+  diverse_data->min_num_diverse_vectors = min_num_diverse_vectors;
+  // checking for min_num_diverse_vectors = 0
+  if (min_num_diverse_vectors < 1) {
+    diverse_data->min_num_diverse_vectors = 1;
+  }
+  diverse_data->max_num_max_distance = max_num_max_distance;
+  diverse_data->var_threshold = var_threshold;
+  diverse_data->max_min_threshold = max_min_threshold;
+  // Setting the rest to zero.
+  diversityCheckerReset(diverse_data);
+}
+
+// Reset
+void diversityCheckerReset(struct DiversityChecker* diverse_data) {
+  ASSERT_NOT_NULL(diverse_data);
+  // Clear data memory.
+  memset(&diverse_data->diverse_data, 0,
+         sizeof(diverse_data->diverse_data));
+  // Resetting counters and data full bit.
+  diverse_data->num_points = 0;
+  diverse_data->num_max_dist_violations = 0;
+  diverse_data->data_full = false;
+}
+
+void diversityCheckerUpdate(
+    struct DiversityChecker* diverse_data, float x, float y, float z) {
+  ASSERT_NOT_NULL(diverse_data);
+  // Converting three single inputs to a vector.
+  const float vec[3] = {x, y, z};
+  // Result vector for vector difference.
+  float vec_diff[3];
+  // normSquared result (k)
+  float norm_squared_result;
+
+  // If memory is full, no need to run through the data.
+  if (!diverse_data->data_full) {
+    size_t i;
+    // Running over all existing data points
+    for (i = 0; i < diverse_data->num_points; ++i) {
+      // v = v1 - v2;
+      vecSub(vec_diff,
+             &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM],
+             vec,
+             THREE_AXIS_DATA_DIM);
+      // k = |v|^2
+      norm_squared_result = vecNormSquared(vec_diff, THREE_AXIS_DATA_DIM);
+      // if k < Threshold then leave the function.
+      if (norm_squared_result < diverse_data->threshold) {
+        return;
+      }
+      // if k > max_distance, count and leave the function.
+      if (norm_squared_result > diverse_data->max_distance) {
+        diverse_data->num_max_dist_violations++;
+        return;
+      }
+    }
+    // If none of the above caused to leave the function, data is diverse.
+    // Notice that the first data vector will be stored no matter what.
+    memcpy(&diverse_data->
+           diverse_data[diverse_data->num_points * THREE_AXIS_DATA_DIM],
+           vec,
+           sizeof(float) * THREE_AXIS_DATA_DIM);
+    // Count new data point.
+    diverse_data->num_points++;
+    // Setting data_full to 1, if memory is full.
+    if (diverse_data->num_points == NUM_DIVERSE_VECTORS) {
+      diverse_data->data_full = true;
+    }
+  }
+}
+
+bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data,
+                                 float x_bias,
+                                 float y_bias,
+                                 float z_bias) {
+  ASSERT_NOT_NULL(diverse_data);
+  // If not enough diverse data points or max distance violations return false.
+  if (diverse_data->num_points <= diverse_data->min_num_diverse_vectors ||
+      diverse_data->num_max_dist_violations >=
+      diverse_data->max_num_max_distance) {
+    return false;
+  }
+  float vec_bias[3] = {x_bias, y_bias, z_bias};
+  float vec_bias_removed[3];
+  float norm_results;
+  float acc_norm = 0.0f;
+  float acc_norm_square = 0.0f;
+  float max;
+  float min;
+  size_t i;
+  for (i = 0; i < diverse_data->num_points; ++i) {
+    // v = v1 - v_bias;
+    vecSub(vec_bias_removed,
+           &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM],
+           vec_bias,
+           THREE_AXIS_DATA_DIM);
+
+    // norm = ||v||
+    norm_results = vecNorm(vec_bias_removed, THREE_AXIS_DATA_DIM);
+
+    // Accumulate for mean and VAR.
+    acc_norm += norm_results;
+    acc_norm_square += norm_results * norm_results ;
+
+    if (i == 0) {
+      min = norm_results;
+      max = norm_results;
+    }
+    // Finding min
+    if (norm_results < min) {
+      min = norm_results;
+    }
+
+    // Finding max.
+    if (norm_results > max) {
+      max = norm_results;
+    }
+    // can leave the function if max-min is violated
+    // no need to continue.
+    if ((max - min) > diverse_data->max_min_threshold) {
+      return false;
+    }
+  }
+
+  float inv = 1.0f / diverse_data->num_points;
+  float var = (acc_norm_square - (acc_norm * acc_norm) * inv) * inv;
+  return (var < diverse_data->var_threshold);
+}
diff --git a/firmware/os/algos/calibration/common/diversity_checker.h b/firmware/os/algos/calibration/common/diversity_checker.h
new file mode 100644
index 00000000..30f53dba
--- /dev/null
+++ b/firmware/os/algos/calibration/common/diversity_checker.h
@@ -0,0 +1,133 @@
+/*
+ * 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 function implements a diversity checker and stores diverse vectors into
+ * a memory. We assume that the data is located on a sphere, and we use the
+ * norm of the difference of two vectors to decide if the vectors are diverse
+ * enough:
+ *
+ * k = norm( v1 - v2 )^2 < Threshold
+ *
+ * Hence when k < Threshold the data is not stored, because the vectors are too
+ * similar. We store diverse vectors in memory and all new incoming vectors
+ * are checked against the already stored data points.
+ *
+ * Furthermore we also check if k > max_distance, since that data is most likely
+ * not located on a sphere anymore and indicates a disturbance. Finally we give
+ * a "data is full" flag to indicate once the memory is full.
+ * The diverse data can be used to improve sphere fit calibrations, ensuring
+ * that the sphere is populated enough resulting in better fits.
+ *
+ * Memory is stored in an array initialized to length of
+ * [THREE_AXIS_DATA_DIM * NUM_DIVERSE_VECTORS], this has been done to be
+ * compatible with the full sphere fit algorithm.
+ *
+ * Notice, this function stops to check if data is diverse, once the memory is
+ * full. This has been done in order to save processing power.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_DIVERSITY_CHECKER_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_DIVERSITY_CHECKER_H_
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define THREE_AXIS_DATA_DIM (3)   // data is three-dimensional.
+#define NUM_DIVERSE_VECTORS (10)  // Storing 10 data points.
+
+// Main data struct.
+struct DiversityChecker {
+  // Data memory.
+  float diverse_data[THREE_AXIS_DATA_DIM * NUM_DIVERSE_VECTORS];
+
+  // Number of data points in the memory.
+  size_t num_points;
+
+  // Number of data points that violated the max_distance condition.
+  size_t num_max_dist_violations;
+
+  // Threshold value that is used to check k against.
+  float threshold;
+
+  // Maximum distance value.
+  float max_distance;
+
+  // Data full bit.
+  bool data_full;
+
+  // Setup variables for NormQuality check.
+
+  size_t min_num_diverse_vectors;
+  size_t max_num_max_distance;
+  float var_threshold;
+  float max_min_threshold;
+};
+
+// Initialization of the function/struct, input:
+// threshold -> sets the threshold value, only distances k that are equal
+//              or higher than that will be stored.
+// max_distance -> sets the maximum allowed distance of k.
+// min_num_diverse_vectors -> sets the gate for a minimum number of data points
+//                           in the memory
+// max_num_max_distance -> sets the value for a max distance violation number
+//                         gate.
+// var_threshold -> is a threshold value for a Norm variance gate.
+// max_min_threshold -> is a value for a gate that rejects Norm variations
+//                      that are larger than this number.
+void diversityCheckerInit(struct DiversityChecker* diverse_data,
+                          float threshold,
+                          float max_distance,
+                          size_t min_num_diverse_vectors,
+                          size_t max_num_max_distance,
+                          float var_threshold,
+                          float max_min_threshold);
+
+// Resetting the memory and the counters, leaves threshold and max_distance
+// as well as the setup variables for NormQuality check untouched.
+void diversityCheckerReset(struct DiversityChecker* diverse_data);
+
+// Main function. Tests the data (x,y,z) against the memory if diverse and
+// stores it, if so.
+void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x,
+                            float y, float z);
+
+// Removing a constant bias from the diverse_data and check if the norm is
+// within a defined bound:
+// implemented 4 gates
+// -> needs a minimum number of data points in the memory
+//    (controlled by min_num_divers_vectors).
+// -> will return false if maximum number of max_distance is reached
+//    (controlled by max_num_max_distance).
+// -> norm must be within a var window.
+// -> norm must be within a MAX/MIN window.
+// Returned value will only be true if all 4 gates are passed.
+bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data,
+                                 float x_bias,
+                                 float y_bias,
+                                 float z_bias);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_DIVERSITY_CHECKER_H_
diff --git a/firmware/os/algos/calibration/common/sphere_fit_calibration.c b/firmware/os/algos/calibration/common/sphere_fit_calibration.c
new file mode 100644
index 00000000..2c26af55
--- /dev/null
+++ b/firmware/os/algos/calibration/common/sphere_fit_calibration.c
@@ -0,0 +1,247 @@
+#include "calibration/common/sphere_fit_calibration.h"
+
+#include <errno.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "calibration/util/cal_log.h"
+#include "common/math/mat.h"
+#include "common/math/vec.h"
+
+// FORWARD DECLARATIONS
+///////////////////////////////////////////////////////////////////////////////
+// Utility for converting solver state to a calibration data structure.
+static void convertStateToCalStruct(const float x[SF_STATE_DIM],
+                                    struct ThreeAxisCalData *calstruct);
+
+static bool runCalibration(struct SphereFitCal *sphere_cal,
+                           const struct SphereFitData *data,
+                           uint64_t timestamp_nanos);
+
+#define MIN_VALID_DATA_NORM (1e-4)
+
+// FUNCTION IMPLEMENTATIONS
+//////////////////////////////////////////////////////////////////////////////
+void sphereFitInit(struct SphereFitCal *sphere_cal,
+                   const struct LmParams *lm_params,
+                   const size_t min_num_points_for_cal) {
+  ASSERT_NOT_NULL(sphere_cal);
+  ASSERT_NOT_NULL(lm_params);
+
+  // Initialize LM solver.
+  lmSolverInit(&sphere_cal->lm_solver, lm_params,
+               &sphereFitResidAndJacobianFunc);
+
+  // Reset other parameters.
+  sphereFitReset(sphere_cal);
+
+  // Set num points for calibration, checking that it is above min.
+  if (min_num_points_for_cal < MIN_NUM_SPHERE_FIT_POINTS) {
+    sphere_cal->min_points_for_cal = MIN_NUM_SPHERE_FIT_POINTS;
+  } else {
+    sphere_cal->min_points_for_cal = min_num_points_for_cal;
+  }
+}
+
+void sphereFitReset(struct SphereFitCal *sphere_cal) {
+  ASSERT_NOT_NULL(sphere_cal);
+
+  // Set state to default (diagonal scale matrix and zero offset).
+  memset(&sphere_cal->x0[0], 0, sizeof(float) * SF_STATE_DIM);
+  sphere_cal->x0[eParamScaleMatrix11] = 1.f;
+  sphere_cal->x0[eParamScaleMatrix22] = 1.f;
+  sphere_cal->x0[eParamScaleMatrix33] = 1.f;
+  memcpy(sphere_cal->x, sphere_cal->x0, sizeof(sphere_cal->x));
+
+  // Reset time.
+  sphere_cal->estimate_time_nanos = 0;
+}
+
+void sphereFitSetSolverData(struct SphereFitCal *sphere_cal,
+                            struct LmData *lm_data) {
+  ASSERT_NOT_NULL(sphere_cal);
+  ASSERT_NOT_NULL(lm_data);
+
+  // Set solver data.
+  lmSolverSetData(&sphere_cal->lm_solver, lm_data);
+}
+
+bool sphereFitRunCal(struct SphereFitCal *sphere_cal,
+                     const struct SphereFitData *data,
+                     uint64_t timestamp_nanos) {
+  ASSERT_NOT_NULL(sphere_cal);
+  ASSERT_NOT_NULL(data);
+
+  // Run calibration if have enough points.
+  if (data->num_fit_points >= sphere_cal->min_points_for_cal) {
+    return runCalibration(sphere_cal, data, timestamp_nanos);
+  }
+
+  return false;
+}
+
+void sphereFitSetInitialBias(struct SphereFitCal *sphere_cal,
+                             const float initial_bias[THREE_AXIS_DIM]) {
+  ASSERT_NOT_NULL(sphere_cal);
+  sphere_cal->x0[eParamOffset1] = initial_bias[0];
+  sphere_cal->x0[eParamOffset2] = initial_bias[1];
+  sphere_cal->x0[eParamOffset3] = initial_bias[2];
+}
+
+void sphereFitGetLatestCal(const struct SphereFitCal *sphere_cal,
+                           struct ThreeAxisCalData *cal_data) {
+  ASSERT_NOT_NULL(sphere_cal);
+  ASSERT_NOT_NULL(cal_data);
+  convertStateToCalStruct(sphere_cal->x, cal_data);
+  cal_data->calibration_time_nanos = sphere_cal->estimate_time_nanos;
+}
+
+void sphereFitResidAndJacobianFunc(const float *state, const void *f_data,
+                                   float *residual, float *jacobian) {
+  ASSERT_NOT_NULL(state);
+  ASSERT_NOT_NULL(f_data);
+  ASSERT_NOT_NULL(residual);
+
+  const struct SphereFitData *data = (const struct SphereFitData*)f_data;
+
+  // Verify that expected norm is non-zero, else use default of 1.0.
+  float expected_norm = 1.0;
+  ASSERT(data->expected_norm > MIN_VALID_DATA_NORM);
+  if (data->expected_norm > MIN_VALID_DATA_NORM) {
+    expected_norm = data->expected_norm;
+  }
+
+  // Convert parameters to calibration structure.
+  struct ThreeAxisCalData calstruct;
+  convertStateToCalStruct(state, &calstruct);
+
+  // Compute Jacobian helper matrix if Jacobian requested.
+  //
+  // J = d(||M(x_data - bias)|| - expected_norm)/dstate
+  //   = (M(x_data - bias) / ||M(x_data - bias)||) * d(M(x_data - bias))/dstate
+  //   = x_corr / ||x_corr|| * A
+  // A = d(M(x_data - bias))/dstate
+  //   = [dy/dM11, dy/dM21, dy/dM22, dy/dM31, dy/dM32, dy/dM3,...
+  //      dy/db1, dy/db2, dy/db3]'
+  // where:
+  // dy/dM11 = [x_data[0] - bias[0], 0, 0]
+  // dy/dM21 = [0, x_data[0] - bias[0], 0]
+  // dy/dM22 = [0, x_data[1] - bias[1], 0]
+  // dy/dM31 = [0, 0, x_data[0] - bias[0]]
+  // dy/dM32 = [0, 0, x_data[1] - bias[1]]
+  // dy/dM33 = [0, 0, x_data[2] - bias[2]]
+  // dy/db1 = [-scale_factor_x, 0, 0]
+  // dy/db2 = [0, -scale_factor_y, 0]
+  // dy/db3 = [0, 0, -scale_factor_z]
+  float A[SF_STATE_DIM * THREE_AXIS_DIM];
+  if (jacobian) {
+    memset(jacobian, 0, sizeof(float) * SF_STATE_DIM * data->num_fit_points);
+    memset(A, 0, sizeof(A));
+    A[0 * SF_STATE_DIM + eParamOffset1] = -calstruct.scale_factor_x;
+    A[1 * SF_STATE_DIM + eParamOffset2] = -calstruct.scale_factor_y;
+    A[2 * SF_STATE_DIM + eParamOffset3] = -calstruct.scale_factor_z;
+  }
+
+  // Loop over all data points to compute residual and Jacobian.
+  // TODO(dvitus): Use fit_data_std when available to weight residuals.
+  float x_corr[THREE_AXIS_DIM];
+  float x_bias_corr[THREE_AXIS_DIM];
+  size_t i;
+  for (i = 0; i < data->num_fit_points; ++i) {
+    const float *x_data = &data->fit_data[i * THREE_AXIS_DIM];
+
+    // Compute corrected sensor data
+    calDataCorrectData(&calstruct, x_data, x_corr);
+
+    // Compute norm of x_corr.
+    const float norm = vecNorm(x_corr, THREE_AXIS_DIM);
+
+    // Compute residual error: f_x = norm - exp_norm
+    residual[i] = norm - data->expected_norm;
+
+    // Compute Jacobian if valid pointer.
+    if (jacobian) {
+      if (norm < MIN_VALID_DATA_NORM) {
+        return;
+      }
+      const float scale = 1.f / norm;
+
+      // Compute bias corrected data.
+      vecSub(x_bias_corr, x_data, calstruct.bias, THREE_AXIS_DIM);
+
+      // Populate non-bias terms for A
+      A[0 + eParamScaleMatrix11] = x_bias_corr[0];
+      A[1 * SF_STATE_DIM + eParamScaleMatrix21] = x_bias_corr[0];
+      A[1 * SF_STATE_DIM + eParamScaleMatrix22] = x_bias_corr[1];
+      A[2 * SF_STATE_DIM + eParamScaleMatrix31] = x_bias_corr[0];
+      A[2 * SF_STATE_DIM + eParamScaleMatrix32] = x_bias_corr[1];
+      A[2 * SF_STATE_DIM + eParamScaleMatrix33] = x_bias_corr[2];
+
+      // Compute J = x_corr / ||x_corr|| * A
+      matTransposeMultiplyVec(&jacobian[i * SF_STATE_DIM], A, x_corr,
+                              THREE_AXIS_DIM, SF_STATE_DIM);
+      vecScalarMulInPlace(&jacobian[i * SF_STATE_DIM], scale, SF_STATE_DIM);
+    }
+  }
+}
+
+void convertStateToCalStruct(const float x[SF_STATE_DIM],
+                             struct ThreeAxisCalData *calstruct) {
+  memcpy(&calstruct->bias[0], &x[eParamOffset1],
+         sizeof(float) * THREE_AXIS_DIM);
+  calstruct->scale_factor_x = x[eParamScaleMatrix11];
+  calstruct->skew_yx = x[eParamScaleMatrix21];
+  calstruct->scale_factor_y = x[eParamScaleMatrix22];
+  calstruct->skew_zx = x[eParamScaleMatrix31];
+  calstruct->skew_zy = x[eParamScaleMatrix32];
+  calstruct->scale_factor_z = x[eParamScaleMatrix33];
+}
+
+bool runCalibration(struct SphereFitCal *sphere_cal,
+                    const struct SphereFitData *data,
+                    uint64_t timestamp_nanos) {
+  float x_sol[SF_STATE_DIM];
+
+  // Run calibration
+  const enum LmStatus status = lmSolverSolve(&sphere_cal->lm_solver,
+                                             sphere_cal->x0, (void *)data,
+                                             SF_STATE_DIM, data->num_fit_points,
+                                             x_sol);
+
+  // Check if solver was successful
+  if (status == RELATIVE_STEP_SUFFICIENTLY_SMALL ||
+      status == GRADIENT_SUFFICIENTLY_SMALL) {
+    // TODO(dvitus): Check quality of new fit before using.
+    // Store new fit.
+#ifdef SPHERE_FIT_DBG_ENABLED
+    CAL_DEBUG_LOG(
+        "[SPHERE CAL]",
+        "Solution found in %d iterations with status %d.\n",
+        sphere_cal->lm_solver.num_iter, status);
+    CAL_DEBUG_LOG(
+        "[SPHERE CAL]",
+        "Solution:\n {%s%d.%06d [M(1,1)], %s%d.%06d [M(2,1)], "
+        "%s%d.%06d [M(2,2)], \n"
+        "%s%d.%06d [M(3,1)], %s%d.%06d [M(3,2)], %s%d.%06d [M(3,3)], \n"
+        "%s%d.%06d [b(1)], %s%d.%06d [b(2)], %s%d.%06d [b(3)]}.",
+        CAL_ENCODE_FLOAT(x_sol[0], 6), CAL_ENCODE_FLOAT(x_sol[1], 6),
+        CAL_ENCODE_FLOAT(x_sol[2], 6), CAL_ENCODE_FLOAT(x_sol[3], 6),
+        CAL_ENCODE_FLOAT(x_sol[4], 6), CAL_ENCODE_FLOAT(x_sol[5], 6),
+        CAL_ENCODE_FLOAT(x_sol[6], 6), CAL_ENCODE_FLOAT(x_sol[7], 6),
+        CAL_ENCODE_FLOAT(x_sol[8], 6));
+#endif
+    memcpy(sphere_cal->x, x_sol, sizeof(x_sol));
+    sphere_cal->estimate_time_nanos = timestamp_nanos;
+    return true;
+  } else {
+#ifdef SPHERE_FIT_DBG_ENABLED
+     CAL_DEBUG_LOG(
+        "[SPHERE CAL]",
+        "Solution failed in %d iterations with status %d.\n",
+        sphere_cal->lm_solver.num_iter, status);
+#endif
+  }
+
+  return false;
+}
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_
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
index 4e87dc9c..1c609551 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
@@ -15,44 +15,126 @@
  */
 
 #include "calibration/gyroscope/gyro_cal.h"
+
+#include <float.h>
+#include <math.h>
+#include <string.h>
+
 #include "calibration/util/cal_log.h"
+#include "common/math/vec.h"
 
 /////// DEFINITIONS AND MACROS ///////////////////////////////////////
 
 // Maximum gyro bias correction (should be set based on expected max bias
 // of the given sensor).
-#define MAX_GYRO_BIAS 0.096f  // [rad/sec]
+#define MAX_GYRO_BIAS (0.096f)  // [rad/sec]
+
+// The time value used to throttle debug messaging.
+#define OVERTEMPCAL_WAIT_TIME_NANOS (250000000)
+
+// Converts units of radians to milli-degrees.
+#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / M_PI)
+
+// Unit conversion: m/sec^2 to g's.
+#define GRAVITY_TO_G (float)(1e3f * 180.0f / M_PI)
 
-// Helper constants for converting units.
-#define RAD_TO_MDEG (float)(1e3 * 180.0 / M_PI)
-#define GRAVITY_TO_G (float)(1.0 / 9.80665)
+// Unit conversion: nanoseconds to seconds.
+#define NANOS_TO_SEC (1.0e-9f)
 
 /////// FORWARD DECLARATIONS /////////////////////////////////////////
 
 static void deviceStillnessCheck(struct GyroCal* gyro_cal,
-                                 uint64_t sample_time);
+                                 uint64_t sample_time_nanos);
 
-static void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time);
+static void computeGyroCal(struct GyroCal* gyro_cal,
+                           uint64_t calibration_time_nanos);
 
-static void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time);
+static void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos);
 
 #ifdef GYRO_CAL_DBG_ENABLED
-static void gyroCalUpdateDebug(struct GyroCal* gyro_cal,
-                               struct DebugGyroCal* debug_gyro_cal);
+static void gyroCalUpdateDebug(struct GyroCal* gyro_cal);
 
-static void gyroCalTuneDebugPrint(struct GyroCal* gyro_cal,
-                                  uint64_t sample_time);
+/*
+ * Updates running calculation of the temperature statistics.
+ *
+ * Behavior:
+ *   1)  If 'debug_temperature' pointer is not NULL then the local calculation
+ *       of the temperature statistics are copied, and the function returns.
+ *   2)  Else, if 'reset_stats' is 'true' then the local statistics are reset
+ *       and the function returns.
+ *   3)  Otherwise, the local temperature statistics are updated according to
+ *       the input value 'temperature'.
+ *
+ * INPUTS:
+ *   debug_temperature:   Pointer to the temperature stats sturcture to update.
+ *   temperature:  Temperature value (Celsius).
+ *   reset_stats:  Flag that determines if the local running stats are reset.
+ */
+static void gyroTempUpdateStats(struct DebugTemperature* debug_temperature,
+                                float temperature, bool reset_stats);
 
-static void gyroCalDebugPrintData(int count, struct DebugGyroCal* debug_data);
-#endif
+/*
+ * Updates running calculation of the gyro's mean sampling rate.
+ *
+ * Behavior:
+ *   1)  If 'debug_mean_sampling_rate_hz' pointer is not NULL then the local
+ *       calculation of the sampling rate is copied, and the function returns.
+ *   2)  Else, if 'reset_stats' is 'true' then the local estimate is reset and
+ *       the function returns.
+ *   3)  Otherwise, the local estimate of the mean sampling rates is updated.
+ *
+ * INPUTS:
+ *   debug_mean_sampling_rate_hz:   Pointer to the mean sampling rate to update.
+ *   timestamp_nanos:  Time stamp (nanoseconds).
+ *   reset_stats:  Flag that signals a reset of the sampling rate estimate.
+ */
+static void gyroSamplingRateUpdate(float* debug_mean_sampling_rate_hz,
+                                   uint64_t timestamp_nanos, bool reset_stats);
+
+// Defines the type of debug data to print.
+enum DebugPrintData {
+  BIAS_CAL = 0,
+  CAL_TIME,
+  ACCEL_STATS,
+  GYRO_STATS,
+  MAG_STATS,
+  TEMP_STATS,
+  STILLNESS_DATA,
+  SAMPLING_RATE
+};
+
+// Helper function for printing out common debug data.
+static void gyroCalDebugPrintData(const struct DebugGyroCal* debug_data,
+                                  enum DebugPrintData print_data);
+
+// This conversion function is necessary for Nanohub firmware compilation (i.e.,
+// can't cast a uint64_t to a float directly). This conversion function was
+// copied from: /third_party/contexthub/firmware/src/floatRt.c
+static float floatFromUint64(uint64_t v)
+{
+    uint32_t hi = v >> 32, lo = v;
+
+    if (!hi) //this is very fast for cases where we fit into a uint32_t
+        return(float)lo;
+    else {
+        return ((float)hi) * 4294967296.0f + (float)lo;
+    }
+}
+
+#ifdef GYRO_CAL_DBG_TUNE_ENABLED
+// Prints debug information useful for tuning the GyroCal parameters.
+static void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal,
+                                  uint64_t timestamp_nanos);
+#endif  // GYRO_CAL_DBG_TUNE_ENABLED
+#endif  // GYRO_CAL_DBG_ENABLED
 
 /////// FUNCTION DEFINITIONS /////////////////////////////////////////
 
 // Initialize the gyro calibration data structure.
-void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration,
-                 uint64_t max_still_duration, float bias_x, float bias_y,
-                 float bias_z, uint64_t calibration_time,
-                 uint64_t window_time_duration, float gyro_var_threshold,
+void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos,
+                 uint64_t max_still_duration_nanos, float bias_x, float bias_y,
+                 float bias_z, uint64_t calibration_time_nanos,
+                 uint64_t window_time_duration_nanos, float gyro_var_threshold,
                  float gyro_confidence_delta, float accel_var_threshold,
                  float accel_confidence_delta, float mag_var_threshold,
                  float mag_confidence_delta, float stillness_threshold,
@@ -61,9 +143,9 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration,
   memset(gyro_cal, 0, sizeof(struct GyroCal));
 
   // Initialize the stillness detectors.
-  // Gyro parameter input units are [rad/sec]
-  // Accel parameter input units are [m/sec^2]
-  // Magnetometer parameter input units are [uT]
+  // Gyro parameter input units are [rad/sec].
+  // Accel parameter input units are [m/sec^2].
+  // Magnetometer parameter input units are [uT].
   gyroStillDetInit(&gyro_cal->gyro_stillness_detect, gyro_var_threshold,
                    gyro_confidence_delta);
   gyroStillDetInit(&gyro_cal->accel_stillness_detect, accel_var_threshold,
@@ -73,23 +155,24 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration,
 
   // Reset stillness flag and start timestamp.
   gyro_cal->prev_still = false;
-  gyro_cal->start_still_time = 0;
+  gyro_cal->start_still_time_nanos = 0;
 
   // Set the min and max window stillness duration.
-  gyro_cal->min_still_duration = min_still_duration;
-  gyro_cal->max_still_duration = max_still_duration;
+  gyro_cal->min_still_duration_nanos = min_still_duration_nanos;
+  gyro_cal->max_still_duration_nanos = max_still_duration_nanos;
 
   // Sets the duration of the stillness processing windows.
-  gyro_cal->window_time_duration = window_time_duration;
+  gyro_cal->window_time_duration_nanos = window_time_duration_nanos;
 
   // Set the watchdog timeout duration.
-  gyro_cal->gyro_watchdog_timeout_duration = 2 * window_time_duration;
+  gyro_cal->gyro_watchdog_timeout_duration_nanos =
+      2 * window_time_duration_nanos;
 
   // Load the last valid cal from system memory.
   gyro_cal->bias_x = bias_x;  // [rad/sec]
   gyro_cal->bias_y = bias_y;  // [rad/sec]
   gyro_cal->bias_z = bias_z;  // [rad/sec]
-  gyro_cal->calibration_time = calibration_time;
+  gyro_cal->calibration_time_nanos = calibration_time_nanos;
 
   // Set the stillness threshold required for gyro bias calibration.
   gyro_cal->stillness_threshold = stillness_threshold;
@@ -98,26 +181,36 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration,
   // collection in sync. Setting this to zero signals that sensor data
   // will be dropped until a valid end time is set from the first gyro
   // timestamp received.
-  gyro_cal->stillness_win_endtime = 0;
+  gyro_cal->stillness_win_endtime_nanos = 0;
 
   // Gyro calibrations will be applied (see, gyroCalRemoveBias()).
   gyro_cal->gyro_calibration_enable = (remove_bias_enable > 0);
 
 #ifdef GYRO_CAL_DBG_ENABLED
-  CAL_DEBUG_LOG("[GYRO_CAL]", "Gyro Cal: Initialized.");
-  CAL_DEBUG_LOG("[GYRO_CAL]",
-                "GyroCalInit = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [mdps]\n",
-                CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 6),
-                CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 6),
-                CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 6));
-
-  // Initialize the debug report state machine.
-  gyro_cal->gyro_debug_state = -1;
-#endif
+  CAL_DEBUG_LOG("[GYRO_CAL:MEMORY]", "sizeof(struct GyroCal): %lu",
+                (unsigned long int)sizeof(struct GyroCal));
+
+  CAL_DEBUG_LOG("[GYRO_CAL:INIT]",
+                "Gyro Bias Calibration [mdps]: %s%d.%06d, %s%d.%06d, %s%d.%06d",
+                CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6),
+                CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6),
+                CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6));
+
+  if (gyro_cal->gyro_calibration_enable) {
+    CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration ENABLED.");
+  } else {
+    CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration DISABLED.");
+  }
+
+  // Ensures that the running temperature statistics and gyro sampling rate
+  // estimate are reset.
+  gyroTempUpdateStats(NULL, 0, /*reset_stats=*/true);
+  gyroSamplingRateUpdate(NULL, 0, /*reset_stats=*/true);
+#endif  // GYRO_CAL_DBG_ENABLED
 }
 
-// Void all pointers in the gyro calibration data structure
-// (prevents compiler warnings).
+// Void all pointers in the gyro calibration data structure (doesn't do anything
+// except prevent compiler warnings).
 void gyroCalDestroy(struct GyroCal* gyro_cal) { (void)gyro_cal; }
 
 // Get the most recent bias calibration value.
@@ -127,23 +220,32 @@ void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y,
     *bias_x = gyro_cal->bias_x;
     *bias_y = gyro_cal->bias_y;
     *bias_z = gyro_cal->bias_z;
+
+#ifdef GYRO_CAL_DBG_ENABLED
+    CAL_DEBUG_LOG(
+        "[GYRO_CAL:STORED]",
+        "Gyro Bias Calibration [mdps]: %s%d.%06d, %s%d.%06d, %s%d.%06d",
+        CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6),
+        CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6),
+        CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6));
+#endif
   }
 }
 
 // Set an initial bias calibration value.
 void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y,
-                    float bias_z, uint64_t calibration_time) {
+                    float bias_z, uint64_t calibration_time_nanos) {
   gyro_cal->bias_x = bias_x;
   gyro_cal->bias_y = bias_y;
   gyro_cal->bias_z = bias_z;
-  gyro_cal->calibration_time = calibration_time;
+  gyro_cal->calibration_time_nanos = calibration_time_nanos;
 
 #ifdef GYRO_CAL_DBG_ENABLED
-  CAL_DEBUG_LOG("[GYRO_CAL]",
-                "GyroCalSetBias = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [mdps]\n",
-                CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 6),
-                CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 6),
-                CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 6));
+  CAL_DEBUG_LOG("[GYRO_CAL:RECALL]",
+                "Gyro Bias Calibration [mdps]: %s%d.%06d, %s%d.%06d, %s%d.%06d",
+                CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6),
+                CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6),
+                CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6));
 #endif
 }
 
@@ -169,60 +271,74 @@ bool gyroCalNewBiasAvailable(struct GyroCal* gyro_cal) {
 }
 
 // Update the gyro calibration with gyro data [rad/sec].
-void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time, float x,
-                       float y, float z, float temperature) {
+void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time_nanos,
+                       float x, float y, float z, float temperature) {
   // Make sure that a valid window end time is set,
   // and start the watchdog timer.
-  if (gyro_cal->stillness_win_endtime <= 0) {
-    gyro_cal->stillness_win_endtime =
-        sample_time + gyro_cal->window_time_duration;
+  if (gyro_cal->stillness_win_endtime_nanos <= 0) {
+    gyro_cal->stillness_win_endtime_nanos =
+        sample_time_nanos + gyro_cal->window_time_duration_nanos;
 
     // Start the watchdog timer.
-    gyro_cal->gyro_watchdog_start = sample_time;
+    gyro_cal->gyro_watchdog_start_nanos = sample_time_nanos;
   }
 
+#ifdef GYRO_CAL_DBG_ENABLED
+  // Update the temperature statistics (on temperature change only).
+  if (NANO_ABS(gyro_cal->latest_temperature_celcius - temperature) > FLT_MIN) {
+    gyroTempUpdateStats(NULL, temperature, /*reset_stats=*/false);
+  }
+
+  // Update the gyro sampling rate estimate.
+  gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/false);
+#endif  // GYRO_CAL_DBG_ENABLED
+
   // Record the latest temperture sample.
   gyro_cal->latest_temperature_celcius = temperature;
 
   // Pass gyro data to stillness detector
   gyroStillDetUpdate(&gyro_cal->gyro_stillness_detect,
-                     gyro_cal->stillness_win_endtime, sample_time, x, y, z);
+                     gyro_cal->stillness_win_endtime_nanos, sample_time_nanos,
+                     x, y, z);
 
   // Perform a device stillness check, set next window end time, and
   // possibly do a gyro bias calibration and stillness detector reset.
-  deviceStillnessCheck(gyro_cal, sample_time);
+  deviceStillnessCheck(gyro_cal, sample_time_nanos);
 }
 
 // Update the gyro calibration with mag data [micro Tesla].
-void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time, float x,
-                      float y, float z) {
+void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time_nanos,
+                      float x, float y, float z) {
   // Pass magnetometer data to stillness detector.
   gyroStillDetUpdate(&gyro_cal->mag_stillness_detect,
-                     gyro_cal->stillness_win_endtime, sample_time, x, y, z);
+                     gyro_cal->stillness_win_endtime_nanos, sample_time_nanos,
+                     x, y, z);
 
   // Received a magnetometer sample; incorporate it into detection.
   gyro_cal->using_mag_sensor = true;
 
   // Perform a device stillness check, set next window end time, and
   // possibly do a gyro bias calibration and stillness detector reset.
-  deviceStillnessCheck(gyro_cal, sample_time);
+  deviceStillnessCheck(gyro_cal, sample_time_nanos);
 }
 
 // Update the gyro calibration with accel data [m/sec^2].
-void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time, float x,
-                        float y, float z) {
+void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time_nanos,
+                        float x, float y, float z) {
   // Pass accelerometer data to stillnesss detector.
   gyroStillDetUpdate(&gyro_cal->accel_stillness_detect,
-                     gyro_cal->stillness_win_endtime, sample_time, x, y, z);
+                     gyro_cal->stillness_win_endtime_nanos, sample_time_nanos,
+                     x, y, z);
 
   // Perform a device stillness check, set next window end time, and
   // possibly do a gyro bias calibration and stillness detector reset.
-  deviceStillnessCheck(gyro_cal, sample_time);
+  deviceStillnessCheck(gyro_cal, sample_time_nanos);
 }
 
 // Checks the state of all stillness detectors to determine
 // whether the device is "still".
-void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time) {
+void deviceStillnessCheck(struct GyroCal* gyro_cal,
+                          uint64_t sample_time_nanos) {
   bool stillness_duration_exceeded = false;
   bool stillness_duration_too_short = false;
   bool device_is_still = false;
@@ -231,7 +347,7 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time) {
   float conf_still = 0;
 
   // Check the watchdog timer.
-  checkWatchdog(gyro_cal, sample_time);
+  checkWatchdog(gyro_cal, sample_time_nanos);
 
   // Is there enough data to do a stillness calculation?
   if ((!gyro_cal->mag_stillness_detect.stillness_window_ready &&
@@ -242,8 +358,8 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time) {
   }
 
   // Set the next window end time for the stillness detectors.
-  gyro_cal->stillness_win_endtime =
-      sample_time + gyro_cal->window_time_duration;
+  gyro_cal->stillness_win_endtime_nanos =
+      sample_time_nanos + gyro_cal->window_time_duration_nanos;
 
   // Update the confidence scores for all sensors.
   gyroStillDetCompute(&gyro_cal->accel_stillness_detect);
@@ -274,15 +390,15 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time) {
     if (!gyro_cal->prev_still) {
       // Record the starting timestamp of the current stillness window.
       // This enables the calculation of total duration of the stillness period.
-      gyro_cal->start_still_time =
+      gyro_cal->start_still_time_nanos =
           gyro_cal->gyro_stillness_detect.window_start_time;
     }
 
-    // Check to see if current stillness period exceeds the desired limit
-    // to avoid corrupting the .
+    // Check to see if current stillness period exceeds the desired limit.
     stillness_duration_exceeded =
         ((gyro_cal->gyro_stillness_detect.last_sample_time -
-          gyro_cal->start_still_time) > gyro_cal->max_still_duration);
+          gyro_cal->start_still_time_nanos) >
+         gyro_cal->max_still_duration_nanos);
 
     if (stillness_duration_exceeded) {
       // The current stillness has gone too long. Do a calibration with the
@@ -290,23 +406,32 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time) {
 
       // Update the gyro bias estimate with the current window data and
       // reset the stats.
-      gyroStillDetReset(&gyro_cal->accel_stillness_detect, true);
-      gyroStillDetReset(&gyro_cal->gyro_stillness_detect, true);
-      gyroStillDetReset(&gyro_cal->mag_stillness_detect, true);
+      gyroStillDetReset(&gyro_cal->accel_stillness_detect,
+                        /*reset_stats=*/true);
+      gyroStillDetReset(&gyro_cal->gyro_stillness_detect, /*reset_stats=*/true);
+      gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/true);
 
       // Perform calibration.
       computeGyroCal(gyro_cal,
                      gyro_cal->gyro_stillness_detect.last_sample_time);
 
+#ifdef GYRO_CAL_DBG_ENABLED
+      // Reset the temperature statistics and sampling rate estimate.
+      gyroTempUpdateStats(NULL, 0, /*reset_stats=*/true);
+      gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
+#endif  // GYRO_CAL_DBG_ENABLED
+
       // Update stillness flag. Force the start of a new stillness period.
       gyro_cal->prev_still = false;
     } else {
       // Continue collecting stillness data.
 
       // Reset stillness detectors, and extend stillness period.
-      gyroStillDetReset(&gyro_cal->accel_stillness_detect, false);
-      gyroStillDetReset(&gyro_cal->gyro_stillness_detect, false);
-      gyroStillDetReset(&gyro_cal->mag_stillness_detect, false);
+      gyroStillDetReset(&gyro_cal->accel_stillness_detect,
+                        /*reset_stats=*/false);
+      gyroStillDetReset(&gyro_cal->gyro_stillness_detect,
+                        /*reset_stats=*/false);
+      gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/false);
 
       // Update stillness flag.
       gyro_cal->prev_still = true;
@@ -318,7 +443,8 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time) {
     // "too short", then do a calibration with the data accumulated thus far.
     stillness_duration_too_short =
         ((gyro_cal->gyro_stillness_detect.window_start_time -
-          gyro_cal->start_still_time) < gyro_cal->min_still_duration);
+          gyro_cal->start_still_time_nanos) <
+         gyro_cal->min_still_duration_nanos);
 
     if (gyro_cal->prev_still && !stillness_duration_too_short) {
       computeGyroCal(gyro_cal,
@@ -326,28 +452,29 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time) {
     }
 
     // Reset stillness detectors and the stats.
-    gyroStillDetReset(&gyro_cal->accel_stillness_detect, true);
-    gyroStillDetReset(&gyro_cal->gyro_stillness_detect, true);
-    gyroStillDetReset(&gyro_cal->mag_stillness_detect, true);
+    gyroStillDetReset(&gyro_cal->accel_stillness_detect, /*reset_stats=*/true);
+    gyroStillDetReset(&gyro_cal->gyro_stillness_detect, /*reset_stats=*/true);
+    gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/true);
+
+#ifdef GYRO_CAL_DBG_ENABLED
+    // Reset the temperature statistics and sampling rate estimate.
+    gyroTempUpdateStats(NULL, 0, /*reset_stats=*/true);
+    gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
+#endif  // GYRO_CAL_DBG_ENABLED
 
     // Update stillness flag.
     gyro_cal->prev_still = false;
   }
 
   // Reset the watchdog timer after we have processed data.
-  gyro_cal->gyro_watchdog_start = sample_time;
-
-#ifdef GYRO_CAL_DBG_ENABLED
-  // Debug information available.
-  gyro_cal->debug_processed_data_available = true;
-  gyro_cal->debug_processed_data_time = sample_time;
-#endif
+  gyro_cal->gyro_watchdog_start_nanos = sample_time_nanos;
 }
 
 // Calculates a new gyro bias offset calibration value.
-void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time) {
+void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) {
   // Current calibration duration.
-  uint64_t cur_cal_dur = calibration_time - gyro_cal->start_still_time;
+  uint64_t cur_cal_dur_nanos =
+      calibration_time_nanos - gyro_cal->start_still_time_nanos;
 
   // Check to see if new calibration values is within acceptable range.
   if (!(gyro_cal->gyro_stillness_detect.prev_mean_x < MAX_GYRO_BIAS &&
@@ -356,6 +483,15 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time) {
         gyro_cal->gyro_stillness_detect.prev_mean_y > -MAX_GYRO_BIAS &&
         gyro_cal->gyro_stillness_detect.prev_mean_z < MAX_GYRO_BIAS &&
         gyro_cal->gyro_stillness_detect.prev_mean_z > -MAX_GYRO_BIAS)) {
+#ifdef GYRO_CAL_DBG_ENABLED
+    CAL_DEBUG_LOG(
+        "[GYRO_CAL:WARNING]",
+        "Rejected Bias Update [mdps]: %s%d.%06d, %s%d.%06d, %s%d.%06d",
+        CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6),
+        CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6),
+        CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6));
+#endif  // GYRO_CAL_DBG_ENABLED
+
     // Outside of range. Ignore, reset, and continue.
     return;
   }
@@ -372,10 +508,10 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time) {
       gyro_cal->mag_stillness_detect.prev_stillness_confidence;
 
   // Store calibration stillness duration.
-  gyro_cal->calibration_time_duration = cur_cal_dur;
+  gyro_cal->calibration_time_duration_nanos = cur_cal_dur_nanos;
 
   // Store calibration time stamp.
-  gyro_cal->calibration_time = calibration_time;
+  gyro_cal->calibration_time_nanos = calibration_time_nanos;
 
   // Set flag to indicate a new gyro calibration value is available.
   gyro_cal->new_gyro_cal_available = true;
@@ -384,62 +520,42 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time) {
   // Increment the total count of calibration updates.
   gyro_cal->debug_calibration_count++;
 
-  // Store the last 'DEBUG_GYRO_SHORTTERM_NUM_CAL' calibration debug data
-  // in a circular buffer, 'debug_cal_data[]'. 'debug_head' is the index
-  // of the last valid calibration.
-  gyro_cal->debug_head++;
-  if (gyro_cal->debug_head >= DEBUG_GYRO_SHORTTERM_NUM_CAL) {
-    gyro_cal->debug_head = 0;
-  }
-  if (gyro_cal->debug_num_cals < DEBUG_GYRO_SHORTTERM_NUM_CAL) {
-    gyro_cal->debug_num_cals++;
-  }
-
   // Update the calibration debug information.
-  gyroCalUpdateDebug(gyro_cal, &gyro_cal->debug_cal_data[gyro_cal->debug_head]);
-
-  // Improve the collection of historic calibrations. Limit frequency to
-  // every N hours.
-  if ((gyro_cal->debug_num_cals_hist <= 0) ||
-      (calibration_time -
-       gyro_cal->debug_cal_data_hist[gyro_cal->debug_head_hist]
-           .calibration_time) >= DEBUG_GYRO_CAL_LIMIT) {
-    gyro_cal->debug_head_hist++;
-    if (gyro_cal->debug_head_hist >= DEBUG_GYRO_LONGTERM_NUM_CAL) {
-      gyro_cal->debug_head_hist = 0;
-    }
-    if (gyro_cal->debug_num_cals_hist < DEBUG_GYRO_LONGTERM_NUM_CAL) {
-      gyro_cal->debug_num_cals_hist++;
-    }
+  gyroCalUpdateDebug(gyro_cal);
 
-    // Update the calibration debug information.
-    gyroCalUpdateDebug(
-        gyro_cal, &gyro_cal->debug_cal_data_hist[gyro_cal->debug_head_hist]);
-  }
+  // Trigger a printout of the debug information.
+  gyro_cal->debug_print_trigger = true;
 #endif
 }
 
 // Check for a watchdog timeout condition.
-void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time) {
+void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) {
   bool watchdog_timeout;
 
   // Check for initialization of the watchdog time (=0).
-  if (gyro_cal->gyro_watchdog_start <= 0) {
+  if (gyro_cal->gyro_watchdog_start_nanos <= 0) {
     return;
   }
 
   // Check for timeout condition of watchdog.
-  watchdog_timeout = ((sample_time - gyro_cal->gyro_watchdog_start) >
-                      gyro_cal->gyro_watchdog_timeout_duration);
+  watchdog_timeout =
+      ((sample_time_nanos - gyro_cal->gyro_watchdog_start_nanos) >
+       gyro_cal->gyro_watchdog_timeout_duration_nanos);
 
   // If a timeout occurred then reset to known good state.
   if (watchdog_timeout) {
     // Reset stillness detectors and restart data capture.
-    gyroStillDetReset(&gyro_cal->accel_stillness_detect, true);
-    gyroStillDetReset(&gyro_cal->gyro_stillness_detect, true);
-    gyroStillDetReset(&gyro_cal->mag_stillness_detect, true);
+    gyroStillDetReset(&gyro_cal->accel_stillness_detect, /*reset_stats=*/true);
+    gyroStillDetReset(&gyro_cal->gyro_stillness_detect, /*reset_stats=*/true);
+    gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/true);
     gyro_cal->mag_stillness_detect.stillness_confidence = 0;
-    gyro_cal->stillness_win_endtime = 0;
+    gyro_cal->stillness_win_endtime_nanos = 0;
+
+#ifdef GYRO_CAL_DBG_ENABLED
+    // Reset the temperature statistics and sampling rate estimate.
+    gyroTempUpdateStats(NULL, 0, /*reset_stats=*/true);
+    gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
+#endif  // GYRO_CAL_DBG_ENABLED
 
     // Force stillness confidence to zero.
     gyro_cal->accel_stillness_detect.prev_stillness_confidence = 0;
@@ -457,340 +573,596 @@ void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time) {
 
     // Assert watchdog timeout flags.
     gyro_cal->gyro_watchdog_timeout |= watchdog_timeout;
-    gyro_cal->gyro_watchdog_start = 0;
+    gyro_cal->gyro_watchdog_start_nanos = 0;
 #ifdef GYRO_CAL_DBG_ENABLED
     gyro_cal->debug_watchdog_count++;
+    CAL_DEBUG_LOG("[GYRO_CAL:WATCHDOG]", "Total#, Timestamp [nsec]: %lu, %llu",
+                  (unsigned long int)gyro_cal->debug_watchdog_count,
+                  (unsigned long long int)sample_time_nanos);
 #endif
   }
 }
 
 #ifdef GYRO_CAL_DBG_ENABLED
-// Update the calibration debug information.
-void gyroCalUpdateDebug(struct GyroCal* gyro_cal,
-                        struct DebugGyroCal* debug_gyro_cal) {
+void gyroCalUpdateDebug(struct GyroCal* gyro_cal) {
   // Probability of stillness (acc, rot, still), duration, timestamp.
-  debug_gyro_cal->accel_stillness_conf =
+  gyro_cal->debug_gyro_cal.accel_stillness_conf =
       gyro_cal->accel_stillness_detect.prev_stillness_confidence;
-  debug_gyro_cal->gyro_stillness_conf =
+  gyro_cal->debug_gyro_cal.gyro_stillness_conf =
       gyro_cal->gyro_stillness_detect.prev_stillness_confidence;
-  debug_gyro_cal->mag_stillness_conf =
+  gyro_cal->debug_gyro_cal.mag_stillness_conf =
       gyro_cal->mag_stillness_detect.prev_stillness_confidence;
 
   // Magnetometer usage.
-  debug_gyro_cal->used_mag_sensor = gyro_cal->using_mag_sensor;
+  gyro_cal->debug_gyro_cal.using_mag_sensor = gyro_cal->using_mag_sensor;
 
   // Temperature at calibration time.
-  debug_gyro_cal->temperature_celcius = gyro_cal->latest_temperature_celcius;
-
-  // Calibration time and stillness duration.
-  debug_gyro_cal->calibration_time_duration =
-      gyro_cal->calibration_time_duration;
-  debug_gyro_cal->calibration_time = gyro_cal->calibration_time;
-
-  // Record the current calibration values
-  debug_gyro_cal->calibration[0] = gyro_cal->bias_x;
-  debug_gyro_cal->calibration[1] = gyro_cal->bias_y;
-  debug_gyro_cal->calibration[2] = gyro_cal->bias_z;
-
-  // Record the previous window means
-  debug_gyro_cal->accel_mean[0] = gyro_cal->accel_stillness_detect.prev_mean_x;
-  debug_gyro_cal->accel_mean[1] = gyro_cal->accel_stillness_detect.prev_mean_y;
-  debug_gyro_cal->accel_mean[2] = gyro_cal->accel_stillness_detect.prev_mean_z;
-
-  debug_gyro_cal->gyro_mean[0] = gyro_cal->gyro_stillness_detect.prev_mean_x;
-  debug_gyro_cal->gyro_mean[1] = gyro_cal->gyro_stillness_detect.prev_mean_y;
-  debug_gyro_cal->gyro_mean[2] = gyro_cal->gyro_stillness_detect.prev_mean_z;
-
-  debug_gyro_cal->mag_mean[0] = gyro_cal->mag_stillness_detect.prev_mean_x;
-  debug_gyro_cal->mag_mean[1] = gyro_cal->mag_stillness_detect.prev_mean_y;
-  debug_gyro_cal->mag_mean[2] = gyro_cal->mag_stillness_detect.prev_mean_z;
-
-  // Record the variance data.
-  debug_gyro_cal->accel_var[0] = gyro_cal->accel_stillness_detect.win_var_x;
-  debug_gyro_cal->accel_var[1] = gyro_cal->accel_stillness_detect.win_var_y;
-  debug_gyro_cal->accel_var[2] = gyro_cal->accel_stillness_detect.win_var_z;
-
-  debug_gyro_cal->gyro_var[0] = gyro_cal->gyro_stillness_detect.win_var_x;
-  debug_gyro_cal->gyro_var[1] = gyro_cal->gyro_stillness_detect.win_var_y;
-  debug_gyro_cal->gyro_var[2] = gyro_cal->gyro_stillness_detect.win_var_z;
-
-  debug_gyro_cal->mag_var[0] = gyro_cal->mag_stillness_detect.win_var_x;
-  debug_gyro_cal->mag_var[1] = gyro_cal->mag_stillness_detect.win_var_y;
-  debug_gyro_cal->mag_var[2] = gyro_cal->mag_stillness_detect.win_var_z;
+  gyro_cal->debug_gyro_cal.temperature_celcius =
+      gyro_cal->latest_temperature_celcius;
+
+  // Stillness start, stop, and duration times.
+  gyro_cal->debug_gyro_cal.start_still_time_nanos =
+      gyro_cal->start_still_time_nanos;
+  gyro_cal->debug_gyro_cal.end_still_time_nanos =
+      gyro_cal->calibration_time_nanos;
+  gyro_cal->debug_gyro_cal.stillness_duration_nanos =
+      gyro_cal->calibration_time_duration_nanos;
+
+  // Records the current calibration values.
+  gyro_cal->debug_gyro_cal.calibration[0] = gyro_cal->bias_x;
+  gyro_cal->debug_gyro_cal.calibration[1] = gyro_cal->bias_y;
+  gyro_cal->debug_gyro_cal.calibration[2] = gyro_cal->bias_z;
+
+  // Records the complete temperature statistics.
+  gyroTempUpdateStats(&gyro_cal->debug_gyro_cal.debug_temperature, 0,
+                      /*reset_stats=*/true);
+  gyroSamplingRateUpdate(&gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0,
+                         /*reset_stats=*/true);
+
+  // Records the previous window means.
+  gyro_cal->debug_gyro_cal.accel_mean[0] =
+      gyro_cal->accel_stillness_detect.prev_mean_x;
+  gyro_cal->debug_gyro_cal.accel_mean[1] =
+      gyro_cal->accel_stillness_detect.prev_mean_y;
+  gyro_cal->debug_gyro_cal.accel_mean[2] =
+      gyro_cal->accel_stillness_detect.prev_mean_z;
+
+  gyro_cal->debug_gyro_cal.gyro_mean[0] =
+      gyro_cal->gyro_stillness_detect.prev_mean_x;
+  gyro_cal->debug_gyro_cal.gyro_mean[1] =
+      gyro_cal->gyro_stillness_detect.prev_mean_y;
+  gyro_cal->debug_gyro_cal.gyro_mean[2] =
+      gyro_cal->gyro_stillness_detect.prev_mean_z;
+
+  gyro_cal->debug_gyro_cal.mag_mean[0] =
+      gyro_cal->mag_stillness_detect.prev_mean_x;
+  gyro_cal->debug_gyro_cal.mag_mean[1] =
+      gyro_cal->mag_stillness_detect.prev_mean_y;
+  gyro_cal->debug_gyro_cal.mag_mean[2] =
+      gyro_cal->mag_stillness_detect.prev_mean_z;
+
+  // Records the variance data.
+  gyro_cal->debug_gyro_cal.accel_var[0] =
+      gyro_cal->accel_stillness_detect.win_var_x;
+  gyro_cal->debug_gyro_cal.accel_var[1] =
+      gyro_cal->accel_stillness_detect.win_var_y;
+  gyro_cal->debug_gyro_cal.accel_var[2] =
+      gyro_cal->accel_stillness_detect.win_var_z;
+
+  gyro_cal->debug_gyro_cal.gyro_var[0] =
+      gyro_cal->gyro_stillness_detect.win_var_x;
+  gyro_cal->debug_gyro_cal.gyro_var[1] =
+      gyro_cal->gyro_stillness_detect.win_var_y;
+  gyro_cal->debug_gyro_cal.gyro_var[2] =
+      gyro_cal->gyro_stillness_detect.win_var_z;
+
+  gyro_cal->debug_gyro_cal.mag_var[0] =
+      gyro_cal->mag_stillness_detect.win_var_x;
+  gyro_cal->debug_gyro_cal.mag_var[1] =
+      gyro_cal->mag_stillness_detect.win_var_y;
+  gyro_cal->debug_gyro_cal.mag_var[2] =
+      gyro_cal->mag_stillness_detect.win_var_z;
 }
 
-// Helper function to print debug statements.
-void gyroCalDebugPrintData(int count, struct DebugGyroCal* debug_data) {
-  CAL_DEBUG_LOG(
-      "[GYRO_CAL]",
-      "#%d Gyro Bias Calibration = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [mdps]\n",
-      count, CAL_ENCODE_FLOAT(debug_data->calibration[0] * RAD_TO_MDEG, 6),
-      CAL_ENCODE_FLOAT(debug_data->calibration[1] * RAD_TO_MDEG, 6),
-      CAL_ENCODE_FLOAT(debug_data->calibration[2] * RAD_TO_MDEG, 6));
-
-  if (debug_data->used_mag_sensor) {
-    CAL_DEBUG_LOG("[GYRO_CAL]", "   Using Magnetometer.\n");
-  } else {
-    CAL_DEBUG_LOG("[GYRO_CAL]", "   Not Using Magnetometer.\n");
+void gyroTempUpdateStats(struct DebugTemperature* debug_temperature,
+                         float temperature, bool reset_stats) {
+  // Using the method of the assumed mean to preserve some numerical stability
+  // while avoiding per-sample divisions that the more numerically stable
+  // Welford method would afford.
+
+  // Reference for the numerical method used below to compute the online mean
+  // and variance statistics:
+  //   1). en.wikipedia.org/wiki/assumed_mean
+
+  // This is used for local calculations of temperature statistics.
+  static struct DebugTemperature local_temperature_stats = {0};
+  static bool set_assumed_mean = true;
+
+  // If 'debug_temperature' is not NULL then this function just reads out the
+  // current statistics, resets, and returns.
+  if (debug_temperature) {
+    if (local_temperature_stats.num_temperature_samples > 1) {
+      // Computes the final calculation of temperature sensor mean and variance.
+      float tmp = local_temperature_stats.temperature_mean_celsius;
+      local_temperature_stats.temperature_mean_celsius /=
+          local_temperature_stats.num_temperature_samples;
+      local_temperature_stats.temperature_var_celsius =
+          (local_temperature_stats.temperature_var_celsius -
+           local_temperature_stats.temperature_mean_celsius * tmp) /
+          (local_temperature_stats.num_temperature_samples - 1);
+
+      // Adds the assumed mean value back to the total mean calculation.
+      local_temperature_stats.temperature_mean_celsius +=
+          local_temperature_stats.assumed_mean;
+    } else {
+      // Not enough samples to compute a valid variance. Indicate this with a -1
+      // value.
+      local_temperature_stats.temperature_var_celsius = -1.0f;
+    }
+
+    memcpy(debug_temperature, &local_temperature_stats,
+           sizeof(struct DebugTemperature));
+    reset_stats = true;
   }
 
-  CAL_DEBUG_LOG("[GYRO_CAL]", "   Temperature = %s%d.%06d [C]\n",
-                CAL_ENCODE_FLOAT(debug_data->temperature_celcius, 6));
+  // Resets the temperature statistics and returns.
+  if (reset_stats) {
+    local_temperature_stats.num_temperature_samples = 0;
+    local_temperature_stats.temperature_mean_celsius = 0.0f;
+    local_temperature_stats.temperature_var_celsius = 0.0f;
+    set_assumed_mean = true;  // Sets flag.
+
+    // Initialize the min/max temperatures values.
+    local_temperature_stats.temperature_min_max_celsius[0] = FLT_MAX;
+    local_temperature_stats.temperature_min_max_celsius[1] =
+        -1.0f * (FLT_MAX - 1.0f);
+    return;
+  }
 
-  CAL_DEBUG_LOG("[GYRO_CAL]", "   Calibration Timestamp = %llu [nsec]\n",
-                debug_data->calibration_time);
+  // The first sample received is taken as the "assumed mean".
+  if (set_assumed_mean) {
+    local_temperature_stats.assumed_mean = temperature;
+    set_assumed_mean = false;  // Resets flag.
+  }
 
-  CAL_DEBUG_LOG("[GYRO_CAL]", "   Stillness Duration = %llu [nsec]\n",
-                debug_data->calibration_time_duration);
+  // Increments the number of samples.
+  local_temperature_stats.num_temperature_samples++;
 
-  CAL_DEBUG_LOG("[GYRO_CAL]",
-                "   Accel Mean = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [g]\n",
-                CAL_ENCODE_FLOAT(debug_data->accel_mean[0] * GRAVITY_TO_G, 6),
-                CAL_ENCODE_FLOAT(debug_data->accel_mean[1] * GRAVITY_TO_G, 6),
-                CAL_ENCODE_FLOAT(debug_data->accel_mean[2] * GRAVITY_TO_G, 6));
+  // Online computation of mean and variance for the running stillness period.
+  float delta = (temperature - local_temperature_stats.assumed_mean);
+  local_temperature_stats.temperature_var_celsius += delta * delta;
+  local_temperature_stats.temperature_mean_celsius += delta;
 
-  CAL_DEBUG_LOG("[GYRO_CAL]",
-                "   Mag Mean = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [uT]\n",
-                CAL_ENCODE_FLOAT(debug_data->mag_mean[0], 6),
-                CAL_ENCODE_FLOAT(debug_data->mag_mean[1], 6),
-                CAL_ENCODE_FLOAT(debug_data->mag_mean[2], 6));
+  // Track the min and max temperature values.
+  if (local_temperature_stats.temperature_min_max_celsius[0] > temperature) {
+    local_temperature_stats.temperature_min_max_celsius[0] = temperature;
+  }
+  if (local_temperature_stats.temperature_min_max_celsius[1] < temperature) {
+    local_temperature_stats.temperature_min_max_celsius[1] = temperature;
+  }
 }
 
-// Print Debug data useful for tuning the stillness detectors.
-void gyroCalTuneDebugPrint(struct GyroCal* gyro_cal, uint64_t sample_time) {
-  static uint64_t start_time = 0;
+void gyroSamplingRateUpdate(float* debug_mean_sampling_rate_hz,
+                            uint64_t timestamp_nanos, bool reset_stats) {
+  // This is used for local calculations of average sampling rate.
+  static uint64_t last_timestamp_nanos = 0;
+  static uint64_t time_delta_accumulator = 0;
+  static size_t num_samples = 0;
+
+  // If 'debug_mean_sampling_rate_hz' is not NULL then this function just reads
+  // out the estimate of the sampling rate.
+  if (debug_mean_sampling_rate_hz) {
+    if (num_samples > 1 && time_delta_accumulator > 0) {
+      // Computes the final mean calculation.
+      *debug_mean_sampling_rate_hz =
+          num_samples /
+          (floatFromUint64(time_delta_accumulator) * NANOS_TO_SEC);
+    } else {
+      // Not enough samples to compute a valid sample rate estimate. Indicate
+      // this with a -1 value.
+      *debug_mean_sampling_rate_hz = -1.0f;
+    }
+    reset_stats = true;
+  }
 
-  // Output sensor variance levels to assist with tuning thresholds.
-  //   i.  Within the first 60 seconds of boot: output interval = 5 seconds.
-  //   ii. Thereafter: output interval is set by DEBUG_GYRO_TUNE_UPDATES.
-  bool condition_i = ((sample_time <= 60000000000) &&
-                      ((sample_time - start_time) > 5000000000));  // nsec
-  bool condition_ii = ((sample_time > 60000000000) &&
-                       ((sample_time - start_time) > DEBUG_GYRO_TUNE_UPDATES));
-
-  if (condition_i || condition_ii) {
-    CAL_DEBUG_LOG("[GYRO_CAL]", "");
-    CAL_DEBUG_LOG(
-        "[GYRO_CAL]",
-        "#%lu Gyro Bias Calibration = {%s%d.%06d, %s%d.%06d, %s%d.%06d} "
-        "[mdps]\n",
-        gyro_cal->debug_calibration_count,
-        CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 6),
-        CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 6),
-        CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 6));
-    CAL_DEBUG_LOG(
-        "[GYRO_CAL]",
-        "   Gyro Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} [rad/sec]^2\n",
-        CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_x, 8),
-        CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_y, 8),
-        CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_z, 8));
-    CAL_DEBUG_LOG(
-        "[GYRO_CAL]",
-        "   Accel Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} [m/sec^2]^2\n",
-        CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_x, 8),
-        CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_y, 8),
-        CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_z, 8));
-    if (gyro_cal->using_mag_sensor) {
+  // Resets the sampling rate mean estimator data if:
+  //   1. The 'reset_stats' flag is set.
+  //   2. A bad timestamp was received (i.e., time not monotonic).
+  //   3. 'last_timestamp_nanos' is zero.
+  if (reset_stats || (timestamp_nanos <= last_timestamp_nanos) ||
+      last_timestamp_nanos == 0) {
+    last_timestamp_nanos = timestamp_nanos;
+    time_delta_accumulator = 0;
+    num_samples = 0;
+    return;
+  }
+
+  // Increments the number of samples.
+  num_samples++;
+
+  // Accumulate the time steps.
+  time_delta_accumulator += timestamp_nanos - last_timestamp_nanos;
+  last_timestamp_nanos = timestamp_nanos;
+}
+
+void gyroCalDebugPrintData(const struct DebugGyroCal* debug_data,
+                           enum DebugPrintData print_data) {
+  // Prints out the desired debug data.
+  float mag_data;
+  switch (print_data) {
+    case BIAS_CAL:
       CAL_DEBUG_LOG(
-          "[GYRO_CAL]",
-          "   Mag Variance = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [uT]^2\n",
-          CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_x, 6),
-          CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_y, 6),
-          CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_z, 6));
+          "[GYRO_CAL:BIAS]",
+          "Gyro Bias Calibration [mdps]: %s%d.%08d, %s%d.%08d, %s%d.%08d",
+          CAL_ENCODE_FLOAT(debug_data->calibration[0] * RAD_TO_MILLI_DEGREES,
+                           8),
+          CAL_ENCODE_FLOAT(debug_data->calibration[1] * RAD_TO_MILLI_DEGREES,
+                           8),
+          CAL_ENCODE_FLOAT(debug_data->calibration[2] * RAD_TO_MILLI_DEGREES,
+                           8));
+      break;
+
+    case CAL_TIME:
+      CAL_DEBUG_LOG("[GYRO_CAL:TIME]", "Stillness Start Time [nsec]: %llu",
+                    (unsigned long long int)debug_data->start_still_time_nanos);
+
+      CAL_DEBUG_LOG("[GYRO_CAL:TIME]", "Stillness End Time [nsec]: %llu",
+                    (unsigned long long int)debug_data->end_still_time_nanos);
+
       CAL_DEBUG_LOG(
-          "[GYRO_CAL]", "   Stillness = {G%s%d.%06d, A%s%d.%06d, M%s%d.%06d}\n",
+          "[GYRO_CAL:TIME]", "Stillness Duration [nsec]: %llu",
+          (unsigned long long int)debug_data->stillness_duration_nanos);
+      break;
+
+    case ACCEL_STATS:
+      CAL_DEBUG_LOG("[GYRO_CAL:ACCEL_STATS]",
+                    "Accel Mean [m/sec^2]: %s%d.%08d, %s%d.%08d, %s%d.%08d",
+                    CAL_ENCODE_FLOAT(debug_data->accel_mean[0], 8),
+                    CAL_ENCODE_FLOAT(debug_data->accel_mean[1], 8),
+                    CAL_ENCODE_FLOAT(debug_data->accel_mean[2], 8));
+      CAL_DEBUG_LOG(
+          "[GYRO_CAL:ACCEL_STATS]",
+          "Accel Variance [(m/sec^2)^2]: %s%d.%08d, %s%d.%08d, %s%d.%08d",
+          CAL_ENCODE_FLOAT(debug_data->accel_var[0], 8),
+          CAL_ENCODE_FLOAT(debug_data->accel_var[1], 8),
+          CAL_ENCODE_FLOAT(debug_data->accel_var[2], 8));
+      break;
+
+    case GYRO_STATS:
+      CAL_DEBUG_LOG(
+          "[GYRO_CAL:GYRO_STATS]",
+          "Gyro Mean [mdps]: %s%d.%08d, %s%d.%08d, %s%d.%08d",
+          CAL_ENCODE_FLOAT(debug_data->gyro_mean[0] * RAD_TO_MILLI_DEGREES, 8),
+          CAL_ENCODE_FLOAT(debug_data->gyro_mean[1] * RAD_TO_MILLI_DEGREES, 8),
+          CAL_ENCODE_FLOAT(debug_data->gyro_mean[2] * RAD_TO_MILLI_DEGREES, 8));
+      CAL_DEBUG_LOG(
+          "[GYRO_CAL:GYRO_STATS]",
+          "Gyro Variance [(rad/sec)^2]: %s%d.%08d, %s%d.%08d, %s%d.%08d",
+          CAL_ENCODE_FLOAT(debug_data->gyro_var[0], 8),
+          CAL_ENCODE_FLOAT(debug_data->gyro_var[1], 8),
+          CAL_ENCODE_FLOAT(debug_data->gyro_var[2], 8));
+      break;
+
+    case MAG_STATS:
+      if (debug_data->using_mag_sensor) {
+        CAL_DEBUG_LOG("[GYRO_CAL:MAG_STATS]",
+                      "Mag Mean [uT]: %s%d.%08d, %s%d.%08d, %s%d.%08d",
+                      CAL_ENCODE_FLOAT(debug_data->mag_mean[0], 8),
+                      CAL_ENCODE_FLOAT(debug_data->mag_mean[1], 8),
+                      CAL_ENCODE_FLOAT(debug_data->mag_mean[2], 8));
+        CAL_DEBUG_LOG("[GYRO_CAL:MAG_STATS]",
+                      "Mag Variance [uT^2]: %s%d.%08d, %s%d.%08d, %s%d.%08d",
+                      CAL_ENCODE_FLOAT(debug_data->mag_var[0], 8),
+                      CAL_ENCODE_FLOAT(debug_data->mag_var[1], 8),
+                      CAL_ENCODE_FLOAT(debug_data->mag_var[2], 8));
+      } else {
+        CAL_DEBUG_LOG("[GYRO_CAL:MAG_STATS]", "Mag Mean [uT]: 0, 0, 0");
+        // The -1's indicate that the magnetometer sensor was not used.
+        CAL_DEBUG_LOG("[GYRO_CAL:MAG_STATS]",
+                      "Mag Variance [uT^2]: -1.0, -1.0, -1.0");
+      }
+      break;
+
+    case TEMP_STATS:
+      CAL_DEBUG_LOG("[GYRO_CAL:TEMP_STATS]",
+                    "Latest Temperature [C]: %s%d.%08d",
+                    CAL_ENCODE_FLOAT(debug_data->temperature_celcius, 8));
+      CAL_DEBUG_LOG(
+          "[GYRO_CAL:TEMP_STATS]",
+          "Min/Max Temperature [C]: %s%d.%08d, %s%d.%08d",
           CAL_ENCODE_FLOAT(
-              gyro_cal->gyro_stillness_detect.stillness_confidence, 6),
+              debug_data->debug_temperature.temperature_min_max_celsius[0], 8),
           CAL_ENCODE_FLOAT(
-              gyro_cal->accel_stillness_detect.stillness_confidence, 6),
-          CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.stillness_confidence,
-                            6));
-    } else {
-      CAL_DEBUG_LOG("[GYRO_CAL]", "   Mag Variance = {---, ---, ---} [uT]^2\n");
+              debug_data->debug_temperature.temperature_min_max_celsius[1], 8));
       CAL_DEBUG_LOG(
-          "[GYRO_CAL]", "   Stillness = {G%s%d.%06d, A%s%d.%06d, M---}\n",
+          "[GYRO_CAL:TEMP_STATS]", "Temperature Mean [C]: %s%d.%08d",
           CAL_ENCODE_FLOAT(
-              gyro_cal->gyro_stillness_detect.stillness_confidence, 6),
+              debug_data->debug_temperature.temperature_mean_celsius, 8));
+      CAL_DEBUG_LOG(
+          "[GYRO_CAL:TEMP_STATS]", "Temperature Variance [C^2]: %s%d.%08d",
           CAL_ENCODE_FLOAT(
-              gyro_cal->accel_stillness_detect.stillness_confidence, 6));
-    }
+              debug_data->debug_temperature.temperature_var_celsius, 8));
+      break;
 
-    CAL_DEBUG_LOG("[GYRO_CAL]", "   Temperature = %s%d.%06d [C]\n",
-                  CAL_ENCODE_FLOAT(gyro_cal->latest_temperature_celcius, 6));
-    CAL_DEBUG_LOG("[GYRO_CAL]", "   Timestamp = %llu [nsec]\n", sample_time);
-    CAL_DEBUG_LOG("[GYRO_CAL]", "   Total Gyro Calibrations: %lu\n",
-                  gyro_cal->debug_calibration_count);
-    start_time = sample_time;  // reset
-  }
-}
+    case STILLNESS_DATA:
+      mag_data = (debug_data->using_mag_sensor)
+                     ? debug_data->mag_stillness_conf
+                     : -1.0f;  // Signals that magnetometer was not used.
+      CAL_DEBUG_LOG("[GYRO_CAL:STILLNESS]",
+                    "Stillness [G/A/M]: %s%d.%08d, %s%d.%08d, %s%d.%08d",
+                    CAL_ENCODE_FLOAT(debug_data->gyro_stillness_conf, 8),
+                    CAL_ENCODE_FLOAT(debug_data->accel_stillness_conf, 8),
+                    CAL_ENCODE_FLOAT(mag_data, 8));
+      break;
 
-// Start the debug data report state machine.
-void gyroCalDebugPrintStart(struct GyroCal* gyro_cal) {
-  if (gyro_cal->gyro_debug_state < 0) {  // if currently in idle state.
-    gyro_cal->gyro_debug_state = 0;      // start printing.
+    case SAMPLING_RATE:
+      CAL_DEBUG_LOG("[GYRO_CAL:SAMPLING_RATE]",
+                    "Gyro Sampling Rate [Hz]: %s%d.%06d",
+                    CAL_ENCODE_FLOAT(
+                        debug_data->mean_sampling_rate_hz, 6));
+      break;
+
+    default:
+      break;
   }
 }
 
-// Print Debug data report.
-void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t sample_time) {
-  static int head_index = 0;
-  static uint64_t wait_timer = 0;
-  static int i = 0;
-  static int next_state = 0;
-
-  // State machine to control reporting out debug print data.
-  switch (gyro_cal->gyro_debug_state) {
-    case 0:
-      // STATE 0 : Print out header
-      if (gyro_cal->debug_num_cals == 0) {
+// Debug printout state enumeration.
+enum GyroCalDebugState {
+  IDLE = 0,
+  WAIT_STATE,
+  PRINT_BIAS,
+  PRINT_TIME,
+  PRINT_TEMP,
+  PRINT_ACCEL,
+  PRINT_GYRO,
+  PRINT_MAG,
+  PRINT_STILLNESS,
+  PRINT_SAMPLING_RATE
+};
+
+void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t timestamp_nanos) {
+  static enum GyroCalDebugState debug_state = IDLE;
+  static enum GyroCalDebugState next_state = IDLE;
+  static uint64_t wait_timer_nanos = 0;
+
+  // This is a state machine that controls the reporting out of debug data.
+  switch (debug_state) {
+    case IDLE:
+      // Wait for a trigger and start the debug printout sequence.
+      if (gyro_cal->debug_print_trigger) {
+        debug_state = PRINT_BIAS;
         CAL_DEBUG_LOG("[GYRO_CAL]", "");
-        CAL_DEBUG_LOG("[GYRO_CAL]", "No Gyro Calibrations To Report.\n");
-        CAL_DEBUG_LOG(
-            "[GYRO_CAL]",
-            "#0 Gyro Bias Calibration = {%s%d.%06d, %s%d.%06d, %s%d.%06d} "
-            "[mdps]\n",
-            CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 6),
-            CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 6),
-            CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 6));
-        wait_timer = sample_time;        // start the wait timer.
-        gyro_cal->gyro_debug_state = 7;  // go to next state.
+        gyro_cal->debug_print_trigger = false;  // Resets trigger.
       } else {
-        CAL_DEBUG_LOG("[GYRO_CAL]", "");
-        CAL_DEBUG_LOG("[GYRO_CAL]",
-                      "---DEBUG REPORT-------------------------------------\n");
-        CAL_DEBUG_LOG("[GYRO_CAL]",
-                      "Gyro Calibrations To Report (newest to oldest): %d\n",
-                      gyro_cal->debug_num_cals);
-        head_index = gyro_cal->debug_head;
-        i = 1;                           // initialize report count
-        wait_timer = sample_time;        // start the wait timer.
-        next_state = 1;                  // set the next state.
-        gyro_cal->gyro_debug_state = 2;  // but first, go to wait state.
+        debug_state = IDLE;
       }
       break;
 
-    case 1:
-      // STATE 1: Print out past N recent calibrations.
-      if (i <= gyro_cal->debug_num_cals) {
-        // Print the data.
-        gyroCalDebugPrintData(i, &gyro_cal->debug_cal_data[head_index]);
-
-        // Move to the previous calibration data set.
-        head_index--;
-        if (head_index < 0) {
-          head_index = DEBUG_GYRO_SHORTTERM_NUM_CAL - 1;
-        }
-
-        // Increment the report count.
-        i++;
-
-        // Go to wait state.
-        wait_timer = sample_time;        // start the wait timer.
-        next_state = 1;                  // set the next state.
-        gyro_cal->gyro_debug_state = 2;  // but first, go to wait state.
-      } else {
-        // Go to wait state.
-        wait_timer = sample_time;        // start the wait timer.
-        next_state = 3;                  // set the next state.
-        gyro_cal->gyro_debug_state = 2;  // but first, go to wait state.
+    case WAIT_STATE:
+      // This helps throttle the print statements.
+      if ((timestamp_nanos - wait_timer_nanos) >= OVERTEMPCAL_WAIT_TIME_NANOS) {
+        debug_state = next_state;
       }
       break;
 
-    case 2:
-      // STATE 2 : Wait state.
-      // Wait for 500msec.
-      // This helps throttle the print statements.
-      if ((sample_time - wait_timer) >= 500000000) {
-        gyro_cal->gyro_debug_state = next_state;
-      }
+    case PRINT_BIAS:
+      CAL_DEBUG_LOG("[GYRO_CAL:BIAS]", "Total # Calibrations: %lu",
+                    (unsigned long int)gyro_cal->debug_calibration_count);
+      gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, BIAS_CAL);
+
+      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      next_state = PRINT_TIME;             // Sets the next state.
+      debug_state = WAIT_STATE;            // First, go to wait state.
       break;
 
-    case 3:
-      // STATE 3 : Print the end of the debug report.
-      CAL_DEBUG_LOG("[GYRO_CAL]", "Total Gyro Calibrations: %lu\n",
-                    gyro_cal->debug_calibration_count);
+    case PRINT_TIME:
+      gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, CAL_TIME);
 
-      CAL_DEBUG_LOG("[GYRO_CAL]", "Total Watchdog Timeouts: %lu\n",
-                    gyro_cal->debug_watchdog_count);
+      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      next_state = PRINT_TEMP;             // Sets the next state.
+      debug_state = WAIT_STATE;            // First, go to wait state.
+      break;
+
+    case PRINT_TEMP:
+      gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, TEMP_STATS);
 
-      CAL_DEBUG_LOG("[GYRO_CAL]",
-                    "---END DEBUG REPORT---------------------------------\n");
-      wait_timer = sample_time;        // start the wait timer.
-      next_state = 4;                  // set the next state.
-      gyro_cal->gyro_debug_state = 2;  // but first, go to wait state.
+      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      next_state = PRINT_ACCEL;            // Sets the next state.
+      debug_state = WAIT_STATE;            // First, go to wait state.
       break;
 
-    case 4:
-      // STATE 4 : Print out header (historic data)
-      CAL_DEBUG_LOG("[GYRO_CAL]", "");
-      CAL_DEBUG_LOG("[GYRO_CAL]",
-                    "---DEBUG REPORT (HISTORY)---------------------------\n");
-      CAL_DEBUG_LOG("[GYRO_CAL]",
-                    "Gyro Calibrations To Report (newest to oldest): %d\n",
-                    gyro_cal->debug_num_cals_hist);
-      head_index = gyro_cal->debug_head_hist;
-      i = 1;                           // initialize report count
-      wait_timer = sample_time;        // start the wait timer.
-      next_state = 5;                  // set the next state.
-      gyro_cal->gyro_debug_state = 2;  // but first, go to wait state.
+    case PRINT_ACCEL:
+      gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, ACCEL_STATS);
+
+      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      next_state = PRINT_GYRO;             // Sets the next state.
+      debug_state = WAIT_STATE;            // First, go to wait state.
       break;
 
-    case 5:
-      // STATE 5: Print out past N historic calibrations.
-      if (i <= gyro_cal->debug_num_cals_hist) {
-        // Print the data.
-        gyroCalDebugPrintData(i, &gyro_cal->debug_cal_data_hist[head_index]);
-
-        // Move to the previous calibration data set.
-        head_index--;
-        if (head_index < 0) {
-          head_index = DEBUG_GYRO_LONGTERM_NUM_CAL - 1;
-        }
-
-        // Increment the report count.
-        i++;
-
-        // Go to wait state.
-        wait_timer = sample_time;        // start the wait timer.
-        next_state = 5;                  // set the next state.
-        gyro_cal->gyro_debug_state = 2;  // but first, go to wait state.
+    case PRINT_GYRO:
+      gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, GYRO_STATS);
+
+      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      next_state = PRINT_MAG;              // Sets the next state.
+      debug_state = WAIT_STATE;            // First, go to wait state.
+      break;
+
+    case PRINT_MAG:
+      gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, MAG_STATS);
+
+      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      next_state = PRINT_STILLNESS;        // Sets the next state.
+      debug_state = WAIT_STATE;            // First, go to wait state.
+      break;
+
+    case PRINT_STILLNESS:
+      gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, STILLNESS_DATA);
+
+      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      next_state = PRINT_SAMPLING_RATE;    // Sets the next state.
+      debug_state = WAIT_STATE;            // First, go to wait state.
+      break;
+
+    case PRINT_SAMPLING_RATE:
+      gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, SAMPLING_RATE);
+      debug_state = IDLE;
+      break;
+
+    default:
+      // Sends this state machine to its idle state.
+      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      next_state = IDLE;                   // Sets the next state.
+      debug_state = WAIT_STATE;            // First, go to wait state.
+  }
+
+#ifdef GYRO_CAL_DBG_TUNE_ENABLED
+  if (debug_state == IDLE) {
+    // This check keeps the tuning printout from interleaving with the above
+    // debug print data.
+    gyroCalTuneDebugPrint(gyro_cal, timestamp_nanos);
+  }
+#endif  // GYRO_CAL_DBG_TUNE_ENABLED
+}
+
+#ifdef GYRO_CAL_DBG_TUNE_ENABLED
+void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal,
+                           uint64_t timestamp_nanos) {
+  static enum GyroCalDebugState debug_state = IDLE;
+  static enum GyroCalDebugState next_state = IDLE;
+  static uint64_t wait_timer_nanos = 0;
+
+  // Output sensor variance levels to assist with tuning thresholds.
+  //   i.  Within the first 180 seconds of boot: output interval = 5
+  //       seconds.
+  //   ii. Thereafter: output interval is 60 seconds.
+  bool condition_i =
+      ((timestamp_nanos <= 180000000000) &&
+       ((timestamp_nanos - wait_timer_nanos) > 5000000000));  // nsec
+  bool condition_ii = ((timestamp_nanos > 60000000000) &&
+                       ((timestamp_nanos - wait_timer_nanos) > 60000000000));
+
+  // This is a state machine that controls the reporting out of debug data.
+  switch (debug_state) {
+    case IDLE:
+      // Wait for a trigger and start the debug printout sequence.
+      if (condition_i || condition_ii) {
+        debug_state = PRINT_BIAS;
       } else {
-        // Go to wait state.
-        wait_timer = sample_time;        // start the wait timer.
-        next_state = 6;                  // set the next state.
-        gyro_cal->gyro_debug_state = 2;  // but first, go to wait state.
+        debug_state = IDLE;
       }
       break;
 
-    case 6:
-      // STATE 6 : Print the end of the debug report.
-      CAL_DEBUG_LOG("[GYRO_CAL]", "Total Gyro Calibrations: %lu\n",
-                    gyro_cal->debug_calibration_count);
+    case WAIT_STATE:
+      // This helps throttle the print statements.
+      if ((timestamp_nanos - wait_timer_nanos) >= OVERTEMPCAL_WAIT_TIME_NANOS) {
+        debug_state = next_state;
+      }
+      break;
+
+    case PRINT_BIAS:
+      CAL_DEBUG_LOG("[GYRO_CAL]", "");
+      CAL_DEBUG_LOG(
+          "[GYRO_CAL:TUNE]",
+          "#%lu Gyro Bias Calibration = {%s%d.%06d, %s%d.%06d, %s%d.%06d} "
+          "[mdps]\n",
+          (unsigned long int)gyro_cal->debug_calibration_count,
+          CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6),
+          CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6),
+          CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6));
+
+      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      next_state = PRINT_TIME;             // Sets the next state.
+      debug_state = WAIT_STATE;            // First, go to wait state.
+      break;
+
+    case PRINT_TIME:
+      CAL_DEBUG_LOG("[GYRO_CAL:TUNE]", "   Timestamp = %llu [nsec]\n",
+                    (unsigned long long int)timestamp_nanos);
+      CAL_DEBUG_LOG("[GYRO_CAL:TUNE]", "   Total Gyro Calibrations: %lu\n",
+                    (unsigned long int)gyro_cal->debug_calibration_count);
+
+      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      next_state = PRINT_TEMP;             // Sets the next state.
+      debug_state = WAIT_STATE;            // First, go to wait state.
+      break;
+
+    case PRINT_TEMP:
+      CAL_DEBUG_LOG("[GYRO_CAL:TUNE]", "   Temperature = %s%d.%06d [C]\n",
+                    CAL_ENCODE_FLOAT(gyro_cal->latest_temperature_celcius, 6));
 
-      CAL_DEBUG_LOG("[GYRO_CAL]", "Total Watchdog Timeouts: %lu\n",
-                    gyro_cal->debug_watchdog_count);
+      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      next_state = PRINT_ACCEL;            // Sets the next state.
+      debug_state = WAIT_STATE;            // First, go to wait state.
+      break;
 
-      CAL_DEBUG_LOG("[GYRO_CAL]",
-                    "---END DEBUG REPORT---------------------------------\n");
-      wait_timer = sample_time;        // start the wait timer.
-      gyro_cal->gyro_debug_state = 7;  // go to next state.
+    case PRINT_ACCEL:
+      CAL_DEBUG_LOG(
+          "[GYRO_CAL:TUNE]",
+          "   Accel Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} "
+          "[m/sec^2]^2\n",
+          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_x, 8),
+          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_y, 8),
+          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_z, 8));
+
+      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      next_state = PRINT_GYRO;             // Sets the next state.
+      debug_state = WAIT_STATE;            // First, go to wait state.
       break;
 
-    case 7:
-      // STATE 7 : Final wait state.
-      // Wait for 10 seconds.
-      // This helps throttle the print statements.
-      if ((sample_time - wait_timer) >= 10000000000) {
-        gyro_cal->gyro_debug_state = -1;
+    case PRINT_GYRO:
+      CAL_DEBUG_LOG(
+          "[GYRO_CAL:TUNE]",
+          "   Gyro Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} [rad/sec]^2\n",
+          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_x, 8),
+          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_y, 8),
+          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_z, 8));
+
+      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      next_state = PRINT_MAG;              // Sets the next state.
+      debug_state = WAIT_STATE;            // First, go to wait state.
+      break;
+
+    case PRINT_MAG:
+      if (gyro_cal->using_mag_sensor) {
+        CAL_DEBUG_LOG(
+            "[GYRO_CAL:TUNE]",
+            "   Mag Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} [uT]^2\n",
+            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_x, 8),
+            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_y, 8),
+            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_z, 8));
+        CAL_DEBUG_LOG(
+            "[GYRO_CAL:TUNE]",
+            "   Stillness = {G%s%d.%03d, A%s%d.%03d, M%s%d.%03d}\n",
+            CAL_ENCODE_FLOAT(
+                gyro_cal->gyro_stillness_detect.stillness_confidence, 3),
+            CAL_ENCODE_FLOAT(
+                gyro_cal->accel_stillness_detect.stillness_confidence, 3),
+            CAL_ENCODE_FLOAT(
+                gyro_cal->mag_stillness_detect.stillness_confidence, 3));
+      } else {
+        CAL_DEBUG_LOG("[GYRO_CAL:TUNE]",
+                      "   Mag Variance = {---, ---, ---} [uT]^2\n");
+        CAL_DEBUG_LOG(
+            "[GYRO_CAL:TUNE]",
+            "   Stillness = {G%s%d.%03d, A%s%d.%03d, M---}\n",
+            CAL_ENCODE_FLOAT(
+                gyro_cal->gyro_stillness_detect.stillness_confidence, 3),
+            CAL_ENCODE_FLOAT(
+                gyro_cal->accel_stillness_detect.stillness_confidence, 3));
       }
+
+      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      next_state = IDLE;                   // Sets the next state.
+      debug_state = WAIT_STATE;            // First, go to wait state.
       break;
 
     default:
-      // Idle state.
-      gyro_cal->gyro_debug_state = -1;
-
-      // Report periodic data useful for tuning the stillness detectors.
-      gyroCalTuneDebugPrint(gyro_cal, sample_time);
+      // Sends this state machine to its idle state.
+      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      next_state = IDLE;                   // Sets the next state.
+      debug_state = WAIT_STATE;            // First, go to wait state.
   }
 }
-#endif
+#endif  // GYRO_CAL_DBG_TUNE_ENABLED
+#endif  // GYRO_CAL_DBG_ENABLED
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.h b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
index f5a9109b..4b5a9c44 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
@@ -1,6 +1,3 @@
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_
-
 /*
  * Copyright (C) 2016 The Android Open Source Project
  *
@@ -17,7 +14,6 @@
  * limitations under the License.
  */
 
-///////////////////////////////////////////////////////////////
 /*
  * This module contains the algorithms for producing a
  * gyroscope offset calibration.  The algorithm looks
@@ -41,8 +37,14 @@
  * Optional Sensors and Units:
  *       - Magnetometer  [micro-Tesla, uT]
  *       - Temperature   [Celcius]
+ *
+ * #define GYRO_CAL_DBG_ENABLED to enable debug printout statements.
+ * #define GYRO_CAL_DBG_TUNE_ENABLED to periodically printout sensor variance
+ * data to assist in tuning the GyroCal parameters.
  */
-///////////////////////////////////////////////////////////////
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_
 
 #include "calibration/gyroscope/gyro_stillness_detect.h"
 
@@ -51,21 +53,26 @@ extern "C" {
 #endif
 
 #ifdef GYRO_CAL_DBG_ENABLED
-// Debug: Number of past calibrations to store.
-#define DEBUG_GYRO_SHORTTERM_NUM_CAL 30
-#define DEBUG_GYRO_LONGTERM_NUM_CAL 30
-#define DEBUG_GYRO_CAL_LIMIT 7200000000000    // 2hours [nsec]
-#define DEBUG_GYRO_TUNE_UPDATES 120000000000  // 120sec [nsec]
+// Temperature debug statistics.
+struct DebugTemperature {
+  size_t num_temperature_samples;
+  float temperature_min_max_celsius[2];
+  float temperature_mean_celsius;
+  float temperature_var_celsius;
+  float assumed_mean;
+};
 
 // Gyro Cal debug information/data tracking structure.
 struct DebugGyroCal {
+  struct DebugTemperature debug_temperature;
+  uint64_t start_still_time_nanos;
+  uint64_t end_still_time_nanos;
+  uint64_t stillness_duration_nanos;
+  float mean_sampling_rate_hz;
   float temperature_celcius;
   float accel_stillness_conf;
   float gyro_stillness_conf;
   float mag_stillness_conf;
-  uint64_t calibration_time;
-  uint64_t calibration_time_duration;
-  bool used_mag_sensor;
   float calibration[3];
   float accel_mean[3];
   float gyro_mean[3];
@@ -73,6 +80,7 @@ struct DebugGyroCal {
   float accel_var[3];
   float gyro_var[3];
   float mag_var[3];
+  bool using_mag_sensor;
 };
 #endif
 
@@ -82,13 +90,6 @@ struct GyroCal {
   struct GyroStillDet mag_stillness_detect;
   struct GyroStillDet gyro_stillness_detect;
 
-  // Flag is "true" when the magnetometer is used.
-  bool using_mag_sensor;
-
-  // Flag set by user to control whether calibrations are used
-  // (default: "true").
-  bool gyro_calibration_enable;
-
   // Latest temperature measurement.
   float latest_temperature_celcius;
 
@@ -96,65 +97,63 @@ struct GyroCal {
   float stillness_threshold;
 
   // Min and max durations for gyro bias calibration.
-  uint64_t min_still_duration;
-  uint64_t max_still_duration;
+  uint64_t min_still_duration_nanos;
+  uint64_t max_still_duration_nanos;
 
   // Duration of the stillness processing windows.
-  uint64_t window_time_duration;
-
-  // Flag to indicate if device was previously still.
-  bool prev_still;
+  uint64_t window_time_duration_nanos;
 
   // Timestamp when device started a still period.
-  uint64_t start_still_time;
+  uint64_t start_still_time_nanos;
 
   // gyro bias estimates and last calibration timestamp.
   float bias_x, bias_y, bias_z;  // [rad/sec]
-  uint64_t calibration_time;
-  uint64_t calibration_time_duration;
+  uint64_t calibration_time_nanos;
+  uint64_t calibration_time_duration_nanos;
   float stillness_confidence;
-  bool new_gyro_cal_available;  // true when a new cal is ready.
 
   // Current window end time for all sensors. Used to assist in keeping
   // sensor data collection in sync. On initialization this will be set to
   // zero indicating that sensor data will be dropped until a valid end time
   // is set from the first gyro timestamp received.
-  uint64_t stillness_win_endtime;
+  uint64_t stillness_win_endtime_nanos;
 
   // Watchdog timer to reset to a known good state when data capture stalls.
-  uint64_t gyro_watchdog_start;
-  uint64_t gyro_watchdog_timeout_duration;
+  uint64_t gyro_watchdog_start_nanos;
+  uint64_t gyro_watchdog_timeout_duration_nanos;
   bool gyro_watchdog_timeout;
 
+  // Flag is "true" when the magnetometer is used.
+  bool using_mag_sensor;
+
+  // Flag set by user to control whether calibrations are used (default:
+  // "true").
+  bool gyro_calibration_enable;
+
+  // Flag is 'true' when a new calibration update is ready.
+  bool new_gyro_cal_available;
+
+  // Flag to indicate if device was previously still.
+  bool prev_still;
+
 //----------------------------------------------------------------
 
 #ifdef GYRO_CAL_DBG_ENABLED
   // Debug info.
-  bool debug_processed_data_available;  // flag on a per window basis.
-  uint64_t debug_processed_data_time;   // flag time stamp.
-  uint32_t debug_calibration_count;     // total number of cals performed.
-  uint32_t debug_watchdog_count;        // total number of watchdog timeouts.
-  int8_t gyro_debug_state;              // debug report state machine.
-
-  // Debug short-term history data.
-  struct DebugGyroCal debug_cal_data[DEBUG_GYRO_SHORTTERM_NUM_CAL];
-  uint8_t debug_num_cals;  // number of calibrations collected.
-  uint8_t debug_head;      // index of last valid calibration.
-
-  // Debug long-term history data (limited collection frequency).
-  struct DebugGyroCal debug_cal_data_hist[DEBUG_GYRO_LONGTERM_NUM_CAL];
-  uint8_t debug_num_cals_hist;  // number of calibrations collected.
-  uint8_t debug_head_hist;      // index of last valid calibration.
-#endif
+  struct DebugGyroCal debug_gyro_cal;  // Debug data structure.
+  size_t debug_calibration_count;      // Total number of cals performed.
+  size_t debug_watchdog_count;         // Total number of watchdog timeouts.
+  bool debug_print_trigger;            // Flag used to trigger data printout.
+#endif                                 // GYRO_CAL_DBG_ENABLED
 };
 
 /////// FUNCTION PROTOTYPES //////////////////////////////////////////
 
 // Initialize the gyro calibration data structure.
 void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration,
-                 uint64_t max_still_duration, float bias_x, float bias_y,
-                 float bias_z, uint64_t calibration_time,
-                 uint64_t window_time_duration, float gyro_var_threshold,
+                 uint64_t max_still_duration_nanos, float bias_x, float bias_y,
+                 float bias_z, uint64_t calibration_time_nanos,
+                 uint64_t window_time_duration_nanos, float gyro_var_threshold,
                  float gyro_confidence_delta, float accel_var_threshold,
                  float accel_confidence_delta, float mag_var_threshold,
                  float mag_confidence_delta, float stillness_threshold,
@@ -169,7 +168,7 @@ void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y,
 
 // Set an initial bias calibration value.
 void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y,
-                    float bias_z, uint64_t calibration_time);
+                    float bias_z, uint64_t calibration_time_nanos);
 
 // Remove gyro bias from the calibration [rad/sec].
 void gyroCalRemoveBias(struct GyroCal* gyro_cal, float xi, float yi, float zi,
@@ -179,23 +178,21 @@ void gyroCalRemoveBias(struct GyroCal* gyro_cal, float xi, float yi, float zi,
 bool gyroCalNewBiasAvailable(struct GyroCal* gyro_cal);
 
 // Update the gyro calibration with gyro data [rad/sec].
-void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time,
+void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time_nanos,
                        float x, float y, float z, float temperature);
 
 // Update the gyro calibration with mag data [micro Tesla].
-void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time, float x,
-                      float y, float z);
+void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time_nanos,
+                      float x, float y, float z);
 
 // Update the gyro calibration with accel data [m/sec^2].
-void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time,
+void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time_nanos,
                         float x, float y, float z);
 
 #ifdef GYRO_CAL_DBG_ENABLED
-// Start the debug data report state machine.
-void gyroCalDebugPrintStart(struct GyroCal* gyro_cal);
-
 // Print debug data report.
-void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t sample_time);
+void gyroCalDebugPrint(struct GyroCal* gyro_cal,
+                       uint64_t timestamp_nanos_nanos);
 #endif
 
 #ifdef __cplusplus
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
index 12763b08..afddb481 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
@@ -52,7 +52,7 @@ void gyroStillDetUpdate(struct GyroStillDet* gyro_still_det,
                         float x, float y, float z) {
   // Using the method of the assumed mean to preserve some numerical
   // stability while avoiding per-sample divisions that the more
-  // numerically stabe Welford method would afford.
+  // numerically stable Welford method would afford.
 
   // Reference for the numerical method used below to compute the
   // online mean and variance statistics:
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h
index 9ddc9075..9a1d876c 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h
@@ -1,6 +1,3 @@
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_STILLNESS_DETECT_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_STILLNESS_DETECT_H_
-
 /*
  * Copyright (C) 2016 The Android Open Source Project
  *
@@ -33,6 +30,8 @@
  * are nanoseconds.
  */
 ///////////////////////////////////////////////////////////////
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_STILLNESS_DETECT_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_STILLNESS_DETECT_H_
 
 #include <math.h>
 #include <stdbool.h>
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal.c
index 340d021e..bd4876ca 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal.c
@@ -30,16 +30,18 @@
 #define MAX_BATCH_WINDOW 15000000UL  // 15 sec
 #define MIN_BATCH_SIZE 25            // samples
 
+#define MAX_DISTANCE_VIOLATIONS 2
+
 // eigen value magnitude and ratio test
-static int moc_eigen_test(struct MagCal *moc) {
+static int moc_eigen_test(struct KasaFit *kasa) {
   // covariance matrix
   struct Mat33 S;
-  S.elem[0][0] = moc->acc_xx - moc->acc_x * moc->acc_x;
-  S.elem[0][1] = S.elem[1][0] = moc->acc_xy - moc->acc_x * moc->acc_y;
-  S.elem[0][2] = S.elem[2][0] = moc->acc_xz - moc->acc_x * moc->acc_z;
-  S.elem[1][1] = moc->acc_yy - moc->acc_y * moc->acc_y;
-  S.elem[1][2] = S.elem[2][1] = moc->acc_yz - moc->acc_y * moc->acc_z;
-  S.elem[2][2] = moc->acc_zz - moc->acc_z * moc->acc_z;
+  S.elem[0][0] = kasa->acc_xx - kasa->acc_x * kasa->acc_x;
+  S.elem[0][1] = S.elem[1][0] = kasa->acc_xy - kasa->acc_x * kasa->acc_y;
+  S.elem[0][2] = S.elem[2][0] = kasa->acc_xz - kasa->acc_x * kasa->acc_z;
+  S.elem[1][1] = kasa->acc_yy - kasa->acc_y * kasa->acc_y;
+  S.elem[1][2] = S.elem[2][1] = kasa->acc_yz - kasa->acc_y * kasa->acc_z;
+  S.elem[2][2] = kasa->acc_zz - kasa->acc_z * kasa->acc_z;
 
   struct Vec3 eigenvals;
   struct Mat33 eigenvecs;
@@ -60,29 +62,29 @@ static int moc_eigen_test(struct MagCal *moc) {
 }
 
 // Kasa sphere fitting with normal equation
-int magCalFit(struct MagCal *moc, struct Vec3 *bias, float *radius) {
+int magKasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius) {
   //    A    *   out   =    b
   // (4 x 4)   (4 x 1)   (4 x 1)
   struct Mat44 A;
-  A.elem[0][0] = moc->acc_xx;
-  A.elem[0][1] = moc->acc_xy;
-  A.elem[0][2] = moc->acc_xz;
-  A.elem[0][3] = moc->acc_x;
-  A.elem[1][0] = moc->acc_xy;
-  A.elem[1][1] = moc->acc_yy;
-  A.elem[1][2] = moc->acc_yz;
-  A.elem[1][3] = moc->acc_y;
-  A.elem[2][0] = moc->acc_xz;
-  A.elem[2][1] = moc->acc_yz;
-  A.elem[2][2] = moc->acc_zz;
-  A.elem[2][3] = moc->acc_z;
-  A.elem[3][0] = moc->acc_x;
-  A.elem[3][1] = moc->acc_y;
-  A.elem[3][2] = moc->acc_z;
+  A.elem[0][0] = kasa->acc_xx;
+  A.elem[0][1] = kasa->acc_xy;
+  A.elem[0][2] = kasa->acc_xz;
+  A.elem[0][3] = kasa->acc_x;
+  A.elem[1][0] = kasa->acc_xy;
+  A.elem[1][1] = kasa->acc_yy;
+  A.elem[1][2] = kasa->acc_yz;
+  A.elem[1][3] = kasa->acc_y;
+  A.elem[2][0] = kasa->acc_xz;
+  A.elem[2][1] = kasa->acc_yz;
+  A.elem[2][2] = kasa->acc_zz;
+  A.elem[2][3] = kasa->acc_z;
+  A.elem[3][0] = kasa->acc_x;
+  A.elem[3][1] = kasa->acc_y;
+  A.elem[3][2] = kasa->acc_z;
   A.elem[3][3] = 1.0f;
 
   struct Vec4 b;
-  initVec4(&b, -moc->acc_xw, -moc->acc_yw, -moc->acc_zw, -moc->acc_w);
+  initVec4(&b, -kasa->acc_xw, -kasa->acc_yw, -kasa->acc_zw, -kasa->acc_w);
 
   struct Size4 pivot;
   mat44DecomposeLup(&A, &pivot);
@@ -112,13 +114,18 @@ int magCalFit(struct MagCal *moc, struct Vec3 *bias, float *radius) {
   return success;
 }
 
-void magCalReset(struct MagCal *moc) {
-  moc->acc_x = moc->acc_y = moc->acc_z = moc->acc_w = 0.0f;
-  moc->acc_xx = moc->acc_xy = moc->acc_xz = moc->acc_xw = 0.0f;
-  moc->acc_yy = moc->acc_yz = moc->acc_yw = 0.0f;
-  moc->acc_zz = moc->acc_zw = 0.0f;
+void magKasaReset(struct KasaFit *kasa) {
+  kasa->acc_x = kasa->acc_y = kasa->acc_z = kasa->acc_w = 0.0f;
+  kasa->acc_xx = kasa->acc_xy = kasa->acc_xz = kasa->acc_xw = 0.0f;
+  kasa->acc_yy = kasa->acc_yz = kasa->acc_yw = 0.0f;
+  kasa->acc_zz = kasa->acc_zw = 0.0f;
+
+  kasa->nsamples = 0;
+}
 
-  moc->nsamples = 0;
+void magCalReset(struct MagCal *moc) {
+  magKasaReset(&moc->kasa);
+  diversityCheckerReset(&moc->diversity_checker);
   moc->start_time = 0;
 }
 
@@ -126,20 +133,32 @@ static int moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us) {
   int complete = 0;
 
   if ((sample_time_us - moc->start_time > MIN_BATCH_WINDOW) &&
-      (moc->nsamples > MIN_BATCH_SIZE)) {
+      (moc->kasa.nsamples > MIN_BATCH_SIZE)) {
     complete = 1;
 
-  } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) {
-    // not enough samples collected in MAX_BATCH_WINDOW
+  } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW ||
+             moc->diversity_checker.num_max_dist_violations
+             >= MAX_DISTANCE_VIOLATIONS) {
+    // not enough samples collected in MAX_BATCH_WINDOW or too many
+    // maximum distance violations detected.
     magCalReset(moc);
   }
 
   return complete;
 }
 
+void initKasa(struct KasaFit *kasa) {
+  magKasaReset(kasa);
+}
+
 void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias,
                 float c00, float c01, float c02, float c10, float c11,
-                float c12, float c20, float c21, float c22) {
+                float c12, float c20, float c21, float c22,
+                float threshold, float max_distance,
+                size_t min_num_diverse_vectors,
+                size_t max_num_max_distance,
+                float var_threshold,
+                float max_min_threshold) {
   magCalReset(moc);
   moc->update_time = 0;
   moc->radius = 0.0f;
@@ -157,6 +176,15 @@ void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias,
   moc->c20 = c20;
   moc->c21 = c21;
   moc->c22 = c22;
+
+  // Diversity Checker Init
+  diversityCheckerInit(&moc->diversity_checker,
+                       threshold,
+                       max_distance,
+                       min_num_diverse_vectors,
+                       max_num_max_distance,
+                       var_threshold,
+                       max_min_threshold);
 }
 
 void magCalDestroy(struct MagCal *moc) { (void)moc; }
@@ -165,66 +193,74 @@ bool magCalUpdate(struct MagCal *moc, uint64_t sample_time_us, float x, float y,
                   float z) {
   bool new_bias = false;
 
+  // Diversity Checker Update.
+  diversityCheckerUpdate(&moc->diversity_checker, x, y, z);
+
   // 1. run accumulators
   float w = x * x + y * y + z * z;
 
-  moc->acc_x += x;
-  moc->acc_y += y;
-  moc->acc_z += z;
-  moc->acc_w += w;
+  moc->kasa.acc_x += x;
+  moc->kasa.acc_y += y;
+  moc->kasa.acc_z += z;
+  moc->kasa.acc_w += w;
 
-  moc->acc_xx += x * x;
-  moc->acc_xy += x * y;
-  moc->acc_xz += x * z;
-  moc->acc_xw += x * w;
+  moc->kasa.acc_xx += x * x;
+  moc->kasa.acc_xy += x * y;
+  moc->kasa.acc_xz += x * z;
+  moc->kasa.acc_xw += x * w;
 
-  moc->acc_yy += y * y;
-  moc->acc_yz += y * z;
-  moc->acc_yw += y * w;
+  moc->kasa.acc_yy += y * y;
+  moc->kasa.acc_yz += y * z;
+  moc->kasa.acc_yw += y * w;
 
-  moc->acc_zz += z * z;
-  moc->acc_zw += z * w;
+  moc->kasa.acc_zz += z * z;
+  moc->kasa.acc_zw += z * w;
 
-  if (++moc->nsamples == 1) {
+  if (++moc->kasa.nsamples == 1) {
     moc->start_time = sample_time_us;
   }
 
   // 2. batch has enough samples?
   if (moc_batch_complete(moc, sample_time_us)) {
-    float inv = 1.0f / moc->nsamples;
+    float inv = 1.0f / moc->kasa.nsamples;
 
-    moc->acc_x *= inv;
-    moc->acc_y *= inv;
-    moc->acc_z *= inv;
-    moc->acc_w *= inv;
+    moc->kasa.acc_x *= inv;
+    moc->kasa.acc_y *= inv;
+    moc->kasa.acc_z *= inv;
+    moc->kasa.acc_w *= inv;
 
-    moc->acc_xx *= inv;
-    moc->acc_xy *= inv;
-    moc->acc_xz *= inv;
-    moc->acc_xw *= inv;
+    moc->kasa.acc_xx *= inv;
+    moc->kasa.acc_xy *= inv;
+    moc->kasa.acc_xz *= inv;
+    moc->kasa.acc_xw *= inv;
 
-    moc->acc_yy *= inv;
-    moc->acc_yz *= inv;
-    moc->acc_yw *= inv;
+    moc->kasa.acc_yy *= inv;
+    moc->kasa.acc_yz *= inv;
+    moc->kasa.acc_yw *= inv;
 
-    moc->acc_zz *= inv;
-    moc->acc_zw *= inv;
+    moc->kasa.acc_zz *= inv;
+    moc->kasa.acc_zw *= inv;
 
     // 3. eigen test
-    if (moc_eigen_test(moc)) {
+    if (moc_eigen_test(&moc->kasa)) {
       struct Vec3 bias;
       float radius;
 
       // 4. Kasa sphere fitting
-      if (magCalFit(moc, &bias, &radius)) {
-        moc->x_bias = bias.x;
-        moc->y_bias = bias.y;
-        moc->z_bias = bias.z;
-
-        moc->radius = radius;
-        moc->update_time = sample_time_us;
-
-        new_bias = true;
+      if (magKasaFit(&moc->kasa, &bias, &radius)) {
+        if (diversityCheckerNormQuality(&moc->diversity_checker,
+                                        bias.x,
+                                        bias.y,
+                                        bias.z)) {
+          moc->x_bias = bias.x;
+          moc->y_bias = bias.y;
+          moc->z_bias = bias.z;
+
+          moc->radius = radius;
+          moc->update_time = sample_time_us;
+
+          new_bias = true;
+        }
       }
     }
 
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.h b/firmware/os/algos/calibration/magnetometer/mag_cal.h
index a73aa22a..eccf35ce 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.h
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal.h
@@ -1,6 +1,3 @@
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_
-
 /*
  * Copyright (C) 2016 The Android Open Source Project
  *
@@ -16,35 +13,50 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_
 
 #include <stdbool.h>
 #include <stdint.h>
 #include <sys/types.h>
+#include "calibration/common/diversity_checker.h"
 #include "common/math/mat.h"
+#include "common/math/vec.h"
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
-struct MagCal {
-  uint64_t start_time;
-  uint64_t update_time;
-
+struct KasaFit {
   float acc_x, acc_y, acc_z, acc_w;
   float acc_xx, acc_xy, acc_xz, acc_xw;
   float acc_yy, acc_yz, acc_yw, acc_zz, acc_zw;
+  size_t nsamples;
+};
+
+struct MagCal {
+  struct DiversityChecker diversity_checker;
+  struct KasaFit kasa;
+
+  uint64_t start_time;
+  uint64_t update_time;
 
   float x_bias, y_bias, z_bias;
   float radius;
 
   float c00, c01, c02, c10, c11, c12, c20, c21, c22;
-
-  size_t nsamples;
 };
 
+void initKasa(struct KasaFit *kasa);
+
 void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias,
                 float c00, float c01, float c02, float c10, float c11,
-                float c12, float c20, float c21, float c22);
+                float c12, float c20, float c21, float c22,
+                float threshold, float max_distance,
+                size_t min_num_diverse_vectors,
+                size_t max_num_max_distance,
+                float var_threshold,
+                float max_min_threshold);
 
 void magCalDestroy(struct MagCal *moc);
 
@@ -65,9 +77,11 @@ void magCalSetSoftiron(struct MagCal *moc, float c00, float c01, float c02,
 void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi,
                           float *xo, float *yo, float *zo);
 
+void magKasaReset(struct KasaFit *kasa);
+
 void magCalReset(struct MagCal *moc);
 
-int magCalFit(struct MagCal *moc, struct Vec3 *bias, float *radius);
+int magKasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius);
 
 #ifdef __cplusplus
 }
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.c b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
new file mode 100644
index 00000000..0b8b6c73
--- /dev/null
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -0,0 +1,940 @@
+/*
+ * 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.
+ */
+
+#include "calibration/over_temp/over_temp_cal.h"
+
+#include <float.h>
+#include <math.h>
+#include <string.h>
+
+#include "calibration/util/cal_log.h"
+#include "common/math/vec.h"
+#include "util/nano_assert.h"
+
+/////// DEFINITIONS AND MACROS ////////////////////////////////////////////////
+
+// The 'temp_sensitivity' parameters are set to this value to indicate that the
+// model is in its initial state.
+#define MODEL_INITIAL_STATE (1e6f)
+
+// Rate-limits the search for the nearest offset estimate to every 10 seconds
+// when no model has been updated yet.
+#define OVERTEMPCAL_NEAREST_NANOS (10000000000)
+
+// Rate-limits the check of old data to every 2 hours.
+#define OVERTEMPCAL_STALE_CHECK_TIME_NANOS (7200000000000)
+
+// A common sensor operating temperature at which to start producing the model
+// jump-start data.
+#define JUMPSTART_START_TEMP_CELSIUS (30.0f)
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+// A debug version label to help with tracking results.
+#define OVERTEMPCAL_DEBUG_VERSION_STRING "[Dec 12, 2016]"
+
+// The time value used to throttle debug messaging.
+#define OVERTEMPCAL_WAIT_TIME_NANOS (250000000)
+
+// Debug log tag string used to identify debug report output data.
+#define OVERTEMPCAL_REPORT_TAG "[OVER_TEMP_CAL:REPORT]"
+
+// Converts units of radians to milli-degrees.
+#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / M_PI)
+
+// Sensor axis label definition with index correspondence: 0=X, 1=Y, 2=Z.
+static const char  kDebugAxisLabel[3] = "XYZ";
+#endif  // OVERTEMPCAL_DBG_ENABLED
+
+/////// FORWARD DECLARATIONS //////////////////////////////////////////////////
+
+// Updates the most recent model estimate data.
+static void setLatestEstimate(struct OverTempCal *over_temp_cal,
+                              const float *offset, float offset_temp,
+                              uint64_t timestamp_nanos);
+
+/*
+ * Determines if a new over-temperature model fit should be performed, and then
+ * updates the model as needed.
+ *
+ * INPUTS:
+ *   over_temp_cal:    Over-temp data structure.
+ *   timestamp_nanos:  Current timestamp for the model update.
+ */
+static void computeModelUpdate(struct OverTempCal *over_temp_cal,
+                               uint64_t timestamp_nanos);
+
+/*
+ * Searches 'model_data' for the sensor offset estimate closest to the current
+ * temperature. Sets the 'latest_offset' pointer to the result.
+ */
+static void setNearestEstimate(struct OverTempCal *over_temp_cal);
+
+/*
+ * Removes the "old" offset estimates from 'model_data' (i.e., eliminates the
+ * drift-compromised data). Returns 'true' if any data was removed.
+ */
+static bool removeStaleModelData(struct OverTempCal *over_temp_cal,
+                                 uint64_t timestamp_nanos);
+
+/*
+ * Removes the offset estimates from 'model_data' at index, 'model_index'.
+ * Returns 'true' if data was removed.
+ */
+static bool removeModelDataByIndex(struct OverTempCal *over_temp_cal,
+                                   size_t model_index);
+
+/*
+ * Since it may take a while for an empty model to build up enough data to start
+ * producing new model parameter updates, the model collection can be
+ * jump-started by using the new model parameters to insert fake data in place
+ * of actual sensor offset data.
+ */
+static bool jumpStartModelData(struct OverTempCal *over_temp_cal);
+
+/*
+ * Provides updated model parameters for the over-temperature model data.
+ *
+ * INPUTS:
+ *   over_temp_cal:    Over-temp data structure.
+ * OUTPUTS:
+ *   temp_sensitivity: Updated modeled temperature sensitivity (array).
+ *   sensor_intercept: Updated model intercept (array).
+ *
+ * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
+ *
+ * Reference: "Comparing two ways to fit a line to data", John D. Cook.
+ * http://www.johndcook.com/blog/2008/10/20/comparing-two-ways-to-fit-a-line-to-data/
+ */
+static void updateModel(const struct OverTempCal *over_temp_cal,
+                        float *temp_sensitivity, float *sensor_intercept);
+
+/*
+ * Removes the over-temp compensated offset from the input sensor data.
+ *
+ * INPUTS:
+ *   over_temp_cal:    Over-temp data structure.
+ *   vi:               Single axis sensor data to be compensated.
+ *   index:            Index for model parameter compensation (0=x, 1=y, 2=z).
+ * OUTPUTS:
+ *   vo:               Single axis sensor data that has been compensated.
+ */
+static void removeSensorOffset(const struct OverTempCal *over_temp_cal,
+                               float vi, size_t index, float *vo);
+
+/*
+ * Computes the over-temperature compensated sensor offset estimate based on the
+ * input model parameters. Note, this is a single axis calculation.
+ *   comp_offset = (temp_sensitivity * temperature + sensor_intercept)
+ *
+ * INPUTS:
+ *   temperature:      Temperature value at which to compute the estimate.
+ *   temp_sensitivity: Temperature sensitivity model parameter.
+ *   sensor_intercept: Sensor intercept model parameter.
+ * RETURNS:
+ *   comp_offset:      Over-temperature compensated sensor offset estimate.
+ */
+static float getCompensatedOffset(float temperature, float temp_sensitivity,
+                                  float sensor_intercept);
+
+/////// FUNCTION DEFINITIONS //////////////////////////////////////////////////
+
+void overTempCalInit(struct OverTempCal *over_temp_cal,
+                     size_t min_num_model_pts,
+                     uint64_t min_update_interval_nanos,
+                     float delta_temp_per_bin, float max_error_limit,
+                     uint64_t age_limit_nanos, float temp_sensitivity_limit,
+                     float sensor_intercept_limit, bool over_temp_enable) {
+  ASSERT_NOT_NULL(over_temp_cal);
+
+  // Clears OverTempCal memory.
+  memset(over_temp_cal, 0, sizeof(struct OverTempCal));
+
+  // Initializes the pointer to the most recent sensor offset estimate. Sets it
+  // as the first element in 'model_data'.
+  over_temp_cal->latest_offset = &over_temp_cal->model_data[0];
+
+  // Sets the temperature sensitivity model parameters to MODEL_INITIAL_STATE to
+  // indicate that the model is in an "initial" state.
+  over_temp_cal->temp_sensitivity[0] = MODEL_INITIAL_STATE;
+  over_temp_cal->temp_sensitivity[1] = MODEL_INITIAL_STATE;
+  over_temp_cal->temp_sensitivity[2] = MODEL_INITIAL_STATE;
+
+  // Initializes the model identification parameters.
+  over_temp_cal->new_overtemp_model_available = false;
+  over_temp_cal->min_num_model_pts = min_num_model_pts;
+  over_temp_cal->min_update_interval_nanos = min_update_interval_nanos;
+  over_temp_cal->delta_temp_per_bin = delta_temp_per_bin;
+  over_temp_cal->max_error_limit = max_error_limit;
+  over_temp_cal->age_limit_nanos = age_limit_nanos;
+  over_temp_cal->temp_sensitivity_limit = temp_sensitivity_limit;
+  over_temp_cal->sensor_intercept_limit = sensor_intercept_limit;
+  over_temp_cal->over_temp_enable = over_temp_enable;
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+  CAL_DEBUG_LOG("[OVER_TEMP_CAL:MEMORY]", "sizeof(struct OverTempCal): %lu",
+                (unsigned long int)sizeof(struct OverTempCal));
+  CAL_DEBUG_LOG("[OVER_TEMP_CAL:INIT]", "Over-Temp Cal: Initialized.");
+
+#endif  // OVERTEMPCAL_DBG_ENABLED
+}
+
+void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
+                         float offset_temp, uint64_t timestamp_nanos,
+                         const float *temp_sensitivity,
+                         const float *sensor_intercept, bool jump_start_model) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  ASSERT_NOT_NULL(offset);
+  ASSERT_NOT_NULL(temp_sensitivity);
+  ASSERT_NOT_NULL(sensor_intercept);
+
+  // Sets the model parameters if they are within the acceptable limits.
+  size_t i;
+  for (i = 0; i < 3; i++) {
+    if (NANO_ABS(temp_sensitivity[i]) < over_temp_cal->temp_sensitivity_limit &&
+        NANO_ABS(sensor_intercept[i]) < over_temp_cal->sensor_intercept_limit) {
+      over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i];
+      over_temp_cal->sensor_intercept[i] = sensor_intercept[i];
+    }
+  }
+
+  // Sets the model update time to the current timestamp.
+  over_temp_cal->modelupdate_timestamp_nanos = timestamp_nanos;
+
+  // Model "Jump-Start".
+  const bool model_jump_started =
+      (jump_start_model) ? jumpStartModelData(over_temp_cal) : false;
+
+  if (!model_jump_started) {
+    // Sets the initial over-temp calibration estimate and model data.
+    setLatestEstimate(over_temp_cal, offset, offset_temp, timestamp_nanos);
+
+    // Now there is one offset estimate in the model.
+    over_temp_cal->num_model_pts = 1;
+  }
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+  // Prints the updated model data.
+  CAL_DEBUG_LOG("[OVER_TEMP_CAL:RECALL]",
+                "Model parameters recalled from memory.");
+
+  // Trigger a debug print out to view the new model parameters.
+  over_temp_cal->debug_print_trigger = true;
+#endif  // OVERTEMPCAL_DBG_ENABLED
+}
+
+void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset,
+                         float *offset_temp, uint64_t *timestamp_nanos,
+                         float *temp_sensitivity, float *sensor_intercept) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  ASSERT_NOT_NULL(over_temp_cal->latest_offset);
+  ASSERT_NOT_NULL(offset);
+  ASSERT_NOT_NULL(offset_temp);
+  ASSERT_NOT_NULL(timestamp_nanos);
+  ASSERT_NOT_NULL(temp_sensitivity);
+  ASSERT_NOT_NULL(sensor_intercept);
+
+  // Updates the latest offset so that it is the one nearest to the current
+  // temperature.
+  setNearestEstimate(over_temp_cal);
+
+  // Gets the over-temp calibration estimate and model data.
+  memcpy(offset, over_temp_cal->latest_offset->offset, 3 * sizeof(float));
+  memcpy(temp_sensitivity, over_temp_cal->temp_sensitivity, 3 * sizeof(float));
+  memcpy(sensor_intercept, over_temp_cal->sensor_intercept, 3 * sizeof(float));
+  *offset_temp = over_temp_cal->latest_offset->offset_temp;
+  *timestamp_nanos = over_temp_cal->latest_offset->timestamp_nanos;
+}
+
+void overTempCalRemoveOffset(struct OverTempCal *over_temp_cal,
+                             uint64_t timestamp_nanos, float xi, float yi,
+                             float zi, float *xo, float *yo, float *zo) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  ASSERT_NOT_NULL(over_temp_cal->latest_offset);
+  ASSERT_NOT_NULL(xo);
+  ASSERT_NOT_NULL(yo);
+  ASSERT_NOT_NULL(zo);
+
+  // Determines whether over-temp compensation will be applied.
+  if (!over_temp_cal->over_temp_enable) {
+    return;
+  }
+
+  // Removes very old data from the collected model estimates (eliminates
+  // drift-compromised data). Only does this when there is more than one
+  // estimate in the model (i.e., don't want to remove all data, even if it is
+  // very old [something is likely better than nothing]).
+  if ((timestamp_nanos - over_temp_cal->stale_data_timer) >=
+          OVERTEMPCAL_STALE_CHECK_TIME_NANOS &&
+      over_temp_cal->num_model_pts > 1) {
+    over_temp_cal->stale_data_timer = timestamp_nanos;  // Resets timer.
+
+    if (removeStaleModelData(over_temp_cal, timestamp_nanos)) {
+      // If anything was removed, then this attempts to recompute the model.
+      computeModelUpdate(over_temp_cal, timestamp_nanos);
+    }
+  }
+
+  // Removes the over-temperature compensated offset from the input sensor data.
+  removeSensorOffset(over_temp_cal, xi, 0, xo);
+  removeSensorOffset(over_temp_cal, yi, 1, yo);
+  removeSensorOffset(over_temp_cal, zi, 2, zo);
+}
+
+bool overTempCalNewModelUpdateAvailable(struct OverTempCal *over_temp_cal) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  const bool update_available = over_temp_cal->new_overtemp_model_available &&
+                                over_temp_cal->over_temp_enable;
+
+  // The 'new_overtemp_model_available' flag is reset when it is read here.
+  over_temp_cal->new_overtemp_model_available = false;
+
+  return update_available;
+}
+
+void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
+                                     uint64_t timestamp_nanos,
+                                     const float *offset,
+                                     float temperature_celsius) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  ASSERT_NOT_NULL(over_temp_cal->latest_offset);
+  ASSERT_NOT_NULL(offset);
+  ASSERT(over_temp_cal->delta_temp_per_bin > 0);
+
+  // Prevent a divide by zero below.
+  if (over_temp_cal->delta_temp_per_bin <= 0) {
+    return;
+  }
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+  // Updates the total count of offset estimates.
+  over_temp_cal->debug_num_estimates++;
+
+  // If there are fewer than the minimum number of points to produce a model,
+  // then trigger a debug printout to view the model building process.
+  over_temp_cal->debug_print_trigger |=
+      (over_temp_cal->num_model_pts <= over_temp_cal->min_num_model_pts);
+#endif  // OVERTEMPCAL_DBG_ENABLED
+
+  // Provides an early escape if this is the first model estimate.
+  if (over_temp_cal->num_model_pts == 0) {
+    setLatestEstimate(over_temp_cal, offset, temperature_celsius,
+                      timestamp_nanos);
+    over_temp_cal->num_model_pts = 1;  // one estimate was added above.
+    return;
+  }
+
+  // Computes the temperature bin range data.
+  const int32_t bin_num =
+      CAL_FLOOR(temperature_celsius / over_temp_cal->delta_temp_per_bin);
+  const float temp_lo_check = bin_num * over_temp_cal->delta_temp_per_bin;
+  const float temp_hi_check = (bin_num + 1) * over_temp_cal->delta_temp_per_bin;
+
+  // The rules for accepting new offset estimates into the 'model_data'
+  // collection:
+  //    1) The temperature domain is divided into bins each spanning
+  //       'delta_temp_per_bin'.
+  //    2) Find and replace the i'th 'model_data' estimate data if:
+  //          Let, bin_num = floor(temperature_celsius / delta_temp_per_bin)
+  //          temp_lo_check = bin_num * delta_temp_per_bin
+  //          temp_hi_check = (bin_num + 1) * delta_temp_per_bin
+  //          Check condition:
+  //            temp_lo_check <= model_data[i].offset_temp < temp_hi_check
+  bool replaced_one = false;
+  size_t i = 0;
+  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
+    if (over_temp_cal->model_data[i].offset_temp < temp_hi_check &&
+        over_temp_cal->model_data[i].offset_temp >= temp_lo_check) {
+      // NOTE - the pointer to the estimate that is getting replaced is set
+      // here; the offset values are set below in the call to
+      // 'setLatestEstimate'.
+      over_temp_cal->latest_offset = &over_temp_cal->model_data[i];
+      replaced_one = true;
+      break;
+    }
+  }
+
+  //    3) If nothing was replaced, and the 'model_data' buffer is not full
+  //       then add the estimate data to the array.
+  //    4) Otherwise (nothing was replaced and buffer is full), replace the
+  //       'latest_offset' with the incoming one. This is done below.
+  if (!replaced_one && over_temp_cal->num_model_pts < OVERTEMPCAL_MODEL_SIZE) {
+    // NOTE - the pointer to the next available array location is set here;
+    // the offset values are set below in the call to 'setLatestEstimate'.
+    over_temp_cal->latest_offset =
+        &over_temp_cal->model_data[over_temp_cal->num_model_pts];
+    over_temp_cal->num_model_pts++;
+  }
+
+  // Updates the latest model estimate data.
+  setLatestEstimate(over_temp_cal, offset, temperature_celsius,
+                    timestamp_nanos);
+
+  // Conditionally updates the over-temp model. See 'computeModelUpdate' for
+  // update conditions.
+  computeModelUpdate(over_temp_cal, timestamp_nanos);
+
+}
+
+void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
+                               uint64_t timestamp_nanos,
+                               float temperature_celsius) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  ASSERT_NOT_NULL(over_temp_cal->latest_offset);
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+#ifdef OVERTEMPCAL_DBG_LOG_TEMP
+  static uint64_t wait_timer = 0;
+  // Prints the sensor temperature trajectory for debugging purposes.
+  // This throttles the print statements.
+  if ((timestamp_nanos - wait_timer) >= 1000000000) {
+    wait_timer = timestamp_nanos;  // Starts the wait timer.
+
+    // Prints out temperature and the current timestamp.
+    CAL_DEBUG_LOG("[OVER_TEMP_CAL:TEMP]",
+                  "Temperature|Time [C|nsec] = %s%d.%06d, %llu",
+                  CAL_ENCODE_FLOAT(temperature_celsius, 6),
+                  (unsigned long long int)timestamp_nanos);
+  }
+#endif  // OVERTEMPCAL_DBG_LOG_TEMP
+#endif  // OVERTEMPCAL_DBG_ENABLED
+
+  // Updates the sensor temperature.
+  over_temp_cal->temperature_celsius = temperature_celsius;
+
+  // If any of the models for the sensor axes are in an initial state, then
+  // this searches for the sensor offset estimate closest to the current
+  // temperature. A timer is used to limit the rate at which this search is
+  // performed.
+  if (over_temp_cal->num_model_pts > 1 &&
+      (over_temp_cal->temp_sensitivity[0] >= MODEL_INITIAL_STATE ||
+       over_temp_cal->temp_sensitivity[1] >= MODEL_INITIAL_STATE ||
+       over_temp_cal->temp_sensitivity[2] >= MODEL_INITIAL_STATE) &&
+      (timestamp_nanos - over_temp_cal->nearest_search_timer) >=
+          OVERTEMPCAL_NEAREST_NANOS) {
+    setNearestEstimate(over_temp_cal);
+    over_temp_cal->nearest_search_timer = timestamp_nanos;  // Reset timer.
+  }
+}
+
+void getModelError(const struct OverTempCal *over_temp_cal,
+                   const float *temp_sensitivity, const float *sensor_intercept,
+                   float *max_error) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  ASSERT_NOT_NULL(temp_sensitivity);
+  ASSERT_NOT_NULL(sensor_intercept);
+  ASSERT_NOT_NULL(max_error);
+
+  size_t i;
+  size_t j;
+  float max_error_test;
+  memset(max_error, 0, 3 * sizeof(float));
+
+  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
+    for (j = 0; j < 3; j++) {
+      max_error_test =
+          NANO_ABS(over_temp_cal->model_data[i].offset[j] -
+              getCompensatedOffset(over_temp_cal->model_data[i].offset_temp,
+                                   temp_sensitivity[j], sensor_intercept[j]));
+      if (max_error_test > max_error[j]) {
+        max_error[j] = max_error_test;
+      }
+    }
+  }
+}
+
+/////// LOCAL HELPER FUNCTION DEFINITIONS /////////////////////////////////////
+
+void setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset,
+                       float offset_temp, uint64_t timestamp_nanos) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  ASSERT_NOT_NULL(offset);
+  ASSERT_NOT_NULL(over_temp_cal->latest_offset);
+
+  // Sets the latest over-temp calibration estimate.
+  over_temp_cal->latest_offset->offset[0] = offset[0];
+  over_temp_cal->latest_offset->offset[1] = offset[1];
+  over_temp_cal->latest_offset->offset[2] = offset[2];
+  over_temp_cal->latest_offset->offset_temp = offset_temp;
+  over_temp_cal->latest_offset->timestamp_nanos = timestamp_nanos;
+}
+
+void computeModelUpdate(struct OverTempCal *over_temp_cal,
+                        uint64_t timestamp_nanos) {
+  ASSERT_NOT_NULL(over_temp_cal);
+
+  // The rules for determining whether a new model fit is computed are:
+  //    1) A minimum number of data points must have been collected:
+  //          num_model_pts >= min_num_model_pts
+  //       NOTE: Collecting 'num_model_pts' and given that only one point is
+  //       kept per temperature bin (spanning a thermal range specified by
+  //       'delta_temp_per_bin'), implies that model data covers at least,
+  //          model_temp_span >= 'num_model_pts' * delta_temp_per_bin
+  //    2) New model updates will not occur for intervals less than:
+  //          (current_timestamp_nanos - modelupdate_timestamp_nanos) <
+  //            min_update_interval_nanos
+  if (over_temp_cal->num_model_pts < over_temp_cal->min_num_model_pts ||
+      (timestamp_nanos - over_temp_cal->modelupdate_timestamp_nanos) <
+          over_temp_cal->min_update_interval_nanos) {
+    return;
+  }
+
+  // Updates the linear model fit.
+  float temp_sensitivity[3];
+  float sensor_intercept[3];
+  updateModel(over_temp_cal, temp_sensitivity, sensor_intercept);
+
+  // Computes the maximum error over all of the model data.
+  float max_error[3];
+  getModelError(over_temp_cal, temp_sensitivity, sensor_intercept, max_error);
+
+  //    3) A new set of model parameters are accepted if:
+  //         i.  The model fit error is less than, 'max_error_limit'. See
+  //             getModelError() for error metric description.
+  //         ii. The model fit parameters must be within certain absolute
+  //             bounds:
+  //               a. NANO_ABS(temp_sensitivity) < temp_sensitivity_limit
+  //               b. NANO_ABS(sensor_intercept) < sensor_intercept_limit
+  size_t i;
+  for (i = 0; i < 3; i++) {
+    if (max_error[i] < over_temp_cal->max_error_limit &&
+        NANO_ABS(temp_sensitivity[i]) < over_temp_cal->temp_sensitivity_limit &&
+        NANO_ABS(sensor_intercept[i]) < over_temp_cal->sensor_intercept_limit) {
+      over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i];
+      over_temp_cal->sensor_intercept[i] = sensor_intercept[i];
+    } else {
+#ifdef OVERTEMPCAL_DBG_ENABLED
+      CAL_DEBUG_LOG(
+          "[OVER_TEMP_CAL:REJECT]",
+          "Rejected %c-Axis Parameters|Max Error [mdps/C|mdps|mdps] = "
+          "%s%d.%06d, %s%d.%06d, %s%d.%06d",
+          kDebugAxisLabel[i],
+          CAL_ENCODE_FLOAT(temp_sensitivity[i] * RAD_TO_MILLI_DEGREES, 6),
+          CAL_ENCODE_FLOAT(sensor_intercept[i] * RAD_TO_MILLI_DEGREES, 6),
+          CAL_ENCODE_FLOAT(max_error[i] * RAD_TO_MILLI_DEGREES, 6));
+#endif  // OVERTEMPCAL_DBG_ENABLED
+    }
+  }
+
+  // Resets the timer and sets the update flag.
+  over_temp_cal->modelupdate_timestamp_nanos = timestamp_nanos;
+  over_temp_cal->new_overtemp_model_available = true;
+
+  // Track the total number of model updates, and set trigger to print log data.
+#ifdef OVERTEMPCAL_DBG_ENABLED
+  over_temp_cal->debug_num_model_updates++;
+  over_temp_cal->debug_print_trigger |= true;
+#endif  // OVERTEMPCAL_DBG_ENABLED
+}
+
+void setNearestEstimate(struct OverTempCal *over_temp_cal) {
+  ASSERT_NOT_NULL(over_temp_cal);
+
+  size_t i = 0;
+  float dtemp_new = 0.0f;
+  float dtemp_old = FLT_MAX;
+  struct OverTempCalDataPt *nearest_estimate = &over_temp_cal->model_data[0];
+  for (i = 1; i < over_temp_cal->num_model_pts; i++) {
+    dtemp_new = NANO_ABS(over_temp_cal->model_data[i].offset_temp -
+                    over_temp_cal->temperature_celsius);
+    if (dtemp_new < dtemp_old) {
+      nearest_estimate = &over_temp_cal->model_data[i];
+      dtemp_old = dtemp_new;
+    }
+  }
+
+  // Set the 'latest_offset' to the estimate nearest the current temperature.
+  over_temp_cal->latest_offset = nearest_estimate;
+}
+
+bool removeStaleModelData(struct OverTempCal *over_temp_cal,
+                          uint64_t timestamp_nanos) {
+  ASSERT_NOT_NULL(over_temp_cal);
+
+  size_t i;
+  bool removed_one = false;
+  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
+    if ((timestamp_nanos - over_temp_cal->model_data[i].timestamp_nanos) >
+        over_temp_cal->age_limit_nanos) {
+      removed_one |= removeModelDataByIndex(over_temp_cal, i);
+    }
+  }
+
+  // Updates the latest offset so that it is the one nearest to the current
+  // temperature.
+  setNearestEstimate(over_temp_cal);
+
+  return removed_one;
+}
+
+bool removeModelDataByIndex(struct OverTempCal *over_temp_cal,
+                            size_t model_index) {
+  ASSERT_NOT_NULL(over_temp_cal);
+
+  // This function will not remove all of the model data. At least one model
+  // sample will be left.
+  if (over_temp_cal->num_model_pts <= 1) {
+    return false;
+  }
+
+  // Remove the model data at 'model_index'.
+  size_t i;
+  for (i = model_index; i < over_temp_cal->num_model_pts - 1; i++) {
+    memcpy(&over_temp_cal->model_data[i], &over_temp_cal->model_data[i + 1],
+           sizeof(struct OverTempCalDataPt));
+  }
+  over_temp_cal->num_model_pts--;
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+  CAL_DEBUG_LOG("[OVER_TEMP_CAL:REMOVE]",
+                "Removed Stale Data:  Model Index = %lu",
+                (unsigned long int)model_index);
+#endif  // OVERTEMPCAL_DBG_ENABLED
+
+  return true;
+}
+
+bool jumpStartModelData(struct OverTempCal *over_temp_cal) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  ASSERT(over_temp_cal->delta_temp_per_bin > 0);
+
+  // Prevent a divide by zero below.
+  if (over_temp_cal->delta_temp_per_bin <= 0) {
+    return false;
+  }
+
+  // In normal operation the offset estimates enter into the 'model_data' array
+  // complete (i.e., x, y, z values are all provided). Therefore, the jumpstart
+  // data produced here requires that the model parameters have all been fully
+  // defined (i.e., no models in an initial state) and are all within the valid
+  // range (this is assumed to have been checked prior to this function). There
+  // must also be no preexisting model data; that is, this function will not
+  // replace any actual offset estimates already buffered.
+  if (over_temp_cal->num_model_pts > 0 ||
+      over_temp_cal->temp_sensitivity[0] >= MODEL_INITIAL_STATE ||
+      over_temp_cal->temp_sensitivity[1] >= MODEL_INITIAL_STATE ||
+      over_temp_cal->temp_sensitivity[2] >= MODEL_INITIAL_STATE) {
+    return false;
+  }
+
+  // This defines the minimum contiguous set of points to allow a model update
+  // when the next offset estimate is received. They are placed at a common
+  // temperature range that is likely to get replaced with actual data soon.
+  const int32_t start_bin_num = CAL_FLOOR(JUMPSTART_START_TEMP_CELSIUS /
+                                          over_temp_cal->delta_temp_per_bin);
+  float offset_temp =
+      start_bin_num * (1.5f * over_temp_cal->delta_temp_per_bin);
+
+  size_t i;
+  size_t j;
+  for (i = 0; i < over_temp_cal->min_num_model_pts; i++) {
+    float offset[3];
+    const uint64_t timestamp_nanos = over_temp_cal->modelupdate_timestamp_nanos;
+    for (j = 0; j < 3; j++) {
+      offset[j] =
+          getCompensatedOffset(offset_temp, over_temp_cal->temp_sensitivity[j],
+                               over_temp_cal->sensor_intercept[j]);
+    }
+    over_temp_cal->latest_offset = &over_temp_cal->model_data[i];
+    setLatestEstimate(over_temp_cal, offset, offset_temp, timestamp_nanos);
+    offset_temp += over_temp_cal->delta_temp_per_bin;
+    over_temp_cal->num_model_pts++;
+  }
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+  if (over_temp_cal->min_num_model_pts > 0) {
+    CAL_DEBUG_LOG("[OVER_TEMP_CAL]", "Jump-Started Model:  #Points = %lu.",
+                  (unsigned long int)over_temp_cal->min_num_model_pts);
+  }
+#endif  // OVERTEMPCAL_DBG_ENABLED
+
+  return (over_temp_cal->min_num_model_pts > 0);
+}
+
+void updateModel(const struct OverTempCal *over_temp_cal,
+                 float *temp_sensitivity, float *sensor_intercept) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  ASSERT_NOT_NULL(temp_sensitivity);
+  ASSERT_NOT_NULL(sensor_intercept);
+  ASSERT(over_temp_cal->num_model_pts > 0);
+
+  float st = 0.0f, stt = 0.0f;
+  float sx = 0.0f, stsx = 0.0f;
+  float sy = 0.0f, stsy = 0.0f;
+  float sz = 0.0f, stsz = 0.0f;
+  const size_t n = over_temp_cal->num_model_pts;
+  size_t i = 0;
+
+  // First pass computes the mean values.
+  for (i = 0; i < n; ++i) {
+    st += over_temp_cal->model_data[i].offset_temp;
+    sx += over_temp_cal->model_data[i].offset[0];
+    sy += over_temp_cal->model_data[i].offset[1];
+    sz += over_temp_cal->model_data[i].offset[2];
+  }
+
+  // Second pass computes the mean corrected second moment values.
+  const float inv_n = 1.0f / n;
+  for (i = 0; i < n; ++i) {
+    const float t = over_temp_cal->model_data[i].offset_temp - st * inv_n;
+    stt += t * t;
+    stsx += t * over_temp_cal->model_data[i].offset[0];
+    stsy += t * over_temp_cal->model_data[i].offset[1];
+    stsz += t * over_temp_cal->model_data[i].offset[2];
+  }
+
+  // Calculates the linear model fit parameters.
+  ASSERT(stt > 0);
+  const float inv_stt = 1.0f / stt;
+  temp_sensitivity[0] = stsx * inv_stt;
+  sensor_intercept[0] = (sx - st * temp_sensitivity[0]) * inv_n;
+  temp_sensitivity[1] = stsy * inv_stt;
+  sensor_intercept[1] = (sy - st * temp_sensitivity[1]) * inv_n;
+  temp_sensitivity[2] = stsz * inv_stt;
+  sensor_intercept[2] = (sz - st * temp_sensitivity[2]) * inv_n;
+}
+
+void removeSensorOffset(const struct OverTempCal *over_temp_cal, float vi,
+                        size_t index, float *vo) {
+  // Removes the over-temperature compensated offset from the input sensor data.
+  if (over_temp_cal->temp_sensitivity[index] >= MODEL_INITIAL_STATE) {
+    // If this axis is in its initial state, use the nearest estimate to perform
+    // the compensation (in this case the latest estimate will be the nearest):
+    // sensor_out = sensor_in - nearest_offset
+    *vo = vi - over_temp_cal->latest_offset->offset[index];
+  } else {
+    // sensor_out = sensor_in - compensated_offset
+    // Where,
+    //  compensated_offset = (temp_sensitivity * temp_meas + sensor_intercept)
+    *vo = vi - getCompensatedOffset(over_temp_cal->temperature_celsius,
+                                    over_temp_cal->temp_sensitivity[index],
+                                    over_temp_cal->sensor_intercept[index]);
+  }
+}
+
+float getCompensatedOffset(float temperature, float temp_sensitivity,
+                           float sensor_intercept) {
+  return temp_sensitivity * temperature + sensor_intercept;
+}
+
+/////// DEBUG FUNCTION DEFINITIONS ////////////////////////////////////////////
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+// Debug printout state enumeration.
+enum DebugState {
+  IDLE = 0,
+  WAIT_STATE,
+  PRINT_HEADER,
+  PRINT_OFFSET,
+  PRINT_SENSITIVITY,
+  PRINT_INTERCEPT,
+  PRINT_ERROR,
+  PRINT_MODEL_PTS,
+  PRINT_MODEL_DATA
+};
+
+void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
+                           uint64_t timestamp_nanos) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  ASSERT_NOT_NULL(over_temp_cal->latest_offset);
+
+  static enum DebugState debug_state = IDLE;
+  static enum DebugState next_state = 0;
+  static uint64_t wait_timer = 0;
+  static size_t i = 0;  // Counter.
+
+  // NOTE - The un-initialized model state is indicated by
+  // temp_sensitivity=MODEL_INITIAL_STATE. The following filters out this
+  // condition for the data printout below.
+  float compensated_offset[3];
+  float temp_sensitivity[3];
+  float max_error[3];
+  size_t j;
+  for (j = 0; j < 3; j++) {
+    temp_sensitivity[j] =
+        (over_temp_cal->temp_sensitivity[j] >= MODEL_INITIAL_STATE)
+            ? 0.0f
+            : over_temp_cal->temp_sensitivity[j];
+  }
+
+  // This is a state machine that controls the reporting out of debug data.
+  switch (debug_state) {
+    case IDLE:
+      // Wait for a trigger and start the debug printout sequence.
+      if (over_temp_cal->debug_print_trigger) {
+        debug_state = PRINT_HEADER;
+        CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "");
+        over_temp_cal->debug_print_trigger = false;  // Resets trigger.
+      } else {
+        debug_state = IDLE;
+      }
+      break;
+
+    case PRINT_HEADER:
+      // Print out header.
+      CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Debug Version: %s",
+                    OVERTEMPCAL_DEBUG_VERSION_STRING);
+
+      // Prints out number of offsets.
+      CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Total Offsets = %lu",
+                    (unsigned long int)over_temp_cal->debug_num_estimates);
+
+      wait_timer = timestamp_nanos;  // Starts the wait timer.
+      next_state = PRINT_OFFSET;     // Sets the next state.
+      debug_state = WAIT_STATE;      // First, go to wait state.
+      break;
+
+    case PRINT_OFFSET:
+      // Computes the compensated sensor offset based on the current
+      // temperature.
+      for (j = 0; j < 3; j++) {
+        compensated_offset[j] =
+            (over_temp_cal->temp_sensitivity[j] >= MODEL_INITIAL_STATE)
+                ? over_temp_cal->latest_offset->offset[j]
+                : getCompensatedOffset(over_temp_cal->temperature_celsius,
+                                       over_temp_cal->temp_sensitivity[j],
+                                       over_temp_cal->sensor_intercept[j]);
+      }
+
+      CAL_DEBUG_LOG(
+          OVERTEMPCAL_REPORT_TAG,
+          "Offset|Temp|Time [mdps|C|nsec] = %s%d.%06d, %s%d.%06d, "
+          "%s%d.%06d, %s%d.%06d, %llu",
+          CAL_ENCODE_FLOAT(compensated_offset[0] * RAD_TO_MILLI_DEGREES, 6),
+          CAL_ENCODE_FLOAT(compensated_offset[1] * RAD_TO_MILLI_DEGREES, 6),
+          CAL_ENCODE_FLOAT(compensated_offset[2] * RAD_TO_MILLI_DEGREES, 6),
+          CAL_ENCODE_FLOAT(over_temp_cal->temperature_celsius, 6),
+          (unsigned long long int)
+            over_temp_cal->latest_offset->timestamp_nanos);
+
+      wait_timer = timestamp_nanos;    // Starts the wait timer.
+      next_state = PRINT_SENSITIVITY;  // Sets the next state.
+      debug_state = WAIT_STATE;        // First, go to wait state.
+      break;
+
+    case WAIT_STATE:
+      // This helps throttle the print statements.
+      if ((timestamp_nanos - wait_timer) >= OVERTEMPCAL_WAIT_TIME_NANOS) {
+        debug_state = next_state;
+      }
+      break;
+
+    case PRINT_SENSITIVITY:
+      // Prints out the modeled temperature sensitivity.
+      CAL_DEBUG_LOG(
+          OVERTEMPCAL_REPORT_TAG,
+          "Modeled Temperature Sensitivity [mdps/C] = %s%d.%06d, %s%d.%06d, "
+          "%s%d.%06d",
+          CAL_ENCODE_FLOAT(temp_sensitivity[0] * RAD_TO_MILLI_DEGREES, 6),
+          CAL_ENCODE_FLOAT(temp_sensitivity[1] * RAD_TO_MILLI_DEGREES, 6),
+          CAL_ENCODE_FLOAT(temp_sensitivity[2] * RAD_TO_MILLI_DEGREES, 6));
+
+      wait_timer = timestamp_nanos;  // Starts the wait timer.
+      next_state = PRINT_INTERCEPT;  // Sets the next state.
+      debug_state = WAIT_STATE;      // First, go to wait state.
+      break;
+
+    case PRINT_INTERCEPT:
+      // Prints out the modeled temperature intercept.
+      CAL_DEBUG_LOG(
+          OVERTEMPCAL_REPORT_TAG,
+          "Modeled Temperature Intercept [mdps] = %s%d.%06d, %s%d.%06d, "
+          "%s%d.%06d",
+          CAL_ENCODE_FLOAT(
+              over_temp_cal->sensor_intercept[0] * RAD_TO_MILLI_DEGREES, 6),
+          CAL_ENCODE_FLOAT(
+              over_temp_cal->sensor_intercept[1] * RAD_TO_MILLI_DEGREES, 6),
+          CAL_ENCODE_FLOAT(
+              over_temp_cal->sensor_intercept[2] * RAD_TO_MILLI_DEGREES, 6));
+
+      wait_timer = timestamp_nanos;  // Starts the wait timer.
+      next_state = PRINT_ERROR;      // Sets the next state.
+      debug_state = WAIT_STATE;      // First, go to wait state.
+      break;
+
+    case PRINT_ERROR:
+      // Computes the maximum error over all of the model data.
+      if (over_temp_cal->num_model_pts > 0) {
+        getModelError(over_temp_cal, temp_sensitivity,
+                      over_temp_cal->sensor_intercept, max_error);
+
+        // Reports the resulting model error.
+        CAL_DEBUG_LOG(
+            OVERTEMPCAL_REPORT_TAG,
+            "Model Error [mdps] = %s%d.%06d, %s%d.%06d, %s%d.%06d",
+            CAL_ENCODE_FLOAT(max_error[0] * RAD_TO_MILLI_DEGREES, 6),
+            CAL_ENCODE_FLOAT(max_error[1] * RAD_TO_MILLI_DEGREES, 6),
+            CAL_ENCODE_FLOAT(max_error[2] * RAD_TO_MILLI_DEGREES, 6));
+      }
+
+      wait_timer = timestamp_nanos;  // Starts the wait timer.
+      next_state = PRINT_MODEL_PTS;  // Sets the next state.
+      debug_state = WAIT_STATE;      // First, go to wait state.
+      break;
+
+    case PRINT_MODEL_PTS:
+      // Prints out the number of model points/updates.
+      CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Number of Model Points = %lu",
+                    (unsigned long int)over_temp_cal->num_model_pts);
+
+      CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Number of Model Updates = %lu",
+                    (unsigned long int)over_temp_cal->debug_num_model_updates);
+
+      i = 0;                          // Resets the counter.
+      wait_timer = timestamp_nanos;   // Starts the wait timer.
+      next_state = PRINT_MODEL_DATA;  // Sets the next state.
+      debug_state = WAIT_STATE;       // First, go to wait state.
+      break;
+
+    case PRINT_MODEL_DATA:
+      // Prints out all of the model data.
+      if (i < over_temp_cal->num_model_pts) {
+        CAL_DEBUG_LOG(
+            OVERTEMPCAL_REPORT_TAG,
+            "  Model[%lu] [mdps|C] = %s%d.%06d, %s%d.%06d, %s%d.%06d, "
+            "%s%d.%03d ",
+            (unsigned long int)i,
+            CAL_ENCODE_FLOAT(
+                over_temp_cal->model_data[i].offset[0] * RAD_TO_MILLI_DEGREES,
+                6),
+            CAL_ENCODE_FLOAT(
+                over_temp_cal->model_data[i].offset[1] * RAD_TO_MILLI_DEGREES,
+                6),
+            CAL_ENCODE_FLOAT(
+                over_temp_cal->model_data[i].offset[2] * RAD_TO_MILLI_DEGREES,
+                6),
+            CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset_temp, 3));
+
+        i++;
+        wait_timer = timestamp_nanos;   // Starts the wait timer.
+        next_state = PRINT_MODEL_DATA;  // Sets the next state.
+        debug_state = WAIT_STATE;       // First, go to wait state.
+      } else {
+        debug_state = IDLE;             // Goes to idle state.
+        CAL_DEBUG_LOG(
+            OVERTEMPCAL_REPORT_TAG, "Last Model Update [nsec] = %llu",
+            (unsigned long long int)over_temp_cal->modelupdate_timestamp_nanos);
+      }
+      break;
+
+    default:
+      // Sends this state machine to its idle state.
+      wait_timer = timestamp_nanos;  // Starts the wait timer.
+      next_state = IDLE;             // Sets the next state.
+      debug_state = WAIT_STATE;      // First, go to wait state.
+  }
+}
+
+#endif  // OVERTEMPCAL_DBG_ENABLED
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.h b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
new file mode 100644
index 00000000..928576f0
--- /dev/null
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
@@ -0,0 +1,327 @@
+/*
+ * 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 provides an online algorithm for compensating a 3-axis sensor's
+ * offset over its operating temperature:
+ *
+ *   1) Estimates of sensor offset with associated temperature are consumed,
+ *      {offset, offset_temperature}.
+ *   2) A temperature dependence model is extracted from the collected set of
+ *      data pairs.
+ *   3) Until a "complete" model has been built and a model equation has been
+ *      computed, the compensation will use the collected offset nearest in
+ *      temperature. If a model is available, then the compensation will take
+ *      the form of:
+ *
+ * Linear Compensation Model Equation:
+ *   sensor_out = sensor_in - compensated_offset
+ *   Where,
+ *     compensated_offset = (temp_sensitivity * current_temp + sensor_intercept)
+ *
+ * NOTE - 'current_temp' is the current measured temperature. 'temp_sensitivity'
+ *        is the modeled temperature sensitivity (i.e., linear slope).
+ *        'sensor_intercept' is linear model intercept.
+ *
+ * Assumptions:
+ *
+ *   1) Sensor hysteresis is negligible.
+ *   2) Sensor offset temperature dependence is sufficiently "linear".
+ *   3) The impact of long-term offset drift/aging compared to the magnitude of
+ *      deviation resulting from the thermal sensitivity of the offset is
+ *      relatively small.
+ *
+ * Sensor Input and Units:
+ *       - General 3-axis sensor data.
+ *       - Temperature measurements [Celsius].
+ *
+ * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
+ *
+ * #define OVERTEMPCAL_DBG_ENABLED to enable debug printout statements.
+ * #define OVERTEMPCAL_DBG_LOG_TEMP to periodically printout sensor temperature.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_CAL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_CAL_H_
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Defines the maximum size of the 'model_data' array.
+#define OVERTEMPCAL_MODEL_SIZE (20)
+
+// Over-temperature sensor offset estimate structure.
+struct OverTempCalDataPt {
+  // Sensor offset estimate, temperature, and timestamp.
+  float offset[3];
+  float offset_temp;         // [Celsius]
+  uint64_t timestamp_nanos;  // [nanoseconds]
+  // TODO(davejacobs) - Design option: add variance to provide weighting info.
+};
+
+// The following data structure contains all of the necessary components for
+// modeling a sensor's temperature dependency and providing over-temperature
+// offset corrections.
+struct OverTempCal {
+  // Storage for over-temperature model data.
+  struct OverTempCalDataPt model_data[OVERTEMPCAL_MODEL_SIZE];
+
+  // Total number of model data points collected.
+  size_t num_model_pts;
+
+  // Modeled temperature sensitivity, dOffset/dTemp [sensor_units/Celsius].
+  float temp_sensitivity[3];
+
+  // Sensor model equation intercept [sensor_units].
+  float sensor_intercept[3];
+
+  // Timestamp of the last model update.
+  uint64_t modelupdate_timestamp_nanos;  // [nanoseconds]
+
+  // The temperature at which the offset compensation is performed.
+  float temperature_celsius;
+
+  // Pointer to the most recent sensor offset estimate. This is also updated
+  // periodically to point to the offset estimate closest to the current sensor
+  // temperature.
+  struct OverTempCalDataPt *latest_offset;
+
+  ///// Online Model Identification Parameters ////////////////////////////////
+  //
+  // The rules for determining whether a new model fit is computed and the
+  // resulting fit parameters are accepted are:
+  //    1) A minimum number of data points must have been collected:
+  //          num_model_pts >= min_num_model_pts
+  //       NOTE: Collecting 'num_model_pts' and given that only one point is
+  //       kept per temperature bin (spanning a thermal range specified by
+  //       'delta_temp_per_bin'), implies that model data covers at least,
+  //          model_temp_span >= 'num_model_pts' * delta_temp_per_bin
+  //    2) New model updates will not occur for intervals less than:
+  //          (current_timestamp_nanos - modelupdate_timestamp_nanos) <
+  //            min_update_interval_nanos
+  //    3) A new set of model parameters are accepted if:
+  //         i.  The model fit error is less than, 'max_error_limit'. See
+  //             getModelError() for error metric description.
+  //         ii. The model fit parameters must be within certain absolute
+  //             bounds:
+  //               a. ABS(temp_sensitivity) < temp_sensitivity_limit
+  //               b. ABS(sensor_intercept) < sensor_intercept_limit
+  size_t min_num_model_pts;
+  uint64_t min_update_interval_nanos;  // [nanoseconds]
+  float max_error_limit;               // [sensor units]
+  float temp_sensitivity_limit;        // [sensor units/Celsius]
+  float sensor_intercept_limit;        // [sensor units]
+
+  // The rules for accepting new offset estimates into the 'model_data'
+  // collection:
+  //    1) The temperature domain is divided into bins each spanning
+  //       'delta_temp_per_bin'.
+  //    2) Find and replace the i'th 'model_data' estimate data if:
+  //          Let, bin_num = floor(current_temp / delta_temp_per_bin)
+  //          temp_lo_check = bin_num * delta_temp_per_bin
+  //          temp_hi_check = (bin_num + 1) * delta_temp_per_bin
+  //          Check condition:
+  //            temp_lo_check <= model_data[i].offset_temp < temp_hi_check
+  //    3) If nothing was replaced, and the 'model_data' buffer is not full then
+  //       add the sensor offset estimate to the array.
+  //    4) Otherwise (nothing was replaced and buffer is full), replace the
+  //    'latest_offset' with the incoming one.
+  // This approach ensures a uniform spread of collected data, keeps the most
+  // recent estimates in cases where they arrive frequently near a given
+  // temperature, and prevents model oversampling (i.e., dominance of estimates
+  // concentrated at given set of temperatures).
+  float delta_temp_per_bin;        // [Celsius/bin]
+
+  // Timer used to limit the rate at which a search for the nearest offset
+  // estimate is performed.
+  uint64_t nearest_search_timer;   // [nanoseconds]
+
+  // Timer used to limit the rate at which old estimates are removed from
+  // the 'model_data' collection.
+  uint64_t stale_data_timer;       // [nanoseconds]
+
+  // Duration beyond which data will be removed to avoid corrupting the model
+  // with drift-compromised data.
+  uint64_t age_limit_nanos;        // [nanoseconds]
+
+  // Flag set by user to control whether over-temp compensation is used.
+  bool over_temp_enable;
+
+  // True when new compensation model values have been computed; and reset when
+  // overTempCalNewModelUpdateAvailable() is called. This variable indicates
+  // that the following should be stored/updated in persistent system memory:
+  //    1) 'temp_sensitivity' and 'sensor_intercept'.
+  //    2) The sensor offset data pointed to by 'latest_offset'
+  //       (saving timestamp information is not required).
+  bool new_overtemp_model_available;
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+  size_t debug_num_model_updates;  // Total number of model updates.
+  size_t debug_num_estimates;      // Total number of offset estimates.
+  bool debug_print_trigger;        // Flag used to trigger data printout.
+#endif  // OVERTEMPCAL_DBG_ENABLED
+};
+
+/////// FUNCTION PROTOTYPES ///////////////////////////////////////////////////
+
+/*
+ * Initializes the over-temp calibration model identification parameters.
+ *
+ * INPUTS:
+ *   over_temp_cal:             Over-temp main data structure.
+ *   min_num_model_pts:         Minimum number of model points per model
+ *                              calculation update.
+ *   min_update_interval_nanos: Minimum model update interval.
+ *   delta_temp_per_bin:        Temperature span that defines the spacing of
+ *                              collected model estimates.
+ *   max_error_limit:           Model acceptance fit error tolerance.
+ *   age_limit_nanos:           Sets the age limit beyond which a offset
+ *                              estimate is removed from 'model_data'.
+ *   temp_sensitivity_limit:    Values that define the upper limits for the
+ *   sensor_intercept_limit:    model parameters. The acceptance of new model
+ *                              parameters must satisfy:
+ *                          i.  ABS(temp_sensitivity) < temp_sensitivity_limit
+ *                          ii. ABS(sensor_intercept) < sensor_intercept_limit
+ *   over_temp_enable:          Flag that determines whether over-temp sensor
+ *                              offset compensation is applied.
+ */
+void overTempCalInit(struct OverTempCal *over_temp_cal,
+                     size_t min_num_model_pts,
+                     uint64_t min_update_interval_nanos,
+                     float delta_temp_per_bin, float max_error_limit,
+                     uint64_t age_limit_nanos, float temp_sensitivity_limit,
+                     float sensor_intercept_limit, bool over_temp_enable);
+
+/*
+ * Sets the over-temp calibration model parameters.
+ *
+ * INPUTS:
+ *   over_temp_cal:    Over-temp main data structure.
+ *   offset:           Update values for the latest offset estimate (array).
+ *   offset_temp:      Measured temperature for the offset estimate.
+ *   timestamp_nanos:  Timestamp for the offset estimate [nanoseconds].
+ *   temp_sensitivity: Modeled temperature sensitivity (array).
+ *   sensor_intercept: Linear model intercept for the over-temp model (array).
+ *   jump_start_model: When 'true' populates an empty 'model_data' array using
+ *                     valid input model parameters.
+ *
+ * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
+ */
+void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
+                         float offset_temp, uint64_t timestamp_nanos,
+                         const float *temp_sensitivity,
+                         const float *sensor_intercept, bool jump_start_model);
+
+/*
+ * Gets the over-temp calibration model parameters.
+ *
+ * INPUTS:
+ *   over_temp_cal:    Over-temp data structure.
+ * OUTPUTS:
+ *   offset:           Offset values for the latest offset estimate (array).
+ *   offset_temp:      Measured temperature for the offset estimate.
+ *   timestamp_nanos:  Timestamp for the offset estimate [nanoseconds].
+ *   temp_sensitivity: Modeled temperature sensitivity (array).
+ *   sensor_intercept: Linear model intercept for the over-temp model (array).
+ *
+ * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
+ */
+void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset,
+                         float *offset_temp, uint64_t *timestamp_nanos,
+                         float *temp_sensitivity, float *sensor_intercept);
+
+/*
+ * Removes the over-temp compensated offset from the input sensor data.
+ *
+ * INPUTS:
+ *   over_temp_cal:    Over-temp data structure.
+ *   timestamp_nanos:  Timestamp of the sensor estimate update.
+ *   xi, yi, zi:       3-axis sensor data to be compensated.
+ * OUTPUTS:
+ *   xo, yo, zo:       3-axis sensor data that has been compensated.
+ */
+void overTempCalRemoveOffset(struct OverTempCal *over_temp_cal,
+                             uint64_t timestamp_nanos, float xi, float yi,
+                             float zi, float *xo, float *yo, float *zo);
+
+// Returns true when a new over-temp model update is available; and the
+// 'new_overtemp_model_available' flag is reset.
+bool overTempCalNewModelUpdateAvailable(struct OverTempCal *over_temp_cal);
+
+/*
+ * Updates the sensor's offset estimate and conditionally assimilates it into
+ * the over-temp model data set, 'model_data'.
+ *
+ * INPUTS:
+ *   over_temp_cal:       Over-temp data structure.
+ *   timestamp_nanos:     Timestamp of the sensor estimate update.
+ *   offset:              3-axis sensor data to be compensated (array).
+ *   temperature_celsius: Measured temperature for the new sensor estimate.
+ *
+ * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
+ */
+void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
+                                     uint64_t timestamp_nanos,
+                                     const float *offset,
+                                     float temperature_celsius);
+
+// Updates the temperature at which the offset compensation is performed (i.e.,
+// the current measured temperature value). This function is provided mainly for
+// flexibility since temperature updates may come in from a source other than
+// the sensor itself, and at a different rate.
+void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
+                               uint64_t timestamp_nanos,
+                               float temperature_celsius);
+
+/*
+ * Computes the maximum absolute error between the 'model_data' estimates and
+ * the estimate determined by the input model parameters.
+ *   max_error (over all i)
+ *     |model_data[i]->offset_xyz -
+ *       getCompensatedOffset(model_data[i]->offset_temp, temp_sensitivity,
+ *                      sensor_intercept)|
+ *
+ * INPUTS:
+ *   over_temp_cal:    Over-temp data structure.
+ *   temp_sensitivity: Model temperature sensitivity to test (array).
+ *   sensor_intercept: Model intercept to test (array).
+ * OUTPUTS:
+ *   max_error:        Maximum absolute error for the candidate model (array).
+ *
+ * NOTE 1: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
+ * NOTE 2: This function is provided for testing purposes.
+ */
+void getModelError(const struct OverTempCal *over_temp_cal,
+                   const float *temp_sensitivity, const float *sensor_intercept,
+                   float *max_error);
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+// This debug printout function assumes the input sensor data is a gyroscope
+// [rad/sec].
+void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
+                           uint64_t timestamp_nanos);
+#endif  // OVERTEMPCAL_DBG_ENABLED
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_CAL_H_
diff --git a/firmware/os/algos/calibration/util/cal_log.h b/firmware/os/algos/calibration/util/cal_log.h
index dca59fdf..64f70f8e 100644
--- a/firmware/os/algos/calibration/util/cal_log.h
+++ b/firmware/os/algos/calibration/util/cal_log.h
@@ -1,6 +1,3 @@
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_
-
 /*
  * Copyright (C) 2016 The Android Open Source Project
  *
@@ -22,16 +19,23 @@
  * Logging macros for printing formatted strings to Nanohub.
  */
 ///////////////////////////////////////////////////////////////
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_
 
-
-#ifndef LOG_FUNC
-#include <seos.h>
-#define LOG_FUNC(level, fmt, ...) osLog(level, fmt, ##__VA_ARGS__)
-#define LOGD_TAG(tag, fmt, ...) \
-    LOG_FUNC(LOG_DEBUG, "%s " fmt "\n", tag, ##__VA_ARGS__)
-#endif
-
-#define CAL_DEBUG_LOG LOGD_TAG
+#ifdef GCC_DEBUG_LOG
+# include <stdio.h>
+# define CAL_DEBUG_LOG(tag, fmt, ...) \
+  printf("%s " fmt "\n", tag, ##__VA_ARGS__);
+#else  // GCC_DEBUG_LOG
+# include <seos.h>
+# ifndef LOG_FUNC
+#  define LOG_FUNC(level, fmt, ...) osLog(level, fmt, ##__VA_ARGS__)
+# endif  // LOG_FUNC
+# define LOGD_TAG(tag, fmt, ...) \
+  LOG_FUNC(LOG_DEBUG, "%s " fmt "\n", tag, ##__VA_ARGS__)
+# define CAL_DEBUG_LOG(tag, fmt, ...) \
+  osLog(LOG_DEBUG, "%s " fmt, tag, ##__VA_ARGS__);
+#endif  // GCC_DEBUG_LOG
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/firmware/os/algos/common/math/levenberg_marquardt.c b/firmware/os/algos/common/math/levenberg_marquardt.c
new file mode 100644
index 00000000..9c179ac0
--- /dev/null
+++ b/firmware/os/algos/common/math/levenberg_marquardt.c
@@ -0,0 +1,270 @@
+#include "common/math/levenberg_marquardt.h"
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "common/math/mat.h"
+#include "common/math/vec.h"
+
+// FORWARD DECLARATIONS
+////////////////////////////////////////////////////////////////////////
+static bool checkRelativeStepSize(const float *step, const float *state,
+                                  size_t dim, float relative_error_threshold);
+
+static bool computeResidualAndGradients(ResidualAndJacobianFunction func,
+                                        const float *state, const void *f_data,
+                                        float *jacobian,
+                                        float gradient_threshold,
+                                        size_t state_dim, size_t meas_dim,
+                                        float *residual, float *gradient,
+                                        float *hessian);
+
+static bool computeStep(const float *gradient, float *hessian, float *L,
+                        float damping_factor, size_t dim, float *step);
+
+const static float kEps = 1e-10f;
+
+// FUNCTION IMPLEMENTATIONS
+////////////////////////////////////////////////////////////////////////
+void lmSolverInit(struct LmSolver *solver, const struct LmParams *params,
+                  ResidualAndJacobianFunction func) {
+  ASSERT_NOT_NULL(solver);
+  ASSERT_NOT_NULL(params);
+  ASSERT_NOT_NULL(func);
+  memset(solver, 0, sizeof(struct LmSolver));
+  memcpy(&solver->params, params, sizeof(struct LmParams));
+  solver->func = func;
+  solver->num_iter = 0;
+}
+
+void lmSolverDestroy(struct LmSolver *solver) {
+  (void)solver;
+}
+
+void lmSolverSetData(struct LmSolver *solver, struct LmData *data) {
+  ASSERT_NOT_NULL(solver);
+  ASSERT_NOT_NULL(data);
+  solver->data = data;
+}
+
+enum LmStatus lmSolverSolve(struct LmSolver *solver, const float *initial_state,
+                            void *f_data, size_t state_dim, size_t meas_dim,
+                            float *state) {
+  // Initialize parameters.
+  float damping_factor = 0.0f;
+  float v = 2.0f;
+
+  // Check dimensions.
+  if (meas_dim > MAX_LM_MEAS_DIMENSION || state_dim > MAX_LM_STATE_DIMENSION) {
+    return INVALID_DATA_DIMENSIONS;
+  }
+
+  // Check pointers (note that f_data can be null if no additional data is
+  // required by the error function).
+  ASSERT_NOT_NULL(solver);
+  ASSERT_NOT_NULL(initial_state);
+  ASSERT_NOT_NULL(state);
+  ASSERT_NOT_NULL(solver->data);
+
+  // Allocate memory for intermediate variables.
+  float state_new[MAX_LM_STATE_DIMENSION];
+  struct LmData *data = solver->data;
+
+  // state = initial_state, num_iter = 0
+  memcpy(state, initial_state, sizeof(float) * state_dim);
+  solver->num_iter = 0;
+
+  // Compute initial cost function gradient and return if already sufficiently
+  // small to satisfy solution.
+  if (computeResidualAndGradients(solver->func, state, f_data, data->temp,
+                                  solver->params.gradient_threshold, state_dim,
+                                  meas_dim, data->residual,
+                                  data->gradient,
+                                  data->hessian)) {
+    return GRADIENT_SUFFICIENTLY_SMALL;
+  }
+
+  // Initialize damping parameter.
+  damping_factor = solver->params.initial_u_scale *
+      matMaxDiagonalElement(data->hessian, state_dim);
+
+  // Iterate solution.
+  for (solver->num_iter = 0;
+       solver->num_iter < solver->params.max_iterations;
+       ++solver->num_iter) {
+
+    // Compute new solver step.
+    if (!computeStep(data->gradient, data->hessian, data->temp, damping_factor,
+                     state_dim, data->step)) {
+      return CHOLESKY_FAIL;
+    }
+
+    // If the new step is already sufficiently small, we have a solution.
+    if (checkRelativeStepSize(data->step, state, state_dim,
+                              solver->params.relative_step_threshold)) {
+      return RELATIVE_STEP_SUFFICIENTLY_SMALL;
+    }
+
+    // state_new = state + step.
+    vecAdd(state_new, state, data->step, state_dim);
+
+    // Compute new cost function residual.
+    solver->func(state_new, f_data, data->residual_new, NULL);
+
+    // Compute ratio of expected to actual cost function gain for this step.
+    const float gain_ratio = computeGainRatio(data->residual,
+                                              data->residual_new,
+                                              data->step, data->gradient,
+                                              damping_factor, state_dim,
+                                              meas_dim);
+
+    // If gain ratio is positive, the step size is good, otherwise adjust
+    // damping factor and compute a new step.
+    if (gain_ratio > 0.0f) {
+      // Set state to new state vector: state = state_new.
+      memcpy(state, state_new, sizeof(float) * state_dim);
+
+      // Check if cost function gradient is now sufficiently small,
+      // in which case we have a local solution.
+      if (computeResidualAndGradients(solver->func, state, f_data, data->temp,
+                                      solver->params.gradient_threshold,
+                                      state_dim, meas_dim, data->residual,
+                                      data->gradient, data->hessian)) {
+        return GRADIENT_SUFFICIENTLY_SMALL;
+      }
+
+      // Update damping factor based on gain ratio.
+      // Note, this update logic comes from Equation 2.21 in the following:
+      // [Madsen, Kaj, Hans Bruun Nielsen, and Ole Tingleff.
+      // "Methods for non-linear least squares problems." (2004)].
+      const float tmp = 2.f * gain_ratio - 1.f;
+      damping_factor *= NANO_MAX(0.33333f, 1.f - tmp * tmp * tmp);
+      v = 2.f;
+    } else {
+      // Update damping factor and try again.
+      damping_factor *= v;
+      v *= 2.f;
+    }
+  }
+
+  return HIT_MAX_ITERATIONS;
+}
+
+float computeGainRatio(const float *residual, const float *residual_new,
+                       const float *step, const float *gradient,
+                       float damping_factor, size_t state_dim,
+                       size_t meas_dim) {
+  // Compute true_gain = residual' residual - residual_new' residual_new.
+  const float true_gain = vecDot(residual, residual, meas_dim)
+      - vecDot(residual_new, residual_new, meas_dim);
+
+  // predicted gain = 0.5 * step' * (damping_factor * step + gradient).
+  float tmp[MAX_LM_STATE_DIMENSION];
+  vecScalarMul(tmp, step, damping_factor, state_dim);
+  vecAddInPlace(tmp, gradient, state_dim);
+  const float predicted_gain = 0.5f * vecDot(step, tmp, state_dim);
+
+  // Check that we don't divide by zero! If denominator is too small,
+  // set gain_ratio = 1 to use the current step.
+  if (predicted_gain < kEps) {
+    return 1.f;
+  }
+
+  return true_gain / predicted_gain;
+}
+
+/*
+ * Tests if a solution is found based on the size of the step relative to the
+ * current state magnitude. Returns true if a solution is found.
+ *
+ * TODO(dvitus): consider optimization of this function to use squared norm
+ * rather than norm for relative error computation to avoid square root.
+ */
+bool checkRelativeStepSize(const float *step, const float *state,
+                           size_t dim, float relative_error_threshold) {
+  // r = eps * (||x|| + eps)
+  const float relative_error = relative_error_threshold *
+      (vecNorm(state, dim) + relative_error_threshold);
+
+  // solved if ||step|| <= r
+  // use squared version of this compare to avoid square root.
+  return (vecNormSquared(step, dim) <= relative_error * relative_error);
+}
+
+/*
+ * Computes the residual, f(x), as well as the gradient and hessian of the cost
+ * function for the given state.
+ *
+ * Returns a boolean indicating if the computed gradient is sufficiently small
+ * to indicate that a solution has been found.
+ *
+ * INPUTS:
+ * state: state estimate (x) for which to compute the gradient & hessian.
+ * f_data: pointer to parameter data needed for the residual or jacobian.
+ * jacobian: pointer to temporary memory for storing jacobian.
+ *           Must be at least MAX_LM_STATE_DIMENSION * MAX_LM_MEAS_DIMENSION.
+ * gradient_threshold: if gradient is below this threshold, function returns 1.
+ *
+ * OUTPUTS:
+ * residual: f(x).
+ * gradient: - J' f(x), where J = df(x)/dx
+ * hessian: df^2(x)/dx^2 = J' J
+ */
+bool computeResidualAndGradients(ResidualAndJacobianFunction func,
+                                 const float *state, const void *f_data,
+                                 float *jacobian, float gradient_threshold,
+                                 size_t state_dim, size_t meas_dim,
+                                 float *residual, float *gradient,
+                                 float *hessian) {
+  // Compute residual and Jacobian.
+  ASSERT_NOT_NULL(state);
+  ASSERT_NOT_NULL(residual);
+  ASSERT_NOT_NULL(gradient);
+  ASSERT_NOT_NULL(hessian);
+  func(state, f_data, residual, jacobian);
+
+  // Compute the cost function hessian = jacobian' jacobian and
+  // gradient = -jacobian' residual
+  matTransposeMultiplyMat(hessian, jacobian, meas_dim, state_dim);
+  matTransposeMultiplyVec(gradient, jacobian, residual, meas_dim, state_dim);
+  vecScalarMulInPlace(gradient, -1.f, state_dim);
+
+  // Check if solution is found (cost function gradient is sufficiently small).
+  return (vecMaxAbsoluteValue(gradient, state_dim) < gradient_threshold);
+}
+
+/*
+ * Computes the Levenberg-Marquardt solver step to satisfy the following:
+ *    (J'J + uI) * step = - J' f
+ *
+ * INPUTS:
+ * gradient:  -J'f
+ * hessian:  J'J
+ * L: temp memory of at least MAX_LM_STATE_DIMENSION * MAX_LM_STATE_DIMENSION.
+ * damping_factor: u
+ * dim: state dimension
+ *
+ * OUTPUTS:
+ * step: solution to the above equation.
+ * Function returns false if the solution fails (due to cholesky failure),
+ * otherwise returns true.
+ *
+ * Note that the hessian is modified in this function in order to reduce
+ * local memory requirements.
+ */
+bool computeStep(const float *gradient, float *hessian, float *L,
+                 float damping_factor, size_t dim, float *step) {
+
+  // 1) A = hessian + damping_factor * Identity.
+  matAddConstantDiagonal(hessian, damping_factor, dim);
+
+  // 2) Solve A * step = gradient for step.
+  // a) compute cholesky decomposition of A = L L^T.
+  if (!matCholeskyDecomposition(L, hessian, dim)) {
+    return false;
+  }
+
+  // b) solve for step via back-solve.
+  return matLinearSolveCholesky(step, L, gradient, dim);
+}
diff --git a/firmware/os/algos/common/math/levenberg_marquardt.h b/firmware/os/algos/common/math/levenberg_marquardt.h
new file mode 100644
index 00000000..2ffc05f3
--- /dev/null
+++ b/firmware/os/algos/common/math/levenberg_marquardt.h
@@ -0,0 +1,163 @@
+/*
+ * This module contains the definition of a Levenberg-Marquardt solver for
+ * non-linear least squares problems of the following form:
+ *
+ *   arg min  ||f(X)||  =  arg min  f(X)^T f(X)
+ *      X                     X
+ *
+ * where X is an Nx1 state vector and f(X) is an Mx1 nonlinear measurement error
+ * function of X which we wish to minimize the norm of.
+ *
+ * Levenberg-Marquardt solves the above problem through a damped Gauss-Newton
+ * method where the solver step on each iteration is chosen such that:
+ *       (J' J + uI) * step = - J' f(x)
+ * where J = df(x)/dx is the Jacobian of the error function, u is a damping
+ * factor, and I is the identity matrix.
+ *
+ * The algorithm implemented here follows Algorithm 3.16 outlined in this paper:
+ * Madsen, Kaj, Hans Bruun Nielsen, and Ole Tingleff.
+ * "Methods for non-linear least squares problems." (2004).
+ *
+ * This algorithm uses a variant of the Marquardt method for updating the
+ * damping factor which ensures that changes in the factor have no
+ * discontinuities or fluttering behavior between solver steps.
+ */
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_LEVENBERG_MARQUARDT_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_LEVENBERG_MARQUARDT_H_
+
+#include <stddef.h>
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Function pointer for computing residual, f(X), and Jacobian, J = df(X)/dX
+// for a given state value, X, and other parameter data needed for computing
+// these terms, f_data.
+//
+// Note if f_data is not needed, it is allowable for f_data to be passed in
+// as NULL.
+//
+// jacobian is also allowed to be passed in as NULL, and in this case only the
+// residual will be computed and returned.
+typedef void (*ResidualAndJacobianFunction)(const float *state,
+                                            const void *f_data,
+                                            float *residual, float *jacobian);
+
+
+#define MAX_LM_STATE_DIMENSION (10)
+#define MAX_LM_MEAS_DIMENSION (20)
+
+// Structure containing fixed parameters for a single LM solve.
+struct LmParams {
+  // maximum number of iterations allowed by the solver.
+  uint32_t max_iterations;
+
+  // initial scaling factor for setting the damping factor, i.e.:
+  // damping_factor = initial_u_scale * max(J'J).
+  float initial_u_scale;
+
+  // magnitude of the cost function gradient required for solution, i.e.
+  // max(gradient) = max(J'f(x)) < gradient_threshold.
+  float gradient_threshold;
+
+  // magnitude of relative error required for solution step, i.e.
+  // ||step|| / ||state|| < relative_step_thresold.
+  float relative_step_threshold;
+};
+
+// Structure containing data needed for a single LM solve.
+// Defining the data here allows flexibility in how the memory is allocated
+// for the solver.
+struct LmData {
+  float step[MAX_LM_STATE_DIMENSION];
+  float residual[MAX_LM_MEAS_DIMENSION];
+  float residual_new[MAX_LM_MEAS_DIMENSION];
+  float gradient[MAX_LM_MEAS_DIMENSION];
+  float hessian[MAX_LM_STATE_DIMENSION * MAX_LM_STATE_DIMENSION];
+  float temp[MAX_LM_STATE_DIMENSION * MAX_LM_MEAS_DIMENSION];
+};
+
+// Enumeration indicating status of the LM solver.
+enum LmStatus {
+  RUNNING = 0,
+  // Successful solve conditions:
+  RELATIVE_STEP_SUFFICIENTLY_SMALL,  // solver step is sufficiently small.
+  GRADIENT_SUFFICIENTLY_SMALL,  // cost function gradient is below threshold.
+  HIT_MAX_ITERATIONS,
+
+  // Solver failure modes:
+  CHOLESKY_FAIL,
+  INVALID_DATA_DIMENSIONS,
+};
+
+// Structure for containing variables needed for a Levenberg-Marquardt solver.
+struct LmSolver {
+  // Solver parameters.
+  struct LmParams params;
+
+  // Pointer to solver data.
+  struct LmData *data;
+
+  // Function for computing residual (f(x)) and jacobian (df(x)/dx).
+  ResidualAndJacobianFunction func;
+
+  // Number of iterations in current solution.
+  uint32_t num_iter;
+};
+
+// Initializes LM solver with provided parameters and error function.
+void lmSolverInit(struct LmSolver *solver, const struct LmParams *params,
+                  ResidualAndJacobianFunction error_func);
+
+void lmSolverDestroy(struct LmSolver *solver);
+
+// Sets pointer for temporary data needed for an individual LM solve.
+// Note, this must be called prior to calling lmSolverSolve().
+void lmSolverSetData(struct LmSolver *solver, struct LmData *data);
+
+/*
+ * Runs the LM solver for a given set of data and error function.
+ *
+ * INPUTS:
+ * solver : pointer to an struct LmSolver structure.
+ * initial_state : initial guess of the estimation state.
+ * f_data : pointer to additional data needed by the error function.
+ * state_dim : dimension of X.
+ * meas_dim : dimension of f(X), defined in the error function.
+ *
+ * OUTPUTS:
+ * LmStatus : enum indicating state of the solver on completion.
+ * est_state : estimated state.
+ */
+enum LmStatus lmSolverSolve(struct LmSolver *solver, const float *initial_state,
+                            void *f_data, size_t state_dim, size_t meas_dim,
+                            float *est_state);
+
+////////////////////////// TEST UTILITIES ////////////////////////////////////
+// This function is exposed here for testing purposes only.
+/*
+ * Computes the ratio of actual cost function gain over expected cost
+ * function gain for the given solver step.  This ratio is used to adjust
+ * the solver step size for the next solver iteration.
+ *
+ * INPUTS:
+ * residual: f(x) for the current state x.
+ * residual_new: f(x + step) for the new state after the solver step.
+ * step: the solver step.
+ * gradient: gradient of the cost function F(x).
+ * damping_factor: the current damping factor used in the solver.
+ * state_dim: dimension of the state, x.
+ * meas_dim: dimension of f(x).
+ */
+float computeGainRatio(const float *residual, const float *residual_new,
+                       const float *step, const float *gradient,
+                       float damping_factor, size_t state_dim,
+                       size_t meas_dim);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_LEVENBERG_MARQUARDT_H_
diff --git a/firmware/os/algos/common/math/mat.c b/firmware/os/algos/common/math/mat.c
index 8f4270b5..5ab66254 100644
--- a/firmware/os/algos/common/math/mat.c
+++ b/firmware/os/algos/common/math/mat.c
@@ -20,15 +20,28 @@
 #include <float.h>
 #include <nanohub_math.h>
 #include <seos.h>
+#include <stddef.h>
 #include <string.h>
-#include <sys/types.h>
 
-#define kEps 1E-5
+#define EPSILON 1E-5
+#define CHOLESKY_TOLERANCE 1E-6
 
-void initZeroMatrix(struct Mat33 *A) { memset(A->elem, 0.0f, sizeof(A->elem)); }
+// Forward declarations.
+static void mat33SwapRows(struct Mat33 *A, uint32_t i, uint32_t j);
+static uint32_t mat33Maxind(const struct Mat33 *A, uint32_t k);
+static void mat33Rotate(struct Mat33 *A, float c, float s, uint32_t k,
+                        uint32_t l, uint32_t i, uint32_t j);
+
+static void mat44SwapRows(struct Mat44 *A, uint32_t i, uint32_t j);
+
+void initZeroMatrix(struct Mat33 *A) {
+  ASSERT_NOT_NULL(A);
+  memset(A->elem, 0.0f, sizeof(A->elem));
+}
 
 UNROLLED
 void initDiagonalMatrix(struct Mat33 *A, float x) {
+  ASSERT_NOT_NULL(A);
   initZeroMatrix(A);
 
   uint32_t i;
@@ -39,6 +52,10 @@ void initDiagonalMatrix(struct Mat33 *A, float x) {
 
 void initMatrixColumns(struct Mat33 *A, const struct Vec3 *v1,
                        const struct Vec3 *v2, const struct Vec3 *v3) {
+  ASSERT_NOT_NULL(A);
+  ASSERT_NOT_NULL(v1);
+  ASSERT_NOT_NULL(v2);
+  ASSERT_NOT_NULL(v3);
   A->elem[0][0] = v1->x;
   A->elem[0][1] = v2->x;
   A->elem[0][2] = v3->x;
@@ -53,7 +70,9 @@ void initMatrixColumns(struct Mat33 *A, const struct Vec3 *v1,
 }
 
 void mat33Apply(struct Vec3 *out, const struct Mat33 *A, const struct Vec3 *v) {
-  // assert(out != v);
+  ASSERT_NOT_NULL(out);
+  ASSERT_NOT_NULL(A);
+  ASSERT_NOT_NULL(v);
   out->x = A->elem[0][0] * v->x + A->elem[0][1] * v->y + A->elem[0][2] * v->z;
   out->y = A->elem[1][0] * v->x + A->elem[1][1] * v->y + A->elem[1][2] * v->z;
   out->z = A->elem[2][0] * v->x + A->elem[2][1] * v->y + A->elem[2][2] * v->z;
@@ -62,8 +81,11 @@ void mat33Apply(struct Vec3 *out, const struct Mat33 *A, const struct Vec3 *v) {
 UNROLLED
 void mat33Multiply(struct Mat33 *out, const struct Mat33 *A,
                    const struct Mat33 *B) {
-  // assert(out != A);
-  // assert(out != B);
+  ASSERT_NOT_NULL(out);
+  ASSERT_NOT_NULL(A);
+  ASSERT_NOT_NULL(B);
+  ASSERT(out != A);
+  ASSERT(out != B);
 
   uint32_t i;
   for (i = 0; i < 3; ++i) {
@@ -82,6 +104,7 @@ void mat33Multiply(struct Mat33 *out, const struct Mat33 *A,
 
 UNROLLED
 void mat33ScalarMul(struct Mat33 *A, float c) {
+  ASSERT_NOT_NULL(A);
   uint32_t i;
   for (i = 0; i < 3; ++i) {
     uint32_t j;
@@ -93,6 +116,8 @@ void mat33ScalarMul(struct Mat33 *A, float c) {
 
 UNROLLED
 void mat33Add(struct Mat33 *out, const struct Mat33 *A) {
+  ASSERT_NOT_NULL(out);
+  ASSERT_NOT_NULL(A);
   uint32_t i;
   for (i = 0; i < 3; ++i) {
     uint32_t j;
@@ -104,6 +129,8 @@ void mat33Add(struct Mat33 *out, const struct Mat33 *A) {
 
 UNROLLED
 void mat33Sub(struct Mat33 *out, const struct Mat33 *A) {
+  ASSERT_NOT_NULL(out);
+  ASSERT_NOT_NULL(A);
   uint32_t i;
   for (i = 0; i < 3; ++i) {
     uint32_t j;
@@ -115,6 +142,7 @@ void mat33Sub(struct Mat33 *out, const struct Mat33 *A) {
 
 UNROLLED
 int mat33IsPositiveSemidefinite(const struct Mat33 *A, float tolerance) {
+  ASSERT_NOT_NULL(A);
   uint32_t i;
   for (i = 0; i < 3; ++i) {
     if (A->elem[i][i] < 0.0f) {
@@ -137,9 +165,11 @@ int mat33IsPositiveSemidefinite(const struct Mat33 *A, float tolerance) {
 UNROLLED
 void mat33MultiplyTransposed(struct Mat33 *out, const struct Mat33 *A,
                              const struct Mat33 *B) {
-  // assert(out != A);
-  // assert(out != B);
-
+  ASSERT(out != A);
+  ASSERT(out != B);
+  ASSERT_NOT_NULL(out);
+  ASSERT_NOT_NULL(A);
+  ASSERT_NOT_NULL(B);
   uint32_t i;
   for (i = 0; i < 3; ++i) {
     uint32_t j;
@@ -158,9 +188,11 @@ void mat33MultiplyTransposed(struct Mat33 *out, const struct Mat33 *A,
 UNROLLED
 void mat33MultiplyTransposed2(struct Mat33 *out, const struct Mat33 *A,
                               const struct Mat33 *B) {
-  // assert(out != A);
-  // assert(out != B);
-
+  ASSERT(out != A);
+  ASSERT(out != B);
+  ASSERT_NOT_NULL(out);
+  ASSERT_NOT_NULL(A);
+  ASSERT_NOT_NULL(B);
   uint32_t i;
   for (i = 0; i < 3; ++i) {
     uint32_t j;
@@ -178,6 +210,8 @@ void mat33MultiplyTransposed2(struct Mat33 *out, const struct Mat33 *A,
 
 UNROLLED
 void mat33Invert(struct Mat33 *out, const struct Mat33 *A) {
+  ASSERT_NOT_NULL(out);
+  ASSERT_NOT_NULL(A);
   float t;
   initDiagonalMatrix(out, 1.0f);
 
@@ -204,7 +238,11 @@ void mat33Invert(struct Mat33 *out, const struct Mat33 *A) {
         out->elem[swap][k] = t;
       }
     }
-
+    // divide by zero guard.
+    ASSERT(fabs(tmp.elem[i][i]) > 0);
+    if(!(fabs(tmp.elem[i][i]) > 0)) {
+      return;
+    }
     t = 1.0f / tmp.elem[i][i];
     for (k = 0; k < 3; ++k) {
       tmp.elem[i][k] *= t;
@@ -225,6 +263,8 @@ void mat33Invert(struct Mat33 *out, const struct Mat33 *A) {
 
 UNROLLED
 void mat33Transpose(struct Mat33 *out, const struct Mat33 *A) {
+  ASSERT_NOT_NULL(out);
+  ASSERT_NOT_NULL(A);
   uint32_t i;
   for (i = 0; i < 3; ++i) {
     uint32_t j;
@@ -235,42 +275,8 @@ void mat33Transpose(struct Mat33 *out, const struct Mat33 *A) {
 }
 
 UNROLLED
-void mat33DecomposeLup(struct Mat33 *LU, struct Size3 *pivot) {
-  const uint32_t N = 3;
-  uint32_t i, j, k;
-
-  for (k = 0; k < N; ++k) {
-    pivot->elem[k] = k;
-    float max = fabsf(LU->elem[k][k]);
-    for (j = k + 1; j < N; ++j) {
-      if (max < fabsf(LU->elem[j][k])) {
-        max = fabsf(LU->elem[j][k]);
-        pivot->elem[k] = j;
-      }
-    }
-
-    if (pivot->elem[k] != k) {
-      mat33SwapRows(LU, k, pivot->elem[k]);
-    }
-
-    if (fabsf(LU->elem[k][k]) < kEps) {
-      continue;
-    }
-
-    for (j = k + 1; j < N; ++j) {
-      LU->elem[k][j] /= LU->elem[k][k];
-    }
-
-    for (i = k + 1; i < N; ++i) {
-      for (j = k + 1; j < 3; ++j) {
-        LU->elem[i][j] -= LU->elem[i][k] * LU->elem[k][j];
-      }
-    }
-  }
-}
-
-UNROLLED
 void mat33SwapRows(struct Mat33 *A, const uint32_t i, const uint32_t j) {
+  ASSERT_NOT_NULL(A);
   const uint32_t N = 3;
   uint32_t k;
 
@@ -286,49 +292,11 @@ void mat33SwapRows(struct Mat33 *A, const uint32_t i, const uint32_t j) {
 }
 
 UNROLLED
-void mat33Solve(const struct Mat33 *A, struct Vec3 *x, const struct Vec3 *b,
-                const struct Size3 *pivot) {
-  const uint32_t N = 3;
-  uint32_t i, k;
-
-  float bCopy[N];
-  bCopy[0] = b->x;
-  bCopy[1] = b->y;
-  bCopy[2] = b->z;
-
-  float _x[N];
-  for (k = 0; k < N; ++k) {
-    if (pivot->elem[k] != k) {
-      float tmp = bCopy[k];
-      bCopy[k] = bCopy[pivot->elem[k]];
-      bCopy[pivot->elem[k]] = tmp;
-    }
-
-    _x[k] = bCopy[k];
-    for (i = 0; i < k; ++i) {
-      _x[k] -= _x[i] * A->elem[k][i];
-    }
-    _x[k] /= A->elem[k][k];
-  }
-
-  for (k = N; k-- > 0;) {
-    for (i = k + 1; i < N; ++i) {
-      _x[k] -= _x[i] * A->elem[k][i];
-    }
-  }
-
-  initVec3(x, _x[0], _x[1], _x[2]);
-}
-
-/* Returns the eigenvalues and corresponding eigenvectors of the _symmetric_
- matrix.
- The i-th eigenvalue corresponds to the eigenvector in the i-th _row_ of
- "eigenvecs".
- */
-
-UNROLLED
 void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals,
                         struct Mat33 *eigenvecs) {
+  ASSERT_NOT_NULL(S);
+  ASSERT_NOT_NULL(eigenvals);
+  ASSERT_NOT_NULL(eigenvecs);
   const uint32_t N = 3;
   uint32_t i, j, k, l, m;
 
@@ -354,7 +322,7 @@ void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals,
     l = ind[m];
     float p = S->elem[k][l];
 
-    if (fabsf(p) < kEps) {
+    if (fabsf(p) < EPSILON) {
       break;
     }
 
@@ -405,7 +373,7 @@ void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals,
       }
     }
 
-    if (sum < kEps) {
+    if (sum < EPSILON) {
       break;
     }
   }
@@ -433,6 +401,7 @@ void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals,
 // index of largest off-diagonal element in row k
 UNROLLED
 uint32_t mat33Maxind(const struct Mat33 *A, uint32_t k) {
+  ASSERT_NOT_NULL(A);
   const uint32_t N = 3;
 
   uint32_t m = k + 1;
@@ -449,13 +418,16 @@ uint32_t mat33Maxind(const struct Mat33 *A, uint32_t k) {
 
 void mat33Rotate(struct Mat33 *A, float c, float s, uint32_t k, uint32_t l,
                  uint32_t i, uint32_t j) {
+  ASSERT_NOT_NULL(A);
   float tmp = c * A->elem[k][l] - s * A->elem[i][j];
   A->elem[i][j] = s * A->elem[k][l] + c * A->elem[i][j];
   A->elem[k][l] = tmp;
 }
 
 void mat44Apply(struct Vec4 *out, const struct Mat44 *A, const struct Vec4 *v) {
-  // assert(out != v);
+  ASSERT_NOT_NULL(out);
+  ASSERT_NOT_NULL(A);
+  ASSERT_NOT_NULL(v);
 
   out->x = A->elem[0][0] * v->x + A->elem[0][1] * v->y + A->elem[0][2] * v->z +
            A->elem[0][3] * v->w;
@@ -472,6 +444,8 @@ void mat44Apply(struct Vec4 *out, const struct Mat44 *A, const struct Vec4 *v) {
 
 UNROLLED
 void mat44DecomposeLup(struct Mat44 *LU, struct Size4 *pivot) {
+  ASSERT_NOT_NULL(LU);
+  ASSERT_NOT_NULL(pivot);
   const uint32_t N = 4;
   uint32_t i, j, k;
 
@@ -489,7 +463,7 @@ void mat44DecomposeLup(struct Mat44 *LU, struct Size4 *pivot) {
       mat44SwapRows(LU, k, pivot->elem[k]);
     }
 
-    if (fabsf(LU->elem[k][k]) < kEps) {
+    if (fabsf(LU->elem[k][k]) < EPSILON) {
       continue;
     }
 
@@ -507,6 +481,7 @@ void mat44DecomposeLup(struct Mat44 *LU, struct Size4 *pivot) {
 
 UNROLLED
 void mat44SwapRows(struct Mat44 *A, const uint32_t i, const uint32_t j) {
+  ASSERT_NOT_NULL(A);
   const uint32_t N = 4;
   uint32_t k;
 
@@ -524,6 +499,10 @@ void mat44SwapRows(struct Mat44 *A, const uint32_t i, const uint32_t j) {
 UNROLLED
 void mat44Solve(const struct Mat44 *A, struct Vec4 *x, const struct Vec4 *b,
                 const struct Size4 *pivot) {
+  ASSERT_NOT_NULL(A);
+  ASSERT_NOT_NULL(x);
+  ASSERT_NOT_NULL(b);
+  ASSERT_NOT_NULL(pivot);
   const uint32_t N = 4;
   uint32_t i, k;
 
@@ -556,3 +535,148 @@ void mat44Solve(const struct Mat44 *A, struct Vec4 *x, const struct Vec4 *b,
 
   initVec4(x, _x[0], _x[1], _x[2], _x[3]);
 }
+
+float matMaxDiagonalElement(const float *square_mat, size_t n) {
+  ASSERT_NOT_NULL(square_mat);
+  ASSERT(n > 0);
+  size_t i;
+  float max = square_mat[0];
+  const size_t n_square = n * n;
+  const size_t offset = n + 1;
+  for (i = offset; i < n_square; i += offset) {
+    if (square_mat[i] > max) {
+      max = square_mat[i];
+    }
+  }
+  return max;
+}
+
+void matAddConstantDiagonal(float *square_mat, float u, size_t n) {
+  ASSERT_NOT_NULL(square_mat);
+  size_t i;
+  const size_t n_square = n * n;
+  const size_t offset = n + 1;
+  for (i = 0; i < n_square; i += offset) {
+    square_mat[i] += u;
+  }
+}
+
+void matTransposeMultiplyMat(float *out, const float *A,
+                             size_t nrows, size_t ncols) {
+  ASSERT_NOT_NULL(out);
+  ASSERT_NOT_NULL(A);
+  size_t i;
+  size_t j;
+  size_t k;
+  memset(out, 0, sizeof(float) * ncols * ncols);
+  for (i = 0; i < ncols; ++i) {
+    for (j = 0; j < ncols; ++j) {
+      // Since A' * A is symmetric, can use upper diagonal elements
+      // to fill in the lower diagonal without recomputing.
+      if (j < i) {
+        out[i * ncols + j] = out[j * ncols + i];
+      } else {
+        // mat_out[i, j] = ai ' * aj
+        out[i * ncols + j] = 0;
+        for (k = 0; k < nrows; ++k) {
+          out[i * ncols + j] += A[k * ncols + i] *
+              A[k * ncols + j];
+        }
+      }
+    }
+  }
+}
+
+void matMultiplyVec(float *out, const float *A, const float *v,
+                    size_t nrows, size_t ncols) {
+  ASSERT_NOT_NULL(out);
+  ASSERT_NOT_NULL(A);
+  ASSERT_NOT_NULL(v);
+  size_t i;
+  for (i = 0; i < nrows; ++i) {
+    const float *row = &A[i * ncols];
+    out[i] = vecDot(row, v, ncols);
+  }
+}
+
+void matTransposeMultiplyVec(float *out, const float *A, const float *v,
+                             size_t nrows, size_t ncols) {
+  ASSERT_NOT_NULL(out);
+  ASSERT_NOT_NULL(A);
+  ASSERT_NOT_NULL(v);
+  size_t i, j;
+  for (i = 0; i < ncols; ++i) {
+    out[i] = 0;
+    for (j = 0; j < nrows; ++j) {
+      out[i] += A[j * ncols + i] * v[j];
+    }
+  }
+}
+
+bool matLinearSolveCholesky(float *x, const float *L, const float *b, size_t n) {
+  ASSERT_NOT_NULL(x);
+  ASSERT_NOT_NULL(L);
+  ASSERT_NOT_NULL(b);
+  int32_t i, j;  // Loops below require signed integers.
+  float sum = 0.0f;
+  // 1. Solve Ly = b through forward substitution. Use x[] to store y.
+  for (i = 0; i < n; ++i) {
+    sum = 0.0f;
+    for (j = 0; j < i; ++j) {
+      sum += L[i * n + j] * x[j];
+    }
+    // Check for non-zero diagonals (don't divide by zero).
+    if (L[i * n + i] < EPSILON) {
+      return false;
+    }
+    x[i] = (b[i] - sum) / L[i * n + i];
+  }
+
+  // 2. Solve L'x = y through backwards substitution. Use x[] to store both
+  // y and x.
+  for (i = n - 1; i >= 0; --i) {
+    sum = 0.0f;
+    for (j = i + 1; j < n; ++j) {
+      sum += L[j * n + i] * x[j];
+    }
+    x[i] = (x[i] - sum) / L[i * n + i];
+  }
+
+  return true;
+}
+
+bool matCholeskyDecomposition(float *L, const float *A, size_t n) {
+  ASSERT_NOT_NULL(L);
+  ASSERT_NOT_NULL(A);
+  size_t i, j, k;
+  float sum = 0.0f;
+  // initialize L to zero.
+  memset(L, 0, sizeof(float) * n * n);
+
+  for (i = 0; i < n; ++i) {
+    // compute L[i][i] = sqrt(A[i][i] - sum_k = 1:i-1 L^2[i][k])
+    sum = 0.0f;
+    for (k = 0; k < i; ++k) {
+      sum += L[i * n + k] * L[i * n + k];
+    }
+    sum = A[i * n + i] - sum;
+    // If diagonal element of L is too small, cholesky fails.
+    if (sum < CHOLESKY_TOLERANCE) {
+      return false;
+    }
+    L[i * n + i] = sqrtf(sum);
+
+    // for j = i+1:N,  compute L[j][i] =
+    //      (1/L[i][i]) * (A[i][j] - sum_k = 1:i-1 L[i][k] * L[j][k])
+    for (j = i + 1; j < n; ++j) {
+      sum = 0.0f;
+      for (k = 0; k < i; ++k) {
+        sum += L[i * n + k] * L[j * n + k];
+      }
+      // division okay because magnitude of L[i][i] already checked above.
+      L[j * n + i] = (A[i * n + j] - sum) / L[i * n + i];
+    }
+  }
+
+  return true;
+}
diff --git a/firmware/os/algos/common/math/mat.h b/firmware/os/algos/common/math/mat.h
index f8aad436..13494f55 100644
--- a/firmware/os/algos/common/math/mat.h
+++ b/firmware/os/algos/common/math/mat.h
@@ -1,6 +1,3 @@
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MAT_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MAT_H_
-
 /*
  * Copyright (C) 2016 The Android Open Source Project
  *
@@ -16,9 +13,28 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
+/////////////////////////////////////////////////////////////////////////
+/*
+ * This module contains matrix math utilities for the following datatypes:
+ * -) Mat33 structures for 3x3 dimensional matrices
+ * -) Mat44 structures for 4x4 dimensional matrices
+ * -) floating point arrays for NxM dimensional matrices.
+ *
+ * Note that the Mat33 and Mat44 utilities were ported from the Android
+ * repository and maintain dependencies in that separate codebase. As a
+ * result, the function signatures were left untouched for compatibility with
+ * this legacy code, despite certain style violations. In particular, for this
+ * module the function argument ordering is outputs before inputs. This style
+ * violation will be addressed once the full set of dependencies in Android
+ * have been brought into this repository.
+ */
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MAT_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MAT_H_
 
+#include <stdbool.h>
+#include <stddef.h>
 #include <stdint.h>
-#include <sys/types.h>
+
 #include "common/math/vec.h"
 
 #ifdef __cplusplus
@@ -41,60 +57,172 @@ struct Size4 {
   uint32_t elem[4];
 };
 
+// 3x3 MATRIX MATH /////////////////////////////////////////////////////////////
 void initZeroMatrix(struct Mat33 *A);
+
+// Updates A with the value x on the main diagonal and 0 on the off diagonals,
+// i.e.:
+// A = [x 0 0
+//      0 x 0
+//      0 0 x]
 void initDiagonalMatrix(struct Mat33 *A, float x);
 
+// Updates A such that the columns are given by the provided vectors, i.e.:
+// A = [v1 v2 v3].
 void initMatrixColumns(struct Mat33 *A, const struct Vec3 *v1,
                        const struct Vec3 *v2, const struct Vec3 *v3);
 
+// Updates out with the multiplication of A with v, i.e.:
+// out = A v.
 void mat33Apply(struct Vec3 *out, const struct Mat33 *A, const struct Vec3 *v);
+
+// Updates out with the multiplication of A with B, i.e.:
+// out =  A B.
 void mat33Multiply(struct Mat33 *out, const struct Mat33 *A,
                    const struct Mat33 *B);
+
+// Updates A by scaling all entries by the provided scalar c, i.e.:
+// A = A c.
 void mat33ScalarMul(struct Mat33 *A, float c);
 
+// Updates out by adding A to out, i.e.:
+// out = out + A.
 void mat33Add(struct Mat33 *out, const struct Mat33 *A);
+
+// Updates out by subtracting A from out, i.e.:
+// out = out - A.
 void mat33Sub(struct Mat33 *out, const struct Mat33 *A);
 
+// Returns 1 if the minimum eigenvalue of the matrix A is greater than the
+// given tolerance. Note that the tolerance is assumed to be greater than 0.
+// I.e., returns: 1[min(eig(A)) > tolerance].
+// NOTE: this function currently only checks matrix symmetry and positivity
+// of the diagonals which is insufficient for testing positive semidefinite.
 int mat33IsPositiveSemidefinite(const struct Mat33 *A, float tolerance);
 
+// Updates out with the inverse of the matrix A, i.e.:
 // out = A^(-1)
 void mat33Invert(struct Mat33 *out, const struct Mat33 *A);
 
+// Updates out with the multiplication of A's transpose with B, i.e.:
 // out = A^T B
 void mat33MultiplyTransposed(struct Mat33 *out, const struct Mat33 *A,
                              const struct Mat33 *B);
 
+// Updates out with the multiplication of A with B's transpose, i.e.:
 // out = A B^T
 void mat33MultiplyTransposed2(struct Mat33 *out, const struct Mat33 *A,
                               const struct Mat33 *B);
 
+// Updates out with the transpose of A, i.e.:
 // out = A^T
 void mat33Transpose(struct Mat33 *out, const struct Mat33 *A);
 
-void mat33DecomposeLup(struct Mat33 *LU, struct Size3 *pivot);
-
-void mat33SwapRows(struct Mat33 *A, const uint32_t i, const uint32_t j);
-
-void mat33Solve(const struct Mat33 *A, struct Vec3 *x, const struct Vec3 *b,
-                const struct Size3 *pivot);
-
+// Returns the eigenvalues and corresponding eigenvectors of the symmetric
+// matrix S.
+// The i-th eigenvalue corresponds to the eigenvector in the i-th row of
+// the matrix eigenvecs.
 void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals,
                         struct Mat33 *eigenvecs);
 
-uint32_t mat33Maxind(const struct Mat33 *A, uint32_t k);
-
-void mat33Rotate(struct Mat33 *A, float c, float s, uint32_t k, uint32_t l,
-                 uint32_t i, uint32_t j);
 
+// 4x4 MATRIX MATH /////////////////////////////////////////////////////////////
+// Updates out with the multiplication of A and v, i.e.:
+// out = Av.
 void mat44Apply(struct Vec4 *out, const struct Mat44 *A, const struct Vec4 *v);
 
+// Decomposes the given matrix LU inplace, such that:
+// LU = P' * L * U.
+// where L is a lower-diagonal matrix, U is an upper-diagonal matrix, and P is a
+// permutation matrix.
+//
+// L and U are stored compactly in the returned LU matrix such that:
+// -) the superdiagonal elements make up "U" (with a diagonal of 1.0s),
+// -) the subdiagonal and diagonal elements make up "L".
+// e.g. if the returned LU matrix is:
+//      LU = [A11 A12 A13 A14
+//            A21 A22 A23 A24
+//            A31 A32 A33 A34
+//            A41 A42 A43 A44], then:
+//       L = [A11  0   0   0      and   U = [ 1  A12 A13 A14
+//            A21 A22  0   0                  0   1  A23 A24
+//            A31 A32 A33  0                  0   0   1  A34
+//            A41 A42 A43 A44]                0   0   0   1 ]
+//
+// The permutation matrix P can be reproduced from returned pivot vector as:
+// matrix P(N);
+// P.identity();
+// for (size_t i = 0; i < N; ++i) {
+//    P.swapRows(i, pivot[i]);
+// }
 void mat44DecomposeLup(struct Mat44 *LU, struct Size4 *pivot);
 
-void mat44SwapRows(struct Mat44 *A, const uint32_t i, const uint32_t j);
-
+// Solves the linear system A x = b for x, where A is a compact LU decomposition
+// (i.e. the LU matrix from mat44DecomposeLup) and pivot is the corresponding
+// row pivots for the permutation matrix (also from mat44DecomposeLup).
 void mat44Solve(const struct Mat44 *A, struct Vec4 *x, const struct Vec4 *b,
                 const struct Size4 *pivot);
 
+// MXN MATRIX MATH /////////////////////////////////////////////////////////////
+/*
+ * The following functions define basic math functionality for matrices of
+ * arbitrary dimension.
+ *
+ * All matrices used in these functions are assumed to be row major, i.e. if:
+ * A = [1 2 3
+ *      4 5 6
+ *      7 8 9]
+ * then when A is passed into one of the functions below, the order of
+ * elements is assumed to be [1 2 3 4 5 6 7 8 9].
+ */
+
+// Returns the maximum diagonal element of the given matrix.
+// The matrix is assumed to be square, of size n x n.
+float matMaxDiagonalElement(const float *square_mat, size_t n);
+
+// Adds a constant value to the diagonal of the given square n x n matrix and
+// returns the updated matrix in place:
+// A = A + uI
+void matAddConstantDiagonal(float *square_mat, float u, size_t n);
+
+// Updates out with the result of A's transpose multiplied with A (i.e. A^T A).
+// A is a matrix with dimensions nrows x ncols.
+// out is a matrix with dimensions ncols x ncols.
+void matTransposeMultiplyMat(float *out, const float *A,
+                             size_t nrows, size_t ncols);
+
+// Updates out with the result of A's transpose multiplied with v (i.e. A^T v).
+// A is a matrix with dimensions nrows x ncols.
+// v is a vector of dimension nrows.
+// out is a vector of dimension ncols.
+void matTransposeMultiplyVec(float* out, const float *A, const float *v,
+                             size_t nrows, size_t ncols);
+
+// Updates out with the result of A multiplied with v (i.e. out = Av).
+// A is a matrix with dimensions nrows x ncols.
+// v is a vector of dimension ncols.
+// out is a vector of dimension nrows.
+void matMultiplyVec(float *out, const float *A, const float *v,
+                    size_t nrows, size_t ncols);
+
+// Solves the linear system L L^T x = b for x, where L is a lower diagonal,
+// symmetric matrix, i.e. the Cholesky factor of a matrix A = L L^T.
+// L is a lower-diagonal matrix of dimension n x n.
+// b is a vector of dimension n.
+// x is a vector of dimension n.
+// Returns true if the solver succeeds.
+bool matLinearSolveCholesky(float *x, const float *L, const float *b,
+                            size_t n);
+
+// Performs the Cholesky decomposition on the given matrix A such that:
+// A = L L^T, where L, the Cholesky factor, is a lower diagonal matrix.
+// Updates the provided L matrix with the Cholesky factor.
+// This decomposition is only successful for symmetric, positive definite
+// matrices A.
+// Returns true if the solver succeeds (will fail if the matrix is not
+// symmetric, positive definite).
+bool matCholeskyDecomposition(float *L, const float *A, size_t n);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/firmware/os/algos/common/math/quat.h b/firmware/os/algos/common/math/quat.h
index f23fce4b..3b6937b1 100644
--- a/firmware/os/algos/common/math/quat.h
+++ b/firmware/os/algos/common/math/quat.h
@@ -1,6 +1,3 @@
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_QUAT_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_QUAT_H_
-
 /*
  * Copyright (C) 2016 The Android Open Source Project
  *
@@ -16,6 +13,8 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_QUAT_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_QUAT_H_
 
 #include "common/math/mat.h"
 #include "common/math/vec.h"
diff --git a/firmware/os/algos/common/math/vec.c b/firmware/os/algos/common/math/vec.c
index 85533cac..97b2b8cb 100644
--- a/firmware/os/algos/common/math/vec.c
+++ b/firmware/os/algos/common/math/vec.c
@@ -19,6 +19,9 @@
 
 void findOrthogonalVector(float inX, float inY, float inZ, float *outX,
                           float *outY, float *outZ) {
+  ASSERT_NOT_NULL(outX);
+  ASSERT_NOT_NULL(outY);
+  ASSERT_NOT_NULL(outZ);
   float x, y, z;
 
   // discard the one with the smallest absolute value
@@ -37,9 +40,95 @@ void findOrthogonalVector(float inX, float inY, float inZ, float *outX,
   }
 
   float magSquared = x * x + y * y + z * z;
-  float invMag = 1.0f / sqrtf(magSquared);
-
+  ASSERT(magSquared > 0);
+  // Only set invMag if magSquared is non-zero.
+  float invMag = 1.0f;
+  if (magSquared > 0) {
+    invMag = 1.0f / sqrtf(magSquared);
+  }
   *outX = x * invMag;
   *outY = y * invMag;
   *outZ = z * invMag;
 }
+
+void vecAdd(float *u, const float *v, const float *w, int dim) {
+  ASSERT_NOT_NULL(u);
+  ASSERT_NOT_NULL(v);
+  ASSERT_NOT_NULL(w);
+  int i;
+  for (i = 0; i < dim; i++) {
+    u[i] = v[i] + w[i];
+  }
+}
+
+void vecAddInPlace(float *v, const float *w, int dim) {
+  ASSERT_NOT_NULL(v);
+  ASSERT_NOT_NULL(w);
+  int i;
+  for (i = 0; i < dim; i++) {
+    v[i] += w[i];
+  }
+}
+
+void vecSub(float *u, const float *v, const float *w, int dim) {
+  ASSERT_NOT_NULL(u);
+  ASSERT_NOT_NULL(v);
+  ASSERT_NOT_NULL(w);
+  int i;
+  for (i = 0; i < dim; i++) {
+    u[i] = v[i] - w[i];
+  }
+}
+
+void vecScalarMul(float *u, const float *v, float c, int dim) {
+  ASSERT_NOT_NULL(u);
+  ASSERT_NOT_NULL(v);
+  int i;
+  for (i = 0; i < dim; i++) {
+    u[i] = c * v[i];
+  }
+}
+
+void vecScalarMulInPlace(float *u, float c, int dim) {
+  ASSERT_NOT_NULL(u);
+  int i;
+  for (i = 0; i < dim; i++) {
+    u[i] *= c;
+  }
+}
+
+float vecNorm(const float *v, int dim) {
+  ASSERT_NOT_NULL(v);
+  float norm_sq = vecNormSquared(v, dim);
+  return sqrtf(norm_sq);
+}
+
+float vecNormSquared(const float *v, int dim) {
+  ASSERT_NOT_NULL(v);
+  return vecDot(v, v, dim);
+}
+
+float vecDot(const float *v, const float *w, int dim) {
+  ASSERT_NOT_NULL(v);
+  ASSERT_NOT_NULL(w);
+  int i;
+  float result = 0;
+  for (i = 0; i < dim; ++i) {
+    result += v[i] * w[i];
+  }
+  return result;
+}
+
+float vecMaxAbsoluteValue(const float *v, int dim) {
+  ASSERT_NOT_NULL(v);
+  float max = NANO_ABS(v[0]);
+  float tmp;
+  int i;
+  for (i = 1; i < dim; ++i) {
+    tmp = NANO_ABS(v[i]);
+    if(tmp > max) {
+      max = tmp;
+    }
+  }
+  return max;
+}
diff --git a/firmware/os/algos/common/math/vec.h b/firmware/os/algos/common/math/vec.h
index cb5f08dd..77ab492d 100644
--- a/firmware/os/algos/common/math/vec.h
+++ b/firmware/os/algos/common/math/vec.h
@@ -1,6 +1,3 @@
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_
-
 /*
  * Copyright (C) 2016 The Android Open Source Project
  *
@@ -17,7 +14,27 @@
  * limitations under the License.
  */
 
+/////////////////////////////////////////////////////////////////////////
+/*
+ * This module contains vector math utilities for the following datatypes:
+ * -) Vec3 structures for 3-dimensional vectors
+ * -) Vec4 structures for 4-dimensional vectors
+ * -) floating point arrays for N-dimensional vectors.
+ *
+ * Note that the Vec3 and Vec4 utilties were ported from the Android
+ * repository and maintain dependenices in that separate codebase. As a
+ * result, the function signatures were left untouched for compatibility with
+ * this legacy code, despite certain style violations. In particular, for this
+ * module the function argument ordering is outputs before inputs. This style
+ * violation will be addressed once the full set of dependencies in Android
+ * have been brought into this repository.
+ */
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_
+
 #include <nanohub_math.h>
+#include <stddef.h>
+#include "util/nano_assert.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -31,66 +48,147 @@ struct Vec4 {
   float x, y, z, w;
 };
 
+#ifndef NANO_ABS
+#define NANO_ABS(x) ((x) > 0 ? (x) : -(x))
+#endif
+
+#ifndef NANO_MAX
+#define NANO_MAX(a, b) ((a) > (b)) ? (a) : (b)
+#endif
+
+// 3-DIMENSIONAL VECTOR MATH ///////////////////////////////////////////
 static inline void initVec3(struct Vec3 *v, float x, float y, float z) {
+  ASSERT_NOT_NULL(v);
   v->x = x;
   v->y = y;
   v->z = z;
 }
 
+// Updates v as the sum of v and w.
 static inline void vec3Add(struct Vec3 *v, const struct Vec3 *w) {
+  ASSERT_NOT_NULL(v);
+  ASSERT_NOT_NULL(w);
   v->x += w->x;
   v->y += w->y;
   v->z += w->z;
 }
 
+// Updates v as the subtraction of w from v.
 static inline void vec3Sub(struct Vec3 *v, const struct Vec3 *w) {
+  ASSERT_NOT_NULL(v);
+  ASSERT_NOT_NULL(w);
   v->x -= w->x;
   v->y -= w->y;
   v->z -= w->z;
 }
 
+// Scales v by the scalar c, i.e. v = c * v.
 static inline void vec3ScalarMul(struct Vec3 *v, float c) {
+  ASSERT_NOT_NULL(v);
   v->x *= c;
   v->y *= c;
   v->z *= c;
 }
 
+// Returns the dot product of v and w.
 static inline float vec3Dot(const struct Vec3 *v, const struct Vec3 *w) {
+  ASSERT_NOT_NULL(v);
+  ASSERT_NOT_NULL(w);
   return v->x * w->x + v->y * w->y + v->z * w->z;
 }
 
+// Returns the square of the L2-norm of the given vector.
 static inline float vec3NormSquared(const struct Vec3 *v) {
+  ASSERT_NOT_NULL(v);
   return vec3Dot(v, v);
 }
 
+// Returns the L2-norm of the given vector.
 static inline float vec3Norm(const struct Vec3 *v) {
+  ASSERT_NOT_NULL(v);
   return sqrtf(vec3NormSquared(v));
 }
 
+// Normalizes the provided vector to unit norm. If the provided vector has a
+// norm of zero, the vector will be unchanged.
 static inline void vec3Normalize(struct Vec3 *v) {
-  float invNorm = 1.0f / vec3Norm(v);
-  v->x *= invNorm;
-  v->y *= invNorm;
-  v->z *= invNorm;
+  ASSERT_NOT_NULL(v);
+  float norm = vec3Norm(v);
+  ASSERT(norm > 0);
+  // Only normalize if norm is non-zero.
+  if (norm > 0) {
+    float invNorm = 1.0f / norm;
+    v->x *= invNorm;
+    v->y *= invNorm;
+    v->z *= invNorm;
+  }
 }
 
+// Updates u as the cross product of v and w.
 static inline void vec3Cross(struct Vec3 *u, const struct Vec3 *v,
                              const struct Vec3 *w) {
+  ASSERT_NOT_NULL(u);
+  ASSERT_NOT_NULL(v);
+  ASSERT_NOT_NULL(w);
   u->x = v->y * w->z - v->z * w->y;
   u->y = v->z * w->x - v->x * w->z;
   u->z = v->x * w->y - v->y * w->x;
 }
 
+// Finds a vector orthogonal to the vector [inX, inY, inZ] and returns
+// this in the components [outX, outY, outZ].  The vector is chosen such
+// that the smallest component of [inX, inY, inZ] is set to zero in the
+// output vector. For example, for the in vector [0.01, 4.0, 5.0], this
+// function will return [0, 5.0, -4.0].
+void findOrthogonalVector(float inX, float inY, float inZ, float *outX,
+                          float *outY, float *outZ);
+
+
+// 4-DIMENSIONAL VECTOR MATH ///////////////////////////////////////////
+// Initialize the Vec4 structure with the provided component values.
 static inline void initVec4(struct Vec4 *v, float x, float y, float z,
                             float w) {
+  ASSERT_NOT_NULL(v);
   v->x = x;
   v->y = y;
   v->z = z;
   v->w = w;
 }
 
-void findOrthogonalVector(float inX, float inY, float inZ, float *outX,
-                          float *outY, float *outZ);
+// N-DIMENSIONAL VECTOR MATH ///////////////////////////////////////////
+// Dimension specified by the last argument in all functions below.
+
+// Adds two vectors and returns the sum in the provided vector, i.e.
+// u = v + w.
+void vecAdd(float *u, const float *v, const float *w, int dim);
+
+// Adds two vectors and returns the sum in the first vector, i.e.
+// v = v + w.
+void vecAddInPlace(float *v, const float *w, int dim);
+
+// Subtracts two vectors and returns in the provided vector, i.e.
+// u = v - w.
+void vecSub(float *u, const float *v, const float *w, int dim);
+
+// Scales vector by a scalar and returns in the provided vector, i.e.
+// u = c * v.
+void vecScalarMul(float *u, const float *v, float c, int dim);
+
+// Scales vector by a scalar and returns in the same vector, i.e.
+// v = c * v.
+void vecScalarMulInPlace(float *v, float c, int dim);
+
+// Returns the L2-norm of the given vector.
+float vecNorm(const float *v, int dim);
+
+// Returns the square of the L2-norm of the given vector.
+float vecNormSquared(const float *v, int dim);
+
+// Returns the dot product of v and w.
+float vecDot(const float *v, const float *w, int dim);
+
+// Returns the maximum absolute value in vector.
+float vecMaxAbsoluteValue(const float *v, int dim);
 
 #ifdef __cplusplus
 }
diff --git a/firmware/os/algos/util/nano_assert.h b/firmware/os/algos/util/nano_assert.h
new file mode 100644
index 00000000..c6389c33
--- /dev/null
+++ b/firmware/os/algos/util/nano_assert.h
@@ -0,0 +1,52 @@
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_ASSERT_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_ASSERT_H_
+
+#if defined(__NANOHUB__) || defined(_OS_BUILD_)
+
+// For external nanoapps (__NANOHUB__ defined), use SRC_FILENAME provided
+// by the build system, which has the directory stripped. But allow the
+// full filename for OS builds, where ASSERT should only be used for local
+// development.
+#if !defined(SRC_FILENAME) && defined(_OS_BUILD_)
+#define SRC_FILENAME __FILENAME__
+#endif
+
+#if defined(_OS_BUILD_)
+  #include <seos.h>
+  #define ASSERT_LOG_FUNC(fmt, ...) osLog(LOG_ERROR, fmt, ##__VA_ARGS__)
+#elif defined(__NANOHUB__)
+  #include <syscallDo.h>
+  #define ASSERT_LOG_FUNC(fmt, ...) eOsLog(LOG_ERROR, fmt, ##__VA_ARGS__)
+#endif
+
+// Note that this just logs and doesn't actually stop execution on Nanohub
+#define ASSERT_IMPL(x) do {                                          \
+    if (!(x)) {                                                      \
+      ASSERT_LOG_FUNC("Assertion: %s:%d\n", SRC_FILENAME, __LINE__); \
+    }                                                                \
+  } while (0)
+
+#else  // Off-target testing, e.g. Google3
+
+#define NANO_ASSERT_ENABLED
+#include <assert.h>
+#define ASSERT_IMPL(x) assert(x)
+
+#endif
+
+#ifdef NANO_ASSERT_ENABLED
+#define ASSERT(x) ASSERT_IMPL(x)
+#else
+#define ASSERT(x) ((void)(x))
+#endif
+
+// Use NULL when compiling for C and nullptr for C++.
+#ifdef __cplusplus
+#define ASSERT_NOT_NULL(ptr) ASSERT((ptr) != nullptr)
+#else
+#define ASSERT_NOT_NULL(ptr) ASSERT((ptr) != NULL)
+#endif
+
+#define UNUSED(x) ((void)(sizeof(x)))
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_ASSERT_H_