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");
}