Journal of Geodesy (1999) 73: 193±203

Adaptive Kalman Filtering for INS/GPS

A. H. Mohamed, K. P. Schwarz
Department of Geomatics Engineering, The University of Calgary, 2500 University Drive NW, Calgary, Alberta, Canada T2N 1N4

Received: 14 September 1998 / Accepted: 21 December 1998

Abstract. After reviewing the two main approaches of The ®xed integration formulation has shown success in
adaptive Kalman ®ltering, namely, innovation-based ful®lling the accuracy requirements of many kinematic
adaptive estimation (IAE) and multiple-model-based applications. There were, however, always applications
adaptive estimation (MMAE), the detailed development where the accuracy requirements could not be ful®lled or
of an innovation-based adaptive Kalman ®lter for an could not be ful®lled at all times. Examples are precise
integrated inertial navigation system/global positioning engineering and cadastral applications requiring an root
system (INS/GPS) is given. The developed adaptive mean square (rms) of 5±10 cm in position and 10 arc-
Kalman ®lter is based on the maximum likelihood seconds in attitude. In these applications, the change in
criterion for the proper choice of the ®lter weight and receiver±satellite geometry or in trajectory geometry or
hence the ®lter gain factors. Results from two kinematic dynamics could be readily seen in the data. It seemed,
®eld tests in which the INS/GPS was compared to highly therefore, reasonable to investigate the question whether
precise reference data are presented. Results show that algorithms that re¯ect these changes in an adaptive
the adaptive Kalman ®lter outperforms the conventional manner would not result in a better overall performance.
Kalman ®lter by tuning either the system noise vari- The problem of achieving better performance (reli-
ance±covariance (V±C) matrix `Q' or the update mea- ability and accuracy) of integrated INS/GPS systems
surement noise V±C matrix `R' or both of them. can be divided into two parts, a modeling problem and
an estimation problem. While the modeling problem is
concerned with developing better error models that
Key words. Adaptive
Kalman ®ltering
GPS/INS more accurately describe the INS/GPS system, the esti-
mation problem is concerned with achieving better tra-
jectory and sensor error estimates through the proper
use of the available process and measurement informa-
tion. The optimality of the estimation algorithm in the
Kalman ®lter setting is closely connected to the quality
1 Introduction of the a priori information about the process noise and
the update measurement noise (Kalman 1960; Gelb
In this section, an overview of the inertial navigation 1988; Brown and Hwang 1992). Conceptually, a good a
system/global positioning system (INS/GPS) integration priori knowledge of the process and measurement in-
problem is presented and the reasons for adaptive formation depends on factors such as the type of ap-
Kalman ®ltering in this speci®c case are given. plication and the process dynamics, which are dicult to
The integration of an INS with the GPS for kine- obtain. Also, the estimation environment in the case of
matic applications in the geomatics engineering ®eld has INS/GPS kinematic applications is not always ®xed but
been implemented for almost two decades through the is subject to change.
use of conventional Kalman ®ltering and ®xed or semi- Insuciently known a priori ®lter statistics will on
®xed integration algorithms; see e.g. Britting (1971), the one hand reduce the precision of the estimated ®lter
Schwarz (1983), Wong (1988), Wei and Schwarz (1990), states or introduce biases to their estimates (robustness)
Schwarz (1991), Knight (1996), and Chat®eld (1997). (Toda et al. 1967). In addition, wrong a priori infor-
mation will lead to practical divergence of the ®lter. For
example: if R and/or Q are too small at the beginning of
Correspondence to: K.P. Schwarz. e-mail: [email protected] the estimation process, the uncertainty tube around the
gary.ca; Tel.: +1 403 220 7377; Fax: +1 403 284 1980 true value, in a probabilistic sense, will tighten and a
194

biased solution will result. If R and/or Q are too large, parameters are assumed, in moving-bank MMAE time-
®lter divergence, in the statistical sense, could result. In varying adaptive parameters are permissible (Maybeck
addition, it will result in a longer estimation transition 1989). In Magill (1965), a parallel-®lter scheme is sug-
for the ®lter. Also, insuciently known a priori statistics gested which is used in Girgis and Brown (1985) to
will, in many cases, lead to an inadequate estimation of classify faults in a three-phase transmission line. A
weak observable components in the ®lter. For the similar technique is used in Levy (1996) to adapt ®lter
problem at hand, accelerometer biases and gyro drifts parameters for the purpose of system identi®cation.
are such components. Since their estimation has a direct The use of IAE, however, is more applicable to INS/
e€ect on the estimation of the main ®lter components GPS systems used in the geomatics ®eld. In Salychev
(position, velocity, and attitude) through the coupling (1993, 1994), a scalar adaptive estimator based on the
e€ect, this problem is serious. Insucient a priori in- ML estimation principle is described. The resulting al-
formation and a frequently changing estimation envi- gorithm has been used in a real-time INS/GPS system to
ronment a€ect the accuracy of the integrated INS/GPS detect sensor failure and abrupt changes. It is also used
system. This implies that using a ®xed ®lter designed by in an INS/GPS airborne gravity system to achieve better
conventional methods is a major drawback in a chang- accuracy estimating the gravity anomaly. In case of GPS
ing dynamics environment. only, an IAE algorithm based on the adaptation of the
From this point of view, the ®xed estimation for- measurement covariance matrix is proposed in Wang
mulation should be replaced by an adaptive estimation et al. (1997), to improve the reliability of the phase
formulation with an adaptive integration throughout the ambiguity resolution. At the University of Calgary, a
INS/GPS trajectory estimation process. It can be ex- full-scale IAE for INS/GPS systems is under develop-
pected that with the adaptive integration scheme better ment and is used in estimating the trajectory for mobile
performance for INS/GPS systems can be achieved. The georeferencing and airborne gravity systems.
main advantage of the adaptive technique is its weaker The outline of the remainder of the paper is as fol-
reliance on the a priori statistical information. An lows. In Sect. 2, the adaptive Kalman ®ltering problem
adaptive ®lter formulation, therefore, tackles the prob- is overviewed and the MMAE and IAE approaches are
lem of imperfect a priori information and provides a brie¯y discussed. A description of the mathematical
signi®cant improvement in performance over the ®xed model and the development of the IAE ®lter used at the
®lter through the ®lter learning process based on the University of Calgary are the subject of Sect. 3. In
innovation sequence (Mehra 1970; 1971). In this case, Sect. 4, results from two ®eld tests in a controlled
perfect knowledge of the a priori information is only of environment are presented to illustrate the concept of
secondary importance because the new measurement the developed IAE ®lter. Section 5 contains a summary
and process covariance matrices are adapted according and conclusions.
to the ®lter learning history. Also, the frequent adap-
tation of the statistical ®lter information, through the
®lter innovation sequence, goes hand in hand with the 2 Adaptive Kalman ®ltering
idea of having a dynamic system in a dynamic envi-
ronment. The emphasis of this section is on the general concept of
The objective of this contribution is to introduce the adaptive Kalman ®ltering. The principles of the MMAE
adaptive Kalman ®lter as an alternative for use with and IAE approaches are brie¯y overviewed. The math-
INS/GPS systems. In this contribution, the general ematical notation used in this paper follows that used in
layout of the adaptive Kalman ®ltering problem and its Gelb (1988) with few additions.
maximum-likelihood (ML) solution will be given. Re- In the context of adaptive Kalman ®ltering, the un-
sults from ®eld tests will be used to illustrate the concept certain parameters that need to be adapted may be part
and to show a case in which the adaptive Kalman ®lter of the system model through the state transition matrix
outperforms the conventional one. U, the measurement design matrix H, or the statistical
The two approaches to the adaptive Kalman ®ltering information through the variance±covariance (V±C)
problem are multiple-model-based adaptive estimation matrices R/Q. The ®rst case is more likely to occur in
(MMAE) and innovation-based adaptive estimation problems where system design/identi®cation is of con-
(IAE). While in the former a bank of Kalman ®lters runs cern. In this development, it is assumed that the used
in parallel under di€erent models for the ®lter's statis- INS/GPS system model is sucient for the intended
tical information, in the latter the adaptation is done applications. The optimization of the ®lter performance
directly to the statistical information matrices R and/or will be done through the adaptive estimation of the ®lter
Q based on the changes in the innovation sequence; see statistical information, the V±C matrices. Therefore, the
Sect. 2 for details. discussion in the following will be restricted to the
The MMAE has its application in the design of problem of adapting the ®lter V±C matrices without
controllers for ¯exible vehicles, tracking problems, and questioning the system modeling.
failure and interference/jamming and spoo®ng detec- The two approaches to adaptive Kalman ®ltering,
tion. For example, in White (1996) the use of MMAE in namely, MMAE and IAE, presented in the following,
detecting the interference, jamming and spoo®ng in a share the same concept of utilizing the new information
DGPS-aided inertial system is discussed (see also White in the innovation (or residual) sequence, but di€er in
et al. 1996). While in standard MMAE only constant their implementation. The innovation sequence mk at
 195

epoch k in the Kalman ®lter algorithm is the di€erence

between the real measurement zk received by the ®lter KF #1
and its estimated (predicted) value zk …ÿ†, and is com-
puted as follows: KF #2 Weighting +
mk ˆ zk ÿ zk …ÿ† …1† . Scheme x̂
.
The predicted measurement is computed by projecting .
the ®lter predicted states xk …ÿ† onto the measurement
space through the measurement design matrix Hk , i.e. KF #L
zk …ÿ† ˆ Hk x^k …ÿ† …2†
At the current time k, the new observation zk does not Fig. 2. The estimate of the bank of ®lters
really provide completely new information because some
X
L
of the information is obtained by prediction from x^k ˆ x^k …ai †P …ai jzk † …3†
previous ®lter states, z…ÿ†. On the other hand, the iˆ1
values of the innovation mi at di€erent instants are, in where P …ai jzk † is the weight of the ith ®lter when
principle, uncorrelated. In other words, the value of the measurements zk up to epoch k are available; ai is an
innovation mi at the current epoch k cannot be predicted unknown random variable with known statistical
from previous values of it, and therefore each observa- distribution P …ai †, which drives the adaptive process
tion mi brings new information. Hence, the innovation of the ®lter, and L is the total number of ®lters used.
sequence represents the information content in the new
observation and is considered as the most relevant As measurements evolve with time, the adaptive
source of information for the ®lter adaptation; the scheme learns which of the ®lters is the correct one, and
interested reader is directed to Genin (1970) and Kailath its weight factor approaches unity while the others are
(1972, 1981) for a more detailed discussion of the going to zero. The bank of ®lters accomplishes this, in
innovation sequence and its use in linear ®lter theory. e€ect, by looking at the sums of the weighted squared
measurement innovations or residuals. The ®lter with
the smallest sum prevails.
2.1 Multiple model adaptive estimation

In the multiple model adaptive estimation (or parallel- 2.2 Innovation-based adaptive estimation
®lter) approach (Magill 1965; Maybeck 1989; Brown
and Hwang 1992; Gary and Maybeck 1996; White 1996; In the innovation-based adaptive estimation (IAE)
White et al. 1996), a bank of Kalman ®lters runs in approach window (Mehra 1970, 1971; Kailath 1972;
parallel under di€erent models for the statistical ®lter Maybeck 1982; Salychev 1994), the covariance matrices
information matrices, i.e. the process noise matrix Q Rk and Qk themselves are adapted as measurements
and/or the update measurement noise matrix R. The evolve with time. Based on the whiteness of the ®lter
structure of each ®lter in the bank of ®lters is depicted in innovation sequence, the ®lter statistical information
Fig. 1 and the ®nal estimate of the bank of ®lters is matrices are adapted as follows:
explained in Fig. 2.
In every run, each ®lter of the bank will have its own R^k ˆ C^mk ÿ Hk Pk…ÿ† HkT …4†
estimate x^k …ai †. At the ®rst epoch, the bank of ®lters and
receives the ®rst measurement z0 , and the P …z0 jai † dis-
tribution is computed for each permissible ai . At each ^ k ˆ Kk C^mk K T
Q …5†
k
recursive step the adaptive ®lter does three things, as
follows. where Pk…ÿ† and Kk are the predicted covariance of state
matrix and gain matrix, respectively. Knowing the
1. First, each ®lter in the bank of ®lters computes its own innovation sequence, Eq. (1), one can compute the
estimate, which is hypothesized on its own model.
2. Second, the system computes the a posteriori proba- innovation V±C matrix, C^mk , at epoch k, through
bilities for each of the hypotheses. averaging inside a moving estimation window of size
3. Finally, the scheme forms the adaptive optimal esti- N, as
mate of x as a weighted sum of the estimates pro- 1X k
duced by each of the individual Kalman ®lters as C^mk ˆ mj mT …6†
N jˆj0 j

Filtering Weighting where j0 ˆ k ÿ N ‡ 1 is the ®rst epoch inside the

Zk KF #i xk(αi) x̂k(αi) estimation. In order to account for such an adaptive
(α=αi) νi
P(αi | zk) approach in the Kalman ®lter algorithm, an additional
block for computing the innovation V±C matrix and
both Q and R is needed, as shown in Fig. 3. Details of
Fig. 1. The estimate of the ith ®lter in the MMAE this approach will be discussed in Sect. 3.
196

z existence of sucient statistics. It should be noted, here,

ν Kalman that the minimum variance formulation not only su€ers
Compute
Compute Cν Qk /Rk
Filter sever analytical diculties when handling this problem,
Loop but also will, in general, result in a biased estimate for
small sample sizes. The sample size puts additional
z(-) restriction on the choice of the estimation window size,
which will be discussed in Sect. 4.1; the interested reader
Fig. 3. The innovation process in the adaptive Kalman ®lter is directed to CrameÂr (1946) and Maybeck (1982) for
algorithm more details.
In this development, the speci®c case of a ®xed-length
3 Innovation-based adaptive Kalman ®ltering memory (windowing) ®lter for INS/GPS kinematic po-
sitioning will be considered. In addition, the V±C matri-
The ML concept is used in this section to derive the ces containing the statistics are to be adapted and not the
innovation-based adaptive Kalman ®lter which is used ®lter states. Therefore, the underlying assumptions to the
for the development at the University of Calgary. Some ML adaptive Kalman ®ltering problem are as follows.
development and implementation aspects are brie¯y
1. The ®lter states x are independent of the adaptive
discussed.
parameters a, i.e. ox=oa ˆ 0.
2. The ®lter transition matrix U and design matrix H are
time invariant and independent of a.
3.1 Maximum likelihood estimator
3. The innovation sequence is a white and ergodic se-
of innovation-based adaptive Kalman ®lter
quence within the estimation window.
4. The covariance matrix Cm (through m) is the key to
Adaptive Kalman ®ltering is one of the methods which
adaptation and hence is the a-dependent parameter.
is not a simple extension of conventional least-squares
(LS) estimation, widely used in geomatics and in many Further, the case will be considered where the data is
other engineering ®elds. The reason for that is that LS Gaussian distributed. According to the central limit
aims at estimating and modifying the ®rst moment theorem, if the random phenomenon we observe is
information (the mean), while in adaptive Kalman generated as the sum of e€ects of many independent
®ltering the adaptation of the second moment informa- in®nitesimal random phenomena, then the distribution
tion (the variance/covariance) is also of concern. It is of the observed phenomenon approaches a Gaussian
worth mentioning that some authors, e.g. Haykin distribution as more random e€ects are summed,
(1996), like to classify the conventional Kalman ®lter regardless of the distribution of each individual phe-
among the adaptive techniques based on its property of nomenon. Therefore, our assumption of Gaussian
sequentially modifying the ®lter states V±C matrix P distribution of the data is not restricting. In this case,
which is in essence an adaptation to the ®lter tap weights the probability density function of the measurements
according to Wiener theory. This, however, is not our conditioned on the adaptive parameter a at the speci®c
intention in this paper. By adaptive, we mean, imposing epoch k is
conditions under which the ®lter statistical information
matrices R and/or Q, which are considered constant in 1 1 T ÿ1

m
 eÿ2mk Cmk mk
P…zja†k ˆ p …7†
the conventional Kalman ®lter, are estimated via the …2p† jCmk j
available new information in the ®lter innovation
sequence. For this reason, the formulas are derived in where m is the number of measurements, j
j is the
the ML setting, which is more suitable for the problem determinant operator, and e is the natural base. To
formulation. The suitability of the ML technique stems simplify the above equation, its logarithmic form is
from the fact that for the case of independent and taken
identically distributed measurements, an unbiased esti- 1
mate with ®nite covariance can always be found through ln P…zja†k ˆ ÿ fm  ln…2p† ‡ ln…jCmk j† ‡ mTk Cvÿ1 mk g …8†
the ML method such that no other unbiased estimate 2 k

with a lower covariance exists (CrameÂr 1946). The INS/ Note that after multiplying Eq. (8) by ÿ2, the ML
GPS measurements are assumed to be independent with criterion of maximizing P becomes the minimization of
identical (usually Gaussian) distribution. The other the resulting right-hand side of the same equation. Also,
attractive property of the ML estimate is its uniqueness for a ®xed-length memory ®lter, the innovation sequence
and its consistency. Uniqueness, the ®rst property, will only be considered inside a window of size N; all
means that only one solution is the outcome of the innovations inside the estimation window will be
ML formulation; consistency, on the other hand, means summed. After multiplying them by ÿ2, summation,
that the ML estimate converges, in a probabilistic sense, and neglecting the constant term, the ML condition
to the true value of the variable as the number of sample becomes
data grows without bound. The ML estimate, however,
will in general be biased for small sample sizes. X
k X
k
Notwithstanding, it will generally provide the unique ln jCmj j ‡ mTj Cmÿ1
j
mj ˆ min …9†
minimum attainable variance estimate under the jˆj0 jˆj0
 197

It is worth mentioning here that k in the above formula Pk…ÿ† ˆ UPkÿ1…‡† UT ‡ Qk …13†
represents the epoch number at which estimation takes
place, while j is the moving counter inside the estimation which after di€erentiation with respect to a yields
window. oPk …ÿ† oP …‡† oQk
In conventional LS, only the second term of Eq. (9) is ˆ U kÿ1 UT ‡ …14†
oak oak oak
considered, which corresponds to the error norm in the
L2 space. Minimizing that norm with respect to the state Assuming that the process inside the estimation window
vector will result in the optimal states estimate (see e.g. is in steady state, the ®rst term can be neglected and
Sorenson 1970; Swerling 1971; Kailath 1972, 1974 for a Eq. (14) can be rewritten as
discussion of the LS method). This, however, is di€erent oPk …ÿ† oQk
for Eq. (9). The V±C matrix of the innovation sequence ˆ …14a†
oak oak
Cm , not the innovation sequence itself, is dependent on
the adaptive parameter a, and is the key to adaptation. Substituting Eq. (14a) into Eq. (12) results in
So, in terms of Cm , the above formula represents a con- oCmk oRk oQkÿ1 T
dition for the decision to choose the error weight, not the ˆ ‡ Hk Hk …15†
state optimal estimate. In other words, while the LS oak oak oak
problem aims at ®nding the smallest error norm ac- Now, substitute Eq. (15) into Eq. (10) and expand it.
cording to a prede®ned weight, the above ML problem The resulting expression, Eq. (16), is the ML equation
aims at ®nding the weight that will result in the smallest for the adaptive Kalman ®lter
error norm. This means that the adaptive estimation of
X h i 
the weight is complementary to the state estimation. k
T ÿ1 oRj oQjÿ1 T
The above formula, then, describes the best estimate tr Cmÿ1 ÿ C ÿ1
m m C
mj j j mj ‡ H j H ˆ0
jˆj0
j
oak oak j
as the one that has the maximum likelihood based on the
adaptive parameter a. Matrix di€erential calculus will be …16†
used to obtain the derivative of Eq. (9) and equate it to
zero. The formula Equation (16) shows that both R and Q can be adapted
based on a.
oP =oa ˆ 0
3.2 Adaptive estimation of the measurement
results in
noise matrix R
Xk    
oCmj T ÿ1 oCmj ÿ1
tr Cmÿ1 ÿ m C
j mj C m j ˆ 0 …10† In order to obtain an explicit expression for R, it is
jˆj0
j
oak oak mj assumed that Q is completely known and independent of
a. The case where ai ˆ Rii will be considered, where i is
where tr is the matrix trace operator. To obtain the the matrix row or column index; i.e. the adaptive
above formula, the following two relations from matrix parameters are the variances of the update measure-
di€erential calculus have been used (Maybeck 1972; ments. This is a situation frequently encountered in
Rogers 1980; Golub and Loan 1989): practice. In this speci®c case, the adaptive Kalman ®lter,
  Eq. (16), reduces to
o ln jAj 1 ojAj ÿ1 oA
ˆ ˆ tr A X
k nh i o
ox jAj ox ox
tr Cmÿ1
j
ÿ C ÿ1
m j
m j m T ÿ1
j C m j
‰I ‡ 0Š ˆ0
and jˆj0

oAÿ1 oA ÿ1 which after expansion becomes

ˆ ÿAÿ1 A n h i o
ox ox X
k
tr Cmÿ1
j
C m j
ÿ mj mT
j Cm
ÿ1
j
ˆ0 …17†
It is clear, from Eq. (10), that the problem of adaptive jˆj0
Kalman ®ltering is reduced to the problem of determin-
ing Cm and its partial derivative with respect to a. Since From the above formula and under the assumption of
there is little interest in Cm itself, but rather in R and Q, an ergodic innovation sequence inside the estimation
the following substitution will be made (see e.g. Gelb window, the expression for the estimated V±C matrix of
1974; Brown and Hwang 1992): the innovation sequence as in Eq. (6) can be obtained.
Substituting Cm from Eq. (6) into Eq. (11), the innova-
Cmk ˆ Rk ‡ Hk Pk …ÿ†HkT …11† tion-based adaptive estimate of R of Eq. (4) is obtained.
It is repeated here for convenience.
The partial derivative of Eq. (11) with respect to a yields
R^k ˆ C^mk ÿ Hk Pk …ÿ†HkT
oCmk oRk oPk …ÿ† T
ˆ ‡ Hk Hk …12† A similar expression using the residual sequence instead
oak oak oak
of the innovation sequence can also be derived. It is
It is also known that computed as follows (see Appendix A for derivation):
198

R^k ˆ C^mk ‡ Hk Pk …‡†HkT …18† backward along a 2-m track (see the test setup in Fig. 4
and the base speci®cations in Table 1). In this case, the
where kinematic trajectory is generated by mounting the INS/
GPS system on top of the moving base. The platform,
1X k
then, goes back and forth along its track according to a
C^mk ˆ mj mTj …19†
N jˆj0 preloaded program to the servo control unit. The cali-
bration of the Anorad system, which is done by com-
and the residual sequence paring the actual trajectory implemented by the system
to the nominal trajectory, shows an accuracy of better
mk ˆ zk ÿ zk …‡† …20† than 0.1 mm (rms). So, results can be compared to a
where zk …‡† is the predicted measurement based on the tenth of a millimeter; this accuracy is more than an order
updated ®lter states and is computed as follows: of magnitude better than that expected from the inte-
grated INS/GPS.
zk …‡† ˆ H x^k …‡† …21† Each of the two trajectories generated and used in
this study consists of three static periods and a kinematic
Judging by the results presented in Sect. 4, this estimator one, as depicted in Fig. 5. The three static data sets were
of R has proven to be numerically more suitable for the collected at the center and at both ends of the track. The
case of INS/GPS systems. static data set at the track center was used to resolve the
GPS phase ambiguity. The three static data sets were
used to orient the data to the WGS-84 reference system,
3.3 Adaptive estimation of the system noise matrix Q and then to a local TM coordinate system.
The dynamics of the test is shown in Fig. 6. One
The same strategy used for R will also be used to obtain complete cycle to travel from a certain point on the track
an estimate of Q. In Eq. (16), R will be considered to be and come back takes 35 s. The cycle in a time-position
completely known and independent of a, i.e. its partial axis, the ®rst subplot of Fig. 6, is a sinusoidal wave of
derivative with respect to a vanishes. Taking ai ˆ Qii , as 1000 incremental distances. To accomplish this trajec-
in the case of R, Eq. (16) reduces to tory, the platform is accelerated and decelerated in a
sinusoidal fashion according to the pro®le shown in the
X
k
trfHjT ‰Cmÿ1 ÿ Cmÿ1 mj mTj Cmÿ1 ŠHj g ˆ 0 …22† third subplot with a value ranging from zero to a max-
jˆj0
j j j
imum of 0.032 m/s2 . The resulting velocity pro®le is a
co-sinusoidal wave and is shown in the second subplot
which is transformed (see Appendix 2 for a proof) to and its value ranges from zero to a maximum of
0.179 m/s.
Xk
^k ˆ 1
Q Dxj DxTj ‡ Pk …‡† ÿ UPkÿ1 …‡†UT …23†
By up-sampling the 10-Hz base-logged data to
N jˆj0 100 Hz and correlating the result with a 100-Hz nominal
sinusoid, it was found that the base-generated sinusoid
where Dx is the state correction sequence (the di€erence has a synchronization error of 70 ms; see Fig. 7. This
between the state before and after updates) and is error results in a periodic residual error with a maximum
computed as value of 0.0125 m in the di€erence sequences. The syn-
chronization error will be removed from the results.
Dxk ˆ x^k …‡† ÿ x^k …ÿ† …24† What remains afterwards represents the actual errors
plus a residual synchronization e€ect.
In steady state, considering only its ®rst term and the
relation
Dxk ˆ Kk mk …25†
Eq. (23) can be approximated by Eq. (5).

4 Tests and results

Two tests along well controlled trajectories are discussed

and analyzed in this section to compare the performance
of the developed adaptive Kalman ®lter with the
conventional Kalman ®lter. The analysis in this section
is meant to illustrate the adaptive Kalman ®ltering
concept.
The reference data was obtained from a kinematic
measurement base, called Anorad AG12-84. The base
provides precision position and velocity data for a
platform moving, under computer control, forward and Fig. 4. Test setup, INS/GPS on Anorad platform
 199

Table 1. Anorad AG12-84 manufacturer's speci®cation. (Anorad Trajectory Dynamics

1993) 1

Position (m)
Controller length 2m
Position resolution 1 count (16 000 000 counts/m) 0
Position range ‹999 999 999 counts
Position accuracy Within 1 count -1
Velocity range ‹16 000 000 counts/s 0 5 10 15 20 25 30 35
Acceleration range 1000 to 127 000 000 counts/s2 0.2

Acceleration (m/s2) Velocity (m/s)

0
The synchronization error can be treated as a random
error with uniform distribution in the region of interest. -0.2
0 5 10 15 20 25 30 35
If T is the synchronization resolution possible, the syn- 0.05
chronization error is ÿT =2 < e < T =2. The uniform
probability density is then 1=T . The resulting root-
0
mean-square error (rmse) can be calculated as
Z
2 1 T =2 2 T2 -0.05
rmseT ˆ e de ˆ 0 5 10 15 20 25 30 35
T ÿT =2 12 Time (sec)

For the 1-Hz data rate, the expected synchronization Fig. 6. INS/GPS trajectory dynamics
rmse is about 300 ms, while it is 30 ms for the 10-Hz
data rate. The ®rst synchronization error corresponds to
a systematic position error of 0.054 m, while the latter 1.005
XCorrelation (Cmax=1.000 at 7 lag)
corresponds to 0.0054 m, under the aforementioned test
Correlation Coefficient

dynamics. 1.000

0.995
4.1 Position error results 0.990

Figure 8 shows the position error of the kinematic part 0.985

of the INS/GPS trajectory when using the conventional
Kalman ®lter. It is worth noting here that the trajectory 0.980
-100 -80 -60 -40 -20 0 20 40 60 80 100
contains a large number of sharp peaks which are Lag
regularly distributed. Each peak coincides with one end
of the track and corresponds to a half-cycle period of 17 Fig. 7. Synchronization error between the base logged data and the
epochs. The maximum errors occur at these turns. nominal trajectory
Before discussing the results of the adaptive ®lter, the
e€ect of the estimation window size on the adaptive ®lter Kinematic Position Error-Conventional Kalman Filter
performance will be discussed. A window of the same 15
size as the data length is essentially converting the
10
adaptive ®lter into a conventional ®lter, since adaptation
Position Error (mm)

will take place only once. The following three cases lead 5

INS/GPS Test Trajectory -5

1.1 Static Kinematic
0.9
-10
0.7
Distance Travelled (m)

0.5 -15
0.3 0 100 200 300 400 500
0.1 Static Epoch (sec)
-0.1
-0.3
Fig. 8. Position error from conventional Kalman ®lter
-0.5
-0.7 to destabilization of the ®lter and to the problem of ®lter
-0.9 Static divergence in practice.
-1.1
0 500 1000 1500 2000 2500 1. A window size smaller than the number of update
Epoch (sec) measurements when adapting R.
2. A window size smaller than the number of ®lter states
Fig. 5. INS/GPS test trajectory when adapting Q.
200

3. A window size smaller than the sum of update mea- 15

Kinematic Position Error-Adaptive Kalman Filter (Q)
surements and ®lter states when adapting both R and
Q simultaneously. 10

Position Error (mm)

The divergence in any of the previous cases occurs 5
because the number of equations required to estimate
the unknown adaptive parameters, is smaller than the 0
number of unknowns themselves.
Referring to the discussion of Sect. 3.1, the ML es- -5
timate, in general, will be biased for small sample sizes.
This suggests an additional constraint on the choice of -10
the estimation window size. The larger the estimation -15
window, the less unlikely the biasness of the estimate. 0 100 200 300 400 500
However, the large estimation window reduces the Epoch (sec)
ability of the algorithm to correctly trace high-frequency
changes of the trajectory, e.g. turns. Therefore, a trade- Fig. 10. Position error from adaptive Kalman ®lter (Q)
o€ between the biasness and the tractability of the esti-
mate according to the application at hand should be 4.2 Velocity and attitude error results
taken into account. In addition, the proper choice of the
window size depends very much on the trajectory dy- With the velocity and attitude errors (not shown), the
namics. Since the dynamics encountered in the two tests pattern of performance is very much the same as with
is benign, the number of states of the INS/GPS ®lter is the position error. As expected, the adaptive ®lter
small, 15, and the update measurements vary between 5 outperforms the conventional one in velocity as well.
and 8, a window of 100 epochs was chosen for the fol- The adaptive ®lter also dramatically improves the
lowing tests. azimuth estimates when adapting the system noise
As can be seen from Figs. 9 and 10, the error am- matrix Q; compare the relative results in the last row
plitude in the adaptive case, and hence the rmse, is re- of Table 2.
duced to about one half of its previous size. The most
likely reason for this improvement in performance is the
use of the proper weights. At turns, one can clearly see 5 Summary and conclusions
that the adaptation of Q produces an error pattern that
is more random (and in fact a ¯atter spectrum, see The integration of INS and GPS is generally imple-
Fig. 13) and hence a better ®lter performance. In gen- mented through a conventional Kalman ®lter. In this
eral, one can state that the adaptation of either R or Q paper, an adaptive Kalman ®lter, based on the ®lter
produces better ®lter performance than the use of con- innovation sequence, is introduced as an alternative for
stant R and Q. integrating INS/GPS systems. The problem of adaptive
The error spectra for the previous three cases are Kalman ®ltering is overviewed and the choice of the
shown in Figs. 11±13. A spike at frequency 0.0285 Hz speci®c ®lter algorithm used in this research is discussed.
( 35 s), corresponding to the system motion period, It is shown that the problem of adaptive Kalman
appears with di€erent power densities in the three ®ltering is complementary to the problem of ®lter state
spectra. This gives an indication that the remaining estimation. While in the latter the ®lter states are of
synchronization e€ect is still contained in the error concern, the determination of proper error weights is the
spectrum. The error spectrum in the Q-only case is, concern of the adaptive Kalman ®ltering problem. This
however, ¯atter than in the other two cases.

Conventional Filter
Kinematic Position Error-Adaptive Kalman Filter (R)
15 10-6

10 10-7
Position Error (mm)

5
PSD (m2)

10-8
0
10-9
-5
10-10
-10

-15 10-11
0 100 200 300 400 500 0 0.1 0.2 0.3 0.4 0.5
Epoch (sec) Frequency (Hz)

Fig. 9. Position error from adaptive Kalman ®lter (R) Fig. 11. Position error spectrum ± conventional
 201

R-Only ®lter showed a major improvement over the conven-

tional one through the adaptation of R/Q. The perfor-
10-7 mance of the adaptive Kalman ®lter, for most of the
navigation parameters used in this study, is improved by
10-8 almost 50% or more when compared to that of the
conventional ®lter. The drawback of the adaptive Kal-
PSD (m2)

10-9 man ®lter is a more complex algorithm which leads to an

additional estimation block in the Kalman ®lter algo-
10-10 rithm. This drawback is acceptable in cases where
highest accuracy is required. For INS/GPS integration,
10-11 such cases are direct georeferencing of airborne remote-
sensing systems and airborne gravimetry.
10-12
0 0.1 0.2 0.3 0.4 0.5
Frequency (Hz) Acknowledgments. The authors would like to thank the members of
the research group at the University of Calgary for the help they
Fig. 12. Position error spectrum ± R Only provided in collecting the test data. The ®nancial support for this
project is provided by an NSERC grant to the second author.

Q-Only

Appendix A
10-7

10-8 Derivation of the residual-based adaptive R matrix

PSD (m2)

10-9 The point of departure here is Eq. (17), which reads

10-10 X
k
trfCmÿ1
j
‰Cmj ÿ mj mTj ŠCmÿ1
j
gˆ0
-11 jˆj0
10
From Kalman ®ltering theory, one has
10-12
0 0.1 0.2 0.3 0.4 0.5 Cmÿ1 m ˆ Rÿ1 m …A1†
Frequency (Hz)
Substitute Eq. (A1) into Eq. (17) to obtain a similar
Fig. 13. Position error spectrum ± Q Only expression in terms of the residual sequence
X
k
Table 2. Performance of conventional (non-adaptive) vs adaptive trfRÿ1 ÿ1 T ÿ1
j ‰Rj Cmj Rj ÿ mj mj ŠRj g ˆ 0 …A2†
Kalman ®lters jˆj0

Kinematic accuracy (rms) Also, from Kalman ®ltering theory

Non adaptive Adaptive
P …ÿ†H T Cmÿ1 ˆ P…‡† H T Rÿ1 …A3†
R Q
Multiply both sides of Eq. (A3) by H and use Eq. (11) to
Position (mm) 5 3 3 obtain
Velocity (mm/s) 6 3 3
Pitch & roll (arcsec) 40 35 35 …Cm ÿ R†Cmÿ1 ˆ HP…‡† H T Rÿ1 …A4†
Azimuth (arcmin) 18 12 4
Multiply both sides of Eq. (A4) by matrix R and
rearrange the terms
is eciently accomplished by adapting the matrices
RCmÿ1 R ˆ R ÿ HP…‡† H T …A5†
R and/or Q which are kept constant in the conventional
Kalman ®lter. The derivation of the adaptive Kalman Now, substitute Eq. (A5) into Eq. (A2)
®lter along with ecient computational formulas is
given in detail. Two tests in a controlled environment X
k
are presented to illustrate the concept. trfRÿ1 T T ÿ1
j ‰Rj ÿ Hj Pj…‡† Hj ÿ mj mj ŠRj g ˆ 0 …A6†
In kinematic applications, neither the trajectory ge- jˆj0
ometry nor the trajectory dynamics remain constant.
Therefore, there is a major drawback in using constant The solution of Eq. (A6) yields the required expression
®lter statistical information as in the conventional Kal- for the residual-based R matrix
man ®lter. This becomes evident when analyzing the
errors in the kinematic results at the turns. The adaptive R^k ˆ C^mk ‡ Hk Pk…‡† HkT
202

Appendix B Substituting Eqs. (B7) and (B8a) into Eq. (B6), one
obtains
Proof of Eq. (23) from Eq. (22)
X
k

Starting from Eq. (22) trfPj …ÿ† ÿ Pj …‡† ÿ Dxj DxTj g ˆ 0 …B9†
jˆj0
X
k
trfHjT ‰Cmÿ1
j
ÿ Cmÿ1
j
mj mTj Cmÿ1
j
ŠHj g ˆ 0 The V±C matrix of the predicted states P …ÿ† is
jˆj0 computed as in Eq. (13) by propagating V±C matrices
of the previous epoch. Thus
the Kalman gain matrix Kk at epoch k is computed as
follows: Pk …ÿ† ˆ UPkÿ1 …‡†UT ‡ Qk …B10†
Kk ˆ Pk …ÿ†HkT Cmÿ1
k
…B1† Substituting Eq. (B10) into Eq. (B9) and moving Q to
the left-hand side, Eq. (23) for the adaptive V±C matrix
see e.g. Gelb (1988), and Brown and Hwang (1992). of the system noise Q can be obtained, i.e.
From the above expression, the following expression can
be deduced: Xk
^k ˆ 1
Q Dxj DxTj ‡ Pk …‡† ÿ UPkÿ1 …‡†UT :
N jˆj0
HkT Cmÿ1
k
ˆ Pkÿ1 …ÿ†Kk …B2†

which after transposing becomes

Cmÿ1
k
Hk ˆ KkT Pkÿ1
…ÿ† …B3† References
Rewriting Eq. (22) in explicit form as Anorad (1993) Anorad I-Series installation and operation manual.
Anorad Corporation, New York 11788
X
k
Britting KR (1971) Inertial navigation systems analysis. John Wi-
trfHjT Cmÿ1
j
Hj ÿ HjT Cmÿ1
j
mj mTj Cmÿ1
j
Hj g ˆ 0 …B4† ley, New York
jˆj0 Brown RG, Hwang PYC (1992) Introduction to random signals
and applied Kalman ®ltering. John Wiley, New York
and substituting Eqs. (B2) and (B3) into Eq. (B4), one Chat®eld AB (1997) Fundamentals of high accuracy inertial navi-
obtains gation. Progress in astronautics and aeronautics, AIAA No.
V-174: (800)
X
k CrameÂr H (1946) Mathematical methods of statistics. Princeton
trfPjÿ1 …ÿ†Kj Hj ÿ Pjÿ1 T T ÿ1
…ÿ† Kj mj mj Kj Pj…ÿ† g ˆ 0 …B5† University Press, Princeton, NJ
jˆj0
Gary RA, Maybeck PS (1996) An integrated GPS/INS/BARO and
RADAR altimeter system for aircraft precision approach
landings. Department of Electrical and Computer Engineering,
Equation (B5) can be further rearranged to Air Force Institute of Technology, OH
Gelb A (ed) (1988) Applied optimal estimation, 10th edn. MIT
X
k
Press, Cambridge, MA
trfPjÿ1 …ÿ†…Kj Hj Pj …ÿ† ÿ Kj mj mTj KjT †Pjÿ1 …ÿ†g ˆ 0 Genin F (1970) Further comments on the derivation of Kalman
jˆj0 ®lters, section II: Gaussian estimates and Kalman ®ltering. In:
…B5a† Leondes CT (ed) Theory and applications of Kalman ®ltering,
AGARDOgraph 139, NATO Advanced Groups for Aerospace
R&D
From Kalman ®lter theory, it is well known that the V± Girgis AA, Brown RG (1985) Adaptive Kalman ®ltering in com-
C matrix of the predicted states P (±) should at least be puter relaying: fault classi®cation using voltage models. IEEE
positive semi-de®nite. Hence, Eq. (B5a) only vanishes Trans Power Apparat and Syst PAS-104(5): 1168±1177
when Golub GH, Loan CFV (1989) Matrix computations, 2nd edn. The
John Hopkins University Press, Baltimore, MD
X
k Haykin S (1996) Adaptive ®lter theory. Prentice-Hall, Englewood
trfKj Hj Pj …ÿ† ÿ Kj mj mTj KjT g ˆ 0 …B6† Cli€
jˆj0 Kailath T (1972) A note on least squares estimation by the inno-
vation method. Soc Ind Appl Math 10(3): 477±486
Also, from Kalman ®ltering theory, one has Kailath T (1974) A view of three decades of linear ®ltering theory.
IEEE Trans Inf Theory IT-20(2): 146±181
Dxk ˆ Kk mk …B7† Kailath T (1981) Lectures on Wiener and Kalman ®ltering, CISM
courses and lectures no. 140. Springer, Berlin Heidelberg New
York
and Kalman RE (1960) A new approach to linear ®ltering and pre-
diction problems. J Basic Engng 82: 35±45
Pk …‡† ˆ Pk …ÿ† ÿ Kk Hk Pk …ÿ† …B8† Knight TK (1996) Rapid development of tightly-coupled GPS/INS
systems. IEEE Plans'96, Atlanta, GA, 22±26 April
from which Levy LJ (1996) Advanced topics in GPS/INS integration with
Kalman ®ltering. Navtech Seminars Tutorials, Kansas City,
Kk Hk Pk …ÿ† ˆ Pk …ÿ† ÿ Pk …‡† …B8a† MO, 10 September
 203

Magill DT (1965) Optimal adaptive estimation of sampled sto- Surveying, and Remote Sensing. Springer, Berlin Heidelberg
chastic processes. IEEE Trans Automat Contr AC-10(4): 434± New York
439 Sorenson HW (1970) Least-squares estimation: from Gauss to
Maybeck PS (1972) Combined estimation of states and parameters Kalman. IEEE Spectrum, July
for on-line applications. Report T577 Draper laboratories; PhD Swerling P (1971) Modern state estimation methods from the
Dissertation, MIT, Cambridge, MA viewpoint of the method of least squares. IEEE Trans Automat
Maybeck PS (1982) Stochastic models, estimation, and control, Contr AC-16(6)
Vols I and II. Academic Press, New York Toda NF, Schlee FH, Obsharsky P (1967) Regions of Kalman ®lter
Maybeck PS (1989) Moving-bank multiple model adaptive esti- convergence for several autonomous navigation modes. Paper
mation and control algorithms: an evaluation. Control and 67±623, AIAA Guidance, Control, and Flight Dynamics Conf,
dynamic systems, vol 31. Academic Press, New York Huntsville, AL 14±16 August
Mehra RK (1970) On the identi®cation of variance and adaptive Wang Jinling, Stewart M, Tsakiri M (1997) Kinematic GPS posi-
Kalman ®ltering. IEEE Trans Automat Contr 4C-15(2): 175± tioning with adaptive Kalman ®ltering techniques. Proc IAG
184 '97, Rio de Janeiro, September
Mehra RK (1971) On-line identi®cation of linear dynamic systems Wei M, Schwarz KP (1990) Testing a decentralized ®lter for GPS/
with applications to Kalman ®ltering. IEEE Trans Automat INS integration. Proc 1990 IEEE position location and navi-
Contr AC-16(1) gation Symp, Las Vegas, NV, March, pp 429±435
Rogers GS (1980) Matrix derivatives. Lecture notes in statistics, vol White NA (1996) MMAE detection of interference/jamming and
2. Marcel Dekker, New York spoo®ng in a DGPS-aided inertial system. MS Thesis, Dept
Salychev OS (1993) Wave and scalar estimation approaches for Electrical and Computer Engineering, Air Force Institute of
GPS/INS integration. Tech rep 20, Inst Geodesy, University of Technology, Ohio
Stuttgart White NA, Maybeck PS, DeVilbiss SL (1996) MMAE detection of
Salychev OS (1994) Special studies in dynamic estimation proce- interference/jamming and spoo®ng in a DGPS-aided inertial
dures with case studies in inertial surveying. ENGO 699.26 system. Dept Electrical and Computer Engineering, Air Force
lecture notes, Department of Geomatics Engineering, Univer- Institute of Technology, Ohio
sity of Calgary Wong RVC (1988) Development of a RLG strapdown survey
Schwarz KP (1983) Inertial surveying and geodesy. Rev Geophys system. UCSE 20027, Dept of Surveying Engineering, Univer-
Space Phys 21(4): 878±890 sity of Calgary
Schwarz KP (1991) Kinematic modeling ± progress and
problems. IAG Symp Kinematic Systems in Geodesy,