src/sensor/pedometer/step_detection.cpp

   1 /*
   2  *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
   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 "step_detection.h"
  18
  19 #include <math.h>
  20 #include <sensor_log.h>
  21
  22 /* Size of average filter. */
  23 #define AV_FILTER_SIZE 7
  24
  25 #define AV_GFILTER_SIZE 500
  26
  27 #define FAST_PEAK_THRESHOLD 0.77
  28
  29 #define SLOW_PEAK_THRESHOLD 0.3
  30
  31 #define PEAK_THRESHOLD_FOR_MEAN 2.0
  32
  33 #define B_COEF 1.08
  34
  35 #define SLOPE_PARAM -0.75
  36
  37 step_detection::step_detection()
  38 : m_average_filter(AV_FILTER_SIZE)
  39 , m_average_gfilter(AV_GFILTER_SIZE)
  40 , m_zero_crossing_up(true)
  41 , m_zero_crossing_down(false)
  42 , m_zc_filter()
  43 , m_peak_threshold(FAST_PEAK_THRESHOLD)
  44 , m_use_savitzky(false)
  45 , m_last_step_timestamp(UNKNOWN_TIMESTAMP)
  46 , m_zero_crossing_up_detected(false)
  47 , m_zero_crossing_down_detected(false)
  48 , m_minimum_acceleration(0)
  49 , m_maximum_acceleration(0)
  50 , m_is_slow_step_detected(false)
  51 {
  52 }
  53
  54 step_detection::~step_detection()
  55 {
  56 }
  57
  58 /************************************************************************
  59  */
  60 void step_detection::reset(void)
  61 {
  62         m_average_filter.reset();
  63         m_average_gfilter.reset();
  64         m_zero_crossing_up.reset();
  65         m_zero_crossing_down.reset();
  66
  67         m_average_gfilter.filter(9.81);
  68         m_zc_filter.reset();
  69
  70         m_zero_crossing_up_detected = false;
  71         m_zero_crossing_down_detected = false;
  72         m_minimum_acceleration = 0;
  73         m_maximum_acceleration = 0;
  74         m_is_slow_step_detected = false;
  75 }
  76
  77 /************************************************************************
  78  */
  79 void step_detection::set_peak_threshold(double threshold)
  80 {
  81         m_peak_threshold = threshold;
  82 }
  83
  84 /************************************************************************
  85  */
  86 void step_detection::set_use_savitzky(bool use_savitzky)
  87 {
  88         m_use_savitzky = use_savitzky;
  89 }
  90
  91 /************************************************************************
  92  */
  93 bool step_detection::is_slow_step(void)
  94 {
  95         return m_is_slow_step_detected;
  96 }
  97
  98 /************************************************************************
  99  */
 100 static double cal_step_length(double time, double sqrt4peak_valley_diff)
 101 {
 102         double step_length = 0;
 103         if (time <= 0 || time > 1.00) {
 104                 step_length = 0.50 * sqrt4peak_valley_diff;
 105         } else if (time < 0.3) {
 106                 step_length = 0;
 107         } else {
 108                 step_length = B_COEF + SLOPE_PARAM * time;
 109         }
 110
 111         if (step_length > 1.5)
 112                 step_length = 0;
 113
 114         return step_length;
 115 }
 116
 117 /************************************************************************
 118  */
 119 bool step_detection::add_acc_sensor_values_average(
 120                 timestamp_t timestamp, double acc, step_event* step)
 121 {
 122         double acceleration = m_average_filter.filter(acc - 9.8066f);
 123
 124         bool n_zero_up = m_zero_crossing_up.detect_step(timestamp, acceleration);
 125         bool n_zero_down = m_zero_crossing_down.detect_step(timestamp, acceleration);
 126
 127         double sqrt4peak_valley_diff = 0;
 128         bool is_step_detected = false;
 129
 130         if (n_zero_up) {
 131                 m_zero_crossing_up_detected = true;
 132                 m_zero_crossing_down_detected = false;
 133         }
 134         if (n_zero_down) {
 135                 m_zero_crossing_up_detected = false;
 136                 m_zero_crossing_down_detected = true;
 137         }
 138         if (m_zero_crossing_up_detected) {
 139                 if (m_maximum_acceleration < acceleration) {
 140                         m_maximum_acceleration = acceleration;
 141                 }
 142         }
 143         if (m_zero_crossing_down_detected) {
 144                 if (m_minimum_acceleration > acceleration) {
 145                         m_minimum_acceleration = acceleration;
 146                 }
 147         }
 148
 149         double peak_threshold;
 150         if (m_zero_crossing_up.m_time_sum / 1E9 < 1.2) {
 151                 peak_threshold = m_peak_threshold;
 152                 m_is_slow_step_detected = false;
 153         } else {
 154                 peak_threshold = SLOW_PEAK_THRESHOLD;
 155                 m_is_slow_step_detected = true;
 156         }
 157
 158         if (n_zero_down) {
 159                 if (m_maximum_acceleration > peak_threshold
 160                                 || (m_maximum_acceleration - m_minimum_acceleration > PEAK_THRESHOLD_FOR_MEAN)) {
 161                         sqrt4peak_valley_diff = pow(m_maximum_acceleration - m_minimum_acceleration, 0.25);
 162                         m_minimum_acceleration = 0;
 163                         m_maximum_acceleration = 0;
 164                         is_step_detected = true;
 165                 }
 166         }
 167
 168         if (m_zero_crossing_up.m_time_sum / 1E9 < 0.3)
 169                 is_step_detected = false;
 170
 171         if (is_step_detected) {
 172                 if (m_last_step_timestamp == UNKNOWN_TIMESTAMP)
 173                         m_last_step_timestamp = timestamp;
 174
 175                 double time = (timestamp - m_last_step_timestamp) / 1E9;
 176                 m_last_step_timestamp = timestamp;
 177                 m_zero_crossing_up.m_time_sum = 0;
 178                 step->m_timestamp = timestamp;
 179                 step->m_step_length = cal_step_length(time, sqrt4peak_valley_diff);
 180
 181                 return true;
 182         }
 183         return false;
 184 }
 185
 186 /************************************************************************
 187  */
 188 bool step_detection::add_acc_sensor_values_savitzky(
 189                 timestamp_t timestamp, double acc, step_event* step)
 190 {
 191         double acceleration = m_zc_filter.filter(acc - m_average_gfilter.filter(acc));
 192         bool n_zero_up = m_zero_crossing_up.detect_step(timestamp, acceleration);
 193         bool n_zero_down = m_zero_crossing_down.detect_step(timestamp, acceleration);
 194
 195         double sqrt4peak_valley_diff = 0;
 196         bool is_step_detected = false;
 197
 198         if (n_zero_up) {
 199                 m_zero_crossing_up_detected = true;
 200                 m_zero_crossing_down_detected = false;
 201         }
 202         if (n_zero_down) {
 203                 m_zero_crossing_up_detected = false;
 204                 m_zero_crossing_down_detected = true;
 205         }
 206         if (m_zero_crossing_up_detected) {
 207                 if (m_maximum_acceleration < acceleration) {
 208                         m_maximum_acceleration = acceleration;
 209                 }
 210         }
 211         if (m_zero_crossing_down_detected) {
 212                 if (m_minimum_acceleration > acceleration) {
 213                         m_minimum_acceleration = acceleration;
 214                 }
 215         }
 216
 217         bool zup = m_zero_crossing_up.m_time_sum / 1E9 > 1.2;
 218         if (n_zero_down) {
 219                 is_step_detected = false;
 220
 221                 if ((m_maximum_acceleration > 0.6 &&
 222                                                 m_minimum_acceleration < -0.852)
 223                                 || (m_maximum_acceleration > 0.714
 224                                                 && m_minimum_acceleration < -0.455)) {
 225                         is_step_detected = true;
 226                         m_is_slow_step_detected = false;
 227                 }
 228                 if (m_maximum_acceleration - m_minimum_acceleration > 3.32) {
 229                         is_step_detected = true;
 230                         m_is_slow_step_detected = false;
 231                 }
 232                 if (zup && m_maximum_acceleration > 0.764 && m_minimum_acceleration < -0.0235) {// slow steps
 233                         is_step_detected = true;
 234                         m_is_slow_step_detected = true;
 235                 }
 236
 237                 if (is_step_detected) {
 238                         sqrt4peak_valley_diff = pow(m_maximum_acceleration - m_minimum_acceleration, 0.25);
 239                         m_minimum_acceleration = 0.0;
 240                         m_maximum_acceleration = 0.0;
 241                 }
 242         }
 243
 244         if (m_zero_crossing_up.m_time_sum / 1E9 < 0.3)
 245                 is_step_detected = false;
 246
 247         if (is_step_detected) {
 248                 if (m_last_step_timestamp == UNKNOWN_TIMESTAMP)
 249                         m_last_step_timestamp = timestamp;
 250
 251                 double time = (timestamp - m_last_step_timestamp) / 1E9;
 252
 253                 m_last_step_timestamp = timestamp;
 254                 m_zero_crossing_up.m_time_sum = 0;
 255                 step->m_timestamp = timestamp;
 256                 step->m_step_length = cal_step_length(time, sqrt4peak_valley_diff);
 257
 258                 return true;
 259         }
 260
 261         return false;
 262 }
 263
 264 /************************************************************************
 265  */
 266 bool step_detection::get_step(timestamp_t timestamp, double acc, step_event* step)
 267 {
 268         return m_use_savitzky
 269                         ? add_acc_sensor_values_savitzky(timestamp, acc, step)
 270                         : add_acc_sensor_values_average(timestamp, acc, step);
 271 }