/firmware/Back_IMU/HARDWARE/IIC/ekf_math.c

https://github.com/golaced/Oldx_fly_controller · C · 833 lines · 743 code · 69 blank · 21 comment · 145 complexity · acb22e650f9e69255698543d788daa29 MD5 · raw file 

    

  1. #include "ekf_math.h"
    2. 

  2. 

  3. // ËÄÔªÊýת»¯³ÉÒ»¸ö½«ÏòÁ¿ÓÉNED×ø±êϵÐýתµ½»úÌå×ø±êϵµÄ¾ØÕó
    4. 

  4. void Quaternion2Tnb(const float q[4], float Tnb[3][3])
    5. 

  5. {    
    6. 

  6. 		// Calculate the ned to body cosine matrix
    7. 

  7. 	  float q00 = q[0] * q[0];
    8. 

  8.     float q11 = q[1] * q[1];
    9. 

  9.     float q22 = q[2] * q[2];
    10. 

  10.     float q33 = q[3] * q[3];
    11. 

  11.     float q01 = q[0] * q[1];
    12. 

  12.     float q02 = q[0] * q[2];
    13. 

  13.     float q03 = q[0] * q[3];
    14. 

  14.     float q12 = q[1] * q[2];
    15. 

  15.     float q13 = q[1] * q[3];
    16. 

  16.     float q23 = q[2] * q[3];
    17. 

  17. 	
    18. 

  18.     Tnb[0][0] = q00 + q11 - q22 - q33;
    19. 

  19. 		Tnb[1][1] = q00 - q11 + q22 - q33;
    20. 

  20. 		Tnb[2][2] = q00 - q11 - q22 + q33;	
    21. 

  21.     Tnb[0][1] = 2 * (q12 + q03);
    22. 

  22.     Tnb[0][2] = 2 * (q13 - q02);
    23. 

  23.     Tnb[1][0] = 2 * (q12 - q03);    
    24. 

  24.     Tnb[1][2] = 2 * (q23 + q01);
    25. 

  25.     Tnb[2][0] = 2 * (q13 + q02);
    26. 

  26.     Tnb[2][1] = 2 * (q23 - q01);
    27. 

  27.     
    28. 

  28. }
    29. 

  29. 

  30. //ËÄÔªÊýת»¯³ÉÒ»¸ö½«ÏòÁ¿ÓÉ»úÌå×ø±êϵÐýתµ½NED×ø±êϵµÄ¾ØÕó
    31. 

  31. void Quaternion2Tbn(const float q[4], float Tbn[3][3])
    32. 

  32. {
    33. 

  33.     // Calculate the body to ned cosine matrix
    34. 

  34.     float q00 = q[0] * q[0];
    35. 

  35.     float q11 = q[1] * q[1];
    36. 

  36.     float q22 = q[2] * q[2];
    37. 

  37.     float q33 = q[3] * q[3];
    38. 

  38.     float q01 = q[0] * q[1];
    39. 

  39.     float q02 = q[0] * q[2];
    40. 

  40.     float q03 = q[0] * q[3];
    41. 

  41.     float q12 = q[1] * q[2];
    42. 

  42.     float q13 = q[1] * q[3];
    43. 

  43.     float q23 = q[2] * q[3];
    44. 

  44. 

  45.     Tbn[0][0] = q00 + q11 - q22 - q33;
    46. 

  46.     Tbn[1][1] = q00 - q11 + q22 - q33;
    47. 

  47.     Tbn[2][2] = q00 - q11 - q22 + q33;
    48. 

  48.     Tbn[0][1] = 2 * (q12 - q03);
    49. 

  49.     Tbn[0][2] = 2 * (q13 + q02);
    50. 

  50.     Tbn[1][0] = 2 * (q12 + q03);
    51. 

  51.     Tbn[1][2] = 2 * (q23 - q01);
    52. 

  52.     Tbn[2][0] = 2 * (q13 - q02);
    53. 

  53.     Tbn[2][1] = 2 * (q23 + q01);
    54. 

  54. }
    55. 

  55. 

  56. // ÓÉËÄÔªÊýת³ÉÅ·À­½Ç
    57. 

  57. void Quaternion2Euler(const float q[4], float euler[3])
    58. 

  58. {
    59. 

  59.     float R13, R11, R12, R23, R33;
    60. 

  60.     float q0s = q[0] * q[0];
    61. 

  61.     float q1s = q[1] * q[1];
    62. 

  62.     float q2s = q[2] * q[2];
    63. 

  63.     float q3s = q[3] * q[3];
    64. 

  64. 

  65.     R13    = 2.0f * (q[1] * q[3] - q[0] * q[2]);
    66. 

  66.     R11    = q0s + q1s - q2s - q3s;
    67. 

  67.     R12    = 2.0f * (q[1] * q[2] + q[0] * q[3]);
    68. 

  68.     R23    = 2.0f * (q[2] * q[3] + q[0] * q[1]);
    69. 

  69.     R33    = q0s - q1s - q2s + q3s;
    70. 

  70. 

  71. 		euler[0] = RAD_DEG * atan2f(R23, R33);
    72. 

  72.     euler[1] = RAD_DEG * asinf(-R13); // pitch always between -pi/2 to pi/2
    73. 

  73.     euler[2] = RAD_DEG * atan2f(R12, R11);       
    74. 

  74. }
    75. 

  75. 

  76. // ÓÉÅ·À­½Çת³ÉËÄÔªÊý
    77. 

  77. void Euler2Quaternion(const float euler[3], float q[4])
    78. 

  78. {
    79. 

  79.     float u1 = cos(0.5f * euler[0] * DEG_RAD);
    80. 

  80.     float u2 = cos(0.5f * euler[1] * DEG_RAD);
    81. 

  81.     float u3 = cos(0.5f * euler[2] * DEG_RAD);
    82. 

  82.     float u4 = sin(0.5f * euler[0] * DEG_RAD);
    83. 

  83.     float u5 = sin(0.5f * euler[1] * DEG_RAD);
    84. 

  84.     float u6 = sin(0.5f * euler[2] * DEG_RAD);
    85. 

  85. 	
    86. 

  86.     q[0] = u1 * u2 * u3 + u4 * u5 * u6;
    87. 

  87.     q[1] = u4 * u2 * u3 - u1 * u5 * u6;
    88. 

  88.     q[2] = u1 * u5 * u3 + u4 * u2 * u6;
    89. 

  89.     q[3] = u1 * u2 * u6 - u4 * u5 * u3;   
    90. 

  90. }
    91. 

  91. 

  92. // ½«Ò»¸öÏòÁ¿ÓÉA×ø±êϵÐýתÖÁB×ø±êϵ
    93. 

  93. void A_Fixed2B_Fixed(float TAB[3][3], float V[3])
    94. 

  94. {
    95. 

  95. 		float Vtmp[3];
    96. 

  96. 		Vtmp[0] = V[0];
    97. 

  97. 		Vtmp[1] = V[1];
    98. 

  98. 		Vtmp[2] = V[2];
    99. 

  99. 	
    100. 

  100. 		V[0]  = TAB[0][0] * Vtmp[0] + TAB[0][1] * Vtmp[1] + TAB[0][2] * Vtmp[2];
    101. 

  101.     V[1]  = TAB[1][0] * Vtmp[0] + TAB[1][1] * Vtmp[1] + TAB[1][2] * Vtmp[2];
    102. 

  102.     V[2]  = TAB[2][0] * Vtmp[0] + TAB[2][1] * Vtmp[1] + TAB[2][2] * Vtmp[2];
    103. 

  103. }
    104. 

  104. 

  105. // ÓÉËÄÔªÊý½«»úÌå²Î¿¼ÏµµÄÏòÁ¿ÐýתÖÁNED×ø±êϵÏÂ
    106. 

  106. void Quaternion2NED(const float q[4], float V_NED[3])
    107. 

  107. {
    108. 

  108. 		float Tbn[3][3];
    109. 

  109. 		Quaternion2Tbn(q, Tbn);
    110. 

  110. 		A_Fixed2B_Fixed(Tbn, V_NED);
    111. 

  111. }
    112. 

  112. 

  113. //??NED???
    114. 

  114. void Cal_Vel_NED(double velNED[3], double gpsCourse, double gpsGndSpd, double gpsVelD)
    115. 

  115. {
    116. 

  116.     velNED[0] = gpsGndSpd*cos(gpsCourse * DEG_RAD);
    117. 

  117.     velNED[1] = gpsGndSpd*sin(gpsCourse * DEG_RAD);
    118. 

  118.     velNED[2] = gpsVelD;
    119. 

  119. }
    120. 

  120. 

  121. // ²ÎÊý˵Ã÷: posNEDr[3] ´æ·Åת»»ºóµÄNEDλÖÃ×ø±ê 
    122. 

  122. // latÎÀÐǶ¨Î»µÄγ¶È*µ¥Î»ÊÇrad/s* lonÎÀÐǶ¨Î»µÄ¾­¶È*µ¥Î»ÊÇrad/s*  hgtÎÀÐǶ¨Î»µÄ¸ß¶È*µ¥Î»ÊÇm*
    123. 

  123. // latReference ²Î¿¼µÄγ¶È(±ÈÈç¿ÉÒÔÊÇ·É»úÆð·ÉµÄÄǸöγ¶È)*µ¥Î»ÊÇrad/s*
    124. 

  124. // lonReference ²Î¿¼µÄ¾­¶È(±ÈÈç¿ÉÒÔÊÇ·É»úÆð·ÉµÄÄǸö¾­¶È)*µ¥Î»ÊÇrad/s*
    125. 

  125. // hgtReference ²Î¿¼¸ß¶È(±ÈÈç¿ÉÒÔÊÇ·É»úÆð·ÉµÄº£°Î¸ß¶È)*µ¥Î»ÊÇm*
    126. 

  126. void Cal_Pos_NED(double posNEDr[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference)
    127. 

  127. {
    128. 

  128.     posNEDr[0] = earthRadius * (lat - latReference);
    129. 

  129.     posNEDr[1] = earthRadius * cos(latReference) * (lon - lonReference);
    130. 

  130.     posNEDr[2] = -(hgt - hgtReference);
    131. 

  131. }
    132. 

  132. 

  133. // 3*3 y = x^-1
    134. 

  134. void Matrix_3X3_Inv(const real_T a[9], real_T c[9])
    135. 

  135. {
    136. 

  136.   real_T x[9];
    137. 

  137.   int32_T p1;
    138. 

  138.   int32_T p2;
    139. 

  139.   int32_T p3;
    140. 

  140.   real_T absx11;
    141. 

  141.   real_T absx21;
    142. 

  142.   real_T absx31;
    143. 

  143.   int32_T itmp;
    144. 

  144.   memcpy(&x[0], &a[0], 9U * sizeof(real_T));
    145. 

  145.   p1 = 0;
    146. 

  146.   p2 = 3;
    147. 

  147.   p3 = 6;
    148. 

  148.   absx11 = fabs(a[0]);
    149. 

  149.   absx21 = fabs(a[1]);
    150. 

  150.   absx31 = fabs(a[2]);
    151. 

  151.   if ((absx21 > absx11) && (absx21 > absx31)) {
    152. 

  152.     p1 = 3;
    153. 

  153.     p2 = 0;
    154. 

  154.     x[0] = a[1];
    155. 

  155.     x[1] = a[0];
    156. 

  156.     absx11 = x[3];
    157. 

  157.     x[3] = x[4];
    158. 

  158.     x[4] = absx11;
    159. 

  159.     absx11 = x[6];
    160. 

  160.     x[6] = x[7];
    161. 

  161.     x[7] = absx11;
    162. 

  162.   } else {
    163. 

  163.     if (absx31 > absx11) {
    164. 

  164.       p1 = 6;
    165. 

  165.       p3 = 0;
    166. 

  166.       x[0] = a[2];
    167. 

  167.       x[2] = a[0];
    168. 

  168.       absx11 = x[3];
    169. 

  169.       x[3] = x[5];
    170. 

  170.       x[5] = absx11;
    171. 

  171.       absx11 = x[6];
    172. 

  172.       x[6] = x[8];
    173. 

  173.       x[8] = absx11;
    174. 

  174.     }
    175. 

  175.   }
    176. 

  176. 

  177.   x[1] /= x[0];
    178. 

  178.   x[2] /= x[0];
    179. 

  179.   x[4] -= x[1] * x[3];
    180. 

  180.   x[5] -= x[2] * x[3];
    181. 

  181.   x[7] -= x[1] * x[6];
    182. 

  182.   x[8] -= x[2] * x[6];
    183. 

  183.   if (fabs(x[5]) > fabs(x[4])) {
    184. 

  184.     itmp = p2;
    185. 

  185.     p2 = p3;
    186. 

  186.     p3 = itmp;
    187. 

  187.     absx11 = x[1];
    188. 

  188.     x[1] = x[2];
    189. 

  189.     x[2] = absx11;
    190. 

  190.     absx11 = x[4];
    191. 

  191.     x[4] = x[5];
    192. 

  192.     x[5] = absx11;
    193. 

  193.     absx11 = x[7];
    194. 

  194.     x[7] = x[8];
    195. 

  195.     x[8] = absx11;
    196. 

  196.   }
    197. 

  197. 

  198.   x[5] /= x[4];
    199. 

  199.   x[8] -= x[5] * x[7];
    200. 

  200.   absx11 = (x[5] * x[1] - x[2]) / x[8];
    201. 

  201.   absx21 = -(x[1] + x[7] * absx11) / x[4];
    202. 

  202.   c[p1] = ((1.0f - x[3] * absx21) - x[6] * absx11) / x[0];
    203. 

  203.   c[p1 + 1] = absx21;
    204. 

  204.   c[p1 + 2] = absx11;
    205. 

  205.   absx11 = -x[5] / x[8];
    206. 

  206.   absx21 = (1.0f - x[7] * absx11) / x[4];
    207. 

  207.   c[p2] = -(x[3] * absx21 + x[6] * absx11) / x[0];
    208. 

  208.   c[p2 + 1] = absx21;
    209. 

  209.   c[p2 + 2] = absx11;
    210. 

  210.   absx11 = 1.0f / x[8];
    211. 

  211.   absx21 = -x[7] * absx11 / x[4];
    212. 

  212.   c[p3] = -(x[3] * absx21 + x[6] * absx11) / x[0];
    213. 

  213.   c[p3 + 1] = absx21;
    214. 

  214.   c[p3 + 2] = absx11;
    215. 

  215. }
    216. 

  216. 

  217. // 4*4 y = x^-1
    218. 

  218. void Matrix_4X4_Inv(const real_T x[16], real_T y[16])
    219. 

  219. {
    220. 

  220. 		real_T A[16];
    221. 

  221. 		int32_T i0;
    222. 

  222. 		int8_T ipiv[4];
    223. 

  223. 		int32_T j;
    224. 

  224. 		int32_T jj;
    225. 

  225. 		int32_T jp1j;
    226. 

  226. 		int32_T pipk;
    227. 

  227. 		int32_T ix;
    228. 

  228. 		real_T smax;
    229. 

  229. 		int32_T jA;
    230. 

  230. 		real_T s;
    231. 

  231. 		int32_T i;
    232. 

  232. 		int8_T p[4];
    233. 

  233. 		for (i0 = 0; i0 < 16; i0++) 
    234. 

  234. 		{
    235. 

  235. 			y[i0] = 0.0f;
    236. 

  236. 			A[i0] = x[i0];
    237. 

  237. 		}
    238. 

  238. 

  239. 		for (i0 = 0; i0 < 4; i0++) 
    240. 

  240. 		{
    241. 

  241. 			ipiv[i0] = (int8_T)(1 + i0);
    242. 

  242. 		}
    243. 

  243. 

  244. 		for (j = 0; j < 3; j++) 
    245. 

  245. 		{
    246. 

  246. 			jj = j * 5;
    247. 

  247. 			jp1j = jj + 2;
    248. 

  248. 			pipk = 1;
    249. 

  249. 			ix = jj;
    250. 

  250. 			smax = fabs(A[jj]);
    251. 

  251. 			for (jA = 2; jA <= 4 - j; jA++) 
    252. 

  252. 			{
    253. 

  253. 				ix++;
    254. 

  254. 				s = fabs(A[ix]);
    255. 

  255. 				if (s > smax) 
    256. 

  256. 				{
    257. 

  257. 					pipk = jA;
    258. 

  258. 					smax = s;
    259. 

  259. 				}
    260. 

  260. 			}
    261. 

  261. 

  262. 			if (A[(jj + pipk) - 1] != 0.0f) 
    263. 

  263. 			{
    264. 

  264. 				if (pipk - 1 != 0) 
    265. 

  265. 				{
    266. 

  266. 					ipiv[j] = (int8_T)(j + pipk);
    267. 

  267. 					ix = j;
    268. 

  268. 					pipk = (j + pipk) - 1;
    269. 

  269. 					for (jA = 0; jA < 4; jA++) 
    270. 

  270. 					{
    271. 

  271. 						smax = A[ix];
    272. 

  272. 						A[ix] = A[pipk];
    273. 

  273. 						A[pipk] = smax;
    274. 

  274. 						ix += 4;
    275. 

  275. 						pipk += 4;
    276. 

  276. 					}
    277. 

  277. 				}
    278. 

  278. 

  279. 				i0 = (jp1j - j) + 2;
    280. 

  280. 				for (i = jp1j; i <= i0; i++) 
    281. 

  281. 				{
    282. 

  282. 					A[i - 1] /= A[jj];
    283. 

  283. 				}
    284. 

  284. 			}
    285. 

  285. 

  286. 			jA = jj + 5;
    287. 

  287. 			pipk = jj + 4;
    288. 

  288. 			for (jj = 1; jj <= 3 - j; jj++) 
    289. 

  289. 			{
    290. 

  290. 				smax = A[pipk];
    291. 

  291. 				if (A[pipk] != 0.0f) 
    292. 

  292. 				{
    293. 

  293. 					ix = jp1j;
    294. 

  294. 					i0 = (jA - j) + 3;
    295. 

  295. 					for (i = jA; i + 1 <= i0; i++) 
    296. 

  296. 					{
    297. 

  297. 						A[i] += A[ix - 1] * -smax;
    298. 

  298. 						ix++;
    299. 

  299. 					}
    300. 

  300. 				}
    301. 

  301. 

  302. 				pipk += 4;
    303. 

  303. 				jA += 4;
    304. 

  304. 			}
    305. 

  305. 		}
    306. 

  306. 

  307. 		for (i0 = 0; i0 < 4; i0++) 
    308. 

  308. 		{
    309. 

  309. 			p[i0] = (int8_T)(1 + i0);
    310. 

  310. 		}
    311. 

  311. 

  312. 		for (jA = 0; jA < 3; jA++) 
    313. 

  313. 		{
    314. 

  314. 			if (ipiv[jA] > 1 + jA) 
    315. 

  315. 			{
    316. 

  316. 				pipk = p[ipiv[jA] - 1];
    317. 

  317. 				p[ipiv[jA] - 1] = p[jA];
    318. 

  318. 				p[jA] = (int8_T)pipk;
    319. 

  319. 			}
    320. 

  320. 		}
    321. 

  321. 

  322. 		for (jA = 0; jA < 4; jA++) 
    323. 

  323. 		{
    324. 

  324. 			y[jA + ((p[jA] - 1) << 2)] = 1.0f;
    325. 

  325. 			for (j = jA; j + 1 < 5; j++) 
    326. 

  326. 			{
    327. 

  327. 				if (y[j + ((p[jA] - 1) << 2)] != 0.0f) 
    328. 

  328. 				{
    329. 

  329. 					for (i = j + 1; i + 1 < 5; i++) 
    330. 

  330. 					{
    331. 

  331. 						y[i + ((p[jA] - 1) << 2)] -= y[j + ((p[jA] - 1) << 2)] * A[i + (j << 2)];
    332. 

  332. 					}
    333. 

  333. 				}
    334. 

  334. 			}
    335. 

  335. 		}
    336. 

  336. 

  337. 		for (j = 0; j < 4; j++) 
    338. 

  338. 		{
    339. 

  339. 			pipk = j << 2;
    340. 

  340. 			for (jA = 3; jA > -1; jA += -1) 
    341. 

  341. 			{
    342. 

  342. 				jj = jA << 2;
    343. 

  343. 				if (y[jA + pipk] != 0.0f) 
    344. 

  344. 				{
    345. 

  345. 					y[jA + pipk] /= A[jA + jj];
    346. 

  346. 					for (i = 0; i + 1 <= jA; i++) 
    347. 

  347. 					{
    348. 

  348. 						y[i + pipk] -= y[jA + pipk] * A[i + jj];
    349. 

  349. 					}
    350. 

  350. 				}
    351. 

  351. 			}
    352. 

  352. 		}
    353. 

  353. }
    354. 

  354. 

  355. // 6*6 y = x^-1
    356. 

  356. void Matrix_6X6_Inv(const real_T x[36], real_T y[36])
    357. 

  357. {
    358. 

  358.   real_T A[36];
    359. 

  359.   int32_T i0;
    360. 

  360.   int8_T ipiv[6];
    361. 

  361.   int32_T j;
    362. 

  362.   int32_T jj;
    363. 

  363.   int32_T jp1j;
    364. 

  364.   int32_T pipk;
    365. 

  365.   int32_T ix;
    366. 

  366.   real_T smax;
    367. 

  367.   int32_T jA;
    368. 

  368.   real_T s;
    369. 

  369.   int32_T i;
    370. 

  370.   int8_T p[6];
    371. 

  371.   for (i0 = 0; i0 < 36; i0++) 
    372. 

  372. 	{
    373. 

  373.     y[i0] = 0.0f;
    374. 

  374.     A[i0] = x[i0];
    375. 

  375.   }
    376. 

  376. 

  377.   for (i0 = 0; i0 < 6; i0++) 
    378. 

  378. 	{
    379. 

  379.     ipiv[i0] = (int8_T)(1 + i0);
    380. 

  380.   }
    381. 

  381. 

  382.   for (j = 0; j < 5; j++) 
    383. 

  383. 	{
    384. 

  384.     jj = j * 7;
    385. 

  385.     jp1j = jj + 2;
    386. 

  386.     pipk = 1;
    387. 

  387.     ix = jj;
    388. 

  388.     smax = fabs(A[jj]);
    389. 

  389.     for (jA = 2; jA <= 6 - j; jA++) {
    390. 

  390.       ix++;
    391. 

  391.       s = fabs(A[ix]);
    392. 

  392.       if (s > smax) {
    393. 

  393.         pipk = jA;
    394. 

  394.         smax = s;
    395. 

  395.       }
    396. 

  396.     }
    397. 

  397. 

  398.     if (A[(jj + pipk) - 1] != 0.0f) {
    399. 

  399.       if (pipk - 1 != 0) {
    400. 

  400.         ipiv[j] = (int8_T)(j + pipk);
    401. 

  401.         ix = j;
    402. 

  402.         pipk = (j + pipk) - 1;
    403. 

  403.         for (jA = 0; jA < 6; jA++) {
    404. 

  404.           smax = A[ix];
    405. 

  405.           A[ix] = A[pipk];
    406. 

  406.           A[pipk] = smax;
    407. 

  407.           ix += 6;
    408. 

  408.           pipk += 6;
    409. 

  409.         }
    410. 

  410.       }
    411. 

  411. 

  412.       i0 = (jp1j - j) + 4;
    413. 

  413.       for (i = jp1j; i <= i0; i++) {
    414. 

  414.         A[i - 1] /= A[jj];
    415. 

  415.       }
    416. 

  416.     }
    417. 

  417. 

  418.     jA = jj + 7;
    419. 

  419.     pipk = jj + 6;
    420. 

  420.     for (jj = 1; jj <= 5 - j; jj++) {
    421. 

  421.       smax = A[pipk];
    422. 

  422.       if (A[pipk] != 0.0f) {
    423. 

  423.         ix = jp1j;
    424. 

  424.         i0 = (jA - j) + 5;
    425. 

  425.         for (i = jA; i + 1 <= i0; i++) {
    426. 

  426.           A[i] += A[ix - 1] * -smax;
    427. 

  427.           ix++;
    428. 

  428.         }
    429. 

  429.       }
    430. 

  430. 

  431.       pipk += 6;
    432. 

  432.       jA += 6;
    433. 

  433.     }
    434. 

  434.   }
    435. 

  435. 

  436.   for (i0 = 0; i0 < 6; i0++) {
    437. 

  437.     p[i0] = (int8_T)(1 + i0);
    438. 

  438.   }
    439. 

  439. 

  440.   for (jA = 0; jA < 5; jA++) {
    441. 

  441.     if (ipiv[jA] > 1 + jA) {
    442. 

  442.       pipk = p[ipiv[jA] - 1];
    443. 

  443.       p[ipiv[jA] - 1] = p[jA];
    444. 

  444.       p[jA] = (int8_T)pipk;
    445. 

  445.     }
    446. 

  446.   }
    447. 

  447. 

  448.   for (jA = 0; jA < 6; jA++) {
    449. 

  449.     y[jA + 6 * (p[jA] - 1)] = 1.0f;
    450. 

  450.     for (j = jA; j + 1 < 7; j++) {
    451. 

  451.       if (y[j + 6 * (p[jA] - 1)] != 0.0f) {
    452. 

  452.         for (i = j + 1; i + 1 < 7; i++) {
    453. 

  453.           y[i + 6 * (p[jA] - 1)] -= y[j + 6 * (p[jA] - 1)] * A[i + 6 * j];
    454. 

  454.         }
    455. 

  455.       }
    456. 

  456.     }
    457. 

  457.   }
    458. 

  458. 

  459.   for (j = 0; j < 6; j++) 
    460. 

  460. 	{
    461. 

  461.     pipk = 6 * j;
    462. 

  462.     for (jA = 5; jA > -1; jA += -1) 
    463. 

  463. 		{
    464. 

  464.       jj = 6 * jA;
    465. 

  465.       if (y[jA + pipk] != 0.0f) 
    466. 

  466. 				{
    467. 

  467.         y[jA + pipk] /= A[jA + jj];
    468. 

  468.         for (i = 0; i + 1 <= jA; i++) 
    469. 

  469. 				{
    470. 

  470.           y[i + pipk] -= y[jA + pipk] * A[i + jj];
    471. 

  471.         }
    472. 

  472.       }
    473. 

  473.     }
    474. 

  474.   }
    475. 

  475. }
    476. 

  476. 

  477. // 7*7 y = x^-1
    478. 

  478. void Matrix_7X7_Inv(const real_T x[49], real_T y[49])
    479. 

  479. {
    480. 

  480.   real_T A[49];
    481. 

  481.   int32_T i0;
    482. 

  482.   int8_T ipiv[7];
    483. 

  483.   int32_T j;
    484. 

  484.   int32_T jj;
    485. 

  485.   int32_T jp1j;
    486. 

  486.   int32_T pipk;
    487. 

  487.   int32_T ix;
    488. 

  488.   real_T smax;
    489. 

  489.   int32_T jA;
    490. 

  490.   real_T s;
    491. 

  491.   int32_T i;
    492. 

  492.   int8_T p[7];
    493. 

  493.   for (i0 = 0; i0 < 49; i0++) {
    494. 

  494.     y[i0] = 0.0;
    495. 

  495.     A[i0] = x[i0];
    496. 

  496.   }
    497. 

  497. 

  498.   for (i0 = 0; i0 < 7; i0++) {
    499. 

  499.     ipiv[i0] = (int8_T)(1 + i0);
    500. 

  500.   }
    501. 

  501. 

  502.   for (j = 0; j < 6; j++) {
    503. 

  503.     jj = j << 3;
    504. 

  504.     jp1j = jj + 2;
    505. 

  505.     pipk = 1;
    506. 

  506.     ix = jj;
    507. 

  507.     smax = fabs(A[jj]);
    508. 

  508.     for (jA = 2; jA <= 7 - j; jA++) {
    509. 

  509.       ix++;
    510. 

  510.       s = fabs(A[ix]);
    511. 

  511.       if (s > smax) {
    512. 

  512.         pipk = jA;
    513. 

  513.         smax = s;
    514. 

  514.       }
    515. 

  515.     }
    516. 

  516. 

  517.     if (A[(jj + pipk) - 1] != 0.0f) {
    518. 

  518.       if (pipk - 1 != 0) {
    519. 

  519.         ipiv[j] = (int8_T)(j + pipk);
    520. 

  520.         ix = j;
    521. 

  521.         pipk = (j + pipk) - 1;
    522. 

  522.         for (jA = 0; jA < 7; jA++) {
    523. 

  523.           smax = A[ix];
    524. 

  524.           A[ix] = A[pipk];
    525. 

  525.           A[pipk] = smax;
    526. 

  526.           ix += 7;
    527. 

  527.           pipk += 7;
    528. 

  528.         }
    529. 

  529.       }
    530. 

  530. 

  531.       i0 = (jp1j - j) + 5;
    532. 

  532.       for (i = jp1j; i <= i0; i++) {
    533. 

  533.         A[i - 1] /= A[jj];
    534. 

  534.       }
    535. 

  535.     }
    536. 

  536. 

  537.     jA = jj + 8;
    538. 

  538.     pipk = jj + 7;
    539. 

  539.     for (jj = 1; jj <= 6 - j; jj++) {
    540. 

  540.       smax = A[pipk];
    541. 

  541.       if (A[pipk] != 0.0f) {
    542. 

  542.         ix = jp1j;
    543. 

  543.         i0 = (jA - j) + 6;
    544. 

  544.         for (i = jA; i + 1 <= i0; i++) {
    545. 

  545.           A[i] += A[ix - 1] * -smax;
    546. 

  546.           ix++;
    547. 

  547.         }
    548. 

  548.       }
    549. 

  549. 

  550.       pipk += 7;
    551. 

  551.       jA += 7;
    552. 

  552.     }
    553. 

  553.   }
    554. 

  554. 

  555.   for (i0 = 0; i0 < 7; i0++) {
    556. 

  556.     p[i0] = (int8_T)(1 + i0);
    557. 

  557.   }
    558. 

  558. 

  559.   for (jA = 0; jA < 6; jA++) {
    560. 

  560.     if (ipiv[jA] > 1 + jA) {
    561. 

  561.       pipk = p[ipiv[jA] - 1];
    562. 

  562.       p[ipiv[jA] - 1] = p[jA];
    563. 

  563.       p[jA] = (int8_T)pipk;
    564. 

  564.     }
    565. 

  565.   }
    566. 

  566. 

  567.   for (jA = 0; jA < 7; jA++) {
    568. 

  568.     y[jA + 7 * (p[jA] - 1)] = 1.0f;
    569. 

  569.     for (j = jA; j + 1 < 8; j++) {
    570. 

  570.       if (y[j + 7 * (p[jA] - 1)] != 0.0f) {
    571. 

  571.         for (i = j + 1; i + 1 < 8; i++) {
    572. 

  572.           y[i + 7 * (p[jA] - 1)] -= y[j + 7 * (p[jA] - 1)] * A[i + 7 * j];
    573. 

  573.         }
    574. 

  574.       }
    575. 

  575.     }
    576. 

  576.   }
    577. 

  577. 

  578.   for (j = 0; j < 7; j++) {
    579. 

  579.     pipk = 7 * j;
    580. 

  580.     for (jA = 6; jA > -1; jA += -1) {
    581. 

  581.       jj = 7 * jA;
    582. 

  582.       if (y[jA + pipk] != 0.0f) {
    583. 

  583.         y[jA + pipk] /= A[jA + jj];
    584. 

  584.         for (i = 0; i + 1 <= jA; i++) {
    585. 

  585.           y[i + pipk] -= y[jA + pipk] * A[i + jj];
    586. 

  586.         }
    587. 

  587.       }
    588. 

  588.     }
    589. 

  589.   }
    590. 

  590. }
    591. 

  591. 

  592. // 10*10 y = x^-1
    593. 

  593. void Matrix_10X10_Inv(const real_T x[100], real_T y[100])
    594. 

  594. {
    595. 

  595. 		real_T A[100];
    596. 

  596. 		int32_T i0;
    597. 

  597. 		int8_T ipiv[10];
    598. 

  598. 		int32_T j;
    599. 

  599. 		int32_T c;
    600. 

  600. 		int32_T pipk;
    601. 

  601. 		int32_T ix;
    602. 

  602. 		real_T smax;
    603. 

  603. 		int32_T k;
    604. 

  604. 		real_T s;
    605. 

  605. 		int32_T jy;
    606. 

  606. 		int32_T ijA;
    607. 

  607. 		int8_T p[10];
    608. 

  608. 		for (i0 = 0; i0 < 100; i0++) 
    609. 

  609. 		{
    610. 

  610. 				y[i0] = 0.0f;
    611. 

  611. 				A[i0] = x[i0];
    612. 

  612. 		}
    613. 

  613. 

  614. 		for (i0 = 0; i0 < 10; i0++) 
    615. 

  615. 		{
    616. 

  616. 				ipiv[i0] = (int8_T)(1 + i0);
    617. 

  617. 		}
    618. 

  618. 

  619. 		for (j = 0; j < 9; j++) 
    620. 

  620. 		{
    621. 

  621. 			c = j * 11;
    622. 

  622. 			pipk = 0;
    623. 

  623. 			ix = c;
    624. 

  624. 			smax = fabs(A[c]);
    625. 

  625. 			for (k = 2; k <= 10 - j; k++) 
    626. 

  626. 			{
    627. 

  627. 					ix++;
    628. 

  628. 					s = fabs(A[ix]);
    629. 

  629. 					if (s > smax) 
    630. 

  630. 					{
    631. 

  631. 							pipk = k - 1;
    632. 

  632. 							smax = s;
    633. 

  633. 					}
    634. 

  634. 			}
    635. 

  635. 

  636. 			if (A[c + pipk] != 0.0f) 
    637. 

  637. 			{
    638. 

  638. 					if (pipk != 0) 
    639. 

  639. 					{
    640. 

  640. 							ipiv[j] = (int8_T)((j + pipk) + 1);
    641. 

  641. 							ix = j;
    642. 

  642. 							pipk += j;
    643. 

  643. 							for (k = 0; k < 10; k++) 
    644. 

  644. 							{
    645. 

  645. 									smax = A[ix];
    646. 

  646. 									A[ix] = A[pipk];
    647. 

  647. 									A[pipk] = smax;
    648. 

  648. 									ix += 10;
    649. 

  649. 									pipk += 10;
    650. 

  650. 							}
    651. 

  651. 					}
    652. 

  652. 

  653. 					i0 = (c - j) + 10;
    654. 

  654. 					for (jy = c + 1; jy + 1 <= i0; jy++) 
    655. 

  655. 					{
    656. 

  656. 							A[jy] /= A[c];
    657. 

  657. 					}
    658. 

  658. 			}
    659. 

  659. 

  660. 			pipk = c;
    661. 

  661. 			jy = c + 10;
    662. 

  662. 			for (k = 1; k <= 9 - j; k++) 
    663. 

  663. 			{
    664. 

  664. 					smax = A[jy];
    665. 

  665. 					if (A[jy] != 0.0f) 
    666. 

  666. 					{
    667. 

  667. 							ix = c + 1;
    668. 

  668. 							i0 = (pipk - j) + 20;
    669. 

  669. 							for (ijA = 11 + pipk; ijA + 1 <= i0; ijA++) 
    670. 

  670. 							{
    671. 

  671. 									A[ijA] += A[ix] * -smax;
    672. 

  672. 									ix++;
    673. 

  673. 							}
    674. 

  674. 					}
    675. 

  675. 

  676. 					jy += 10;
    677. 

  677. 					pipk += 10;
    678. 

  678. 			}
    679. 

  679. 		}
    680. 

  680. 

  681. 		for (i0 = 0; i0 < 10; i0++) 
    682. 

  682. 		{
    683. 

  683. 				p[i0] = (int8_T)(1 + i0);
    684. 

  684. 		}
    685. 

  685. 

  686. 		for (k = 0; k < 9; k++) 
    687. 

  687. 		{
    688. 

  688. 				if (ipiv[k] > 1 + k) 
    689. 

  689. 				{
    690. 

  690. 						pipk = p[ipiv[k] - 1];
    691. 

  691. 						p[ipiv[k] - 1] = p[k];
    692. 

  692. 						p[k] = (int8_T)pipk;
    693. 

  693. 				}
    694. 

  694. 		}
    695. 

  695. 

  696. 		for (k = 0; k < 10; k++) 
    697. 

  697. 		{
    698. 

  698. 				y[k + 10 * (p[k] - 1)] = 1.0f;
    699. 

  699. 				for (j = k; j + 1 < 11; j++) 
    700. 

  700. 				{
    701. 

  701. 						if (y[j + 10 * (p[k] - 1)] != 0.0f) 
    702. 

  702. 						{
    703. 

  703. 								for (jy = j + 1; jy + 1 < 11; jy++) 
    704. 

  704. 								{
    705. 

  705. 										y[jy + 10 * (p[k] - 1)] -= y[j + 10 * (p[k] - 1)] * A[jy + 10 * j];
    706. 

  706. 								}
    707. 

  707. 						}
    708. 

  708. 				}
    709. 

  709. 		}
    710. 

  710. 

  711. 		for (j = 0; j < 10; j++) 
    712. 

  712. 		{
    713. 

  713. 				c = 10 * j;
    714. 

  714. 				for (k = 9; k > -1; k += -1) 
    715. 

  715. 				{
    716. 

  716. 						pipk = 10 * k;
    717. 

  717. 						if (y[k + c] != 0.0f) 
    718. 

  718. 						{
    719. 

  719. 								y[k + c] /= A[k + pipk];
    720. 

  720. 								for (jy = 0; jy + 1 <= k; jy++) 
    721. 

  721. 								{
    722. 

  722. 										y[jy + c] -= y[k + c] * A[jy + pipk];
    723. 

  723. 								}
    724. 

  724. 						}
    725. 

  725. 				}
    726. 

  726. 		}
    727. 

  727. }
    728. 

  728. 

  729. 

  730. //Mc = Ma' Ma = line * row
    731. 

  731. void Matrix_Tran(const real_T *a, real_T *c, int line, int row)
    732. 

  732. {
    733. 

  733. 		int i0;
    734. 

  734. 		int i1;
    735. 

  735. 		for (i0 = 0; i0 < line; i0++) 
    736. 

  736. 		{		  
    737. 

  737. 				for (i1 = 0; i1 < row; i1++) 
    738. 

  738. 				{				
    739. 

  739. 						c[i1*line + i0] = 0;
    740. 

  740. 				    c[i1*line + i0] = a[i0*row + i1];
    741. 

  741. 				}
    742. 

  742. 		}
    743. 

  743. }
    744. 

  744. 

  745. // Ma * Mb = Mc ; Ma ==> al * ar ; Mb ==> bl * br 
    746. 

  746. void ML_R_X_ML_R(const real_T *a, const real_T *b, real_T *c, int al, int ar, int bl, int br)
    747. 

  747. {
    748. 

  748. 		int i0;
    749. 

  749. 		int i1;
    750. 

  750. 		int i2;
    751. 

  751. 		#if 1
    752. 

  752. 		for(i2 = 0; i2 < al * br; i2++)
    753. 

  753. 		c[i2] = 0;
    754. 

  754. 		#endif
    755. 

  755. 		for(i2 = 0; i2 < br; i2++)
    756. 

  756. 		{
    757. 

  757. 				for (i0 = 0; i0 < al; i0++) 
    758. 

  758. 				{
    759. 

  759. 						for (i1 = 0; i1 < bl; i1++) 
    760. 

  760. 						{
    761. 

  761. 								c[i0 * br + i2] += a[i0 * ar + i1] * b[i1 * br + i2];
    762. 

  762. 						}
    763. 

  763. 				}
    764. 

  764. 		}
    765. 

  765. }
    766. 

  766. 

  767. void swap(float *a, float *b)
    768. 

  768. {  
    769. 

  769.     float c;
    770. 

  770.     c = *a;
    771. 

  771.     *a = *b;
    772. 

  772.     *b = c;
    773. 

  773. }
    774. 

  774. 

  775. int inv(float *p,int n)
    776. 

  776. {
    777. 

  777.   int *is,*js,i,j,k;
    778. 

  778.   float temp, fmax;
    779. 

  779.   is=(int *)malloc(n*sizeof(int));
    780. 

  780.   js=(int *)malloc(n*sizeof(int));
    781. 

  781.   for(k=0;k<n;k++)
    782. 

  782. 	{
    783. 

  783. 				fmax=0.0f;
    784. 

  784. 				for(i=k;i<n;i++)
    785. 

  785. 					for(j=k;j<n;j++)
    786. 

  786. 					{ 
    787. 

  787. 							temp=fabs(*(p+i*n+j));//????
    788. 

  788. 							if(temp>fmax)
    789. 

  789. 							{ 
    790. 

  790. 									fmax=temp;
    791. 

  791. 									is[k]=i;js[k]=j;
    792. 

  792. 							}
    793. 

  793. 					}
    794. 

  794.      if((fmax+1.0f)==1.0f)
    795. 

  795. 		 {  
    796. 

  796. 				free(is);
    797. 

  797. 				free(js);				
    798. 

  798. 				return 0;
    799. 

  799. 		 }
    800. 

  800.     if((i=is[k])!=k)
    801. 

  801.       for(j=0;j<n;j++)
    802. 

  802.         swap((p+k*n+j),(p+i*n+j));//????
    803. 

  803.    if((j=js[k])!=k)
    804. 

  804.      for(i=0;i<n;i++)
    805. 

  805.         swap((p+i*n+k),(p+i*n+j));  //????
    806. 

  806.    p[k*n+k]=1.0f/p[k*n+k];
    807. 

  807.    for(j=0;j<n;j++)
    808. 

  808.      if(j!=k)
    809. 

  809.         p[k*n+j]*=p[k*n+k];
    810. 

  810.    for(i=0;i<n;i++)
    811. 

  811.       if(i!=k)
    812. 

  812.         for(j=0;j<n;j++)
    813. 

  813.           if(j!=k)
    814. 

  814.              p[i*n+j]=p[i*n+j]-p[i*n+k]*p[k*n+j];
    815. 

  815.    for(i=0;i<n;i++)
    816. 

  816.      if(i!=k)
    817. 

  817.        p[i*n+k]*=-p[k*n+k];
    818. 

  818.  }
    819. 

  819.  for(k=n-1;k>=0;k--)
    820. 

  820.  {
    821. 

  821.      if((j=js[k])!=k)
    822. 

  822.         for(i=0;i<n;i++)
    823. 

  823.           swap((p+j*n+i),(p+k*n+i));
    824. 

  824.      if((i=is[k])!=k)
    825. 

  825.         for(j=0;j<n;j++)
    826. 

  826.           swap((p+j*n+i),(p+j*n+k));
    827. 

  827.   }
    828. 

  828.   free(is);
    829. 

  829.   free(js);
    830. 

  830.   return 1;
    831. 

  831. }
    832.