Diff: gtrackMatrix.c

Revision:

1:5794ff4efa9a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gtrackMatrix.c	Fri May 20 18:37:10 2022 +0000
@@ -0,0 +1,606 @@
+/**
+*  @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");
+}
\ No newline at end of file