USNA-UMBC-Project Receiver - Add noise to CAN-bus received data and Implement Kalman Filter
Dependencies: ServoOut mcp2515 BNO055
gtrackMatrix.c
00001 /** 00002 * @b Description 00003 * @n 00004 * This function is used to create identity matrix 00005 * 00006 * @param[in] size 00007 * Size of identity matrix 00008 * @param[out] A 00009 * Matrix A(size,size) = eye(size) 00010 * 00011 * \ingroup GTRACK_ALG_MATH_FUNCTION 00012 * 00013 * @retval 00014 * None 00015 */ 00016 void gtrack_matrixEye(int size, float *A) 00017 { 00018 /* A(size*size) = eye(size) */ 00019 int i; 00020 00021 for (i = 0U; i < size*size; i++) { 00022 A[i] = 0.f; 00023 } 00024 00025 for (i = 0U; i < size; i++) { 00026 A[i+i*size] = 1.0f; 00027 } 00028 } 00029 00030 /** 00031 * @b Description 00032 * @n 00033 * This function is used to initialise matrix to a value 00034 * 00035 * @param[in] rows 00036 * Number of rows 00037 * @param[in] cols 00038 * Number of cols 00039 * @param[in] value 00040 * value to set 00041 * @param[out] A 00042 * Matrix A(rows,cols) = ones(rows,cols)*value 00043 * 00044 * \ingroup GTRACK_ALG_MATH_FUNCTION 00045 * 00046 * @retval 00047 * None 00048 */ 00049 void gtrack_matrixInit(int rows, int cols, float value, float *A) 00050 { 00051 /* A(rows*cols) = ones(rows,cols)*value */ 00052 int i; 00053 00054 for (i = 0U; i < rows*cols; i++) { 00055 A[i] = value; 00056 } 00057 } 00058 00059 /** 00060 * @b Description 00061 * @n 00062 * This function is used to create diagonal square matrix 00063 * 00064 * @param[in] size 00065 * Size of square matrix 00066 * @param[in] v 00067 * Diagonal vector 00068 * @param[out] A 00069 * Matrix A(size,size) = diag(v(size)) 00070 * 00071 * \ingroup GTRACK_ALG_MATH_FUNCTION 00072 * 00073 * @retval 00074 * None 00075 */ 00076 void gtrack_matrixSetDiag(int size, float *v, float *A) 00077 { 00078 /* A(size*size) = diag(v(size)) */ 00079 int i; 00080 00081 for (i = 0U; i < size*size; i++) { 00082 A[i] = 0; 00083 } 00084 00085 for (i = 0U; i < size; i++) { 00086 A[i+i*size] = v[i]; 00087 } 00088 } 00089 00090 /** 00091 * @b Description 00092 * @n 00093 * This function is used to get diagonal from the square matrix 00094 * 00095 * @param[in] size 00096 * Size of square matrix 00097 * @param[in] A 00098 * Matrix A(size,size) 00099 * @param[out] v 00100 * Diagonal vector, v(size) = diag(A(size*size)) 00101 * 00102 * \ingroup GTRACK_ALG_MATH_FUNCTION 00103 * 00104 * @retval 00105 * None 00106 */ 00107 void gtrack_matrixGetDiag(int size, float *A, float *v) 00108 { 00109 /* v(size) = diag(A(size*size)) */ 00110 int i; 00111 for (i = 0U; i < size; i++) { 00112 v[i] = A[i+i*size]; 00113 } 00114 } 00115 00116 /** 00117 * @b Description 00118 * @n 00119 * This function is used to multiply two matrices. 00120 * Matrices are all real, single precision floating point. 00121 * Matrices are in row-major order 00122 * 00123 * @param[in] rows 00124 * Outer dimension, number of rows 00125 * @param[in] m 00126 * Inner dimension 00127 * @param[in] cols 00128 * Outer dimension, number of cols 00129 * @param[in] A 00130 * Matrix A 00131 * @param[in] B 00132 * Matrix B 00133 * @param[out] C 00134 * Matrix C(rows,cols) = A(rows,m) X B(cols,m)T 00135 * 00136 * \ingroup GTRACK_ALG_MATH_FUNCTION 00137 * 00138 * @retval 00139 * None 00140 */ 00141 void gtrack_matrixMultiply(int rows, int m, int cols, float *A, float *B, float *C) 00142 { 00143 /* C(rows*cols) = A(rows*m)*B(m*cols) */ 00144 int i,j, k; 00145 for (i = 0; i < rows; i++) { 00146 for (j = 0; j < cols; j++) { 00147 C[i*cols + j] = 0; 00148 for (k = 0; k < m; k++) { 00149 C[i*cols+j] += A[i*m+k] * B[k*cols + j]; 00150 } 00151 } 00152 } 00153 } 00154 00155 /** 00156 * @b Description 00157 * @n 00158 * This function is used to multiply two matrices. Second Matrix is getting transposed first 00159 * Matrices are all real, single precision floating point. 00160 * Matrices are in row-major order 00161 * 00162 * @param[in] rows 00163 * Outer dimension, number of rows 00164 * @param[in] m 00165 * Inner dimension 00166 * @param[in] cols 00167 * Outer dimension, number of cols 00168 * @param[in] A 00169 * Matrix A 00170 * @param[in] B 00171 * Matrix B 00172 * @param[out] C 00173 * Matrix C(rows,cols) = A(rows,m) X B(cols,m)T 00174 * 00175 * \ingroup GTRACK_ALG_MATH_FUNCTION 00176 * 00177 * @retval 00178 * None 00179 */ 00180 void gtrack_matrixTransposeMultiply(int rows, int m, int cols, float *A, float *B, float *C) 00181 { 00182 /* C(rows*cols) = A(rows*m)*B(cols*m)T */ 00183 int i,j, k; 00184 for (i = 0; i < rows; i++) { 00185 for (j = 0; j < cols; j++) { 00186 C[i*cols + j] = 0; 00187 for (k = 0; k < m; k++) { 00188 C[i*cols+j] += A[i*m+k] * B[k + j*m]; 00189 } 00190 } 00191 } 00192 } 00193 00194 /** 00195 * @b Description 00196 * @n 00197 * This function is used to multiply matrix by a scalar. 00198 * Matrices are all real, single precision floating point. 00199 * Matrices are in row-major order 00200 * 00201 * @param[in] rows 00202 * Number of rows 00203 * @param[in] cols 00204 * Number of cols 00205 * @param[in] A 00206 * Matrix A 00207 * @param[in] c 00208 * Scalar c 00209 * @param[out] B 00210 * Matrix B(rows,cols) = A(rows,cols) * c 00211 * 00212 * \ingroup GTRACK_ALG_MATH_FUNCTION 00213 * 00214 * @retval 00215 * None 00216 */ 00217 void gtrack_matrixScalarMultiply(int rows, int cols, float *A, float c, float *B) 00218 { 00219 /* B(rows*cols) = A(rows*cols)*C */ 00220 int i; 00221 for (i = 0U; i < rows*cols; i++) { 00222 B[i] = A[i] * c; 00223 } 00224 } 00225 00226 /** 00227 * @b Description 00228 * @n 00229 * This function is used to add two matrices. 00230 * Matrices are all real, single precision floating point. 00231 * Matrices are in row-major order 00232 * 00233 * @param[in] rows 00234 * Number of rows 00235 * @param[in] cols 00236 * Number of cols 00237 * @param[in] A 00238 * Matrix A 00239 * @param[in] B 00240 * Matrix B 00241 * @param[out] C 00242 * Matrix C(rows,cols) = A(rows,cols) + B(rows,cols) 00243 * 00244 * \ingroup GTRACK_ALG_MATH_FUNCTION 00245 * 00246 * @retval 00247 * None 00248 */ 00249 void gtrack_matrixAdd(int rows, int cols, float *A, float *B, float *C) 00250 { 00251 /* C(rows*cols) = A(rows*cols) + B(rows*cols) */ 00252 int i; 00253 for (i = 0U; i < rows*cols; i++) { 00254 C[i] = A[i] + B[i]; 00255 } 00256 } 00257 00258 /** 00259 * @b Description 00260 * @n 00261 * This function is used to subtract two matrices. 00262 * Matrices are all real, single precision floating point. 00263 * Matrices are in row-major order 00264 * 00265 * @param[in] rows 00266 * Number of rows 00267 * @param[in] cols 00268 * Number of cols 00269 * @param[in] A 00270 * Matrix A 00271 * @param[in] B 00272 * Matrix B 00273 * @param[out] C 00274 * Matrix C(rows,cols) = A(rows,cols) - B(rows,cols) 00275 * 00276 * \ingroup GTRACK_ALG_MATH_FUNCTION 00277 * 00278 * @retval 00279 * None 00280 */ 00281 void gtrack_matrixSub(int rows, int cols, float *A, float *B, float *C) 00282 { 00283 /* C(rows*cols) = A(rows*cols) - B(rows*cols) */ 00284 int i; 00285 for (i = 0U; i < rows*cols; i++) { 00286 C[i] = A[i] - B[i]; 00287 } 00288 } 00289 00290 /** 00291 * @b Description 00292 * @n 00293 * This function is used to force matrix symmetry by averaging off-diagonal elements 00294 * Matrices are squared, real, single precision floating point. 00295 * Matrices are in row-major order 00296 * 00297 * @param[in] m (m=rows=cols) 00298 * Number of rows and cols 00299 * @param[in] A 00300 * Matrix A 00301 * @param[out] B 00302 * Matrix B 00303 * 00304 * \ingroup GTRACK_ALG_MATH_FUNCTION 00305 * 00306 * @retval 00307 * None 00308 */ 00309 void gtrack_matrixMakeSymmetrical(int m, float *A, float *B) 00310 { 00311 /* Make square matrix symmetrical by averaging off-diagonal elements */ 00312 int i,j; 00313 for (i = 0U; i < m-1; i++) { 00314 B[i*m + i] = A[i*m + i]; 00315 for (j = i+1; j < m; j++) { 00316 B[i*m+j] = B[j*m+i] = 0.5f*(A[i*m+j]+A[j*m+i]); 00317 } 00318 } 00319 B[i*m + i] = A[i*m + i]; 00320 } 00321 00322 00323 /** 00324 * @b Description 00325 * @n 00326 * This function is used to initialise vector to a value 00327 * 00328 * @param[in] size 00329 * Size of vector 00330 * @param[in] value 00331 * value to set 00332 * @param[out] A 00333 * Vector A 00334 * 00335 * A(size) = ones(size,1)*value 00336 * 00337 * \ingroup GTRACK_ALG_MATH_FUNCTION 00338 * 00339 * @retval 00340 * None 00341 */ 00342 void gtrack_vectorInit(int size, float value, float *A) 00343 { 00344 /* A(size) = ones(size,1)*value */ 00345 int i; 00346 00347 for (i = 0U; i < size; i++) { 00348 A[i] = value; 00349 } 00350 } 00351 /** 00352 * @b Description 00353 * @n 00354 * This function adds two vectors 00355 * Vectors are real, single precision floating point. 00356 * 00357 * @param[in] size 00358 * Size of vector 00359 * @param[in] A 00360 * Vector A 00361 * @param[in] B 00362 * Vector B 00363 * @param[out] C 00364 * Vector C = A + B; 00365 * 00366 * \ingroup GTRACK_ALG_MATH_FUNCTION 00367 * 00368 * @retval 00369 * None 00370 */ 00371 void gtrack_vectorAdd(int size, float *A, float *B, float *C) 00372 { 00373 int i; 00374 for (i = 0U; i < size; i++) { 00375 C[i] = A[i] + B[i]; 00376 } 00377 } 00378 00379 /** 00380 * @b Description 00381 * @n 00382 * This function subtracts two vectors 00383 * Vectors are real, single precision floating point. 00384 * 00385 * @param[in] size 00386 * Size of vectors 00387 * @param[in] A 00388 * Vector A 00389 * @param[in] B 00390 * Vector B 00391 * @param[out] C 00392 * Vector C = A - B; 00393 * 00394 * \ingroup GTRACK_ALG_MATH_FUNCTION 00395 * 00396 * @retval 00397 * None 00398 */ 00399 void gtrack_vectorSub(int size, float *A, float *B, float *C) 00400 { 00401 int i; 00402 for (i = 0U; i < size; i++) { 00403 C[i] = A[i] - B[i]; 00404 } 00405 } 00406 00407 /** 00408 * @b Description 00409 * @n 00410 * This function multiplies vector by scalar 00411 * Vectors are real, single precision floating point. 00412 * Scalar is real, single precision floating point. 00413 * 00414 * @param[in] size 00415 * Size of vector 00416 * @param[in] A 00417 * Vector A 00418 * @param[in] c 00419 * Scalar c 00420 * @param[out] B 00421 * Vector B = A*c; 00422 * 00423 * \ingroup GTRACK_ALG_MATH_FUNCTION 00424 * 00425 * @retval 00426 * None 00427 */ 00428 void gtrack_vectorScalarMul(int size, float *A, float c, float *B) 00429 { 00430 int i; 00431 for (i = 0U; i < size; i++) { 00432 B[i] = A[i]*c; 00433 } 00434 } 00435 00436 /** 00437 * @b Description 00438 * @n 00439 * This function performs multiplies vector by scalar and accumulates the results 00440 * Vectors are real, single precision floating point. 00441 * Scalar is real, single precision floating point. 00442 * 00443 * @param[in] size 00444 * Size of vector 00445 * @param[in] A 00446 * Vector A 00447 * @param[in] c 00448 * Scalar c 00449 * @param[in, out] B 00450 * Vector B = B + A*c; 00451 * 00452 * \ingroup GTRACK_ALG_MATH_FUNCTION 00453 * 00454 * @retval 00455 * None 00456 */ 00457 void gtrack_vectorScalarMulAcc(int size, float *A, float c, float *B) 00458 { 00459 int i; 00460 for (i = 0U; i < size; i++) { 00461 B[i] = B[i] + A[i]*c; 00462 } 00463 } 00464 00465 /** 00466 * @b Description 00467 * @n 00468 * This function performs IIR vector filtering 00469 * Vectors are real, single precision floating point. 00470 * Alpha is real, single precision floating point. 00471 * 00472 * @param[in] size 00473 * Size of vector 00474 * @param[in, out] A 00475 * Vector A 00476 * @param[in] alpha 00477 * Weighting factor for new information, (0<=alpha<=1.0f) 00478 * @param[in] B 00479 * New information vector B 00480 * 00481 * Vector A = A*(1.0f-alpha) + B*alpha; 00482 * 00483 * \ingroup GTRACK_ALG_MATH_FUNCTION 00484 * 00485 * @retval 00486 * None 00487 */ 00488 void gtrack_vectorFilter(int size, float *A, float alpha, float *B) 00489 { 00490 int i; 00491 for (i = 0U; i < size; i++) { 00492 A[i] = A[i]*(1.0f - alpha) + B[i]*alpha; 00493 } 00494 } 00495 00496 00497 /** 00498 * @b Description 00499 * @n 00500 * This function accumulates covariance matrix with variances from input vector and mean 00501 * Matrices are all real, single precision floating point. 00502 * Vectors are real, single precision floating point. 00503 * 00504 * @param[in] size 00505 * Size of square matrix 00506 * @param[in] A 00507 * Matrix A 00508 * @param[in] v 00509 * Vector v 00510 * @param[in] mean 00511 * Vector mean 00512 * 00513 * \ingroup GTRACK_ALG_MATH_FUNCTION 00514 * 00515 * @retval 00516 * None 00517 */ 00518 void gtrack_matrixCovAcc(int size, float *A, float *v, float *mean) 00519 { 00520 int i,j; 00521 float d1, d2; 00522 00523 for (i = 0U; i < size; i++) { 00524 d1 = v[i]-mean[i]; 00525 for (j = i; j < size; j++) { 00526 d2 = v[j]-mean[j]; 00527 A[i*size+j] += d1*d2; 00528 } 00529 } 00530 } 00531 00532 /** 00533 * @b Description 00534 * @n 00535 * This function normalizes covariance matrix 00536 * Matrices are all real, single precision floating point. 00537 * 00538 * @param[in] size 00539 * Size of square matrix 00540 * @param[in,out] A 00541 * Matrix A 00542 * @param[in] num 00543 * Number of measurments num 00544 * 00545 * \ingroup GTRACK_ALG_MATH_FUNCTION 00546 * 00547 * @retval 00548 * None 00549 */ 00550 void gtrack_matrixCovNormalize(int size, float *A, int num) 00551 { 00552 int i,j; 00553 for (i = 0U; i < size; i++) { 00554 A[i*size+i] /= num; 00555 for (j = i+1; j < size; j++) { 00556 A[i*size+j] /= num; 00557 A[i+j*size] = A[i*size+j]; 00558 } 00559 } 00560 } 00561 /** 00562 * @b Description 00563 * @n 00564 * This function filters covariance matrix 00565 * Matrices are all real, single precision floating point. 00566 * 00567 * @param[in] size 00568 * Size of square matrix 00569 * @param[in,out] A 00570 * Matrix A 00571 * @param[in] B 00572 * Matrix B 00573 * @param[in] alpha 00574 * Filtering coefficient alpha 00575 * Matrix A = (1-alpha)*A + alpha*B 00576 * \ingroup GTRACK_ALG_MATH_FUNCTION 00577 * 00578 * @retval 00579 * None 00580 */ 00581 00582 void gtrack_matrixCovFilter(int size, float *A, float *B, float alpha) 00583 { 00584 int i,j; 00585 for (i = 0U; i < size; i++) { 00586 A[i*size+i] = (1-alpha)*A[i*size+i] + alpha*B[i*size+i]; 00587 for (j = i+1; j < size; j++) { 00588 A[i*size+j] = (1-alpha)*A[i*size+j] + alpha*B[i*size+j]; 00589 A[i+j*size] = A[i*size+j]; 00590 } 00591 } 00592 } 00593 00594 void gtrack_matrixPrint(int rows, int cols, float *A) 00595 { 00596 int i,j; 00597 for (i = 0U; i < rows; i++) 00598 { 00599 for (j = 0U; j < cols-1; j++) 00600 { 00601 printf("%6.4f\t", A[i*cols +j]); 00602 } 00603 printf("%6.4f\n\r", A[i*cols +j]); 00604 } 00605 printf("\n\r"); 00606 }
Generated on Fri Sep 9 2022 09:08:11 by 1.7.2