A Simple Kalman Filter Implementation | by Felipe Cunha | Towards Data Science

A sensor by itself is not a solution for your measurement problems. They are intrinsically noisy and this can lead to severe issues down the road. For example, noisy data can generate unit impulses when used as input to PID controllers — the derivative of high frequency signal goes to infinite causing the actuators to saturate and the process control to go through the window.
Another situation where you need accuracy is in bulk transfers. In this case, the vendor wants to measure the volume of transferred product as precisely as possible to prevent losing money or overcharging the client.
In this article, we will demonstrate a simple example on how to develop a Kalman Filter to measure the level of a tank of water using an ultrasonic sensor.
The HC-SR04 has an acoustic receiver and transmitter. The transmitter issues a wave that travels, reflects on an obstacle and reaches the receiver. The travel time divided by two times the speed of sound, gives you the distance between the sensor and the object of interest. This sensor operational range is 2 to 400cm.
Differently from many sensors, this one doesn't require an analog port. Upon activating the ‘Trig’ pin via the digital output of your microcontroller, the sensor will emit a sound wave that will reflect and return to the sensor hitting the receiver, the ‘Echo’ pin will then go to high level. The time between the activation of ‘Trig’ and ‘Echo’ divided by two is the wave transit time.
I have some plants in my apartment’s balcony, for that reason I’ve set up a tank with a pump to water them regularly. Every time that this tank is below 30% I get an email notification from the system. If you are interested in learning how to do implement IoT you can check out this other article I wrote:
medium.com
This algorithm is divided into 2 stages, prediction and innovation.
Don’t let the algebraic symbols intimidate you, let’s break these equations down.
During the prediction, the system estimates the volume of the tank by performing a linear combination of the past estimate and the system input. In our case, the matrix F will be approximately equal to 1 (we can disregard the effect of evaporation for the sake of simplicity), so the volume of the tank at a given moment will be the volume of the past moment minus the volume pumped by the pump (my system input).
Because I’m only irrigating the plants once a day and my data is being transmitted via wi-fi, I want to minimize the payload. For this reason, we can neglect the effect of the system input ‘u’. Note however, that there is a price to pay, the dynamic response of the filter will be affected.
The second step of prediction is to estimate covariance Pk. We don’t know the real value of the tank level, the best we can do is to estimate the most probable value and assign it with the most likely variance.
In the innovation step, we will add more information and try to refine this estimate. Let’s have a look at it:
First we need to calculate the gain K. Disregard the math for this calculation now, we will simplify it.
Once calculating K, we can then calculate the ‘a posteriori’ state estimate. Note that this amount corresponds to a linear combination of the predicted state and the ‘innovation’, which is defined by the multiplication of the gain and the error.
And finally, we need to update the covariance P.
The algorithm will always get the more contribution from the component (prediction or innovation) that yields to a smaller value for P.
So, for our state observer we will have:
F = [1]B = [0]H = [1]Q = [1e-5]R = [3e-3]
So we can rewrite the equations as:
There are plenty of Kalman Filter libraries for C++ that you can use. I still think it’s useful though to write your own code just to consolidate the knowledge. In my case, I declared all variables locally so they could be accessed by the function, but there are more elegant ways to do it.
int kalman_filter(){    //prediction    x_hat_k_a_priori = x_hat_k_minus_1;    P_k_a_priori = P_k_minus_1 + Q;        //obtaining z_k: my tank height = 25.4, calculates %    digitalWrite(trigger, LOW);    delayMicroseconds(2);    digitalWrite(trigger, HIGH);    delayMicroseconds(10);    digitalWrite(trigger, LOW);    duration = pulseIn(echo, HIGH);    z_k = (25.4-duration*0.017)/25.4;         //innovation     K_k = P_k_a_priori * (P_k_a_priori + R);    x_hat_k = x_hat_k_a_priori + K_k * (z_k - x_hat_k_a_priori);    P_k = (1 - K_k) * P_k_a_priori;        return x_hat_k;}
The image below shows the results obtained. Note that the time series is smooth.
The large sudden increase in volume is due to the the tank refill. The small decreases happens because of the pump activation.
The KF Algorithm was implemented and tested to acquire data from the ultrasonic sensor HC-SR04. The test results show that the algorithm is able to reduce noise significantly.
Kalman Filters can be used in a wide range of applications like sensor fusion, state estimation of unaccessible variables or even stock market prediction.
If you like this article or would like to share your thoughts don’t hesitate to leave your comment down below.