footPDR.c 22 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929
  1. #include "footPDR.h"
  2. //当地的重力加速度
  3. float g = 9.8179995f;
  4. float dt = 0.01f;
  5. float P[81], acc_n[3];
  6. float Temporary_array1[9], Temporary_array2[9];
  7. float K[27], P_prev[81], delta_x[9];
  8. float C[9], C_prev[9];
  9. float vel_n[3], pos_n[3];
  10. float last_pos_n[3];
  11. float pos_offset[3];
  12. uint32_t frame_index = 0;
  13. int stand_num = 0;
  14. float acc_norm_window[20];
  15. float gyr_extreme[6];
  16. float gyr_mean[3];
  17. int num_peak;
  18. //float acc_mean[3];
  19. float gyrBias[3];
  20. //float last_gyr[3];
  21. //float last_acc[3];
  22. //float last_vel_n[3];
  23. float accSum;
  24. //float accSize;
  25. //int press_data[10];
  26. //int ZUPT_STATUS;
  27. //int press_index;
  28. //int time_zupt;
  29. int SAVEFLASH = 1;
  30. //uint32_t gyrFlash[3];
  31. //last_stage:0 为 走路状态;
  32. //last_stage:1 为 静止状态
  33. //int last_stage;
  34. uint8_t step_count;
  35. uint16_t step_distance;
  36. //int step_time = 0;
  37. //int testCon = 0;
  38. //UKF参数 发散并不是出现在姿态的估计
  39. //int UKF_L = 4;
  40. //float UKF_alpha = 0.01f;
  41. //float UKF_beta = 2.0f;
  42. //float UKF_kappa = 0.0f;
  43. //float gamma;
  44. //float wm[9];
  45. //float wc[9];
  46. //float UKF_Q[4];
  47. //float UKF_P[4][4];
  48. //int UKF_iter;
  49. //float mag_prev[3];
  50. //float deltaC[9];
  51. //float UKF_C[9];
  52. //float UKF_roll;
  53. //float UKF_pitch;
  54. //float UKF_yaw;
  55. //float EKF_roll;
  56. //float EKF_pitch;
  57. //float EKF_yaw;
  58. //int canUKF = 0;
  59. //int stand_zupt_count = 0;
  60. //重置磁航向,计算双脚的磁航向,以确定身体的正朝向
  61. //float magSum_xyz[3];
  62. //float accSum_xyz[3];
  63. //float standMag[3];
  64. //float mag_buff[3][20];
  65. float heading_buff[20];
  66. float zupt_gyr_norm = 99999;
  67. float zupt_mag[3];
  68. float zupt_heading;
  69. int step_index = 0;
  70. //int press_zupt_count = 20;
  71. int out_zupt = 1;
  72. void saveStepData(int index, float heading, float mag[3])
  73. {
  74. // for(int i = 0; i < 3; i++)
  75. // {
  76. // mag_buff[i][index % 20] = mag[i];
  77. // }
  78. heading_buff[index % 20] = heading;
  79. }
  80. float calDeltaHeading(int index, float now_heading, float now_mag[3])
  81. {
  82. //寻找相似的方向
  83. int start_index = index - 20;
  84. if(start_index < 0)
  85. start_index = 0;
  86. float deltaHeading;
  87. for(int i = start_index; i < index; i++)
  88. {
  89. deltaHeading = now_heading - heading_buff[i % 20];
  90. if(deltaHeading < -3.141591f)
  91. {
  92. deltaHeading = (deltaHeading + 6.2831852f);
  93. }
  94. else if(deltaHeading > 3.141591f)
  95. {
  96. deltaHeading = (deltaHeading - 6.2831852f);
  97. }
  98. if(fabsf(deltaHeading) < 0.0873f)//5/180*pi
  99. {
  100. return deltaHeading;
  101. }
  102. }
  103. return 99;
  104. }
  105. void calKafmanGain9x4(float *K, float *P)
  106. {
  107. float m_rever[4][4];
  108. float m[4][4];
  109. m[0][0] = P[20];m[0][1] = P[24];m[0][2] = P[25];m[0][3] = P[26];
  110. m[1][0] = P[56];m[1][1] = P[60];m[1][2] = P[61];m[1][3] = P[62];
  111. m[2][0] = P[65];m[2][1] = P[69];m[2][2] = P[70];m[2][3] = P[71];
  112. m[3][0] = P[74];m[3][1] = P[78];m[3][2] = P[79];m[3][3] = P[80];
  113. for(int i = 0; i <4; i++)
  114. {
  115. m[i][i] += SIGMA_V * SIGMA_V;
  116. }
  117. matrix_inverse(m, m_rever);
  118. 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];
  119. 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];
  120. 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];
  121. 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];
  122. 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];
  123. 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];
  124. 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];
  125. 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];
  126. 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];
  127. 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];
  128. 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];
  129. 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];
  130. 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];
  131. 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];
  132. 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];
  133. 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];
  134. 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];
  135. 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];
  136. 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];
  137. 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];
  138. 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];
  139. 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];
  140. 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];
  141. 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];
  142. 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];
  143. 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];
  144. 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];
  145. 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];
  146. 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];
  147. 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];
  148. 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];
  149. 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];
  150. 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];
  151. 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];
  152. 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];
  153. 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];
  154. }
  155. void calDeltaX9x4(float *K, float *measure, float *delta_x)
  156. {
  157. for(int i = 0; i < 9; i++)
  158. {
  159. delta_x[i] = 0.0f;
  160. for(int j = 0; j < 4; j ++)
  161. {
  162. delta_x[i] += (K[i * 4 + j] *measure[j]);
  163. }
  164. }
  165. }
  166. void calStateCov9x4(float *P, float *K)
  167. {
  168. static float P_copy[81];
  169. for(int i = 0; i < 9; i++)
  170. {
  171. for(int j = 0; j < 9; j++)
  172. {
  173. 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];
  174. }
  175. }
  176. for(int i = 0; i < 81 ; i ++)
  177. {
  178. P[i] -= P_copy[i];
  179. }
  180. }
  181. float calHeading(float mag[3], float acc[3])
  182. {
  183. float hSqrt;
  184. float eSqrt;
  185. float h[3]; //东向
  186. h[0] = mag[1] * acc[2] - mag[2] * acc[1];
  187. h[1] = mag[2] * acc[0] - mag[0] * acc[2];
  188. h[2] = mag[0] * acc[1] - mag[1] * acc[0];
  189. hSqrt = 1/sqrt(h[0] * h[0] + h[1] * h[1] + h[2] * h[2]);
  190. for(int i = 0; i < 3; i++)
  191. {
  192. h[i] *= hSqrt;
  193. }
  194. float e[3]; //北向
  195. e[0] = acc[1] * h[2] - acc[2] * h[1];
  196. e[1] = acc[2] * h[0] - acc[0] * h[2];
  197. e[2] = acc[0] * h[1] - acc[1] * h[0];
  198. eSqrt = 1/sqrt(e[0] * e[0] + e[1] * e[1] + e[2] * e[2]);
  199. for(int i = 0; i < 3; i++)
  200. {
  201. e[i] *= eSqrt;
  202. }
  203. return atan2(-h[1], e[1]);
  204. }
  205. void resetAttbyMag(float C[9], float acc[3], float mag[3])
  206. {
  207. float accScale = sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
  208. float pitch = asin(-acc[0]/accScale);
  209. float roll = atan2(acc[1], acc[2]);
  210. float pitch_sin = sin(pitch);
  211. float pitch_cos = cos(pitch);
  212. float roll_sin = sin(roll);
  213. float roll_cos = cos(roll);
  214. float mag_heading;
  215. C[0] = pitch_cos;
  216. C[1] = pitch_sin * roll_sin;
  217. C[2] = pitch_sin * roll_cos;
  218. C[3] = 0.0;
  219. C[4] = roll_cos;
  220. C[5] = -roll_sin;
  221. mag_heading = atan2(-C[4] * mag[1] - C[5] * mag[2], C[0] * mag[0] + C[1] * mag[1] + C[2] * mag[2]);
  222. float yaw_sin = sin(mag_heading);
  223. float yaw_cos = cos(mag_heading);
  224. C[0] = pitch_cos * yaw_cos;
  225. C[1] = pitch_sin * roll_sin * yaw_cos - roll_cos * yaw_sin;
  226. C[2] = pitch_sin * roll_cos * yaw_cos + roll_sin * yaw_sin;
  227. C[3] = pitch_cos * yaw_sin;
  228. C[4] = pitch_sin * roll_sin * yaw_sin + roll_cos * yaw_cos;
  229. C[5] = pitch_sin * roll_cos * yaw_sin - roll_sin * yaw_cos;
  230. C[6] = acc[0];
  231. C[7] = acc[1];
  232. C[8] = acc[2];
  233. }
  234. void cal_step_data(void)
  235. {
  236. static int step_count_sum = 0;
  237. step_count = 1;
  238. step_count_sum += step_count;
  239. 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]));
  240. if (step_length > 5.0f)
  241. {
  242. step_length = 1.2f;
  243. }
  244. step_distance = (uint16_t)(step_length * 100.0f);
  245. //sen_step_to_host(step_count, step_distance);
  246. }
  247. uint8_t get_step_count(void)
  248. {
  249. return step_count;
  250. }
  251. uint16_t get_step_length(void)
  252. {
  253. return step_distance;
  254. }
  255. void Initialize(float *gyr, float *acc)
  256. {
  257. frame_index = 1;
  258. stand_num = 0;
  259. // accSize = 1.0f;
  260. accSum = 0.0f;
  261. // ZUPT_STATUS = 0;
  262. memset(last_pos_n, 0, 3 * sizeof(float));
  263. memset(pos_offset, 0, 3 * sizeof(float));
  264. memset(P, 0, 81 * sizeof(float));
  265. memset(acc_n, 0, 3 * sizeof(float));
  266. memset(vel_n, 0, 3 * sizeof(float));
  267. memset(pos_n, 0, 3 * sizeof(float));
  268. memset(Temporary_array1, 0, 9 * sizeof(float));
  269. memset(Temporary_array2, 0, 9 * sizeof(float));
  270. memset(K, 0, 27 * sizeof(float));
  271. memset(P_prev, 0, 81 * sizeof(float));
  272. memset(delta_x, 0, 9 * sizeof(float));
  273. memset(C, 0, 9 * sizeof(float));
  274. memset(Temporary_array1, 0, 9 * sizeof(float));
  275. memset(Temporary_array2, 0, 9 * sizeof(float));
  276. // memset(press_data, 0, 10 * sizeof(int));
  277. init_attitude_matrix(C, acc, g);
  278. memcpy(C_prev, C, 9 * sizeof(float));
  279. // memcpy(gyrBias, (uint32_t *)FLASH_ADD, 3 * sizeof(float));
  280. }
  281. void attitude_matrix_update(float *C, float *Temporary_array1, float *Temporary_array2, float *gyr, float dt)
  282. {
  283. Temporary_array1[0] = 2.0f;
  284. Temporary_array1[1] = dt * gyr[2];
  285. Temporary_array1[2] = -dt * gyr[1];
  286. Temporary_array1[3] = -dt * gyr[2];
  287. Temporary_array1[4] = 2.0f;
  288. Temporary_array1[5] = dt * gyr[0];
  289. Temporary_array1[6] = dt * gyr[1];
  290. Temporary_array1[7] = -dt * gyr[0];
  291. Temporary_array1[8] = 2.0f;
  292. invert3x3(Temporary_array1, Temporary_array2);
  293. memset(Temporary_array1, 0, 9 * sizeof(float));
  294. Temporary_array1[0] = 2 * C[0] + C[1] * dt * gyr[2] - C[2] * dt * gyr[1];
  295. Temporary_array1[1] = 2 * C[1] - C[0] * dt * gyr[2] + C[2] * dt * gyr[0];
  296. Temporary_array1[2] = 2 * C[2] + C[0] * dt * gyr[1] - C[1] * dt * gyr[0];
  297. Temporary_array1[3] = 2 * C[3] + C[4] * dt * gyr[2] - C[5] * dt * gyr[1];
  298. Temporary_array1[4] = 2 * C[4] - C[3] * dt * gyr[2] + C[5] * dt * gyr[0];
  299. Temporary_array1[5] = 2 * C[5] + C[3] * dt * gyr[1] - C[4] * dt * gyr[0];
  300. Temporary_array1[6] = 2 * C[6] + C[7] * dt * gyr[2] - C[8] * dt * gyr[1];
  301. Temporary_array1[7] = 2 * C[7] - C[6] * dt * gyr[2] + C[8] * dt * gyr[0];
  302. Temporary_array1[8] = 2 * C[8] + C[6] * dt * gyr[1] - C[7] * dt * gyr[0];
  303. multiply3x3(Temporary_array1, Temporary_array2, C);
  304. }
  305. float max_window_val(float *window, int window_size)
  306. {
  307. float val = window[0];
  308. for (int i = 0; i < window_size; i++)
  309. {
  310. if (window[i] > val)
  311. val = window[i];
  312. }
  313. return val;
  314. }
  315. int max_window_int(int *window, int window_size)
  316. {
  317. int val = window[0];
  318. for (int i = 0; i < window_size; i++)
  319. {
  320. if (window[i] > val)
  321. val = window[i];
  322. }
  323. return val;
  324. }
  325. float min_window_val(float *window, int window_size)
  326. {
  327. float val = window[0];
  328. for (int i = 0; i < window_size; i++)
  329. {
  330. if (window[i] < val)
  331. val = window[i];
  332. }
  333. return val;
  334. }
  335. int min_window_int(int *window, int window_size)
  336. {
  337. int val = window[0];
  338. for (int i = 0; i < window_size; i++)
  339. {
  340. if (window[i] < val)
  341. val = window[i];
  342. }
  343. return val;
  344. }
  345. //press_tren 函数功能:提供走路过程中上升沿,下降沿
  346. //1 为上升 2 为 下降 0为不需要得状态
  347. int press_trend(int index, int *window, int window_size)
  348. {
  349. int i;
  350. int max_val = window[(index - 1) % window_size];
  351. int max_index = index;
  352. int min_val = max_val;
  353. int min_index = max_index;
  354. for (i = 1; i < window_size + 1; i++)
  355. {
  356. if (max_val < window[(index - i) % window_size])
  357. {
  358. max_index = index - i + 1;
  359. max_val = window[(index - i) % window_size];
  360. }
  361. if (min_val > window[(index - i) % window_size])
  362. {
  363. min_index = index - i + 1;
  364. min_val = window[(index - i) % window_size];
  365. }
  366. }
  367. if (max_index > min_index && max_val > min_val + 50000)
  368. {
  369. return 1;
  370. }
  371. if (max_index < min_index && max_val > min_val + 50000)
  372. {
  373. return 2;
  374. }
  375. return 0;
  376. }
  377. void dcm2angle(float *dcm, float *roll, float *pitch, float *yaw)
  378. {
  379. *yaw = atan2(dcm[3], dcm[0]);
  380. *pitch = asin(-dcm[6]);
  381. *roll = atan2(dcm[7], dcm[8]);
  382. }
  383. void quat2angleTest(float qin[4], float *roll, float *pitch, float *yaw)
  384. {
  385. //float r11 = qin[0] * qin[0] + qin[1] * qin[1] - qin[2] * qin[2] - qin[3] * qin[3];
  386. float r11 = 2.0f * (qin[1] * qin[2] + qin[0] * qin[3]);
  387. //float r21 = 2.0f * (qin[1] * qin[2] - qin[0] * qin[3]);
  388. float r12 = qin[0] * qin[0] + qin[1] * qin[1] - qin[2] * qin[2] - qin[3] * qin[3];
  389. float r21 = -2.0f * (qin[1] * qin[3] - qin[0] * qin[2]);
  390. float r31 = 2.0f * (qin[2] * qin[3] + qin[0] * qin[1]);
  391. float r32 = qin[0] * qin[0] - qin[1] * qin[1] - qin[2] * qin[2] + qin[3] * qin[3];
  392. if (r21 < -0.999999999f)
  393. r21 = -1.0f;
  394. else if (r21 > 0.999999999f)
  395. r21 = 1.0f;
  396. *roll = atan2(r11, r12);
  397. *pitch = asin(r21);
  398. *yaw = atan2(r31, r32);
  399. }
  400. void dcm2angleTest(float C[9], short att[3])
  401. {
  402. float yaw, pitch, roll;
  403. pitch = asin(-C[6]);
  404. if(C[6] > 0.999999f || C[6] < -0.999999f)
  405. {
  406. //当俯仰角为90度的时候,则假设翻滚角为0度
  407. yaw = atan2(-C[1], C[4]);
  408. roll = 0.0f;
  409. }
  410. else
  411. {
  412. yaw = atan2(C[3], C[0]);
  413. roll = atan2(C[7], C[8]);
  414. }
  415. att[0] = (short)(yaw * 10000.f); //yaw
  416. att[1] = (short)(pitch * 10000.f); //pitch
  417. att[2] = (short)(roll * 10000.f); //roll
  418. }
  419. void quat2dcm(float *qin, float *dcm)
  420. {
  421. float qin_norm = 1 / sqrt(qin[0] * qin[0] + qin[1] * qin[1] + qin[2] * qin[2] + qin[3] * qin[3]);
  422. for (int i = 0; i < 4; i++)
  423. qin[i] *= qin_norm;
  424. dcm[0] = qin[0] * qin[0] + qin[1] * qin[1] - qin[2] * qin[2] - qin[3] * qin[3];
  425. dcm[1] = 2.0f * (qin[1] * qin[2] + qin[0] * qin[3]);
  426. dcm[2] = 2.0f * (qin[1] * qin[3] - qin[0] * qin[2]);
  427. dcm[3] = 2.0f * (qin[1] * qin[2] - qin[0] * qin[3]);
  428. dcm[4] = qin[0] * qin[0] - qin[1] * qin[1] + qin[2] * qin[2] - qin[3] * qin[3];
  429. dcm[5] = 2.0f * (qin[2] * qin[3] + qin[0] * qin[1]);
  430. dcm[6] = 2.0f * (qin[1] * qin[3] + qin[0] * qin[2]);
  431. dcm[7] = 2.0f * (qin[2] * qin[3] - qin[0] * qin[1]);
  432. dcm[8] = qin[0] * qin[0] - qin[1] * qin[1] - qin[2] * qin[2] + qin[3] * qin[3];
  433. }
  434. void multiply3x3T(float *a, float *b, float* dst)
  435. {
  436. dst[0] = a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
  437. dst[1] = a[0] * b[3] + a[1] * b[4] + a[2] * b[5];
  438. dst[2] = a[0] * b[6] + a[1] * b[7] + a[2] * b[8];
  439. dst[3] = a[3] * b[0] + a[4] * b[1] + a[5] * b[2];
  440. dst[4] = a[3] * b[3] + a[4] * b[4] + a[5] * b[5];
  441. dst[5] = a[3] * b[6] + a[4] * b[7] + a[5] * b[8];
  442. dst[6] = a[6] * b[0] + a[7] * b[1] + a[8] * b[2];
  443. dst[7] = a[6] * b[3] + a[7] * b[4] + a[8] * b[5];
  444. dst[8] = a[6] * b[6] + a[7] * b[7] + a[8] * b[8];
  445. }
  446. void deltaAttMatrix(float C_prev[9], float C_now[9], float deltaC[9])
  447. {
  448. //detaC = C_prev'* C;
  449. deltaC[0] = C_now[0] * C_prev[0] + C_now[3] * C_prev[3] + C_now[6] * C_prev[6];
  450. deltaC[1] = C_now[1] * C_prev[0] + C_now[4] * C_prev[3] + C_now[7] * C_prev[6];
  451. deltaC[2] = C_now[2] * C_prev[0] + C_now[5] * C_prev[3] + C_now[8] * C_prev[6];
  452. deltaC[3] = C_now[0] * C_prev[1] + C_now[3] * C_prev[4] + C_now[6] * C_prev[7];
  453. deltaC[4] = C_now[1] * C_prev[1] + C_now[4] * C_prev[4] + C_now[7] * C_prev[7];
  454. deltaC[5] = C_now[2] * C_prev[1] + C_now[5] * C_prev[4] + C_now[8] * C_prev[7];
  455. deltaC[6] = C_now[0] * C_prev[2] + C_now[3] * C_prev[5] + C_now[6] * C_prev[8];
  456. deltaC[7] = C_now[1] * C_prev[2] + C_now[4] * C_prev[5] + C_now[7] * C_prev[8];
  457. deltaC[8] = C_now[2] * C_prev[2] + C_now[5] * C_prev[5] + C_now[8] * C_prev[8];
  458. }
  459. void normVector(float a[3])
  460. {
  461. float norm = 1.0f/sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
  462. a[0] *= norm;
  463. a[1] *= norm;
  464. a[2] *= norm;
  465. }
  466. void resetUKF(float *UKF_Q, float UKF_P[4][4], float *mag_prev, float *mag, float *UKF_C, float *C)
  467. {
  468. memset(UKF_Q, 0, 4 * sizeof(float));
  469. UKF_Q[0] = 1.0f;
  470. memcpy(mag_prev, mag, 3 * sizeof(float));
  471. memcpy(UKF_C, C, 9 * sizeof(float));
  472. for (int i = 0; i < 4; i++)
  473. for (int j = 0; j < 4; j++)
  474. {
  475. UKF_P[i][j] = 0.0f;
  476. }
  477. for (int i = 0; i < 4; i++)
  478. {
  479. UKF_P[i][i] = 0.0000001f;
  480. }
  481. }
  482. //利用陀螺仪的双极端盘判断是否在稳定的范围
  483. int isStandCon(float gyr_extreme[6])
  484. {
  485. 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)
  486. {
  487. return 1;
  488. }
  489. return 0;
  490. }
  491. unsigned char footPDR(uint32_t num, float *gyr, float *acc, float *mag ,int press, int32_t* pos_res, int16_t* att,
  492. int16_t x_zupt, int16_t y_zupt, int16_t z_zupt)
  493. {
  494. unsigned char movement_e = 0;
  495. for (int i = 0; i < 3; i++)
  496. {
  497. // accSum_xyz[i] += acc[i];
  498. // magSum_xyz[i] += mag[i];
  499. gyr[i] *= (PI / 180);
  500. acc[i] *= g;
  501. }
  502. if (num_peak == 0)
  503. {
  504. for (int i = 0; i < 3; i++)
  505. {
  506. gyr_extreme[2 * i] = gyr[i];
  507. gyr_extreme[2 * i + 1] = gyr[i];
  508. }
  509. }
  510. for (int i = 0; i < 3; i++)
  511. {
  512. if (gyr[i] < gyr_extreme[2 * i])
  513. {
  514. gyr_extreme[2 * i] = gyr[i];
  515. }
  516. if (gyr[i] > gyr_extreme[2 * i + 1])
  517. {
  518. gyr_extreme[2 * i + 1] = gyr[i];
  519. }
  520. }
  521. // accSum += sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
  522. for (int i = 0; i < 3; i++)
  523. {
  524. gyr_mean[i] += gyr[i];
  525. }
  526. num_peak++;
  527. //在线估计陀螺仪的零偏, 6050的零偏偏大
  528. if (num_peak == 300)
  529. {
  530. if (isStandCon(gyr_extreme))
  531. {
  532. if(SAVEFLASH)
  533. {
  534. //识别每一次游戏模式下,静止状态的陀螺仪令零偏
  535. for(int i = 0; i < 3; i++)
  536. {
  537. gyrBias[i] = gyr_mean[i] * 0.0033f;
  538. }
  539. }
  540. }
  541. num_peak = 0;
  542. memset(gyr_mean, 0, 3 * sizeof(float));
  543. // memset(accSum_xyz, 0, 3 * sizeof(float));
  544. // memset(magSum_xyz, 0, 3 * sizeof(float));
  545. }
  546. gyr[0] -= gyrBias[0];
  547. gyr[1] -= gyrBias[1];
  548. gyr[2] -= gyrBias[2];
  549. float gyr_norm_xyz = sqrt(gyr[0] * gyr[0] + gyr[1] * gyr[1] + gyr[2] * gyr[2]);
  550. float acc_norm_xyz = sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
  551. float mag_norm = sqrt(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
  552. //需要一个滑动窗口来判断脚步是否在地上
  553. frame_index++;
  554. //下面为惯导解算
  555. if (num == 0|| frame_index == 0)
  556. {
  557. Initialize(gyr, acc);
  558. gpio_mt_run(500);
  559. //UKF_para(UKF_L, UKF_alpha, UKF_beta, UKF_kappa, &gamma, wm, wc);
  560. return movement_e;
  561. }
  562. //惯导解算: 姿态矩阵更新
  563. attitude_matrix_update(C, Temporary_array1, Temporary_array2, gyr, dt);
  564. //惯导解算: 将IMU坐标系的加速度转换到“导航坐标系”下
  565. multiply3x1(C, acc, acc_n);
  566. //惯导解算: 更新IMU速度
  567. vel_n[0] = vel_n[0] + acc_n[0] * dt;
  568. vel_n[1] = vel_n[1] + acc_n[1] * dt;
  569. vel_n[2] = vel_n[2] + (acc_n[2] - g) * dt;
  570. //惯导解算: 更新IMU位置
  571. pos_n[0] = pos_n[0] + vel_n[0] * dt;
  572. pos_n[1] = pos_n[1] + vel_n[1] * dt;
  573. pos_n[2] = pos_n[2] + vel_n[2] * dt;
  574. //ekf步骤: 状态协方差矩阵预测更新
  575. //P = F*P*F' + Q;
  576. State_covariance_matrix_update(P, acc_n, dt);
  577. int window_index = (frame_index - 1) % 20;
  578. acc_norm_window[window_index] = acc_norm_xyz;
  579. float max_val = max_window_val(acc_norm_window, 20);
  580. float min_val = min_window_val(acc_norm_window, 20);
  581. if(max_val - min_val < 0.15f*g && fabsf((max_val + min_val)*0.5f - g) < 0.08f*g)
  582. {
  583. z_zupt = 1;
  584. }
  585. //zupt
  586. if(z_zupt || x_zupt || y_zupt)
  587. {
  588. //ekf步骤: 计算卡尔曼滤波增益
  589. //K = P*H'/(H*P*H' + R);
  590. calKafmanGain9x4(K, P);
  591. //ekf步骤: 观测误差更新
  592. //delta_x = K * [vel_n(:,i);];
  593. float now_heading = atan2(C[3], C[0]);
  594. float measure[4];
  595. //计算理想的角度
  596. memset(measure, 0, 4 *sizeof(float));
  597. measure[1] = vel_n[0];
  598. measure[2] = vel_n[1];
  599. measure[3] = vel_n[2];
  600. calDeltaX9x4(K, measure, delta_x);
  601. //ekf步骤: 状态协方差矩阵观测更新
  602. calStateCov9x4(P, K);
  603. //修正姿态矩阵
  604. Att_matrix_corr(C, C_prev, Temporary_array1, Temporary_array2, delta_x);
  605. //修正位置
  606. pos_n_corr(pos_n, delta_x);
  607. //修正速度
  608. vel_n_corr(vel_n, delta_x);
  609. memset(vel_n, 0 , 3 * sizeof(float));
  610. // last_stage = 1;
  611. memcpy(last_pos_n, pos_n, 3 * sizeof(float));
  612. }
  613. else
  614. {
  615. //存放一步内记录的mag数据以及heading数据
  616. if(stand_num != 0)
  617. {
  618. zupt_gyr_norm = 99999;
  619. saveStepData(step_index, zupt_heading, zupt_mag);
  620. step_index ++;
  621. }
  622. stand_num = 0;
  623. out_zupt = 0;
  624. }
  625. //状态协方差矩阵保持正交性,以防出现退化
  626. State_covariance_matrix_orthogonalization(P);
  627. // memcpy(last_vel_n, vel_n, 3 * sizeof(float));
  628. pos_offset[0] = pos_offset[0] + pos_n[0] - last_pos_n[0];
  629. pos_offset[1] = pos_offset[1] + pos_n[1] - last_pos_n[1];
  630. pos_offset[2] = pos_offset[2] + pos_n[2] - last_pos_n[2];
  631. memcpy(last_pos_n, pos_n, 3 * sizeof(float));
  632. dcm2angleTest(C, att); //航向角,俯仰角, 翻滚角(z y x)
  633. if(PCB_VERSION == 1)
  634. {
  635. pos_res[0] = (int32_t) (pos_offset[1] * 100.0f);
  636. pos_res[1] = (int32_t) (-pos_offset[0] * 100.0f);
  637. }
  638. else if(PCB_VERSION == 3)
  639. {
  640. pos_res[0] = (int32_t) (-pos_offset[1] * 100.0f);
  641. pos_res[1] = (int32_t) (pos_offset[0] * 100.0f);
  642. }
  643. else if(PCB_VERSION == 5)
  644. {
  645. pos_res[0] = (int32_t) (-pos_offset[1] * 100.0f);
  646. pos_res[1] = (int32_t) (pos_offset[0] * 100.0f);
  647. }
  648. else
  649. {
  650. pos_res[0] = (int32_t) (pos_offset[1] * 100.0f);
  651. pos_res[1] = (int32_t) (-pos_offset[0] * 100.0f);
  652. }
  653. pos_res[2] = (int32_t) (pos_offset[2] * 100.0f);
  654. return movement_e;
  655. }