## Abstract

We provide a tutorial-like description of Kalman filter and extended Kalman filter. This chapter aims for those who need to teach Kalman filters to others, or for those who do not have a strong background in estimation theory. Following a problem definition of state estimation, filtering algorithms will be presented with supporting examples to help readers easily grasp how the Kalman filters work. Implementations on INS/GNSS navigation, target tracking, and terrain-referenced navigation (TRN) are given. In each example, we discuss how to choose, implement, tune, and modify the algorithms for real world practices. Source codes for implementing the examples are also provided. In conclusion, this chapter will become a prerequisite for other contents in the book.

### Keywords

- Kalman filter
- extended Kalman filter
- INS/GNSS navigation
- target tracking
- terrain-referenced navigation

## 1. Introduction

Kalman filtering is an algorithm that provides estimates of some unknown variables given the measurements observed over time. Kalman filters have been demonstrating its usefulness in various applications. Kalman filters have relatively simple form and require small computational power. However, it is still not easy for people who are not familiar with estimation theory to understand and implement the Kalman filters. Whereas there exist some excellent literatures such as [1] addressing derivation and theory behind the Kalman filter, this chapter focuses on a more practical perspective.

Following two chapters will devote to introduce algorithms of Kalman filter and extended Kalman filter, respectively, including their applications. With linear models with additive Gaussian noises, the Kalman filter provides optimal estimates. Navigation with a global navigation satellite system (GNSS) will be provided as an implementation example of the Kalman filter. The extended Kalman filter is utilized for nonlinear problems like bearing-angle target tracking and terrain-referenced navigation (TRN). How to implement the filtering algorithms for such applications will be presented in detail.

## 2. Kalman filter

### 2.1 Problem definition

Kalman filters are used to estimate states based on linear dynamical systems in state space format. The process model defines the evolution of the state from time

where

The process model is paired with the measurement model that describes the relationship between the state and the measurement at the current time step

where

The role of the Kalman filter is to provide estimate of

### 2.2 Kalman filter algorithm

Kalman filter algorithm consists of two stages: prediction and update. Note that the terms “prediction” and “update” are often called “propagation” and “correction,” respectively, in different literature. The Kalman filter algorithm is summarized as follows:

*Prediction:*

Predicted state estimate | |

Predicted error covariance |

*Update:*

Measurement residual | |

Kalman gain | |

Updated state estimate | |

Updated error covariance |

In the above equations, the hat operator,

The predicted state estimate is evolved from the updated previous updated state estimate. The new term

In the update stage, the measurement residual

We need an initialization stage to implement the Kalman filter. As initial values, we need the initial guess of state estimate,

Note that Kalman filters are derived based on the assumption that the process and measurement models are linear, i.e., they can be expressed with the matrices

### 2.3 Example

An example for implementing the Kalman filter is navigation where the vehicle state, position, and velocity are estimated by using sensor output from an inertial measurement unit (IMU) and a global navigation satellite system (GNSS) receiver. In this example, we consider only position and velocity, omitting attitude information. The three-dimensional position and velocity comprise the state vector:

where

where

where

Now, we have the process model as:

where

The GNSS receiver provides position and velocity measurements corrupted by measurement noise

It is straightforward to derive the measurement model as:

where

In order to conduct a simulation to see how it works, let us consider

We need to generate noise of acceleration output and GNSS measurements for every time step. Suppose the acceleration output, GNSS position, and GNSS velocity are corrupted with noise with variances of 0.3^{2}, 3^{2}, and 0.03^{2}, respectively. For each axis, one can use MATLAB function randn or normrnd for generating the Gaussian noise.

The process noise covariance matrix,

Let us start filtering with the initial guesses

where

In this simulation,

The time history of estimation errors of two Monte-Carlo runs is depicted in Figure 1. We observe that the estimation results of different simulation runs are different even if the initial guess for the state estimate is the same. You can also run the Monte-Carlo simulation with different initial guesses (sampled from a distribution) for the state estimate.

The standard deviation of the estimation errors and the estimated standard deviation for x-axis position and velocity are drawn in Figure 2. The standard deviation of the estimation error, or the root mean square error (RMSE), can be obtained by computing standard deviation of

In real applications, you will be able to acquire only the estimated covariance because you will hardly have a chance to conduct Monte-Carlo runs. Also, getting a good estimate of

Source code of MATLAB implementation for this example can be found in [5]. It is recommended for the readers to change the parameters and aircraft trajectory by yourself and see what happens.

## 3. Extended Kalman filter

### 3.1 Problem definition

Suppose you have a nonlinear dynamic system where you are not able to define either the process model or measurement model with multiplication of vectors and matrices as in (1) and (2). The extended Kalman filter provides us a tool for dealing with such nonlinear models in an efficient way. Since it is computationally cheaper than other nonlinear filtering methods such as point-mass filters and particle filters, the extended Kalman filter has been used in various real-time applications like navigation systems.

The extended Kalman filter can be viewed as a nonlinear version of the Kalman filter that linearized the models about a current estimate. Suppose we have the following models for state transition and measurement

where

### 3.2. Extended Kalman filter algorithm

All you need is to obtain the Jacobian matrix, first-order partial derivative of a vector function with respect to a vector, of each model in each time step as:

Note the subscripts of

*Prediction:*

Predicted state estimate | |

Predicted error covariance |

*Update:*

Measurement residual | |

Kalman gain | |

Updated state estimate | |

Updated error covariance |

As in the Kalman filter algorithm, the hat operator,

### 3.3 Example

#### 3.3.1 Target tracking

We are going to estimate a 3-dimensional target state (position and velocity) by using measurements provided by a range sensor and an angle sensor. For example, a radar system can provide range and angle measurement and a combination of a camera and a rangefinder can do the same. We define the target state as:

where

The process noise has the covariance of

The measurement vector is composed of line-of-sight angles to the target,

where

where

In the simulation, the sensor is initially located at

In the filter side, the covariance matrix for the process noise can be set as:

where

The above equation means that the error of the initial guess for the target state is randomly sampled from a Gaussian distribution with a standard deviation of

Time history of an estimation result for x-axis position and velocity is drawn together with the true value in Figure 4. The shape of the line will be different at each run. The statistical result can be shown as Figure 5. Note that the filter worked inconsistently with the estimated error covariance different from the actual value. This is because the process error covariance is set to a very large number. In this example, the large process error covariance is the only choice a user can make because the measurement cannot correct the velocity. One can notice that the measurement Eq. (26) has no term dependent on the velocity, and therefore, matrix

Source code of MATLAB implementation for this example can be found in [5]. It is recommended for the readers to change the parameters and trajectories by yourself and see what happens.

#### 3.3.2 Terrain-referenced navigation

Terrain-referenced navigation (TRN), also known as terrain-aided navigation (TAN), provides positioning data by comparing terrain measurements with a digital elevation model (DEM) stored on an on-board computer of an aircraft. The TRN algorithm blends a navigational solution from an inertial navigation system (INS) with the measured terrain profile underneath the aircraft. Terrain measurements have generally been obtained by using radar altimeters. TRN systems using cameras [7], airborne laser sensors [8], and interferometric radar altimeters [9] have also been addressed. Unlike GNSS’s, TRN systems are resistant to electronic jamming and interference, and are able to operate in a wide range of weather conditions. Thus, TRN systems are expected to be alternative/supplement systems to GNSS’s.

The movement of the aircraft is modeled by the following Markov process:

where

Typical TRN systems utilize measurements of the terrain elevation underneath an aircraft. The terrain elevation measurement is modeled as:

where

The process model in (33) and the measurement model in (34) can be linearized as:

where

The DEMs are essentially provided as matrices containing grid-spaced elevation data. For obtaining finer-resolution data, interpolation techniques are often used to estimate the unknown value in between the grid points. One of the simplest methods is linear interpolation. Linear interpolation is quick and easy, but it is not very precise. A generalization of linear interpolation is polynomial interpolation. Polynomial interpolation expresses data points as higher degree polynomial. Polynomial interpolation overcomes most of the problems of linear interpolation. However, calculating the interpolating polynomial is computationally expensive. Furthermore, the shape of the resulting curve may be different to what is known about the data, especially for very high or low values of the independent variable. These disadvantages can be resolved by using spline interpolation. Spline interpolation uses low-degree polynomials in each of the data intervals and let the polynomial pieces fit smoothly together. That is, its second derivative is zero at the grid points (see [11] for more details). Classical approach to use polynomials of degree 3 is called cubic spline. Because the elevation data are contained in a two-dimensional array, bilinear or bicubic interpolation are generally used. Interpolation for two-dimensional gridded data can be realized by interp2 function in MATLAB. Cubic spline interpolation is used in this example.

The DEM we are using in this example has a

An aircraft, initially located at

The process noise

Let us consider

The above equation means the error of the initial guess for the target state is randomly sampled from a Gaussian distribution with a standard deviation of

The time history of RMSE of the navigation is shown in Figure 9. One can observe the RMSE converges relatively slower than other examples. Because the TRN estimates 2D position by using the height measurements, it often lacks information on the vehicle state. Moreover, note that the extended Kalman filter linearizes the terrain model and deals with the slope that is effective locally. If the gradient of the terrain is zero, the measurement matrix

Source code of MATLAB implementation for this example can be found in [5]. It is recommended for the readers to change the parameters and aircraft trajectory by yourself and see what happens.

## 4. Conclusion

In this chapter, we introduced the Kalman filter and extended Kalman filter algorithms. INS/GNSS navigation, target tracking, and terrain-referenced navigation were provided as examples for reader’s better understanding of practical usage of the Kalman filters. This chapter will become a prerequisite for other contents in the book for those who do not have a strong background in estimation theory.