Revision 1:5794ff4efa9a, committed 2022-05-20

Comitter:

professorrodriguezse

Date:

Fri May 20 18:37:10 2022 +0000

Parent:

0:ee3eb98ec375

Commit message:

USNA-UMBC-KalmanFilter-ReceiverSide

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/NODE-KF-2-v2-noise.cpp	Fri May 20 18:37:10 2022 +0000
@@ -0,0 +1,169 @@
+/*
+Sends and Reads position of servos in degrees and prints them all.
+ */
+
+#include "mbed.h"
+#include "platform/mbed_thread.h"
+//#include "BNO055.h"
+#include "CAN3.h"
+//#include "ServoOut.h"
+#include "gtrackMatrix.c"
+#include "time.h"
+
+int myID = 2;
+
+Serial pc(USBTX, USBRX);    //pc serial (tx, rx) uses USB PA_9 and PA_10 on Nucleo D1 and D0 pins
+//BNO055 bno(D4, D5);
+SPI spi(D11, D12, D13);   // mosi, miso, sclk
+CAN3 can3(spi, D10, D2);         // spi bus, CS for MCP2515 controller
+//ServoOut servoOut1(A0);   //A0);     // PA_0 is the servo output pulse
+
+CANMessage canTx_msg;
+CANMessage canRx_msg;
+
+Timer t;
+
+int main()
+{
+    srand(time(0));
+    thread_sleep_for(500);
+    float currTime, dt, ytrue, y;
+    float T = 0.017; //s
+    t.start();
+    pc.baud(115200);
+    pc.printf("Starting Program... \n\r");
+    //can3.reset();            // reset the can bus interface
+    can3.frequency(500000);    // set up for 500K baudrate
+    char msg_send[8];
+    char msg_read_char[8];
+    thread_sleep_for(1000);
+
+    int n = 2; //# states
+    int p = 2; //# outouts
+    int rows=n;
+    int m=2;
+    int cols=n;
+
+    float A[2][2]= {{0.0,1.0},{0.0,0.0}};
+    float B[2][2]= {{0.0,0.0},{0.0,1.0}};
+    float C[1][2]= {{1.0,0.0}};
+    float Phi[n][n];
+    float Eye[n][n];
+    float tempMatrix[n][n];
+    float tempMatrix2[n][n];
+    float tempMatrix3[n][n];
+    float tempMatrix4[n][n];
+    gtrack_matrixEye(n, *Eye);
+    gtrack_matrixScalarMultiply(rows, cols, *A, T, *tempMatrix);
+    gtrack_matrixAdd(rows, cols, *Eye, *tempMatrix, *Phi);
+
+    float Gam[n][n];
+    gtrack_matrixScalarMultiply(rows, cols, *B, T, *tempMatrix);
+    gtrack_matrixScalarMultiply(rows, cols, *B, T*T/2, *tempMatrix2);
+    gtrack_matrixMultiply(rows, m, cols, *A, *tempMatrix2, *tempMatrix3);
+    gtrack_matrixAdd(rows, cols, *tempMatrix, *tempMatrix3, *Gam);
+    pc.printf("\n\r Gamma: \n\r");
+    gtrack_matrixPrint(rows, cols, *Gam);
+    //pc.printf("%.4f \t %.4f \t %.4f \t %.4f\n\r",Gam[0][0],Gam[0][1],Gam[1][0],Gam[1][1]);
+
+    float sig_w=1;            //Measurement noise parameter
+    float sig_v=50;     //Trial process noise parameters
+    float Q[n][n];
+    float R = sig_w*sig_w;
+    gtrack_matrixScalarMultiply(rows, cols, *Eye, sig_v*sig_v, *Q);
+    float hatx_0[2][1] = {{0},{0}};
+    float varx_0[n][n];
+    float P_0[n][n];
+    gtrack_matrixScalarMultiply(rows, cols, *Q, 1, *varx_0);
+    gtrack_matrixScalarMultiply(rows, cols, *varx_0, 1, *P_0);
+    pc.printf("P_0: \n\r");
+    gtrack_matrixPrint(rows, cols, *P_0);
+    //pc.printf("%.4f \t %.4f \t %.4f \t %.4f\n\r",P_0[0][0],P_0[0][1],P_0[1][0],P_0[1][1]);
+
+    float xhat_nminus1[n][1];
+    gtrack_matrixScalarMultiply(n, 1, *hatx_0, 1, *xhat_nminus1);
+    float P_nminus1[n][n];
+    gtrack_matrixScalarMultiply(n, n, *P_0, 1, *P_nminus1);
+    float y_minus1[1][1];
+    gtrack_matrixMultiply(1, 2, 1, *C, *hatx_0, *y_minus1);
+    pc.printf("y_minus1: \n\r");
+    gtrack_matrixPrint(1, 1, *y_minus1);
+
+    float xhat_n_pre[n][1];
+    float P_n_pre[n][n];
+    float yhat_n[1][1];
+    float S;
+    float epsilon = 0.00001;
+    int count = 0;
+
+    float tempVector[1][n];
+    float tempScalar[1][1];
+    float KFGain[n][1];
+    float tempVector2[n][1];
+    float P_n[n][n];
+    float xhat_n[n][1];
+    pc.printf("Sample: \t Time: \t Yaw: \t Yaw+Noise: \t Estimate: \t KFGain: (1) and (2)\n\r");
+    while(1) {
+        currTime = t.read();
+        count = count +1;
+        if (count > 2000) {
+            break;
+        }
+        while(1) {
+            if(can3.read(&canRx_msg) == CAN_OK) {
+                if(canRx_msg.id == 1) {
+                    for (int i = 0; i < 8; i++) {
+                        msg_read_char[i] = (char)canRx_msg.data[i];
+                    }
+                    sscanf(msg_read_char, "%f", &ytrue);
+                    y = ytrue + ((rand() % 100) - 50)/10;
+                    gtrack_matrixMultiply(n, n, 1, *Phi, *xhat_nminus1, *xhat_n_pre);  //xhat_n_pre=Phi*xhat_nminus1
+                    gtrack_matrixMultiply(n, n, n, *Gam, *Q, *tempMatrix);                      //Gam*Q
+                    gtrack_matrixTransposeMultiply(n, n, n, *tempMatrix, *Gam, *tempMatrix2);   //Gam*Q*Gam'
+                    gtrack_matrixMultiply(n, n, n, *Phi, *P_nminus1, *tempMatrix);              //Phi*P_nminus1
+                    gtrack_matrixTransposeMultiply(n, n, n, *tempMatrix, *Phi, *tempMatrix3);   //Phi*P_nminus1*Phi'
+                    gtrack_matrixAdd(n, n, *tempMatrix2, *tempMatrix3, *P_n_pre);               //P_n_pre=Phi*P_nminus1*Phi'+Gam*Q*Gam'
+                    gtrack_matrixMultiply(1, 2, 1, *C, *xhat_n_pre, *yhat_n);
+                    //pc.printf("yhat_n: %.4f\n\r", yhat_n[0][0]);
+
+                    gtrack_matrixMultiply(1, 2, 2, *C, *P_n_pre, *tempVector);
+                    gtrack_matrixTransposeMultiply(1, 2, 1, *tempVector, *C, *tempScalar);
+                    S = tempScalar[0][0] + R;
+                    if ((S >= -1*epsilon) && (S <= 1*epsilon)) {
+                        pc.printf("Alert!!! S is very small %.8f \n\r", S);
+                        if (S > 0) {
+                            S = epsilon;
+                        } else {
+                            S = -1*epsilon;
+                        }
+                    }
+
+                    gtrack_matrixTransposeMultiply(2, 2, 1, *P_n_pre, *C, *tempVector2);
+                    gtrack_matrixScalarMultiply(2, 1, *tempVector2, 1/S, *KFGain);
+                    //pc.printf("S: %.5f \t KFGain: %.4f, %.4f\n\r", S, KFGain[0][0], KFGain[1][0]);
+
+                    gtrack_matrixMultiply(2, 1, 2, *KFGain, *C, *tempMatrix);
+                    gtrack_matrixSub(n, n, *Eye, *tempMatrix, *tempMatrix2);
+                    gtrack_matrixMultiply(n,n,n, *tempMatrix2, *P_n_pre, *P_n);
+
+                    gtrack_matrixMultiply(1,2,1, *C, *xhat_n_pre, *tempScalar); //C*xhat_n_pre
+                    tempScalar[0][0] = y - tempScalar[0][0];
+                    gtrack_matrixScalarMultiply(2, 1, *KFGain, tempScalar[0][0], *tempVector2);
+                    gtrack_matrixAdd(2,1, *xhat_n_pre, *tempVector2, *xhat_n);  // xhat_n=xhat_n_pre+KFGain*(y-C*xhat_n_pre);
+                    //pc.printf("xhat_n: %.4f, %.4f\n\r", xhat_n[0][0], xhat_n[1][0]);
+
+                    gtrack_matrixScalarMultiply(2, 1, *xhat_n, 1, *xhat_nminus1);
+                    gtrack_matrixScalarMultiply(2, 2, *P_n, 1, *P_nminus1);
+                    y_minus1[0][0] = y;
+
+                    pc.printf("%d \t %.3f \t %.1f \t %.1f \t %.4f \t %.4f \t %.4f\n\r", count, t.read(), ytrue, y, xhat_n[0][0], KFGain[0][0], KFGain[1][0]);
+                    dt = T-(t.read()-currTime);
+                    if (dt > 0) {
+                        thread_sleep_for(dt*1000);
+                    }
+                    break;
+                }
+            }
+        }
+    }//while(1)
+}//main
\ No newline at end of file

--- a/Nucleo_L432KC_CAN3_BNO_Servo_EW485A.cpp	Thu Jul 22 23:05:18 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,113 +0,0 @@
-/* mbed Microcontroller Library
- * Copyright (c) 2019 ARM Limited
- * SPDX-License-Identifier: Apache-2.0
- */
-// Program to test the CAN bus using SPI on the L432KC Nucleo board
-// to an MCP2551 CAN transceiver bus IC using https://os.mbed.com/users/tecnosys/code/mcp2515/
-
-// J. Bradshaw 20210512
-#include "mbed.h"
-#include "platform/mbed_thread.h"
-#include "CAN3.h"
-#include "BNO055.h"
-#include "ServoOut.h"
-
-#define THIS_CAN_ID     0x05   //Address of this CAN device
-#define DEST_CAN_ID     0   //Address of destination
-
-Serial pc(USBTX, USBRX);    //pc serial (tx, rx) uses USB PA_9 and PA_10 on Nucleo D1 and D0 pins
-BNO055 bno(D4, D5);
-SPI spi(D11, D12, D13);   // mosi, miso, sclk
-CAN3 can3(spi, D10, D2);         // spi bus, CS for MCP2515 controller
-ServoOut servoOut1(PA_0);   //A0);     // PA_0 is the servo output pulse
-AnalogIn ain3(A3);
-
-unsigned char can_txBufLen = 0;
-unsigned char can_tx_buf[8] = {0, 0, 0, 0, 0, 0, 0, 0};
-CANMessage canTx_msg;
-
-unsigned char can_rx_bufLen = 8;
-unsigned char can_rx_buf[8];
-unsigned short can_rxId = 0;
-CANMessage canRx_msg;
-
-Timer t;
-
-void bno_init(void){    
-    if(bno.check()){
-        pc.printf("BNO055 connected\r\n");
-        bno.setmode(OPERATION_MODE_CONFIG);
-        bno.SetExternalCrystal(1);
-        //bno.set_orientation(1);
-        bno.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
-        //bno.setmode(OPERATION_MODE_NDOF_FMC_OFF);   //no magnetometer
-        bno.set_angle_units(RADIANS);
-    }
-    else{
-        pc.printf("BNO055 NOT connected\r\n Program Trap.");
-        while(1);
-    }    
-}
-    
-int main() {        
-    thread_sleep_for(500);
-    
-    t.start();        
-    pc.baud(115200);
-    bno_init();
-    //can3.reset();            // reset the can bus interface
-    can3.frequency(500000);    // set up for 500K baudrate
-    
-    servoOut1.pulse_us = 1500;
-            
-    pc.printf("CAN MCP2515 test: %s\r\n", __FILE__);
-    while(1) {
-        
-        servoOut1.pulse_us = 1000 + (1000.0*ain3);
-        
-        bno.get_angles();
-        
-        pc.printf("%.2f %.2f %.2f\r\n",bno.euler.roll, bno.euler.pitch, bno.euler.yaw);
-        //sprintf(can_rx_buf, "b%.2f\r\n", bno.euler.yaw);    //format output message string
-        can_tx_buf[0] = bno.euler.rawroll & 0x00ff;
-        can_tx_buf[1] = (bno.euler.rawroll >> 8) & 0x00ff;
-        can_tx_buf[2] = bno.euler.rawpitch & 0x00ff;
-        can_tx_buf[3] = (bno.euler.rawpitch >> 8) & 0x00ff;
-        can_tx_buf[4] = bno.euler.rawyaw & 0x00ff;
-        can_tx_buf[5] = (bno.euler.rawyaw >> 8) & 0x00ff;
-        can_tx_buf[6] = 0;
-        can_tx_buf[7] = 0;
-
-        // CAN write message
-        for(int i=0;i<8;i++){
-            canTx_msg.data[i] = can_tx_buf[i];
-        }        
-        canTx_msg.id = THIS_CAN_ID; //(rand() % 0xff);    // Randomize transmit ID or  THIS_CAN_ID;
-        
-        can3.write(&canTx_msg);
-        pc.printf("%.2f CAN TX id=%02X data: ", t.read(), canTx_msg.id);
-        for(int i=0;i<8;i++){
-            pc.printf(" %2X", can_tx_buf[i]);
-        }
-        pc.printf("\r\n");
-        //set up a random delay time of up to .79 seconds
-        //delayT = ((rand() % 7) * 31.0)  + ((rand() % 9) * .1) + runTime.sec_total;                        
-        
-        // CAN receive message       
-        if(can3.read(&canRx_msg) == CAN_OK){ //if message is available, read into msg
-            pc.printf("CAN RX id=0x%02X data: ", canRx_msg.id);
-            for (int i = 0; i < canRx_msg.len; i++) {
-                pc.printf(" %2X", canRx_msg.data[i]);
-            }
-            pc.printf("\r\n");
-        }
-        
-        //if(canTxErrors > 50){            
-//            can.reset();
-//        }
-//        if(canRxErrors > 50){            
-//            can.reset();
-//        }
-        thread_sleep_for(50);     
-    }//while(1)
-}//main

--- /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