In this article, I’m going to present the details of Kalman filtering which is one of the most important subjects in engineering. It is also essential for our computer vision project. After giving some information about the method, I will share the python codes at the end. In this project, we will use the Kalman filtering for the following purposes:
- Visual object tracking
- Position and velocity estimation
The kalman filter is used in a wide variety of fields such as control theory, computer vision, radar engineering, econometry, etc. It is a state estimator or observer which estimates the internal state of a dynamic system. The number of observable states of the most systems is generally less than the number of actual states. For instance, assume that you are tracking a moving object in a video. Whether it is noisy or not, you can directly observe its position by looking at the center pixel values. However, it also has some other states such as velocity and acceleration. Those are called hidden states which are possible to observe by means of Kalman filtering. There are some other state observers in the literature, such as extended Kalman filter (EKF), unscented Kalman filter (UKF), high gain observer (HGO). Those are typical state observers for non-linear systems. In this article, we will discuss the linear version of the Kalman filter. The following figure shows a typical implementation of Kalman filter for control systems.
As we see in the figure, the filter is connected to both input and output of the system. However, in some other cases, such as visual tracking, there could be only one branch which is connected to the output. This can be seen in the figure below.
For 2D (x,y) linear motion model, one can construct the set of equations as follows:
Here, we simply assume that our model is represented by the equation X(k+1) = F*X(k) + W(k) where X represents the state vector, F stands for state transition matrix, and W represents the independent identically distributed process noise samples. For the measurement part, we have Z(k) = H*X(k) + V(k) where Z is the measurement vector, H is the measurement matrix, and finally V is the vector that holds the independent identically distributed measurement noise samples. The following figure shows the 2D (x,y) measurement equations.
In the equations, Q and R are process and measurement covariance matrices, respectively. Now, it is time to implement the Kalman filtering technique in python. Here, we use a library filterpy which already has the implementation of Kalman filter. You can install this library in your conda prompt by typing ‘pip install filterpy’. The code that we provide does the required initializations which are quite important. Now, in your working folder, create a file with name ‘kalmanFilter.py’. Copy and paste the following lines of codes.
from filterpy.kalman import KalmanFilter import numpy as np class kalmanFilter(): def __init__(self, state_dim, measurement_dim, dt, Q, R): self.kalman = KalmanFilter(dim_x=state_dim, dim_z=measurement_dim) #state variables self.kalman.x = np.zeros((state_dim,1)) self.kalman.F = np.eye(state_dim,state_dim) L = int(state_dim/2) temp = np.eye(L,L)*dt self.kalman.F[0:L,L:] = temp self.kalman.H = np.zeros((measurement_dim,state_dim)) self.kalman.H[0:measurement_dim,0:measurement_dim] = np.eye(measurement_dim,measurement_dim) self.kalman.P = np.eye(state_dim,state_dim)*100 self.kalman.R = np.eye(measurement_dim,measurement_dim)*R self.kalman.Q = np.eye(state_dim,state_dim)*Q self.kalman.Q[0:L,0:L] = np.zeros((L,L)) def predict(self, z): #z: measurements self.kalman.predict() self.kalman.update(z) return self.kalman.x.T
That is it! Your kalman filter is ready to take action. Enjoy your filtering!