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.
gyroStillDetInit(struct GyroStillDet * gyro_still_det,float var_threshold,float confidence_delta)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.
gyroStillDetUpdate(struct GyroStillDet * gyro_still_det,uint64_t stillness_win_endtime,uint64_t sample_time,float x,float y,float z)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].
gyroStillDetCompute(struct GyroStillDet * gyro_still_det)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.
gyroStillDetReset(struct GyroStillDet * gyro_still_det,bool reset_stats)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].
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