저작자표시-비영리-변경금지 2.0 대한민국

이용자는 아래의 조건을 따르는 경우에 한하여 자유롭게

l 이 저작물을 복제, 배포, 전송, 전시, 공연 및 방송할 수 있습니다.

다음과 같은 조건을 따라야 합니다:

l 귀하는, 이 저작물의 재이용이나 배포의 경우, 이 저작물에 적용된 이용허락조건을 명확하게 나타내어야 합니다.

l 저작권자로부터 별도의 허가를 받으면 이러한 조건들은 적용되지 않습니다.

저작권법에 따른 이용자의 권리는 위의 내용에 의하여 영향을 받지 않습니다.

이것은 이용허락규약(Legal Code)을 이해하기 쉽게 요약한 것입니다.

Disclaimer

저작자표시. 귀하는 원저작자를 표시하여야 합니다.

비영리. 귀하는 이 저작물을 영리 목적으로 이용할 수 없습니다.

변경금지. 귀하는 이 저작물을 개작, 변형 또는 가공할 수 없습니다.

http://creativecommons.org/licenses/by-nc-nd/2.0/kr/legalcodehttp://creativecommons.org/licenses/by-nc-nd/2.0/kr/

공학박사 학위논문

Point Mass Filter Based Terrain

Referenced Navigation Using Slant

Range Measurement

경사거리 측정치를 사용하는 PMF 기반

지형대조항법

2018년 8월

서울대학교 대학원

기계항공공학부

전 현 철

Point Mass Filter Based Terrain Referenced Navigation Using Slant

Range Measurement

경사거리 측정치를 사용하는 PMF 기반

지형대조항법

지도교수 박 찬 국

이 논문을 공학박사 학위논문으로 제출함

2018년 8월

서울대학교 대학원

기계항공공학부

전 현 철

전현철의 공학박사 학위논문을 인준함

2018년 8월

위 원 장 : (인)

부위원장 : (인)

위 원 : (인)

위 원 : (인)

위 원 : (인)

Point Mass Filter Based Terrain Referenced Navigation Using Slant Range Measurement

A Dissertationby

Hyun Cheol Jeon

Submitted to the Department of Mechanical and Aerospace Engineering in partial fulfillment of the requirements for the degree of

DOCTOR OF PHILOSOPHY

In Aerospace Engineering at the

SEOUL NATIONAL UNIVERSITY

August 2018

Approved as to style and content by:

Prof. Youdan KimDept. of Mechanical and Aerospace Engineering, Chairman of Committee

Prof. Chan Gook ParkDept. of Mechanical and Aerospace Engineering, Principal Advisor

Prof. Changdon KeeDept. of Mechanical and Aerospace Engineering

Prof. Hyochoong BangDept. of Aerospace Engineering, KAIST

Prof. Jayhyun KwonDept. of Geoinformatics, University of Seoul

i

Abstract

Point Mass Filter Based Terrain Referenced Navigation Using Slant

Range Measurement

Hyun Cheol Jeon

Department of Mechanical and Aerospace Engineering

The Graduate School

Seoul National University

In this dissertation, accurate and efficient terrain referenced navigation (TRN)

algorithm based on point mass filter (PMF) is proposed when a slant range is

measured. The INS/GNSS, which integrates an inertial navigation systems (INS)

and a global navigation satellite systems has been widely used because it combine

the advantages of both systems to provide fast update rated and bounded position

error. However, the GNSS is vulnerable to signal disturbances, such as jamming or

spoofing, so the TRN has been investigated to replace the traditional navigation

methods.

The TRN estimates a vehicle’s position by comparing the measured terrain

information with digital elevation map (DEM) data loaded onboard before flight.

Generally, a radar altimeter (RA) and a barometric altimeter (BA) have been used

as sensors for measuring terrain information. Also, nonlinear filtering techniques

such as the PMF have been applied to the sequential processing TRN because the

ii

terrain measurement is expressed nonlinearly. However, the RA has a disadvantage

in that the measurable range is short and the accuracy is degraded as the flight

altitude increases. The PMF estimates the states by calculating a probability

distribution at grid points which are placed in the state space with regular interval

and it is well known that more accurate results can be obtained in the grid

condition that high resolution and wide area. In this condition, the computational

load of the PMF increases drastically hence the PMF is used in a limited manner.

In order to overcome these limitations of the conventional TRN, the algorithm

of performing the TRN using an interferometric radar altimeter (IRA) is proposed

in this dissertation. The IRA is a range sensor that measures the distance to the

nearest point of the vehicle and it measures slant range unlike the RA, which

measures the vertical downward range. Therefore, to apply the IRA to the TRN, it

is necessary to develop an algorithm different from the conventional TRN that only

considers vertical range and error since the IRA outputs slant range and angles. As

stated, it is generally known that the grid with high resolution and wide area can

improve the performance of the PMF. However, no matter how good grid condition

is applied, the accuracy of the estimation results is limited by the uncertainties of

the models since the PMF is model based filtering technique.

In this dissertation, it is proposed that a grid design method considering

system and measurement model uncertainties to provide the best performance with

minimum computational load when the PMF based TRN (PTRN) is performed.

When performing the grid design considering the system uncertainty, the prediction

effect that a resultant prior distribution appears to be lowered and spread over

larger area should be shown, hence sufficiently high resolution and large support

are needed. When performing the grid design considering the measurement

iii

uncertainty, on the other hands, it should be considered whether the measurement

could distinguish between the terrain elevations of two consecutive grid points. By

choosing the higher resolution and the support designed by the system of both

designs, the PTRN can be performed up to about 60 times faster, while maintaining

similar performance to existing methods.

In addition, it is shown that the PTRN can be performed by using the slant

range measurement by deriving the measurement model reflecting the

characteristics of the IRA. The IRA outputs the range to the target point and two

angle information. These outputs contain measurement errors, and these errors can

be expressed as horizontal and vertical error in the navigation frame. The

horizontal errors can be combined with terrain information to be reflected to

measurement uncertainty existed in vertical direction. It is derived a new

measurement model that uses terrain elevation like a conventional TRN and its

measurement uncertainty that reflects the slant range errors to the vertical direction.

When the proposed measurement model is applied to perform PTRN using IRA,

the performance improvement is about 37% higher than the existing PTRN using

RA. Therefore, from the proposed studies, it is expected that accurate and efficient

PTRN using slant range measurement can be performed.

Keywords: Inertial navigation, Terrain referenced navigation, Terrain aided

navigation, Nonlinear filter, Bayesian recursive relations, Point mass filter, Grid

design, Interferometric radar altimeter

Student number: 2013-30210

iv

Contents

Chapter 1 Introduction........................................................................1

1.1 Motivation and Background....................................................................1

1.2 Objectives and Contributions ..................................................................6

1.3 Organization of the Dissertation..............................................................9

Chapter 2 Preliminaries for Terrain Referenced Navigation ........... 11

2.1 Nonlinear Filtering Techniques ............................................................. 11

2.1.1 Bayesian Recursive Relations .................................................. 11

2.1.2 Kalman Filter ..........................................................................14

2.1.3 Extended Kalman Filter...........................................................21

2.1.4 Point Mass Filter .....................................................................26

2.2 Typical Approaches to Filter Based TRN ..............................................31

2.2.1 Navigation Components for TRN ............................................32

2.2.2 EKF Based TRN .....................................................................35

2.2.3 PMF Based TRN.....................................................................40

2.2.4 Performance Comparison of TRN algorithms ..........................42

Chapter 3 Grid Design for PMF Based TRN ....................................49

3.1 Previous Grid Design Methods .............................................................49

3.1.1 Grid Design by Point Elimination............................................50

3.1.2 Anticipative Grid Design.........................................................53

3.2 Proposed Grid Design Considering Model Uncertainty .........................56

3.2.1 Time Update Using Convolution Kernel ..................................57

v

3.2.2 Effects of Model Uncertainties ................................................60

3.2.3 Determining Grid Conditions ..................................................67

3.3 Numerical Evaluation of the Proposed Grid Design ..............................71

3.4 Summary..............................................................................................89

Chapter 4 Derivation of Measurement Model for Slant Range .......92

4.1 Modified Measurement Model for Slant Range.....................................92

4.1.1 Simple One Dimensional Case ................................................92

4.1.2 General Three Dimensional Case.............................................98

4.2 PTRN Performance Analysis ..............................................................105

4.3 Summary............................................................................................ 117

Chapter 5 Conclusion ...................................................................... 119

Bibliography.......................................................................................... 122

Appendix. A Coefficients of Measurement Variance ........................... 134

국문초록 ................................................................................................ 138

vi

List of Figures

Figure 1.1 Block Diagram of the PTRN.................................................................9

Figure 2.1 Expressions of the pdf in PMF............................................................28

Figure 2.2 General PMF algorithm ......................................................................30

Figure 2.3 Terrain information as a measurement in the TRN ..............................34

Figure 2.4 IRA measurements by view position ...................................................35

Figure 2.5 Conceptual explanation of ETRN .......................................................39

Figure 2.6 Conceptual explanation of PTRN........................................................40

Figure 2.7 Terrain profile and flight trajectory .....................................................46

Figure 2.8 Simulation results under Gaussian RA noise .......................................47

Figure 2.9 Simulation results under non-Gaussian RA noise ................................48

Figure 3.1 Concept of grid design proposed by Bergman .....................................50

Figure 3.2 Grid redesign by elimination and interpolation....................................52

Figure 3.3 Grid design in rotated coordinate system.............................................54

Figure 3.4 Time update of general PMF...............................................................58

Figure 3.5 Propagated points of linear system model ...........................................59

Figure 3.6 Similarity test results of the time update according to grid resolution ..62

Figure 3.7 Effect of measurement noise on resolution determination....................63

Figure 3.8 Relation between measurement noise and position error......................64

Figure 3.9 Likelihood test where the peak occur according to measurement

uncertainty .......................................................................................66

Figure 3.10 Latitude estimation of the PTRN according to the resolution.............75

Figure 3.11 Longitude estimation of the PTRN according to the resolution ..........76

vii

Figure 3.12 Latitude estimation of the PTRN according to the support.................79

Figure 3.13 Longitude estimation of the PTRN according to the support..............80

Figure 3.14 Representation area of the probability distribution according to the size

of the support ...................................................................................81

Figure 3.15 Simulation time according to resolutions ..........................................81

Figure 3.16 Comparison of the PTRN results based on various grid design methods

according to the system uncertainties when the precise measurement is

given..............................오류! 책갈피가 정의되어 있지 않습니다.

Figure 3.17 Comparison of the PTRN results based on various grid design methods

according to the system uncertainties when the noisy measurement is

given..............................오류! 책갈피가 정의되어 있지 않습니다.

Figure 4.1 Conceptual explanation of PTRN using slant range measurement

without measurement errors .............................................................94

Figure 4.2 Conceptual explanation of PTRN using noisy slant range measurement

........................................................................................................95

Figure 4.3 Various expressions of slant range measurement ...............................101

Figure 4.4 IRA measurement in PTRN simulation .............................................107

Figure 4.5 Terrain profiles measured by the IRA according to flight altitude ......108

Figure 4.6 Terrain profiles along the latitude......................................................109

Figure 4.7 PTRN results according to various conditions................................... 111

Figure 4.8 Horizontal and vertical errors caused by slant range errors and those

error bound calculated by the proposed method .............................. 112

Figure 4.9 Terrain profile of the RA-PTRN along the latitude ............................ 115

Figure 4.10 Standard deviations of the measurement uncertainty in the RA-PTRN

and the IRA-PTRN ........................................................................ 115

viii

Figure 4.11 Results of the RA-PTRN and IRA-PTRN ....................................... 116

ix

List of Tables

Table 2.1 Summary of Kalma filter algorithm......................................................20

Table 2.2 Summary of EKF algorithm .................................................................24

Table 2.3 Specifications of the navigation grade IMU..........................................44

Table 2.4 Simulation conditions ..........................................................................44

Table 2.5 Standard deviation values of the measurement noise.............................45

Table 2.6 Grid condition for PTRN......................................................................45

Table 3.1 Specifications of the navigation grade IMU used in PTRN simulation ..72

Table 3.2 Simulation conditions ..........................................................................73

Table 3.3 Grid and noise conditions for PTRN simulations ..................................73

Table 3.4 The PTRN results based on various grid design methods when the precise

measurement is given.......................................................................85

Table 3.5 The PTRN results based on various grid design methods when the

inaccurate measurement is given ......................................................87

Table 4.1 Simulation conditions for the PTRN using slant range........................105

Table 4.2 Specifications of navigation grade IMU for PTRN simulations...........105

Table 4.3 Slant range errors for PTRN simulations ............................................106

Table 4.4 Specifications of RA .......................................................................... 114

x

Abbreviations

AGD Anticipative Grid Design

BA Barometric Altimeter

BGD Boundary based Grid Design

BRR Bayesian Recursive Relations

DBRN DataBase Referenced Navigation

DCM Direction Cosine Matrix

DEM Digital Elevation Map

DTED Digital Terrain Elevation Data

ECEF Earth Centered Earth Fixed

EKF Extended Kalman Filter

ETRN EKF based Terrain Referenced Navigation

FOV Field Of View

GNSS Global Navigation Satellite System

IMU Inertial Measurement Unit

INS Inertial Navigation System

IRA Interferometric Radar Altimeter

LiDAR Light Detection And Ranging

pdf Probability density function

PF Particle Filter

PMF Point Mass Filter

PTAN Precision Terrain Aided Navigation

PTRN PMF based Terrain Referenced Navigation

RA Radar Altimeter

xi

SDINS StrapDown Inertial Navigation System

SRTM Shuttle Radar Topography Mission

TRN Terrain Referenced Navigation

UKF Unscented Kalman Filter

1

1.1 Motivation and Background

An inertial navigation system (INS) is a traditional navigation system that

measures acceleration and angular velocity using an inertial measurement unit

(IMU), and calculates the position of a vehicle through integration of the measured

values. It is possible to update the navigation information quickly for the INS due

to the IMU's fast sampling rate and it provides fairly accurate results for a short

period of time, but there is a disadvantage that the acceleration and the angular

velocity errors included in the IMU outputs are continuously accumulated. This

causes navigation information to diverge over time [1-3]. To solve this problem, the

INS/GNSS has been developed which integrates the INS with global navigation

satellite system (GNSS) . The GNSS is a navigation system that determines the

vehicle’s position by receiving navigation information from satellites. It has low

update rate but its errors do not diverge over time. Therefore, the INS/GNSS

combines the advantages of each navigation system with fast update rate and

bounded error characteristics, and has been widely used [4-7]. However, as

intentional signal disturbance such as jamming/spoofing which causes failure of the

GNSS recently become a problem [8-11], an alternative navigation system that can

Chapter 1

Introduction

2

replace the INS/GNSS has been required. Databased referenced navigation (DBRN)

is one of the alternatives. The DBRN uses geophysical information that has fixed

value at certain location on the earth such as those containing gravity [12-18] or

geomagnetic information [19-24] etc. to estimate vehicle position.

Terrain referenced navigation (TRN) is one of the DBRN algorithms that uses

terrain information [25-29]. The TRN estimates a vehicle’s position by comparing

the measured terrain information with digital elevation map (DEM) data loaded

onboard before flight. Generally, the terrain information is measured using

barometric altimeter (BA) and radar altimeter (RA). There are two representative

TRN algorithms with different measurement processing methods: batch processing

and sequential processing TRN. The batch processing TRN achieves its position fix

by matching the measurement profile that consists of measurements and candidate

profiles that consist of the DEM values [30-38]. The sequential processing TRN,

which is mainly dealt with in this dissertation, performs position correction with a

filtering technique whenever terrain information is acquired.

Various researches have been performed to improve the TRN results.

Especially in the sequential processing TRN, various filters have been applied.

Because the terrain is expressed nonlinearly, many nonlinear filtering techniques

have been applied to the TRN to use the terrain measurement; local methods

including extended Kalman filter (EKF) [39-43] and unscented Kalman filter (UKF)

[42, 44-46] etc., and global methods including particle filter (PF) [47-53] and point

mass filter (PMF) [54-64]. The local methods assume the Gaussian noise and

describe the probability distributions by some local points and these characteristics

can cause degraded estimation results or filter divergence under the highly

nonlinearity although the local methods estimates the states analytically with small

3

computational load [65, 66]. By replace the local methods to global methods, these

problems can be solved. The representative global method is Bayesian estimation

or Bayesian recursive relations (BRR). The BRR describes entire probability

density function (pdf) based on the Bayes’ rule and estimates states using the

computed pdf [42, 67, 68]. However, it is possible to express the pdf analytically

based on the BRR for only a few cases, e.g. Kalman filter [69]. Since it is difficult

to express the pdf analytically for most nonlinear/non-Gaussian problems,

numerical methods have been studied, e.g. the PF and the PMF.

The PF is a nonlinear filtering technique that approximates the pdf and

estimates states using Monte Carlo integration. The particles representing the state

vector of a specific value are stochastically distributed in the state space and the

probability, or weight, at the particle is computed [47, 70]. The PMF is also a

numerical nonlinear filter that calculates weights at points representing the state

vector and distributed in the state space like as PF. Unlike the PF, the PMF is a

grid-based filter that expresses probability distributions at points with regular

interval, i.e. grid [61, 69]. The PF is advantageous in that the computational load is

smaller than that of the PMF because the particles are stochastically sampled

without considering the relations between particles. However, due to the nature of

these stochastic algorithm, additional algorithms are needed to solve the

degeneracy and sample impoverishment; the degeneracy is that weights are

concentrated at one particle and the sample impoverishment is that the particles are

concentrated around the peak of the probability distribution. In addition, there is a

disadvantage in that the stochastic characteristic causes nondeterministic behavior

for repetitive processing of the same experimental data . On the other hand, the

PMF has a disadvantage in that it has a larger computational load than that of the

4

PF because it needs to consider the relations between grid points to calculate the

prior distribution during the time update process. As opposed to the PF, it is

mathematically very simple and easy to express the entire probability distribution

because it expresses the distribution on the grid points arranged uniformly in the

state space designed by user [55, 61, 71, 72]. This characteristic is advantageous

when the probability distribution has significant tail or multi-modality. Also, it is

possible to obtain consistent results for the same data due to its deterministic nature

[55, 72].

Due to these merits of the PMF, many researches have been performed on the

PMF and PMF based TRN (PTRN), particularly grid design because the

computational load and the estimation accuracy depend on the grid. Bergman [55-

57] proposed a way to reduce the computational load by eliminating gird points

with negligible weight. The method is that after calculating the posterior

distribution, delete the points where the weight is smaller than the predetermined

threshold, and if the number of remaining grid points is within the minimum and

maximum range set by the user, filtering is continued. If the number of remaining

points is smaller than the minimum value, the resolution is doubled, and the

weights at the newly generated points are computed through interpolation. If the

number of remaining points is larger than the maximum value, the resolution is

halved, and some points are deleted to keep the computational load within a certain

range. However this method can reduce the grid area overt time, and there is a

possibility that the probability distribution cannot be expressed entirely at the next

time step if the system uncertainty is large. Also, the grid area may distorted after

point removal, which can reduce the accuracy of state estimation. Šimandl [58-61]

proposed a method to set the grid by roughly predicting the mean and covariance of

5

the prior distribution based on the propagated grid points and the system model

uncertainty. Then, the eigenvalue decomposition is performed using the predicted

covariance to find the principal axes of the predicted distribution, and the grid

design is performed based on this axes. This method can express the prior

distribution as well as possible with minimum points but there is a disadvantage

that only the system model uncertainty is considered although both the system and

measurement model uncertainties act on the estimation accuracy.

In addition to improving the filtering techniques, there has been an attempt to

improve the TRN performance by applying a new sensor that gives much more

precise information than that of the RA. Most TRN studies have been performed in

a manner that uses the RA to measure the terrain information located vertically

below the vehicle under the assumption that there is no attitude change. However,

it is possible that the RA may measure the slant range instead of the vertical range

due to the attitude or attitude error of the vehicle. In addition, various kinds of

sensors such as light detection and ranging (LiDAR) [73-75] and interferometric

radar altimeter (IRA) [76-80] have been applied to improve the performance of

TRN, recently [78, 81-94]. Unlike the RA, these sensors measure the slant range

although the attitude of the vehicle is not changed. Therefore, the characteristics of

these sensors should be considered when performing the TRN using the new

sensors.

Some researches which consider the effect of the vehicle attitude performed

[93, 94]. These researches reveal the cause of the slant range measurement by the

RA and propose how to compensate it. However, it is limited to the batch

processing TRN that selectively uses the RA measurements by using a

measurement validity evaluation according to the geometric effects. Vadlamani et

6

al. [86-89] investigated the TRN using LiDAR. These researches proposed

converting the coordinate of the terrain location measured by LiDAR for the local

navigation frame. However, it is also limited to the batch processing TRN and

lacks the performance analysis according to the LiDAR measurement errors.

Honeywell Inc. developed a precision terrain aided navigation (PTAN) [81] that

uses the IRA to overcome the limitation of a conventional RA that cannot measure

the range at a high altitude . However, the details of the PTAN have not been

revealed to the public. Recently, studies on TRN using IRA have been

performed[78, 83], but this method is related to PF based TRN, and it is necessary

to calculate the slant ranges from each particle using given measurement

information. In addition, it is expected that the computational load of this method is

large because it estimates the flight altitude of the vehicle unlike other PF based

TRN which estimates 2D position to calculate slant ranges at each particle.

1.2 Objectives and Contributions

The main goal of this dissertation is to improve the performance of the PTRN

using slant range measurement, especially, IRA. The contribution of this study is as

follows. First, grid design method shown in Figure 1.1 for efficient and accurate

PMF based TRN is proposed. As mentioned in section 1.1, it is known that more

accurate estimation results can be obtained as the integral in the BRRs is

approximated precisely; grid with very small interval in very large area. However,

it is very inefficient to perform the PMF with this grid condition because extremely

large computational load is generated in the time update process. In addition, no

7

matter how good grid condition is applied, the accuracy of the estimation results is

limited by the uncertainties of the models since the PMF is model based filtering

technique. Therefore, even if the grid condition is improved, only the

computational load is increased, and meaningful performance improvement cannot

be obtained. In this dissertation, in order to prevent such computational inefficiency

and to reach the maximum achievable performance from a given models, grid

design method for the PTRN is proposed considering system and measurement

uncertainties. The proposed grid design makes it possible to obtain almost the same

accuracy as the results that are obtained when a very large area and very high

resolution are applied with a much lower computational load, and it is expected to

enable the use of the real-time PTRN.

Second, a method for implementing the PTRN by applying the slant range

measurement that gives very precise range and angle information, i.e. the IRA, is

proposed. The previous TRN using the RA is assumed to measure a vertical range,

but the IRA measures slant range because it finds closest location to the vehicle

within the field of view (FOV). Hence, it is impossible to use conventional

measurement model that describes vertical range. In this dissertation, a modified

measurement model used to calculate likelihood shown in Figure 1.1 considering

the slant range measurement is proposed. In the proposed measurement model, the

terrain information shifted by slant range from the vehicle position is considered.

Also, unlike the conventional RA measurement model, the slant range

measurement must reflect the range and angle information to measurement

variance because the effect of the angle information including the measurement and

attitude is amplified by the flight altitude. It is shown that accurate and robust TRN

can be performed by appropriately reflecting the variance that changes according to

8

altitude and terrain conditions.

9

Figure 1.1 Block Diagram of the PTRN

1.3 Organization of the Dissertation

Chapter 1 provides the motivation and background of this dissertation as well

10

the objective and contributions. In Chapter 2, detailed information to understand

the main contributions of this dissertation is provided including nonlinear filters,

and typical sequential processing TRN algorithms. Chapter 3 describes the grid

design method for the PMF. The conventional grid design methods proposed by

Bergman and Simandl are explained. Also, the new grid design method considering

model uncertainties is described in detail and it is shown that the novelty of the

proposed grid design by the PTRN simulations. Chapter 4 provides detailed

derivation of a modified measurement model for slant range measurement. It is

shown how to reflect on horizontal and vertical distance to the measurement model

caused by the slant range. Also, the influence of the vehicle attitude and the

measurement angle to the slant range is separated and measurement variance that

reflects the attitude errors and the measurement angle errors is proposed. Chapter 5

gives conclusions.

11

2.1 Nonlinear Filtering Techniques

The TRN is a navigation system that corrects the position of a vehicle using

terrain information based on the filtering techniques. One of the most popular filter

is Kalman filter which estimates states based on linear system and measurement

models. It has advantages that it is easy for implementation and provides optimal

results for linear models in the sense that it is unbiased and a minimum mean

square errors [42]. However, it is difficult to apply the Kalman filter to the TRN

because the terrain information is nonlinear quantity. Therefore, nonlinear filtering

techniques that can handle nonlinear models is needed. In this section, Bayesian

recursive relations (BRRs) that is the foundation for filtering techniques is

introduced. Also, based on the BRRs, Kalman filter is introduced which is one of

the BRRs derived analytically, and the most popular nonlinear filter, EKF, and one

of the representative numerical BRRs, PMF, are introduced.

2.1.1 Bayesian Recursive Relations

The purpose of the estimation theory is to estimate the object to be estimated,

e.g. the state variables, as accurately as possible. The estimation results are based

Chapter 2

Preliminaries for Terrain Referenced Navigation

12

on observations or measurements. Let x be the object to be estimated in the

Bayesian context and y be the measurement. x and y are considered to be random

variables, and let p(x), p(y) be the pdf describing the random variables, respectively.

The Bayes' law provides a tool to calculate the conditional pdf for x given a

measurement y, as follows.

( )( ) ( )

( )

( ) ( ) ( )

||

|

p y x p xp x y

p y

p y p y x p x dx

=

= ò

(2.1)

Based on Eq. (2.1), it is possible to perform filtering on the discretized

nonlinear system and measurement models defined by,

( )

( )1k k k k

k k k k

+ = +

= +

x f x w

y h x v(2.2)

where ,k kx y are states and measurement vectors, respectively. ( ), ( )k k× ×f h are

nonlinear system and measurement models, respectively. ,k kw v are uncertainties

of the system and measurement models, respectively including noises and

unmodeled effects. These noise sequences are assumed to be white and

independent like,

( ) ( ) ( )

( ) ( ) ( )( ) ( ) ( )

, , 0

, , 0

,

k i k k i k

k j k k j k

k k k k

p p p i

p p p j

p p p

+ +

+ +

= " ¹

= " ¹

=

w w w w

v v v v

w v w v

(2.3)

The purpose of the BRRs is to calculate the conditional pdf given models Eq.

(2.2) and conditions Eq. (2.3), and this is called posterior pdf ( | )k kp X Y .

13

{ }0 1, , ,k k=X x x xK and { }0 1, , ,k k=Y y y yK are the stacked vector of all the

states and measurement up to time step k. Based on Eq. (2.2), (2.3), the states kx

is the first order Markov process, which is only affected by the previous states

1k-x , and the pdf of kx is represented as,

( ) ( ) ( )10

|k

k k t tt

p p p -=

= =ÕX x x x (2.4)

Therefore, given Eq. (2.2), (2.3), the posterior pdf is expressed as Eq. (2.5)

and this is called measurement update.

( )( ) ( )

( )

( ) ( ) ( )

1

1

1 1

| ||

|

| | |

k k k kk k

k k

k k k k k k k

p pp

p

p p p d

-

-

- -

=

= ò

y x x Yx Y

y Y

y Y y x x Y x

(2.5)

The time update is calculated by the law of total probability as follows.

( ) ( ) ( )1 1| | |k k k k k k kp p p d+ += òx Y x x x Y x (2.6)

To perform a time update and a measure update, it is essential to know the

pdfs of the likelihood ( | )k kp y x and the transition prior 1( | )k kp +x x . Although it

is known to calculate the likelihood and transition priorities for a general nonlinear,

non-Gaussian model [69], this dissertation considers additive Gaussian noise,

hence the pdfs are represented like,

( ) ( )( )( ) ( )( )

1 1|

|

k

k

k k k k k

k k k k k

p p

p p

+ += -

= -

w

v

x x x f x

y x y h x(2.7)

Theoretically, all the filtering problems can be solved by the BRRs, but it is

difficult to perform Eq. (2.5) and (2.6), analytically, so alternative methods to solve

14

these problems have been studied.

2.1.2 Kalman Filter

In this section, it is provided the derivation of Kalman filter based on the

BRRs for the following linear system and measurement models.

1k k k k

k k k k

+ = +

= +

x F x w

y H x v(2.8)

where, ,k kx y are states and measurements that has dimension of ,x yn n ,

respectively. ,k kw v are system and measurement uncertaines, respectively which

have the properties in Eq. (2.3). ,k kw v has same dimension with ,k kx y ,

respectively and each noise has normal distribution like,

( )

( )

~ 0,

~ 0,

k k

k k

N

N

w Q

v R(2.9)

First, consider the time update Eq. (2.6). Let assume that the posterior pdf at

time step k is ( )| |ˆ; ,k k k k kN x x P and

( )( )

( ) ( )1| | |/2|

1 1ˆ ˆ| exp

22 detxT

k k k k k k k k k kn

k k

pp

-é ù= - - -ê úë ûx Y x x P x x

P(2.10)

For a linear system model, the transition prior is defined as follows.

( ) ( ) ( )

( )( ) ( )

1 1 1

11 1/ 2

| ; ,

1 1exp

22 det

k

x

k k k k k k k k k

T

k k k k k k kn

k

p p N

p

+ + +

-+ +

= - = -

é ù= - - -ê ú

ë û

wx x x F x x F x 0 Q

x F x Q x F xQ

(2.11)

Substituting Eq. (2.10), (2.11) into Eq. (2.3),

15

( )

( )

( ) ( ) ( ) ( )

1

|

1 1| | | 1 1

|

1

2 det det

1 1ˆ ˆexp

2 2

x

k k

n

k k k

T T

k k k k k k k k k k k k k k k k

p

d

p

-

- -+ +

=é ù

´ - - - - - -ê úë û

ò

x Y

P Q

x x P x x x F x Q x F x x

(2.12)

Define the variables in Eq. (2.12) as,

|

1 1 |

ˆ

ˆ

k k k k

k k k k k+ +

= -

= -

x x x

x x F x

%(2.13)

Eq. (2.13) is substituted into Eq. (2.12), and the inside of the exponential is

expanded as,

( ) ( ) ( ) ( )

( ) ( )

1 1| | | 1 1

1 1| 1 1

ˆ ˆT T

k k k k k k k k k k k k k k k

TTk k k k k k k k k k k

- -+ +

- -+ +

- - + - -

= + - -

x x P x x x F x Q x F x

x P x x F x Q x F x% % % %(2.14)

Express Eq. (2.14) in the form of augmented matrix like,

( ) ( )1 1| 1 11|

11 1

1 1 1|

1 11 1

TTk k k k k k k k k k k

T T

k kk k

k k k kk

T T Tk kk k k k k k k

k kk k k

- -+ +

-

-+ +

- - -

- -+ +

+ - -

é ùé ù é ù é ù é ù= ê úê ú ê ú ê ú ê ú- -ë û ë û ë û ë ûë û

é ù+ -é ù é ù= ê úê ú ê ú

-ë û ë ûë û

x P x x F x Q x F x

x I 0 I 0 xP 0

x F I F I x0 Q

x xP F Q F F Q

x xQ F Q

% % % %

% %

% %

(2.15)

Since the integral of Eq. (2.12) is performed on the random variable kx , kx

should be separated independently for simplicity. Hence, rearrange the second

matrix in Eq. (2.15).

16

11 1 1|

11 11|

TT Tkk kk k k k k k k

k kk k k

-- - -

-- -+

é ùé ù - -+ - é ù é ù= ê úê ú ê ú ê ú

- ê úë û ë ûë û ë û

Θ 0I L I LP F Q F F Q

0 P0 I 0 IQ F Q(2.16)

where

( )

1 1 1|

1

11 1 1 1 11|

Tk k k k k k

Tk k k k

Tk k k k k k k k

- - -

-

-- - - - -+

= +

=

= -

Θ P F Q F

L Θ F Q

P Q Q F Θ F Q

(2.17)

By matrix inversion lemma [42], 1|k k+P in Eq. (2.17) is expressed as,

1| |T

k k k k k k k+ = +P F P F Q (2.18)

The determinant of Eq. (2.12) can be summarized as follows using properties

of block matrices [95].

11 1 || 1

|

1|

1

1|

1

1det det det

det det

det det det

det

k kk k k

k k k k

T

k k

k kk

T

k k

k kk

-- -

-

-

-

-

-

æ öé ù= = ç ÷ê úç ÷

ë ûè ø

æ ö æ ö æ öé ùé ù é ùç ÷= ç ÷ ç ÷ê úê ú ê úç ÷ç ÷- -ë û ë ûë û è øè øè ø

æ öé ùé ù é ùç ÷= ê úê ú ê úç ÷- -ë û ë ûë ûè ø

P 0P Q

P Q 0 Q

I 0 I 0P 0

F I F I0 Q

I 0 I 0P 0

F I F I0 Q

(2.19)

By equality in Eq. (2.16), the determinant of Eq. (2.12) is expressed like,

1|

1

1

11|

1 11|

1|

det

det

1det det

det det

T

k k

k kk

T

kk k

k k

k k k

k k k

-

-

-

-+

- -+

+

æ öé ùé ù é ùç ÷ê úê ú ê úç ÷- -ë û ë ûë ûè ø

æ öé ù- -é ù é ùç ÷= ê úê ú ê úç ÷ê úë û ë ûë ûè ø

= =

I 0 I 0P 0

F I F I0 Q

Θ 0I L I L

0 P0 I 0 I

Θ PΘ P

(2.20)

17

Substituting Eq. (2.13) to Eq. (2.20) into Eq. (2.12), the prior pdf 1( | )k kp +x Y

can be computed and the time update process is completed.

( )

( )

( )( ) ( )

( )

1

11 1

11 11|1|

11 | 1| 1 |/2

1|

1 1| 1|

|

1 1exp

22 det det

1 1ˆ ˆexp

22 det

ˆ; ,

x

x

k k

T

k k k k k kk

knk kk kk k k

T

k k k k k k k k k kn

k k

k k k k k

p

d

N

p

p

+

-+ +

-+ +++

-+ + +

+

+ + +

é ùé ù- -é ù é ù= -ê úê úê ú ê ú

ê úê úë û ë ûë ûë û

é ù= - - -ê ú

ë û

=

ò

x Y

x L x x L xΘ 0x

x x0 PΘ P

x F x P x F xP

x x P

% %

(2.21)

where

( )( ) ( )11 1/2

1 1exp 1

22 detxT

k k k k k k k kn

k

A dp

-+ +

é ù= - - - =ê ú

ë ûò x L x Θ x L x x

Θ% %

(2.22)

The measurement update process in Eq. (2.5) is also derived in a similar way

to time updates. The likelihood in Eq. (2.5) are represented as,

( ) ( ) ( )11 1 1 1 1 1 1 1 1

| ; ,kk k k k k k k k k

p p N++ + + + + + + + +

= - = -wy x y H x y H x 0 R (2.23)

Then, by substituting the prior pdf and the likelihood, the posterior pdf is

( )( )

( )

( ) ( ) ( ) ( )

1

11 1 ( ) 2

1| 1

1 11 1 1 1 1 1 1 1| 1| 1 1|

||

2 det det

1 1ˆ ˆexp

2 2

x y

k kk k n n

k k k

TT

k k k k k k k k k k k k k k k

pp

p

-

++ + +

+ +

- -+ + + + + + + + + + +

=

é ù´ - - - - - -ê ú

ë û

y Yx Y

P R

y H x R y H x x x P x x

(2.24)

where the evidence 1( | )k kp +y Y is

18

( )

( )

( ) ( )

( ) ( )

1

( ) 2

1| 1

11 1 1 1 1 1

11 1| 1| 1 1| 1

|

1

2 det det

1exp

2

1ˆ ˆexp

2

x y

k k

n n

k k k

T

k k k k k k k

T

k k k k k k k k k

p

d

p

-

+

+ +

-+ + + + + +

-+ + + + + +

é ù= ´ - - -ê úë û

é ù´ - - -ê ú

ë û

ò

y Y

P R

y H x R y H x

x x P x x x

(2.25)

The same derivation process is used to calculate the evidence. Define the

variable as follows.

1 1 1|

1 1 1 1|

ˆ

ˆ

k k k k

k k k k k

+ + +

+ + + +

= -

= -

x x x

x y H x

%(2.26)

Eq. (2.26) is substituted into Eq. (2.25), and the inside of the exponential is

expanded as follows.

( ) ( ) ( ) ( )1 11 1 1 1 1 1 1 1| 1| 1 1|1

1 11|

11 1 1 11

11 1 1| 1

11 1

ˆ ˆTT

k k k k k k k k k k k k k k k

T T

k kk k

k k k kk

T T

k k k k

k k

- -+ + + + + + + + + + +

-+ ++

-+ + + ++

-+ + + +

-+ +

- - + - -

é ùé ù é ù é ù é ù= ê úê ú ê ú ê ú ê ú- -ë û ë û ë û ë ûë û

é-é ù é ù= ê ú ê ú

ë ûë û ë

y H x R y H x x x P x x

x I 0 I 0 xP 0

x H I H I x0 R

x I K P 0

x 0 I 0 S

% %

%11

1

kk

k

++

+

ù - é ùé ùê ú ê úê ú

ë û ë ûû

xI K

x0 I

%

(2.27)

where

1 1 1| 1 1

11 1| 1 1

1| 1 1| 1 1 1

Tk k k k k k

Tk k k k k

Tk k k k k k k

+ + + + +

-+ + + +

+ + + + + +

= +

=

= -

S H P H R

K P H S

P P K S K

(2.28)

The determinant of Eq. (2.25) can be summarized as follows using properties

of block matrices.

19

1 11| 1

1| 1 1| 1 1

1 1det det

det det det detk k k

k k k k k k

- -+ +

+ + + + +

= =P RP R P S

(2.29)

Substituting Eq. (2.23), (2.24) and (2.25) into Eq. (2.20), then the evidence is

( )( )

11 1 1 1/2

1

1 1| exp

22 detyT

k k k k kn

k

pp

-- + + +

+

é ù= -ê ú

ë ûy Y x S x

S(2.30)

Substituting Eq. (2.30) into Eq. (2.24), the posterior pdf is computed and the

measurement update process is completed. Table 2.1 shows the Kalman filter

algorithm.

( )

( )( ) ( )

( )

1 1

11 1 1 1| 1 1 1 1( ) 2

1| 1

1 1| 1 1| 1

|

1 1exp

22 det

ˆ; ,

x

k k

T

k k k k k k k kn

k k

k k k k k

p

N

p

+ +

-+ + + + + + + +

+ +

+ + + + +

é ù= - - -ê ú

ë û

=

x Y

x K x P x K xP

x x P

% %

(2.31)

20

Table 2.1 Summary of Kalma filter algorithm

For linear system and measurement models

1k k k k

k k k k

+ = +

= +

x F x w

y H x v

where

( )

( )

( )| |

~ 0,

~ 0,

ˆ~ ; ,

k k

k k

k k k k k k

N

N

N

w Q

v R

x x x P

Time update

( ) ( )1 1 1| 1|ˆ| ; ,k k k k k k kp N+ + + +=x Y x x P

where

1| |

1| |

ˆ ˆk k k k k

Tk k k k k k k

+

+

=

= +

x F x

P F P F Q

Measurement update

( ) ( )1 1 1 1| 1 1| 1ˆ| ; ,k k k k k k kp N+ + + + + + +=x Y x x P

where

( )1| 1 1| 1 1 1 1|1 1 1| 1 1

11 1| 1 1

1| 1 1| 1 1 1

ˆ ˆ ˆk k k k k k k k k

Tk k k k k k

Tk k k k k

Tk k k k k k k

+ + + + + + +

+ + + + +

-+ + + +

+ + + + + +

= + -

= +

=

= -

x x K y H x

S H P H R

K P H S

P P K S K

21

2.1.3 Extended Kalman Filter

In general, since the BRRs must perform integration, it is difficult to express

analytically for general nonlinear and non-Gaussian problems. The Kalman filter

derived in section 2.2.2 is a very special case where the BRRs is expressed

analytically. Therefore, a method to solve the nonlinear / non-Gaussian problem is

needed.

The EKF is one of the most popular nonlinear filtering techniques. The EKF

linearizes the system and measurement model around the currently estimated states

values and estimates the states by applying a standard Kalman filter algorithm

shown in Table 2.1. However, it is a risky method that the filter can diverge when

the error is large because there is a constraint that the current estimation error

should be small due to the linearization assumption.

Consider the nonlinear system and measurement models with additive

Gaussian noises.

( )

( )1k k k k

k k k k

+ = +

= +

x f x w

y h x v(2.32)

where ,k kw v are system and measurement noises, respectively which have the

properties in Eq. (2.3).

Assume that the currently estimated value ˆ kx is defined as

ˆk k kd= +x x x (2.33)

where kx is true states value and kd x is current states error.

The EKF is divided into two methods; direct method and indirect method.

Both methods are derived through a very similar process. First, let us derive the

22

direct method. Before the filter is applied, the models in Eq. (2.32) should be

linearized. Eq. (2.33) is substituted into the system model in Eq. (2.32).

( )

( )1

ˆ

k k k k

k k k kd

+ = +

= - +

x f x w

f x x w(2.34)

Under the assumption that the current error kd x is small, the Taylor series

expansion can be applied to the function kf in Eq. (2.34) and by ignoring higher

order terms,

( )

( ) ( )

1

ˆ

ˆ

ˆ

ˆ ˆ

k k

k k

kk k k k k

k

kk k k k k

k

d+=

=

¶= - +

¶

¶= - - +

¶

x x

x x

fx f x x w

x

ff x x x w

x

(2.35)

By setting the Jacobian ˆk kk k =

¶ ¶x x

f x as kF , Eq. (2.35) is rearranged by

1k k k k k+ = + +x F x u w (2.36)

where ( )ˆ ˆk k k k k= -u f x F x is known value.

The same process is applied to the measurement model. Substitute Eq. (2.33)

to the measurement model in Eq. (2.32).

( )ˆk k k k kd= - +y h x x v (2.37)

By applying the Taylor series and ignoring higher order terms,

( )

( ) ( )

ˆ

ˆ

ˆ

ˆ ˆ

k k

k k

kk k k k k

k

kk k k k k

k

d=

=

¶= - +

¶

¶= - - +

¶

x x

x x

hy h x x v

x

hh x x x v

x

(2.38)

23

By setting the Jacobian in Eq. (2.38) as kH , the linearization of the

measurement model is completed.

k k k k k= + +y H x ν v (2.39)

where ( )ˆ ˆk k k k k= -ν h x H x is known value.

The indirect method is a method of estimating the states error and reflecting

the estimated result to the state variable. The derivation of the indirect method is

the same with direct method. Substituting the Eq. (2.33) into Eq. (2.32).

( )

( )1 1

ˆ ˆ

ˆ ˆ

k k k k k k

k k k k k k

d d

d d

+ +- = - +

- = - +

x x f x x w

y y h x x v(2.40)

Applying the Taylor series, the linearization of the models is completed.

1k k k k

k k k k

d d

d d+ = -

= -

x F x w

y H x v(2.41)

Because the models are linearized, the Kalman filter algorithm can be applied.

Table 2.2 is summary of the EKF algorithm.

24

Table 2.2 Summary of EKF algorithm

For nonlinear system and measurement models

( )

( )1k k k k

k k k k

+ = +

= +

x f x w

y h x v

where

( )

( )

( )| |

~ 0,

~ 0,

ˆ~ ; ,

k k

k k

k k k k k k

N

N

N

w Q

v R

x x x P

Direct method

System and measurement models

1k k k k k

k k k k k

+ = + +

= + +

x F x u w

y H x ν v

where

ˆ ˆ

,

k k k k

k kk k

k k= =

¶ ¶= =¶ ¶

x x x x

f hF H

x x

( )

( )

ˆ ˆ

ˆ ˆ

k k k k k

k k k k k

= -

= -

u f x F x

ν h x H x

Time update

( ) ( )1 1 1| 1|ˆ| ; ,k k k k k k kp N+ + + +=x Y x x P

where

1| |

1| |

ˆ ˆk k k k k

Tk k k k k k k

+

+

=

= +

x F x

P F P F Q

Measurement update

( ) ( )1 1 1 1| 1 1| 1ˆ| ; ,k k k k k k kp N+ + + + + + +=x Y x x P

where

25

( )1| 1 1| 1 1 1 1|1 1 1| 1 1

11 1| 1 1

1| 1 1| 1 1 1

ˆ ˆ ˆk k k k k k k k k

Tk k k k k k

Tk k k k k

Tk k k k k k k

+ + + + + + +

+ + + + +

-+ + + +

+ + + + + +

= + -

= +

=

= -

x x K y H x

S H P H R

K P H S

P P K S K

Indirect method

System and measurement models

1k k k k

k k k k

d d

d d+ = +

= +

x F x w

y H x v

where

ˆ ˆ

,

k k k k

k kk k

k k= =

¶ ¶= =¶ ¶

x x x x

f hF H

x x

( )

( )

ˆ ˆ

ˆ ˆ

k k k k k

k k k k k

= -

= -

u f x F x

ν h x H x

Time update

( ) ( )1 1 1|| ; ,k k k k kp N d+ + +=x Y x 0 P

where

1|

1| |

ˆk k

Tk k k k k k k

d +

+

=

= +

x 0

P F P F Q

Measurement update

( ) ( )1 1 1 1| 1 1| 1ˆ| ; ,k k k k k k kp N d d+ + + + + + +=x Y x x P

where

1| 1 1 1

1 1 1| 1 1

11 1| 1 1

1| 1 1| 1 1 1

ˆk k k k

Tk k k k k k

Tk k k k k

Tk k k k k k k

d d+ + + +

+ + + + +

-+ + + +

+ + + + + +

=

= +

=

= -

x K y

S H P H R

K P H S

P P K S K

26

2.1.4 Point Mass Filter

Because the EKF, which is the most popular nonlinear filter, is derived based

on the assumption of linearization, there is a restriction that the error of the state

variable should be small and it is a risky method that the filter can diverge if it is

not satisfied. It is also possible to perform filtering on nonlinear models, but since

Gaussian noise is assumed, there is a disadvantage that the result may be degraded

for non-Gaussian noise problems.

The PMF is a numerical nonlinear filtering technique that can provide a

solution for nonlinear model / non-Gaussian noise problems. The PMF is a

mathematically simple method of expressing a probability distribution by

approximating the integral of the BRR described in section 2.2.1 as summation. In

order to represent the probability distribution, points of a uniform interval

(resolution) are arranged in a constant area (support) in the state space; the grid.

Because the integral of the BRRs is integrated with respect to the differential dx for

the entire state space, in order to approximate it as closely as possible, the interval

of the grid points is set to be small (high resolution) and the area is set to be large

(large support). It is generally known that the estimation performance is improved

in this grid condition. While the EKF is a method to explicitly compute states

estimation, the PMF is an implicit method that calculates the posterior distribution

and estimates the states as,

( )ˆ |k k k k kp d= òx x x Y x (2.42)

There are two ways to express probability distribution in the PMF. One

expresses a probability density function assuming a piecewise uniform distribution

as shown in the Figure 2.1(a) and the other is a method of assigning a probability

27

mass or a weight to one grid point as shown in Figure 2.1(b). Each method

expresses the probability distribution as Eq. (2.43), (2.44), respectively.

(a) Piecewise uniform distribution

(b) Probability mass function

0

-10

10-5

0.01

5

x1

0

x2

0

0.02

5-5

10 -10

0.03

0.04

28

Figure 2.1 Expressions of the pdf in PMF

( ) ( ) { }1

| | ; ,N

i ik k k k k k k

i

p p S=

» Dåx Y ξ Y x ξ (2.43)

( ) ( ) ( )1

| |N

i ik k k k k k

i

p p d=

» -åx Y ξ Y x ξ (2.44)

where {}S × represents uniform distribution around the grid point ikξ with

resolution kD . ( )d × represents dirac delta.

{ }1,

; ,0,

k kik k kS

otherwhise

ÎDìD = í

î

xx ξ (2.45)

( ) ( ), : 1, ,

2 2

k ki ik k k x

i ii n

ì üé D D ùï ïD = - + =í ýê ú

ï ïë ûî þξ ξ K (2.46)

( )1, 0

0,

xx

otherwised

=ì= íî

(2.47)

As a result, because both of the expressions show the same result for the states

estimation, expression in Eq. (2.44) is used in this dissertation. The general PMF

method is shown in the Figure 2.2. First, initialization is performed. The

initialization process includes the grid design for expressing the initial probability

distribution, the definition of the probability distribution and the initial value of the

states, etc. Next, the system model is used to propagate the grid. In this process, the

arrangement of the grid points that are defined at constant resolution may change

nonlinearly because of the nonlinear system model. Because the PMF represents

the probability distribution at uniformly distributed points, the points should be

rearranged, that is, the grid redesign should be performed. In the redesigned grid,

29

the probability distribution after the time update, that is, the prior probability

distribution is calculated. Finally, after computing likelihoods on the same grid,

posterior distribution is calculated by using the prior distribution and likelihoods,

and repeat these process recursively.

A more detailed description of the method for calculating the probability

distribution is as follows. First, consider the following nonlinear model.

( )

( )1k k k k

k k k k

+ = +

= +

x f x w

y h x v(2.48)

where ,k kw v are system and measurement noises, respectively which have the

properties in Eq. (2.3). ( ), ( )k k× ×f h are nonlinear process and measurement models.

30

Figure 2.2 General PMF algorithm

Assume that a grid support and resolution are redesigned and a posterior

distribution is calculated on the grid at time step k.

|1

( | ) ( )N

i ik k k k k

i

p w d=

= -åx Y x ξ (2.49)

where ikξ is a ith redesigned grid point and |

ik kw is a probability or weight at the

corresponding grid point. N is the total number of grid points.

The time update is performed by using the system model in Eq. (2.48). The

grid points are propagated nonlinearly by the system model ( )k ×f and scattered in

state space. To describe a prior distribution in a regular grid, a grid redesign which

designs an area of grid support and a grid resolution should be performed. For the

31

efficient and accurate PMF, the grid design should be treated as important, but this

section assumes the grid redesign has been performed. A detailed explanation of

the grid redesign is provided in Chapter 3. Let the redesigned grid points at time

step k+1, 1ik+ξ and the corresponding weights can be calculated as

1| 1 |1

( | )N

i i j jk k k k k k

j

pw w+ +=

=å ξ ξ (2.50)

Normalizing Eq. (2.50),

1|

1|

1|1

ik ki

k k Nik k

i

ww

w

+

+

+=

=

å(2.51)

Next, after calculating prior distribution, describe likelihood on the redesigned

grid using the measurement model in Eq. (2.48). It is possible to express the

relation among prior distribution, likelihood and posterior distribution as,

1( | ) ( | ) ( | )k k k k k kp p p -µx Y y x x Y (2.52)

Then the filtered weights can be calculated numerically,

1| 1 1 1 1|

1| 1

1| 1

1| 11

( | )i i ik k k k k kik ki

k k Nik k

i

pw w

ww

w

+ + + + +

+ +

+ +

+ +=

=

=

å

ξ y

(2.53)

2.2 Typical Approaches to Filter Based TRN

In this section, it is described that the navigation components and their

32

characteristics for performing the TRN and how they are reflected in the TRN. In

addition, the nonlinear filtering methods described in Section 2.2 are applied to the

TRN and the numerical simulation results are introduced.

2.2.1 Navigation Components for TRN

In order to perform the TRN, various sensors for measuring the terrain

information and a DEM to be compared with measurement are required. In general,

terrain information can be defined as the terrain elevation or the vertical range

(clearance) between the vehicle and the surface of the terrain as shown in Figure

2.3. When the vertical range is used as a measurement, a RA is generally used as a

terrain measuring sensor, and recently, an IRA has been used to improve the TRN

performance. If the terrain elevation is used as a measurement, it is calculated by

difference between the current altitude of the vehicle and the RA measurement. The

altitude of the vehicle is measured using a BA.

The BA measures the altitude from the mean sea level to the vehicle. The

ambient air pressure around the vehicle can be measured by pressure sensor and the

altitude as the output of the BA can be calculated using the standard atmospheric

model [96].

The RA is a range sensor that measures the range between the vehicle and the

target, i.e. terrain. Generally, the RA emits a radio signal and processes the

reflected signal to calculate the distance between the vehicle and the target. The

output range is calculated by averaging the reflected signals [80], thus the accuracy

of the RA measurement is degraded in rough terrain where the variation of the

terrain within the footprint is complex. On the other hand on flat terrain, similar

distances are calculated from the reflected signals, which improves the accuracy of

33

the RA measurements. Because the footprint of the transmitted signal become

wider as the flight altitude increases, there is a possibility that variation of the

terrain within the footprint increases, and it causes degrade of the RA output.

Therefore, it is difficult to apply the RA to the TRN at too high altitude where the

RA output is degraded, whereas it is known that the TRN performance is degraded

or diverges in the flat terrain where the quality of the RA output is improved.

Recently, studies on the TRN using the IRA have been conducted to overcome

the limitation of the RA and to improve the performance of TRN [81-83]. The IRA

precisely extracts the relative three-dimensional position of the target through a

multi-baseline consisting of three receivers [76, 77, 80]. The basic principle of the

IRA is as follows [80]. The FFT to the slant range direction is performed for the

reflected signal input to the IRA receiver and a high resolution range profile

expressed in the range domain is computed. Second FFT to the moving direction of

the vehicle is performed for this result and the profile can also be expressed in the

Doppler domain. Through the successive FFTs, range-Doppler map is obtained and

the closest target can be extracted in the zero Doppler area. The same process is

applied to the remaining two channels, and the angle information can be calculated

using the principle of the interferometer [79, 80]. When applying the IRA to the

TRN, the IRA detects the strongest reflected signal, thus measures the distance to

the closest location to the vehicle, not the vertical distance like the RA. Therefore,

the IRA measures slant range includes range r , cross-track angle a which

exists in the y-z plane Figure 2.4(a), and along-track angle b which exists in the

x-z plane as shown in Figure 2.4(b). Although the outputs of the IRA are derived

through a more complex process, only the implementation of the TRN using the

IRA measurement is studied in this dissertation.

34

Figure 2.3 Terrain information as a measurement in the TRN

35

(a) Front view

(b) Side view

Figure 2.4 IRA measurements by view position

A database of terrain elevation or DEM for the mission area is essential for

comparison of measured terrain information. The DEM is a collection of terrain

elevation at a certain location according to latitude and longitude. It is usually

measured, collected and processed by aircraft or satellite. There are many DEMs

that are provided free of charge to the public, such as SRTM 3arcsec, GTOP30,

GLOBE, etc. Especially, SRTM [97] provides the public with resolution of 1 arcsec

for a limited area. Recently, some private companies have also offered much higher

resolution and higher precision DEMs [98].

2.2.2 EKF Based TRN

As explained in section 2.3.1, terrain elevation or vertical range can be used as

36

terrain measurement in the filter based TRN. According to the terrain measurement,

different system and measurement model of the filter based TRN can be used. First,

consider that the measurement is terrain elevation ,k terrainy . In this case, 2-D system

model can be used which estimates latitude kL and longitude kl as in Eq. (2.54).

( )

1

1

,

1 0

0 1

,

k k k

k

k k k

k terrain terrrain k k k

L L L

l l l

y h L l v

+

+

Dé ù é ù é ùé ù= + +ê ú ê ú ê úê ú Dë ûë û ë û ë û

= +

w(2.54)

where ,k kL lD D are travel distance to latitudinal and longitudinal directions,

respectively which are computed by the INS. ,k kvw are system and measurement

noises, respectively. ( ),terrrain k kh L l is terrain elevation at the given horizontal

position.

Next, consider that the measurement is vertical range, i.e. the RA

measurement ,k radary . In this case, the system model estimates latitude, longitude

and altitude kh . Because the system model estimates the vehicle’s altitude, it can

replace the BA measurement in 2-D case, hence the models are,

( )

1

1

1

,

1 0 0

0 1 0

0 0 1

,

k k k

k k k k

k k k

k radar k terrain k k k

L L L

l l l

h h h

y h h L l v

+

+

+

Dé ù é ù é ù é ùê ú ê ú ê ú ê ú= + D +ê ú ê ú ê ú ê úê ú ê ú ê ú ê úDë û ë û ë û ë û

= - +

w(2.55)

where khD is travel distance to vertical direction from the INS.

However, it is possible to estimate more states for the EKF since the EKF is a

computationally efficient method that can be expressed analytically as explained in

section 2.2.3. Therefore, more than three states including velocity and attitude can

37

be estimated in the ETRN. The navigation equation in Section 2.1 is used as a

nonlinear system model for position, velocity and attitude, thus the nonlinear

system model is,

( )

( ( ) )cos

(2 )

0

0

N

m

E

t

D

n n b n n n nb ie en

n n b n nb b ib in b

b

g

vL

R L h

vl

R L h L

h v

=+

=+

= -

= - + ´ +

= -

Ñ =

Ñ =

V C f ω ω V g

C C Ω Ω C

&

&

&

&

&

&

&

(2.56)

where ,b gÑ Ñ are biases of the acceleration and gyro assumed as random constant.

The same measurement model in Eq. (2.55) is used. Based on the linearization

explained in section 2.2.3 and discretization, the linearized system model [3] is

1k k k kd d+ = +x F x w (2.57)

where the state vector

[ ]

[ ]

[ ]

Tb gk k k k k k

T

k

T

k N E D

T

k

Tb b b bk x y z

Tg g g gk x y z

L l h

v v v

d d d

d d d d

d d d d

d df dq dy

é ù= Ñ Ñë û

=

=

=

é ùÑ = Ñ Ñ Ñë û

é ùÑ = Ñ Ñ Ñë û

x p v a

p

v

a(2.58)

To perform the ETRN, the measurement model in Eq. (2.55) which uses DEM

given as a table function should be linearized. The derivation of the measurement

38

model is as follow [39, 40]. As shown in Figure 2.5, it is assumed that the RA

measures a vertical range thus the range error exists only in the vertical direction.

The measurement residual kyd can be expressed as

,ˆ

k k k radary y yd = - (2.59)

where ˆky is the estimated range, and ,k radary is the range measured by the RA.

Each value can be expressed like Eq. (2.60) as shown in Figure 2.5.

, ,

ˆ ˆˆˆ ( , )

( , )

terraink k DEM k k

terraink radar k true k k k radar

y h h L l

y h h L l v

= -

= - +(2.60)

where , ,k k kL l h are the true latitude, longitude and altitude of the vehicle, and

ˆ ˆˆ , ,k k kL l h are the estimated position. ( , )terraintrue k kh L l is a true terrain elevation at the

true vehicle position, and ˆˆ( , )terrainDEM k kh L l is a DEM value at the estimated vehicle

position, and ,k radarv is RA noise.

( ) ( ),ˆ ˆˆ( , ) ( , )terrain terraink k DEM k k k true k k k radary h h L l h h L l vd = - - - + (2.61)

The DEM value can be expressed as the sum of the true terrain elevation and

DEM noise, then Eq. (2.61) is arranged as Eq. (2.62).

( ) ( ) ,ˆ ˆˆ( , ) ( , )terrain terraink k k DEM k k DEM k k DEM k radary h h h L l h L l v vd = - - - - - (2.62)

where, DEMv is DEM noise.

The estimated position is the sum of the true value and error, ˆk k kL L Ld= + ,

ˆk k kl l ld= + ,

ˆk k kh h hd= + , thus the linearized measurement model is derived by

applying a Taylor series expansion.

39

1 6

ˆ ˆˆ ˆ, ,

1terrain terrainDEM DEM

k k

L l L l

k

k k k

h hy v

L l

v

d

d d

d

d

´

é ùé ù¶ ¶ ê ú= - - +ê ú ê ú¶ ¶ê úë û ê úë û

= +

p

0 v

a

H x

(2.63)

where, ,k DEM k radarv v v= - - . ˆˆ

,terrain terrainDEM DEM

L l

h h

L l

¶ ¶

¶ ¶are Jacobian of the nonlinear

model, i.e. latitudinal and longitudinal terrain gradients or slopes, respectively. The

terrain slopes are calculated by using the stochastic linearization method when the

position error is large [43].

With the system model in Eq. (2.57) and the measurement model in Eq. (2.63),

the time update and the measurement update equations in section 2.2.3 are

performed, and the states can be finally estimated [42, 67].

Figure 2.5 Conceptual explanation of ETRN

40

Figure 2.6 Conceptual explanation of PTRN

2.2.3 PMF Based TRN

As described in section 2.2.4, the PMF has a disadvantage that the

computational load is proportional to the square of the total number of grid points.

It is almost impossible to estimate many state variables like the ETRN in Eq. (2.57)

in section 2.3.1. Therefore, 2-D system model and terrain elevation measurement in

Eq. (2.54) are applied to the PTRN.

( )

1

1

,

1 0

0 1

,

k k k

k

k k k

k terrain terrain k k k

L L L

l l l

y h L l v

+

+

Dé ù é ù é ùé ù= + +ê ú ê ú ê úê ú Dë ûë û ë û ë û

= +

w(2.64)

Assume that the posterior distribution is approximated by

( ) ( )1

| | ( )N

i ik k k k k

i

p p d=

» -åx Y ξ Y x ξ (2.65)

41

where T

i i ik k kL lé ù= ë ûξ is i

th grid point.

The prior distribution after the time update is calculated as

( ) ( ) ( )1 11 1

| | |N N

j i ik k k k k k

j i

p p p+ += =

»ååx Y ξ ξ ξ Y (2.66)

Under the assumption that the system noise is Gaussian, the transition prior in

Eq. (2.66) can be represented as form of Gaussian distribution.

( ) ( )( ) ( )1 11 1

| |k

N Nj i i

k k k k k k kj i

p p p+ += =

» -åå wx Y ξ f ξ ξ Y (2.67)

where ( )i ik k k k= + Df ξ ξ x

Eq. (2.67) is also called a convolution operation because it is same with the

convolution ( ) ( )f t g dt t t-ò [55]. The drawback of the PMF, the computational

load, arises from this process.

In the TRN, the measurement is a terrain elevation that is measured using a

BA and a RA. The BA measures the altitude from the sea-level, and the RA

measures the vertical range between the vehicle and the terrain. The terrain

elevation measurement is obtained by subtracting the vertical range from the

altitude as shown in Figure 2.6.

, ,

, ,

, ,

k k baro k radar

k baro k baro

k radar terrain k radar

y h y

h h v

h h h v

= -

= +

= - +

(2.68)

where , ,~ (0, ), ~ (0, )k baro baro k radar radarv N R v N R represent the measurement noises

of the BA and the RA that follow normal distribution.

42

The likelihood is described by comparing the measured terrain elevation with

the DEM value in Eq. (2.69), at a given horizontal position ikξ , i.e. grid point as

shown Figure 2.6.

( ) ( )terrain i terrain iDEM k true k DEMh h v= +ξ ξ (2.69)

Under the assumption that all noises, including the BA, RA and DEM follow a

Gaussian distribution, the total measurement noise also follows a Gaussian

distribution because the noise is a linear combination of the random variables. Then,

the likelihood is represented as

( ) ( )

( )( )( | ) ;0,

i terrain ik k DEM k

i ik k k radar baro DEM

y y h

p y N y R R R

d

d

= -

= + +

ξ ξ

ξ ξ(2.70)

Using Eq. (2.70), the posterior probability is calculated, and the vehicle

position is estimated using the weighted sum,

1 1 1| 11

ˆN

i ik k k k

i

w+ + + +=

=åx ξ (2.71)

2.2.4 Performance Comparison of TRN algorithms

In this section, the result of the ETRN and the PTRN described in the previous

sections are compared. The simulation conditions are set as follows. The vehicle is

assumed to fly in the north direction with a constant velocity and flight altitude as

shown in Table 2.4 and Figure 2.7. Because it is well known that the TRN degrades

or diverge on flat terrain, the simulations are performed on rough terrain [55]. A

navigation grade IMU is used to simulate inertial navigation and its specifications

43

are shown in Table 2.3. Generally, there are many error components of the IMU

such as bias, scale factor, misalignment and noise etc., but only the bias and noise

are considered in this simulation. The DEM used in the simulation is from Shuttle

Radar Topography Mission (SRTM) with 3arcsec resolution (DTED level 1) [97].

It is assumed that the RA measures the terrain vertically below the vehicle and

there is only white Gaussian noise in the RA and the BA which exist only in the

vertical direction. The detailed value of measurement noises is shown in Table 2.5.

In the simulations, the sampling frequencies of the IMU and the measurement are

assumed to be 50Hz and 1Hz, respectively. 100 Monte-Carlo simulations are

conducted for all cases to vary the IMU errors, range sensor errors, and initial

errors. The results are expressed as the root mean square (RMS) error. For the

PTRN, the constant grid is used as shown in Table 2.6.

Figure 2.8 shows well-known results. As shown in the Figure 2.8, it can be

seen that the result of the PTRN is more accurate than that of the ETRN although

the ETRN estimates velocity and attitude that gives positive effect to the position

estimation. It is because the PTRN is less affected by the nonlinearity of the terrain.

Figure 2.8 is the results taking into account only the nonlinearity caused by the

terrain profile. In order to reflect the nonlinearity more realistically, the RA noise

distribution as Eq. (2.72) is applied.

( ) ( ),

2 2, ,0.8 ; 0, 10 0.2 ; 20, 20k radarv k radar k radarp N v N v= + (2.72)

This noise distribution reflects that the RA output value is biased and the

accuracy of the RA is degraded by the dense forest or other obstacles. Although

this model do not very accurately reflect the RA measurements, it is known to be a

quite accurate model by some previous researches [48-50, 55]. It is assumed that

44

the RA noise by second term in Eq. (2.72) occur when the vehicle flies over

obstacles (100-150 sec).

Figure 2.9 shows the TRN simulation results using the non-Gaussian noise of

Eq. (2.72). Although the nonlinearity is greatly increased as compared with the

case of Gaussian noise, since the ETRN cannot reflect this effect properly, the error

greatly increases from the time when the obstacle is encountered. On the other

hand, the PTRN shows a much more accurate result than the ETRN because it can

reflect non-Gaussian noise such as Eq. (2.72).

Table 2.3 Specifications of the navigation grade IMU

Bias Random walk

Gyro 0.01°/hr 0.005°/√hr

Acc. 100μg 12μg/√Hz

Table 2.4 Simulation conditions

Parameters Values

Latitude 35.15° - 35.45°

Longitude 127.75°

45

Velocity 240m/s

Altitude 2000m

Initial position error(L,l,h)

30m, 30m, 15m

Table 2.5 Standard deviation values of the measurement noise

Measurement Value

Barometric altimeter 10 m

Radar altimeter 10 m

Table 2.6 Grid condition for PTRN

Parameters Support Resolution # of points

Values 200m×200m 2.5 m 81×81

46

Figure 2.7 Terrain profile and flight trajectory

47

(a) Latitude error

(b) Longitude error

Figure 2.8 Simulation results under Gaussian RA noise

48

(a) Latitude error

(b) Longitude error

Figure 2.9 Simulation results under non-Gaussian RA noise

49

Although the advantages that mathematically simplicity and it is easy to

represent entire probability distribution regardless of its shape, the computational

load is the biggest problem of the PMF. It is because the PMF approximates the

probability distribution at deterministic grid point. When the PMF is recursively

executed, a grid redesign should be performed every time step in order to express a

probability distribution, since the grid points are propagated and scattered

nonlinearly in state space by the nonlinear system model. Therefore the grid design

is very important for efficient and accurate result because it depends on grid

condition. In this chapter, previous researches on grid design for the PMF are

introduced. Also a new grid redesign method for efficient and accurate PTRN is

proposed and the improved PTRN performance compared with previous studies is

shown by numerical simulations.

3.1 Previous Grid Design Methods

There are two representative grid redesign method for reducing computational

load proposed by Bergman [55, 56] and Šimandl [58, 61]. Each method effectively

reduced the computational load through its own ideas. This section introduces the

Chapter 3

Grid Design for PMF Based TRN

50

detailed algorithm of the two design methods.

3.1.1 Grid Design by Point Elimination

Bergman introduced a grid design method for the efficient PTRN

implementation. The key idea of this method is to eliminate points with negligible

probability value. Before performing the PTRN, determine the allowable range of

the computational load, that is, the minimum value minN and the maximum value

maxN of the number of grid points as a design parameters. Next, design the

threshold value e for determining whether to remove the grid points with small

weight. Based on these design parameters, grid redesign is performed as shown in

Figure 3.1.

Figure 3.1 Concept of grid design proposed by Bergman

51

A detailed explanation is as follows. First, compute the posterior distribution

( )|k kp x Y at current time step k, then a grid point having a weight smaller than a

threshold is deleted by the relation in Eq. (3.1) as shown in Figure 3.2(a).

( )|ik kk k

pN

e<

Dξ Y

ξ(3.1)

where ikξ is ith grid point, kDξ is a grid resolution, and kN is a total number of

grid point.

Let the number of remaining grid points be 1kN + . If 1kN + is within the

allowable range of computation, the time update is performed and the prior

distribution ( )1 |k kp +x Y is calculated.

If 1kN + is smaller than the minimum number minN , the grid resolution is

doubled as shown in Figure 3.2(b), and new points are generated because the

computational load is very sufficient. In this case, the probability at the newly

generated grid point is calculated through interpolation. On the other hands, if

1kN + is larger than the maximum number maxN , lower the resolution to half as

shown in Figure 3.2(b), and remove every second rows and columns.

52

(a) Elimination of grid points with negligible weight

(b) Grid redesign by adjusting resolution

Figure 3.2 Grid redesign by elimination and interpolation

53

3.1.2 Anticipative Grid Design

Šimandl proposed a method of predicting mean and covariance of the prior

distribution before calculating it, which takes a long time to calculate, and

redesigning the grid based on them. There are representative algorithms using this

idea such as anticipative grid design (AGD) and boundary-based grid design

(BGD). Both methods are based on predicting prior distribution, and there is a

difference in how to design the grid support. In addition, based on the redesigned

grid, thrifty convolution for reducing the load by ignoring negligible points and

multigrid design for reducing the load for multimodal distributions with distant

peaks are proposed. In this section, the most basic algorithm of his research [61],

i.e. AGD, is explained. The details are as follows.

Propagates the grid points ikξ using the system model in Eq. (3.2).

( )1~ ( , )

k k k k

k kN

+ = +x f x w

w 0 Q(3.2)

The set of propagated points 1k+E before the grid redesign is

( ){ }1 1 1| , 1, ,i i ik k k k k i N+ + += = =E η η f ξ K (3.3)

By using these points, the grid resolution and the support that can adequately

represent the prior distribution should be determined. If the prior distribution is

assumed to be a Gaussian distribution, the mean 1ˆ k +η and covariance 1ˆ

k +C of the

prior distribution are predicted as,

54

( ) ( )

1 | 11

1 | 1 1 1 11

ˆ

ˆ ˆ ˆ

Ni i

k k k ki

N Ti i ik k k k k k k k

i

w

w

+ +=

+ + + + +=

=

= - - +

å

å

η η

C η η η η Q

(3.4)

As shown in Figure 3.3, the predicted covariance 1ˆ

k +C can be represented as

a rotated ellipsoid in the state space.

Figure 3.3 Grid design in rotated coordinate system

55

When the grid is designed based on the original coordinate, many unnecessary

points having negligible weight are generated as shown in Figure 3.3, which does

not affect the estimation result but cause the computational load. It can be expected

that this inefficiency will be prevented if the grid is redesigned in the rotated

coordinate as shown in the Figure 3.3. Therefore, the rotated coordinate system

should be derived from 1ˆ

k +C and the grid should be designed in this coordinate

system. First, eigenvalue decomposition is performed on 1ˆ

k +C to calculate the

principal axis of the rotated coordinate system.

1ˆ T

k + =C TΛT (3.5)

where Λ