├── README.md
└── hc-sr04_kalman_filter
    └── hc-sr04_kalman_filter.ino


/README.md:
--------------------------------------------------------------------------------
1 | # Ultrasonic Sensor HC-SR04 with Kalman Filter  
2 | Arduino code for ultrasonic sensor HC-SR04 with kalman filter implementation
3 | 


--------------------------------------------------------------------------------
/hc-sr04_kalman_filter/hc-sr04_kalman_filter.ino:
--------------------------------------------------------------------------------
 1 | /* credits to VDEngineering video for the kalman filter algorithm
 2 |  *  youtube video: https://www.youtube.com/watch?v=ruB917YmtgE
 3 |  */
 4 | 
 5 | // ULTRASONIC //
 6 | // Pins
 7 | const int echo = 2;
 8 | const int trig = 3;
 9 | // Constrains
10 | const int maxdist = 335;
11 | const float mindist = 2.5;
12 | 
13 | double distance, duration;
14 | double kaldist;
15 | 
16 | double kalman(double U){
17 |   static const double R = 40;
18 |   static const double H = 1.00;
19 |   static double Q = 10;
20 |   static double P = 0;
21 |   static double U_hat = 0;
22 |   static double K = 0;
23 |   K = P*H/(H*P*H+R);
24 |   U_hat += + K*(U-H*U_hat);
25 |   P = (1-K*H)*P+Q;
26 |   return U_hat;
27 | }
28 | 
29 | void setup(){
30 |   pinMode(trig, OUTPUT);
31 |   pinMode(echo, INPUT);
32 |   Serial.begin(9600);
33 | }
34 | 
35 | void usonic_transmit() {
36 |   digitalWrite(trig, HIGH);
37 |   delayMicroseconds(10);
38 |   digitalWrite(trig, LOW);
39 |   delayMicroseconds(2);
40 | }
41 | 
42 | void loop(){
43 |   usonic_transmit();
44 |   
45 |   duration = pulseIn(echo, HIGH);
46 |   distance = (duration*.034)/2;
47 |   kaldist = kalman(distance);
48 | 
49 |   Serial.print("Distance (in cm): %f", distance);
50 |   Serial.print("Corrected distance (in cm): %f", kaldist);
51 |   
52 |   delay(1/30);
53 | }
54 | 


--------------------------------------------------------------------------------