DO NOT MERGE 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.

Tip of G3 CL: 145593175

Change-Id: I37cb444c8974659b47752fa61ad1c3b2fdc3fdaa

9 files changed, 1183 insertions, 908 deletions

diff --git a/firmware/os/algos/calibration/common/diversity_checker.c b/firmware/os/algos/calibration/common/diversity_checker.c
index 8e5c3e28..312be69c 100644
--- a/firmware/os/algos/calibration/common/diversity_checker.c
+++ b/firmware/os/algos/calibration/common/diversity_checker.c
@@ -25,24 +25,37 @@
 // 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) {
+    float max_min_threshold,
+    float local_field,
+    float threshold_tuning_param,
+    float max_distance_tuning_param) {
   ASSERT_NOT_NULL(diverse_data);
+
   // Initialize parameters.
-  diverse_data->threshold = threshold;
-  diverse_data->max_distance = max_distance;
+  diverse_data->threshold_tuning_param_sq =
+      (threshold_tuning_param * threshold_tuning_param);
+  diverse_data->max_distance_tuning_param_sq =
+      (max_distance_tuning_param * max_distance_tuning_param);
+
+  // Updating the threshold and max_distance using assumed local field.
+  // Testing for zero and negative local_field.
+  if (local_field <= 0) {
+    local_field = 1;
+  }
+  diversityCheckerLocalFieldUpdate(diverse_data, local_field);
   diverse_data->min_num_diverse_vectors = min_num_diverse_vectors;
-  // checking for min_num_diverse_vectors = 0
+
+  // 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);
 }
@@ -53,6 +66,7 @@ void diversityCheckerReset(struct DiversityChecker* 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;
@@ -62,10 +76,13 @@ void diversityCheckerReset(struct DiversityChecker* diverse_data) {
 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;
 
@@ -79,18 +96,22 @@ void diversityCheckerUpdate(
              &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->
@@ -99,6 +120,7 @@ void diversityCheckerUpdate(
            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;
@@ -163,3 +185,16 @@ bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data,
   float var = (acc_norm_square - (acc_norm * acc_norm) * inv) * inv;
   return (var < diverse_data->var_threshold);
 }
+
+void diversityCheckerLocalFieldUpdate(struct DiversityChecker* diverse_data,
+                                      float local_field) {
+  if ( local_field > 0 ) {
+    // Updating threshold based on the local field information.
+    diverse_data->threshold = diverse_data->threshold_tuning_param_sq *
+        (local_field * local_field);
+
+    // Updating max distance based on the local field information.
+    diverse_data->max_distance = diverse_data->max_distance_tuning_param_sq *
+        (local_field * local_field);
+  }
+}
diff --git a/firmware/os/algos/calibration/common/diversity_checker.h b/firmware/os/algos/calibration/common/diversity_checker.h
index 30f53dba..95813434 100644
--- a/firmware/os/algos/calibration/common/diversity_checker.h
+++ b/firmware/os/algos/calibration/common/diversity_checker.h
@@ -53,7 +53,7 @@ extern "C" {
 #endif
 
 #define THREE_AXIS_DATA_DIM (3)   // data is three-dimensional.
-#define NUM_DIVERSE_VECTORS (10)  // Storing 10 data points.
+#define NUM_DIVERSE_VECTORS (20)  // Storing 20 data points.
 
 // Main data struct.
 struct DiversityChecker {
@@ -69,9 +69,17 @@ struct DiversityChecker {
   // Threshold value that is used to check k against.
   float threshold;
 
+  // Threshold tuning paramter used to calculate threshold (k_algo):
+  // threshold = threshold_tuning_param_sq * (local_field)^2.
+  float threshold_tuning_param_sq;
+
   // Maximum distance value.
   float max_distance;
 
+  // Max Distance tuning parameter:
+  // max_distance = max_distance_tuning_param_sq * (local_field)^2.
+  float max_distance_tuning_param_sq;
+
   // Data full bit.
   bool data_full;
 
@@ -84,9 +92,6 @@ struct DiversityChecker {
 };
 
 // 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
@@ -94,13 +99,17 @@ struct DiversityChecker {
 // 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.
+// local_field -> is the assumed local_field (radius of the sphere).
+// threshold_tuning_param ->  threshold tuning parameter used to calculate
+//                            threshold (k_algo).
+// max_distance_tuning_param -> Max distance tuning parameter used to calculate
+//                             max_distance.
 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);
+                          size_t max_num_max_distance, float var_threshold,
+                          float max_min_threshold, float local_field,
+                          float threshold_tuning_param,
+                          float max_distance_tuning_param);
 
 // Resetting the memory and the counters, leaves threshold and max_distance
 // as well as the setup variables for NormQuality check untouched.
@@ -126,6 +135,14 @@ bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data,
                                  float y_bias,
                                  float z_bias);
 
+// This function updates the threshold value and max distance value based on the
+// local field. This ensures a local field independent operation of the
+// diversity checker.
+//
+// threshold = (threshold_tuning_param * local_field)^2
+// max_distance = (max_distance_tuning_param * local_field)^2
+void diversityCheckerLocalFieldUpdate(struct DiversityChecker* diverse_data,
+                                      float local_field);
 #ifdef __cplusplus
 }
 #endif
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
index 1c609551..e6bc275f 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
@@ -29,18 +29,26 @@
 // of the given sensor).
 #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)
+#ifdef GYRO_CAL_DBG_ENABLED
+// The time value used to throttle debug messaging.
+#define GYROCAL_WAIT_TIME_NANOS (300000000)
 
 // Unit conversion: nanoseconds to seconds.
 #define NANOS_TO_SEC (1.0e-9f)
 
+// A debug version label to help with tracking results.
+#define GYROCAL_DEBUG_VERSION_STRING "[Jan 20, 2017]"
+
+// Debug log tag string used to identify debug report output data.
+#define GYROCAL_REPORT_TAG "[GYRO_CAL:REPORT]"
+
+// Debug log tag string used to identify debug tuning output data.
+#define GYROCAL_TUNE_TAG "[GYRO_CAL:TUNE]"
+#endif  // GYRO_CAL_DBG_ENABLED
+
 /////// FORWARD DECLARATIONS /////////////////////////////////////////
 
 static void deviceStillnessCheck(struct GyroCal* gyro_cal,
@@ -51,27 +59,51 @@ static void computeGyroCal(struct GyroCal* gyro_cal,
 
 static void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos);
 
-#ifdef GYRO_CAL_DBG_ENABLED
-static void gyroCalUpdateDebug(struct GyroCal* gyro_cal);
+// Data tracker command enumeration.
+enum GyroCalTrackerCommand {
+  DO_RESET = 0,    // Resets the local data used for data tracking.
+  DO_UPDATE_DATA,  // Updates the local tracking data.
+  DO_STORE_DATA,   // Stores intermediate results for later recall.
+  DO_EVALUATE      // Computes and provides the results of the gate function.
+};
 
 /*
- * Updates running calculation of the temperature statistics.
+ * Updates the temperature min/max and mean during the stillness period. Returns
+ * 'true' if the min and max temperature values exceed the range set by
+ * 'temperature_delta_limit_celsius'.
  *
- * 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:
+ *   gyro_cal:     Pointer to the GyroCal data structure.
+ *   temperature_celsius:  New temperature sample to include.
+ *   do_this:      Command enumerator that controls function behavior:
+ */
+static bool gyroTemperatureStatsTracker(struct GyroCal* gyro_cal,
+                                        float temperature_celsius,
+                                        enum GyroCalTrackerCommand do_this);
+
+/*
+ * Tracks the minimum and maximum gyroscope stillness window means.
+ * Returns 'true' when the difference between gyroscope min and max window
+ * means are outside the range set by 'stillness_mean_delta_limit'.
  *
  * 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.
+ *   gyro_cal:     Pointer to the GyroCal data structure.
+ *   do_this:      Command enumerator that controls function behavior.
  */
-static void gyroTempUpdateStats(struct DebugTemperature* debug_temperature,
-                                float temperature, bool reset_stats);
+static bool gyroStillMeanTracker(struct GyroCal* gyro_cal,
+                                 enum GyroCalTrackerCommand do_this);
+
+#ifdef GYRO_CAL_DBG_ENABLED
+// Defines the type of debug data to print.
+enum DebugPrintData {
+  OFFSET = 0,
+  STILLNESS_DATA,
+  SAMPLE_RATE_AND_TEMPERATURE,
+  GYRO_MINMAX_STILLNESS_MEAN,
+  ACCEL_STATS,
+  GYRO_STATS,
+  MAG_STATS
+};
 
 /*
  * Updates running calculation of the gyro's mean sampling rate.
@@ -91,20 +123,12 @@ static void gyroTempUpdateStats(struct DebugTemperature* debug_temperature,
 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
-};
+// Updates the information used for debug printouts.
+static void gyroCalUpdateDebug(struct GyroCal* gyro_cal);
 
 // Helper function for printing out common debug data.
-static void gyroCalDebugPrintData(const struct DebugGyroCal* debug_data,
+static void gyroCalDebugPrintData(const struct GyroCal* gyro_cal,
+                                  char* debug_tag,
                                   enum DebugPrintData print_data);
 
 // This conversion function is necessary for Nanohub firmware compilation (i.e.,
@@ -138,7 +162,9 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos,
                  float gyro_confidence_delta, float accel_var_threshold,
                  float accel_confidence_delta, float mag_var_threshold,
                  float mag_confidence_delta, float stillness_threshold,
-                 int remove_bias_enable) {
+                 float stillness_mean_delta_limit,
+                 float temperature_delta_limit_celsius,
+                 bool gyro_calibration_enable) {
   // Clear gyro_cal structure memory.
   memset(gyro_cal, 0, sizeof(struct GyroCal));
 
@@ -177,59 +203,52 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos,
   // Set the stillness threshold required for gyro bias calibration.
   gyro_cal->stillness_threshold = stillness_threshold;
 
-  // Current window end time used to assist in keeping sensor data
-  // 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.
+  // Current window end-time used to assist in keeping sensor data 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_nanos = 0;
 
   // Gyro calibrations will be applied (see, gyroCalRemoveBias()).
-  gyro_cal->gyro_calibration_enable = (remove_bias_enable > 0);
+  gyro_cal->gyro_calibration_enable = (gyro_calibration_enable > 0);
+
+  // Sets the stability limit for the stillness window mean acceptable delta.
+  gyro_cal->stillness_mean_delta_limit = stillness_mean_delta_limit;
+
+  // Sets the min/max temperature delta limit for the stillness period.
+  gyro_cal->temperature_delta_limit_celsius = temperature_delta_limit_celsius;
+
+  // Ensures that the data tracking functionality is reset.
+  gyroStillMeanTracker(gyro_cal, DO_RESET);
+  gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
 
 #ifdef GYRO_CAL_DBG_ENABLED
   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);
+  // Ensures that the gyro sampling rate estimate is reset.
   gyroSamplingRateUpdate(NULL, 0, /*reset_stats=*/true);
 #endif  // GYRO_CAL_DBG_ENABLED
 }
 
-// Void all pointers in the gyro calibration data structure (doesn't do anything
+// Void pointer in the gyro calibration data structure (doesn't do anything
 // except prevent compiler warnings).
-void gyroCalDestroy(struct GyroCal* gyro_cal) { (void)gyro_cal; }
+void gyroCalDestroy(struct GyroCal* gyro_cal) {
+  (void)gyro_cal;
+}
 
 // Get the most recent bias calibration value.
 void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y,
-                    float* bias_z) {
-  if (gyro_cal->gyro_calibration_enable) {
-    *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
-  }
+                    float* bias_z, float* temperature_celsius) {
+  *bias_x = gyro_cal->bias_x;
+  *bias_y = gyro_cal->bias_y;
+  *bias_z = gyro_cal->bias_z;
+  *temperature_celsius = gyro_cal->bias_temperature_celsius;
 }
 
 // Set an initial bias calibration value.
@@ -246,7 +265,7 @@ void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y,
                 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
+#endif  // GYRO_CAL_DBG_ENABLED
 }
 
 // Remove bias from a gyro measurement [rad/sec].
@@ -272,9 +291,11 @@ 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_nanos,
-                       float x, float y, float z, float temperature) {
-  // Make sure that a valid window end time is set,
-  // and start the watchdog timer.
+                       float x, float y, float z, float temperature_celsius) {
+  static float latest_temperature_celsius = 0.0f;
+
+  // Make sure that a valid window end-time is set, and start the watchdog
+  // timer.
   if (gyro_cal->stillness_win_endtime_nanos <= 0) {
     gyro_cal->stillness_win_endtime_nanos =
         sample_time_nanos + gyro_cal->window_time_duration_nanos;
@@ -283,25 +304,22 @@ void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time_nanos,
     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 temperature statistics (only on a temperature change).
+  if (NANO_ABS(temperature_celsius - latest_temperature_celsius) > FLT_MIN) {
+    gyroTemperatureStatsTracker(gyro_cal, temperature_celsius, DO_UPDATE_DATA);
   }
 
+#ifdef GYRO_CAL_DBG_ENABLED
   // 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_nanos, sample_time_nanos,
                      x, y, z);
 
-  // Perform a device stillness check, set next window end time, and
+  // 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_nanos);
 }
@@ -317,7 +335,7 @@ void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time_nanos,
   // 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
+  // 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_nanos);
 }
@@ -330,17 +348,20 @@ void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time_nanos,
                      gyro_cal->stillness_win_endtime_nanos, sample_time_nanos,
                      x, y, z);
 
-  // Perform a device stillness check, set next window end time, and
+  // 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_nanos);
 }
 
+// TODO(davejacobs): Consider breaking this function up to improve readability.
 // Checks the state of all stillness detectors to determine
 // whether the device is "still".
 void deviceStillnessCheck(struct GyroCal* gyro_cal,
                           uint64_t sample_time_nanos) {
   bool stillness_duration_exceeded = false;
   bool stillness_duration_too_short = false;
+  bool min_max_temp_exceeded = false;
+  bool mean_not_stable = false;
   bool device_is_still = false;
   float conf_not_rot = 0;
   float conf_not_accel = 0;
@@ -357,7 +378,7 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal,
     return;  // Not yet, wait for more data.
   }
 
-  // Set the next window end time for the stillness detectors.
+  // Set the next window end-time for the stillness detectors.
   gyro_cal->stillness_win_endtime_nanos =
       sample_time_nanos + gyro_cal->window_time_duration_nanos;
 
@@ -371,17 +392,26 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal,
     gyro_cal->mag_stillness_detect.stillness_confidence = 1.0f;
   }
 
+  // Updates the mean tracker data.
+  gyroStillMeanTracker(gyro_cal, DO_UPDATE_DATA);
+
   // Determine motion confidence scores (rotation, accelerating, and stillness).
   conf_not_rot = gyro_cal->gyro_stillness_detect.stillness_confidence *
                  gyro_cal->mag_stillness_detect.stillness_confidence;
   conf_not_accel = gyro_cal->accel_stillness_detect.stillness_confidence;
   conf_still = conf_not_rot * conf_not_accel;
 
-  // determine if the device is currently still.
-  device_is_still = (conf_still > gyro_cal->stillness_threshold);
+  // Evaluate the mean and temperature gate functions.
+  mean_not_stable = gyroStillMeanTracker(gyro_cal, DO_EVALUATE);
+  min_max_temp_exceeded =
+      gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_EVALUATE);
+
+  // Determines if the device is currently still.
+  device_is_still = (conf_still > gyro_cal->stillness_threshold) &&
+      !mean_not_stable && !min_max_temp_exceeded ;
 
   if (device_is_still) {
-    // Device is still logic:
+    // Device is "still" logic:
     // If not previously still, then record the start time.
     // If stillness period is too long, then do a calibration.
     // Otherwise, continue collecting stillness data.
@@ -400,24 +430,31 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal,
           gyro_cal->start_still_time_nanos) >
          gyro_cal->max_still_duration_nanos);
 
+    // Track the new stillness mean and temperature data.
+    gyroStillMeanTracker(gyro_cal, DO_STORE_DATA);
+    gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_STORE_DATA);
+
     if (stillness_duration_exceeded) {
       // The current stillness has gone too long. Do a calibration with the
       // current data and reset.
 
-      // Update the gyro bias estimate with the current window data and
-      // reset the stats.
+      // Updates the gyro bias estimate with the current window data and
+      // resets the stats.
       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.
+      // Resets the local calculations because the stillness period is over.
+      gyroStillMeanTracker(gyro_cal, DO_RESET);
+      gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
+
+      // Computes a new gyro offset estimate.
       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);
+      // Resets the sampling rate estimate.
       gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
 #endif  // GYRO_CAL_DBG_ENABLED
 
@@ -426,14 +463,14 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal,
     } else {
       // Continue collecting stillness data.
 
-      // Reset stillness detectors, and extend stillness period.
+      // Extend the stillness period.
       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.
+      // Update the stillness flag.
       gyro_cal->prev_still = true;
     }
   } else {
@@ -451,14 +488,17 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal,
                      gyro_cal->gyro_stillness_detect.window_start_time);
     }
 
-    // Reset stillness detectors and the stats.
+    // Reset the stillness detectors and the stats.
     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);
 
+    // Resets the temperature and sensor mean data.
+    gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
+    gyroStillMeanTracker(gyro_cal, DO_RESET);
+
 #ifdef GYRO_CAL_DBG_ENABLED
-    // Reset the temperature statistics and sampling rate estimate.
-    gyroTempUpdateStats(NULL, 0, /*reset_stats=*/true);
+    // Resets the sampling rate estimate.
     gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
 #endif  // GYRO_CAL_DBG_ENABLED
 
@@ -472,10 +512,6 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal,
 
 // Calculates a new gyro bias offset calibration value.
 void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) {
-  // Current calibration duration.
-  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 &&
         gyro_cal->gyro_stillness_detect.prev_mean_x > -MAX_GYRO_BIAS &&
@@ -484,35 +520,44 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) {
         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));
+    CAL_DEBUG_LOG("[GYRO_CAL:REJECT]",
+                  "Offset|Temp|Time [mdps|C|nsec]: %s%d.%06d, %s%d.%06d, "
+                  "%s%d.%06d, %s%d.%06d, %llu",
+                  CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_x *
+                                       RAD_TO_MILLI_DEGREES,
+                                   6),
+                  CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_y *
+                                       RAD_TO_MILLI_DEGREES,
+                                   6),
+                  CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_z *
+                                       RAD_TO_MILLI_DEGREES,
+                                   6),
+                  CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 6),
+                  (unsigned long long int)calibration_time_nanos);
 #endif  // GYRO_CAL_DBG_ENABLED
 
     // Outside of range. Ignore, reset, and continue.
     return;
   }
 
-  // Record new gyro bias offset calibration.
+  // Record the new gyro bias offset calibration.
   gyro_cal->bias_x = gyro_cal->gyro_stillness_detect.prev_mean_x;
   gyro_cal->bias_y = gyro_cal->gyro_stillness_detect.prev_mean_y;
   gyro_cal->bias_z = gyro_cal->gyro_stillness_detect.prev_mean_z;
 
-  // Record final stillness confidence.
+  // Store the calibration temperature (using the mean temperature over the
+  // "stillness" period).
+  gyro_cal->bias_temperature_celsius = gyro_cal->temperature_mean_celsius;
+
+  // Store the calibration time stamp.
+  gyro_cal->calibration_time_nanos = calibration_time_nanos;
+
+  // Record the final stillness confidence.
   gyro_cal->stillness_confidence =
       gyro_cal->gyro_stillness_detect.prev_stillness_confidence *
       gyro_cal->accel_stillness_detect.prev_stillness_confidence *
       gyro_cal->mag_stillness_detect.prev_stillness_confidence;
 
-  // Store calibration stillness duration.
-  gyro_cal->calibration_time_duration_nanos = cur_cal_dur_nanos;
-
-  // Store calibration time stamp.
-  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;
 
@@ -520,11 +565,8 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) {
   // Increment the total count of calibration updates.
   gyro_cal->debug_calibration_count++;
 
-  // Update the calibration debug information.
+  // Update the calibration debug information and trigger a printout.
   gyroCalUpdateDebug(gyro_cal);
-
-  // Trigger a printout of the debug information.
-  gyro_cal->debug_print_trigger = true;
 #endif
 }
 
@@ -548,15 +590,19 @@ void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) {
     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_nanos = 0;
+
+    // Resets the temperature and sensor mean data.
+    gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
+    gyroStillMeanTracker(gyro_cal, DO_RESET);
 
 #ifdef GYRO_CAL_DBG_ENABLED
-    // Reset the temperature statistics and sampling rate estimate.
-    gyroTempUpdateStats(NULL, 0, /*reset_stats=*/true);
+    // Resets the sampling rate estimate.
     gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
 #endif  // GYRO_CAL_DBG_ENABLED
 
+    // Resets the stillness window end-time.
+    gyro_cal->stillness_win_endtime_nanos = 0;
+
     // Force stillness confidence to zero.
     gyro_cal->accel_stillness_detect.prev_stillness_confidence = 0;
     gyro_cal->gyro_stillness_detect.prev_stillness_confidence = 0;
@@ -579,12 +625,210 @@ void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) {
     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
+#endif  // GYRO_CAL_DBG_ENABLED
+  }
+}
+
+// TODO(davejacobs) -- Combine the following two functions into one or consider
+// implementing a separate helper module for tracking the temperature and mean
+// statistics.
+bool gyroTemperatureStatsTracker(struct GyroCal* gyro_cal,
+                                 float temperature_celsius,
+                                 enum GyroCalTrackerCommand do_this) {
+  // This is used for local calculations of the running mean.
+  static float mean_accumulator = 0.0f;
+  static float temperature_min_max_celsius[2] = {0.0f, 0.0f};
+  static size_t num_points = 0;
+  bool min_max_temp_exceeded = false;
+
+  switch (do_this) {
+    case DO_RESET:
+      // Resets the mean accumulator.
+      num_points = 0;
+      mean_accumulator = 0.0f;
+
+      // Initializes the min/max temperatures values.
+      temperature_min_max_celsius[0] = FLT_MAX;
+      temperature_min_max_celsius[1] = -1.0f * (FLT_MAX - 1.0f);
+      break;
+
+    case DO_UPDATE_DATA:
+      // Does the mean accumulation.
+      mean_accumulator += temperature_celsius;
+      num_points++;
+
+      // Tracks the min and max temperature values.
+      if (temperature_min_max_celsius[0] > temperature_celsius) {
+        temperature_min_max_celsius[0] = temperature_celsius;
+      }
+      if (temperature_min_max_celsius[1] < temperature_celsius) {
+        temperature_min_max_celsius[1] = temperature_celsius;
+      }
+      break;
+
+    case DO_STORE_DATA:
+      // Store the most recent "stillness" mean data to the GyroCal data
+      // structure. This functionality allows previous results to be recalled
+      // when the device suddenly becomes "not still".
+      if (num_points > 0) {
+        memcpy(gyro_cal->temperature_min_max_celsius,
+               temperature_min_max_celsius, 2 * sizeof(float));
+        gyro_cal->temperature_mean_celsius = mean_accumulator / num_points;
+      }
+      break;
+
+    case DO_EVALUATE:
+      // Determines if the min/max delta exceeded the set limit.
+      if (num_points > 0) {
+        min_max_temp_exceeded =
+            (temperature_min_max_celsius[1] -
+             temperature_min_max_celsius[0]) >
+            gyro_cal->temperature_delta_limit_celsius;
+
+#ifdef GYRO_CAL_DBG_ENABLED
+        if (min_max_temp_exceeded) {
+          CAL_DEBUG_LOG(
+              "[GYRO_CAL:TEMP_GATE]",
+              "Exceeded the max temperature variation during stillness.");
+        }
+#endif  // GYRO_CAL_DBG_ENABLED
+      }
+      break;
+
+    default:
+      break;
+  }
+
+  return min_max_temp_exceeded;
+}
+
+bool gyroStillMeanTracker(struct GyroCal* gyro_cal,
+                          enum GyroCalTrackerCommand do_this) {
+  static float gyro_winmean_min[3] = {0.0f, 0.0f, 0.0f};
+  static float gyro_winmean_max[3] = {0.0f, 0.0f, 0.0f};
+  bool mean_not_stable = false;
+  size_t i;
+
+  switch (do_this) {
+    case DO_RESET:
+      // Resets the min/max window mean values to a default value.
+      for (i = 0; i < 3; i++) {
+        gyro_winmean_min[i] = FLT_MAX;
+        gyro_winmean_max[i] = -1.0f * (FLT_MAX - 1.0f);
+      }
+      break;
+
+    case DO_UPDATE_DATA:
+      // Computes the min/max window mean values.
+      if (gyro_winmean_min[0] > gyro_cal->gyro_stillness_detect.win_mean_x) {
+        gyro_winmean_min[0] = gyro_cal->gyro_stillness_detect.win_mean_x;
+      }
+      if (gyro_winmean_max[0] < gyro_cal->gyro_stillness_detect.win_mean_x) {
+        gyro_winmean_max[0] = gyro_cal->gyro_stillness_detect.win_mean_x;
+      }
+
+      if (gyro_winmean_min[1] > gyro_cal->gyro_stillness_detect.win_mean_y) {
+        gyro_winmean_min[1] = gyro_cal->gyro_stillness_detect.win_mean_y;
+      }
+      if (gyro_winmean_max[1] < gyro_cal->gyro_stillness_detect.win_mean_y) {
+        gyro_winmean_max[1] = gyro_cal->gyro_stillness_detect.win_mean_y;
+      }
+
+      if (gyro_winmean_min[2] > gyro_cal->gyro_stillness_detect.win_mean_z) {
+        gyro_winmean_min[2] = gyro_cal->gyro_stillness_detect.win_mean_z;
+      }
+      if (gyro_winmean_max[2] < gyro_cal->gyro_stillness_detect.win_mean_z) {
+        gyro_winmean_max[2] = gyro_cal->gyro_stillness_detect.win_mean_z;
+      }
+      break;
+
+    case DO_STORE_DATA:
+      // Store the most recent "stillness" mean data to the GyroCal data
+      // structure. This functionality allows previous results to be recalled
+      // when the device suddenly becomes "not still".
+      memcpy(gyro_cal->gyro_winmean_min, gyro_winmean_min, 3 * sizeof(float));
+      memcpy(gyro_cal->gyro_winmean_max, gyro_winmean_max, 3 * sizeof(float));
+    break;
+
+    case DO_EVALUATE:
+      // Performs the stability check and returns the 'true' if the difference
+      // between min/max window mean value is outside the stable range.
+      for (i = 0; i < 3; i++) {
+        mean_not_stable |= (gyro_winmean_max[i] - gyro_winmean_min[i]) >
+                           gyro_cal->stillness_mean_delta_limit;
+      }
+#ifdef GYRO_CAL_DBG_ENABLED
+      if (mean_not_stable) {
+        CAL_DEBUG_LOG(
+            "[GYRO_CAL:MEAN_STABILITY_GATE]",
+            "Exceeded the max variation in the stillness window mean values.");
+      }
+#endif  // GYRO_CAL_DBG_ENABLED
+      break;
+
+    default:
+      break;
   }
+
+  return mean_not_stable;
 }
 
 #ifdef GYRO_CAL_DBG_ENABLED
+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;
+  }
+
+  // Resets the sampling rate mean estimator data.
+  if (reset_stats) {
+    last_timestamp_nanos = 0;
+    time_delta_accumulator = 0;
+    num_samples = 0;
+    return;
+  }
+
+  // Skip adding this data to the accumulator if:
+  //   1. A bad timestamp was received (i.e., time not monotonic).
+  //   2. 'last_timestamp_nanos' is zero.
+  if (timestamp_nanos <= last_timestamp_nanos || last_timestamp_nanos == 0) {
+    last_timestamp_nanos = timestamp_nanos;
+    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 gyroCalUpdateDebug(struct GyroCal* gyro_cal) {
+  // Only update this data if debug printing is not currently in progress
+  // (i.e., don't want to risk overwriting debug information that is actively
+  // being reported).
+  if (gyro_cal->debug_state != GYRO_IDLE) {
+    return;
+  }
+
   // Probability of stillness (acc, rot, still), duration, timestamp.
   gyro_cal->debug_gyro_cal.accel_stillness_conf =
       gyro_cal->accel_stillness_detect.prev_stillness_confidence;
@@ -596,30 +840,36 @@ void gyroCalUpdateDebug(struct GyroCal* gyro_cal) {
   // Magnetometer usage.
   gyro_cal->debug_gyro_cal.using_mag_sensor = gyro_cal->using_mag_sensor;
 
-  // Temperature at calibration time.
-  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;
+      gyro_cal->calibration_time_nanos - gyro_cal->start_still_time_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);
+  // Records the mean gyroscope sampling rate.
   gyroSamplingRateUpdate(&gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0,
                          /*reset_stats=*/true);
 
-  // Records the previous window means.
+  // Records the min/max and mean temperature values.
+  gyro_cal->debug_gyro_cal.temperature_mean_celsius =
+      gyro_cal->temperature_mean_celsius;
+  memcpy(gyro_cal->debug_gyro_cal.temperature_min_max_celsius,
+         gyro_cal->temperature_min_max_celsius, 2 * sizeof(float));
+
+  // Records the min/max gyroscope window stillness mean values.
+  memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_min, gyro_cal->gyro_winmean_min,
+         3 * sizeof(float));
+  memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_max, gyro_cal->gyro_winmean_max,
+         3 * sizeof(float));
+
+  // Records the previous stillness 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] =
@@ -642,6 +892,9 @@ void gyroCalUpdateDebug(struct GyroCal* gyro_cal) {
       gyro_cal->mag_stillness_detect.prev_mean_z;
 
   // Records the variance data.
+  // NOTE: These statistics include the final captured window, which may be
+  // outside of the "stillness" period. Therefore, these values may exceed the
+  // stillness thresholds.
   gyro_cal->debug_gyro_cal.accel_var[0] =
       gyro_cal->accel_stillness_detect.win_var_x;
   gyro_cal->debug_gyro_cal.accel_var[1] =
@@ -662,244 +915,143 @@ void gyroCalUpdateDebug(struct GyroCal* gyro_cal) {
       gyro_cal->mag_stillness_detect.win_var_y;
   gyro_cal->debug_gyro_cal.mag_var[2] =
       gyro_cal->mag_stillness_detect.win_var_z;
-}
-
-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;
-  }
-
-  // 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;
-  }
-
-  // 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.
-  }
-
-  // Increments the number of samples.
-  local_temperature_stats.num_temperature_samples++;
-
-  // 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;
-
-  // 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;
-  }
-}
-
-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;
-  }
-
-  // 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;
+  // Trigger a printout of the debug information.
+  gyro_cal->debug_print_trigger = true;
 }
 
-void gyroCalDebugPrintData(const struct DebugGyroCal* debug_data,
+void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
                            enum DebugPrintData print_data) {
   // Prints out the desired debug data.
   float mag_data;
   switch (print_data) {
-    case BIAS_CAL:
-      CAL_DEBUG_LOG(
-          "[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));
+    case OFFSET:
+      CAL_DEBUG_LOG(debug_tag,
+                    "Cal#|Offset|Temp|Time [mdps|C|nsec]: %lu, %s%d.%06d, "
+                    "%s%d.%06d, %s%d.%06d, %s%d.%03d, %llu",
+                    (unsigned long int)gyro_cal->debug_calibration_count,
+                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[0] *
+                                         RAD_TO_MILLI_DEGREES,
+                                     6),
+                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[1] *
+                                         RAD_TO_MILLI_DEGREES,
+                                     6),
+                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[2] *
+                                         RAD_TO_MILLI_DEGREES,
+                                     6),
+                    CAL_ENCODE_FLOAT(
+                        gyro_cal->debug_gyro_cal.temperature_mean_celsius, 3),
+                    (unsigned long long int)
+                        gyro_cal->debug_gyro_cal.end_still_time_nanos);
       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);
-
+    case STILLNESS_DATA:
+      mag_data = (gyro_cal->debug_gyro_cal.using_mag_sensor)
+                     ? gyro_cal->debug_gyro_cal.mag_stillness_conf
+                     : -1.0f;  // Signals that magnetometer was not used.
       CAL_DEBUG_LOG(
-          "[GYRO_CAL:TIME]", "Stillness Duration [nsec]: %llu",
-          (unsigned long long int)debug_data->stillness_duration_nanos);
+          debug_tag,
+          "Cal#|Start|End|Confidence [nsec]: %lu, %llu, %llu, "
+          "%s%d.%03d, %s%d.%03d, %s%d.%03d",
+          (unsigned long int)gyro_cal->debug_calibration_count,
+          (unsigned long long int)
+              gyro_cal->debug_gyro_cal.start_still_time_nanos,
+          (unsigned long long int)gyro_cal->debug_gyro_cal.end_still_time_nanos,
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_stillness_conf, 3),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_stillness_conf, 3),
+          CAL_ENCODE_FLOAT(mag_data, 3));
       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));
+    case SAMPLE_RATE_AND_TEMPERATURE:
       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));
+          debug_tag,
+          "Cal#|Mean|Min|Max|Delta|Sample Rate [C|Hz]: %lu, %s%d.%03d, "
+          "%s%d.%03d, %s%d.%03d, %s%d.%04d, %s%d.%03d",
+          (unsigned long int)gyro_cal->debug_calibration_count,
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius,
+                           3),
+          CAL_ENCODE_FLOAT(
+              gyro_cal->debug_gyro_cal.temperature_min_max_celsius[0], 3),
+          CAL_ENCODE_FLOAT(
+              gyro_cal->debug_gyro_cal.temperature_min_max_celsius[1], 3),
+          CAL_ENCODE_FLOAT(
+              gyro_cal->debug_gyro_cal.temperature_min_max_celsius[1] -
+                  gyro_cal->debug_gyro_cal.temperature_min_max_celsius[0],
+              4),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 3));
       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));
+    case GYRO_MINMAX_STILLNESS_MEAN:
       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));
+          debug_tag,
+          "Cal#|Gyro Peak Stillness Variation [mdps]: %lu, %s%d.%06d, "
+          "%s%d.%06d, %s%d.%06d",
+          (unsigned long int)gyro_cal->debug_calibration_count,
+          CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[0] -
+                            gyro_cal->debug_gyro_cal.gyro_winmean_min[0]) *
+                               RAD_TO_MILLI_DEGREES,
+                           6),
+          CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[1] -
+                            gyro_cal->debug_gyro_cal.gyro_winmean_min[1]) *
+                               RAD_TO_MILLI_DEGREES,
+                           6),
+          CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[2] -
+                            gyro_cal->debug_gyro_cal.gyro_winmean_min[2]) *
+                               RAD_TO_MILLI_DEGREES,
+                           6));
       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");
-      }
+    case ACCEL_STATS:
+      CAL_DEBUG_LOG(
+          debug_tag,
+          "Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^2]: %lu, "
+          "%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d",
+          (unsigned long int)gyro_cal->debug_calibration_count,
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[0], 6),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[1], 6),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[2], 6),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[0], 8),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[1], 8),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[2], 8));
       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));
+    case GYRO_STATS:
       CAL_DEBUG_LOG(
-          "[GYRO_CAL:TEMP_STATS]",
-          "Min/Max Temperature [C]: %s%d.%08d, %s%d.%08d",
-          CAL_ENCODE_FLOAT(
-              debug_data->debug_temperature.temperature_min_max_celsius[0], 8),
+          debug_tag,
+          "Cal#|Gyro Mean|Var [mdps|(rad/sec)^2]: %lu, %s%d.%06d, "
+          "%s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d",
+          (unsigned long int)gyro_cal->debug_calibration_count,
           CAL_ENCODE_FLOAT(
-              debug_data->debug_temperature.temperature_min_max_celsius[1], 8));
-      CAL_DEBUG_LOG(
-          "[GYRO_CAL:TEMP_STATS]", "Temperature Mean [C]: %s%d.%08d",
+              gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MILLI_DEGREES, 6),
           CAL_ENCODE_FLOAT(
-              debug_data->debug_temperature.temperature_mean_celsius, 8));
-      CAL_DEBUG_LOG(
-          "[GYRO_CAL:TEMP_STATS]", "Temperature Variance [C^2]: %s%d.%08d",
+              gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MILLI_DEGREES, 6),
           CAL_ENCODE_FLOAT(
-              debug_data->debug_temperature.temperature_var_celsius, 8));
+              gyro_cal->debug_gyro_cal.gyro_mean[2] * RAD_TO_MILLI_DEGREES, 6),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[0], 8),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[1], 8),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[2], 8));
       break;
 
-    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;
+    case MAG_STATS:
+      if (gyro_cal->debug_gyro_cal.using_mag_sensor) {
+        CAL_DEBUG_LOG(
+            debug_tag,
+            "Cal#|Mag Mean|Var [uT|uT^2]: %lu, %s%d.%06d, "
+            "%s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d",
+            (unsigned long int)gyro_cal->debug_calibration_count,
+            CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 6),
+            CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 6),
+            CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 6),
+            CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[0], 8),
+            CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[1], 8),
+            CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[2], 8));
+      } else {
+        CAL_DEBUG_LOG(debug_tag,
+                      "Cal#|Mag Mean|Var [uT|uT^2]: %lu, 0, 0, 0, -1.0, -1.0, "
+                      "-1.0",
+                      (unsigned long int)gyro_cal->debug_calibration_count);
+      }
 
-    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:
@@ -907,117 +1059,91 @@ void gyroCalDebugPrintData(const struct DebugGyroCal* debug_data,
   }
 }
 
-// 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 enum GyroCalDebugState next_state = GYRO_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:
+  switch (gyro_cal->debug_state) {
+    case GYRO_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(GYROCAL_REPORT_TAG, "");
+        CAL_DEBUG_LOG(GYROCAL_REPORT_TAG, "Debug Version: %s",
+                      GYROCAL_DEBUG_VERSION_STRING);
         gyro_cal->debug_print_trigger = false;  // Resets trigger.
+        gyro_cal->debug_state = GYRO_PRINT_OFFSET;
       } else {
-        debug_state = IDLE;
+        gyro_cal->debug_state = GYRO_IDLE;
       }
       break;
 
-    case WAIT_STATE:
+    case GYRO_WAIT_STATE:
       // This helps throttle the print statements.
-      if ((timestamp_nanos - wait_timer_nanos) >= OVERTEMPCAL_WAIT_TIME_NANOS) {
-        debug_state = next_state;
+      if ((timestamp_nanos - wait_timer_nanos) >= GYROCAL_WAIT_TIME_NANOS) {
+        gyro_cal->debug_state = next_state;
       }
       break;
 
-    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.
+    case GYRO_PRINT_OFFSET:
+      gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, OFFSET);
+      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
+      next_state = GYRO_PRINT_STILLNESS_DATA;   // Sets the next state.
+      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
       break;
 
-    case PRINT_TIME:
-      gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, CAL_TIME);
-
-      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.
+    case GYRO_PRINT_STILLNESS_DATA:
+      gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, STILLNESS_DATA);
+      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
+      next_state = GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE;  // Sets next state.
+      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
       break;
 
-    case PRINT_TEMP:
-      gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, TEMP_STATS);
-
-      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.
+    case GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE:
+      gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG,
+                            SAMPLE_RATE_AND_TEMPERATURE);
+      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
+      next_state = GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN;  // Sets next state.
+      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
       break;
 
-    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.
+    case GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN:
+      gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG,
+                            GYRO_MINMAX_STILLNESS_MEAN);
+      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
+      next_state = GYRO_PRINT_ACCEL_STATS;      // Sets the next state.
+      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
       break;
 
-    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.
+    case GYRO_PRINT_ACCEL_STATS:
+      gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, ACCEL_STATS);
+      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
+      next_state = GYRO_PRINT_GYRO_STATS;       // Sets the next state.
+      gyro_cal->debug_state = GYRO_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.
+    case GYRO_PRINT_GYRO_STATS:
+      gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, GYRO_STATS);
+      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
+      next_state = GYRO_PRINT_MAG_STATS;        // Sets the next state.
+      gyro_cal->debug_state = GYRO_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;
+    case GYRO_PRINT_MAG_STATS:
+      gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, MAG_STATS);
+      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
+      next_state = GYRO_IDLE;                   // Sets the next state.
+      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
       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.
+      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
+      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
   }
 
 #ifdef GYRO_CAL_DBG_TUNE_ENABLED
-  if (debug_state == IDLE) {
+  if (gyro_cal->debug_state == GYRO_IDLE) {
     // This check keeps the tuning printout from interleaving with the above
     // debug print data.
     gyroCalTuneDebugPrint(gyro_cal, timestamp_nanos);
@@ -1028,8 +1154,8 @@ void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t timestamp_nanos) {
 #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 enum GyroCalDebugState debug_state = GYRO_IDLE;
+  static enum GyroCalDebugState next_state = GYRO_IDLE;
   static uint64_t wait_timer_nanos = 0;
 
   // Output sensor variance levels to assist with tuning thresholds.
@@ -1042,126 +1168,57 @@ void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal,
   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.
+  // This is a state machine that controls the reporting out of tuning data.
   switch (debug_state) {
-    case IDLE:
-      // Wait for a trigger and start the debug printout sequence.
+    case GYRO_IDLE:
+      // Wait for a trigger and start the data tuning printout sequence.
       if (condition_i || condition_ii) {
-        debug_state = PRINT_BIAS;
+        CAL_DEBUG_LOG(GYROCAL_TUNE_TAG, "");
+        debug_state = GYRO_PRINT_OFFSET;
       } else {
-        debug_state = IDLE;
+        debug_state = GYRO_IDLE;
       }
       break;
 
-    case WAIT_STATE:
+    case GYRO_WAIT_STATE:
       // This helps throttle the print statements.
-      if ((timestamp_nanos - wait_timer_nanos) >= OVERTEMPCAL_WAIT_TIME_NANOS) {
+      if ((timestamp_nanos - wait_timer_nanos) >= GYROCAL_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.
+    case GYRO_PRINT_OFFSET:
+      gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, OFFSET);
+      wait_timer_nanos = timestamp_nanos;   // Starts the wait timer.
+      next_state = GYRO_PRINT_ACCEL_STATS;  // Sets the next state.
+      debug_state = GYRO_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));
-
-      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 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));
-
+    case GYRO_PRINT_ACCEL_STATS:
+      gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, 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.
+      next_state = GYRO_PRINT_GYRO_STATS;  // Sets the next state.
+      debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
       break;
 
-    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));
-
+    case GYRO_PRINT_GYRO_STATS:
+      gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, 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.
+      next_state = GYRO_PRINT_MAG_STATS;   // Sets the next state.
+      debug_state = GYRO_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));
-      }
-
+    case GYRO_PRINT_MAG_STATS:
+      gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, MAG_STATS);
       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.
+      next_state = GYRO_IDLE;              // Sets the next state.
+      debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
       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.
+      debug_state = GYRO_IDLE;
   }
 }
 #endif  // GYRO_CAL_DBG_TUNE_ENABLED
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.h b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
index 4b5a9c44..9cf06d2c 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
@@ -36,7 +36,7 @@
  *
  * Optional Sensors and Units:
  *       - Magnetometer  [micro-Tesla, uT]
- *       - Temperature   [Celcius]
+ *       - Temperature   [Celsius]
  *
  * #define GYRO_CAL_DBG_ENABLED to enable debug printout statements.
  * #define GYRO_CAL_DBG_TUNE_ENABLED to periodically printout sensor variance
@@ -53,23 +53,25 @@ extern "C" {
 #endif
 
 #ifdef GYRO_CAL_DBG_ENABLED
-// 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;
+// Debug printout state enumeration.
+enum GyroCalDebugState {
+  GYRO_IDLE = 0,
+  GYRO_WAIT_STATE,
+  GYRO_PRINT_OFFSET,
+  GYRO_PRINT_STILLNESS_DATA,
+  GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE,
+  GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN,
+  GYRO_PRINT_ACCEL_STATS,
+  GYRO_PRINT_GYRO_STATS,
+  GYRO_PRINT_MAG_STATS
 };
 
 // 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;
@@ -80,6 +82,10 @@ struct DebugGyroCal {
   float accel_var[3];
   float gyro_var[3];
   float mag_var[3];
+  float gyro_winmean_min[3];
+  float gyro_winmean_max[3];
+  float temperature_min_max_celsius[2];  // 0=min; 1=max
+  float temperature_mean_celsius;
   bool using_mag_sensor;
 };
 #endif
@@ -90,9 +96,6 @@ struct GyroCal {
   struct GyroStillDet mag_stillness_detect;
   struct GyroStillDet gyro_stillness_detect;
 
-  // Latest temperature measurement.
-  float latest_temperature_celcius;
-
   // Aggregated sensor stillness threshold required for gyro bias calibration.
   float stillness_threshold;
 
@@ -106,15 +109,16 @@ struct GyroCal {
   // Timestamp when device started a still period.
   uint64_t start_still_time_nanos;
 
-  // gyro bias estimates and last calibration timestamp.
+  // Gyro offset estimate, and the associated calibration temperature,
+  // timestamp, and stillness confidence values.
   float bias_x, bias_y, bias_z;  // [rad/sec]
-  uint64_t calibration_time_nanos;
-  uint64_t calibration_time_duration_nanos;
+  float bias_temperature_celsius;
   float stillness_confidence;
+  uint64_t calibration_time_nanos;
 
-  // Current window end time for all sensors. Used to assist in keeping
+  // 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
+  // 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_nanos;
 
@@ -136,11 +140,26 @@ struct GyroCal {
   // Flag to indicate if device was previously still.
   bool prev_still;
 
+  // Min and maximum stillness window mean. This is used to check the stability
+  // of the mean values computed for the gyroscope (i.e., provides further
+  // validation for stillness).
+  float gyro_winmean_min[3];
+  float gyro_winmean_max[3];
+  float stillness_mean_delta_limit;
+
+  // Computes the min/max/mean temperature over the stillness period. This is
+  // used to check the temperature stability and provide a gate for when
+  // temperature is rapidly changing.
+  float temperature_min_max_celsius[2];  // 0=min; 1=max
+  float temperature_mean_celsius;
+  float temperature_delta_limit_celsius;
+
 //----------------------------------------------------------------
 
 #ifdef GYRO_CAL_DBG_ENABLED
   // Debug info.
   struct DebugGyroCal debug_gyro_cal;  // Debug data structure.
+  enum GyroCalDebugState debug_state;  // Debug printout state machine.
   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.
@@ -157,14 +176,16 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration,
                  float gyro_confidence_delta, float accel_var_threshold,
                  float accel_confidence_delta, float mag_var_threshold,
                  float mag_confidence_delta, float stillness_threshold,
-                 int remove_bias_enable);
+                 float stillness_mean_delta_limit,
+                 float temperature_delta_limit_celsius,
+                 bool gyro_calibration_enable);
 
 // Void all pointers in the gyro calibration data structure.
 void gyroCalDestroy(struct GyroCal* gyro_cal);
 
 // Get the most recent bias calibration value.
 void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y,
-                    float* bias_z);
+                    float* bias_z, float* temperature_celsius);
 
 // Set an initial bias calibration value.
 void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y,
@@ -179,7 +200,7 @@ 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_nanos,
-                       float x, float y, float z, float temperature);
+                       float x, float y, float z, float temperature_celsius);
 
 // Update the gyro calibration with mag data [micro Tesla].
 void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time_nanos,
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
index afddb481..0b38e5b2 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
@@ -122,28 +122,41 @@ void gyroStillDetUpdate(struct GyroStillDet* gyro_still_det,
 // Calculates and returns the stillness confidence score [0,1].
 float gyroStillDetCompute(struct GyroStillDet* gyro_still_det) {
   float tmp_denom = 1.f;
-  float tmp_mean = 1.f;
+  float tmp_denom_mean = 1.f;
 
   // Don't divide by zero (not likely, but a precaution).
   if (gyro_still_det->num_acc_win_samples > 1) {
     tmp_denom = 1.f / (gyro_still_det->num_acc_win_samples - 1);
+    tmp_denom_mean = 1.f / gyro_still_det->num_acc_win_samples;
   } else {
     // Return zero stillness confidence.
     gyro_still_det->stillness_confidence = 0;
     return gyro_still_det->stillness_confidence;
   }
 
-  // Update the final calculation of variance.
-  // variance_x = win_var_x / (num_samples - 1).
-  tmp_mean = gyro_still_det->win_mean_x * tmp_denom;
+  // Update the final calculation of window mean and variance.
+  float tmp = gyro_still_det->win_mean_x;
+  gyro_still_det->win_mean_x *= tmp_denom_mean;
   gyro_still_det->win_var_x =
-      (gyro_still_det->acc_var_x * tmp_denom) - tmp_mean * tmp_mean;
-  tmp_mean = gyro_still_det->win_mean_y * tmp_denom;
+      (gyro_still_det->acc_var_x - gyro_still_det->win_mean_x * tmp) *
+      tmp_denom;
+
+  tmp = gyro_still_det->win_mean_y;
+  gyro_still_det->win_mean_y *= tmp_denom_mean;
   gyro_still_det->win_var_y =
-      (gyro_still_det->acc_var_y * tmp_denom) - tmp_mean * tmp_mean;
-  tmp_mean = gyro_still_det->win_mean_z * tmp_denom;
+      (gyro_still_det->acc_var_y - gyro_still_det->win_mean_y * tmp) *
+      tmp_denom;
+
+  tmp = gyro_still_det->win_mean_z;
+  gyro_still_det->win_mean_z *= tmp_denom_mean;
   gyro_still_det->win_var_z =
-      (gyro_still_det->acc_var_z * tmp_denom) - tmp_mean * tmp_mean;
+      (gyro_still_det->acc_var_z - gyro_still_det->win_mean_z * tmp) *
+      tmp_denom;
+
+  // Adds the assumed mean value back to the total mean calculation.
+  gyro_still_det->win_mean_x += gyro_still_det->assumed_mean_x;
+  gyro_still_det->win_mean_y += gyro_still_det->assumed_mean_y;
+  gyro_still_det->win_mean_z += gyro_still_det->assumed_mean_z;
 
   // Define the variance thresholds.
   float upper_var_thresh =
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal.c
index 9c9849a3..86035759 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal.c
@@ -30,8 +30,8 @@
 #define MIN_BATCH_SIZE 25            // samples
 #else
 #define MAX_EIGEN_RATIO 15.0f
-#define MAX_EIGEN_MAG 60.0f          // uT
-#define MIN_EIGEN_MAG 30.0f          // uT
+#define MAX_EIGEN_MAG 70.0f          // uT
+#define MIN_EIGEN_MAG 20.0f          // uT
 #define MAX_FIT_MAG 70.0f
 #define MIN_FIT_MAG 20.0f
 #define MIN_BATCH_WINDOW 3000000UL   // 3 sec
@@ -149,13 +149,7 @@ static int moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us) {
       (moc->kasa.nsamples > MIN_BATCH_SIZE)) {
     complete = 1;
 
-  } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW
-#ifdef DIVERSITY_CHECK_ENABLED
-             ||
-             moc->diversity_checker.num_max_dist_violations
-             >= MAX_DISTANCE_VIOLATIONS
-#endif
-             ) {
+  } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) {
     // not enough samples collected in MAX_BATCH_WINDOW or too many
     // maximum distance violations detected.
     magCalReset(moc);
@@ -172,11 +166,13 @@ 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
 #ifdef DIVERSITY_CHECK_ENABLED
-                ,float threshold, float max_distance
                 ,size_t min_num_diverse_vectors
                 ,size_t max_num_max_distance
                 ,float var_threshold
                 ,float max_min_threshold
+                ,float local_field
+                ,float threshold_tuning_param
+                ,float max_distance_tuning_param
 #endif
                 ) {
   magCalReset(moc);
@@ -200,12 +196,13 @@ void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias,
 #ifdef DIVERSITY_CHECK_ENABLED
   // Diversity Checker Init
   diversityCheckerInit(&moc->diversity_checker,
-                       threshold,
-                       max_distance,
                        min_num_diverse_vectors,
                        max_num_max_distance,
                        var_threshold,
-                       max_min_threshold);
+                       max_min_threshold,
+                       local_field,
+                       threshold_tuning_param,
+                       max_distance_tuning_param);
 #endif
 }
 
@@ -269,14 +266,17 @@ bool magCalUpdate(struct MagCal *moc, uint64_t sample_time_us, float x, float y,
     if (moc_eigen_test(&moc->kasa)) {
       struct Vec3 bias;
       float radius;
-
       // 4. Kasa sphere fitting
       if (magKasaFit(&moc->kasa, &bias, &radius)) {
 #ifdef DIVERSITY_CHECK_ENABLED
+        diversityCheckerLocalFieldUpdate(&moc->diversity_checker,
+                                         radius);
         if (diversityCheckerNormQuality(&moc->diversity_checker,
                                         bias.x,
                                         bias.y,
-                                        bias.z)) {
+                                        bias.z) &&
+            moc->diversity_checker.num_max_dist_violations
+            <= MAX_DISTANCE_VIOLATIONS) {
 #endif
           moc->x_bias = bias.x;
           moc->y_bias = bias.y;
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.h b/firmware/os/algos/calibration/magnetometer/mag_cal.h
index 2640cafe..e5158c52 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.h
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal.h
@@ -57,11 +57,9 @@ 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 threshold, float max_distance,
-                size_t min_num_diverse_vectors,
-                size_t max_num_max_distance,
-                float var_threshold,
-                float max_min_threshold);
+                size_t min_num_diverse_vectors, size_t max_num_max_distance,
+                float var_threshold, float max_min_threshold, float local_field,
+                float threshold_tuning_param, float max_distance_tuning_param);
 #else
 void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias,
                 float c00, float c01, float c02, float c10, float c11,
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.c b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
index 0b8b6c73..f1d02b56 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -30,9 +30,8 @@
 // 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 search for the nearest offset estimate to every 2 seconds.
+#define OVERTEMPCAL_NEAREST_NANOS (2000000000)
 
 // Rate-limits the check of old data to every 2 hours.
 #define OVERTEMPCAL_STALE_CHECK_TIME_NANOS (7200000000000)
@@ -43,10 +42,10 @@
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
 // A debug version label to help with tracking results.
-#define OVERTEMPCAL_DEBUG_VERSION_STRING "[Dec 12, 2016]"
+#define OVERTEMPCAL_DEBUG_VERSION_STRING "[Jan 20, 2017]"
 
 // The time value used to throttle debug messaging.
-#define OVERTEMPCAL_WAIT_TIME_NANOS (250000000)
+#define OVERTEMPCAL_WAIT_TIME_NANOS (300000000)
 
 // Debug log tag string used to identify debug report output data.
 #define OVERTEMPCAL_REPORT_TAG "[OVER_TEMP_CAL:REPORT]"
@@ -61,8 +60,8 @@ static const char  kDebugAxisLabel[3] = "XYZ";
 /////// FORWARD DECLARATIONS //////////////////////////////////////////////////
 
 // Updates the most recent model estimate data.
-static void setLatestEstimate(struct OverTempCal *over_temp_cal,
-                              const float *offset, float offset_temp,
+static void setNearestEstimate(struct OverTempCal *over_temp_cal,
+                              const float *offset, float offset_temp_celsius,
                               uint64_t timestamp_nanos);
 
 /*
@@ -78,9 +77,9 @@ static void computeModelUpdate(struct OverTempCal *over_temp_cal,
 
 /*
  * Searches 'model_data' for the sensor offset estimate closest to the current
- * temperature. Sets the 'latest_offset' pointer to the result.
+ * temperature. Sets the 'nearest_offset' pointer to the result.
  */
-static void setNearestEstimate(struct OverTempCal *over_temp_cal);
+static void findNearestEstimate(struct OverTempCal *over_temp_cal);
 
 /*
  * Removes the "old" offset estimates from 'model_data' (i.e., eliminates the
@@ -126,28 +125,34 @@ static void updateModel(const struct OverTempCal *over_temp_cal,
  *
  * INPUTS:
  *   over_temp_cal:    Over-temp data structure.
- *   vi:               Single axis sensor data to be compensated.
+ *   axis_in:          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.
+ *   axis_out:         Single axis sensor data that has been compensated.
  */
 static void removeSensorOffset(const struct OverTempCal *over_temp_cal,
-                               float vi, size_t index, float *vo);
+                               float axis_in, size_t index, float *axis_out);
 
 /*
- * 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)
+ * Checks new offset estimates to determine if they could be an outlier that
+ * should be rejected. Operates on a per-axis basis determined by 'axis_index'.
  *
  * 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.
+ *   over_temp_cal:    Over-temp data structure.
+ *   offset:           Offset array.
+ *   axis_index:       Index of the axis to check (0=x, 1=y, 2=z).
+ *
+ * Returns 'true' if the deviation of the offset value from the linear model
+ * exceeds 'max_error_limit'.
  */
-static float getCompensatedOffset(float temperature, float temp_sensitivity,
-                                  float sensor_intercept);
+static bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset,
+                         size_t axis_index, float temperature_celsius);
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+// This helper function stores all of the debug tracking information necessary
+// for printing log messages.
+static void updateDebugData(struct OverTempCal* over_temp_cal);
+#endif  // OVERTEMPCAL_DBG_ENABLED
 
 /////// FUNCTION DEFINITIONS //////////////////////////////////////////////////
 
@@ -164,7 +169,7 @@ void overTempCalInit(struct OverTempCal *over_temp_cal,
 
   // 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];
+  over_temp_cal->nearest_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.
@@ -186,13 +191,19 @@ void overTempCalInit(struct OverTempCal *over_temp_cal,
 #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.");
 
+  if (over_temp_cal->over_temp_enable) {
+    CAL_DEBUG_LOG("[OVER_TEMP_CAL:INIT]",
+                  "Over-temperature compensation ENABLED.");
+  } else {
+    CAL_DEBUG_LOG("[OVER_TEMP_CAL:INIT]",
+                  "Over-temperature compensation DISABLED.");
+  }
 #endif  // OVERTEMPCAL_DBG_ENABLED
 }
 
 void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
-                         float offset_temp, uint64_t timestamp_nanos,
+                         float offset_temp_celsius, uint64_t timestamp_nanos,
                          const float *temp_sensitivity,
                          const float *sensor_intercept, bool jump_start_model) {
   ASSERT_NOT_NULL(over_temp_cal);
@@ -219,7 +230,8 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
 
   if (!model_jump_started) {
     // Sets the initial over-temp calibration estimate and model data.
-    setLatestEstimate(over_temp_cal, offset, offset_temp, timestamp_nanos);
+    setNearestEstimate(over_temp_cal, offset, offset_temp_celsius,
+                      timestamp_nanos);
 
     // Now there is one offset estimate in the model.
     over_temp_cal->num_model_pts = 1;
@@ -228,50 +240,46 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
 #ifdef OVERTEMPCAL_DBG_ENABLED
   // Prints the updated model data.
   CAL_DEBUG_LOG("[OVER_TEMP_CAL:RECALL]",
-                "Model parameters recalled from memory.");
+                "Over-temperature model parameters recalled.");
 
-  // Trigger a debug print out to view the new model parameters.
-  over_temp_cal->debug_print_trigger = true;
+  // Triggers a debug print out to view the new model parameters.
+  updateDebugData(over_temp_cal);
 #endif  // OVERTEMPCAL_DBG_ENABLED
 }
 
 void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset,
-                         float *offset_temp, uint64_t *timestamp_nanos,
+                         float *offset_temp_celsius, 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(over_temp_cal->nearest_offset);
   ASSERT_NOT_NULL(offset);
-  ASSERT_NOT_NULL(offset_temp);
+  ASSERT_NOT_NULL(offset_temp_celsius);
   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(offset, over_temp_cal->nearest_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;
+  *offset_temp_celsius = over_temp_cal->nearest_offset->offset_temp_celsius;
+  *timestamp_nanos = over_temp_cal->nearest_offset->timestamp_nanos;
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+  CAL_DEBUG_LOG("[OVER_TEMP_CAL:STORED]",
+                "Over-temperature model parameters stored.");
+#endif  // OVERTEMPCAL_DBG_ENABLED
 }
 
 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(over_temp_cal->nearest_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
@@ -283,10 +291,17 @@ void overTempCalRemoveOffset(struct OverTempCal *over_temp_cal,
 
     if (removeStaleModelData(over_temp_cal, timestamp_nanos)) {
       // If anything was removed, then this attempts to recompute the model.
-      computeModelUpdate(over_temp_cal, timestamp_nanos);
+      if (over_temp_cal->num_model_pts >= over_temp_cal->min_num_model_pts) {
+        computeModelUpdate(over_temp_cal, timestamp_nanos);
+      }
     }
   }
 
+  // Determines whether over-temp compensation will be applied.
+  if (!over_temp_cal->over_temp_enable) {
+    return;
+  }
+
   // 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);
@@ -309,7 +324,7 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
                                      const float *offset,
                                      float temperature_celsius) {
   ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(over_temp_cal->latest_offset);
+  ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
   ASSERT_NOT_NULL(offset);
   ASSERT(over_temp_cal->delta_temp_per_bin > 0);
 
@@ -318,22 +333,35 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
     return;
   }
 
-#ifdef OVERTEMPCAL_DBG_ENABLED
-  // Updates the total count of offset estimates.
-  over_temp_cal->debug_num_estimates++;
+  // Checks whether this offset estimate is a likely outlier. A limit is placed
+  // on 'num_outliers', the previous number of successive rejects, to prevent
+  // too many back-to-back rejections.
+  if (over_temp_cal->num_outliers < OVERTEMPCAL_MAX_OUTLIER_COUNT) {
+    if (outlierCheck(over_temp_cal, offset, 0, temperature_celsius) ||
+        outlierCheck(over_temp_cal, offset, 1, temperature_celsius) ||
+        outlierCheck(over_temp_cal, offset, 2, temperature_celsius)) {
+      // Increments the count of rejected outliers.
+      over_temp_cal->num_outliers++;
 
-  // 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);
+#ifdef OVERTEMPCAL_DBG_ENABLED
+      CAL_DEBUG_LOG("[OVER_TEMP_CAL:OUTLIER]",
+                    "Offset|Temperature|Time [mdps|Celcius|nsec] = "
+                    "%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%03d, %llu",
+                    CAL_ENCODE_FLOAT(offset[0] * RAD_TO_MILLI_DEGREES, 6),
+                    CAL_ENCODE_FLOAT(offset[1] * RAD_TO_MILLI_DEGREES, 6),
+                    CAL_ENCODE_FLOAT(offset[2] * RAD_TO_MILLI_DEGREES, 6),
+                    CAL_ENCODE_FLOAT(temperature_celsius, 3),
+                    (unsigned long long int)timestamp_nanos);
 #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;
+      return;  // Skips the process of adding this offset to the model.
+    } else {
+      // Resets the count of rejected outliers.
+      over_temp_cal->num_outliers = 0;
+    }
+  } else {
+    // Resets the count of rejected outliers.
+    over_temp_cal->num_outliers = 0;
   }
 
   // Computes the temperature bin range data.
@@ -351,48 +379,83 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
   //          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
+  //          temp_lo_check <= model_data[i].offset_temp_celsius < 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];
+    if (over_temp_cal->model_data[i].offset_temp_celsius < temp_hi_check &&
+        over_temp_cal->model_data[i].offset_temp_celsius >= temp_lo_check) {
+      // NOTE - the pointer to the new model data point is set here; the offset
+      // data is set below in the call to 'setNearestEstimate'.
+      over_temp_cal->nearest_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.
+  // NOTE - the pointer to the new model data point is set here; the offset
+  // data is set below in the call to 'setNearestEstimate'.
   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++;
+    if (over_temp_cal->num_model_pts < OVERTEMPCAL_MODEL_SIZE) {
+      // 3) If nothing was replaced, and the 'model_data' buffer is not full
+      //    then add the estimate data to the array.
+      over_temp_cal->nearest_offset =
+          &over_temp_cal->model_data[over_temp_cal->num_model_pts];
+      over_temp_cal->num_model_pts++;
+    } else {
+      // 4) Otherwise (nothing was replaced and buffer is full), replace the
+      //    oldest data with the incoming one.
+      over_temp_cal->nearest_offset = &over_temp_cal->model_data[0];
+      for (i = 1; i < over_temp_cal->num_model_pts; i++) {
+        if (over_temp_cal->nearest_offset->timestamp_nanos <
+            over_temp_cal->model_data[i].timestamp_nanos) {
+          over_temp_cal->nearest_offset = &over_temp_cal->model_data[i];
+        }
+      }
+    }
   }
 
-  // Updates the latest model estimate data.
-  setLatestEstimate(over_temp_cal, offset, temperature_celsius,
-                    timestamp_nanos);
+  // Updates the model estimate data nearest to the sensor's temperature.
+  setNearestEstimate(over_temp_cal, offset, temperature_celsius,
+                     timestamp_nanos);
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+  // Updates the latest sensor offset estimate so this can be tracked for debug
+  // printout later.
+  memcpy(&over_temp_cal->debug_overtempcal.latest_offset,
+         over_temp_cal->nearest_offset, sizeof(struct OverTempCalDataPt));
 
-  // Conditionally updates the over-temp model. See 'computeModelUpdate' for
-  // update conditions.
-  computeModelUpdate(over_temp_cal, timestamp_nanos);
+  // Updates the total number of received sensor offset estimates.
+  over_temp_cal->debug_num_estimates++;
+#endif  // OVERTEMPCAL_DBG_ENABLED
 
+  // 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_temperature_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) {
+#ifdef OVERTEMPCAL_DBG_ENABLED
+    // Triggers a log printout to show the updated sensor offset estimate.
+    updateDebugData(over_temp_cal);
+#endif  // OVERTEMPCAL_DBG_ENABLED
+  } else {
+    // The conditions satisfy performing a new model update.
+    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
@@ -414,17 +477,13 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
   // 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
+  // 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) &&
+  if (over_temp_cal->num_model_pts > 0 &&
       (timestamp_nanos - over_temp_cal->nearest_search_timer) >=
           OVERTEMPCAL_NEAREST_NANOS) {
-    setNearestEstimate(over_temp_cal);
+    findNearestEstimate(over_temp_cal);
     over_temp_cal->nearest_search_timer = timestamp_nanos;  // Reset timer.
   }
 }
@@ -446,8 +505,9 @@ void getModelError(const struct OverTempCal *over_temp_cal,
     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]));
+                   (temp_sensitivity[j] *
+                        over_temp_cal->model_data[i].offset_temp_celsius +
+                    sensor_intercept[j]));
       if (max_error_test > max_error[j]) {
         max_error[j] = max_error_test;
       }
@@ -457,40 +517,22 @@ void getModelError(const struct OverTempCal *over_temp_cal,
 
 /////// LOCAL HELPER FUNCTION DEFINITIONS /////////////////////////////////////
 
-void setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset,
-                       float offset_temp, uint64_t timestamp_nanos) {
+void setNearestEstimate(struct OverTempCal *over_temp_cal, const float *offset,
+                       float offset_temp_celsius, uint64_t timestamp_nanos) {
   ASSERT_NOT_NULL(over_temp_cal);
   ASSERT_NOT_NULL(offset);
-  ASSERT_NOT_NULL(over_temp_cal->latest_offset);
+  ASSERT_NOT_NULL(over_temp_cal->nearest_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;
+  memcpy(over_temp_cal->nearest_offset->offset, offset, 3 * sizeof(float));
+  over_temp_cal->nearest_offset->offset_temp_celsius = offset_temp_celsius;
+  over_temp_cal->nearest_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];
@@ -508,55 +550,62 @@ void computeModelUpdate(struct OverTempCal *over_temp_cal,
   //               a. NANO_ABS(temp_sensitivity) < temp_sensitivity_limit
   //               b. NANO_ABS(sensor_intercept) < sensor_intercept_limit
   size_t i;
+  bool updated_one = false;
   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];
+      updated_one = true;
     } 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",
+          "%c-Axis Parameters|Max Error|Time [mdps/C|mdps|mdps|nsec] = "
+          "%s%d.%06d, %s%d.%06d, %s%d.%06d, %llu",
           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));
+          CAL_ENCODE_FLOAT(max_error[i] * RAD_TO_MILLI_DEGREES, 6),
+          (unsigned long long int)timestamp_nanos);
 #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;
+  // If at least one of the axes updated then consider this a valid model
+  // update.
+  if (updated_one) {
+    // 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;
+    // Updates the total number of model updates, the debug data package, and
+    // triggers a log printout.
+    over_temp_cal->debug_num_model_updates++;
+    updateDebugData(over_temp_cal);
 #endif  // OVERTEMPCAL_DBG_ENABLED
+  }
 }
 
-void setNearestEstimate(struct OverTempCal *over_temp_cal) {
+void findNearestEstimate(struct OverTempCal *over_temp_cal) {
   ASSERT_NOT_NULL(over_temp_cal);
 
+  // Performs a brute force search for the estimate nearest the current sensor
+  // temperature.
   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->nearest_offset = &over_temp_cal->model_data[0];
+  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
+    dtemp_new = NANO_ABS(over_temp_cal->model_data[i].offset_temp_celsius -
                     over_temp_cal->temperature_celsius);
     if (dtemp_new < dtemp_old) {
-      nearest_estimate = &over_temp_cal->model_data[i];
+      over_temp_cal->nearest_offset = &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,
@@ -566,15 +615,16 @@ bool removeStaleModelData(struct OverTempCal *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) {
+    if (timestamp_nanos > over_temp_cal->model_data[i].timestamp_nanos &&
+        (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);
+  findNearestEstimate(over_temp_cal);
 
   return removed_one;
 }
@@ -589,6 +639,26 @@ bool removeModelDataByIndex(struct OverTempCal *over_temp_cal,
     return false;
   }
 
+#ifdef OVERTEMPCAL_DBG_ENABLED
+  CAL_DEBUG_LOG(
+      "[OVER_TEMP_CAL:REMOVE]",
+      "Offset|Temp|Time [mdps|C|nsec] = %s%d.%06d, %s%d.%06d, %s%d.%06d, "
+      "%s%d.%03d, %llu",
+      CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[0] *
+                           RAD_TO_MILLI_DEGREES,
+                       6),
+      CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[1] *
+                           RAD_TO_MILLI_DEGREES,
+                       6),
+      CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[1] *
+                           RAD_TO_MILLI_DEGREES,
+                       6),
+      CAL_ENCODE_FLOAT(
+          over_temp_cal->model_data[model_index].offset_temp_celsius, 3),
+      (unsigned long long int)over_temp_cal->model_data[model_index]
+          .timestamp_nanos);
+#endif  // OVERTEMPCAL_DBG_ENABLED
+
   // Remove the model data at 'model_index'.
   size_t i;
   for (i = model_index; i < over_temp_cal->num_model_pts - 1; i++) {
@@ -597,12 +667,6 @@ bool removeModelDataByIndex(struct OverTempCal *over_temp_cal,
   }
   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;
 }
 
@@ -634,8 +698,8 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal) {
   // 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);
+  float offset_temp_celsius =
+      (start_bin_num + 0.5f) * over_temp_cal->delta_temp_per_bin;
 
   size_t i;
   size_t j;
@@ -643,19 +707,19 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal) {
     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]);
+      offset[j] = over_temp_cal->temp_sensitivity[j] * offset_temp_celsius +
+                  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->nearest_offset = &over_temp_cal->model_data[i];
+    setNearestEstimate(over_temp_cal, offset, offset_temp_celsius,
+                      timestamp_nanos);
+    offset_temp_celsius += 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.",
+    CAL_DEBUG_LOG("[OVER_TEMP_CAL:INIT]", "Model Jump-Start:  #Points = %lu.",
                   (unsigned long int)over_temp_cal->min_num_model_pts);
   }
 #endif  // OVERTEMPCAL_DBG_ENABLED
@@ -679,7 +743,7 @@ void updateModel(const struct OverTempCal *over_temp_cal,
 
   // First pass computes the mean values.
   for (i = 0; i < n; ++i) {
-    st += over_temp_cal->model_data[i].offset_temp;
+    st += over_temp_cal->model_data[i].offset_temp_celsius;
     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];
@@ -688,7 +752,8 @@ void updateModel(const struct OverTempCal *over_temp_cal,
   // 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;
+    const float t =
+        over_temp_cal->model_data[i].offset_temp_celsius - 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];
@@ -706,205 +771,232 @@ void updateModel(const struct OverTempCal *over_temp_cal,
   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) {
+void removeSensorOffset(const struct OverTempCal *over_temp_cal, float axis_in,
+                        size_t index, float *axis_out) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
+  ASSERT_NOT_NULL(axis_out);
+
   // 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];
+  if (over_temp_cal->temp_sensitivity[index] >= MODEL_INITIAL_STATE ||
+      NANO_ABS(over_temp_cal->temperature_celsius -
+               over_temp_cal->nearest_offset->offset_temp_celsius) <
+          over_temp_cal->delta_temp_per_bin) {
+    // Use the nearest estimate to perform the compensation if either of the
+    // following is true:
+    //    1) This axis model is in its initial state.
+    //    2) The current temperature is within a small neighborhood of the
+    //       'nearest_offset'.
+    // axis_out = axis_in - nearest_offset
+    *axis_out = axis_in - over_temp_cal->nearest_offset->offset[index];
   } else {
-    // sensor_out = sensor_in - compensated_offset
+    // axis_out = axis_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]);
+    //  compensated_offset = (temp_sensitivity * temperature + sensor_intercept)
+    *axis_out = axis_in - (over_temp_cal->temp_sensitivity[index] *
+                               over_temp_cal->temperature_celsius +
+                           over_temp_cal->sensor_intercept[index]);
   }
 }
 
-float getCompensatedOffset(float temperature, float temp_sensitivity,
-                           float sensor_intercept) {
-  return temp_sensitivity * temperature + sensor_intercept;
+bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset,
+                  size_t axis_index, float temperature_celsius) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  ASSERT_NOT_NULL(offset);
+
+  // If a model has been defined, then check to see if this offset could be a
+  // potential outlier:
+  if (over_temp_cal->temp_sensitivity[axis_index] < MODEL_INITIAL_STATE) {
+    const float max_error_test = NANO_ABS(
+        offset[axis_index] -
+        (over_temp_cal->temp_sensitivity[axis_index] * temperature_celsius +
+         over_temp_cal->sensor_intercept[axis_index]));
+
+    if (max_error_test > over_temp_cal->max_error_limit) {
+      return true;
+    }
+  }
+
+  return false;
 }
 
 /////// 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) {
+void updateDebugData(struct OverTempCal* over_temp_cal) {
   ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(over_temp_cal->latest_offset);
+  ASSERT_NOT_NULL(over_temp_cal->nearest_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.
+  // Only update this data if debug printing is not currently in progress
+  // (i.e., don't want to risk overwriting debug information that is actively
+  // being reported).
+  if (over_temp_cal->debug_state != OTC_IDLE) {
+    return;
+  }
+
+  // Triggers a debug log printout.
+  over_temp_cal->debug_print_trigger = true;
+
+  // Initializes the debug data structure.
+  memset(&over_temp_cal->debug_overtempcal, 0, sizeof(struct DebugOverTempCal));
+
+  // Copies over the relevant data.
+  memcpy(over_temp_cal->debug_overtempcal.sensor_intercept,
+         over_temp_cal->sensor_intercept, 3 * sizeof(float));
+  memcpy(&over_temp_cal->debug_overtempcal.nearest_offset,
+         over_temp_cal->nearest_offset, sizeof(struct OverTempCalDataPt));
+
+  over_temp_cal->debug_overtempcal.num_model_pts = over_temp_cal->num_model_pts;
+  over_temp_cal->debug_overtempcal.modelupdate_timestamp_nanos =
+      over_temp_cal->modelupdate_timestamp_nanos;
+  over_temp_cal->debug_overtempcal.temperature_celsius =
+      over_temp_cal->temperature_celsius;
 
-  // 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->debug_overtempcal.temp_sensitivity[j] =
         (over_temp_cal->temp_sensitivity[j] >= MODEL_INITIAL_STATE)
             ? 0.0f
             : over_temp_cal->temp_sensitivity[j];
   }
 
+  // Computes the maximum error over all of the model data.
+  getModelError(over_temp_cal,
+                over_temp_cal->debug_overtempcal.temp_sensitivity,
+                over_temp_cal->debug_overtempcal.sensor_intercept,
+                over_temp_cal->debug_overtempcal.max_error);
+}
+
+void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
+                           uint64_t timestamp_nanos) {
+  ASSERT_NOT_NULL(over_temp_cal);
+
+  static enum OverTempCalDebugState next_state = 0;
+  static uint64_t wait_timer = 0;
+  static size_t i = 0;  // Counter.
+
   // This is a state machine that controls the reporting out of debug data.
-  switch (debug_state) {
-    case IDLE:
+  switch (over_temp_cal->debug_state) {
+    case OTC_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, "");
+        CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Debug Version: %s",
+                      OVERTEMPCAL_DEBUG_VERSION_STRING);
         over_temp_cal->debug_print_trigger = false;  // Resets trigger.
+        over_temp_cal->debug_state = OTC_PRINT_OFFSET;
       } else {
-        debug_state = IDLE;
+        over_temp_cal->debug_state = OTC_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:
+    case OTC_WAIT_STATE:
       // This helps throttle the print statements.
       if ((timestamp_nanos - wait_timer) >= OVERTEMPCAL_WAIT_TIME_NANOS) {
-        debug_state = next_state;
+        over_temp_cal->debug_state = next_state;
       }
       break;
 
-    case PRINT_SENSITIVITY:
-      // Prints out the modeled temperature sensitivity.
+    case OTC_PRINT_OFFSET:
+      // Prints out the latest GyroCal offset estimate (input data).
       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#|Offset|Temp|Time [mdps|C|nsec]: %lu, %s%d.%06d, "
+          "%s%d.%06d, %s%d.%06d, %s%d.%03d, %llu",
+          (unsigned long int)over_temp_cal->debug_num_estimates,
           CAL_ENCODE_FLOAT(
-              over_temp_cal->sensor_intercept[0] * RAD_TO_MILLI_DEGREES, 6),
+              over_temp_cal->debug_overtempcal.nearest_offset.offset[0] *
+                  RAD_TO_MILLI_DEGREES,
+              6),
           CAL_ENCODE_FLOAT(
-              over_temp_cal->sensor_intercept[1] * RAD_TO_MILLI_DEGREES, 6),
+              over_temp_cal->debug_overtempcal.nearest_offset.offset[1] *
+                  RAD_TO_MILLI_DEGREES,
+              6),
           CAL_ENCODE_FLOAT(
-              over_temp_cal->sensor_intercept[2] * RAD_TO_MILLI_DEGREES, 6));
+              over_temp_cal->debug_overtempcal.nearest_offset.offset[2] *
+                  RAD_TO_MILLI_DEGREES,
+              6),
+          CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.nearest_offset
+                               .offset_temp_celsius,
+                           6),
+          (unsigned long long int)
+              over_temp_cal->debug_overtempcal.nearest_offset.timestamp_nanos);
 
-      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.
+      wait_timer = timestamp_nanos;                 // Starts the wait timer.
+      next_state = OTC_PRINT_MODEL_PARAMETERS;      // Sets the next state.
+      over_temp_cal->debug_state = OTC_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.
+    case OTC_PRINT_MODEL_PARAMETERS:
+      // Prints out the model parameters.
+      CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG,
+                    "Cal#|Sensitivity|Intercept [mdps/C|mdps]: %lu, %s%d.%06d, "
+                    "%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%06d",
+                    (unsigned long int)over_temp_cal->debug_num_estimates,
+                    CAL_ENCODE_FLOAT(
+                        over_temp_cal->debug_overtempcal.temp_sensitivity[0] *
+                            RAD_TO_MILLI_DEGREES,
+                        6),
+                    CAL_ENCODE_FLOAT(
+                        over_temp_cal->debug_overtempcal.temp_sensitivity[1] *
+                            RAD_TO_MILLI_DEGREES,
+                        6),
+                    CAL_ENCODE_FLOAT(
+                        over_temp_cal->debug_overtempcal.temp_sensitivity[2] *
+                            RAD_TO_MILLI_DEGREES,
+                        6),
+                    CAL_ENCODE_FLOAT(
+                        over_temp_cal->debug_overtempcal.sensor_intercept[0] *
+                            RAD_TO_MILLI_DEGREES,
+                        6),
+                    CAL_ENCODE_FLOAT(
+                        over_temp_cal->debug_overtempcal.sensor_intercept[1] *
+                            RAD_TO_MILLI_DEGREES,
+                        6),
+                    CAL_ENCODE_FLOAT(
+                        over_temp_cal->debug_overtempcal.sensor_intercept[2] *
+                            RAD_TO_MILLI_DEGREES,
+                        6));
+
+      wait_timer = timestamp_nanos;                 // Starts the wait timer.
+      next_state = OTC_PRINT_MODEL_ERROR;           // Sets the next state.
+      over_temp_cal->debug_state = OTC_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);
+    case OTC_PRINT_MODEL_ERROR:
+      // Computes the maximum error over all of the model data.
+      CAL_DEBUG_LOG(
+          OVERTEMPCAL_REPORT_TAG,
+          "Cal#|#Updates|#ModelPts|Model Error|Update Time [mdps|nsec]: %lu, "
+          "%lu, %lu, %s%d.%06d, %s%d.%06d, %s%d.%06d, %llu",
+          (unsigned long int)over_temp_cal->debug_num_estimates,
+          (unsigned long int)over_temp_cal->debug_num_model_updates,
+          (unsigned long int)over_temp_cal->debug_overtempcal.num_model_pts,
+          CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[0] *
+                               RAD_TO_MILLI_DEGREES,
+                           6),
+          CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[1] *
+                               RAD_TO_MILLI_DEGREES,
+                           6),
+          CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[2] *
+                               RAD_TO_MILLI_DEGREES,
+                           6),
+          (unsigned long long int)
+              over_temp_cal->debug_overtempcal.modelupdate_timestamp_nanos);
 
-      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.
+      i = 0;                          // Resets the model data printer counter.
+      wait_timer = timestamp_nanos;       // Starts the wait timer.
+      next_state = OTC_PRINT_MODEL_DATA;  // Sets the next state.
+      over_temp_cal->debug_state = OTC_WAIT_STATE;  // First, go to wait state.
       break;
 
-    case PRINT_MODEL_DATA:
+    case OTC_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 ",
+            "  Model[%lu] [mdps|C|nsec] = %s%d.%06d, %s%d.%06d, %s%d.%06d, "
+            "%s%d.%03d, %llu",
             (unsigned long int)i,
             CAL_ENCODE_FLOAT(
                 over_temp_cal->model_data[i].offset[0] * RAD_TO_MILLI_DEGREES,
@@ -915,26 +1007,30 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
             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));
+            CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset_temp_celsius,
+                             3),
+            (unsigned long long int)over_temp_cal->model_data[i]
+                .timestamp_nanos);
 
         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.
+        wait_timer = timestamp_nanos;       // Starts the wait timer.
+        next_state = OTC_PRINT_MODEL_DATA;  // Sets the next state.
+        over_temp_cal->debug_state =
+            OTC_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);
+        // Sends this state machine to its idle state.
+        wait_timer = timestamp_nanos;  // Starts the wait timer.
+        next_state = OTC_IDLE;         // Sets the next state.
+        over_temp_cal->debug_state =
+            OTC_WAIT_STATE;            // First, go to wait state.
       }
       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.
+      wait_timer = timestamp_nanos;                 // Starts the wait timer.
+      next_state = OTC_IDLE;                        // Sets the next state.
+      over_temp_cal->debug_state = OTC_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
index 928576f0..4ae8952d 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
@@ -66,16 +66,49 @@ extern "C" {
 #endif
 
 // Defines the maximum size of the 'model_data' array.
-#define OVERTEMPCAL_MODEL_SIZE (20)
+#define OVERTEMPCAL_MODEL_SIZE (40)
+
+// The maximum number of successive outliers that may be rejected.
+#define OVERTEMPCAL_MAX_OUTLIER_COUNT (3)
 
 // 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.
+  float offset_temp_celsius;  // [Celsius]
+  uint64_t timestamp_nanos;   // [nanoseconds]
+};
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+// Debug printout state enumeration.
+enum OverTempCalDebugState {
+  OTC_IDLE = 0,
+  OTC_WAIT_STATE,
+  OTC_PRINT_OFFSET,
+  OTC_PRINT_MODEL_PARAMETERS,
+  OTC_PRINT_MODEL_ERROR,
+  OTC_PRINT_MODEL_DATA
+};
+
+// OverTempCal debug information/data tracking structure.
+struct DebugOverTempCal {
+  uint64_t modelupdate_timestamp_nanos;
+
+  // The most recent offset estimate received.
+  struct OverTempCalDataPt latest_offset;
+
+  // The offset estimate nearest the current sensor temperature.
+  struct OverTempCalDataPt nearest_offset;
+
+  // The maximum model error over all model_data points.
+  float max_error[3];
+
+  float temp_sensitivity[3];
+  float sensor_intercept[3];
+  float temperature_celsius;
+  size_t num_model_pts;
 };
+#endif  // OVERTEMPCAL_DBG_ENABLED
 
 // The following data structure contains all of the necessary components for
 // modeling a sensor's temperature dependency and providing over-temperature
@@ -99,10 +132,8 @@ struct OverTempCal {
   // 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;
+  // Pointer to the offset estimate closest to the current sensor temperature.
+  struct OverTempCalDataPt *nearest_offset;
 
   ///// Online Model Identification Parameters ////////////////////////////////
   //
@@ -130,6 +161,11 @@ struct OverTempCal {
   float temp_sensitivity_limit;        // [sensor units/Celsius]
   float sensor_intercept_limit;        // [sensor units]
 
+  // The number of successive outliers rejected in a row. This is used to
+  // prevent the possibility of a bad state where an initial bad fit causes
+  // good data to be continually rejected.
+  size_t num_outliers;
+
   // The rules for accepting new offset estimates into the 'model_data'
   // collection:
   //    1) The temperature domain is divided into bins each spanning
@@ -139,15 +175,15 @@ struct OverTempCal {
   //          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
+  //          temp_lo_check <= model_data[i].offset_temp_celsius < 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.
+  //       oldest data 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).
+  // concentrated at a 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
@@ -169,11 +205,13 @@ struct OverTempCal {
   // 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'
+  //    2) The sensor offset data pointed to by 'nearest_offset'
   //       (saving timestamp information is not required).
   bool new_overtemp_model_available;
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
+  struct DebugOverTempCal debug_overtempcal;  // Debug data structure.
+  enum OverTempCalDebugState debug_state;     // Debug printout state machine.
   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.
@@ -216,7 +254,7 @@ void overTempCalInit(struct OverTempCal *over_temp_cal,
  * 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.
+ *   offset_temp_celsius: 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).
@@ -226,7 +264,7 @@ void overTempCalInit(struct OverTempCal *over_temp_cal,
  * 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,
+                         float offset_temp_celsius, uint64_t timestamp_nanos,
                          const float *temp_sensitivity,
                          const float *sensor_intercept, bool jump_start_model);
 
@@ -237,7 +275,7 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
  *   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.
+ *   offset_temp_celsius: 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).
@@ -245,7 +283,7 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
  * 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 *offset_temp_celsius, uint64_t *timestamp_nanos,
                          float *temp_sensitivity, float *sensor_intercept);
 
 /*
@@ -296,8 +334,8 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
  * 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)|
+ *       getCompensatedOffset(model_data[i]->offset_temp_celsius,
+ *         temp_sensitivity, sensor_intercept)|
  *
  * INPUTS:
  *   over_temp_cal:    Over-temp data structure.