惯性室内导航入门到精通（3）-计步算法

接上文，已经获得三轴加速度了，如何由三轴的加速度计算出步数呢。当手机移动时，手机三轴加速度也会变化，且这些变化中是有一定的规律的，通过规律就能写出相应的算法来解决。（具体可通过下载测试软件进行观察）
如图，人行走规律，跨步时z轴急剧变化。可以通过变化最大的轴为判定轴，该轴变化超过某值为一步。这里使用的是三轴开方共同计算。

step类

通过step类来实现计步算法，原理，检测波峰，当波峰超过阀值且满足一定的条件，当做一步。

    public   int step=0;//步数
    private final int num = 5;//数组大小
    private float[] diffValue = new float[num];//存放波峰波谷差值
    private int diffCount = 0;//实际波峰波谷差值的数量
    private  boolean Up = false; //波形上升标志位
    private  int UpCount = 0;  //上升次数
    private  int UpCount_before = 0;    //上一点的持续上升的次数
    private  int DownCount_before=0;    //上一次的持续下降次数
    private  int DownCount=0;            //下降次数
    private  boolean State_befor = false; //上一点的状态，上升还是下降
    private   float peak = 0;    //波峰值
    private  float valley = 0;    //波谷值
    private   long peaktime_now = 0; //此次波峰的时间
    private  long peaktime_before = 0;//上次波峰的时间
    private  long timeOfNow = 0;    //当前的时间
    private   float sensor_old = 0;    //上次传感器的值

    //控制参数
    private    final float init_limit_value = (float) 3.5;    //初始阈值
    private   float Auto_limit_value = (float) 4;//动态阈值需要动态的数据，这个值用于这些动态数据的阈值
    private  final  int PEAK_TIME_DIFF=400;//满足条件--两个波峰的时间差
    private  final  int UP_DOWN_COUNT=5;//满足条件--上升或者下降连续次数

在主界面中实例化step，并计算初始值

private Step step1;
.....
step1=new Step();

测步

如果是第一次测量，将当前值赋值给上一此的值，跳过不测量。否者，如果检测到波峰且满足两次波峰时间差大于某值，波峰波谷差值大于动态阀值则计做一次步。如果波峰波谷差值大于初始阀值则进行动态阀值的更新。

    public void step_run(float values) {
            if(sensor_old != 0)
            {
               if (check_peak(values, sensor_old)) //检测到波峰
               {
                    peaktime_before = peaktime_now;
                    timeOfNow = System.currentTimeMillis();//系统时间毫秒数
                    if (timeOfNow - peaktime_before >= PEAK_TIME_DIFF && (peak - valley >= Auto_limit_value))
                    {
                      peaktime_now = timeOfNow;//将当前时间定为当前波峰时间
                      step++;
                    }
                    if (timeOfNow - peaktime_before >= PEAK_TIME_DIFF && (peak - valley >= init_limit_value))
                    {
                    peaktime_now = timeOfNow;
                    Auto_limit_value = limit_value_update(peak - valley);//更新动态阈值
                    }
              }
            }
        sensor_old = values;
    }

波峰检测

传入当前值和上一次的值，如果当前值大于上次值则波型为上升状态，修改状态值，否者为下降，修改状态值。如果当前为下降，之前为上升则为波峰，如果当前为上升之前为下降则为波谷。将波峰波谷值进行记录。

 private boolean check_peak(float newValue, float oldValue)
    {
        State_befor = Up;//保存上次的状态
        if (newValue >= oldValue)//上升
        {
            Up = true;
            UpCount++;
            DownCount_before=DownCount;
            DownCount=0;

        }
        else//下降
         {
               UpCount_before = UpCount;
               UpCount = 0;

               DownCount++;
               Up = false;
          }

        if (!Up && State_befor && (UpCount_before >= UP_DOWN_COUNT || oldValue >= 25)) //当前下降之前上升为波峰
        {
            peak = oldValue;
            return true;
        } else if (!State_befor && Up&&DownCount_before>=UP_DOWN_COUNT)//当前上升之前下降为波谷
        {
            valley = oldValue;
            return false;
        } else
         {
            return false;
        }
    }

动态阀值更新

如果动态阀值数组没有满，则进行保存，返回原始动态阀值。否则，将数组值进行替换返回更新后的动态法制…阀值

private float limit_value_update(float value) //传入波峰与波谷的差值
    {
        float temp = Auto_limit_value;
        if (diffCount < num)
        {
            diffValue[diffCount] = value;
            diffCount++;
        } else
            {
               temp = averageValue(diffValue, num);
               for (int i = 1; i < num; i++)
               {
                diffValue[i - 1] = diffValue[i];
               }
                diffValue[num - 1] = value;
            }
        return temp;

    }

阀值梯度

动态配置阀值的梯度，调节感应的灵敏度。

private float averageValue(float value[], int n) {
        float ave = 0;
        for (int i = 0; i < n; i++) {
            ave += value[i];
        }
        ave = ave / n;
        if (ave >= 8)
            ave = (float) 6.5;
        else if (ave >= 7 && ave < 8)
            ave = (float) 5.5;
        else if (ave >= 6 && ave < 7)
            ave = (float) 4.5;
        else {
            ave = (float) 3.5;
        }
        return ave;
    }

在主界面传入temp，通过step可以获得步数。

float temp = (float) Math.sqrt(x_ * x_ + y_ * y_ + z_ * z_);
  step1.step_run(temp);
   textView.setText("当前步数："+step1.step);