firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c - device/google/contexthub - Git at Google

 /*
  * Copyright (C) 2016 The Android Open Source Project
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
  *      http://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */

 #include "calibration/gyroscope/gyro_stillness_detect.h"

 #include <string.h>

 /////// FORWARD DECLARATIONS /////////////////////////////////////////

 // Enforces the limits of an input value [0,1].
 static float gyroStillDetLimit(float value);

 /////// FUNCTION DEFINITIONS /////////////////////////////////////////

 // Initialize the GyroStillDet structure.
 void gyroStillDetInit(struct GyroStillDet* gyro_still_det, float var_threshold,
                       float confidence_delta) {
   // Clear all data structure variables to 0.
   memset(gyro_still_det, 0, sizeof(struct GyroStillDet));

   // Set the delta about the variance threshold for calculation
   // of the stillness confidence score.
   if (confidence_delta < var_threshold) {
     gyro_still_det->confidence_delta = confidence_delta;
   } else {
     gyro_still_det->confidence_delta = var_threshold;
   }

   // Set the variance threshold parameter for the stillness
   // confidence score.
   gyro_still_det->var_threshold = var_threshold;

   // Signal to start capture of next stillness data window.
   gyro_still_det->start_new_window = true;
 }

 // Update the stillness detector with a new sample.
 void gyroStillDetUpdate(struct GyroStillDet* gyro_still_det,
                         uint64_t stillness_win_endtime, uint64_t sample_time,
                         float x, float y, float z) {
   // Using the method of the assumed mean to preserve some numerical
   // stability while avoiding per-sample divisions that the more
   // numerically 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

   float delta = 0;

   // If the window end time is not valid then wait till it is.
   if (stillness_win_endtime <= 0) {
     return;
   }

   // Increment the number of samples.
   gyro_still_det->num_acc_samples++;

   // Online computation of mean for the running stillness period.
   gyro_still_det->mean_x += x;
   gyro_still_det->mean_y += y;
   gyro_still_det->mean_z += z;

   // Is this the first sample of a new window?
   if (gyro_still_det->start_new_window) {
     // Record the window start time.
     gyro_still_det->window_start_time = sample_time;
     gyro_still_det->start_new_window = false;

     // Update assumed mean values.
     gyro_still_det->assumed_mean_x = x;
     gyro_still_det->assumed_mean_y = y;
     gyro_still_det->assumed_mean_z = z;

     // Reset current window mean and variance.
     gyro_still_det->num_acc_win_samples = 0;
     gyro_still_det->win_mean_x = 0;
     gyro_still_det->win_mean_y = 0;
     gyro_still_det->win_mean_z = 0;
     gyro_still_det->acc_var_x = 0;
     gyro_still_det->acc_var_y = 0;
     gyro_still_det->acc_var_z = 0;
   } else {
     // Check to see if we have enough samples to compute a stillness
     // confidence score.
     gyro_still_det->stillness_window_ready =
         (sample_time >= stillness_win_endtime) &&
         (gyro_still_det->num_acc_samples > 1);
   }

   // Record the most recent sample time stamp.
   gyro_still_det->last_sample_time = sample_time;

   // Online window mean and variance ("one-pass" accumulation).
   gyro_still_det->num_acc_win_samples++;

   delta = (x - gyro_still_det->assumed_mean_x);
   gyro_still_det->win_mean_x += delta;
   gyro_still_det->acc_var_x += delta * delta;

   delta = (y - gyro_still_det->assumed_mean_y);
   gyro_still_det->win_mean_y += delta;
   gyro_still_det->acc_var_y += delta * delta;

   delta = (z - gyro_still_det->assumed_mean_z);
   gyro_still_det->win_mean_z += delta;
   gyro_still_det->acc_var_z += delta * delta;
 }

 // Calculates and returns the stillness confidence score [0,1].
 float gyroStillDetCompute(struct GyroStillDet* gyro_still_det) {
   float tmp_denom = 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 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 - 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 - 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 - 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 =
       (gyro_still_det->var_threshold + gyro_still_det->confidence_delta);

   float lower_var_thresh =
       (gyro_still_det->var_threshold - gyro_still_det->confidence_delta);

   // Compute the stillness confidence score.
   if ((gyro_still_det->win_var_x > upper_var_thresh) ||
       (gyro_still_det->win_var_y > upper_var_thresh) ||
       (gyro_still_det->win_var_z > upper_var_thresh)) {
     // Sensor variance exceeds the upper threshold (i.e., motion detected).
     // Set stillness confidence equal to 0.
     gyro_still_det->stillness_confidence = 0;

   } else {
     if ((gyro_still_det->win_var_x <= lower_var_thresh) &&
         (gyro_still_det->win_var_y <= lower_var_thresh) &&
         (gyro_still_det->win_var_z <= lower_var_thresh)) {
       // Sensor variance is below the lower threshold (i.e., stillness
       // detected).
       // Set stillness confidence equal to 1.
       gyro_still_det->stillness_confidence = 1.f;

     } else {
       // Motion detection thresholds not exceeded. Compute the stillness
       // confidence score.

       float var_thresh = gyro_still_det->var_threshold;

       // Compute the stillness confidence score.
       // Each axis score is limited [0,1].
       tmp_denom = 1.f / (upper_var_thresh - lower_var_thresh);
       gyro_still_det->stillness_confidence =
           gyroStillDetLimit(0.5f - (gyro_still_det->win_var_x - var_thresh) *
                                        tmp_denom) *
           gyroStillDetLimit(0.5f - (gyro_still_det->win_var_y - var_thresh) *
                                        tmp_denom) *
           gyroStillDetLimit(0.5f - (gyro_still_det->win_var_z - var_thresh) *
                                        tmp_denom);
     }
   }

   // Return the stillness confidence.
   return gyro_still_det->stillness_confidence;
 }

 // Resets the stillness detector and initiates a new detection window.
 // 'reset_stats' determines whether the stillness statistics are reset.
 void gyroStillDetReset(struct GyroStillDet* gyro_still_det, bool reset_stats) {
   float tmp_denom = 1.f;

   // Reset the stillness data ready flag.
   gyro_still_det->stillness_window_ready = false;

   // Signal to start capture of next stillness data window.
   gyro_still_det->start_new_window = true;

   // Track the stillness confidence (current->previous).
   gyro_still_det->prev_stillness_confidence =
       gyro_still_det->stillness_confidence;

   // Track changes in the mean estimate.
   if (gyro_still_det->num_acc_samples > 1) {
     tmp_denom = 1.f / gyro_still_det->num_acc_samples;
   }
   gyro_still_det->prev_mean_x = gyro_still_det->mean_x * tmp_denom;
   gyro_still_det->prev_mean_y = gyro_still_det->mean_y * tmp_denom;
   gyro_still_det->prev_mean_z = gyro_still_det->mean_z * tmp_denom;

   // Reset the current statistics to zero.
   if (reset_stats) {
     gyro_still_det->num_acc_samples = 0;
     gyro_still_det->mean_x = 0;
     gyro_still_det->mean_y = 0;
     gyro_still_det->mean_z = 0;
     gyro_still_det->acc_var_x = 0;
     gyro_still_det->acc_var_y = 0;
     gyro_still_det->acc_var_z = 0;
   }
 }

 // Enforces the limits of an input value [0,1].
 float gyroStillDetLimit(float value) {
   // Fix limits [0,1].
   if (value < 0) {
     value = 0;
   } else {
     if (value > 1.f) {
       value = 1.f;
     }
   }

   return value;
 }
	/*
	* Copyright (C) 2016 The Android Open Source Project
	*
	* Licensed under the Apache License, Version 2.0 (the "License");
	* you may not use this file except in compliance with the License.
	* You may obtain a copy of the License at
	*
	* http://www.apache.org/licenses/LICENSE-2.0
	*
	* Unless required by applicable law or agreed to in writing, software
	* distributed under the License is distributed on an "AS IS" BASIS,
	* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	* See the License for the specific language governing permissions and
	* limitations under the License.
	*/

	#include "calibration/gyroscope/gyro_stillness_detect.h"

	#include <string.h>

	/////// FORWARD DECLARATIONS /////////////////////////////////////////

	// Enforces the limits of an input value [0,1].
	static float gyroStillDetLimit(float value);

	/////// FUNCTION DEFINITIONS /////////////////////////////////////////

	// Initialize the GyroStillDet structure.
	void gyroStillDetInit(struct GyroStillDet* gyro_still_det, float var_threshold,
	float confidence_delta) {
	// Clear all data structure variables to 0.
	memset(gyro_still_det, 0, sizeof(struct GyroStillDet));

	// Set the delta about the variance threshold for calculation
	// of the stillness confidence score.
	if (confidence_delta < var_threshold) {
	gyro_still_det->confidence_delta = confidence_delta;
	} else {
	gyro_still_det->confidence_delta = var_threshold;
	}

	// Set the variance threshold parameter for the stillness
	// confidence score.
	gyro_still_det->var_threshold = var_threshold;

	// Signal to start capture of next stillness data window.
	gyro_still_det->start_new_window = true;
	}

	// Update the stillness detector with a new sample.
	void gyroStillDetUpdate(struct GyroStillDet* gyro_still_det,
	uint64_t stillness_win_endtime, uint64_t sample_time,
	float x, float y, float z) {
	// Using the method of the assumed mean to preserve some numerical
	// stability while avoiding per-sample divisions that the more
	// numerically 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

	float delta = 0;

	// If the window end time is not valid then wait till it is.
	if (stillness_win_endtime <= 0) {
	return;
	}

	// Increment the number of samples.
	gyro_still_det->num_acc_samples++;

	// Online computation of mean for the running stillness period.
	gyro_still_det->mean_x += x;
	gyro_still_det->mean_y += y;
	gyro_still_det->mean_z += z;

	// Is this the first sample of a new window?
	if (gyro_still_det->start_new_window) {
	// Record the window start time.
	gyro_still_det->window_start_time = sample_time;
	gyro_still_det->start_new_window = false;

	// Update assumed mean values.
	gyro_still_det->assumed_mean_x = x;
	gyro_still_det->assumed_mean_y = y;
	gyro_still_det->assumed_mean_z = z;

	// Reset current window mean and variance.
	gyro_still_det->num_acc_win_samples = 0;
	gyro_still_det->win_mean_x = 0;
	gyro_still_det->win_mean_y = 0;
	gyro_still_det->win_mean_z = 0;
	gyro_still_det->acc_var_x = 0;
	gyro_still_det->acc_var_y = 0;
	gyro_still_det->acc_var_z = 0;
	} else {
	// Check to see if we have enough samples to compute a stillness
	// confidence score.
	gyro_still_det->stillness_window_ready =
	(sample_time >= stillness_win_endtime) &&
	(gyro_still_det->num_acc_samples > 1);
	}

	// Record the most recent sample time stamp.
	gyro_still_det->last_sample_time = sample_time;

	// Online window mean and variance ("one-pass" accumulation).
	gyro_still_det->num_acc_win_samples++;

	delta = (x - gyro_still_det->assumed_mean_x);
	gyro_still_det->win_mean_x += delta;
	gyro_still_det->acc_var_x += delta * delta;

	delta = (y - gyro_still_det->assumed_mean_y);
	gyro_still_det->win_mean_y += delta;
	gyro_still_det->acc_var_y += delta * delta;

	delta = (z - gyro_still_det->assumed_mean_z);
	gyro_still_det->win_mean_z += delta;
	gyro_still_det->acc_var_z += delta * delta;
	}

	// Calculates and returns the stillness confidence score [0,1].
	float gyroStillDetCompute(struct GyroStillDet* gyro_still_det) {
	float tmp_denom = 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 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 - 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 - 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 - 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 =
	(gyro_still_det->var_threshold + gyro_still_det->confidence_delta);

	float lower_var_thresh =
	(gyro_still_det->var_threshold - gyro_still_det->confidence_delta);

	// Compute the stillness confidence score.
	if ((gyro_still_det->win_var_x > upper_var_thresh) \|\|
	(gyro_still_det->win_var_y > upper_var_thresh) \|\|
	(gyro_still_det->win_var_z > upper_var_thresh)) {
	// Sensor variance exceeds the upper threshold (i.e., motion detected).
	// Set stillness confidence equal to 0.
	gyro_still_det->stillness_confidence = 0;

	} else {
	if ((gyro_still_det->win_var_x <= lower_var_thresh) &&
	(gyro_still_det->win_var_y <= lower_var_thresh) &&
	(gyro_still_det->win_var_z <= lower_var_thresh)) {
	// Sensor variance is below the lower threshold (i.e., stillness
	// detected).
	// Set stillness confidence equal to 1.
	gyro_still_det->stillness_confidence = 1.f;

	} else {
	// Motion detection thresholds not exceeded. Compute the stillness
	// confidence score.

	float var_thresh = gyro_still_det->var_threshold;

	// Compute the stillness confidence score.
	// Each axis score is limited [0,1].
	tmp_denom = 1.f / (upper_var_thresh - lower_var_thresh);
	gyro_still_det->stillness_confidence =
	gyroStillDetLimit(0.5f - (gyro_still_det->win_var_x - var_thresh) *
	tmp_denom) *
	gyroStillDetLimit(0.5f - (gyro_still_det->win_var_y - var_thresh) *
	tmp_denom) *
	gyroStillDetLimit(0.5f - (gyro_still_det->win_var_z - var_thresh) *
	tmp_denom);
	}
	}

	// Return the stillness confidence.
	return gyro_still_det->stillness_confidence;
	}

	// Resets the stillness detector and initiates a new detection window.
	// 'reset_stats' determines whether the stillness statistics are reset.
	void gyroStillDetReset(struct GyroStillDet* gyro_still_det, bool reset_stats) {
	float tmp_denom = 1.f;

	// Reset the stillness data ready flag.
	gyro_still_det->stillness_window_ready = false;

	// Signal to start capture of next stillness data window.
	gyro_still_det->start_new_window = true;

	// Track the stillness confidence (current->previous).
	gyro_still_det->prev_stillness_confidence =
	gyro_still_det->stillness_confidence;

	// Track changes in the mean estimate.
	if (gyro_still_det->num_acc_samples > 1) {
	tmp_denom = 1.f / gyro_still_det->num_acc_samples;
	}
	gyro_still_det->prev_mean_x = gyro_still_det->mean_x * tmp_denom;
	gyro_still_det->prev_mean_y = gyro_still_det->mean_y * tmp_denom;
	gyro_still_det->prev_mean_z = gyro_still_det->mean_z * tmp_denom;

	// Reset the current statistics to zero.
	if (reset_stats) {
	gyro_still_det->num_acc_samples = 0;
	gyro_still_det->mean_x = 0;
	gyro_still_det->mean_y = 0;
	gyro_still_det->mean_z = 0;
	gyro_still_det->acc_var_x = 0;
	gyro_still_det->acc_var_y = 0;
	gyro_still_det->acc_var_z = 0;
	}
	}

	// Enforces the limits of an input value [0,1].
	float gyroStillDetLimit(float value) {
	// Fix limits [0,1].
	if (value < 0) {
	value = 0;
	} else {
	if (value > 1.f) {
	value = 1.f;
	}
	}

	return value;
	}