Erick Rodriguez / Mbed OS USNA-UMBC-KF-02_v3-noise

USNA-UMBC-Project Receiver - Add noise to CAN-bus received data and Implement Kalman Filter

Dependencies:   ServoOut mcp2515 BNO055

NODE-KF-2-v2-noise.cpp@1:5794ff4efa9a, 23 months ago (annotated)

Committer:
professorrodriguezse
Date:
Fri May 20 18:37:10 2022 +0000
Revision:
1:5794ff4efa9a
USNA-UMBC-KalmanFilter-ReceiverSide

Who changed what in which revision?

UserRevisionLine numberNew contents of line
professorrodriguezse 1:5794ff4efa9a 1 /*
professorrodriguezse 1:5794ff4efa9a 2 Sends and Reads position of servos in degrees and prints them all.
professorrodriguezse 1:5794ff4efa9a 3 */
professorrodriguezse 1:5794ff4efa9a 4
professorrodriguezse 1:5794ff4efa9a 5 #include "mbed.h"
professorrodriguezse 1:5794ff4efa9a 6 #include "platform/mbed_thread.h"
professorrodriguezse 1:5794ff4efa9a 7 //#include "BNO055.h"
professorrodriguezse 1:5794ff4efa9a 8 #include "CAN3.h"
professorrodriguezse 1:5794ff4efa9a 9 //#include "ServoOut.h"
professorrodriguezse 1:5794ff4efa9a 10 #include "gtrackMatrix.c"
professorrodriguezse 1:5794ff4efa9a 11 #include "time.h"
professorrodriguezse 1:5794ff4efa9a 12
professorrodriguezse 1:5794ff4efa9a 13 int myID = 2;
professorrodriguezse 1:5794ff4efa9a 14
professorrodriguezse 1:5794ff4efa9a 15 Serial pc(USBTX, USBRX); //pc serial (tx, rx) uses USB PA_9 and PA_10 on Nucleo D1 and D0 pins
professorrodriguezse 1:5794ff4efa9a 16 //BNO055 bno(D4, D5);
professorrodriguezse 1:5794ff4efa9a 17 SPI spi(D11, D12, D13); // mosi, miso, sclk
professorrodriguezse 1:5794ff4efa9a 18 CAN3 can3(spi, D10, D2); // spi bus, CS for MCP2515 controller
professorrodriguezse 1:5794ff4efa9a 19 //ServoOut servoOut1(A0); //A0); // PA_0 is the servo output pulse
professorrodriguezse 1:5794ff4efa9a 20
professorrodriguezse 1:5794ff4efa9a 21 CANMessage canTx_msg;
professorrodriguezse 1:5794ff4efa9a 22 CANMessage canRx_msg;
professorrodriguezse 1:5794ff4efa9a 23
professorrodriguezse 1:5794ff4efa9a 24 Timer t;
professorrodriguezse 1:5794ff4efa9a 25
professorrodriguezse 1:5794ff4efa9a 26 int main()
professorrodriguezse 1:5794ff4efa9a 27 {
professorrodriguezse 1:5794ff4efa9a 28 srand(time(0));
professorrodriguezse 1:5794ff4efa9a 29 thread_sleep_for(500);
professorrodriguezse 1:5794ff4efa9a 30 float currTime, dt, ytrue, y;
professorrodriguezse 1:5794ff4efa9a 31 float T = 0.017; //s
professorrodriguezse 1:5794ff4efa9a 32 t.start();
professorrodriguezse 1:5794ff4efa9a 33 pc.baud(115200);
professorrodriguezse 1:5794ff4efa9a 34 pc.printf("Starting Program... \n\r");
professorrodriguezse 1:5794ff4efa9a 35 //can3.reset(); // reset the can bus interface
professorrodriguezse 1:5794ff4efa9a 36 can3.frequency(500000); // set up for 500K baudrate
professorrodriguezse 1:5794ff4efa9a 37 char msg_send[8];
professorrodriguezse 1:5794ff4efa9a 38 char msg_read_char[8];
professorrodriguezse 1:5794ff4efa9a 39 thread_sleep_for(1000);
professorrodriguezse 1:5794ff4efa9a 40
professorrodriguezse 1:5794ff4efa9a 41 int n = 2; //# states
professorrodriguezse 1:5794ff4efa9a 42 int p = 2; //# outouts
professorrodriguezse 1:5794ff4efa9a 43 int rows=n;
professorrodriguezse 1:5794ff4efa9a 44 int m=2;
professorrodriguezse 1:5794ff4efa9a 45 int cols=n;
professorrodriguezse 1:5794ff4efa9a 46
professorrodriguezse 1:5794ff4efa9a 47 float A[2][2]= {{0.0,1.0},{0.0,0.0}};
professorrodriguezse 1:5794ff4efa9a 48 float B[2][2]= {{0.0,0.0},{0.0,1.0}};
professorrodriguezse 1:5794ff4efa9a 49 float C[1][2]= {{1.0,0.0}};
professorrodriguezse 1:5794ff4efa9a 50 float Phi[n][n];
professorrodriguezse 1:5794ff4efa9a 51 float Eye[n][n];
professorrodriguezse 1:5794ff4efa9a 52 float tempMatrix[n][n];
professorrodriguezse 1:5794ff4efa9a 53 float tempMatrix2[n][n];
professorrodriguezse 1:5794ff4efa9a 54 float tempMatrix3[n][n];
professorrodriguezse 1:5794ff4efa9a 55 float tempMatrix4[n][n];
professorrodriguezse 1:5794ff4efa9a 56 gtrack_matrixEye(n, *Eye);
professorrodriguezse 1:5794ff4efa9a 57 gtrack_matrixScalarMultiply(rows, cols, *A, T, *tempMatrix);
professorrodriguezse 1:5794ff4efa9a 58 gtrack_matrixAdd(rows, cols, *Eye, *tempMatrix, *Phi);
professorrodriguezse 1:5794ff4efa9a 59
professorrodriguezse 1:5794ff4efa9a 60 float Gam[n][n];
professorrodriguezse 1:5794ff4efa9a 61 gtrack_matrixScalarMultiply(rows, cols, *B, T, *tempMatrix);
professorrodriguezse 1:5794ff4efa9a 62 gtrack_matrixScalarMultiply(rows, cols, *B, T*T/2, *tempMatrix2);
professorrodriguezse 1:5794ff4efa9a 63 gtrack_matrixMultiply(rows, m, cols, *A, *tempMatrix2, *tempMatrix3);
professorrodriguezse 1:5794ff4efa9a 64 gtrack_matrixAdd(rows, cols, *tempMatrix, *tempMatrix3, *Gam);
professorrodriguezse 1:5794ff4efa9a 65 pc.printf("\n\r Gamma: \n\r");
professorrodriguezse 1:5794ff4efa9a 66 gtrack_matrixPrint(rows, cols, *Gam);
professorrodriguezse 1:5794ff4efa9a 67 //pc.printf("%.4f \t %.4f \t %.4f \t %.4f\n\r",Gam[0][0],Gam[0][1],Gam[1][0],Gam[1][1]);
professorrodriguezse 1:5794ff4efa9a 68
professorrodriguezse 1:5794ff4efa9a 69 float sig_w=1; //Measurement noise parameter
professorrodriguezse 1:5794ff4efa9a 70 float sig_v=50; //Trial process noise parameters
professorrodriguezse 1:5794ff4efa9a 71 float Q[n][n];
professorrodriguezse 1:5794ff4efa9a 72 float R = sig_w*sig_w;
professorrodriguezse 1:5794ff4efa9a 73 gtrack_matrixScalarMultiply(rows, cols, *Eye, sig_v*sig_v, *Q);
professorrodriguezse 1:5794ff4efa9a 74 float hatx_0[2][1] = {{0},{0}};
professorrodriguezse 1:5794ff4efa9a 75 float varx_0[n][n];
professorrodriguezse 1:5794ff4efa9a 76 float P_0[n][n];
professorrodriguezse 1:5794ff4efa9a 77 gtrack_matrixScalarMultiply(rows, cols, *Q, 1, *varx_0);
professorrodriguezse 1:5794ff4efa9a 78 gtrack_matrixScalarMultiply(rows, cols, *varx_0, 1, *P_0);
professorrodriguezse 1:5794ff4efa9a 79 pc.printf("P_0: \n\r");
professorrodriguezse 1:5794ff4efa9a 80 gtrack_matrixPrint(rows, cols, *P_0);
professorrodriguezse 1:5794ff4efa9a 81 //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]);
professorrodriguezse 1:5794ff4efa9a 82
professorrodriguezse 1:5794ff4efa9a 83 float xhat_nminus1[n][1];
professorrodriguezse 1:5794ff4efa9a 84 gtrack_matrixScalarMultiply(n, 1, *hatx_0, 1, *xhat_nminus1);
professorrodriguezse 1:5794ff4efa9a 85 float P_nminus1[n][n];
professorrodriguezse 1:5794ff4efa9a 86 gtrack_matrixScalarMultiply(n, n, *P_0, 1, *P_nminus1);
professorrodriguezse 1:5794ff4efa9a 87 float y_minus1[1][1];
professorrodriguezse 1:5794ff4efa9a 88 gtrack_matrixMultiply(1, 2, 1, *C, *hatx_0, *y_minus1);
professorrodriguezse 1:5794ff4efa9a 89 pc.printf("y_minus1: \n\r");
professorrodriguezse 1:5794ff4efa9a 90 gtrack_matrixPrint(1, 1, *y_minus1);
professorrodriguezse 1:5794ff4efa9a 91
professorrodriguezse 1:5794ff4efa9a 92 float xhat_n_pre[n][1];
professorrodriguezse 1:5794ff4efa9a 93 float P_n_pre[n][n];
professorrodriguezse 1:5794ff4efa9a 94 float yhat_n[1][1];
professorrodriguezse 1:5794ff4efa9a 95 float S;
professorrodriguezse 1:5794ff4efa9a 96 float epsilon = 0.00001;
professorrodriguezse 1:5794ff4efa9a 97 int count = 0;
professorrodriguezse 1:5794ff4efa9a 98
professorrodriguezse 1:5794ff4efa9a 99 float tempVector[1][n];
professorrodriguezse 1:5794ff4efa9a 100 float tempScalar[1][1];
professorrodriguezse 1:5794ff4efa9a 101 float KFGain[n][1];
professorrodriguezse 1:5794ff4efa9a 102 float tempVector2[n][1];
professorrodriguezse 1:5794ff4efa9a 103 float P_n[n][n];
professorrodriguezse 1:5794ff4efa9a 104 float xhat_n[n][1];
professorrodriguezse 1:5794ff4efa9a 105 pc.printf("Sample: \t Time: \t Yaw: \t Yaw+Noise: \t Estimate: \t KFGain: (1) and (2)\n\r");
professorrodriguezse 1:5794ff4efa9a 106 while(1) {
professorrodriguezse 1:5794ff4efa9a 107 currTime = t.read();
professorrodriguezse 1:5794ff4efa9a 108 count = count +1;
professorrodriguezse 1:5794ff4efa9a 109 if (count > 2000) {
professorrodriguezse 1:5794ff4efa9a 110 break;
professorrodriguezse 1:5794ff4efa9a 111 }
professorrodriguezse 1:5794ff4efa9a 112 while(1) {
professorrodriguezse 1:5794ff4efa9a 113 if(can3.read(&canRx_msg) == CAN_OK) {
professorrodriguezse 1:5794ff4efa9a 114 if(canRx_msg.id == 1) {
professorrodriguezse 1:5794ff4efa9a 115 for (int i = 0; i < 8; i++) {
professorrodriguezse 1:5794ff4efa9a 116 msg_read_char[i] = (char)canRx_msg.data[i];
professorrodriguezse 1:5794ff4efa9a 117 }
professorrodriguezse 1:5794ff4efa9a 118 sscanf(msg_read_char, "%f", &ytrue);
professorrodriguezse 1:5794ff4efa9a 119 y = ytrue + ((rand() % 100) - 50)/10;
professorrodriguezse 1:5794ff4efa9a 120 gtrack_matrixMultiply(n, n, 1, *Phi, *xhat_nminus1, *xhat_n_pre); //xhat_n_pre=Phi*xhat_nminus1
professorrodriguezse 1:5794ff4efa9a 121 gtrack_matrixMultiply(n, n, n, *Gam, *Q, *tempMatrix); //Gam*Q
professorrodriguezse 1:5794ff4efa9a 122 gtrack_matrixTransposeMultiply(n, n, n, *tempMatrix, *Gam, *tempMatrix2); //Gam*Q*Gam'
professorrodriguezse 1:5794ff4efa9a 123 gtrack_matrixMultiply(n, n, n, *Phi, *P_nminus1, *tempMatrix); //Phi*P_nminus1
professorrodriguezse 1:5794ff4efa9a 124 gtrack_matrixTransposeMultiply(n, n, n, *tempMatrix, *Phi, *tempMatrix3); //Phi*P_nminus1*Phi'
professorrodriguezse 1:5794ff4efa9a 125 gtrack_matrixAdd(n, n, *tempMatrix2, *tempMatrix3, *P_n_pre); //P_n_pre=Phi*P_nminus1*Phi'+Gam*Q*Gam'
professorrodriguezse 1:5794ff4efa9a 126 gtrack_matrixMultiply(1, 2, 1, *C, *xhat_n_pre, *yhat_n);
professorrodriguezse 1:5794ff4efa9a 127 //pc.printf("yhat_n: %.4f\n\r", yhat_n[0][0]);
professorrodriguezse 1:5794ff4efa9a 128
professorrodriguezse 1:5794ff4efa9a 129 gtrack_matrixMultiply(1, 2, 2, *C, *P_n_pre, *tempVector);
professorrodriguezse 1:5794ff4efa9a 130 gtrack_matrixTransposeMultiply(1, 2, 1, *tempVector, *C, *tempScalar);
professorrodriguezse 1:5794ff4efa9a 131 S = tempScalar[0][0] + R;
professorrodriguezse 1:5794ff4efa9a 132 if ((S >= -1*epsilon) && (S <= 1*epsilon)) {
professorrodriguezse 1:5794ff4efa9a 133 pc.printf("Alert!!! S is very small %.8f \n\r", S);
professorrodriguezse 1:5794ff4efa9a 134 if (S > 0) {
professorrodriguezse 1:5794ff4efa9a 135 S = epsilon;
professorrodriguezse 1:5794ff4efa9a 136 } else {
professorrodriguezse 1:5794ff4efa9a 137 S = -1*epsilon;
professorrodriguezse 1:5794ff4efa9a 138 }
professorrodriguezse 1:5794ff4efa9a 139 }
professorrodriguezse 1:5794ff4efa9a 140
professorrodriguezse 1:5794ff4efa9a 141 gtrack_matrixTransposeMultiply(2, 2, 1, *P_n_pre, *C, *tempVector2);
professorrodriguezse 1:5794ff4efa9a 142 gtrack_matrixScalarMultiply(2, 1, *tempVector2, 1/S, *KFGain);
professorrodriguezse 1:5794ff4efa9a 143 //pc.printf("S: %.5f \t KFGain: %.4f, %.4f\n\r", S, KFGain[0][0], KFGain[1][0]);
professorrodriguezse 1:5794ff4efa9a 144
professorrodriguezse 1:5794ff4efa9a 145 gtrack_matrixMultiply(2, 1, 2, *KFGain, *C, *tempMatrix);
professorrodriguezse 1:5794ff4efa9a 146 gtrack_matrixSub(n, n, *Eye, *tempMatrix, *tempMatrix2);
professorrodriguezse 1:5794ff4efa9a 147 gtrack_matrixMultiply(n,n,n, *tempMatrix2, *P_n_pre, *P_n);
professorrodriguezse 1:5794ff4efa9a 148
professorrodriguezse 1:5794ff4efa9a 149 gtrack_matrixMultiply(1,2,1, *C, *xhat_n_pre, *tempScalar); //C*xhat_n_pre
professorrodriguezse 1:5794ff4efa9a 150 tempScalar[0][0] = y - tempScalar[0][0];
professorrodriguezse 1:5794ff4efa9a 151 gtrack_matrixScalarMultiply(2, 1, *KFGain, tempScalar[0][0], *tempVector2);
professorrodriguezse 1:5794ff4efa9a 152 gtrack_matrixAdd(2,1, *xhat_n_pre, *tempVector2, *xhat_n); // xhat_n=xhat_n_pre+KFGain*(y-C*xhat_n_pre);
professorrodriguezse 1:5794ff4efa9a 153 //pc.printf("xhat_n: %.4f, %.4f\n\r", xhat_n[0][0], xhat_n[1][0]);
professorrodriguezse 1:5794ff4efa9a 154
professorrodriguezse 1:5794ff4efa9a 155 gtrack_matrixScalarMultiply(2, 1, *xhat_n, 1, *xhat_nminus1);
professorrodriguezse 1:5794ff4efa9a 156 gtrack_matrixScalarMultiply(2, 2, *P_n, 1, *P_nminus1);
professorrodriguezse 1:5794ff4efa9a 157 y_minus1[0][0] = y;
professorrodriguezse 1:5794ff4efa9a 158
professorrodriguezse 1:5794ff4efa9a 159 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]);
professorrodriguezse 1:5794ff4efa9a 160 dt = T-(t.read()-currTime);
professorrodriguezse 1:5794ff4efa9a 161 if (dt > 0) {
professorrodriguezse 1:5794ff4efa9a 162 thread_sleep_for(dt*1000);
professorrodriguezse 1:5794ff4efa9a 163 }
professorrodriguezse 1:5794ff4efa9a 164 break;
professorrodriguezse 1:5794ff4efa9a 165 }
professorrodriguezse 1:5794ff4efa9a 166 }
professorrodriguezse 1:5794ff4efa9a 167 }
professorrodriguezse 1:5794ff4efa9a 168 }//while(1)
professorrodriguezse 1:5794ff4efa9a 169 }//main