# What is Kalman filter?

This post’s subject is Kalman filter. It is a very useful tool in control systems, where there is no certainty in system’s information.

### What is it?

Kalman filter is an optimization algorithm to estimate the state of a system with noise and uncertainties. This filter receives unprecise measures with noise, it is able to estimate current state with good precision and make a prediction of future state.

### Why use Kalman filters?

Exist situations where a desired value cant be measured directly, some examples: Temperature of an airplane engine or a rockets combustion engine. If the temperature is too high, mechanical parts will be damaged. It isnt possible to measure temperature $T_{in}$ directly with a sensor, because it doesnt stand high temperatures. So the sensor must be positioned in a cooler place and temperature must be measured indirectly $T_{ext}$. And then, enters Kalman filter, obtaining an estimate of true temperature $T_{in}$ from $T_{ext}$. Also can be used to estimate the state from measurement of many sensors. For example, a car or drone has GPS, inertial measurement unit (IMU) and odometer, which measures relative speed. The sensors are subjected to noise and errors. Kalman filter uses measures to get an optimal estimative of position and speed of vehicle or drone. ### How it works?

In this part is explained an introduction of standard Kalman filter (KF), which works only with linear equations. Other filter variations will be subject to future posts.

First it is necessary to create a mathematical model of system, however this is only an approximation of real system. Supposing that u is the systems input, y and $\widehat{y}$ are outputs of real system and model respectively. While x and $\widehat{x}$ are variable vectors of real system and model respectively which have to estimate. The outlined part in red is state observer. Calculation of error $e$.

$e=x-\hat{x}$

A, B and C are matrices. Kalman filter must have a discrete time counter k. To improve precision, the process noise $w_{k}$ and random variable $v_{k}$ must be considered. $v_{k}$ and $w_{k}$ are gaussian curves. A gaussian curve has the shape shown below, in this example, mean $\mu$ is 0 and standard deviation $\sigma$ is 1. $v_{k}$ and $w_{k}$ have zero mean and covariance matrices $R_{k}$ and $Q_{k}$ respectively.

#### Algorithm

Kalman filter has to combine $\widehat{x}_{k}$ and $y_{k}$ to get the best estimate. Algorithm has two steps: prediction, where states are estimated with uncertainties and update, measuring part with noise. These are prediction equations.

$\hat{x}_{k}^{-}=A\hat{x}_{k-1}+Bu_{k}$

$P_{k}^{-}=AP_{k-1}A^{T}+Q$

• $\hat{x}_{k}^{-}$ is estimate vector a priori, before measurement.
• $P_{k}^{-}$ is the covariance error a priori, uncertainly measurement in measured state.
• $\hat{x}_{k-1}$ and $P_{k-1}$ are values which must be estimated in the beginning of algorithm with k=0.

The next step is update part and these are the equations.

$K_{k}=\frac{P_{k}^{-}C^{T}}{CP_{k}^{-}C^{T}+R}$

$\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}(y_{k}-C\hat{x}_{k}^{-})$

$P_{k}=(I-K_{k}C)P_{k}^{-}$

$\hat{x}_{k}^{-}$ and $P_{k}^{-}$ are used to calculate Kalman gain $K_{k}$ and update a posteriori values of $\hat{x}_{k}$ and $P_{k}$. Kalman gain indicates the influence of measurement $y_{k}$ and a priori estimate $\hat{x}_{k}^{-}$ to calculate $\hat{x}_{k}$. If R tends to zero, limit of Kalman gain is 1, because in most cases matrix C is 1.

$\lim_{R\rightarrow 0}K_{k}=C^{-1}$

$\hat{x}_{k}=y_{k}$

In contrast, if covariance error $P_{k}^{-}$ tends to zero, Kalman gain will also be zero.

$\lim_{P_{k}^{-}\rightarrow 0}K_{k}=0$

$\hat{x}_{k}=\hat{x}_{k}^{-}$

Add +1 at discrete time k, the next prediction to next instant is made and the cycle continues. Some sources exchange C for H and put z as measurement vector. ### Applications in robotics

Autonomous robots navigating in an environment full of obstacles, need to predict future position and speed to plan a track and arrive at right position. In addition to that, use many sensors showing unperfect mesures in current state. This filter will be very useful in driverless cars, passenger will indicate the local where wants to go and the car will plan a trajectory considering distance and traffic jam. Also has application in machine vision to know an objects trajectory. 