jianbo_lib/c/motion/detect_step_by_acc.c

#include "detect_step_by_acc.h"

static int ACC_WINDOW_SIZE=20;

static float acc_y_buff[20];
static float acc_z_buff[20];
static float acc_x_buff[20];

static float acc_xyz_buff[20];

//static int acc_y_wait;
//static int acc_z_wait;
//static int acc_x_wait;

static int acc_y_status = 1;
static int acc_z_status = 1;
static int acc_x_status = 1;

//static int last_zupt;

static int last_acc_x_status;
static int last_acc_y_status;
static int last_acc_z_status;


static int x_raised;
static int y_raised;
static int z_raised;

static int x_hollow;
static int y_hollow;
static int z_hollow;

static int zero_x_wait;
static int zero_y_wait;
static int zero_z_wait;

//static int can_detect_zupt = 1;
//static int stop_out_zupt = 0;

//static int acc_diff_wait_count = 0;

static int last_time_stamp = 0;

//static int PDR_step_count = 0;

static float max_z_unzero = 1.0f;
static float min_z_unzero = 1.0f;



void detect_raised_hollow(float *acc, int length, float max_val_thresh, float min_val_thresh, float distance, int *raised, int *hollow)
{
	//本函数用来检测加速度曲线的凹凸标志
	//1、遍历加速度窗口的最大值及最小值
	int start_index = ACC_WINDOW_SIZE - length;
	
	if(start_index < 0)
	{
		return;
	}
	
	float max_val = acc[ACC_WINDOW_SIZE - length / 2];
	float min_val = acc[ACC_WINDOW_SIZE - length / 2];
	
	for(int i = ACC_WINDOW_SIZE - length / 2; i < ACC_WINDOW_SIZE; i++)
	{
		if(max_val < acc[i])
		{
			max_val = acc[i];
		}
		
		if(min_val > acc[i])
		{
			min_val = acc[i];
		}
	}
	//检测到平稳信号，需要重新检测凹凸性
	if(max_val - min_val < 0.2f)
	{
		*raised = 0; 
		*hollow = 0; 
		
		return;
	}
	
	for(int i = 0; i < ACC_WINDOW_SIZE - length / 2; i++)
	{
		if(max_val < acc[i])
		{
			max_val = acc[i];
		}
		
		if(min_val > acc[i])
		{
			min_val = acc[i];
		}
	}
	
	
	//检测凹凸性
	if(max_val - acc[0] > distance && max_val - acc[ACC_WINDOW_SIZE-1] > distance && *raised == 0 && max_val > max_val_thresh)
	{
		*raised = 1;
	}
	
	if(acc[0] - min_val > distance && acc[ACC_WINDOW_SIZE - 1] - min_val > distance && *hollow == 0 && min_val < min_val_thresh)
	{
		*hollow = 1;
	}

}

//求取拐点
int detect_turn_line(float *acc, int length, int tag)
{
	int start_index = ACC_WINDOW_SIZE - length;
	
	float min_val = acc[start_index];
	
	int min_index = start_index;
	
	float max_val = acc[start_index];
	
	int max_index = start_index;
	
	for(int i = 8;  i < ACC_WINDOW_SIZE; i ++)
	{
		 if(min_val > acc[i])
		 {
			 min_val = acc[i];
			 min_index = i;
		 }
		 
		 if(max_val < acc[i])
		 {
			 max_val = acc[i];
			 max_index = i;
		 }
		 
	}
	
	float min_val_5 = acc[ACC_WINDOW_SIZE - 5];
	float max_val_5 = acc[ACC_WINDOW_SIZE - 5];
	
	for(int i = ACC_WINDOW_SIZE - 5; i < ACC_WINDOW_SIZE; i++ )
	{
		if(min_val_5 > acc[i])
		{
			min_val_5 = acc[i];
		}
		
		if(max_val_5 < acc[i])
		{
			max_val_5 = acc[i];
		}
	}
	
	if(max_val - min_val > 0.5f && max_val_5 - min_val_5 < 0.2f)
	{
		if(tag == 1)
		{
			if(max_index < min_index)
			{
				return 1;
			}
			else
			{
				return 0;
			}
		}
		return 1;
	}
	
	return 0;
}


//求取平稳信号
int station_signal(float *acc, int length)
{
	int start_index = ACC_WINDOW_SIZE - length;
	
	float max_val = acc[start_index];
	float min_val = acc[start_index];
	
	
	for(int i = start_index; i < ACC_WINDOW_SIZE; i++)
	{
		if(max_val < acc[i])
		{
			max_val = acc[i];
		}
		
		if(min_val > acc[i])
		{
			min_val = acc[i];
		}
	}
	
	if(max_val - min_val < 0.15f)
	{
		
		return 1;
	}
	
	return 0;
}

//剧烈信号探测拐点

int severe_turn_point_z(float *acc, int length)
{
	float max_val = acc[0];
	float min_val = acc[0];
	
	float min_temp = acc[0];
	
	float max_distance = max_val - min_val;
	
	for(int i = 1 ; i < ACC_WINDOW_SIZE; i++)
	{
		float distance_tmp = acc[i-1] - acc[i];
		
		if(i > 1 && distance_tmp < (acc[i-2] - acc[i]))
			distance_tmp = (acc[i-2] - acc[i]);
		
		if(i > 2 && distance_tmp < (acc[i-3] - acc[i]))
			distance_tmp = (acc[i-3] - acc[i]);
		
		if(i > 3 && distance_tmp < (acc[i-4] - acc[i]))
			distance_tmp = (acc[i-4] - acc[i]);
		
		if(i > 4 && distance_tmp < (acc[i-5] - acc[i]))
			distance_tmp = (acc[i-5] - acc[i]);
		
		if(max_distance < distance_tmp)
		{
			max_distance = distance_tmp;
		}
		
		if(min_temp > acc[i])
		{
			min_temp = acc[i];
		}
	}

	
	max_val = acc[ACC_WINDOW_SIZE-5];
	min_val = acc[ACC_WINDOW_SIZE-5];
	
	for(int i = ACC_WINDOW_SIZE - 5; i< ACC_WINDOW_SIZE; i++)
	{
		if(max_val < acc[i])
		{
			max_val = acc[i];
		}
		
		if(min_val > acc[i])
		{
			min_val = acc[i];
		}
	}
	
	float min_distance = max_val - min_val;
	
	if(max_distance > 1.5f && min_distance < 0.3f && min_temp > 0.5f)
	{
		return 1;
	}
	
	return 0;
	
}


int severe_turn_point(float *acc, int length)
{
	float max_val = acc[0];
	float min_val = acc[0];
	
	float max_distance = max_val - min_val;
	
	for(int i = 1 ; i < ACC_WINDOW_SIZE; i++)
	{
		float distance_tmp = acc[i] - acc[i-1];
		
		if(i > 1 && distance_tmp < fabsf(acc[i] - acc[i-2]))
			distance_tmp = fabsf(acc[i] - acc[i-2]);
		
		if(i > 2 && distance_tmp < fabsf(acc[i] - acc[i-3]))
			distance_tmp = fabsf(acc[i] - acc[i-3]);
		
		if(i > 3 && distance_tmp < fabsf(acc[i] - acc[i-4]))
			distance_tmp = fabsf(acc[i] - acc[i-4]);
		
		if(i > 4 && distance_tmp < fabsf(acc[i] - acc[i-5]))
			distance_tmp = fabsf(acc[i] - acc[i-5]);
		
		if(max_distance < distance_tmp)
		{
			max_distance = distance_tmp;
		}
	}

	
	max_val = acc[ACC_WINDOW_SIZE-5];
	min_val = acc[ACC_WINDOW_SIZE-5];
	
	for(int i = ACC_WINDOW_SIZE - 5; i< ACC_WINDOW_SIZE; i++)
	{
		if(max_val < acc[i])
		{
			max_val = acc[i];
		}
		
		if(min_val > acc[i])
		{
			min_val = acc[i];
		}
	}
	
	float min_distance = max_val - min_val;
	
	if(max_distance > 1.3f && min_distance < 0.3f)
	{
		return 1;
	}
	
	return 0;
	
}


float var_acc(float *acc, int length)
{
	if(length < 10)
	{
		return 0.0f;
	}
	
	float mean_x = 0.0f;
	
	float sum_x  = 0.0f;
	
	for(int i = length - 10; i < length; i++)
	{
		sum_x += acc[i];
	}
	
	mean_x = sum_x * 0.1f;
	
	sum_x = 0.0f;
	
	for(int i = length - 10; i < length; i++)
	{
		sum_x += ((acc[i]- mean_x) * (acc[i]- mean_x));
	}
	
	
	return sum_x * 0.1f;
}


//寻找窗口的最小值
float min_window(float *acc, int length)
{
	int start_index = ACC_WINDOW_SIZE - length;
	
	float min_val = acc[start_index];
	
	for(int i = 0; i < ACC_WINDOW_SIZE; i++)
	{
		if(acc[i] < min_val)
		{
			min_val = acc[i];
		}
	}
	
	return min_val;
}

//寻找窗口的最大值
float max_window(float *acc, int length)
{
	int start_index = ACC_WINDOW_SIZE - length;
	
	float max_val = acc[start_index];
	
	for(int i = 0; i < ACC_WINDOW_SIZE; i++)
	{
		if(acc[i] > max_val)
		{
			max_val = acc[i];
		}
	}
	
	return max_val;
}

void detect_step_by_acc(uint32_t time_stamp, float *acc,  int *x_zero, int *y_zero, int *z_zero, int *zupt)
{
	//缓存加速度的滑动窗口
	
	memcpy(acc_x_buff, acc_x_buff + 1, (ACC_WINDOW_SIZE - 1) * sizeof(float));
	
	acc_x_buff[(ACC_WINDOW_SIZE - 1)] = acc[0];
	
	memcpy(acc_y_buff, acc_y_buff + 1, (ACC_WINDOW_SIZE - 1) * sizeof(float));
	
	acc_y_buff[(ACC_WINDOW_SIZE - 1)] = acc[1];
	
	memcpy(acc_z_buff, acc_z_buff + 1, (ACC_WINDOW_SIZE - 1)  * sizeof(float));
	
	acc_z_buff[(ACC_WINDOW_SIZE - 1)] = acc[2];
	
	memcpy(acc_xyz_buff, acc_xyz_buff + 1, (ACC_WINDOW_SIZE - 1)  * sizeof(float));
	
	acc_xyz_buff[(ACC_WINDOW_SIZE - 1)] = sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
	
	
	
	//检测拐点
	//detect_raised_hollow(float *acc, int length, float max_val_thresh, float min_val_thresh, float distance, int *raised, int *hollow)
	detect_raised_hollow(acc_x_buff, ACC_WINDOW_SIZE, 0.7f, -0.7f, 0.5f, &x_raised, &x_hollow);
	
	detect_raised_hollow(acc_y_buff, ACC_WINDOW_SIZE, 0.7f, -0.7f, 0.5f, &y_raised, &y_hollow);
	
	detect_raised_hollow(acc_z_buff, ACC_WINDOW_SIZE, 1.2f, 0.8f, 0.4f,  &z_raised, &z_hollow);

	if(x_raised == 1 && x_hollow == 1 && zero_x_wait == 0)
	{
		zero_x_wait = 30;
	}
	
	if(y_raised == 1 && y_hollow == 1 && zero_y_wait == 0)
	{
		zero_y_wait = 30;
	}
	
	if(z_raised == 1 && z_hollow == 1 && zero_z_wait == 0)
	{
		zero_z_wait = 30;
	}

	acc_x_status = 0;
	acc_y_status = 0;
	acc_z_status = 0;
	
	if((zero_x_wait > 0 || acc_x_status  == 1) && detect_turn_line(acc_x_buff, 10, 0))
	{
		acc_x_status  = 1;
	}
	
	if((zero_y_wait > 0 || acc_y_status  == 1) && detect_turn_line(acc_y_buff, 10, 0))
	{
		acc_y_status  = 1;
	}
	
	if((zero_z_wait > 0 || acc_z_status  == 1) && detect_turn_line(acc_z_buff, 10, 1))
	{
		acc_z_status  = 1;
	}
	
	
	//剧烈信号探测拐点
	if(severe_turn_point(acc_x_buff, ACC_WINDOW_SIZE))
	{
		acc_x_status = 1;
	}
	
	if(severe_turn_point(acc_y_buff, ACC_WINDOW_SIZE))
	{
		acc_y_status = 1;
	}
	
	if(severe_turn_point_z(acc_z_buff, ACC_WINDOW_SIZE))
	{
		acc_z_status = 4;
	}
	
		//标志垫着脚跑的变量
	int special_zupt = 0;
	
	if(var_acc(acc_z_buff, ACC_WINDOW_SIZE) > 0.5f && max_window(acc_z_buff, 10) > 1.0f && acc[2] > 0.7f)
	{
		acc_x_status = 1;
		acc_y_status = 1;
		acc_z_status = 3;
		
		special_zupt = 1;
	}
	
	//当上次信号大于0的时候, 判断下滑曲线
	if(last_acc_x_status  && station_signal(acc_x_buff, 5))
	{
		acc_x_status  = 2;
		
	}
	
	if(last_acc_y_status  && station_signal(acc_y_buff, 5))
	{
		acc_y_status  = 2;
	}

	if(last_acc_z_status && station_signal(acc_z_buff, 5))
	{
		acc_z_status  = 2;
	}	
	
	if((acc_x_status || acc_y_status) && fabsf(acc[2] - 1.0f) < 0.2f && station_signal(acc_z_buff, 10))
	{
		acc_z_status  = 2;
	}
	
	if(acc_z_status == 0 && last_acc_z_status == 3 && max_window(acc_z_buff, 10) - acc[2] > 1.0f && acc[2] > 0.7f && acc[2] <1.5f)
	{
		acc_z_status = 3;
	}
	//因为会有断开的现象，所以大概估计到200ms内出现平缓信号，视为在地上
	if(time_stamp - last_time_stamp  < 20)
	{
		if(max_window(acc_xyz_buff, 10) - min_window(acc_xyz_buff, 10) < 0.1f)
		{
			acc_x_status = 1;
			acc_y_status = 1;
			acc_z_status = 1;
		}
	}
	
//利用奇异值来去掉误判信号
	if(special_zupt == 0 && (acc[2] < 0.8f || fabsf(acc[0]) > 0.5f ||fabsf(acc[1]) > 0.5f))
	{
		acc_x_status = 0;
		acc_y_status = 0;
		acc_z_status = 0;
	}
	
	
	//凹凸信号都检测信号，需要延时等待
	if(zero_x_wait > 0)
	{
		zero_x_wait --;
		if(zero_x_wait == 0)
		{
			x_raised = 0;
			x_hollow = 0;
		}
	}

  if(zero_y_wait > 0)
	{
		zero_y_wait --;
		
		if(zero_y_wait == 0)
		{
			y_raised = 0;
			y_hollow = 0;
		}
	}
	
	if(zero_z_wait > 0)
	{
		zero_z_wait --;
		
		if(zero_z_wait == 0)
		{
			z_raised = 0;
			z_hollow = 0;
		}
	}

	last_acc_x_status = acc_x_status;
	last_acc_y_status = acc_y_status;
	last_acc_z_status = acc_z_status;
	
	if(time_stamp - last_time_stamp < 15)
	{
		if(acc[2] > max_z_unzero)
		{
			max_z_unzero = acc[2];
		}
		
		if(acc[2] < min_z_unzero)
		{
			min_z_unzero = acc[2];
		}
	}
	else
	{
		max_z_unzero = 1.0f;
    min_z_unzero = 1.0f;
	}
	
	
	//记录zupt
	if( (acc_z_status > 0 || (acc_x_status > 0 && acc_y_status > 0)) )
	{
		*zupt = 1;
		
		if(time_stamp - last_time_stamp > 15)
		{
			//PDR_step_count ++;
			mFlash.mStep.stepCur[0]++;
		}
		else if(min_z_unzero < 0.0f && max_z_unzero > 2.0f)
		{
			mFlash.mStep.stepCur[0]++;
			
			max_z_unzero = 1.0f;
			min_z_unzero = 1.0f;
			
		}
		
		last_time_stamp = time_stamp;
	}
	else
	{
		*zupt = 0;
	}
	
	
	*x_zero = 0;
	*y_zero = 0;
	*z_zero = acc_z_status;
	
}