Intuition behind Kalman filter

Kalman filters: Intuition (Part 1)

Kalman Filter is one of the most prevalent algorithms engineers use to tune the control system. It was implemented in many space programs by NASA to estimate the trajectories of Apollo rockets, Sensor fusion is a handy algorithm used in modern automation such as self-driving cars and humanoid robots made by Boston or Honda based on the Kalman filter. Kalman filter was first implemented 60 years ago and is still being implemented in many applications nowadays.

What is a control system?

Kalman filter is a type of control system. A control system is a branch in engineering where control loops as made which gives us the desired outputs as per the inputs given to that loop.

For example, if you turn on the AC if you feel that it's still too hot you would decrease the temperature, here you and AC make a control loop. The control system is by itself another branch of engineering.

However, the Control system has a problem. Let's look into one

You have an oven that reaches around 2000 degrees Celsius and you would like to control its temperature but 2000 Celsius is way too hot for any sensor so what can you do is that place the sensor a bit outside of the oven and with a mathematical model calculate its internal temperature.

With a control system, you can create a PID controller(to learn more about PID controller). Which takes the feedback of the sensor and applies the changes in the mathematical model of the controller and outputs the external temperature.

Now since sensors are noisy their value will fluctuate and so will the internal temperature. This might not be a problem for an oven however for rocket propulsion can be critical. Imagine having a fluctuating trajectory of a rocket going to the moon it will get lost after a few days in space imagine for several months.

here yellow represents the possibility of a rocket landing if the sensor data keeps fluctuating

What is the Kalman filter?

There are 2 ways to fix this problem

1. Make better sensors
2. Improve Algorithm

Making sensors more precise will be doing the work but it will still have accuracy upto a range and even if sensors got precise enough but still have some fluctuation, these fluctuations will add up and after some time the reading will get useless

Improving Algorithms that might open a blind eye on that the world around us is noisy. Kalman filter is one such algorithm that accounts for fluctuations.

Here's how the Kalman filter works

In the Kalman filter, you have to model

1. Actual state - it is meant by the real equation containing all the noise which governs the dynamics of the sensor. When experimenting the values which are obtained from performing a practical is what the actual state is
2. Estimated state- It is the state which is derived by mathematical equations. When performing a practical, when we calculate the theoretical values, the estimated state can be defined by it

(they are similar to that of belief and state in a SLAM process, refer here to understand more)

We denote the estimated state with a hat over it and our overall goal is to make these 2 systems as similar as possible, to check whether the 2 systems are showing similar results we match the actual sensor reading to the estimated sensor reading.

And that the intuition behind Kalman filters

the Actual state 

${\dot{x}=\,Ax\,+\,Bu\,+\,Q}\\$

${y=Cx\,+\,R}\\$

the estimated state will be

${\dot{\hat{x}}\,=\,A\hat{x\,}+\,Bu}\\$

${\hat{y}\,=\,C\hat{x}}\\$

Let's understand the similarities first,

Here future values of the state depend on the previous state x, the command given u, and y is some proportion of x, that's because they are correlated to each other as they can be considered as an output that is affected by the command u so there is no extra variable needed for the relation.

For example let's take the example of the oven, in the oven let's say the flame intensity provided is our u, the internal temperature is x, and y will be the internal temperature.

So intuitively new values of x will depend on the previous value of x as well as the intensity of the flame and y will be some constant of x because they both are temperatures of the oven but at different locations so y is directly proportional to x. 

The equation cannot  be written in terms of y because of values Q and R

These Q and R are process noise and random noise

Process noise is noise or uncertainty of command given and

R is the uncertainty of the sensor

Let's say you have a dial and let's say with each 30-degree rotation of the dial, the intensity of flame increases by 5%. Still, it's humanly impossible to be precise in rotating the dial 30 degree or multiple 30 so if you were asked to rotate the dial 90 degree then deviation from 90 degrees is considered as process noise. R is the accuracy of the sensor which always has its limit.

Next, there is what the Kalman filter does

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

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

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

AND

${\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}}\\$

In prediction, the Kalman filter first takes the value of $\tilde{\hat{x}}_{k}$ without any noise and $\tilde{P}_{k}$ from the $P_{k-1}$ and Q and uses these values to update Kalman gain or $K_{k}$ then this Kalman gain is used to tune the estimated value of $\hat{x_{k}}$ and $P_{k}$.

Process noise is needed to be updated because that’s the beauty of the Kalman filter it does not let the inaccuracy of the sensor blows out of usable range.

The derivation of the Kalman filter will be in the next post.

However, it's an interesting fact that in ${\hat{x}_{k}\,=\,\tilde{\hat{x}}_{k\,}+\,K_{k}\left(y_{k}\,-\,C\tilde{\hat{x}}_{k}\right)}\\$ is similar to deterministic PID gain. It is so because the Kalman filter is just a modification of the PID controller such that values of gain change based on the accuracy of sensors and our input noise. Next, these estimated states are used for the next values of X and P In the next article, we will derive the Kalman filter and why there are so many variables in the prediction step and update steps.