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 }