Home | History | Annotate | Download | only in gyroscope 
      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.
     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.
     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].
    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.
    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].
    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