Extended Kalman Filter

In Kalman filter, we are assuming that prediction & updation steps are linear functions but most real world problems involve non-linear functions. In most cases, the system is looking into some direction and taking measurement in another direction. This involves angles and sine, cosine functions which are non-linear functions which then lead to problems. We receive these non-linear measurements especially from radar as laser sensors represent measurements in Cartesian coordinate while the radar sensors represent in polar coordinate.

Our predicated state x is represented by Gaussian distribution which is mapped to Nonlinear function (Radar measurement), then resulting distribution will not be a Gaussian anymore. Non linearity destroys the Gaussian and it does not makes sense to compute the mean and variances.

Kalman filter is not applicable any more as it works with Gaussian functions only. One of the solution is to linearize the non-linear h(x) function and that’s the key of EKF.

EKF uses a method called First Order Taylor Expansion to linearize the function to obtain linear approximation of the non-linear functions. In this process, a Jacobian matrix is produced, which represents the linear mapping from polar to Cartesian coordinate, applied at the update step.

First Order Taylor Expansion

In mathematics, a Taylor series is a representation of a function as an infinite sum of terms that are calculated from the values of the function’s derivatives at a single point. In simple terms, we take a point and perform a bunch of derivatives on that point.

In case of an EKF, we take mean of the Gaussian, since it is the best representation of the distribution and perform a number of derivatives to approximate. But this again will give a curve right which is non-linear, so we are just interested in the first derivative of Taylor series.

This produces a linear value which is the slope is around that mean. Therefore this method of getting the slope from the first derivative of the Taylor series is known as First order Taylor expansion.

For every non-linear function, we just draw a tangent around the mean and try to approximate the function linearly.

Effect of EKF on the Predict and update equation of KF

It is necessary to know the source of data i.e. for in case of Autonomous Vehicle we need to know whether the data on which we have to perform prediction, is coming from Lidar or Radar.

LIDAR provides us the distance in the form of Cartesian coordinate system and a RADAR provides us the distance and velocity in Polar coordinate system. The measurement vector for both of them is as follows:

LIDAR : {px ,py}

Radar : { ρ, Φ , ρ_dot}

where

px: x coordinate of object in Cartesian System

py: y coordinate of object in Cartesian System

ρ : distance of the object from vehicle

Φ : anti clockwise angle between x and ρ

ρ_dot : rate of change of ρ

x-axis : location in which car is moving.

It is now understood that data provided by LIDAR are linear in nature while that of the radar are non-linear. So the equations for the Lidar data will remain the same as that of Kalman Filter but for the Radar measurement it will be different. As Radar measurement is non-linear, we need to use EKF on Measurement Update cycle, but we can use the same formula in prediction cycle.

Prediction Step:

x′ = F.x + B.μ + ν

P′ = FPFᵀ + Q

It will remain same as that of KF irrespective of the device.

Update Step for Radar Measurement:

y= z — h(x′)

z: actual measurement in polar coordinate system

x′: Predicted Value in the Cartesian system
y: Difference between the measured value and actual measurement.

h(x′) : mapping of predicted Cartesian coordinate system to polar coordinate system. This mapping is required because we are predicting in Cartesian coordinates but our measurement (z) that is coming from the sensor is in Polar Coordinates.

S= HⱼP′Hⱼᵀ + R
K= P′HⱼᵀS⁻¹

R: Noise in the measurement phase

S: Total error

S⁻¹: The inverse of S. Since in matrix representation we cannot divide so we are taking S⁻¹

K: Kalman Gain

Hⱼ: Jacobian Matrix

As we have already seen that to convert the non-linear function to linear we use the Taylor series expansion and takes it first derivative. This first order derivative is given by Jacobian Matrix as we are dealing with matrix in the equations. Since in case of RADAR we have 4 measurements, 2 for distance and 2 for velocity, the state vector for radar consists of px,py,vx and vy.

By applying the derivative we will get the final value of Hj as follow:

x = x′ + K.y
P = (I- KHⱼ)P′

Finally we update are state vector and the covariance matrix and will move on to next prediction step to get the other values.

Summary:

In a LIDAR based measurement, we will apply a Kalman Filter because the measurements from the sensor are Linear. But in case of a Radar we need to apply Extended Kalman Filter because it includes angles that are non-linear, hence we do an approximation of the non-linear function using first derivative of Taylor series called Jacobian Matrix (Hⱼ) . Then we convert our Cartesian space to polar space using h(x’) and finally we replace H with Hⱼ in all further equations of a KF.

Drawbacks of EKF:

• When the prediction & updation steps are highly non-linear, EKF will give relatively poor performance.
• In extended Kalman filter the approximation was done based on a single point i.e. mean of the distribution. This approximation may not be the best possible approximation and lead to poor performance
• There is a high computational cost for calculating Jacobians matrix.