Sensor Fusion: Extended Kalman Filter (EKF)

Overview

In this post I am going to briefly tell you about Kalman filter and one of its extensions to non-linear cases, ie. the Extended Kalman Filter (EKF). I will give a concrete example from Robotics on sensor fusion of IMU measurements and Odometry estimates from other SLAM algorithm. In robotics literature, this is referred to as loosely coupled sensor fusion. Another kind of sensor fusion (not dealt in this post) is tightly coupled sensor fusion.

Kalman filter basics
Camera + IMU : Loosely coupled sensor fusion
- States
- IMU Process model
- Camera observation model
Extended Kalman Filter

Kalman Filter

There are lots of study material out there which does an awesome job at burring students and professional alikes in a lot of equations on Kalman filter. Here, lets try and understand the essence of the kalman filter without unnecessarily going to mathematical equations. In robotics, Kalman filters are common way for sensor fusion. Please quickly watch the following video on how to merge IMU measurements and GPS measurements.

I hope the above youtube-video, gives you a practical understanding on how to use the kalman filter for sensor fusion. a) At the core it boils down to thinking about the process in terms of the state. In this case the position. b) Instead of denoting the state by a single number, in robotics it is common practice to talk about the ‘likely value and uncertainity of the measurement’, ie. the probability distribution. We assume the underlying distribution to be the Gaussian distribution. The reason we do this is that it is mathematically tractable and only needs a mean value and the covariance matrix to fully define. Moreever for a large number of processes it is a good enough assumption. c) Yet another reason is that a linear transformation of the state which is represented by a Gaussian distribution leads to a linear transform of the mean of the gaussian distribution and quadratic transformation of the variance.

Now if the we start with an initial state, $x_0$ , and suppose the next state is defined as $\textbf{x}_{n+1} = \textbf{A} \times \textbf{x}_n + \textbf{b}$ . In this case if we know the distribution of the initial state we can come up with the distribution of the next states. This is referred to as the process model., in the Kalman filter literature.

For our IMU+odometry example, state is the position, orientation of the IMU. The process model is the physical set of equation of the IMU which gives the next state. This is the character of the IMU. In a next section we will enlist the equations of the IMU. This is sometimes called IMU dead-reckoning.

Now, if by some other sensor we can independently measure the state (or partial state) we can merge these measurements. This produces a fused estimate of the state. This is referred to as the Observation model in Kalman filter literature.

Now I highly recommend to read Chp3 of the probabilistic robotics book at this state. It gives a more formal introduction to the Kalman filter.

IMU Process Model

Here, I explain and list the equations related to the IMU (Innerial Measurement Unit) for measurement of live position and orientation. Particularly we will talk about the a) states and b) the IMU process model. Note that an IMU consists of a gyroscope and an accelerometer.

IMUs are essentially gravity sensors and measure its own a) linear acceleration (3-dimensions) and b) angular velocity (3-dimensions). IMUs typically produce measurements at 100 hz (100 measurements per sec).

States

Gyroscope Measurements

angular_velocity_measured = true_angular_velocity + gyro_bias + noise

$\omega \in R^3$ . Also note, the imu measures in the body (ie. imu) frame of reference and not the world-frame. States are expressed in the world frame of reference.

[p,q,r] = angular velocity in imu frame of reference. q=[ $\phi, \theta, \psi$ ] = orientations in world frame.

[p,q,r] = angular velocity in imu frame of reference. q=[ $\phi, \theta, \psi$ ] = orientations in world frame.

Accelerometer Measurements

linear_acceleration_measured = true_linear_acceleration + accelerometer_bias + noise

Process Model

The following differential equation completely describes how the IMU’s position and orientation (state) will behave due to some starting states and the measurements. Integrating this equation essentially means the IMU deadreckoning.

Linearization

We are interested in linearization of the process-model. Linearization of a non-linear process model is the core theme of the EKF (extended kalman filter). Such techniques can be used to fuse estimates from other sensors/algorithms into a unified framework. The sucess of the EKF lies with how well the linear function is the approximation of the non-linear function. The linear function is important because in the Kalman filter the state is approximated as Gaussian distribution and this is preserved only under linear operation.

See my earlier post for a toy example of linearization of vector valued function.

I will use sympy (a python based symbolic algebra system) to deal with this.

# TODO

Continous ODE to Discrete ODE

The IMU process is represented as continous variable dynamical system, ie. as a continous ODE. However we need to discretise it for our purpose. The simplest solution is the use of Euler Integration. It states:

Given a continous ODE and an initial condition, $y'(t) = f( t, y(t) ) , \ y(t_0) = y_0$ , the discretized ODE is given by $y_{n+1} = y_n + h f( t_n, y_n )$ where $h$ is the step size.