A simple Kalman Filter
For a static process

Whenever you have a sensor collecting raw data from the environment, the recieved measurement values are going to have noise in them no matter how well designed your sensor is.
A Kalman filter is a crucial part of the control loop.
In the most basic sense it tries to estimate the unknown readings by weighing the inaccurate and noisy sensor readings and the past estimations.

For how powerful a Kalman filter is, its implementation is surprisingly simple.

In this post I am going to show you haw to implement a Kalman filter from scratch on python(using only numpy) and also how to use it with a simulated noisy GPS sensor in a 2D world to determine our coordinates.

First, let us get right to the point and make a class that implements the filter for a simple static model.

import numpy as np
class KalmanFilter(object):
    def __init__(self, X=None, D=None, H=None, Q=None, R=None, P = None, n = None):
        self.D = np.array(D) # the linear dynamics model
        self.Q = np.array(Q) # process noise covariance matrix
        self.R = np.array(R) # Measurement noise 
        self.P = np.array(P) # Initial Uncertainity
        self.H = np.array(H) # measurement matrix
        self.X = np.array(X) # intial guess

    def predict(self):
        self.X = np.dot(self.D, self.X) # this will predict the value of the sensor at the next time step
        self.P = np.dot(self.D, np.dot(self.P, self.D.T)) + self.Q # this will calculate the uncertainity of the prediction
        return self.X, self.P
    
    def update(self, Y):
        # Y is the measurement vector(raw sensor data)
        predicted_measurement = np.dot(self.H, self.X) # multipication by the mesurement matrix converts it into the form we need it for further analysis 
        kalman_gain = np.dot(self.P, np.dot(self.H.T, np.linalg.inv(self.R + np.dot(self.H, np.dot(self.P, self.H.T))))) # kalman gain can be understood as the 
        #factor that weighs between the measurement uncertainity and the process uncertainity
        self.X = self.X + np.dot(kalman_gain, (Y-predicted_measurement)) # calculates the value of the filter after filtering
        self.P = self.P - np.dot(kalman_gain, np.dot(self.R + np.dot(self.H, np.dot(self.P, self.H.T)), kalman_gain.T)) # calculates the uncertainity after filtering
        return self.X, self.P, kalman_gain

Now that we have the core of the operation sorted, we proceed towards generating simulated sensor data to see how our filter works.

import numpy as np

iters = 1000

GPS_ref_mean = [10, 15] # true position of our static state
GPS_ref_cov = [[5, 0], [0, 5]] # the uncertainity in our measurement (this depends on the sensor and may need to be estimated with some raw data collected)

# Noisy sensor data
# simulating the measurements with gaussian noise
GPS_ref_x, GPS_ref_y = np.random.multivariate_normal(GPS_ref_mean, GPS_ref_cov, iters).T # this will create 1000 measurement values

We make an intitial prediction (can be random and inaccurate) about our state and feed the estimate and the raw sensor data into the filter.

x = [5, 5] # initialize the intital state with any random value
n = np.size(x) # get the dimenstions of the state variable
D = [[1,0], [0,1]] # the linear dynamics model (an identity matrix in our case since we have a static model)
M = [[1,0], [0,1]] # measurement matrix
sigma_d = np.zeros((n, n)) # model uncertainity
sigma_initial = np.eye(n) * 500 # a huge covariance here because our initialization is random

# creating an instance
kf_ref = KalmanFilter(x, D, M, sigma_d, GPS_ref_cov, sigma_initial, n)

#to store the values for later plotting
predictions_ref_state = np.empty((np.size(GPS_ref_x), 2))
corrections_ref_state = np.empty((np.size(GPS_ref_x), 2))
time = [] # required only for plotting
for i in range(np.size(GPS_ref_x)):
    time.append(i)
    predictions, _ = kf_ref.predict()
    predictions_ref_state[i,0] = predictions[0]
    predictions_ref_state[i,1] = predictions[1]
    corrections, _, gainzz = kf_ref.update([GPS_ref_x[i], GPS_ref_y[i]])
    corrections_ref_state[i,0] = corrections[0]
    corrections_ref_state[i,1] = corrections[1]

Finally we plot what we generated

import matplotlib.pyplot as plt
plt.plot(GPS_ref_x, time, label='Measurement')
plt.plot(corrections_ref_state[:,0], time, label='Kalman')
plt.title('A simple Kalman filter implementation')
plt.legend()
plt.show()

Once plotted, the graph should look like this (only one of the 2 dimensions is plotted for better clarity)

you will see that the orange line has barely any deviation from our true position. It may not look that ‘amazing’ given our simple static process but, in a dynamic process this almost looks magical.

Perhaps I’ll cover that some other day.

Things you need to consider before actually incorporating it into your application:

Ideas? comments? suggestions for improvement?
Feel free to reach me on my E-mail

*****
"A chance not taken is an opportunity missed."

☕