Open access peer-reviewed chapter

# Introduction to Kalman Filter and Its Applications

By Youngjoo Kim and Hyochoong Bang

Submitted: April 26th 2018Reviewed: July 30th 2018Published: November 5th 2018

DOI: 10.5772/intechopen.80600

## 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
• target tracking

## 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  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 k1to time kas:

xk=Fxk1+Buk1+wk1E1

where Fis the state transition matrix applied to the previous state vector xk1, Bis the control-input matrix applied to the control vector uk1, and wk1is the process noise vector that is assumed to be zero-mean Gaussian with the covariance Q, i.e., wk1N0Q.

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

zk=Hxk+νkE2

where zkis the measurement vector, His the measurement matrix, and νkis the measurement noise vector that is assumed to be zero-mean Gaussian with the covariance R, i.e., νkN0R. Note that sometimes the term “measurement” is called “observation” in different literature.

The role of the Kalman filter is to provide estimate of xkat time k, given the initial estimate of x0, the series of measurement, z1,z2,,zk, and the information of the system described by F, B, H, Q, and R. Note that subscripts to these matrices are omitted here by assuming that they are invariant over time as in most applications. Although the covariance matrices are supposed to reflect the statistics of the noises, the true statistics of the noises is not known or not Gaussian in many practical applications. Therefore, Qand Rare usually used as tuning parameters that the user can adjust to get desired performance.

### 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 x̂k−=Fx̂k−1++Buk−1 Predicted error covariance Pk−=FPk−1+FT+Q

Update:

 Measurement residual y∼k=zk−Hx̂k− Kalman gain Kk=Pk−HTR+HPk−HT−1 Updated state estimate x̂k+=x̂k−+Kky∼ Updated error covariance Pk+=I−KkHPk−

In the above equations, the hat operator,  ̂, means an estimate of a variable. That is, x̂is an estimate of x. The superscripts and +denote predicted (prior) and updated (posterior) estimates, respectively.

The predicted state estimate is evolved from the updated previous updated state estimate. The new term Pis called state error covariance. It encrypts the error covariance that the filter thinks the estimate error has. Note that the covariance of a random variable xis defined as covx=Exx̂xx̂TTwhere Edenotes the expected (mean) value of its argument. One can observe that the error covariance becomes larger at the prediction stage due to the summation with Q, which means the filter is more uncertain of the state estimate after the prediction step.

In the update stage, the measurement residual ykis computed first. The measurement residual, also known as innovation, is the difference between the true measurement, zk, and the estimated measurement, Hx̂k. The filter estimates the current measurement by multiplying the predicted state by the measurement matrix. The residual, yk, is later then multiplied by the Kalman gain, Kk, to provide the correction, Kkyk, to the predicted estimate x̂k. After it obtains the updated state estimate, the Kalman filter calculates the updated error covariance, Pk+, which will be used in the next time step. Note that the updated error covariance is smaller than the predicted error covariance, which means the filter is more certain of the state estimate after the measurement is utilized in the update stage.

We need an initialization stage to implement the Kalman filter. As initial values, we need the initial guess of state estimate, x̂0+, and the initial guess of the error covariance matrix, P0+. Together with Qand R, x̂0+and P0+play an important role to obtain desired performance. There is a rule of thumb called “initial ignorance,” which means that the user should choose a large P0+for quicker convergence. Finally, one can obtain implement a Kalman filter by implementing the prediction and update stages for each time step, k=1,2,3,, after the initialization of estimates.

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 F, B, and H, and the process and measurement noise are additive Gaussian. Hence, a Kalman filter provides optimal estimate only if the assumptions are satisfied.

### 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:

x=pTvTTE3

where p=pxpypzTis the position vector and v=vxvyvzTis the velocity vector whose elements are defined in x, y, z axes. The state in time kcan be predicted by the previous state in time k1as:

xk=pkvk=pk1+vk1Δt+12ak1Δt2vk1+ak1ΔtE4

where ak1is the acceleration applied to the vehicle. The above equation can be rearranged as:

xk=I3×3I3×3Δt03×3I3×3xk1+12I3×3Δt2I3×3Δtak1E5

where I3×3and 03×3denote 3×3identity and zero matrices, respectively. The process noise comes from the accelerometer output, ak1=ak1+ek1, where ek1denotes the noise of the accelerometer output. Suppose ek1N0I3×3σe2. From the covariance relationship, CovAx=ATwhere Covx=Σ, we get the covariance matrix of the process noise as:

Q=12I3×3Δt2I3×3ΔtI3×3σe212I3×3Δt2I3×3ΔtT=14I3×3Δt403×303×3I3×3Δt2σe2E6

Now, we have the process model as:

xk=Fxk1+Bak1+wk1E7

where

F=I3×3I3×3Δt03×3I3×3E8
B=12I3×3Δt2I3×3ΔtE9
wk1N0QE10

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

zk=pkvk+νkE11

It is straightforward to derive the measurement model as:

zk=Hxk+νkE12

where

H=I6×6E13
νkN0RE14

In order to conduct a simulation to see how it works, let us consider N=20time steps (k=1,2,3,,N)with Δt=1. It is recommended to generate a time history of true state, or a true trajectory, first. The most convenient way is to generate the series of true accelerations over time and integrate them to get true velocity and position. In this example, the true acceleration is set to zero and the vehicle is moving with a constant velocity, vk=550Tfor all k=1,2,3,,N, from the initial position, p0=000. Note that one who uses the Kalman filter to estimate the vehicle state is usually not aware whether the vehicle has a constant velocity or not. This case is not different from nonzero acceleration case in perspective of this Kalman filter models. If the filter designer (you) has some prior knowledge of the vehicle maneuver, process models can be designed in different forms for best describing various maneuvers as in .

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.32, 32, and 0.032, respectively. For each axis, one can use MATLAB function randn or normrnd for generating the Gaussian noise.

The process noise covariance matrix, Q, and measurement noise covariance matrix, R, can be constructed following the real noise statistics described above to get the best performance. However, have in mind that in real applications, we do not know the real statistics of the noises and the noises are often not Gaussian. Common practice is to conservatively set Qand Rslightly larger than the expected values to get robustness.

Let us start filtering with the initial guesses

x̂0+=2205,5.1,0.1TE15
P0+=I3×34203×303×3I3×30.42E16
and noise covariance matrices
Q=14I3×3Δt403×303×3I3×3Δt20.32E17
R=I3×33203×303×3I3×30.032E18

where Qand Rare constant for every time step. The more uncertain your initial guess for the state is, the larger the initial error covariance should be.

In this simulation, M=100Monte-Carlo runs were conducted. A single run is not sufficient for verifying the statistic characteristic of the filtering result because each sample of a noise differs whenever the noise is sampled from a given distribution, and therefore, every simulation run results in different state estimate. The repetitive Monte-Carlo runs enable us to test a number of different noise samples for each time step.

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 Mestimation errors for each time step. The estimated standard deviation was obtained by taking squared root of the corresponding diagonal term of Pk+. Drawing the estimated standard deviation for each axis is possible because the state estimates are independent to each other in this example. A care is needed if Pk+has nonzero off-diagonal terms. The estimated standard deviation and the actual standard deviation of estimate errors are very similar. In this case, the filter is called consistent. Note that the estimated error covariance matrix is affected solely by P0+, Q, and R, judging from the Kalman filter algorithm. Different settings to these matrices will result in different Pk+and therefore different state estimates.

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 Qand Ris often difficult. One practical approach to estimate the noise covariance matirces is the autocovariance least-squares (ALS) technique  or an adaptive Kalman filter where the noise covariance matrices are adjusted in real time can be used .

Source code of MATLAB implementation for this example can be found in . 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

xk=fxk1uk1+wk1E19
zk=hxk+νkE20

where fis the function of the previous state, xk1, and the control input, uk1, that provides the current state xk. his the measurement function that relates the current state, xk, to the measurement zk. wk1and νkare Gaussian noises for the process model and the measurement model with covariance Qand R, respectively.

### 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:

Fk1=fxx̂k1+,uk1E21
Hk=hxx̂kE22

Note the subscripts of Fand Hare maintained here since the matrices are often varying with different values of the state vector for each time step. By doing this, you linearize the models about the current estimate. The filter algorithm is very similar to Kalman filter.

Prediction:

 Predicted state estimate x̂k−=fx̂k−1+uk−1 Predicted error covariance Pk−=Fk−1Pk−1+Fk−1T+Q

Update:

 Measurement residual y∼k=zk−hx̂k− Kalman gain Kk=Pk−HkTR+HkPk−HkT−1 Updated state estimate x̂k+=x̂k−+Kky∼ Updated error covariance Pk+=I−KkHkPk−

As in the Kalman filter algorithm, the hat operator,  ̂, means an estimate of a variable. That is, x̂is an estimate of x. The superscripts and +denote predicted (prior) and updated (posterior) estimates, respectively. The main difference from the Kalman filter is that the extended Kalman filter obtains predicted state estimate and predicted measurement by the nonlinear functions fxk1uk1and hxk, respectively.

### 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:

x=pTvTTE23

where pand vdenote position and velocity of the target, respectively. The system model is described as a near-constant-velocity model  in discrete time space by:

xk=pkvk=fxk1uk1=pk1+vk1Δtvk1+wk1E24

The process noise has the covariance of wk1N0Qwhere

Q=03×303×303×3σx2000σy2000σz2E25
and σx,σy,and σzare the standard deviations of the process noise on the velocity in x, y, and z directions, respectively.

The measurement vector is composed of line-of-sight angles to the target, Aand E, and the range, R, to the target. The relationship between the measurement and the relative target state with respect to the sensor comprises the measurement model as:

zk=AER=hxk=atanxtxsytysatanztzsxtxs2+ytys2xtxs2+ytys2+ztzs2+νkE26

where pk=xtytztTis the position vector of the target and xsyszsTis the position vector of the sensor. The target position is the variable in this measurement model. Note that the measurement has nonlinear relationship with the target state. This cannot be expressed in a matrix form as in (2) whereas the process model can be. If at least one model is nonlinear, we should use nonlinear filtering technique. In order to apply extended Kalman filter to this problem, let us take first derivatives of the process model and measurement model as:

Fk1=fxx̂k1+,uk1=I3×3I3×3Δt03×3I3×3E27
Hk=hxx̂k=yx2+y2xx2+y20xzx2+y2+z2x2+y2yzx2+y2+z2x2+y21x2+y2xx2+y2+z2yx2+y2+z2zx2+y2+z203×3E28

where xyzT=xtxsytysztzsTis the relative position vector. Note that the matrix Hkvaries with different values of xyzTon which the filtering result will, therefore, depend. Thus, one can plan the trajectory of the sensor to get a better filtering result . Developing such a method is one of active research topics.

In the simulation, the sensor is initially located at xsyszsT=402050Tand the sensor is moving in a circular pattern with a radius of 20 centered at 202050T. The initial state of the target is x0=10100120T. The sensor is moving with a constant velocity of 120T. The trajectory of the target and the sensor is shown in Figure 3. Note that this is the case where we are aware that the target has a constant velocity, unlike the example in Section 2.3, which is why we modeled the state transition as the near-constant-velocity model in (4). Let us consider N=20time steps (k=1,2,3,,N)with Δt=1. Suppose the measurements are corrupted with a Gaussian noise whose standard deviation is 0.020.021.0T.

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

Q=03×303×303×3I3×3σv2E29

where σv=5is the tuning parameter that denotes how uncertain the velocity estimate is. The measurement covariance matrix was constructed following the real noise statistics as:

R=0.0220000.0220001.02E30

M=100Monte-Carlo runs were conducted with the following initial guesses:

x̂0+=x0+normrnd0110000E31
P0+=I3×31203×303×3I3×30.12E32

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 110000.

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 Hin (28) has zero elements on the right side of the matrix where the derivatives of the measurement equation with respect to velocity are located. As a result, the measurement residual has no effect on velocity correction. In this case, we say the system has no observability on velocity. In practice, this problem can be mitigated by setting the process noise covariance to a large number so that the filter believes the measurement is more reliable. In this way, we can prevent at least the position estimate from diverging. Figure 4.Time history of an estimation result for x-axis position and velocity. Figure 5.Actual and estimated standard deviation for x axis estimate errors.

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

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 , airborne laser sensors , and interferometric radar altimeters  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:

xk=xk1+uk1+wk1E33

where xk1, uk1, and wk1denote the state vector, the relative movement, and the additive Gaussian process noise, respectively, at time k1. xk=ϕλTis a two-dimensional state vector, which denotes the aircraft’s horizontal position. Estimates of the relative movement (velocity) are provided by the INS and their error is absorbed into wk1to limit the dimensionality of the state. The simple model in (33) is considered realistic without details of INS integration if an independent attitude solution is available so that the velocity can be resolved in an earth-fixed frame . The estimation models we deal with belong to the TRN filter block in Figure 6, taking relative movement information from the INS as uk.

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

zk=hxk+υkE34

where hxkdenotes terrain elevation from the DEM evaluated at the horizontal position, xk, and υkdenotes the additive Gaussian measurement noise. The elevation measurement is obtained by subtracting the ground clearance measurement from a radar altimeter, hr, from the barometric altimeter measurement, hb. υkcontains errors of the radar altimeter, barometric altimeter, and DEM. The ground clearance and the barometric altitude correspond to the above ground level (AGL) height and the mean sea level (MSL) height, respectively. The relationship between the measurements is depicted in Figure 7. Note that the terrain elevation that comprises the measurement model in (34) is highly nonlinear.

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

Fk1=fxx̂k1+,uk1=I2×2E35
Hk=hxx̂k=DϕλϕDϕλλE36

where Dϕλdenotes the terrain elevation from the DEM on the horizontal position ϕλT.

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  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 100×100grid with a resolution of 30. The profile of the DEM can be depicted as Figure 8. The figure represents contours of the terrain where brighter color denotes regions with higher altitude. The point (20, 10) in the grid corresponds to the position 600300Tin the navigation frame.

An aircraft, initially located at x0=400400T, is moving by 20 every time step in x direction. The aircraft is equipped with a radar altimter and a barometric altimter, which are used for obtaining the terrain elevation. This measured terrain elevation is compared to the DEM to estimate the vehicle’s position.

The process noise wk1is a zero-mean Gaussian noise with the standard deviation of 0.50.5T. The radar altimeter is corrupted with a zero-mean Gaussian noise with the standard deviation of 3. The matrices Qand Rare following the real statistics of the noises as:

Q=0.52000.52E37
R=32E38

Let us consider N=100time steps (k=1,2,3,,N)with Δt=1. M=100Monte-Carlo runs were conducted with the following initial guesses:

x̂0+=x0+normrnd05050E39
P0+=50200502E40

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 5050.

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 Hhas zero-diagonal terms that has zero effect on the state correction. In this case, the measurement is called ambiguous  and this ambiguous measurement often causes filter degradation and divergence even in nonlinear filtering techniques. With highly nonlinear terrain models, TRN systems have recently been constructed with other nonlinear filtering methods such as point-mass filters and particle filters, rather than extended Kalman filters.

Source code of MATLAB implementation for this example can be found in . 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.

## More

© 2018 The Author(s). Licensee IntechOpen. This chapter is distributed under the terms of the Creative Commons Attribution 3.0 License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

## How to cite and reference

### Cite this chapter Copy to clipboard

Youngjoo Kim and Hyochoong Bang (November 5th 2018). Introduction to Kalman Filter and Its Applications, Introduction and Implementations of the Kalman Filter, Felix Govaers, IntechOpen, DOI: 10.5772/intechopen.80600. Available from:

### Related Content

#### Introduction and Implementations of the Kalman Filter

Edited by Felix Govaers

Next chapter

#### Tuning of the Kalman Filter Using Constant Gains

By Mudambi R. Ananthasayanam

#### Advances in Wavelet Theory and Their Applications in Engineering, Physics and Technology

Edited by Dumitru Baleanu

First chapter

#### Real-Time DSP-Based License Plate Character Segmentation Algorithm Using 2D Haar Wavelet Transform

By Zoe Jeffrey, Soodamani Ramalingam and Nico Bekooy

We are IntechOpen, the world's leading publisher of Open Access books. Built by scientists, for scientists. Our readership spans scientists, professors, researchers, librarians, and students, as well as business professionals. We share our knowledge and peer-reveiwed research papers with libraries, scientific and engineering societies, and also work with corporate R&D departments and government entities.

View all Books