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
19 #include <string.h>
20
21 /////// FORWARD DECLARATIONS /////////////////////////////////////////
22
23 // Enforces the limits of an input value [0,1].
24 static float gyroStillDetLimit(float value);
25
26 /////// FUNCTION DEFINITIONS /////////////////////////////////////////
27
28 // Initialize the GyroStillDet structure.
gyroStillDetInit(struct GyroStillDet * gyro_still_det,float var_threshold,float confidence_delta)29 void gyroStillDetInit(struct GyroStillDet* gyro_still_det, float var_threshold,
30 float confidence_delta) {
31 // Clear all data structure variables to 0.
32 memset(gyro_still_det, 0, sizeof(struct GyroStillDet));
33
34 // Set the delta about the variance threshold for calculation
35 // of the stillness confidence score.
36 if (confidence_delta < var_threshold) {
37 gyro_still_det->confidence_delta = confidence_delta;
38 } else {
39 gyro_still_det->confidence_delta = var_threshold;
40 }
41
42 // Set the variance threshold parameter for the stillness
43 // confidence score.
44 gyro_still_det->var_threshold = var_threshold;
45
46 // Signal to start capture of next stillness data window.
47 gyro_still_det->start_new_window = true;
48 }
49
50 // Update the stillness detector with a new sample.
gyroStillDetUpdate(struct GyroStillDet * gyro_still_det,uint64_t stillness_win_endtime,uint64_t sample_time,float x,float y,float z)51 void gyroStillDetUpdate(struct GyroStillDet* gyro_still_det,
52 uint64_t stillness_win_endtime, uint64_t sample_time,
53 float x, float y, float z) {
54 // Using the method of the assumed mean to preserve some numerical
55 // stability while avoiding per-sample divisions that the more
56 // numerically stable Welford method would afford.
57
58 // Reference for the numerical method used below to compute the
59 // online mean and variance statistics:
60 // 1). en.wikipedia.org/wiki/assumed_mean
61
62 float delta = 0;
63
64 // If the window end time is not valid then wait till it is.
65 if (stillness_win_endtime <= 0) {
66 return;
67 }
68
69 // Increment the number of samples.
70 gyro_still_det->num_acc_samples++;
71
72 // Online computation of mean for the running stillness period.
73 gyro_still_det->mean_x += x;
74 gyro_still_det->mean_y += y;
75 gyro_still_det->mean_z += z;
76
77 // Is this the first sample of a new window?
78 if (gyro_still_det->start_new_window) {
79 // Record the window start time.
80 gyro_still_det->window_start_time = sample_time;
81 gyro_still_det->start_new_window = false;
82
83 // Update assumed mean values.
84 gyro_still_det->assumed_mean_x = x;
85 gyro_still_det->assumed_mean_y = y;
86 gyro_still_det->assumed_mean_z = z;
87
88 // Reset current window mean and variance.
89 gyro_still_det->num_acc_win_samples = 0;
90 gyro_still_det->win_mean_x = 0;
91 gyro_still_det->win_mean_y = 0;
92 gyro_still_det->win_mean_z = 0;
93 gyro_still_det->acc_var_x = 0;
94 gyro_still_det->acc_var_y = 0;
95 gyro_still_det->acc_var_z = 0;
96 } else {
97 // Check to see if we have enough samples to compute a stillness
98 // confidence score.
99 gyro_still_det->stillness_window_ready =
100 (sample_time >= stillness_win_endtime) &&
101 (gyro_still_det->num_acc_samples > 1);
102 }
103
104 // Record the most recent sample time stamp.
105 gyro_still_det->last_sample_time = sample_time;
106
107 // Online window mean and variance ("one-pass" accumulation).
108 gyro_still_det->num_acc_win_samples++;
109
110 delta = (x - gyro_still_det->assumed_mean_x);
111 gyro_still_det->win_mean_x += delta;
112 gyro_still_det->acc_var_x += delta * delta;
113
114 delta = (y - gyro_still_det->assumed_mean_y);
115 gyro_still_det->win_mean_y += delta;
116 gyro_still_det->acc_var_y += delta * delta;
117
118 delta = (z - gyro_still_det->assumed_mean_z);
119 gyro_still_det->win_mean_z += delta;
120 gyro_still_det->acc_var_z += delta * delta;
121 }
122
123 // Calculates and returns the stillness confidence score [0,1].
gyroStillDetCompute(struct GyroStillDet * gyro_still_det)124 float gyroStillDetCompute(struct GyroStillDet* gyro_still_det) {
125 float tmp_denom = 1.f;
126 float tmp_denom_mean = 1.f;
127
128 // Don't divide by zero (not likely, but a precaution).
129 if (gyro_still_det->num_acc_win_samples > 1) {
130 tmp_denom = 1.f / (gyro_still_det->num_acc_win_samples - 1);
131 tmp_denom_mean = 1.f / gyro_still_det->num_acc_win_samples;
132 } else {
133 // Return zero stillness confidence.
134 gyro_still_det->stillness_confidence = 0;
135 return gyro_still_det->stillness_confidence;
136 }
137
138 // Update the final calculation of window mean and variance.
139 float tmp = gyro_still_det->win_mean_x;
140 gyro_still_det->win_mean_x *= tmp_denom_mean;
141 gyro_still_det->win_var_x =
142 (gyro_still_det->acc_var_x - gyro_still_det->win_mean_x * tmp) *
143 tmp_denom;
144
145 tmp = gyro_still_det->win_mean_y;
146 gyro_still_det->win_mean_y *= tmp_denom_mean;
147 gyro_still_det->win_var_y =
148 (gyro_still_det->acc_var_y - gyro_still_det->win_mean_y * tmp) *
149 tmp_denom;
150
151 tmp = gyro_still_det->win_mean_z;
152 gyro_still_det->win_mean_z *= tmp_denom_mean;
153 gyro_still_det->win_var_z =
154 (gyro_still_det->acc_var_z - gyro_still_det->win_mean_z * tmp) *
155 tmp_denom;
156
157 // Adds the assumed mean value back to the total mean calculation.
158 gyro_still_det->win_mean_x += gyro_still_det->assumed_mean_x;
159 gyro_still_det->win_mean_y += gyro_still_det->assumed_mean_y;
160 gyro_still_det->win_mean_z += gyro_still_det->assumed_mean_z;
161
162 // Define the variance thresholds.
163 float upper_var_thresh =
164 (gyro_still_det->var_threshold + gyro_still_det->confidence_delta);
165
166 float lower_var_thresh =
167 (gyro_still_det->var_threshold - gyro_still_det->confidence_delta);
168
169 // Compute the stillness confidence score.
170 if ((gyro_still_det->win_var_x > upper_var_thresh) ||
171 (gyro_still_det->win_var_y > upper_var_thresh) ||
172 (gyro_still_det->win_var_z > upper_var_thresh)) {
173 // Sensor variance exceeds the upper threshold (i.e., motion detected).
174 // Set stillness confidence equal to 0.
175 gyro_still_det->stillness_confidence = 0;
176
177 } else {
178 if ((gyro_still_det->win_var_x <= lower_var_thresh) &&
179 (gyro_still_det->win_var_y <= lower_var_thresh) &&
180 (gyro_still_det->win_var_z <= lower_var_thresh)) {
181 // Sensor variance is below the lower threshold (i.e., stillness
182 // detected).
183 // Set stillness confidence equal to 1.
184 gyro_still_det->stillness_confidence = 1.f;
185
186 } else {
187 // Motion detection thresholds not exceeded. Compute the stillness
188 // confidence score.
189
190 float var_thresh = gyro_still_det->var_threshold;
191
192 // Compute the stillness confidence score.
193 // Each axis score is limited [0,1].
194 tmp_denom = 1.f / (upper_var_thresh - lower_var_thresh);
195 gyro_still_det->stillness_confidence =
196 gyroStillDetLimit(0.5f - (gyro_still_det->win_var_x - var_thresh) *
197 tmp_denom) *
198 gyroStillDetLimit(0.5f - (gyro_still_det->win_var_y - var_thresh) *
199 tmp_denom) *
200 gyroStillDetLimit(0.5f - (gyro_still_det->win_var_z - var_thresh) *
201 tmp_denom);
202 }
203 }
204
205 // Return the stillness confidence.
206 return gyro_still_det->stillness_confidence;
207 }
208
209 // Resets the stillness detector and initiates a new detection window.
210 // 'reset_stats' determines whether the stillness statistics are reset.
gyroStillDetReset(struct GyroStillDet * gyro_still_det,bool reset_stats)211 void gyroStillDetReset(struct GyroStillDet* gyro_still_det, 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].
gyroStillDetLimit(float value)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