shoes/chenjunchao/esb_ptx/motion/trajAlgorithm/footPDR.c

#include "footPDR.h"

//当地的重力加速度
float g = 9.8179995f;

float dt = 0.01f;

float P[81], acc_n[3];

float Temporary_array1[9], Temporary_array2[9];

float K[27], P_prev[81], delta_x[9];

float C[9], C_prev[9];

float vel_n[3], pos_n[3];

float last_pos_n[3];

float pos_offset[3];

int frame_index = 0;

int stand_num = 0;

float gyr_norm_window[10];

float gyr_extreme[6];

float gyr_mean[3];

float num_peak;

float acc_mean[3];

float gyrBias[3];

float last_gyr[3];
float last_acc[3];
float last_vel_n[3];

float accSum;

float accSize;

int press_data[10];

int ZUPT_STATUS;

int press_index;

int time_zupt;

int SAVEFLASH = 1;

uint32_t gyrFlash[3];

//last_stage:0 为 走路状态；
//last_stage:1 为 静止状态

int last_stage;
uint8_t step_count;
uint16_t step_distance;

int step_time = 0;

int testCon = 0;

//UKF参数
int UKF_L = 4;
float UKF_alpha = 0.01f;
float UKF_beta = 2.0f;
float UKF_kappa = 0.0f;
float gamma;
float wm[9];
float wc[9];
float UKF_Q[4];
float UKF_P[4][4];
int UKF_iter;
float mag_prev[3];
float deltaC[9];
float UKF_C[9];
float UKF_roll;
float UKF_pitch;
float UKF_yaw;
float EKF_roll;
float EKF_pitch;
float EKF_yaw;
int canUKF = 0;
int stand_zupt_count = 0;

//重置磁航向，计算双脚的磁航向，以确定身体的正朝向
float magSum_xyz[3];
float accSum_xyz[3];
float standMag[3];

void calKafmanGain9x4(float *K, float *P)
{
	float m_rever[4][4];
	float m[4][4];
	
	m[0][0] = P[20];m[0][1] = P[24];m[0][2] = P[25];m[0][3] = P[26];
	m[1][0] = P[56];m[1][1] = P[60];m[1][2] = P[61];m[1][3] = P[62];
	m[2][0] = P[65];m[2][1] = P[69];m[2][2] = P[70];m[2][3] = P[71];
	m[3][0] = P[74];m[3][1] = P[78];m[3][2] = P[79];m[3][3] = P[80];
	
	for(int i = 0; i <4; i++)
	{
		m[i][i] += SIGMA_V * SIGMA_V;
	}
	
	matrix_inverse(m, m_rever);
	
	K[0] = P[2] * m_rever[0][0] + P[6] * m_rever[1][0] + P[7] * m_rever[2][0]  + P[8] * m_rever[3][0];
	K[1] = P[2] * m_rever[0][1] + P[6] * m_rever[1][1] + P[7] * m_rever[2][1]  + P[8] * m_rever[3][1];
	K[2] = P[2] * m_rever[0][2] + P[6] * m_rever[1][2] + P[7] * m_rever[2][2]  + P[8] * m_rever[3][2];
	K[3] = P[2] * m_rever[0][3] + P[6] * m_rever[1][3] + P[7] * m_rever[2][3]  + P[8] * m_rever[3][3];
	
	K[4] = P[11] * m_rever[0][0] + P[15] * m_rever[1][0] + P[16] * m_rever[2][0] + P[17] * m_rever[3][0];
	K[5] = P[11] * m_rever[0][1] + P[15] * m_rever[1][1] + P[16] * m_rever[2][1] + P[17] * m_rever[3][1];
	K[6] = P[11] * m_rever[0][2] + P[15] * m_rever[1][2] + P[16] * m_rever[2][2] + P[17] * m_rever[3][2];
	K[7] = P[11] * m_rever[0][3] + P[15] * m_rever[1][3] + P[16] * m_rever[2][3] + P[17] * m_rever[3][3];
	
	K[8] = P[20] * m_rever[0][0] + P[24] * m_rever[1][0] + P[25] * m_rever[2][0] + P[26] * m_rever[3][0];
	K[9] = P[20] * m_rever[0][1] + P[24] * m_rever[1][1] + P[25] * m_rever[2][1] + P[26] * m_rever[3][1];
	K[10] = P[20] * m_rever[0][2] + P[24] * m_rever[1][2] + P[25] * m_rever[2][2] + P[26] * m_rever[3][2];
	K[11] = P[20] * m_rever[0][3] + P[24] * m_rever[1][3] + P[25] * m_rever[2][3] + P[26] * m_rever[3][3];
	
	K[12] = P[29] * m_rever[0][0] + P[33] * m_rever[1][0] + P[34] * m_rever[2][0] + P[35] * m_rever[3][0];
	K[13] = P[29] * m_rever[0][1] + P[33] * m_rever[1][1] + P[34] * m_rever[2][1] + P[35] * m_rever[3][1];
	K[14] = P[29] * m_rever[0][2] + P[33] * m_rever[1][2] + P[34] * m_rever[2][2] + P[35] * m_rever[3][2];
	K[15] = P[29] * m_rever[0][3] + P[33] * m_rever[1][3] + P[34] * m_rever[2][3] + P[35] * m_rever[3][3];
	
	K[16] = P[38] * m_rever[0][0] + P[42] * m_rever[1][0] + P[43] * m_rever[2][0] + P[44] * m_rever[3][0];
	K[17] = P[38] * m_rever[0][1] + P[42] * m_rever[1][1] + P[43] * m_rever[2][1] + P[44] * m_rever[3][1];
	K[18] = P[38] * m_rever[0][2] + P[42] * m_rever[1][2] + P[43] * m_rever[2][2] + P[44] * m_rever[3][2];
	K[19] = P[38] * m_rever[0][3] + P[42] * m_rever[1][3] + P[43] * m_rever[2][3] + P[44] * m_rever[3][3];
	
	K[20] = P[47] * m_rever[0][0] + P[51] * m_rever[1][0] + P[52] * m_rever[2][0] + P[53] * m_rever[3][0];
	K[21] = P[47] * m_rever[0][1] + P[51] * m_rever[1][1] + P[52] * m_rever[2][1] + P[53] * m_rever[3][1];
	K[22] = P[47] * m_rever[0][2] + P[51] * m_rever[1][2] + P[52] * m_rever[2][2] + P[53] * m_rever[3][2];
	K[23] = P[47] * m_rever[0][3] + P[51] * m_rever[1][3] + P[52] * m_rever[2][3] + P[53] * m_rever[3][3];
	
	K[24] = P[56] * m_rever[0][0] + P[60] * m_rever[1][0] + P[61] * m_rever[2][0] + P[62] * m_rever[3][0];
	K[25] = P[56] * m_rever[0][1] + P[60] * m_rever[1][1] + P[61] * m_rever[2][1] + P[62] * m_rever[3][1];
	K[26] = P[56] * m_rever[0][2] + P[60] * m_rever[1][2] + P[61] * m_rever[2][2] + P[62] * m_rever[3][2];
	K[27] = P[56] * m_rever[0][3] + P[60] * m_rever[1][3] + P[61] * m_rever[2][3] + P[62] * m_rever[3][3];
	
	
	K[28] = P[65] * m_rever[0][0] + P[69] * m_rever[1][0] + P[70] * m_rever[2][0] + P[71] * m_rever[3][0];
	K[29] = P[65] * m_rever[0][1] + P[69] * m_rever[1][1] + P[70] * m_rever[2][1] + P[71] * m_rever[3][1];
	K[30] = P[65] * m_rever[0][2] + P[69] * m_rever[1][2] + P[70] * m_rever[2][2] + P[71] * m_rever[3][2];
	K[31] = P[65] * m_rever[0][3] + P[69] * m_rever[1][3] + P[70] * m_rever[2][3] + P[71] * m_rever[3][3];
	
	K[32] = P[74] * m_rever[0][0] + P[78] * m_rever[1][0] + P[79] * m_rever[2][0] + P[80] * m_rever[3][0];
	K[33] = P[74] * m_rever[0][1] + P[78] * m_rever[1][1] + P[79] * m_rever[2][1] + P[80] * m_rever[3][1];
	K[34] = P[74] * m_rever[0][2] + P[78] * m_rever[1][2] + P[79] * m_rever[2][2] + P[80] * m_rever[3][2];
	K[35] = P[74] * m_rever[0][3] + P[78] * m_rever[1][3] + P[79] * m_rever[2][3] + P[80] * m_rever[3][3];

}

void calDeltaX9x4(float *K, float *measure, float *delta_x)
{
	for(int i = 0; i < 9; i++)
	{
		delta_x[i] = 0.0f;
		
		for(int j = 0; j < 4; j ++)
		{
			delta_x[i] += (K[i * 4 + j] *measure[j]);
		}
	}
}

void calStateCov9x4(float *P, float *K)
{
	static float P_copy[81];
	
	for(int i = 0; i < 9; i++)
	{
		for(int j = 0; j < 9; j++)
		{
			P_copy[i * 9 + j]  = K[i * 4] * P[18 + j] + K[i * 4 + 1] * P[54 + j] + K[i * 4 + 2] * P[63 + j] + K[i * 4 + 3] * P[72 + j]; 
		}
	}
	
	for(int i = 0; i < 81 ; i ++)
	{
		P[i] -= P_copy[i];
	}
}

 float calHeading(float mag[3], float acc[3])
 {
	 float hSqrt;
	 float eSqrt;

	 
	 float h[3]; //东向
	 
	 h[0] = mag[1] * acc[2] - mag[2] * acc[1];
	 h[1] = mag[2] * acc[0] - mag[0] * acc[2];
	 h[2] = mag[0] * acc[1] - mag[1] * acc[0];
	 
	 hSqrt = 1/sqrt(h[0] * h[0] + h[1] * h[1] + h[2] * h[2]);
	 
	 for(int i = 0; i < 3; i++)
	 {
		 h[i] *= hSqrt;
	 }
	 
	 float e[3]; //北向
	 
	 e[0] = acc[1] * h[2] - acc[2] * h[1];
	 e[1] = acc[2] * h[0] - acc[0] * h[2];
	 e[2] = acc[0] * h[1] - acc[1] * h[0];
	 
	 eSqrt = 1/sqrt(e[0] * e[0] + e[1] * e[1] + e[2] * e[2]);
	 
	 for(int i = 0; i < 3; i++)
	 {
		 e[i] *= eSqrt;
	 }

	return atan2(-h[1], e[1]);
	 
 }
 
 void resetAttbyMag(float C[9], float acc[3], float mag[3])
 {	
		float accScale = sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
	 
	 	float pitch = asin(-acc[0]/accScale);

		float roll = atan2(acc[1], acc[2]);
	
		float pitch_sin = sin(pitch);

		float pitch_cos = cos(pitch);

		float roll_sin = sin(roll);

		float roll_cos = cos(roll);
	 
	  float mag_heading;

		C[0] = pitch_cos;
		C[1] = pitch_sin * roll_sin;
		C[2] = pitch_sin * roll_cos;

		C[3] = 0.0;
		C[4] = roll_cos;
		C[5] = -roll_sin;
		
		mag_heading = atan2(-C[4] * mag[1] - C[5] * mag[2], C[0] * mag[0] + C[1] * mag[1] + C[2] * mag[2]);
		
	  float yaw_sin = sin(mag_heading);
		
	  float yaw_cos = cos(mag_heading);
		
		C[0] = pitch_cos * yaw_cos;
		C[1] = pitch_sin * roll_sin * yaw_cos - roll_cos * yaw_sin;
		C[2] = pitch_sin * roll_cos * yaw_cos + roll_sin * yaw_sin;

		C[3] = pitch_cos * yaw_sin;
		C[4] = pitch_sin * roll_sin * yaw_sin + roll_cos * yaw_cos;
		C[5] = pitch_sin * roll_cos * yaw_sin - roll_sin * yaw_cos;
		
		C[6] = acc[0];
		C[7] = acc[1];
		C[8] = acc[2];
		
 }

void cal_step_data(void)
{
	static  int  step_count_sum = 0;

	step_count = 1;
	step_count_sum += step_count;

	float step_length = sqrt((last_pos_n[0] - pos_n[0])*(last_pos_n[0] - pos_n[0]) + (last_pos_n[1] - pos_n[1])*(last_pos_n[1] - pos_n[1]));

	if (step_length > 5.0f)
	{
		step_length = 1.2f;
	}

	step_distance = (uint16_t)(step_length * 100.0f);

	//sen_step_to_host(step_count, step_distance);

}
uint8_t get_step_count(void)
{
	return step_count;
}

uint16_t get_step_length(void)
{
	return step_distance;
}


void  Initialize(float *gyr, float *acc)
{
	frame_index = 1;

	stand_num = 0;

	accSize = 1.0f;

	accSum = 0.0f;

	ZUPT_STATUS = 0;

	memset(last_pos_n, 0, 3 * sizeof(float));

	memset(pos_offset, 0, 3 * sizeof(float));

	memset(gyr_norm_window, 0, 10 * sizeof(float));

	memset(P, 0, 81 * sizeof(float));

	memset(acc_n, 0, 3 * sizeof(float));

	memset(vel_n, 0, 3 * sizeof(float));

	memset(pos_n, 0, 3 * sizeof(float));

	memset(Temporary_array1, 0, 9 * sizeof(float));

	memset(Temporary_array2, 0, 9 * sizeof(float));

	memset(K, 0, 27 * sizeof(float));

	memset(P_prev, 0, 81 * sizeof(float));

	memset(delta_x, 0, 9 * sizeof(float));

	memset(C, 0, 9 * sizeof(float));

	memset(Temporary_array1, 0, 9 * sizeof(float));

	memset(Temporary_array2, 0, 9 * sizeof(float));

	memset(press_data, 0, 10 * sizeof(int));

	init_attitude_matrix(C, acc, g);

	memcpy(C_prev, C, 9 * sizeof(float));
	
//	memcpy(gyrBias, (uint32_t *)FLASH_ADD, 3 * sizeof(float));
}

void attitude_matrix_update(float *C, float *Temporary_array1, float *Temporary_array2, float *gyr, float dt)
{
	Temporary_array1[0] = 2.0f;
	Temporary_array1[1] = dt * gyr[2];
	Temporary_array1[2] = -dt * gyr[1];

	Temporary_array1[3] = -dt * gyr[2];
	Temporary_array1[4] = 2.0f;
	Temporary_array1[5] = dt * gyr[0];

	Temporary_array1[6] = dt * gyr[1];
	Temporary_array1[7] = -dt * gyr[0];
	Temporary_array1[8] = 2.0f;

	invert3x3(Temporary_array1, Temporary_array2);

	memset(Temporary_array1, 0, 9 * sizeof(float));

	Temporary_array1[0] = 2 * C[0] + C[1] * dt * gyr[2] - C[2] * dt * gyr[1];
	Temporary_array1[1] = 2 * C[1] - C[0] * dt * gyr[2] + C[2] * dt * gyr[0];
	Temporary_array1[2] = 2 * C[2] + C[0] * dt * gyr[1] - C[1] * dt * gyr[0];

	Temporary_array1[3] = 2 * C[3] + C[4] * dt * gyr[2] - C[5] * dt * gyr[1];
	Temporary_array1[4] = 2 * C[4] - C[3] * dt * gyr[2] + C[5] * dt * gyr[0];
	Temporary_array1[5] = 2 * C[5] + C[3] * dt * gyr[1] - C[4] * dt * gyr[0];

	Temporary_array1[6] = 2 * C[6] + C[7] * dt * gyr[2] - C[8] * dt * gyr[1];
	Temporary_array1[7] = 2 * C[7] - C[6] * dt * gyr[2] + C[8] * dt * gyr[0];
	Temporary_array1[8] = 2 * C[8] + C[6] * dt * gyr[1] - C[7] * dt * gyr[0];

	multiply3x3(Temporary_array1, Temporary_array2, C);
}

float max_window_val(float *window, int window_size)
{
	float val = window[0];

	for (int i = 0; i < window_size; i++)
	{
		if (window[i] > val)
			val = window[i];
	}

	return val;
}

int max_window_int(int *window, int window_size)
{
	int val = window[0];

	for (int i = 0; i < window_size; i++)
	{
		if (window[i] > val)
			val = window[i];
	}

	return val;
}

float min_window_val(float *window, int window_size)
{
	float val = window[0];

	for (int i = 0; i < window_size; i++)
	{
		if (window[i] < val)
			val = window[i];
	}

	return val;
}

int min_window_int(int *window, int window_size)
{
	int val = window[0];

	for (int i = 0; i < window_size; i++)
	{
		if (window[i] < val)
			val = window[i];
	}

	return val;
}

//press_tren 函数功能：提供走路过程中上升沿，下降沿
//1 为上升 2 为 下降 0为不需要得状态
int press_trend(int index, int *window, int window_size)
{
	int i;
	int max_val = window[(index - 1) % window_size];
	int max_index = index;

	int min_val = max_val;
	int min_index = max_index;

	for (i = 1; i < window_size + 1; i++)
	{
		if (max_val < window[(index - i) % window_size])
		{
			max_index = index - i + 1;
			max_val = window[(index - i) % window_size];
		}

		if (min_val > window[(index - i) % window_size])
		{
			min_index = index - i + 1;
			min_val = window[(index - i) % window_size];
		}
	}

	if (max_index > min_index && max_val > min_val + 50000)
	{
		return 1;
	}

	if (max_index < min_index  && max_val > min_val + 50000)
	{
		return 2;
	}

	return 0;
}

void dcm2angle(float *dcm, float *roll, float *pitch, float *yaw)
{
	*yaw = atan2(dcm[3], dcm[0]);

	*pitch = asin(-dcm[6]);

	*roll = atan2(dcm[7], dcm[8]);
}

void quat2angleTest(float qin[4], float *roll, float *pitch, float *yaw)
{
	//float r11 = qin[0] * qin[0] + qin[1] * qin[1] - qin[2] * qin[2] - qin[3] * qin[3];
	float r11 = 2.0f * (qin[1] * qin[2] + qin[0] * qin[3]);
	//float r21 = 2.0f * (qin[1] * qin[2] - qin[0] * qin[3]);
	float r12 = qin[0] * qin[0] + qin[1] * qin[1] - qin[2] * qin[2] - qin[3] * qin[3];

	float r21 = -2.0f * (qin[1] * qin[3] - qin[0] * qin[2]);

	float r31 = 2.0f * (qin[2] * qin[3] + qin[0] * qin[1]);

	float r32 = qin[0] * qin[0] - qin[1] * qin[1] - qin[2] * qin[2] + qin[3] * qin[3];

	if (r21 < -0.999999999f)

		r21 = -1.0f;

	else if (r21 > 0.999999999f)
		r21 = 1.0f;

	*roll = atan2(r11, r12);
	
	*pitch = asin(r21);
	
	*yaw = atan2(r31, r32);
}

void dcm2angleTest(float C[9], short att[3])
{
	float yaw, pitch, roll;
	
	pitch = asin(-C[6]);
	
	if(C[6] > 0.999999f || C[6] < -0.999999f)
	{
		   //当俯仰角为90度的时候，则假设翻滚角为0度
			yaw = atan2(-C[1], C[4]);
		
			roll = 0.0f;
	}
	else
	{
			yaw = atan2(C[3], C[0]);
			
			roll  = atan2(C[7], C[8]);
	}
	
	att[0] = (short)(yaw * 10000.f); //yaw
	att[1] = (short)(pitch * 10000.f);  //pitch
	att[2] = (short)(roll * 10000.f);	//roll
}

void quat2dcm(float *qin, float *dcm)
{
	float qin_norm = 1 / sqrt(qin[0] * qin[0] + qin[1] * qin[1] + qin[2] * qin[2] + qin[3] * qin[3]);
	
	for (int i = 0; i < 4; i++)
		qin[i] *= qin_norm;

	dcm[0] = qin[0] * qin[0] + qin[1] * qin[1] - qin[2] * qin[2] - qin[3] * qin[3];
	dcm[1] = 2.0f * (qin[1] * qin[2] + qin[0] * qin[3]);
	dcm[2] = 2.0f * (qin[1] * qin[3] - qin[0] * qin[2]);

	dcm[3] = 2.0f * (qin[1] * qin[2] - qin[0] * qin[3]);
	dcm[4] = qin[0] * qin[0] - qin[1] * qin[1] + qin[2] * qin[2] - qin[3] * qin[3];
	dcm[5] = 2.0f * (qin[2] * qin[3] + qin[0] * qin[1]);

	dcm[6] = 2.0f * (qin[1] * qin[3] + qin[0] * qin[2]);
	dcm[7] = 2.0f * (qin[2] * qin[3] - qin[0] * qin[1]);
	dcm[8] = qin[0] * qin[0] - qin[1] * qin[1] - qin[2] * qin[2] + qin[3] * qin[3];
}

void multiply3x3T(float *a, float *b, float* dst)
{
	dst[0] = a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
	dst[1] = a[0] * b[3] + a[1] * b[4] + a[2] * b[5];
	dst[2] = a[0] * b[6] + a[1] * b[7] + a[2] * b[8];

	dst[3] = a[3] * b[0] + a[4] * b[1] + a[5] * b[2];
	dst[4] = a[3] * b[3] + a[4] * b[4] + a[5] * b[5];
	dst[5] = a[3] * b[6] + a[4] * b[7] + a[5] * b[8];

	dst[6] = a[6] * b[0] + a[7] * b[1] + a[8] * b[2];
	dst[7] = a[6] * b[3] + a[7] * b[4] + a[8] * b[5];
	dst[8] = a[6] * b[6] + a[7] * b[7] + a[8] * b[8];
}

void deltaAttMatrix(float C_prev[9], float C_now[9], float deltaC[9])
{
	//detaC = C_prev'* C;
	deltaC[0] = C_now[0] * C_prev[0] + C_now[3] * C_prev[3] + C_now[6] * C_prev[6];
	deltaC[1] = C_now[1] * C_prev[0] + C_now[4] * C_prev[3] + C_now[7] * C_prev[6];
	deltaC[2] = C_now[2] * C_prev[0] + C_now[5] * C_prev[3] + C_now[8] * C_prev[6];

	deltaC[3] = C_now[0] * C_prev[1] + C_now[3] * C_prev[4] + C_now[6] * C_prev[7];
	deltaC[4] = C_now[1] * C_prev[1] + C_now[4] * C_prev[4] + C_now[7] * C_prev[7];
	deltaC[5] = C_now[2] * C_prev[1] + C_now[5] * C_prev[4] + C_now[8] * C_prev[7];

	deltaC[6] = C_now[0] * C_prev[2] + C_now[3] * C_prev[5] + C_now[6] * C_prev[8];
	deltaC[7] = C_now[1] * C_prev[2] + C_now[4] * C_prev[5] + C_now[7] * C_prev[8];
	deltaC[8] = C_now[2] * C_prev[2] + C_now[5] * C_prev[5] + C_now[8] * C_prev[8];
}

void normVector(float a[3])
{
	float norm = 1.0f/sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
	
	a[0] *= norm;
	a[1] *= norm;
	a[2] *= norm;
}

void resetUKF(float *UKF_Q, float UKF_P[4][4], float *mag_prev, float *mag, float *UKF_C, float *C)
{
	memset(UKF_Q, 0, 4 * sizeof(float));

	UKF_Q[0] = 1.0f;

	memcpy(mag_prev, mag, 3 * sizeof(float));

	memcpy(UKF_C, C, 9 * sizeof(float));

	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 4; j++)
		{
			UKF_P[i][j] = 0.0f;
		}

	for (int i = 0; i < 4; i++)
	{
		UKF_P[i][i] = 0.0000001f;
	}
}

//利用陀螺仪的双极端盘判断是否在稳定的范围
int isStandCon(float gyr_extreme[6])
{
	if(gyr_extreme[1] - gyr_extreme[0] < 0.015f && gyr_extreme[3] - gyr_extreme[2] < 0.015f && gyr_extreme[5] - gyr_extreme[4] < 0.015f)
	{
		return 1;
	}
	
	return 0;
}


unsigned char footPDR(int num, float *gyr, float *acc, float *mag ,int press, int16_t* pos_res, int16_t* att, int16_t* zupt)
{
	unsigned char movement_e = 0;

	for (int i = 0; i < 3; i++)
	{
		accSum_xyz[i] += acc[i];
		magSum_xyz[i] += mag[i];
		
		gyr[i] *= (PI / 180);
		acc[i] *= g;
	}
	

	if (num_peak == 0)
	{
		for (int i = 0; i < 3; i++)
		{
			gyr_extreme[2 * i] = gyr[i];

			gyr_extreme[2 * i + 1] = gyr[i];
		}
	}



	for (int i = 0; i < 3; i++)
	{
		if (gyr[i] < gyr_extreme[2 * i])
		{
			gyr_extreme[2 * i] = gyr[i];
		}

		if (gyr[i] > gyr_extreme[2 * i + 1])
		{
			gyr_extreme[2 * i + 1] = gyr[i];
		}

	}

	accSum += sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);

	for (int i = 0; i < 3; i++)
	{
		gyr_mean[i] += gyr[i];
	}

	num_peak++;

	//在线估计陀螺仪的零偏， 6050的零偏偏大
	if (num_peak == 300)
	{
		if (isStandCon(gyr_extreme))
		{
			if(SAVEFLASH)
			{
					//识别每一次游戏模式下，静止状态的陀螺仪令零偏
					for(int i = 0; i < 3; i++)
					{
						gyrBias[i]     = gyr_mean[i] *  0.0033f;
					}
					
//					SAVEFLASH = 0;
			
//					memcpy(gyrFlash, gyrBias, 3 * sizeof(uint32_t));
//					
//					nrf_nvmc_page_erase(FLASH_ADD);
//					
//					nrf_nvmc_write_words(FLASH_ADD, gyrFlash , 3);
//					
			}	
			
//			//利用加速度及以及磁力计，计算磁航向，俯仰角以及翻滚角
//			float accSqrt = 1/sqrt(accSum_xyz[0] * accSum_xyz[0] + accSum_xyz[1] * accSum_xyz[1] + accSum_xyz[2] * accSum_xyz[2]);
//			float magSqrt = 1/sqrt(magSum_xyz[0] * magSum_xyz[0] + magSum_xyz[1] * magSum_xyz[1] + magSum_xyz[2] * magSum_xyz[2]);
//			
//			for(int i = 0; i < 3; i++)
//			{
//				accSum_xyz[i] *= accSqrt;
//				standMag[i]    = magSum_xyz[i] * magSqrt;
//			}

//			resetAttbyMag(C, accSum_xyz, standMag);
//			
//			resetUKF(UKF_Q, UKF_P, mag_prev, standMag, UKF_C, C);
//			
//			memset(P, 0, 81 * sizeof(float));
//			
//			canUKF = 1;
		}
		
		
		num_peak = 0;

		accSum = 0.0f;

		memset(gyr_mean, 0, 3 * sizeof(float));
		memset(accSum_xyz, 0, 3 * sizeof(float));
		memset(magSum_xyz, 0, 3 * sizeof(float));
	}

	gyr[0] -= gyrBias[0];
	gyr[1] -= gyrBias[1];
	gyr[2] -= gyrBias[2];

	float gyr_norm_xyz = sqrt(gyr[0] * gyr[0] + gyr[1] * gyr[1] + gyr[2] * gyr[2]);
	
	float mag_norm = sqrt(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);


	//需要一个滑动窗口来判断脚步是否在地上
	frame_index++;

	//下面为惯导解算
	if (num == 1 || frame_index < 0)
	{
		Initialize(gyr, acc);

		UKF_para(UKF_L, UKF_alpha, UKF_beta, UKF_kappa, &gamma, wm, wc);

		return movement_e;
	}

	//惯导解算: 姿态矩阵更新
	attitude_matrix_update(C, Temporary_array1, Temporary_array2, gyr, dt);

	//惯导解算: 将IMU坐标系的加速度转换到“导航坐标系”下
	multiply3x1(C, acc, acc_n);

	//惯导解算: 更新IMU速度
	vel_n[0] = vel_n[0] + acc_n[0] * dt;
	vel_n[1] = vel_n[1] + acc_n[1] * dt;
	vel_n[2] = vel_n[2] + (acc_n[2] - g) * dt;

	//惯导解算: 更新IMU位置
	pos_n[0] = pos_n[0] + vel_n[0] * dt;
	pos_n[1] = pos_n[1] + vel_n[1] * dt;
	pos_n[2] = pos_n[2] + vel_n[2] * dt;
	
	//ekf步骤: 状态协方差矩阵预测更新
	//P = F*P*F' + Q;
	State_covariance_matrix_update(P, acc_n, dt);

	int window_index = (frame_index - 1) % 10;

	gyr_norm_window[window_index] = gyr_norm_xyz;

	press_data[window_index] = press;

	//当press_trend函数返回是1，判断为踩地上
	// 返回2 的时候，判断为离地
	// 返回0 的时候，需要保持状态
	int press_trend_val = press_trend(frame_index, press_data, 10);

	if (press_trend_val == 1)
	{
		ZUPT_STATUS = 1;
	}
	else if (press_trend_val == 2)
	{
		ZUPT_STATUS = 2;
	}

	//RUN_ZUPT mean detect on floor when running

	int RUN_ZUPT = 0;
	if ((frame_index > 10 && ZUPT_STATUS == 1))
		RUN_ZUPT = 1;

	//STAND_ZUPT mean detect on floor when no any moving
	int STAND_ZUPT = 0;
	if ((frame_index > 15 && gyr_norm_window[window_index] < 0.35f && fabs(min_window_val(gyr_norm_window, 10) - max_window_val(gyr_norm_window, 10)) < 0.1f))
		STAND_ZUPT = 1;
	
	//zupt
	if ((STAND_ZUPT || RUN_ZUPT))
	{
		//ekf步骤: 计算卡尔曼滤波增益
		//K = P*H'/(H*P*H' + R);
		calKafmanGain9x4(K, P);

		//ekf步骤: 观测误差更新
		//delta_x = K * [vel_n(:,i);];
		float measure[4];
		measure[0] = 0.0f;
		measure[1] = vel_n[0];
		measure[2] = vel_n[1];
		measure[3] = vel_n[2];
		
		//如果进行磁力计的融合，根据acc,mag计算出当前的磁航向，再与C的航向比较，得出航向差
		//缺少磁干扰的时候判断。但是跳舞毯游戏需要一个朝向估计，暂时先不考虑强磁了
		//if(STAND_ZUPT && (fabs(mag_norm - 1.0f) < 0.5f))
		if(STAND_ZUPT)
		{
			normVector(mag);
			
			resetAttbyMag(C_prev, acc, mag);
			
			multiply3x3T(C_prev, C, deltaC);
			
			dcm2angle(deltaC, &EKF_roll, &EKF_pitch, &EKF_yaw);
			
			measure[0] = -EKF_yaw;
			
		}
	
		
		calDeltaX9x4(K, measure, delta_x);

		//ekf步骤: 状态协方差矩阵观测更新
		calStateCov9x4(P, K);

		//这里先从设置 delta_x(3) = atan2(C(2,1),C(1,1));
		//意味着每一步的参考方向是IMU X轴方向
//		delta_x[2] = atan2(C[3], C[0]);
//		theta = -1.7801 + atan2(C[3], C[0]);
//		theta = 0.0f;

		//delta_x[2] = 0.0f;

		//修正姿态矩阵
		Att_matrix_corr(C, C_prev, Temporary_array1, Temporary_array2, delta_x);

		//修正位置
		pos_n_corr(pos_n, delta_x);

		//修正速度
		vel_n_corr(vel_n, delta_x);
		
		memset(vel_n, 0, 3 * sizeof(float));

		last_pos_n[0] = pos_n[0];
		last_pos_n[1] = pos_n[1];
		last_pos_n[2] = pos_n[2];

		last_stage = 1;

		*zupt = 1;
	}
	else
	{
		stand_num = 0;
		*zupt = 0;
	}

//融合磁力计数据
	
//	if(canUKF && STAND_ZUPT)
//	{
//		resetAttbyMag(C_prev, acc, mag, g);
//		
//		multiply3x3T(C_prev, C, deltaC);
//		
//		dcm2angle(deltaC, &EKF_roll, &EKF_pitch, &EKF_yaw);
//		
//		float angleErr[3];

//		angleErr[0] = -EKF_roll;
//		angleErr[1] = -EKF_pitch;
//		angleErr[2] = -EKF_yaw;
//		
//		Kalfman_gain_angle(P, Temporary_array1, Temporary_array2, K);

//		State_covariance_matrix_corr_angle(P, P_prev, K);
//		
//		multiply9x3(K, angleErr, delta_x);

//		Att_matrix_corr(C, C_prev, Temporary_array1, Temporary_array2, delta_x);

//		//修正位置
//		pos_n_corr(pos_n, delta_x);

//		//修正速度
//		vel_n_corr(vel_n, delta_x);@
//		  
//	}
	

	
	//状态协方差矩阵保持正交性，以防出现退化
	State_covariance_matrix_orthogonalization(P);

	memcpy(last_vel_n, vel_n, 3 * sizeof(float));


	pos_offset[0] = pos_n[0] - last_pos_n[0];
	pos_offset[1] = pos_n[1] - last_pos_n[1];
	pos_offset[2] = pos_n[2] - last_pos_n[2];
//	pos_offset[0] = gyr[0]*1000.0f;
//	pos_offset[1] = gyr[1]*1000.0f;
//	pos_offset[2] = gyr[2]*1000.0f;
	
	dcm2angleTest(C, att); //航向角，俯仰角， 翻滚角（z y x）
	
	if(PCB_VERSION == 1 || PCB_VERSION == 2)
	{
		pos_res[0] = (short) (pos_offset[1] * 100.0f);
		pos_res[1] = (short) (-pos_offset[0] * 100.0f);
	}
	else if(PCB_VERSION == 0)
	{
		pos_res[0] = (short) (pos_offset[0] * 100.0f);
		pos_res[1] = (short) (pos_offset[1] * 100.0f);
	}
	else
	{
		pos_res[0] = (short) (pos_offset[0] * 100.0f);
		pos_res[1] = (short) (pos_offset[1] * 100.0f);
	}
	
	pos_res[2] = (short) (pos_offset[2] * 100.0f);


	if ((pos_res[0] * pos_res[0] + pos_res[1] * pos_res[1] + pos_res[2] * pos_res[2]) > 10)
	{
		last_stage = 2;
	}

	return movement_e;
}