In the midst of going through Autumn Term at Imperial with courses of varying relevance to my future and applying for Masters programs, I decided to finally take a look at what Kalman Filtering.

I was already familiar with Cyrill Stachniss’ (very helpful) content on Robotics and Computer Vision from watching his videos on ICP and SIFT for my internship, so I was pleasantly surprised to see that he also had a video on Kalman Filtering.


The video introduced the mathematical basis of Kalman filtering as a probabalistic model of robot state. It assumes that robot states are modelled as Gaussian distributions - there is always some degree of uncertainty in where the robot (thinks it) is at any time.
Gaussian distributions are neat as they lend themselves to the various mathematical tricks that are used through the Kalman Filter.

Somewhat logically, a Kalman filter uses the previous state \(x_{t-1}\) and the current control input \(u_t\), along with allowance for external uncertainty \(\epsilon_t\) to come up with a Prediction of the current robot state, \(x_t\).

Subsequently, the filter uses observations about its surroundings \(z_t\) in the Correction step, allowing for uncertainty in measurement \(\delta_t\). The interesting part about the filter is how it comes up with the weighted average of its Predictions and Observations based on their covariances - a value with a greater covariance is trusted less.

The video also introduced pseudocode for a Kalman filter, which helped to anchor the more theoretical maths aspects to a practical implementation I could visualise coding up. Essentially:

def kalman_filt(prev_mean, prev_variance, curr_control, curr_observation):
    # Prediction Step 
    predicted_mean = A_t @ prev_mean + B_T @ curr_control
    predicted_variance = A_t @ prev_variance @ A_t.T + R_t

    # Correction Step
    K_t = predicted_variance @ C_t.T @ inverse(C_t @ predicted_variance @ C_t.T + Q_t)  # K_t, the kalman gain, determines how much to trust the predicted variance.
    curr_mean = predicted_mean + K_t @ (curr_observation - C_t @ predicted_mean)
    curr_variance = (I - K_t @ C_t) @ predicted_variance

    return curr_mean, curr_variance

Where @ denotes matrix multiplication.
A_t denotes how the previous state \(x_{t-1}\) may translate into the current state \(x_t\).
B_t denotes how the current control input \(u_t\) may modify the current state \(x_t\).
C_t denotes how the current state \(x_t\) may be transformed into the current observation \(z_t\).
R_t and Q_t are covariance matrices corresponding to the random variables \(\epsilon_t\) and \(\delta_t\) for uncertainty in process and observations respectively. As they are assumed to be zero-mean Gaussianv variables, a covariance matrix is able to represent them.

The algorithm then returns its prediction of the current robot state as a Gaussian distribution - defined by it’s mean and variance.

I was pleasantly surprised to be engaged and able to follow through the entire lecture in a single sitting, even though involved some probability and linear algebra - not my favorite topics by any means - and can’t wait to apply the more practical aspects of an (Extended) Kalman Filter to some real-world robots!