#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 acc_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]; float mag_buff[3][20]; float heading_buff[20]; float zupt_gyr_norm = 99999; float zupt_mag[3]; float zupt_heading; int step_index = 0; void saveStepData(int index, float heading, float mag[3]) { for(int i = 0; i < 3; i++) { mag_buff[i][index % 20] = mag[i]; } heading_buff[index % 20] = heading; } float calDeltaHeading(int index, float now_heading, float now_mag[3]) { //寻找相似的方向 int start_index = index - 20; if(start_index < 0) start_index = 0; for(int i = start_index; i < index; i++) { if((fabsf(mag_buff[0][i % 20] - now_mag[0]) < 0.12f) && (fabsf(mag_buff[1][i % 20] - now_mag[1]) < 0.12f) && (fabsf(mag_buff[2][i % 20] - now_mag[2]) < 0.12f)) { float deltaHeading = now_heading - heading_buff[i % 20]; if(deltaHeading < -3.141591f) { return deltaHeading + 6.2831852f; } else if(deltaHeading > 3.141591f) { return (deltaHeading - 6.2831852f); } else { return deltaHeading; } } } return 99; } 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, int32_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); // gpio_mt_run(500); // } // //利用加速度及以及磁力计，计算磁航向，俯仰角以及翻滚角 // 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 acc_norm_xyz = sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[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; acc_norm_window[window_index] = acc_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 > 10 && gyr_norm_window[window_index] < 0.55f && fabsf(min_window_val(gyr_norm_window, 10) - max_window_val(gyr_norm_window, 10)) < 0.1f)) // STAND_ZUPT = 1; int STAND_ZUPT = 0; if (frame_index > 10 && fabsf(min_window_val(acc_norm_window, 10) - g) < 0.1*g && fabsf(max_window_val(acc_norm_window, 10) - g) < 0.1*g) STAND_ZUPT = 1; //zupt if ((STAND_ZUPT || (RUN_ZUPT && press > 0))) { //ekf步骤: 计算卡尔曼滤波增益 //K = P*H'/(H*P*H' + R); calKafmanGain9x4(K, P); //ekf步骤: 观测误差更新 //delta_x = K * [vel_n(:,i);]; float now_heading = atan2(C[3], C[0]); float measure[4]; measure[0] = calDeltaHeading(step_index, now_heading, mag); if(measure[0] > 10) { measure[0] = 0; } 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; //修正姿态矩阵 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); 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; // if((press > 12000000 && STAND_ZUPT) || RUN_ZUPT ) //// if((ZUPT_STATUS == 1 && STAND_ZUPT) || RUN_ZUPT ) // *zupt = 1; // else // *zupt = 0; //存放gyr_norm最小的mag数据以及heading数据 if(gyr_norm_xyz < zupt_gyr_norm) { zupt_gyr_norm = gyr_norm_xyz; memcpy(zupt_mag, mag, 3 * sizeof(float)); zupt_heading = now_heading; } stand_num ++; } else { //存放一步内记录的mag数据以及heading数据 if(stand_num != 0) { zupt_gyr_norm = 99999; saveStepData(step_index, zupt_heading, zupt_mag); step_index ++; } 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]; pos_offset[1] = pos_n[1]; pos_offset[2] = pos_n[2]; dcm2angleTest(C, att); //航向角，俯仰角， 翻滚角（z y x） if(DIRECTION_SH == BLACK_SH) { pos_res[0] = (int32_t) (-pos_offset[1] * 100.0f); pos_res[1] = (int32_t) (pos_offset[0] * 100.0f); } else if(DIRECTION_SH == BLUE_SH) { pos_res[0] = (int32_t) (pos_offset[1] * 100.0f); pos_res[1] = (int32_t) (-pos_offset[0] * 100.0f); } else if(DIRECTION_SH == WHITE_SH) { pos_res[0] = (int32_t) (pos_offset[0] * 100.0f); pos_res[1] = (int32_t) (pos_offset[1] * 100.0f); } else if(DIRECTION_SH == GIRL_SH) { pos_res[0] = (int32_t) (pos_offset[0] * 100.0f); pos_res[1] = (int32_t) (pos_offset[1] * 100.0f); } else { pos_res[0] = (int32_t) (pos_offset[0] * 100.0f); pos_res[1] = (int32_t) (pos_offset[1] * 100.0f); } pos_res[2] = (int32_t) (pos_offset[2] * 100.0f); return movement_e; }