1 /* 2 * Copyright (C) 2016 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 #include "calibration/gyroscope/gyro_stillness_detect.h" 18 #include <string.h> 19 20 /////// FORWARD DECLARATIONS ///////////////////////////////////////// 21 22 // Enforces the limits of an input value [0,1]. 23 static float gyroStillDetLimit(float value); 24 25 /////// FUNCTION DEFINITIONS ///////////////////////////////////////// 26 27 // Initialize the GyroStillDet structure. 28 void gyroStillDetInit(struct GyroStillDet* gyro_still_det, 29 float var_threshold, float confidence_delta) { 30 // Clear all data structure variables to 0. 31 memset(gyro_still_det, 0, sizeof(struct GyroStillDet)); 32 33 // Set the delta about the variance threshold for calculation 34 // of the stillness confidence score. 35 if (confidence_delta < var_threshold) { 36 gyro_still_det->confidence_delta = confidence_delta; 37 } else { 38 gyro_still_det->confidence_delta = var_threshold; 39 } 40 41 // Set the variance threshold parameter for the stillness 42 // confidence score. 43 gyro_still_det->var_threshold = var_threshold; 44 45 // Signal to start capture of next stillness data window. 46 gyro_still_det->start_new_window = true; 47 } 48 49 // Update the stillness detector with a new sample. 50 void gyroStillDetUpdate(struct GyroStillDet* gyro_still_det, 51 uint64_t stillness_win_endtime, uint64_t sample_time, 52 float x, float y, float z) { 53 // Using the method of the assumed mean to preserve some numerical 54 // stability while avoiding per-sample divisions that the more 55 // numerically stable Welford method would afford. 56 57 // Reference for the numerical method used below to compute the 58 // online mean and variance statistics: 59 // 1). en.wikipedia.org/wiki/assumed_mean 60 61 float delta = 0; 62 63 // If the window end time is not valid then wait till it is. 64 if (stillness_win_endtime <= 0) { 65 return; 66 } 67 68 // Increment the number of samples. 69 gyro_still_det->num_acc_samples++; 70 71 // Online computation of mean for the running stillness period. 72 gyro_still_det->mean_x += x; 73 gyro_still_det->mean_y += y; 74 gyro_still_det->mean_z += z; 75 76 // Is this the first sample of a new window? 77 if (gyro_still_det->start_new_window) { 78 // Record the window start time. 79 gyro_still_det->window_start_time = sample_time; 80 gyro_still_det->start_new_window = false; 81 82 // Update assumed mean values. 83 gyro_still_det->assumed_mean_x = x; 84 gyro_still_det->assumed_mean_y = y; 85 gyro_still_det->assumed_mean_z = z; 86 87 // Reset current window mean and variance. 88 gyro_still_det->num_acc_win_samples = 0; 89 gyro_still_det->win_mean_x = 0; 90 gyro_still_det->win_mean_y = 0; 91 gyro_still_det->win_mean_z = 0; 92 gyro_still_det->acc_var_x = 0; 93 gyro_still_det->acc_var_y = 0; 94 gyro_still_det->acc_var_z = 0; 95 } else { 96 // Check to see if we have enough samples to compute a stillness 97 // confidence score. 98 gyro_still_det->stillness_window_ready = 99 (sample_time >= stillness_win_endtime) && 100 (gyro_still_det->num_acc_samples > 1); 101 } 102 103 // Record the most recent sample time stamp. 104 gyro_still_det->last_sample_time = sample_time; 105 106 // Online window mean and variance ("one-pass" accumulation). 107 gyro_still_det->num_acc_win_samples++; 108 109 delta = (x - gyro_still_det->assumed_mean_x); 110 gyro_still_det->win_mean_x += delta; 111 gyro_still_det->acc_var_x += delta * delta; 112 113 delta = (y - gyro_still_det->assumed_mean_y); 114 gyro_still_det->win_mean_y += delta; 115 gyro_still_det->acc_var_y += delta * delta; 116 117 delta = (z - gyro_still_det->assumed_mean_z); 118 gyro_still_det->win_mean_z += delta; 119 gyro_still_det->acc_var_z += delta * delta; 120 } 121 122 // Calculates and returns the stillness confidence score [0,1]. 123 float gyroStillDetCompute(struct GyroStillDet* gyro_still_det) { 124 float tmp_denom = 1.f; 125 float tmp_denom_mean = 1.f; 126 127 // Don't divide by zero (not likely, but a precaution). 128 if (gyro_still_det->num_acc_win_samples > 1) { 129 tmp_denom = 1.f / (gyro_still_det->num_acc_win_samples - 1); 130 tmp_denom_mean = 1.f / gyro_still_det->num_acc_win_samples; 131 } else { 132 // Return zero stillness confidence. 133 gyro_still_det->stillness_confidence = 0; 134 return gyro_still_det->stillness_confidence; 135 } 136 137 // Update the final calculation of window mean and variance. 138 float tmp = gyro_still_det->win_mean_x; 139 gyro_still_det->win_mean_x *= tmp_denom_mean; 140 gyro_still_det->win_var_x = 141 (gyro_still_det->acc_var_x - gyro_still_det->win_mean_x * tmp) * 142 tmp_denom; 143 144 tmp = gyro_still_det->win_mean_y; 145 gyro_still_det->win_mean_y *= tmp_denom_mean; 146 gyro_still_det->win_var_y = 147 (gyro_still_det->acc_var_y - gyro_still_det->win_mean_y * tmp) * 148 tmp_denom; 149 150 tmp = gyro_still_det->win_mean_z; 151 gyro_still_det->win_mean_z *= tmp_denom_mean; 152 gyro_still_det->win_var_z = 153 (gyro_still_det->acc_var_z - gyro_still_det->win_mean_z * tmp) * 154 tmp_denom; 155 156 // Adds the assumed mean value back to the total mean calculation. 157 gyro_still_det->win_mean_x += gyro_still_det->assumed_mean_x; 158 gyro_still_det->win_mean_y += gyro_still_det->assumed_mean_y; 159 gyro_still_det->win_mean_z += gyro_still_det->assumed_mean_z; 160 161 // Define the variance thresholds. 162 float upper_var_thresh = 163 (gyro_still_det->var_threshold + gyro_still_det->confidence_delta); 164 165 float lower_var_thresh = 166 (gyro_still_det->var_threshold - gyro_still_det->confidence_delta); 167 168 // Compute the stillness confidence score. 169 if ((gyro_still_det->win_var_x > upper_var_thresh) || 170 (gyro_still_det->win_var_y > upper_var_thresh) || 171 (gyro_still_det->win_var_z > upper_var_thresh)) { 172 // Sensor variance exceeds the upper threshold (i.e., motion detected). 173 // Set stillness confidence equal to 0. 174 gyro_still_det->stillness_confidence = 0; 175 176 } else { 177 if ((gyro_still_det->win_var_x <= lower_var_thresh) && 178 (gyro_still_det->win_var_y <= lower_var_thresh) && 179 (gyro_still_det->win_var_z <= lower_var_thresh)) { 180 // Sensor variance is below the lower threshold (i.e., stillness 181 // detected). 182 // Set stillness confidence equal to 1. 183 gyro_still_det->stillness_confidence = 1.f; 184 185 } else { 186 // Motion detection thresholds not exceeded. Compute the stillness 187 // confidence score. 188 189 float var_thresh = gyro_still_det->var_threshold; 190 191 // Compute the stillness confidence score. 192 // Each axis score is limited [0,1]. 193 tmp_denom = 1.f / (upper_var_thresh - lower_var_thresh); 194 gyro_still_det->stillness_confidence = 195 gyroStillDetLimit( 196 0.5f - (gyro_still_det->win_var_x - var_thresh) * tmp_denom) * 197 gyroStillDetLimit( 198 0.5f - (gyro_still_det->win_var_y - var_thresh) * tmp_denom) * 199 gyroStillDetLimit( 200 0.5f - (gyro_still_det->win_var_z - var_thresh) * tmp_denom); 201 } 202 } 203 204 // Return the stillness confidence. 205 return gyro_still_det->stillness_confidence; 206 } 207 208 // Resets the stillness detector and initiates a new detection window. 209 // 'reset_stats' determines whether the stillness statistics are reset. 210 void gyroStillDetReset(struct GyroStillDet* gyro_still_det, 211 bool reset_stats) { 212 float tmp_denom = 1.f; 213 214 // Reset the stillness data ready flag. 215 gyro_still_det->stillness_window_ready = false; 216 217 // Signal to start capture of next stillness data window. 218 gyro_still_det->start_new_window = true; 219 220 // Track the stillness confidence (current->previous). 221 gyro_still_det->prev_stillness_confidence = 222 gyro_still_det->stillness_confidence; 223 224 // Track changes in the mean estimate. 225 if (gyro_still_det->num_acc_samples > 1) { 226 tmp_denom = 1.f / gyro_still_det->num_acc_samples; 227 } 228 gyro_still_det->prev_mean_x = gyro_still_det->mean_x * tmp_denom; 229 gyro_still_det->prev_mean_y = gyro_still_det->mean_y * tmp_denom; 230 gyro_still_det->prev_mean_z = gyro_still_det->mean_z * tmp_denom; 231 232 // Reset the current statistics to zero. 233 if (reset_stats) { 234 gyro_still_det->num_acc_samples = 0; 235 gyro_still_det->mean_x = 0; 236 gyro_still_det->mean_y = 0; 237 gyro_still_det->mean_z = 0; 238 gyro_still_det->acc_var_x = 0; 239 gyro_still_det->acc_var_y = 0; 240 gyro_still_det->acc_var_z = 0; 241 } 242 } 243 244 // Enforces the limits of an input value [0,1]. 245 float gyroStillDetLimit(float value) { 246 // Fix limits [0,1]. 247 if (value < 0) { 248 value = 0; 249 } else { 250 if (value > 1.f) { 251 value = 1.f; 252 } 253 } 254 255 return value; 256 } 257