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, 2022-05-20 (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?
User | Revision | Line number | New 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 |