Before getting into what a Kalman filter is or what it does, let’s first do an exercise. Open the google maps application on your phone and check your device’s current location. Did you

check it? How accurately is it able to pinpoint your location? Quite accurately isn’t it. The errors from your actual location might range from 2m to 20m depending on the quality of the GPS

or whether you are indoors or outdoors but still, it did a pretty good job. In fact, it did a great job considering the fact that earth is so huge and it’s able to pinpoint your location,

with a small error radius which is represented by a circle around the point. But now let’s say you install a GPS on your car and you are told to remote control it from your house, given the

is the task of reducing the error in the position of our vehicle to cm level accuracy. Now to do this, instead of just relying on our GPS, we install other instruments such as IMU, camera,

lidar, and radar to our vehicle and use the additional information to get a better understanding of the environment. Ok, so now we have a much detailed sense of the environment and, if now

the task is given to you, you’ll probably feel more confident in being able to navigate the car through the streets with just the device feeds. At Least the less crowded ones. But what if

this task is given to a computer? Humans are quite good at recognizing patterns without much effort but, for a computer, it still needs to process the information, and factors like

measurement error, measurement noise, process error, and delays add further complexity. Here’s where Kalman filter comes in. Kalman filter is an algorithm, named after Rudolf E. Kálmán, one

of the primary developers of this theory, which is extensively used for many applications. A common application is for Guidance, Navigation, and Control of Vehicles. Kalman filters are based

on the theory of Bayesian probabilities. There are typically two steps in the Kalman filter: _Predict_ and _Update_. To understand what each of the steps does, we first define something

called the _State_ of the vehicle. The state represents a collection of measurements that we would like to describe our vehicle with. It can be a combination of anything from, position,

velocity, lateral acceleration, yaw, yaw rate, and many more. Let’s take a simple example of a car that can only move along the x-axis with only the position as part of the state. We start

with a good guess for the state of the vehicle. Let’s start with position 0 m. We assume a constant velocity model i.e., we will not consider the effect of acceleration to our vehicle’s

position estimate. Now we bump up the velocity of our vehicle to 1 m/s. Remember that we have assumed a constant velocity model, so we will not consider the acceleration in between. Where do

you expect the vehicle to be after 1second has elapsed? You probably answered it correctly. The answer is at 1m. The velocity of the vehicle is 1 m/s and according to distance = velocity *

time, we get distance as 1m. Now, can you predict the position after 2 more seconds have elapsed with the same velocity? The answer is 3m. Easy, right? Given the velocity of the vehicle at

all times, you were able to predict the position of the vehicle. If we were able to predict the position of the vehicle without any hustle, why do we need Kalman filter? The truth is, we

wouldn’t have needed Kalman filters if we could have been 100% sure of all our measurements. But in reality, no measurement is perfect. So the place where we assume our velocity to be 1 m/s

may not be 100% accurate. It may be 0.9 m/s or 1.1 m/s or something else, we aren’t sure. There is always some margin for error. Now, what will happen to our position estimate? Because there

is an error term in velocity, there will also be an associated error term for the position estimate as well. As the vehicle moves with time, we can propagate the state of the vehicle along

with it’s associated error term. This is the prediction step of the Kalman filter. Now the prediction step alone cannot solve the problem of localization as the errors will keep accumulating

with time, degrading our position estimate. Our estimates degrade with time because no new information is collected while the car is moving and hence some information is lost during each

propagation step. This is evident from the image above where the probability distribution of position estimate gets wider as it moves ahead. And, if this is continued, the probability

distribution of the vehicle position will tend to uniform distribution. The Update step solves this problem. Let’s say we have a GPS installed on our vehicle which tells us our position

every 2 seconds. So after every 2 seconds, using this new information, we can update the state of the vehicle based on the measurements and previous belief of the state. The update step

makes sure that the errors don’t keep accumulating with time. KALMAN FILTER EQUATIONS Cool, if you have understood everything, you have got a good grasp of what Kalman filters are, and how

they work. So, let’s see how all this is implemented in mathematical equations. PREDICT: UPDATE: Did I suddenly confuse you with all the equations? Don’t worry, even I felt the same way when

I had faced these equations for the first time. Instead of getting too deep into the linear algebra that is making this possible, we will try to understand the essence of each equation. You

can still follow the equations without getting into the derivation of the advanced linear algebra concepts. Consider an example of a car initially at position 0. We measure its position

every Δt seconds and we keep track of the car’s position and velocity along the x- dimension. STEP 1: (PREDICT) 1.1 PROPAGATION OF STATE: As we are describing our car using position and

velocity, these quantities will be part of the car’s state. _x _is the position and _ẋ _is the derivative of position wrt to time which is velocity. Control inputs represent the collection

of external parameters that are used to control the car. In our case, we consider lateral acceleration ‘_a’ _as an input. The state-transition model describes how the car’s state propagates

in the time, given the state of the previous time step. The propagation of state is governed by the two equations below: The multiplication _F(k) X(k-1)_ is translated by the above

equations. Here the subscript _k_ of _F _has no significance because _F_ does not vary with time. The control-input model is a matrix that defines the effect of control-inputs on the car’s

state. For only lateral acceleration as the input, its effect on the state parameters are as follows: The equations above are what you will get on the multiplication _B(k) u(k)_. Similar to

_F_, the subscript _k_ of _B_ also does not have any significance as _B_ is also constant throughout. You might wonder what the hat in _X̂ (k|k-1) _means? In linear algebra to represent an

estimate of a variable, we put a hat over the variable. As we can only estimate the state of the car is, the hat represents that it is an estimate of the state. 1.2 PROPAGATION OF ESTIMATE

COVARIANCE MATRIX: _P(k) _represents the estimate covariance matrix of the uncertainty in the state parameters i.e. position and velocity. It keeps track of the errors associated with our

state. The subscript _k|k-1_ represents the estimate covariance at time step _k _given the estimate covariance at time step _k-1_. It is initialized with some prior variance. Because we have

two parameters in our state, the estimate covariance matrix is 2 x 2 matrix. We initialize the matrix based on our initial beliefs of the uncertainty in position and velocity. These values

occupy the major diagonal of the matrix in the same order as that of the order in the state. When there is no measurement available, the estimate covariance matrix is propagated based on the

state-transition model _F _with _Q_ as the process noise. _Q _is calculated as follows: Keep in mind the uncertainty of the state parameters will increase in this step because of the

propagation step. STEP 2: (UPDATE) 2.1 MEASUREMENT: Measurement step also known as the Innovation step is the step where we gain information from the instruments installed on the car. This

is a crucial step as, without any external information, the car is as good as _blind_. In our case, we will assume a GPS-like system on our car, that can tell the position of the car

periodically. You can call the observation model a matrix, that transforms our state to the format of that of the measurement matrix _z(k)_. In our case, we only use GPS to measure the

position so, Hence, the observation model in our case will be: Again, we can drop the subscript _k_ as it is time-invariant. On multiplying observation model with the state, we get: We now

have our state transformed into the format of measurement. So for this step, we calculate the residual _ỹ(k)_ which is observed measurement minus calculated measurement. 2.2 KALMAN GAIN: We

have now arrived at the most cryptic equation of the Kalman filter algorithm, the computation of Kalman gain ( _K(k) )_. This is the real game player in the algorithm for which we went

through all of this trouble. Kalman gain holds the information of uncertainties in state parameters fused with the uncertainties in observations of the instrument readings. _P(k) _holds

uncertainty in state parameters while _R(k) _represents the observation noise matrix. These are fused into Kalman gain using the above equation. In the next steps, we will see the role of

Kalman gain in enhancing our prior estimates. 2.3 UPDATE STATE: We now have _X̂ (k|k-1), _which is the estimate of the state which we calculated in the propagation step. We have _ỹ(k),_

which is the residual we calculated in the innovation step, and we have _K(k)_, Kalman gain which we calculated in the previous step. Based on these three we update the estimate of the

state. The significance of _k | k _(read as _k_ given _k_) in _X̂ (k|k) _is that we are enhancing our estimates which we calculated in the propagation step using the observed measurements.

To get an intuition of how this is happening, consider the state measured in the propagation step and the residual calculated in the measurement step. Both of the things kind of represent

the same thing but in different formats. Where _X̂ (k|k-1) _represents the prior belief of state, the residual _ỹ(k)_, represents the difference in measured state and prior belief. The

Kalman gain acts as a bridge between them. It contains the information regarding the proportions in which the prior belief of state and the observations will be weighted to calculate the

updated state. The cool thing about this is, you don’t need to worry about knowing which measurement contributes how much towards the estimate. All of that has been computed as part of the

Kalman gain which contains all this information. 2.4 UPDATE ESTIMATE COVARIANCE MATRIX: In this is the final step of the Kalman filter, we use the Kalman gain computed, the observation

model, and the prior belief of estimate covariance to update the estimate covariance matrix. As Kalman gain holds the gain in information gathered from instruments, the uncertainties in

state parameters decrease as we have gained information. With a continuous feed of the measurements, the Kalman filter is able to reduce the uncertainty in the car’s state and is able to

localize the car. SUMMARY We can summarize the algorithm as follows: UFF! That was a lot, wasn’t it! But by decoding the equations, line by line along with me, we have finally understood how

Kalman filters work, and how it is implemented mathematically. Understanding the equations isn’t hard. Don’t let the notations and the linear algebra intrigue you. It’s more important to

understand the meaning that they are trying to convey. CONCLUSION Kudos! If you got everything, you now understand one of the most widely used algorithms for localization in the autonomous

industry. There are variants to the original Kalman filter algorithms like Extended Kalman Filter (EKF) or Unscented Kalman Filter (UKF) to name a few but they all rely on the same concepts

that we learned today. I would also suggest you go through Wikipedia, and read about Kalman filters there. If you found this very interesting, you can also checkout EKF and UKF online and

try to see what limitations the original Kalman filter faces, and how they are tried to resolve in these variants.