Gao Et Al - Adaptive Kalman Filtering With Recursive Noise Estimator ForIntegrated SINSDVL Systems

THE JOURNAL OF NAVIGATION (2015), 68, 142–161.
© The Royal Institute of Navigation 2014

doi:10.1017/S0373463314000484
Adaptive Kalman Filtering with

Recursive Noise Estimator for
Integrated SINS/DVL Systems
Wei Gao, Jingchun Li, Guangtao Zhou and Qian Li
(College of Automation, Harbin Engineering University, Harbin, China)
(E-mail: ljc150001@163.com)
This paper considers the estimation of the process state and noise parameters when the
statistics of the process and measurement noise are unknown or time varying in the
integration system. An adaptive Kalman Filter (AKF) with a recursive noise estimator that is
based on maximum a posteriori estimation and one-step smoothing filtering is proposed, and
the AKF can provide accurate noise statistical parameters for the Kalman filter in real-time.
An exponentially weighted fading memory method is introduced to increase the weights of
the recent innovations when the noise statistics are time varying. Also, the innovation
covariances within a moving window are averaged to correct the noise statistics estimator.
Experiments on the integrated Strapdown Inertial Navigation System (SINS)/ Doppler
Velocity Log (DVL) system show that the proposed AKF improves the estimation accuracy
effectively and the AKF is robust in the presence of vigorous-manoeuvres and rough sea
conditions.
KEYWORDS
1. Adaptive Kalman filtering. 2. Noise statistics. 3. Maximum a posteriori.
4. Integrated navigation.
Submitted: 25 December 2013. Accepted: 6 July 2014. First published online: 15 August 2014.
1. I N T R O D U C T I O N . The Kalman Filter (KF) is considered to be an

optimal estimation of the dynamic state and parameters from noisy measurements,
and it has been widely used in process state estimation of integrated navigation
systems. Nevertheless, the fact that the KF highly depends on a priori
knowledge of the dynamic model parameters and noise statistics (Sangsuk-Iam
and Bullock, 1990; Fang and Yang, 2011) is a major drawback, and the KF
will potentially lose the optimal estimation quality if the a priori knowledge is
unknown or time-varying. Unfortunately, the correct a priori knowledge of model
parameters and noise statistics depends on several factors such as the process
dynamics, which are difficult to obtain in many practical applications. In addition,
the statistics of the process and measurement noise may change with time in a
dynamic process. All of them will lead to the degradation of the estimation accuracy
Downloaded from https://www.cambridge.org/core. ETH-Bibliothek, on 22 Oct 2019 at 16:57:31, subject to the Cambridge Core terms of use, available at
https://www.cambridge.org/core/terms. https://doi.org/10.1017/S0373463314000484
NO. 1 ADAPTIVE FILTERING WITH RECURSIVE NOISE ESTIMATOR 143
or even the unexpected divergence of the filters (Zhou et al., 2010; Al-Shabi et al.,
2012).
Adaptive Kalman filtering (AKF) (Myers and Tapley, 1976; Ding et al.,
2007; Sarkka and Nummenmaa, 2009; Karasalo and Hu, 2011) can estimate the
statistical characteristics of the process and measurement noise, simultaneously
estimating the dynamic state and error covariance. As a result, various kinds of
AKF have been applied to state and parameters estimation in dynamic systems,
and they are broadly classified into four categories: Bayesian, maximum
likelihood methods, correlation and covariance matching (Mehra, 1972; Wang,
2000; Odelson et al., 2006). In particular, adaptive filtering based on Maximum
Likelihood Estimation (MLE) (Mehra, 1970; Mohamed and Schwarz, 1999) can
obtain the approximate values of the process and measurement noise covariance by
constantly tuning the noise covariance matrices online. Requiring quite a long
innovation covariance sequence to achieve a reliable estimation, MLE still has the
problem that the estimated noise covariance cannot be adjusted immediately with the
recent innovation sequences in spite of the improvement in the filter stability (Mehra,
1970).
AKF based on Maximum a Posteriori (MAP) estimation (Sage and Husa, 1969b;
Gao et al., 2012) is affiliated with the Bayesian approach, and the noise statistical
characteristics are estimated or modified consistently with the changes of the
innovation sequence in the MAP noise estimator. In addition, the well-known Sage-
Husa adaptive Kalman filtering (Sage and Husa, 1969a), which is widely used in the
estimation fields with unknown noise statistics, is just based on the MAP estimation.
However, the MAP noise estimator at present has been shown to give biased
estimation of true covariance (Bavdekar et al., 2011).
To solve the degradation of accuracy or the divergence of the filter in the integration
navigation system, an AKF algorithm based on MAP estimation and one-step
smoothing filtering is proposed in this paper. An unbiased recursive noise estimator
where noise statistics are recursively estimated or modified with the changes of
innovation sequences is derived. In order to improve the accuracy of estimation,
the AKF utilises the one-step smoothing value to modify the KF; moreover, the
innovation covariances within a fixed moving window are averaged to correct the
MAP noise estimator. Since the integration of the Strapdown Inertial Navigation
System (SINS) and Doppler Velocity Log (DVL) system is a common navigation
installation for vessels (Zhang, 2013), the performance of the proposed AKF
algorithm is evaluated in the integrated SINS/DVL system, compared with the MLE
and the Sage-Husa adaptive filtering algorithms. Moreover, the proposed AKF
algorithm is also used to process the real voyage data of a marine vehicle for testing
the actual estimation accuracy of the filter, and a long endurance test is also carried
out to investigate the stability of the proposed AKF algorithm over long application
periods.
This paper is organised as follows. In Section 2, the MAP noise
estimator is given. In Section 3, an adaptive Kalman filtering algorithm with
unbiased recursive noise estimator is derived in detail. In Section 4, experiments
and analysis on the integrated SINS/DVL navigation system are carried
out to evaluate the performance of the AKF. The conclusions are presented
in Section 5.
144 WEI GAO AND OTHERS VOL. 68
2. M A P N O I S E E S T I M AT O R . The dynamic system model is described by

the following equations:
X k = Φk−1 X k−1 + Γk−1 W k−1 (1a)
Zk = HkXk + Vk (1b)
where Xk [ Rn, Zk [ Rm, Φk − 1 [ Rn × n, Γk − 1 [ Rn × p and Hk[Rm × n. Both
Wk − 1 [ Rp and Vk[Rm are assumed to be uncorrelated Gaussian white noise
sequences with mean and covariance matrices
E[W k ] = qk , Cov[W k , W j ] = Qk δkj
E[V k ] = rk , Cov[V k , V j ] = Rk δkj
Cov[W k , V j ] = 0
where E[·] denotes the expectation, Cov[·] denotes the covariance, and δkj denotes the
Kronecker delta function.
It is noted that the first moments of the process and measurement noise are not
“zero”, which means the process and measurement noises are biased. Generally, the
biases in the process and measurement noise are treated as “new” state variables in the
functional modelling phase and are estimated during the propagation of the filter
utilizing the approach of augmenting the dimension of system states. For example,
Hewitson and Wang (2010), and Knight et al. (2010) implemented the outlier
detection, identification and reliability theory on the basis of the augmented
dimension approach in order to remove the biases or fault measurements or outliers.
Meanwhile, Sage and Husa (1969a; 1969b), Myers and Tapley (1976), and Gao et al.
(2012) provide another approach to deal with the biased process and/or measurement
noise by establishing the relevant statistical mean and covariance estimators according
to some optimal or suboptimal estimation rules. These noise estimators can estimate
the statistics of the biased noise directly and correct the unrealistic noise parameters
during the propagation of the filter, cutting down the estimation errors caused by the
biased noise. In this paper, we deal with estimation problems of the biased noise
according to motivation behind the second method and make some efforts to obtain
the MAP estimator of the biased process and measurement noise.
If a priori means and covariances of the process noise qk, Qk and measurement noise
rk, Rk are constant but unknown or random, the MAP estimations of q̂, Q̂, r̂, R̂ can be
obtained by maximizing the a posteriori density function (Sage and Husa, 1969a). The
noise estimator based on MAP estimation in coupling form can be expressed as
follows:
1X k
Γk q̂k = [X̂ j,k − Φ j−1 X̂ j−1,k ] (2a)
k j=1
1X k
Γk Q̂k ΓTk = [X̂ j,k − Φj−1 X̂ j−1,k − Γj−1 q][X̂ j,k − Φj−1,k X̂ j−1,k − Γj−1 q]T (2b)
k j=1
1X k
r̂k = [Z j − H j X̂ j,k ] (2c)
k j=1
1X k
R̂k = [Z j − H j X̂ j,k − r][Z j − H j X̂ j,k − r]T (2d)
k j=1
Notice that complicated multistep smoothing terms (e.g. X̂ j,k , X̂ j−1,k ) appear in
Equations (2a)–(2d), and all previous state vectors are smoothed repeatedly by the
latest measurements at every instant. In theory, the MAP noise estimator above could
optimally estimate the statistical characteristics of the process and measurement noise,
and it could modify the KF when noise statistics are unknown or time variant.
However, it is impossible to estimate these noise statistics by directly utilising
the MAP estimator in practical operations, as the MAP noise estimator cannot be
described in a recursive form, in view of Equations (2a)–(2b), and each previous state
vector needs to be smoothed by the latest measurements at every instant, which results
in the inefficiency of the algorithm.
3. A D A P T I V E K A L M A N F I LT E R I N G . In this paper, an AKF algor-

ithm is proposed to solve the problems above on the basis of the MAP estimation, and
the proposed AKF is divided into two main parts: one is a suboptimal noise estimator
that can estimate the noise statistics; and the other is an improved Kalman filter, which
utilises the outputs of the noise estimator to correct the estimate results.
3.1. Suboptimal MAP Noise Estimator. The inefficiency of the MAP noise
estimator is due to the complicated multistep smoothing terms in Equations (2a)–(2b),
thus some simplifications are taken in order to obtain the recursive form. Additionally,
it is convenient and efficient if the noise estimator updates recursively, because only
the estimate value at the previous step k-1 and the latest filter parameters at step k are
used in the recursive update process, which greatly simplifies the process and decreases
the calculations.
First, the one-step smoothing value X̂ j−1,j takes the place of the multistep
smoothing term X̂ j−1,k in Equations (2a) and (2d), and then the estimate value X̂ j,j
replaces the term X̂ j,k in Equations (2a)–(2d). After these simplifications, a suboptimal
MAP noise estimator is obtained as follows.
1X k
Γk q̂k = [X̂ j,j − Φ j−1 X̂ j−1,j ] (3a)
k j=1
1X k
Γk Q̂k ΓTk = [X̂ j,j − Φj−1 X̂ j−1,j − Γj−1 q][X̂ j,j − Φj−1 X̂ j−1,j − Γj−1 q]T (3b)
k j=1
1X k
r̂k = [Z j − H j X̂ j,j ] (3c)
k j=1
1X k
R̂k = [Z j − H j X̂ j,j − r][Z j − H j X̂ j,j − r]T (3d)
k j=1
In view of Equations (3a)–(3d), only the estimate value X̂ j,j and one-step
smoothing value X̂ j−1,j ( j = 1, 2 . . . k) need to be updated in the suboptimal MAP
noise estimator. It is noted that X̂ j,j can be obtained from the conventional KF,
but X̂ j−1,j cannot, so our first task is to obtain the one-step smoothing value X̂ j−1,j .
3.2. Improved Kalman Filter. The improved KF consists of two procedures: the
former utilises the estimate value at step k-1 and the innovation at k to calculate the
one-step smoothing gain matrix and its corresponding smoothing value, and the latter
modifies the estimate value X̂ k−1,k−1 with the one-step smoothing value X̂ k−1,k. Finally
both the estimate value and the one-step smoothing value are propagated recurrently
in the improved KF.
3.2.1. One-step Smoothing Filter. Firstly, our task is to obtain the one-step
smoothing value X̂ k−1,k , which is used for the propagation of the noise estimator.
According to the fixed point smoothing algorithm (Gustafsson, 2000), the one-step
smoothing filter is obtained by utilising the measurements at step k.
Xˉ k,k−1 = Φk−1 X̂ k−1,k−1 + Γk−1 qk−1 (4a)
εˉ k = Z k − H k Xˉ k,k−1 − rk (4b)
Pk,k−1 = Φk−1 Pk−1 ΦTk−1 + Γk−1 Qk−1 ΓTk−1 (4c)
Kˉ k = Pk−1 ΦTk−1 H Tk (H k Pk,k−1 H Tk + Rk ) −1
(4d)
X̂ k−1,k = X̂ k−1,k−1 + Kˉ k εˉ k (4e)
where Xˉ k,k−1 denotes the a priori estimate value, εˉ k denotes the innovation, Pk,k − 1
denotes the a priori estimate error covariance, Kˉ k denotes the gain matrix, which is
different from the KF in form, and X̂ k−1,k denotes the one-step smoothing value of the
one-step smoothing filter, respectively. Additionally, the derivation of the one-step
smoothing filter in detail is presented by Gustafsson (2000), which is not derived in this
paper.
3.2.2. Modified Kalman Filter. From Equations (3a)–(3b), it is noted that the
connection between X̂ k,k and X̂ k−1,k is important to estimate the process noise
statistics qk and Qk; moreover, the connection is also important in forming a closed-
loop propagation for the noise estimator. Hence, we need to establish the connection
between X̂ k,k and X̂ k−1,k . Specifically, the one-step smoothing value X̂ k−1,k is used to
modify the a priori estimate value X̂ k,k−1 by replacing X̂ k−1,k−1 , the estimate value at
step k-1, in the KF. The modified KF can be written as
X̂ k,k−1 = Φk−1 X̂ k−1,k + Γk−1 qk−1 (5a)
εk = Z k − H k X̂ k,k−1 − rk (5b)
K k = Pk,k−1 H Tk (H k Pk,k−1 H Tk + Rk )−1 (5c)
X̂ k,k = X̂ k,k−1 + K k εk (5d)
Pk = (I − K k H k )Pk,k−1 (5e)
where X̂ k,k−1 denotes the a priori estimate value, εk denotes the innovation, Kk denotes
the gain matrix, and X̂ k,k denotes the estimate value of the modified KF. It is noted
that Pk,k − 1 is the same in Equations (4d) and (5c).
Note that the connection between X̂ k,k and X̂ k−1,k−1 is described by Equations (5a)
and (5d). It is convenient to introduce X̂ k−1,k into Equation (5a) to calculate the
a priori estimate value X̂ k,k−1 , since X̂ k−1,k has been obtained in Equation (4e);
besides, the estimation accuracy may be improved because X̂ k−1,k has been already
estimated by the measurements at step k in Equation (5a).
At this point, the expected recursive suboptimal MAP noise estimator is obtained
through substituting Equations (4a)–(5e) into Equations (3a)–(3d), but our work has
not been finished yet. After these simplifications and arrangements, the MAP
estimator has lost its optimal estimation quality, and our next task is to guarantee the
unbiased estimation of the suboptimal MAP noise estimator.
3.3. Unbiased Suboptimal MAP Noise Estimator. An unbiased estimation test
on the suboptimal noise estimator is performed, in order to guarantee that the
estimations for the noise statistical means and covariances are unbiased.
In view of Equations (5a) and (5d), the general term X̂ j,j − Φ j−1 X̂ j−1,j in
Equation (3a) can be written as
X̂ j,j − Φ j−1 X̂ j−1,j = K j εj + Γ j−1 q j−1 (6)
Similarly, from Equations (5a)–(5d), the term Z j − H j X̂ j,j in Equation (3c) can be
represented as
Z j − H j X̂ j,j = Z j − H j (X̂ j,j−1 + K j εj )
(7)
= (I − H j K j )εj + rj
Therefore, an equivalent expression of the suboptimal MAP noise estimator is

obtained after rearranging Equations (3a)–(3d).
1X k
Γk q̂k = [K j εj + Γ j−1 q̂ j−1 ] (8a)
k j=1
1X k
Γk Q̂k ΓTk = [K j εj εTj K Tj ] (8b)
k j=1
1X k
r̂k = [(I − H j K j )εj + rj ] (8c)
k j=1
1X k
R̂k = [(I − H k K j )εj εTj (I − H j K j )T ] (8d)
k j=1
Notice that the innovation εj and its covariance εjεTj are contained in the process and
measurement noise estimator, as described by Equations (8a)–(8d), and we can obtain
the following:
εj = Z j − H j X̂ j,j−1 − rj
(9)
= H j X̃ j,j−1 + (V j − rj )
K j εj εTj K Tj = K j H j PTj,j−1
= K j H j Pj,j−1 = Pj,j−1 − Pj (10)
= Φj−1 Pj−1 ΦTj−1 − Pj + Γj−1 Qj−1 ΓTj−1
Additionally, the derivation of Equation (10) refers to Equations (A1)–(A3) in the

Appendix. Then by taking the expectations of εj and εj εTj yields the following:
E[εj ] = 0 (11)
E[εj εTj ] = H j Pj,j−1 H Tj + R (12)
Substitute Equations (9)–(12) into Equations (8a)–(8d), and then the expectations of
the process noise mean and covariance can be expressed as
( )
1X k
E[Γk q̂k ] = E [K j εj + Γ j−1 q j−1 ] = Γq (13)
k j=1
" #
1 X k
E[Γk Q̂k ΓTk ] = E K j εj εTj K Tj
k j=1
( )
1X k
=E [Φj−1 Pj−1 Φj−1 − Pj + Γj−1 Qj−1 Γj−1 ]
T T
(14)
k j=1
( )
1X k
= ΓQΓ + E
T
[Φj−1 Pj−1 Φj−1 − Pj ]
T
k j=1
Equation (13) demonstrates that Equation (8a) is the unbiased estimation of the
process noise statistical mean. The unbiased estimation of process noise statistical
covariance is obtained by rearranging Equation (14), and it can be written as
1X k
Γk Q̂k ΓTk = [K j εj εTj K Tj + Pj − Φ j−1 P j−1 ΦTj−1 ] (15)
k j=1
Similarly, the unbiased measurement noise estimator is obtained. In particular,

Equation (8c) represents the unbiased estimation of measurement noise mean and the
unbiased estimation of the covariance can be expressed as (derivations are shown in
the Appendix):
1X k
R̂k = (I − H j K j )[εj εTj (I − H j K j )T + H j P j,j−1 H Tj ] (16)
k j=1
Consequently, Equations (8a), (8c), (15) and (16) make up the unbiased suboptimal
MAP noise estimator.
3.4. Time-varying Unbiased Noise Estimator. The derivations of the noise
estimator above are based on the assumption that the noise statistics are unknown
constants. However, the noise statistics are generally time varying or random in
practical situations.
Consider the case that the noise statistics are time varying, an exponentially
weighted fading memory method is introduced to increase the weights of the recent
innovations (Gao et al., 2012). A time-varying noise estimator is obtained after the
weighted factor τk replaces the term 1/k in Equations (8a), (8c), (15) and (16).
Therefore, its recursive decoupling form can be represented as
q̂k = q̂k−1 + Ξk {τ k [K k εk ]} (17a)
Q̂k = (1 − τ k )Q̂k−1 + Ξk {τ k [K k εk εTk K Tk + Pk − Φk−1 Pk−1 ΦTk−1 ]}ΞTk (17b)
r̂k = r̂k−1 + τ k [(I − H k K k )εk ] (17c)
R̂ = (1 − τ k )R̂k−1 + τ k (I − H k K k )[εk εTk (I − H k K k )T + H k Pk,k−1 H Tk ] (17d)
where Ξk = (ΓTk Γk )−1 ΓTk , τ k = (1 − b)/(1 − bk ), and b is a fading factor, 0 < b < 1.
Equations (17a)–(17d) compose the unbiased recursive noise estimator that
can provide accurate noise statistical parameters for the improved KF. However, Q̂k
and/or R̂k may be increased when applied to long application periods. The reason is
that the estimation error of Q̂k and/or R̂k , which is caused by a sudden measurement
jump or outlier, will accumulate with the propagation of the recursive noise estimator.
For this case, we should choose a small value of b to decrease the weights of the
incorrect estimations in the recursive noise estimator and generally we choose a value
below 0·2 in practice.
Additionally, in the KF, the covariance of the process noise Q̂k is required to be
a non-negative definite matrix and the covariance of the measurement noise R̂k to be a
positive definite matrix in order to guarantee the stability of the filter. Since we obtain
these noise estimators from the KF and one-step smoothing filter without any extra
assumptions, in theory, the estimator Q̂k is a non-negative definite matrix from
Equations (12), (14) and (17b), and R̂k is a positive definite matrix from Equations
(5e), (12), (17d) and (A7). However, in view of Equations (17b) and (17d), the
propagation of Q̂k and R̂k depends on the actual covariance εk εTk of the innovation
sequence instead of its theoretical covariance. The covariance estimator Q̂k or R̂k may
be likely to lose its non-negative definite or positive definite quality if a measurement
outlier occurs unexpectedly during its recursive procedure in practice. Thus, we
introduce an innovation covariance estimator to depress the estimation errors caused
by the measurement outlier in Section 3.5.
3.5. Innovation Covariance Estimator. In view of Equations (17b) and (17d),
there are complicated matrix iteration operations on Q̂k and R̂k ; moreover,
the innovation covariance εk εTk is relied on for the propagation of the noise
estimator. When applied to long application periods, Q̂k and/or R̂k may be
increased, which is caused by a sudden measurement jump or outlier and degrades
the performance of the filter. Besides, it may even lead the AKF to divergence if
some “bad” outliers from the innovation sequences occur during the propagation. In
order to identify and remove the faulty measurements or outliers, some other
procedures have been proposed in the past decade. For example, an outlier test based
on the reliability theory is implemented by Knight et al. (2010) to deal with the
measurement outliers. More detailed information can be found in Hewitson and
Wang (2006, 2010).
To solve this problem, we can also utilise the discrepancy between the actual
covariance and theoretical covariance of the innovation sequence. In this section, an
innovation covariance estimator (ICE) (Jwo and Weng, 2008) is introduced for
guaranteeing the non-negative definite or positive definite quality of Q̂k or R̂k . It
estimates the innovation covariance online, through averaging the recent N-step
innovation covariances. The ICE can be written as

1NX−1
1
Ck = [ε εT ] = C k−1 + [εk εTk − εk−N εTk−N ] (18)
N j=0 k−1 k−j N
where N denotes the length of the moving window.

Then the ICE modifies the noise estimator by substituting the term εk εTk in
Equations (17b) and (17d). Besides, the effect of the ICE on correcting the calculation
of εk εTk contaminated by the measurement outlier is evident through averaging the
recent N-step innovation covariances in practical applications.
In view of Equation (18), it is noted that the estimation accuracy of the innovation
covariance estimators relies on the adjustments of the window size N. However, the
proper choice of N highly depends on the dynamic characteristics of the system and
the motion methods of the vehicle, and usually it is established by empirical
knowledge. For details, consider a case of high-frequency changes of the trajectory; N
needs to be a small value to adjust to the fast changes though the innovation
covariance estimator will be biased. On the other hand, N needs to be a larger value to
improve the stability and unbiased quality of the innovation covariance estimation
when the system dynamics change slowly.
Furthermore, the discrepancy between ICE and theoretical covariance of the
innovation sequence can be used for identifying the measurement outliers and
evaluating the stability of the noise estimators. When the noise estimators are in stable
status, the inequality satisfies
tr(Ck ) 4 κ · tr(H k Pk,k−1 H Tk + Rk ) (19)
where κ is a reserve factor, κ 5 1, and κ is set up from the empirical knowledge; tr(·) is
the trace operation on a matrix.
Actually, κ is a threshold value that reflects whether the noise estimators are
in stable status or not. It indicates that the estimates of the noise statistics are in-
correct and that the measurement outliers occur if the ratio between tr(Ck) and
tr(HkPk,k − 1HTk + Rk) is beyond the threshold value κ at current step. Therefore,
the current innovation covariance εk εTk will be isolated from the calculation of the
ICE for keeping the ICE robust and keeping the noise estimator Q̂k and R̂k stable
when the measurement outliers occur.
In view of Equations (4a)–(5e) and Equations (17a)–(17d), a complete adaptive
Kalman filer is obtained. The AKF contains two segments: the former is the improved
KF that utilises the innovation at step k to smooth the estimate value at step k-1, and
then uses the one-step smoothing value to correct estimate results of the KF; the latter
is the noise estimator that estimates or modifies the noise statistics with the innovation
sequences to provide accurate statistics for the improved KF.
The operation of the adaptive Kalman filtering is represented as shown in Figure 1.
4. E X P E R I M E N T R E S U LT S AND A N A LY S I S IN THE
I N T E G R A T E D S I N S / D V L S Y S T E M . To evaluate the performance of the
proposed AKF in a dynamic system with unknown and time-varying measurement
noise statistics, simulations in the integrated SINS/DVL system are performed firstly,
and then real voyage data are also processed using the proposed AKF offline.
Figure 1. Operation of the adaptive Kalman filtering.
IMU (ve ,vn ) DVL

DVL
Ax
b
ACCELEROMETERS f
SIGNAL
Ay CORRECTION
– Adaptive
(δλ ,δϕ)
Az Strapdown Kalman
(δve ,δvn)
Inertial + Filter
Navigation
Gx Algorithm
– Navigation
GYROSCOPES ωb (λ ,ϕ) SINS + Solution
Gy SIGNAL
CORRECTION (ve ,vn ) SINS
Gz OUTPUT
SINS CORRECTION
Figure 2. Diagram of the integrated SINS/DVL navigation system.
4.1. Simulation and Analysis in the SINS/DVL System. The SINS/DVL is

integrated by a loosely coupled method and the diagram of the integration navigation
system is shown in Figure 2. DVL velocity information is used to compensate SINS
navigation errors through the adaptive Kalman filter, in which the navigation
information from the subsystem is fused adaptively. Moreover, the output correction
is applied to correct the navigation parameters from the SINS for navigation
solutions.
The components of the position and velocity errors at a level plane are only
considered in this paper as the vertical motion is negligible when a marine vehicle is
sailing. Hence, the state vector x is composed of seven navigation error variables,
and it is expressed as x = [δL δλ δνe δνn φe φn φu]T. The state space model of
the SINS that is a time varying linear stochastic system is described in Equation (20).
ẋ(t) = Ax(t) + Bw(t) (20)
where w(t) = [0 0 wax way wgx wgy wgz] , and subscripts e, n, u denote the
T
east, north and up components in the navigation frame, respectively, and x, y, z denote
the right, front and up components in the body frame, respectively. The process noise
vector w(t) consists of the Gaussian white noises of the accelerometers and gyroscopes.
A is the state transition matrix and B is the process noise matrix. Besides, the elements
of matrix A and B are listed as follows.
2 3 2 3
A11 A12 A13 B11 04×3
A=4 5, B =4 5
A21 A22 A23 03×4 B22
2 3 2 3
1
0 0 0
6 7 6 Rm 7
6 7 6 7
6
ve
7 6 7
6 sec L tan L 07 6 sec L 7
6 R n 7 6 0 7
6 7 6 Rn 7
6 v v 7 6 7
A11 = 6 2w v cos L + e n
sec2 L 0 7, A12 =6 7
6 ie n 7 6 vn v e 7
6 Rn 7 6 tan L 2wie sin L + tan L 7
6 ! 7 6 Rn Rn 7
6 7 6 7
4 v2e 5 6 7
− 2wie vn cos L + sec2 L 0 4 ve 5
Rn − 2wie sin L + tan L 0
Rn
2 3 2 3
1
0 0 0 2 3 0 −
6 7 0 0 6 Rm 7
6 7 6 7
6 0 7 6 7 6 7
6 0 0 7 6 7 6 1 7
A13 =6 7, A21 = 6 6
−wie sin L 0 7A = 6
7 22 6 Rn 0 7
7
6 7 4 5 6 7
6 0 −g fn 7 v e 6 7
4 5 wie cos L + 2
sec L 0 4 1 5
Rn tan L 0
g 0 −fe Rn
2 ve ve 3
0 wie sin L + tan L −(wie cos L + )
6 R n R n 7
6 7
6 7
6 ve vn 7
A23 = 6 − wie sin L + tan L 0 − 7
6 Rn R m 7
6 7
4 ve vn 5
wie cos L + 0
Rn Rm
2 3
0 0 0 0 2 3
6 7 Cbn (1, 1) Cbn (1, 2) Cbn (1, 3)
6 7 6 7
60 0 0 0 7 6 n 7
6 7
B 11 =6 7, B22 = 6 6 b C (2, 1) C n (2, 2) C n (2, 3) 7
7
6 0 0 C n (1, 1) C n (1, 2) 7 4
b b
5
6 7
4 b b 5 n n
Cb (3, 1) Cb (3, 2) Cb (3, 3) n
0 0 Cbn (2, 1) Cbn (2, 2)
where Rm and Rn are the main curvature radii along the meridian and the vertical
plane of the meridian, respectively, L is the local latitude and Cnb is the direction cosine
matrix from the body frame to the navigation frame.
Moreover, the measurement error model can be determined from the level velocity
errors obtained from the DVL receiver and the navigation solution from the SINS.
Therefore, the measurement model can be written as
SINS
v −vDVL
z(t) = eSINS e
= Hx(t) + v(t) (21)
vn −vDVL
n
Table 1. Initial Simulation Specification.
Initial position Initial velocity Initial attitude
Longitute Latitute Altitute Horizon Vertical Heading Pitch Roll
126·67° 45·77° 170 m 10 m/s 0 m/s 5° 0·1° 0·1°
T
where H = [02 × 2 I2 × 2 03 × 3], ν(t) = νDVL
e νDVL
n . H is the measurement matrix
and ν(t) is the measurement noise with time-varying covariance R(t).
The MLE algorithm and the Sage-Husa algorithm are two popular types of
adaptive Kalman algorithm dealing with the parameter estimation problems when the
noise statistics are unknown and/or time varying. Therefore, the MLE algorithm in
Mohamed and Schwarz (1999) and the Sage-Husa algorithm in Sage and Husa
(1969b) and the proposed AKF are adopted respectively to estimate the navigation
parameter errors, e.g. attitude and velocity errors in the dynamic integration system.
Thus, the performance of the proposed AKF can be measured by contrasting the
outputs with other AKF. Besides, output correction is used to modify the parameters
of SINS. The initial simulation information is shown in Table 1.
In addition, the size of innovation sequence window is N = 10.
The following situations are considered in the simulation.
1) The initial value of the measurement noise is unknown and set ten times the true
value.
2) The magnitude of the measurement noise is time varying and enlarged four times
during 720 s * 1440 s (12 min * 24 min).
3) The DVL signals are lost during 1695s * 1700s (in the 28 min), which denotes
the outliers that appear at that period and the outputs of the SINS are the only
measurements to the filters.
The estimate results of the attitude, velocity and position errors are shown
in Figures 3 to 5, respectively.
These results show that MLE, Sage-Husa and the proposed AKF, when the initial
value of the measurement noise covariance is unknown, can make adaptive
adjustments. However, a temporary divergence occurs in MLE at the period 12 to
24 minutes when the magnitude of the noise covariance is enlarged four times, as
shown in Figures 3 to 5, and a sharp jump appears in Sage-Husa after 28 minutes due
to the loss of the DVL signals.
When the measurement noise is enlarged or lost, the estimate errors of the MLE
and Sage-Husa increase immediately. The proposed AKF can effectively restrain the
disturbances from the measurement signals and improve the accuracy, e.g. the heading
error declines 35·7%. A comparison of the heading RMS errors is shown in Table 2.
Subsequently, compared with the MLE and Sage-Husa algorithms, the proposed
AKF performs better on the stability as shown in Figures 3 to 5. The AKF shows a
strong robustness as it works steadily during the simulation, while a temporary
divergence occurs in MLE and a sharp jump appears in Sage-Husa.
Figure 6 shows the estimate result of measurement noise covariance R. The true
value of R is set as 0·25 and R is enlarged four times from 12 to 24 minutes. The
estimate result is consistent with the changes of the measurement noise. In particular,
Figure 3. Estimation of attitude errors.
Figure 4. Estimation of velocity errors.
Table 2. Heading RMS Error.
Filtering Method Heading RMS error (°)
MLE 0·0014
Sage-Husa 0·0018
AKF 0·0009
an impulse occurs after 28 minutes when the measurement signals are lost and it lasts
for 5 s, but the AKF can eliminate the disturbance rapidly and then reach
convergence. Table 3 denotes the estimation error of measurement noise covariance
Longitude error(m)
Figure 5. Estimation of position errors.
Figure 6. Estimation of measurement noise covariance.
Table 3. Estimation error of measurement noise covariance.
Filtering Method Estimation RMS error of R ((m/s)2)
MLE 0·040
Sage-Husa 0·203
AKF 0·009
after convergence (RMS value, taken during the last 10 minutes), and it shows that the
AKF has the best accuracy among these adaptive filters.
4.2. Experiments and Results of Real Voyage Data. Besides the simulation in the
SINS/DVL integration system, the proposed AKF algorithm is also used to process
the real voyage data of a marine vehicle for testing the actual performance of the filter.
Figure 7. Estimation of measurement noise covariance with different window sizes.
The voyage data from the marine vehicle were collected in the Bohai Sea of China. In
the experiment, the attitude and velocity references are given by the PHotonic Inertial
Navigation System (PHINS), which is a complete INS product from the company
IXSEA, and the attitude and velocity accuracy of the PHINS are 0·01° and 0·1 m/s,
respectively. Moreover, a Fibre Optic Gyroscope (FOG)-based SINS (named HZ-24)
which is developed by our laboratory is adopted as the inertial measurement unit
(IMU), and it provides the raw IMU information at 100 Hz. Specifically, the
gyroscope constant drift and the accelerometer constant bias of the HZ-24 are about
0·01°/h and 100°μg, respectively. The DVL provides the resultant velocity with the
accuracy ±1% of the speed and it updates at 10 Hz. Hence, the filter updates at 10 Hz
when the measurements are available and the outputs of the adaptive filter are used to
correct the solutions of the SINS in the experiment.
As the estimations of the noise covariance depend on the calculation of Ck, which is
calculated by averaging the recent N-step innovation covariances, a test (Li et al.,
2013) is done first for choosing an appropriate value of N.
The estimation of measurement noise covariance with different sizes of the
innovation sequence window are shown in Figure 7 and all the estimates converge to
the value of around 0·25 (m2/s2) finally. From the figure, we can see that the estimation
oscillation becomes obvious when short window sizes are used to adapt to the fast
changes and that the trend of the estimation convergence becomes slow but stable
when long window sizes are used. Consequently, the size of the innovation sequence is
selected as N = 100 for the actual SINS/DVL system.
Figures 8 to 9 show the comparison of attitude and velocity errors of the actual
SINS/DVL system. The experimental results show that the proposed AKF algorithm
is the most robust during the test, while the estimation fluctuations of the other two
filters are obvious. As expected, the estimation accuracy of the AKF is the best of all,
and different biases of the heading error of the other two adaptive filters occur after
the filters come to the steady state. Therefore, the proposed AKF algorithm can keep
robust against the disturbances and improve the accuracy of navigation results with
the real voyage data.
4.3. Long Endurance Test. In order to test the stability of the proposed AKF
algorithm for long application periods, a long endurance test was done with 30 hours
of real voyage data.
Figure 8. Comparison of attitude errors.
Figure 9. Comparison of velocity errors.
The sailing trajectory of the marine vehicle for the long endurance test is shown in
Figure 10 and the turning manoeuvres are well illustrated by the sailing trajectory.
Figure 11 to 12 describe the attitude results and corresponding errors of AKF
algorithm. It is noted that, from Figure 11, the marine vehicle takes turning
manoeuvres with complete ±180° turns five times during the test. Figure 13 to 14
describe the velocity results and corresponding errors with vigorous manoeuvres in the
long endurance test.
From the figures, it can be seen that the manoeuvre methods of the marine vehicle in
the test include acceleration, deceleration and sharp turns. Besides, the sea status,
during the voyage, is rough as the velocity oscillation becomes obvious when the
vehicle is in anchoring state. Actually, the performances of the filter and the
integration navigation system are highly affected by the motion methods and the sea
conditions. Though the application environments are tough and complicated, the
Figure 10. Sailing trajectory of the marine vehicle in the long endurance test.
Figure 11. Attitude results in the long Figure 12. Attitude errors in the long
endurance test. endurance test.
Figure 13. Velocity results in the long Figure 14. Velocity errors in the long
endurance test. endurance test.
proposed AKF algorithm can remain robust against the disturbances and improve the
accuracy of navigation results in the presence of heavy manoeuvres and complicated
sea conditions during the long voyage. Therefore, as illustrated by the figures, the
performance of the proposed AKF algorithm is up to the requirements for long
application periods.
5. C O N C L U S I O N S . In this paper, an adaptive Kalman filtering algorithm is

proposed to solve the estimation problems when noise statistics are unknown or time
varying in an integrated SINS/ DVL system. A recursive noise estimator based on
MAP estimation and one-step-smoothing filtering is derived to provide more accurate
noise statistical characteristics for the filter. Besides, an innovation covariance
estimator with a fixed memory is introduced to modify the noise estimator. Compared
with the MLE and Sage-Husa adaptive filtering, the proposed AKF is more robust in
the presence of the time-varying noise, which denotes that the proposed AKF can
adaptively tune the filter gain and estimate the noise statistics with the changes of the
innovation sequences. The steady state errors decline prominently, e.g. the heading
RMS errors decline 35·7%. Moreover, the proposed AKF algorithm is also used to
process the real voyage data of a marine vehicle for testing the actual performance of
the filter. The experiment results show that the proposed AKF algorithm can remain
robust against disturbances and improve the accuracy of navigation results with real
voyage data. In addition, a long endurance test demonstrates that the performance of
the proposed AKF algorithm is up to the requirements for long application periods.
Consequently, the performance of the proposed AKF is better than that of the MLE
and Sage-Husa adaptive filtering.
ACKNOWLEDGEMENTS
The work described in the paper was supported by the National Natural Science Foundation of
China (61203225) and the National Science Foundation for Post-doctoral Scientists of China
(2012M510083). The authors would like to thank all members of the inertial navigation research
group at Harbin Engineering University for the technical assistance with the integrated
navigation system.
REFERENCES
Al-Shabi, M., Gadsden, S.A. and Habibi, S.R. (2012). Kalman filtering strategies utilizing the chattering
effects of the smooth variable structure filter. Signal Processing, 93, 420–431.
Bavdekar, V.A., Deshpande, A.P. and Patwardhan, S.C. (2011). Identification of process and measurement
noise covariance for state and parameter estimation using extended Kalman filter. Journal of Process
Control, 21(4), 585–601.
Ding, W., Wang, J., Rizos, C. and Kinlyside, D. (2007). Improving adaptive Kalman estimation in GPS/
INS integration. Journal of Navigation, 60(3), 517–529.
Fang, J.C. and Yang, S. (2011). Study on innovation adaptive EKF for in-flight alignment of airborne POS.
Instrumentation and Measurement, IEEE Transactions on, 60(4), 1378–1388.
Gao, X., You, D. and Katayama, S. (2012). Seam Tracking Monitoring Based on Adaptive Kalman Filter
Embedded Elman Neural Network During High-Power Fiber Laser Welding. Industrial Electronics,
IEEE Transactions on, 59(11), 4315–4325.
Gustafsson, F. (2000). Adaptive filtering and change detection. John Wiley & Sons, New York.
Hewitson, S. and Wang, J. (2006). GPS/GLONASS/Galileo receiver autonomous integrity monitoring
(RAIM) performance analysis. GPS Solutions, 10(3), 155–170.
Hewitson, S. and Wang, J. (2010). Extended Receiver Autonomous Integrity Monitoring (eRAIM) for
GNSS/INS Integration. Journal of Surveying Engineering, 136(1), 13–22.
Jwo, D.J. and Weng, T.P. (2008). An adaptive sensor fusion method with applications in integrated
navigation. Journal of Navigation, 61(4), 705–721.
Karasalo, M. and Hu, X. (2011). An optimization approach to adaptive Kalman filtering. Automatica,
47(8), 1785–1793.
Knight, N.L., Wang, J. and Rizos, C. (2010). Generalised Measures of Reliability for Multiple Outliers.
Journal of Geodesy, 84(10), 625–635.
Mehra, R. (1970). On the identification of variances and adaptive Kalman filtering. Automatic Control,
IEEE Transactions on, 15(2), 175–184.
Mehra, R. (1972). Approaches to adaptive filtering. Automatic Control, IEEE Transactions on, 17(5),
693–698.
Mohamed, A.H. and Schwarz, K.P. (1999). Adaptive Kalman filtering for INS/GPS. Journal of Geodesy,
73(4), 193–203.
Myers, K. and Tapley, B. (1976). Adaptive sequential estimation with unknown noise statistics. Automatic
Control, IEEE Transactions on, 21(4), 520–523.
Odelson, B.J., Rajamani, M.R. and Rawlings, J.B. (2006). A new autocovariance least-squares method for
estimating noise covariances. Automatica, 42(2), 303–308.
Sage, A.P. and Husa, G.W. (1969a). Adaptive filtering with unknown prior statistics. In Proceedings of joint
automatic control conference, Boulder, 760–769.
Sage, A.P. and Husa, G. W. (1969b). Algorithms for sequential adaptive estimation of prior statistics. In
Adaptive Processes (8th) Decision and Control, 1969 IEEE Symposium on, 61–70.
Sangsuk-Iam, S. and Bullock, T.E. (1990). Analysis of discrete-time Kalman filtering under incorrect noise
covariances. Automatic Control, IEEE Transactions on, 35(12), 1304–1309.
Sarkka, S. and Nummenmaa, A. (2009). Recursive noise adaptive Kalman filtering by variational Bayesian
approximations. Automatic Control, IEEE Transactions on, 54(3), 596–600.
Wang, J. (2000). Stochastic modeling for RTK GPS/GLONASS positioning. Navigation: Journal of the US
Institute of Navigation, 46(4), 297–305.
Li, W., Wang, J., Lu, L. and Wu, W. (2013). A novel scheme for DVL-aided SINS in-motion alignment
using UKF techniques. Sensors, 13, 1046–1063.
Zhang, Y. (2013). An approach of DVL-aided SDINS alignment for in-motion vessel. Optik, 124,
6270–6275.
Zhou, J., Knedlik, S. and Loffeld, O. (2010). INS/GPS Tightly-coupled integration using adaptive
unscented particle filter. Journal of Navigation, 63(3), 491–511.
APPENDIX
D E R I VAT I O N O F T H E U N B I A S E D M E A S U R E M E N T
NOISE ESTIMATOR
In theory, the following is satisfied
εj εTj = H j P j,j−1 H Tj + R (A1)
Substitute Equation (A1) into Equation (5c) to obtain an equivalent expression
K j εj εTj = P j,j−1 H Tj (A2)
εj εTj K Tj = (K j εj εTj )T = H j PTj,j−1 = H j Pj,j−1 (A3)
The expansion of the term in Equation (8d) is written as
(I − H j K j )εj εTj (I − H j K j )T
(A4)
= εj εTj − H j K j εj εTj − εj εTj K Tj H Tj + H j K j εj εTj K Tj H Tj
Substituting Equations (A1)–(A3) into Equation (A4) leads to the following:
(I − H j K j )εj εTj (I − H j K j )T
= (H j P j,j−1 H Tj + R) − 2H j P j,j−1 H Tj + H j Kj H j P j,j−1 H Tj (A5)
= R − (I − H j Kj )H j P j,j−1 H Tj
The expectations of the measurement noise mean and covariance can be expressed as
( )
1X k
E[r̂k ] = E [(I − H j K j )εj + r] = r (A6)
k j=1
( )
1X k
E[R̂k ] =E [(I − H j K j )εj εj (I − H j K j ) ]
T T
k j=1
( )
1X k
=E [R − (I − H j Kj )H j Pj,j−1 H j ]T
(A7)
k j=1
( )
1X k
=R−E [(I − H j Kj )H j Pj,j−1 H Tj ]
k j=1
Equation (A6) indicates that Equation (8c) is the unbiased estimation of measure-
ment noise statistical mean. The unbiased estimation of measurement noise
statistical covariance is obtained through rearranging Equation (A7), and it can
be expressed as
1X k
R̂k = (I − H j K j )[εj εTj (I − H j K j )T + H j P j,j−1 H Tj ] (A8)
k j=1

Gao Et Al - Adaptive Kalman Filtering With Recursive Noise Estimator ForIntegrated SINSDVL Systems

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Gao Et Al - Adaptive Kalman Filtering With Recursive Noise Estimator ForIntegrated SINSDVL Systems

Uploaded by

Copyright:

Available Formats

THE JOURNAL OF NAVIGATION (2015), 68, 142–161.

© The Royal Institute of Navigation 2014

Adaptive Kalman Filtering with

1. I N T R O D U C T I O N . The Kalman Filter (KF) is considered to be an

2. M A P N O I S E E S T I M AT O R . The dynamic system model is described by

3. A D A P T I V E K A L M A N F I LT E R I N G . In this paper, an AKF algor-

Therefore, an equivalent expression of the suboptimal MAP noise estimator is

Additionally, the derivation of Equation (10) refers to Equations (A1)–(A3) in the

Similarly, the unbiased measurement noise estimator is obtained. In particular,

Q̂k = (1 − τ k )Q̂k−1 + Ξk {τ k [K k εk εTk K Tk + Pk − Φk−1 Pk−1 ΦTk−1 ]}ΞTk (17b)

r̂k = r̂k−1 + τ k [(I − H k K k )εk ] (17c)

R̂ = (1 − τ k )R̂k−1 + τ k (I − H k K k )[εk εTk (I − H k K k )T + H k Pk,k−1 H Tk ] (17d)

innovation covariances. The ICE can be written as

where N denotes the length of the moving window.

Figure 1. Operation of the adaptive Kalman ﬁltering.

IMU (ve ,vn ) DVL

Figure 2. Diagram of the integrated SINS/DVL navigation system.

4.1. Simulation and Analysis in the SINS/DVL System. The SINS/DVL is

Initial position Initial velocity Initial attitude

Longitute Latitute Altitute Horizon Vertical Heading Pitch Roll

126·67° 45·77° 170 m 10 m/s 0 m/s 5° 0·1° 0·1°

Figure 3. Estimation of attitude errors.

Figure 4. Estimation of velocity errors.

Table 2. Heading RMS Error.

Filtering Method Heading RMS error (°)

Figure 5. Estimation of position errors.

Figure 6. Estimation of measurement noise covariance.

Table 3. Estimation error of measurement noise covariance.

Filtering Method Estimation RMS error of R ((m/s)2)

Figure 7. Estimation of measurement noise covariance with different window sizes.

Figure 8. Comparison of attitude errors.

Figure 9. Comparison of velocity errors.

5. C O N C L U S I O N S . In this paper, an adaptive Kalman ﬁltering algorithm is

In theory, the following is satisﬁed

εj εTj = H j P j,j−1 H Tj + R (A1)

Substitute Equation (A1) into Equation (5c) to obtain an equivalent expression

K j εj εTj = P j,j−1 H Tj (A2)

εj εTj K Tj = (K j εj εTj )T = H j PTj,j−1 = H j Pj,j−1 (A3)

You might also like