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
- Committer:
- professorrodriguezse
- Date:
- 2022-05-20
- Revision:
- 1:5794ff4efa9a
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