USNA-UMBC-Project Receiver - Add noise to CAN-bus received data and Implement Kalman Filter
Dependencies: ServoOut mcp2515 BNO055
gtrackMatrix.c
- Committer:
- professorrodriguezse
- Date:
- 2022-05-20
- Revision:
- 1:5794ff4efa9a
File content as of revision 1:5794ff4efa9a:
/** * @b Description * @n * This function is used to create identity matrix * * @param[in] size * Size of identity matrix * @param[out] A * Matrix A(size,size) = eye(size) * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_matrixEye(int size, float *A) { /* A(size*size) = eye(size) */ int i; for (i = 0U; i < size*size; i++) { A[i] = 0.f; } for (i = 0U; i < size; i++) { A[i+i*size] = 1.0f; } } /** * @b Description * @n * This function is used to initialise matrix to a value * * @param[in] rows * Number of rows * @param[in] cols * Number of cols * @param[in] value * value to set * @param[out] A * Matrix A(rows,cols) = ones(rows,cols)*value * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_matrixInit(int rows, int cols, float value, float *A) { /* A(rows*cols) = ones(rows,cols)*value */ int i; for (i = 0U; i < rows*cols; i++) { A[i] = value; } } /** * @b Description * @n * This function is used to create diagonal square matrix * * @param[in] size * Size of square matrix * @param[in] v * Diagonal vector * @param[out] A * Matrix A(size,size) = diag(v(size)) * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_matrixSetDiag(int size, float *v, float *A) { /* A(size*size) = diag(v(size)) */ int i; for (i = 0U; i < size*size; i++) { A[i] = 0; } for (i = 0U; i < size; i++) { A[i+i*size] = v[i]; } } /** * @b Description * @n * This function is used to get diagonal from the square matrix * * @param[in] size * Size of square matrix * @param[in] A * Matrix A(size,size) * @param[out] v * Diagonal vector, v(size) = diag(A(size*size)) * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_matrixGetDiag(int size, float *A, float *v) { /* v(size) = diag(A(size*size)) */ int i; for (i = 0U; i < size; i++) { v[i] = A[i+i*size]; } } /** * @b Description * @n * This function is used to multiply two matrices. * Matrices are all real, single precision floating point. * Matrices are in row-major order * * @param[in] rows * Outer dimension, number of rows * @param[in] m * Inner dimension * @param[in] cols * Outer dimension, number of cols * @param[in] A * Matrix A * @param[in] B * Matrix B * @param[out] C * Matrix C(rows,cols) = A(rows,m) X B(cols,m)T * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_matrixMultiply(int rows, int m, int cols, float *A, float *B, float *C) { /* C(rows*cols) = A(rows*m)*B(m*cols) */ int i,j, k; for (i = 0; i < rows; i++) { for (j = 0; j < cols; j++) { C[i*cols + j] = 0; for (k = 0; k < m; k++) { C[i*cols+j] += A[i*m+k] * B[k*cols + j]; } } } } /** * @b Description * @n * This function is used to multiply two matrices. Second Matrix is getting transposed first * Matrices are all real, single precision floating point. * Matrices are in row-major order * * @param[in] rows * Outer dimension, number of rows * @param[in] m * Inner dimension * @param[in] cols * Outer dimension, number of cols * @param[in] A * Matrix A * @param[in] B * Matrix B * @param[out] C * Matrix C(rows,cols) = A(rows,m) X B(cols,m)T * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_matrixTransposeMultiply(int rows, int m, int cols, float *A, float *B, float *C) { /* C(rows*cols) = A(rows*m)*B(cols*m)T */ int i,j, k; for (i = 0; i < rows; i++) { for (j = 0; j < cols; j++) { C[i*cols + j] = 0; for (k = 0; k < m; k++) { C[i*cols+j] += A[i*m+k] * B[k + j*m]; } } } } /** * @b Description * @n * This function is used to multiply matrix by a scalar. * Matrices are all real, single precision floating point. * Matrices are in row-major order * * @param[in] rows * Number of rows * @param[in] cols * Number of cols * @param[in] A * Matrix A * @param[in] c * Scalar c * @param[out] B * Matrix B(rows,cols) = A(rows,cols) * c * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_matrixScalarMultiply(int rows, int cols, float *A, float c, float *B) { /* B(rows*cols) = A(rows*cols)*C */ int i; for (i = 0U; i < rows*cols; i++) { B[i] = A[i] * c; } } /** * @b Description * @n * This function is used to add two matrices. * Matrices are all real, single precision floating point. * Matrices are in row-major order * * @param[in] rows * Number of rows * @param[in] cols * Number of cols * @param[in] A * Matrix A * @param[in] B * Matrix B * @param[out] C * Matrix C(rows,cols) = A(rows,cols) + B(rows,cols) * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_matrixAdd(int rows, int cols, float *A, float *B, float *C) { /* C(rows*cols) = A(rows*cols) + B(rows*cols) */ int i; for (i = 0U; i < rows*cols; i++) { C[i] = A[i] + B[i]; } } /** * @b Description * @n * This function is used to subtract two matrices. * Matrices are all real, single precision floating point. * Matrices are in row-major order * * @param[in] rows * Number of rows * @param[in] cols * Number of cols * @param[in] A * Matrix A * @param[in] B * Matrix B * @param[out] C * Matrix C(rows,cols) = A(rows,cols) - B(rows,cols) * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_matrixSub(int rows, int cols, float *A, float *B, float *C) { /* C(rows*cols) = A(rows*cols) - B(rows*cols) */ int i; for (i = 0U; i < rows*cols; i++) { C[i] = A[i] - B[i]; } } /** * @b Description * @n * This function is used to force matrix symmetry by averaging off-diagonal elements * Matrices are squared, real, single precision floating point. * Matrices are in row-major order * * @param[in] m (m=rows=cols) * Number of rows and cols * @param[in] A * Matrix A * @param[out] B * Matrix B * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_matrixMakeSymmetrical(int m, float *A, float *B) { /* Make square matrix symmetrical by averaging off-diagonal elements */ int i,j; for (i = 0U; i < m-1; i++) { B[i*m + i] = A[i*m + i]; for (j = i+1; j < m; j++) { B[i*m+j] = B[j*m+i] = 0.5f*(A[i*m+j]+A[j*m+i]); } } B[i*m + i] = A[i*m + i]; } /** * @b Description * @n * This function is used to initialise vector to a value * * @param[in] size * Size of vector * @param[in] value * value to set * @param[out] A * Vector A * * A(size) = ones(size,1)*value * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_vectorInit(int size, float value, float *A) { /* A(size) = ones(size,1)*value */ int i; for (i = 0U; i < size; i++) { A[i] = value; } } /** * @b Description * @n * This function adds two vectors * Vectors are real, single precision floating point. * * @param[in] size * Size of vector * @param[in] A * Vector A * @param[in] B * Vector B * @param[out] C * Vector C = A + B; * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_vectorAdd(int size, float *A, float *B, float *C) { int i; for (i = 0U; i < size; i++) { C[i] = A[i] + B[i]; } } /** * @b Description * @n * This function subtracts two vectors * Vectors are real, single precision floating point. * * @param[in] size * Size of vectors * @param[in] A * Vector A * @param[in] B * Vector B * @param[out] C * Vector C = A - B; * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_vectorSub(int size, float *A, float *B, float *C) { int i; for (i = 0U; i < size; i++) { C[i] = A[i] - B[i]; } } /** * @b Description * @n * This function multiplies vector by scalar * Vectors are real, single precision floating point. * Scalar is real, single precision floating point. * * @param[in] size * Size of vector * @param[in] A * Vector A * @param[in] c * Scalar c * @param[out] B * Vector B = A*c; * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_vectorScalarMul(int size, float *A, float c, float *B) { int i; for (i = 0U; i < size; i++) { B[i] = A[i]*c; } } /** * @b Description * @n * This function performs multiplies vector by scalar and accumulates the results * Vectors are real, single precision floating point. * Scalar is real, single precision floating point. * * @param[in] size * Size of vector * @param[in] A * Vector A * @param[in] c * Scalar c * @param[in, out] B * Vector B = B + A*c; * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_vectorScalarMulAcc(int size, float *A, float c, float *B) { int i; for (i = 0U; i < size; i++) { B[i] = B[i] + A[i]*c; } } /** * @b Description * @n * This function performs IIR vector filtering * Vectors are real, single precision floating point. * Alpha is real, single precision floating point. * * @param[in] size * Size of vector * @param[in, out] A * Vector A * @param[in] alpha * Weighting factor for new information, (0<=alpha<=1.0f) * @param[in] B * New information vector B * * Vector A = A*(1.0f-alpha) + B*alpha; * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_vectorFilter(int size, float *A, float alpha, float *B) { int i; for (i = 0U; i < size; i++) { A[i] = A[i]*(1.0f - alpha) + B[i]*alpha; } } /** * @b Description * @n * This function accumulates covariance matrix with variances from input vector and mean * Matrices are all real, single precision floating point. * Vectors are real, single precision floating point. * * @param[in] size * Size of square matrix * @param[in] A * Matrix A * @param[in] v * Vector v * @param[in] mean * Vector mean * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_matrixCovAcc(int size, float *A, float *v, float *mean) { int i,j; float d1, d2; for (i = 0U; i < size; i++) { d1 = v[i]-mean[i]; for (j = i; j < size; j++) { d2 = v[j]-mean[j]; A[i*size+j] += d1*d2; } } } /** * @b Description * @n * This function normalizes covariance matrix * Matrices are all real, single precision floating point. * * @param[in] size * Size of square matrix * @param[in,out] A * Matrix A * @param[in] num * Number of measurments num * * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_matrixCovNormalize(int size, float *A, int num) { int i,j; for (i = 0U; i < size; i++) { A[i*size+i] /= num; for (j = i+1; j < size; j++) { A[i*size+j] /= num; A[i+j*size] = A[i*size+j]; } } } /** * @b Description * @n * This function filters covariance matrix * Matrices are all real, single precision floating point. * * @param[in] size * Size of square matrix * @param[in,out] A * Matrix A * @param[in] B * Matrix B * @param[in] alpha * Filtering coefficient alpha * Matrix A = (1-alpha)*A + alpha*B * \ingroup GTRACK_ALG_MATH_FUNCTION * * @retval * None */ void gtrack_matrixCovFilter(int size, float *A, float *B, float alpha) { int i,j; for (i = 0U; i < size; i++) { A[i*size+i] = (1-alpha)*A[i*size+i] + alpha*B[i*size+i]; for (j = i+1; j < size; j++) { A[i*size+j] = (1-alpha)*A[i*size+j] + alpha*B[i*size+j]; A[i+j*size] = A[i*size+j]; } } } void gtrack_matrixPrint(int rows, int cols, float *A) { int i,j; for (i = 0U; i < rows; i++) { for (j = 0U; j < cols-1; j++) { printf("%6.4f\t", A[i*cols +j]); } printf("%6.4f\n\r", A[i*cols +j]); } printf("\n\r"); }