Diff: NODE-KF-2-v2-noise.cpp

Revision:

1:5794ff4efa9a

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