NODE-KF-2-v2-noise.cpp

00001 /*
00002 Sends and Reads position of servos in degrees and prints them all.
00003  */
00004 
00005 #include "mbed.h"
00006 #include "platform/mbed_thread.h"
00007 //#include "BNO055.h"
00008 #include "CAN3.h"
00009 //#include "ServoOut.h"
00010 #include "gtrackMatrix.c"
00011 #include "time.h"
00012 
00013 int myID = 2;
00014 
00015 Serial pc(USBTX, USBRX);    //pc serial (tx, rx) uses USB PA_9 and PA_10 on Nucleo D1 and D0 pins
00016 //BNO055 bno(D4, D5);
00017 SPI spi(D11, D12, D13);   // mosi, miso, sclk
00018 CAN3 can3(spi, D10, D2);         // spi bus, CS for MCP2515 controller
00019 //ServoOut servoOut1(A0);   //A0);     // PA_0 is the servo output pulse
00020 
00021 CANMessage canTx_msg;
00022 CANMessage canRx_msg;
00023 
00024 Timer t;
00025 
00026 int main()
00027 {
00028     srand(time(0));
00029     thread_sleep_for(500);
00030     float currTime, dt, ytrue, y;
00031     float T = 0.017; //s
00032     t.start();
00033     pc.baud(115200);
00034     pc.printf("Starting Program... \n\r");
00035     //can3.reset();            // reset the can bus interface
00036     can3.frequency(500000);    // set up for 500K baudrate
00037     char msg_send[8];
00038     char msg_read_char[8];
00039     thread_sleep_for(1000);
00040 
00041     int n = 2; //# states
00042     int p = 2; //# outouts
00043     int rows=n;
00044     int m=2;
00045     int cols=n;
00046 
00047     float A[2][2]= {{0.0,1.0},{0.0,0.0}};
00048     float B[2][2]= {{0.0,0.0},{0.0,1.0}};
00049     float C[1][2]= {{1.0,0.0}};
00050     float Phi[n][n];
00051     float Eye[n][n];
00052     float tempMatrix[n][n];
00053     float tempMatrix2[n][n];
00054     float tempMatrix3[n][n];
00055     float tempMatrix4[n][n];
00056     gtrack_matrixEye(n, *Eye);
00057     gtrack_matrixScalarMultiply(rows, cols, *A, T, *tempMatrix);
00058     gtrack_matrixAdd(rows, cols, *Eye, *tempMatrix, *Phi);
00059 
00060     float Gam[n][n];
00061     gtrack_matrixScalarMultiply(rows, cols, *B, T, *tempMatrix);
00062     gtrack_matrixScalarMultiply(rows, cols, *B, T*T/2, *tempMatrix2);
00063     gtrack_matrixMultiply(rows, m, cols, *A, *tempMatrix2, *tempMatrix3);
00064     gtrack_matrixAdd(rows, cols, *tempMatrix, *tempMatrix3, *Gam);
00065     pc.printf("\n\r Gamma: \n\r");
00066     gtrack_matrixPrint(rows, cols, *Gam);
00067     //pc.printf("%.4f \t %.4f \t %.4f \t %.4f\n\r",Gam[0][0],Gam[0][1],Gam[1][0],Gam[1][1]);
00068 
00069     float sig_w=1;            //Measurement noise parameter
00070     float sig_v=50;     //Trial process noise parameters
00071     float Q[n][n];
00072     float R = sig_w*sig_w;
00073     gtrack_matrixScalarMultiply(rows, cols, *Eye, sig_v*sig_v, *Q);
00074     float hatx_0[2][1] = {{0},{0}};
00075     float varx_0[n][n];
00076     float P_0[n][n];
00077     gtrack_matrixScalarMultiply(rows, cols, *Q, 1, *varx_0);
00078     gtrack_matrixScalarMultiply(rows, cols, *varx_0, 1, *P_0);
00079     pc.printf("P_0: \n\r");
00080     gtrack_matrixPrint(rows, cols, *P_0);
00081     //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]);
00082 
00083     float xhat_nminus1[n][1];
00084     gtrack_matrixScalarMultiply(n, 1, *hatx_0, 1, *xhat_nminus1);
00085     float P_nminus1[n][n];
00086     gtrack_matrixScalarMultiply(n, n, *P_0, 1, *P_nminus1);
00087     float y_minus1[1][1];
00088     gtrack_matrixMultiply(1, 2, 1, *C, *hatx_0, *y_minus1);
00089     pc.printf("y_minus1: \n\r");
00090     gtrack_matrixPrint(1, 1, *y_minus1);
00091 
00092     float xhat_n_pre[n][1];
00093     float P_n_pre[n][n];
00094     float yhat_n[1][1];
00095     float S;
00096     float epsilon = 0.00001;
00097     int count = 0;
00098 
00099     float tempVector[1][n];
00100     float tempScalar[1][1];
00101     float KFGain[n][1];
00102     float tempVector2[n][1];
00103     float P_n[n][n];
00104     float xhat_n[n][1];
00105     pc.printf("Sample: \t Time: \t Yaw: \t Yaw+Noise: \t Estimate: \t KFGain: (1) and (2)\n\r");
00106     while(1) {
00107         currTime = t.read();
00108         count = count +1;
00109         if (count > 2000) {
00110             break;
00111         }
00112         while(1) {
00113             if(can3.read(&canRx_msg) == CAN_OK) {
00114                 if(canRx_msg.id == 1) {
00115                     for (int i = 0; i < 8; i++) {
00116                         msg_read_char[i] = (char)canRx_msg.data[i];
00117                     }
00118                     sscanf(msg_read_char, "%f", &ytrue);
00119                     y = ytrue + ((rand() % 100) - 50)/10;
00120                     gtrack_matrixMultiply(n, n, 1, *Phi, *xhat_nminus1, *xhat_n_pre);  //xhat_n_pre=Phi*xhat_nminus1
00121                     gtrack_matrixMultiply(n, n, n, *Gam, *Q, *tempMatrix);                      //Gam*Q
00122                     gtrack_matrixTransposeMultiply(n, n, n, *tempMatrix, *Gam, *tempMatrix2);   //Gam*Q*Gam'
00123                     gtrack_matrixMultiply(n, n, n, *Phi, *P_nminus1, *tempMatrix);              //Phi*P_nminus1
00124                     gtrack_matrixTransposeMultiply(n, n, n, *tempMatrix, *Phi, *tempMatrix3);   //Phi*P_nminus1*Phi'
00125                     gtrack_matrixAdd(n, n, *tempMatrix2, *tempMatrix3, *P_n_pre);               //P_n_pre=Phi*P_nminus1*Phi'+Gam*Q*Gam'
00126                     gtrack_matrixMultiply(1, 2, 1, *C, *xhat_n_pre, *yhat_n);
00127                     //pc.printf("yhat_n: %.4f\n\r", yhat_n[0][0]);
00128 
00129                     gtrack_matrixMultiply(1, 2, 2, *C, *P_n_pre, *tempVector);
00130                     gtrack_matrixTransposeMultiply(1, 2, 1, *tempVector, *C, *tempScalar);
00131                     S = tempScalar[0][0] + R;
00132                     if ((S >= -1*epsilon) && (S <= 1*epsilon)) {
00133                         pc.printf("Alert!!! S is very small %.8f \n\r", S);
00134                         if (S > 0) {
00135                             S = epsilon;
00136                         } else {
00137                             S = -1*epsilon;
00138                         }
00139                     }
00140 
00141                     gtrack_matrixTransposeMultiply(2, 2, 1, *P_n_pre, *C, *tempVector2);
00142                     gtrack_matrixScalarMultiply(2, 1, *tempVector2, 1/S, *KFGain);
00143                     //pc.printf("S: %.5f \t KFGain: %.4f, %.4f\n\r", S, KFGain[0][0], KFGain[1][0]);
00144 
00145                     gtrack_matrixMultiply(2, 1, 2, *KFGain, *C, *tempMatrix);
00146                     gtrack_matrixSub(n, n, *Eye, *tempMatrix, *tempMatrix2);
00147                     gtrack_matrixMultiply(n,n,n, *tempMatrix2, *P_n_pre, *P_n);
00148 
00149                     gtrack_matrixMultiply(1,2,1, *C, *xhat_n_pre, *tempScalar); //C*xhat_n_pre
00150                     tempScalar[0][0] = y - tempScalar[0][0];
00151                     gtrack_matrixScalarMultiply(2, 1, *KFGain, tempScalar[0][0], *tempVector2);
00152                     gtrack_matrixAdd(2,1, *xhat_n_pre, *tempVector2, *xhat_n);  // xhat_n=xhat_n_pre+KFGain*(y-C*xhat_n_pre);
00153                     //pc.printf("xhat_n: %.4f, %.4f\n\r", xhat_n[0][0], xhat_n[1][0]);
00154 
00155                     gtrack_matrixScalarMultiply(2, 1, *xhat_n, 1, *xhat_nminus1);
00156                     gtrack_matrixScalarMultiply(2, 2, *P_n, 1, *P_nminus1);
00157                     y_minus1[0][0] = y;
00158 
00159                     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]);
00160                     dt = T-(t.read()-currTime);
00161                     if (dt > 0) {
00162                         thread_sleep_for(dt*1000);
00163                     }
00164                     break;
00165                 }
00166             }
00167         }
00168     }//while(1)
00169 }//main