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 can`t be measured directly, some examples: Temperature of an airplane engine or a rocket`s combustion engine. If the temperature is too high, mechanical parts will be damaged. It isn`t possible to measure temperature T_{in} directly with a sensor, because it doesn`t 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 system`s 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 object`s trajectory.
