#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>

#define ZUPT_threshold  0.81
#define SIGMA  0.01
#define SIGMA_V  0.01

#define PI 3.1416

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

float dt = 0.01;

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 last_num = 0;

int frame_index = 0;

int last_zupt_index = 30;

int output_signal = 0;

int stand_num = 0;

int last_zupt = 0;

float gyr_norm_window[15];

float max_window_val;

float min_window_val;

int last_running = 1;

void invert3x3(float * src, float * dst)
{
	float det;

	/* Compute adjoint: */
	dst[0] = +src[4] * src[8] - src[5] * src[7];
	dst[1] = -src[1] * src[8] + src[2] * src[7];
	dst[2] = +src[1] * src[5] - src[2] * src[4];
	dst[3] = -src[3] * src[8] + src[5] * src[6];
	dst[4] = +src[0] * src[8] - src[2] * src[6];
	dst[5] = -src[0] * src[5] + src[2] * src[3];
	dst[6] = +src[3] * src[7] - src[4] * src[6];
	dst[7] = -src[0] * src[7] + src[1] * src[6];
	dst[8] = +src[0] * src[4] - src[1] * src[3];

	/* Compute determinant: */
	det = src[0] * dst[0] + src[1] * dst[3] + src[2] * dst[6];

	/* Multiply adjoint with reciprocal of determinant: */
	det = 1.0f / det;

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

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

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

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

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

void init_attitude_matrix(float *C, float *acc)
{
	float pitch = asin(-acc[0] / g);

	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);

	C[0] = cos(pitch);
	C[1] = pitch_sin * roll_sin;
	C[2] = pitch_sin * roll_cos;

	C[4] = roll_cos;
	C[5] = -roll_sin;

	C[6] = -pitch_sin;
	C[7] = pitch_cos * roll_sin;
	C[8] = pitch_cos * roll_cos;
}

void reset_yaw_C(float *C)
{
	float pitch = asin(-C[6]);

	float roll = atan2(C[7], C[8]);

	float pitch_sin = sin(pitch);

	float pitch_cos = cos(pitch);

	float roll_sin = sin(roll);

	float roll_cos = cos(roll);

	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;
}

// F * P * F'

void State_covariance_matrix_update(float *P, float *acc_n)
{
	//  P2 + P3*dt,   
	P[3] = P[3] + P[6] * dt;
	P[4] = P[4] + P[7] * dt;
	P[5] = P[5] + P[8] * dt;
	P[12] = P[12] + P[15] * dt;
	P[13] = P[13] + P[16] * dt;
	P[14] = P[14] + P[17] * dt;
	P[21] = P[21] + P[24] * dt;
	P[22] = P[22] + P[25] * dt;
	P[23] = P[23] + P[26] * dt;

	//P4 + P7*dt,
	P[27] = P[27] + P[54] * dt;
	P[28] = P[28] + P[55] * dt;
	P[29] = P[29] + P[56] * dt;
	P[36] = P[36] + P[63] * dt;
	P[37] = P[37] + P[64] * dt;
	P[38] = P[38] + P[65] * dt;
	P[45] = P[45] + P[72] * dt;
	P[46] = P[46] + P[73] * dt;
	P[47] = P[47] + P[74] * dt;

	// P5 + P8*dt + dt*(P6 + P9*dt)
	P[30] = P[30] + P[57] * dt + dt * (P[33] + P[60] * dt);
	P[31] = P[31] + P[58] * dt + dt * (P[34] + P[61] * dt);
	P[32] = P[32] + P[59] * dt + dt * (P[35] + P[62] * dt);
	P[39] = P[39] + P[66] * dt + dt * (P[42] + P[69] * dt);
	P[40] = P[40] + P[67] * dt + dt * (P[43] + P[70] * dt);
	P[41] = P[41] + P[68] * dt + dt * (P[44] + P[71] * dt);
	P[48] = P[48] + P[75] * dt + dt * (P[51] + P[78] * dt);
	P[49] = P[49] + P[76] * dt + dt * (P[52] + P[79] * dt);
	P[50] = P[50] + P[77] * dt + dt * (P[53] + P[80] * dt);

	//P6 + P9*dt + matr*(P4 + P7*dt)
	P[33] = P[33] + P[60] * dt + acc_n[2] * dt * P[28] - acc_n[1] * dt * P[29];
	P[34] = P[34] + P[61] * dt - acc_n[2] * dt * P[27] + acc_n[0] * dt * P[29];
	P[35] = P[35] + P[62] * dt + acc_n[1] * dt * P[27] - acc_n[0] * dt * P[28];

	P[42] = P[42] + P[69] * dt + acc_n[2] * dt * P[37] - acc_n[1] * dt * P[38];
	P[43] = P[43] + P[70] * dt - acc_n[2] * dt * P[36] + acc_n[0] * dt * P[38];
	P[44] = P[44] + P[71] * dt + acc_n[1] * dt * P[36] - acc_n[0] * dt * P[37];

	P[51] = P[51] + P[78] * dt + acc_n[2] * dt * P[46] - acc_n[1] * dt * P[47];
	P[52] = P[52] + P[79] * dt - acc_n[2] * dt * P[45] + acc_n[0] * dt * P[47];
	P[53] = P[53] + P[80] * dt + acc_n[1] * dt * P[45] - acc_n[0] * dt * P[46];

	//P7 + P1*matr
	P[54] = P[54] + P[9] * acc_n[2] * dt - P[18] * acc_n[1] * dt;
	P[55] = P[55] + P[10] * acc_n[2] * dt - P[19] * acc_n[1] * dt;
	P[56] = P[56] + P[11] * acc_n[2] * dt - P[20] * acc_n[1] * dt;

	P[63] = P[63] - P[0] * acc_n[2] * dt + P[18] * acc_n[0] * dt;
	P[64] = P[64] - P[1] * acc_n[2] * dt + P[19] * acc_n[0] * dt;
	P[65] = P[65] - P[2] * acc_n[2] * dt + P[20] * acc_n[0] * dt;

	P[72] = P[72] + P[0] * acc_n[1] * dt - P[9] * acc_n[0] * dt;
	P[73] = P[73] + P[1] * acc_n[1] * dt - P[10] * acc_n[0] * dt;
	P[74] = P[74] + P[2] * acc_n[1] * dt - P[11] * acc_n[0] * dt;

	//P8 + P2*matr + dt*(P9 + P3*matr),
	P[57] = P[57] + dt * P[60] + P[12] * acc_n[2] * dt - P[21] * acc_n[1] * dt;
	P[58] = P[58] + dt * P[61] + P[13] * acc_n[2] * dt - P[22] * acc_n[1] * dt;
	P[59] = P[59] + dt * P[62] + P[14] * acc_n[2] * dt - P[23] * acc_n[1] * dt;

	P[66] = P[66] + dt * P[69] - P[3] * acc_n[2] * dt + P[21] * acc_n[0] * dt;
	P[67] = P[67] + dt * P[70] - P[4] * acc_n[2] * dt + P[22] * acc_n[0] * dt;
	P[68] = P[68] + dt * P[71] - P[5] * acc_n[2] * dt + P[23] * acc_n[0] * dt;

	P[75] = P[75] + dt * P[78] + P[3] * acc_n[1] * dt - P[12] * acc_n[0] * dt;
	P[76] = P[76] + dt * P[79] + P[4] * acc_n[1] * dt - P[13] * acc_n[0] * dt;
	P[77] = P[77] + dt * P[80] + P[5] * acc_n[1] * dt - P[14] * acc_n[0] * dt;

	// P9 + P3*matr + matr*(P7 + P1*matr)
	P[60] = P[60] + P[15] * acc_n[2] * dt - P[24] * acc_n[1] * dt + acc_n[2] * dt * P[55] - acc_n[1] * dt * P[56];
	P[61] = P[61] + P[16] * acc_n[2] * dt - P[25] * acc_n[1] * dt - acc_n[2] * dt * P[54] + acc_n[0] * dt * P[56];
	P[62] = P[62] + P[17] * acc_n[2] * dt - P[26] * acc_n[1] * dt + acc_n[1] * dt * P[54] - acc_n[0] * dt * P[55];

	P[69] = P[69] - P[6] * acc_n[2] * dt + P[24] * acc_n[0] * dt + acc_n[2] * dt * P[64] - acc_n[1] * dt * P[65];
	P[70] = P[70] - P[7] * acc_n[2] * dt + P[25] * acc_n[0] * dt - acc_n[2] * dt * P[63] + acc_n[0] * dt * P[65];
	P[71] = P[71] - P[8] * acc_n[2] * dt + P[26] * acc_n[0] * dt + acc_n[1] * dt * P[63] - acc_n[0] * dt * P[64];

	P[78] = P[78] + P[6] * acc_n[1] * dt - P[15] * acc_n[0] * dt + acc_n[2] * dt * P[73] - acc_n[1] * dt * P[74];
	P[79] = P[79] + P[7] * acc_n[1] * dt - P[16] * acc_n[0] * dt - acc_n[2] * dt * P[72] + acc_n[0] * dt * P[74];
	P[80] = P[80] + P[8] * acc_n[1] * dt - P[17] * acc_n[0] * dt + acc_n[1] * dt * P[72] - acc_n[0] * dt * P[73];

	//P3 + P1 * matr
	P[6] = P[6] + P[1] * acc_n[2] * dt - P[2] * acc_n[1] * dt;
	P[7] = P[7] - P[0] * acc_n[2] * dt + P[2] * acc_n[0] * dt;
	P[8] = P[8] + P[0] * acc_n[1] * dt - P[1] * acc_n[0] * dt;

	P[15] = P[15] + P[10] * acc_n[2] * dt - P[11] * acc_n[1] * dt;
	P[16] = P[16] - P[9] * acc_n[2] * dt + P[11] * acc_n[0] * dt;
	P[17] = P[17] + P[9] * acc_n[1] * dt - P[10] * acc_n[0] * dt;

	P[24] = P[24] + P[19] * acc_n[2] * dt - P[20] * acc_n[1] * dt;
	P[25] = P[25] - P[18] * acc_n[2] * dt + P[20] * acc_n[0] * dt;
	P[26] = P[26] + P[18] * acc_n[1] * dt - P[19] * acc_n[0] * dt;

	float noise = SIGMA * SIGMA * dt *dt;

	for (int i = 0; i < 9; i++)
	{
		P[i * 9 + i] += noise;
	}
}

void Kalfman_gain(float *P, float *Temporary_array, float *Temporary_array1, float *K)
{
	Temporary_array[0] = P[60] + SIGMA_V * SIGMA_V;
	Temporary_array[1] = P[61];
	Temporary_array[2] = P[62];

	Temporary_array[3] = P[69];
	Temporary_array[4] = P[70] + SIGMA_V * SIGMA_V;
	Temporary_array[5] = P[71];

	Temporary_array[6] = P[78];
	Temporary_array[7] = P[79];
	Temporary_array[8] = P[80] + SIGMA_V * SIGMA_V * 0.01;

	invert3x3(Temporary_array, Temporary_array1);

	memcpy(Temporary_array, Temporary_array1, 9 * sizeof(float));

	K[0] = P[6] * Temporary_array[0] + P[7] * Temporary_array[3] + P[8] * Temporary_array[6];
	K[1] = P[6] * Temporary_array[1] + P[7] * Temporary_array[4] + P[8] * Temporary_array[7];
	K[2] = P[6] * Temporary_array[2] + P[7] * Temporary_array[5] + P[8] * Temporary_array[8];

	K[3] = P[15] * Temporary_array[0] + P[16] * Temporary_array[3] + P[17] * Temporary_array[6];
	K[4] = P[15] * Temporary_array[1] + P[16] * Temporary_array[4] + P[17] * Temporary_array[7];
	K[5] = P[15] * Temporary_array[2] + P[16] * Temporary_array[5] + P[17] * Temporary_array[8];

	K[6] = P[24] * Temporary_array[0] + P[25] * Temporary_array[3] + P[26] * Temporary_array[6];
	K[7] = P[24] * Temporary_array[1] + P[25] * Temporary_array[4] + P[26] * Temporary_array[7];
	K[8] = P[24] * Temporary_array[2] + P[25] * Temporary_array[5] + P[26] * Temporary_array[8];

	K[9] = P[33] * Temporary_array[0] + P[34] * Temporary_array[3] + P[35] * Temporary_array[6];
	K[10] = P[33] * Temporary_array[1] + P[34] * Temporary_array[4] + P[35] * Temporary_array[7];
	K[11] = P[33] * Temporary_array[2] + P[34] * Temporary_array[5] + P[35] * Temporary_array[8];

	K[12] = P[42] * Temporary_array[0] + P[43] * Temporary_array[3] + P[44] * Temporary_array[6];
	K[13] = P[42] * Temporary_array[1] + P[43] * Temporary_array[4] + P[44] * Temporary_array[7];
	K[14] = P[42] * Temporary_array[2] + P[43] * Temporary_array[5] + P[44] * Temporary_array[8];

	K[15] = P[51] * Temporary_array[0] + P[52] * Temporary_array[3] + P[53] * Temporary_array[6];
	K[16] = P[51] * Temporary_array[1] + P[52] * Temporary_array[4] + P[53] * Temporary_array[7];
	K[17] = P[51] * Temporary_array[2] + P[52] * Temporary_array[5] + P[53] * Temporary_array[8];

	K[18] = P[60] * Temporary_array[0] + P[61] * Temporary_array[3] + P[62] * Temporary_array[6];
	K[19] = P[60] * Temporary_array[1] + P[61] * Temporary_array[4] + P[62] * Temporary_array[7];
	K[20] = P[60] * Temporary_array[2] + P[61] * Temporary_array[5] + P[62] * Temporary_array[8];

	K[21] = P[69] * Temporary_array[0] + P[70] * Temporary_array[3] + P[71] * Temporary_array[6];
	K[22] = P[69] * Temporary_array[1] + P[70] * Temporary_array[4] + P[71] * Temporary_array[7];
	K[23] = P[69] * Temporary_array[2] + P[70] * Temporary_array[5] + P[71] * Temporary_array[8];

	K[24] = P[78] * Temporary_array[0] + P[79] * Temporary_array[3] + P[80] * Temporary_array[6];
	K[25] = P[78] * Temporary_array[1] + P[79] * Temporary_array[4] + P[80] * Temporary_array[7];
	K[26] = P[78] * Temporary_array[2] + P[79] * Temporary_array[5] + P[80] * Temporary_array[8];
}

void multiply9x3(float *K, float *vel_n, float* delta_x)
{
	int i = 0;
	for (i = 0; i < 9; i++)
	{
		delta_x[i] = K[i * 3] * vel_n[0] + K[i * 3 + 1] * vel_n[1] + K[i * 3 + 2] * vel_n[2];
	}
}

void State_covariance_matrix_corr(float *P, float *P_tmp, float *K)
{
	int i = 0;
	int j = 0;

	for (i = 0; i < 9; i++) {
		for (j = 0; j < 9; j++) {
			P_tmp[i * 9 + j] = P[i * 9 + j] - K[3 * i] * P[54 + j] - K[3 * i + 1] * P[63 + j] - K[3 * i + 2] * P[72 + j];
		}
	}
	memcpy(P, P_tmp, 81 * sizeof(float));
}

void Att_matrix_corr(float *C, float *C_prev, float *Temporary_array, float *Temporary_array1, float *delta_x)
{
	Temporary_array[0] = 2.0;
	Temporary_array[1] = -delta_x[2];
	Temporary_array[2] = delta_x[1];

	Temporary_array[3] = delta_x[2];
	Temporary_array[4] = 2.0;
	Temporary_array[5] = -delta_x[0];

	Temporary_array[6] = -delta_x[1];
	Temporary_array[7] = delta_x[0];
	Temporary_array[8] = 2.0;

	invert3x3(Temporary_array, Temporary_array1);

	Temporary_array[0] = 2.0;
	Temporary_array[1] = delta_x[2];
	Temporary_array[2] = -delta_x[1];

	Temporary_array[3] = -delta_x[2];
	Temporary_array[4] = 2.0;
	Temporary_array[5] = delta_x[0];

	Temporary_array[6] = delta_x[1];
	Temporary_array[7] = -delta_x[0];
	Temporary_array[8] = 2.0;

	multiply3x3(Temporary_array, Temporary_array1, C_prev);

	memcpy(Temporary_array, C_prev, 9 * sizeof(float));

	multiply3x3(Temporary_array, C, C_prev);

	memcpy(C, C_prev, 9 * sizeof(float));
}

void pos_n_corr(float *pos_n, float *delta_x)
{
	pos_n[0] -= delta_x[3];

	pos_n[1] -= delta_x[4];

	pos_n[2] -= delta_x[5];
}

void vel_n_corr(float *vel_n, float *delta_x)
{
	vel_n[0] -= delta_x[6];

	vel_n[1] -= delta_x[7];

	vel_n[2] -= delta_x[8];
}

void State_covariance_matrix_orthogonalization(float *P)
{
	int i = 0;

	int j = 0;

	float temp;

	for (i = 0; i < 9; i++)
		for (j = i + 1; j < 9; j++)
		{
			temp = 0.5f*(P[i * 9 + j] + P[j * 9 + i]);

			P[i * 9 + j] = temp;

			P[j * 9 + i] = temp;
		}
}

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

	last_num = 0;

	last_zupt_index = 30;

	output_signal = 0;

	stand_num = 0;

	last_zupt = 0;

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

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

	memset(gyr_norm_window, 0, 15 * 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));

	init_attitude_matrix(C, acc);

	memcpy(C_prev, C, 9 * sizeof(float));
}

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

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

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

	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(float *window)
{
	float val = -1.0;

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

	return val;
}

float min_window(float *window)
{
	float val = 1000.0;

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

	return val;
}

unsigned char footPDR(int num, float *gyr, float *acc, float *vel, float *pos)
{
	unsigned char movement_e = 0;

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

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

	//下面为惯导解算
	if (num == 1)
	{
		Initialize(gyr, acc);
		return movement_e;
	}

	attitude_matrix_update(C, Temporary_array1, Temporary_array2, gyr);

	multiply3x1(C, acc, acc_n);

	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;

	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;

	//P = F*P*F' + Q;
	State_covariance_matrix_update(P, acc_n);

	int window_index = (frame_index - 1) % 15;

	gyr_norm_window[window_index] = sqrt(gyr[0] * gyr[0] + gyr[1] * gyr[1] + gyr[2] * gyr[2]);

	if (frame_index > 15)
	{
		max_window_val = max_window(gyr_norm_window);
		min_window_val = min_window(gyr_norm_window);
	}

	float sub_window = max_window_val - min_window_val;

	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];

	//临时值2.0f，后期需要优化
	if (((gyr_norm_window[window_index] < 0.6f) && (frame_index > 15) && (sub_window < 0.6f))
		|| ((gyr_norm_window[window_index] < 2.0f) && ((frame_index - last_zupt_index) > 25) && (frame_index > 15) && (sub_window > 6.0f))
		|| ((gyr_norm_window[window_index] < 2.0f) && ((frame_index - last_zupt_index) > 50) && (frame_index > 15) && (sub_window > 5.0f)))//这里往后再优化
	{
		if (last_zupt == 1)
		{
			//输出信号
			output_signal = 1;

			stand_num = stand_num + 1;

			//K = P*H'/(H*P*H' + R);

			Kalfman_gain(P, Temporary_array1, Temporary_array2, K);

			//delta_x = K * [vel_n(:,i);];
			multiply9x3(K, vel_n, delta_x);

			State_covariance_matrix_corr(P, P_prev, K);

			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);

			reset_yaw_C(C);

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

		last_zupt = 1;

		last_running = frame_index;
	}
	else
	{
		if (last_zupt == 1)
		{
			last_zupt = 0;

			last_zupt_index = frame_index - 1;
		}

		//最先判断左右， 再判断前后，最后再判断起跳，都不符合视作为跑步
		if (output_signal == 1 && frame_index - last_zupt_index > 5)	//&& pos_offset[2] < -0.001
		{
			if (pos_offset[1] < -0.25f)
			{
				movement_e = 4;
				output_signal = 0;
			}
			else if (pos_offset[1] > 0.25f)
			{
				movement_e = 5;
				output_signal = 0;
			}
			else if (pos_offset[0] < -0.25f)
			{
				movement_e = 8;
				output_signal = 0;
			}
			else if (pos_offset[2] < -0.25f)
			{
				movement_e = 7;
				output_signal = 0;
			}
			else if (pos_offset[0] > 0.25f)
			{
				movement_e = 6;
				output_signal = 0;
			}
			else if (frame_index - last_running > 30)
			{
				movement_e = 6;
				last_running = frame_index;
			}
		}
	}

	State_covariance_matrix_orthogonalization(P);

	if (output_signal == 1 && stand_num > 100)
	{
		output_signal = 0;

		movement_e = 1;

		stand_num = 0;
	}

	memcpy(vel, vel_n, 3 * sizeof(float));
//	memcpy(pos, pos_n, 3 * sizeof(float));
	memcpy(pos, pos_offset, 3 * sizeof(float));

	return movement_e;
}

//int main()
//{
//	FILE *fp = NULL;

//	FILE *out = NULL;

//	char *line, *record;

//	char buffer[1024];

//	int num = 0;
//	float *gyr = (float *)malloc(3 * sizeof(float));
//	float *acc = (float *)malloc(3 * sizeof(float));

//	out = fopen("F:\\work\\result.txt", "a+");

//	if ((fp = fopen("F:\\work\\LoggedData2_CalInertialAndMag.csv", "r")) != NULL)

//	{

//		fseek(fp, 170L, SEEK_SET);  //定位到第二行，每个英文字符大小为1

//		char delims[] = ",";

//		char *result = NULL;

//		int j = 0;

//		while ((line = fgets(buffer, sizeof(buffer), fp)) != NULL)//当没有读取到文件末尾时循环继续

//		{

//			record = strtok(line, ",");

//			while (record != NULL)//读取每一行的数据

//			{

//				if (strcmp(record, "Ps:") == 0)//当读取到Ps那一行时，不再继续读取

//					return 0;

//				//printf("%s ", record);//将读取到的每一个数据打印出来
//			

//				switch (j) {

//					case 1:
//						gyr[0] = atof(record);
//						break;
//					case 2:
//						gyr[1] = atof(record);
//						break;
//					case 3:
//						gyr[2] = atof(record);
//						break;
//					case 4:
//						acc[0] = atof(record);
//						break;
//					case 5:
//						acc[1] = atof(record);
//						break;
//					case 6:
//						acc[2] = atof(record);
//						break;
//					default:
//						break;
//				}
//				

//				if (j == 6)  //只需读取前9列

//					break;

//				record = strtok(NULL, ",");

//				j++;

//			}



//			if (num > 0)
//			{
//				//printf("%6.6f  %6.6f  %6.6f  \n", gyr[0], gyr[1], gyr[2]);
//				unsigned char movement_e = footPDR(num, gyr, acc, vel_n, pos_n);

//				for (int i = 0; i < 3; i++)
//				{
//					fprintf(out, "%f ", pos_n[i]);
//				}
//				fprintf(out, "\n ");
//				
//				switch (movement_e) {
//				case 7:
//					printf("MOVE_JUMP\n");
//					break;			
//				}
//				
//			}

//			num++;
//			j = 0;

//			
//		}

//		fclose(fp);

//		fp = NULL;

//	}

//	/*
//	float *gyr = (float *)malloc(3 * sizeof(float));
//	float *acc = (float *)malloc(3 * sizeof(float));

//	int num;
//	float gyr_x, gyr_y, gyr_z, acc_x, acc_y, acc_z, mag_x, mag_y, mag_z;
//	while (fscanf(file, "%d%f%f%f%f%f%f%f%f%f", &num, &gyr_x, &gyr_y, &gyr_z, &acc_x, &acc_y, &acc_z, &mag_x, &mag_y, &mag_z)) {
//		printf("%d %f %f %f %f %f %f %f %f %f", gyr_x, gyr_y, gyr_z, acc_x, acc_y, acc_z, mag_x, mag_y, mag_z);
//	
//	}
//	fclose(file);
//	
//	return 0;
//	*/

//}


/*
int main()
{
	float acc_x = 0.0;
	float acc_y = 0.1;
	float acc_z = 9.75;
	float acc[] = { acc_x , acc_y, acc_z };

	float gyr_x = 0.01;
	float gyr_z = 0.02;
	float gyr_y = 0.03;
	float gyr[] = { gyr_x , gyr_y, gyr_z };


	P = (float *)malloc(81*sizeof(float));

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

	P_prev = (float *)malloc(81*sizeof(float));

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

	C = (float *)malloc(9);



	float* C_inv = (float *) malloc(9 * sizeof(float));

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

	acc_n = (float *) malloc(3 * sizeof(float));

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

	vel_n = (float *) malloc(3 * sizeof(float));

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

	pos_n = (float *) malloc(3  * sizeof(float));

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

	K = (float *)malloc(27 * sizeof(float));

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

	Temporary_array1 = (float *)malloc(9 * sizeof(float));

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

	Temporary_array2 = (float *)malloc(9 * sizeof(float));

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

	delta_x = (float *)malloc(9 * sizeof(float));

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

	C_inv[0] = 2.0;
	C_inv[1] = dt*gyr_z;
	C_inv[2] = -dt*gyr_y;

	C_inv[3] = -dt*gyr_z;
	C_inv[4] = 2.0;
	C_inv[5] = dt*gyr_x;

	C_inv[6] = dt*gyr_y;
	C_inv[7] = -dt*gyr_x;
	C_inv[9] = 2.0;


	float* C_res =(float *) malloc(9 * sizeof(float));

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

	invert3x3(C_inv, C_res);

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

	C_inv[0] = 2 * C[0] + C[1] * dt * gyr_z - C[2] * dt * gyr_y;
	C_inv[1] = 2 * C[1] - C[0] * dt * gyr_z + C[2] * dt * gyr_x;
	C_inv[2] = 2 * C[2] + C[0] * dt * gyr_y - C[1] * dt * gyr_x;

	C_inv[3] = 2 * C[3] + C[4] * dt * gyr_z - C[5] * dt * gyr_y;
	C_inv[4] = 2 * C[4] - C[3] * dt * gyr_z + C[5] * dt * gyr_x;
	C_inv[5] = 2 * C[5] + C[3] * dt * gyr_y - C[4] * dt * gyr_x;

	C_inv[6] = 2 * C[6] + C[7] * dt * gyr_z - C[8] * dt * gyr_y;
	C_inv[7] = 2 * C[7] - C[6] * dt * gyr_z + C[8] * dt * gyr_x;
	C_inv[8] = 2 * C[8] + C[6] * dt * gyr_y - C[7] * dt * gyr_x;

	multiply3x3(C_inv, C_res, C);

	//C*acc

	multiply3x1(C, acc, acc_n);

	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;


	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;


	float P_copy[] = { 0.0095, -0.0000 , 0.0020, 0.0003, -0.0015, 0.0000 , 0.0002 , - 0.0401, - 0.0011,
		-0.0000 ,0.0094 ,-0.0002, 0.0020 ,-0.0000, -0.0005 ,  0.0410, -0.0001 ,-0.0051
	,0.0020 , -0.0002 ,   0.0830   , 0.0115  ,  0.0238  ,  0.0006    ,0.0077   , 0.0420  ,  0.0000
	,0.0003 ,   0.0020 ,   0.0115 ,   0.6790   , 0.0047  ,  0.0009 ,   0.1350 ,   0.0059 , -0.0015
  , -0.0015 , -0.0000    ,0.0238   , 0.0047   , 0.6851   , 0.0012   , 0.0023   , 0.1462   , 0.0006
	,0.0000 , -0.0005   , 0.0006   , 0.0009   , 0.0012   , 0.5172    ,-0.0027 ,-0.0004  ,  0.0850
	,0.0002   , 0.0410  ,  0.0077 ,   0.1350  ,  0.0023,  -0.0027  ,  0.3347 ,   0.0040, -0.0270
  , -0.0401, -0.0001 ,   0.0420 ,   0.0059 ,   0.1462 , -0.0004 ,   0.0040  ,  0.3562  ,  0.0046
  , -0.0011 ,-0.0051 ,   0.0000, -0.0015  ,  0.0006 ,   0.0850 ,-0.0270  ,  0.0046  ,  0.0384 };

	for (int i = 0; i < 81; i++)
	{
		if (i % 9 == 0)
			printf("\n");
		printf("%6.8f ", P_copy[i]);

		P_copy[i] /= 10000;
	}
	printf("\n");

	Kalfman_gain(P_copy, Temporary_array1, Temporary_array2, K);

	for (int i = 0; i < 27; i++)
	{
		if (i % 3 == 0)
			printf("\n");
		printf("%6.4f ", K[i]);
	}

	//inv

	return 0;
}
*/