File content as of revision 1:5794ff4efa9a:

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