Kalman Filter

 Kalman Filter: Derivation(Part 2)

Last time we left that the Kalman filter is a type of control system that helps in controlling the uncertainties and is shown by the given algorithm, where it has 2 separate processes,

${\text{Prediction}\;}\\$

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

${\tilde{P}_{k}=\,AP_{k-1}A^{T}\,+\,Q}\\$

${\text{Update}}\\$

${K_{k}\,=\,\frac{\tilde{P}_{k}C^{T}}{C\tilde{P}_{k}C^{T}+\,R}}\\$

${\hat{x}_{k}\,=\,\tilde{\hat{x}}_{k\,}+\,K_{k}\left(y_{k}\,-\,C\tilde{\hat{x}}_{k}\right)}\\$

${P_{k}\,=\,\left(I\,-\,K_{k}C\right)\tilde{P}_{k}}\\$

Here 

~ means that the value is assumed or predicted( hence in the prediction state). It is meant that the value which is obtained currently is not completely correct or close to the actual state.

K represents a particular timestamp and K - 1 represents a timestamp before the current timestamp

^ means the estimated state Such as $\hat{x}$ and the actual state is left as just x.

A, B, and C are matrixes representations of mathematical models

P, Q, and R are covariance matrixes, each represents deviations in values and P is related to Q

Prediction of states

Suppose we have a bot that can know how fast it is going and how far it has gone ie velocity and positions, then it has 2 states Position and Velocity and the best way to represent any given data in a computer-readable format is by a matrix.

$\hat{x}=\,\begin{bmatrix}{\text{position}}\\{\text{velocity}}\\\end{bmatrix}=\begin{bmatrix}{\text{p}}\\{\text{v}}\\\end{bmatrix}$

If our robot has just booted up then x(k-1) will be a null matrix, that because our position and velocity are zero, it should be noted that not always the initial arguments are zero they might be an identity matrix. 

And A here will be $\begin{bmatrix}{\text{1}}&{\Delta\text{t}}\\{0}&{1}\\\end{bmatrix}$ that because 

${\text{p}_{k}\;\text{=}\;\text{p}_{k-1}\;\text{+}\;\text{v}_{k-1}\Delta t}\\{\text{v}\,=\,0\,+\,\left(1\right)\text{v}_{k-1}}\\$

That's because the position is also dependent on velocity and the previous position. You might be wondering how can changes be made to velocity. That's where $\text{B}u_{k}$ comes in.

$\text{B}u_{k}$ is a command given to the Robot. It may be acceleration or velocity or anything which moves the robot, and if moves the robot it will change its state of it as well and in this case it is velocity.

Let our given robot command be given by the acceleration

And by kinematics 

 

${\text{p}_{k}\,=\,\text{p}_{k-1}\,+\,\text{v}_{k-1}\Delta t+\frac{1}{2}a\Delta t^{2}}\\{\text{v}_{k}\,=\,0\,+\,\text{v}_{k-1}\,+\,a\Delta t}\\$

If $\text{u}_{k}=a$ then 

$\text{B}\;=\,\begin{bmatrix}{\frac{1}{2}\Delta t^{2}}\\{\Delta t}\\\end{bmatrix}$

 Noise Matrix

However, the above mathematical model is still incomplete because we haven't considered noise in our reading.

That's where P and Q come in, they are the noise matrix of our above model. Every sensor, every command given to any robot, or anything which can be measured in numbers such as speed, distance,  and acceleration has noise for example

deviation of velocity given as a command to a robot 120 decimeter/min

We cannot just add P for the noise we also have to squeeze A matrix into P as well. The P matrix is a covariance matrix of $\hat{x}_{k-1}$, an example of a covariance matrix is shown below.

$\mathbf{P}_k = \begin{bmatrix} \Sigma_{pp} \Sigma_{pv} \\ \Sigma_{vp} \Sigma_{vv} \\\end{bmatrix}$

And  Q is another noise that we cannot measure mathematically it's a stochastic noise. It can only be measured(How to calculate the Q matrix will be demonstrated in the following article).

Hence after the prediction step, our data is ready to be represented in a Gaussian distribution form 

Kalman Gain

Next, we have to calculate the Gaussian distribution of the sensor and then somehow have to use that matrix to our advantage to control errors.

Our sensor data looks like this

This data will always deviate and hence the mean and standard deviation of our data will be

(in the following article we will learn how to calculate the mean deviation of our data)

Next comes the interesting part, we find the average of the Gaussian distribution of data the average data of Gaussian data will be our most possible estimate of our robot's position.

To find the average of Gaussian data we have to find new mean and standard deviation as this is what constitutes a Gaussian distribution and after some putting in the formula we would get

$\color{royalblue}{\mu’} = \mu_0 + \frac{\sigma_0^2 (\mu_1 – \mu_0)} {\sigma_0^2 + \sigma_1^2}$

$\color{mediumblue}{\sigma’}^2 = \sigma_0^2 – \frac{\sigma_0^4} {\sigma_0^2 + \sigma_1^2} $

Where can we substitute

$\color{royalblue}{\mu’} = \mu_0 + \color{purple}{\mathbf{k}} (\mu_1 – \mu_0)$

$\color{mediumblue}{\sigma’}^2 = \sigma_0^2 – \color{purple}{\mathbf{k}} \sigma_0^2$

what it does is it squeezes THE AVERAGE of Means and Deviation and squeezes the Gaussian distribution so that its sizes match that of the other two.

 Similar can be done for matrixes as well and our equation will look like 

$\color{royalblue}{\vec{\mu}’} = \vec{\mu_0} + \color{purple}{\mathbf{K}} (\vec{\mu_1} – \vec{\mu_0})$

$\color{mediumblue}{\Sigma’} = \Sigma_0 – \color{purple}{\mathbf{K}} \Sigma_0$

Now after substituting we get 

$\hat{x}_{k}\,=\,\tilde{\hat{x}}_{k\,}+\,K_{k}\left(y_{k}\,-\,C\tilde{\hat{x}}_{k}\right)$

$P_{k}\,=\,\left(I\,-\,K_{k}C\right)\tilde{P}_{k}$

$K\,=\,AK^{\prime}$

Hence we have calculated the estimation for our timestamp k we can use this prediction for later stages 

One awesome advantage of the Kalman filter you might have observed is that you don't need to include data from another timestamp except for the previous timestamp. This make the memory footprint low for our machine which was very necessary in the late 1970s because machines were about the size of a few KB.