# A New Adaptive Square-Root Unscented Kalman Filter for Nonlinear Systems with Additive Noise

Authors: Yong Zhou; Chao Zhang; Yufeng Zhang; Juzhong Zhang
Journal: International Journal of Aerospace Engineering
 (2015)

---

## Abstract

The Kalman filter (KF), extended KF, and unscented KF all lack a self-adaptive capacity to deal with system noise. This paper describes a new adaptive filtering approach for nonlinear systems with additive noise. Based on the square-root unscented KF (SRUKF), traditional Maybeck’s estimator is modified and extended to nonlinear systems. The square root of the process noise covariance matrixQ or that of the measurement noise covariance matrix R is estimated straightforwardly. Because positive semidefiniteness of Q or R is guaranteed, several shortcomings of traditional Maybeck’s algorithm are overcome. Thus, the stability and accuracy of the filter are greatly improved. In addition, based on three different nonlinear systems, a new adaptive filtering technique is described in detail. Specifically, simulation results are presented, where the new filter was applied to a highly nonlinear model (i.e., the univariate nonstationary growth model (UNGM)). The UNGM is compared with the standard SRUKF to demonstrate its superior filtering performance. The adaptive SRUKF (ASRUKF) algorithm can complete direct recursion and calculate the square roots of the variance matrixes of the system state and noise, which ensures the symmetry and nonnegative definiteness of the matrixes and greatly improves the accuracy, stability, and self-adaptability of the filter.


---

## Body

## 1. Introduction

In 1960, Kalman described a recursive minimum mean-square estimation (RMMSE) solution of the linear discrete-time dynamical system filter problem [1]. Since then, the Kalman filter (KF) has been the subject of extensive research in many applications, particularly in autonomous or assisted navigation. However, many real-world systems are nonlinear. Sunahara and Bucy et al. extended the use of Kalman filtering through a linearization procedure, which is referred to as the extended Kalman filter (EKF) [2, 3].The EKF applies the standard linear Kalman filter methodology by linearizing the true nonlinear system. This approach is suboptimal and can easily lead to divergence. Intuitively, a fixed number of parameters should be easier to approximate with a Gaussian distribution than with an arbitrary nonlinear function/transformation. Julier et al. proposed the unscented Kalman filter (UKF) as a derivative-free alternative to the EKF [4, 5]. The UKF is more accurate, more stable, and far easier to implement than the EKF [6–8], as proven in aircraft state estimation [9], unmanned aerial vehicle (UAV) attitude estimation [10], rotorcraft UAV actuator failure estimation [11], and satellite attitude estimation [12], among other fields. The UKF was extended to the square-root unscented Kalman filter (SRUKF) by van der Merwe and Wan [13].All of the aforementioned filtering approaches can achieve good performance under certain assumptions. However, the assumptions are typically not entirely satisfied. Thus, the performance of the filters may be seriously downgraded from the theoretical performance, which can potentially lead to divergence. To fulfill the requirement of achieving an optimal Kalman filter and to prevent divergence, the so-called adaptive Kalman filter (AKF) approach has been used to dynamically adjust the parameters of the supposedly optimum filter based on estimating the unknown parameters for online estimation of motion and by estimating the signal and noise statistics from the available data [14, 15]. Maybeck’s estimator, which was originally proposed by Mehra [16, 17] and later summarized by Maybeck [18], is a maximum-likelihood estimator. This simple, real-time AKF-type algorithm has become extremely popular [19–21].However, there are two well-known disadvantages associated with the traditional Maybeck estimator. First, it is not very stable and often leads to a nonpositive semidefiniteness for the estimatedQ or R. Second, the Maybeck estimator cannot estimate Q and R simultaneously [18–21]. In addition, Maybeck’s algorithm is primarily designed for linear systems (such as KF) or linearized nonlinear systems (such as EKF). Thus, there are few applications for Maybeck’s algorithm in the nonlinear field.To overcome these shortcomings, we developed the concept of a new adaptive square-root unscented Kalman filter (ASRUKF) in 2013 [22]. However, in that work, we only presented the algorithm briefly and did not take the next step of validating its effectiveness. Therefore, in this study, we detailed the ASRUKF for nonlinear systems with additive noise by extending the SRUKF with the traditional Maybeck estimator to nonlinear systems and then tested its effectiveness. To achieve these objectives, a new ASRUKF technique is described in detail based on three different nonlinear systems.


## 2. The AKF Based on the Maybeck’s Estimator

### 2.1. Kalman Filter

The dynamical system considered here is a linear, discrete, stochastic sequence described by(1)
x
k
=
Φ
k
,
k
-
1
x
k
-
1
+
B
k
,
k
-
1
u
k
-
1
+
w
k
-
1
,
z
k
=
H
k
x
k
+
v
k
,where x
(
k
)
∈
R
L is the state vector, Φ
k
,
k
-
1
(
k
)
∈
R
L
×
L is the state transition matrix taking state x
(
k
-
1
) from time k
-
1 to time k, u
(
k
)
∈
R
r is a known exogenous input, B
k
,
k
-
1
∈
R
L
×
r is the gain of u
(
k
-
1
), z
(
k
)
∈
R
m is the observation vector, and H
(
k
)
∈
R
m
×
L is the observation mapping matrix. The process noise w
(
k
)
∈
R
L and the measurement noise v
(
k
)
∈
R
m are assumed to be additive, independent, white, and Gaussian with the following properties:(2)
E
w
k
=
0
,
E
w
k
w
j
T
=
Q
k
δ
k
j
,
E
v
k
=
0
,
E
v
k
v
j
T
=
R
k
δ
k
j
,
E
w
k
v
j
T
=
0
.The Kalman filter is represented by the following equations:(3)
x
^
-
k
=
Φ
k
,
k
-
1
x
^
k
-
1
+
B
k
,
k
-
1
u
k
-
1
,
P
-
k
=
Φ
k
,
k
-
1
P
k
-
1
Φ
k
,
k
-
1
T
+
Q
k
-
1
,
K
k
=
P
-
k
H
k
T
H
k
P
-
k
H
k
T
+
R
k
-
1
,
x
^
k
=
x
^
-
k
+
K
k
z
k
-
H
k
x
^
-
k
,
P
k
=
I
-
K
k
H
k
P
-
k
.As shown in (3), if the initial values x
^
(
0
) and P
(
0
) are given, according to z
(
k
) at time k, the state estimation x
^
(
k
)
(
k
=
1,2
,
…
) can be recursively calculated. The Kalman filter algorithm is shown in Figure 1.Figure 1
Kalman filtering process.The algorithm works in a two-step process, with a prediction step and an update step. As prior estimates, the current state variables are used in the update equations. The prior estimates are calibrated via the update equations to obtain the a posteriori estimates. After these two steps, the process repeats. The last a posteriori estimates are calculated as the next a priori estimates. This recursive process is one of the most attractive features of the Kalman filter and makes it easier to implement than other filters.


### 2.2. Extended Kalman Filter

The EKF system is a nonlinear, discrete, stochastic sequence described by(4)
x
k
=
f
x
k
-
1
,
u
k
-
1
+
w
k
-
1
,
z
k
=
h
x
k
+
v
k
,where f
(
x
) and h
(
x
) are nonlinear functions of the system. The other parameters are defined as in (1). The EKF can be expressed as(5)
x
^
-
k
=
f
x
^
k
-
1
,
u
k
-
1
,
P
-
k
=
Φ
k
,
k
-
1
P
k
-
1
Φ
k
,
k
-
1
T
+
Q
k
-
1
,
K
k
=
P
-
k
H
k
T
H
k
P
-
k
H
k
T
+
R
k
-
1
,
x
^
k
=
x
^
-
k
+
K
k
z
k
-
h
x
^
-
k
,
P
k
=
I
-
K
k
H
k
P
-
k
,where(6)
Φ
k
,
k
-
1
=
∂
f
∂
x
x
=
x
^
k
-
1
u
=
u
k
-
1
,
H
k
=
∂
h
∂
x
x
=
x
^
-
k
.The EKF algorithm uses the filter x
^
(
k
) and a Taylor series expansion to linearize the nonlinear functions f
(
x
) and h
(
x
). The higher order terms are omitted, and the linearization model is established. The EKF is a suboptimal filter, which is suitable only when the filtering error x
(
k
)
-
x
^
(
k
) and the prediction error x
(
k
)
-
x
^
-
(
k
) are small.


### 2.3. Unscented Kalman Filter

The UKF is a type of classic Kalman filter. According to the unscented transformation (UT), the UKF chooses sample points that exhibit the same statistical characteristics as the system state. These sample points are nonlinearly transformed through the nonlinear state equations and measurement equations. The mean and covariance can then be calculated using a weighted calculation. Finally, combined with the KF, the estimated system states can be recursively calculated.The UT uses the sample point setS (the so-called sigma points) as the input distribution. This algorithm addresses the entire system as a “black box” and does not depend on the specific nonlinear function or the calculation of the Jacobian matrix.The random vectorx (with dimension L) is nonlinearly transformed according to(7)
y
=
g
x
,where x
- is the mean and P
x is the variance. To obtain the statistical characteristics (mean and covariance) of y, a matrix χ is created. This matrix contains 2
L
+
1 sigma vectors represented by(8)
χ
0
=
x
-
,
χ
i
=
x
-
+
L
+
λ
P
x
i
,
i
=
1
,
…
,
L
,
χ
i
=
x
-
-
L
+
λ
P
x
i
-
L
,
i
=
L
+
1
,
…
,
2
L
,where (
(
L
+
λ
)
P
x
)
i is the ith column of the square root of the matrix (
L
+
λ
)
P
x. All weights that correspond to sigma points are defined according to(9)
W
0
m
=
λ
L
+
λ
,
W
0
c
=
λ
L
+
λ
+
1
-
α
2
+
β
,
W
i
m
=
W
i
c
=
1
2
L
+
λ
,
i
=
1
,
…
,
2
L
,where W
i
(
m
) is the weight of the mean, W
i
(
c
) is the weight of the covariance, and λ
=
α
2
(
L
+
κ
)
-
L is a scaling parameter. The constant α controls the distribution range of the sigma points around the mean x
-. Generally, α is a small, positive number (e.g., 1
0
-
4
≤
α
≤
1). The auxiliary scaling parameter is represented by κ, which is typically set at κ
=
0 to maintain a positive, semidefinite variance matrix. If β
≥
0, the prior distribution of the high-order matrix contains the random vector x.To obtain the mean and covariance of the output vectory, the sigma points (7) are obtained using the following nonlinear transformation:(10)
ζ
i
=
g
χ
i
,
i
=
0
,
…
,
2
L
.The mean and covariance of output vectory can then be obtained as follows:(11)
y
-
≈
∑
i
=
0
2
L
W
i
m
ζ
i
,
P
y
≈
∑
i
=
0
2
L
W
i
c
ζ
i
-
y
-
ζ
i
-
y
-
T
.The main advantage of UKF is that it does not use linearization to calculate the state predictions, covariance matrices. Instead, it uses a deterministic sampling approach to capture the mean and covariance estimates with a minimal set of2
L
+
1 sigma points based on a square-root decomposition of the prior covariance [23].According to the stochastic nonlinear discrete systems shown in (4), the UKF algorithm is the following:(I)
Initializing,k
=
0:(12)
x
^
0
=
E
x
0
,
P
0
=
E
x
0
-
x
^
0
x
0
-
x
^
0
T
.
(II)
Iterating,k
=
1,2
,
…:
(a)
Time update.
First, the sigma points are calculated, and the matrix is constructed:(13)
χ
k
-
1
=
x
^
k
-
1
x
^
k
-
1
+
L
+
λ
P
k
-
1
x
^
k
-
1
-
L
+
λ
P
k
-
1
.
The sigma point set is nonlinearly transformed by the state equations. The state and variance are then predicted:(14)
χ
^
∗
k
=
f
χ
k
-
1
,
u
k
-
1
,
x
^
-
k
=
∑
i
=
0
2
L
W
i
m
χ
^
i
∗
k
,
P
-
k
=
∑
i
=
0
2
L
W
i
c
χ
^
i
∗
k
-
x
^
-
k
χ
^
i
∗
k
-
x
^
-
k
T
+
Q
k
-
1
.
Then, the sigma points are calculated again:(15)
χ
-
k
=
x
^
-
k
x
^
-
k
+
L
+
λ
P
-
k
x
^
-
k
-
L
+
λ
P
-
k
.
The sigma point set is nonlinearly transformed by the measurement equations. The predicted output of system is calculated:(16)
Z
^
-
k
=
h
χ
^
-
k
,
z
^
-
k
=
∑
i
=
0
2
L
W
i
m
Z
^
i
-
k
.
(b)
Measurement update.
Calculate the filter gain:(17)
P
x
z
k
=
∑
i
=
0
2
L
W
i
c
χ
^
i
-
k
-
x
^
-
k
Z
^
i
-
k
-
z
^
-
k
T
,
P
z
k
=
∑
i
=
0
2
L
W
i
c
Z
^
i
-
k
-
z
^
-
k
Z
^
i
-
k
-
z
^
-
k
T
+
R
k
,
K
k
=
P
x
z
k
P
z
-
1
k
.
Estimate the state value after correction:(18)
x
^
k
=
x
^
-
k
+
K
k
z
k
-
z
^
-
k
.
Calculate the a posteriori state variance:(19)
P
k
=
P
-
k
-
K
k
P
z
k
K
k
T
.


### 2.4. AKF

In the AKF, the uncertain parameters that need to be adapted may be part of the system model throughΦ or H or the statistical information through R
/
Q [24]. The AKF based on Maybeck’s estimator is as follows.Initialize with(20)
x
^
0
=
E
x
0
,
P
0
=
E
x
0
-
x
^
0
x
0
-
x
^
0
T
,
Q
^
0
=
Q
_
0
,
R
^
0
=
R
_
0
.Fork
=
1,2
,
…
∞, Maybeck’s estimator is(21)
x
~
k
=
x
^
k
-
x
^
-
k
,
Q
^
k
=
Q
^
k
-
1
+
1
L
Q
x
~
k
x
~
k
T
+
P
k
-
Φ
k
,
k
-
1
P
k
-
1
Φ
k
,
k
-
1
T
-
Q
^
k
-
1
,
z
~
k
=
z
k
-
H
k
x
^
k
,
R
^
k
=
R
^
k
-
1
+
1
L
R
z
~
k
z
~
k
T
-
H
k
P
k
H
k
T
-
R
^
k
-
1
,where L
Q
/
L
R is the window size. Maybeck’s estimator sets the number of updates being averaged together or the window of past measurements that are averaged [20].


### 2.5. The New ASRUKF

The KF, EKF, and UKF assume that the statistical property of system noise is known. However, in the real world, the process noise covariance matrixQ or observation noise variance matrix R is often unknown. In addition, these parameters can vary with time. Hence, to adjust Q and R, an adaptive Kalman filter must be constructed, where it is important to improve the filtering accuracy and stability. Combined with the square-root filter algorithm, the traditional Sage-Husa adaptive filtering algorithm can be improved. The new ASRUKF can estimate the square root of Q or R in a straightforward manner such that positive semidefiniteness is guaranteed, which greatly improves the stability and accuracy of the filter.Based on (1), Sage and Husa proposed an adaptive filtering algorithm based on the maximum a posteriori estimation [25]. Based on this algorithm, the forgetting factor (sometimes called the “fading factor” [26, 27]) was introduced by Deng and Guo [28]. Subsequently, the algorithm has the ability to estimate the unknown time-varying noise. The specific calculation process is as follows:(I)
Initializing,k
=
0:(22)
x
^
0
=
E
x
0
,
P
0
=
E
x
0
-
x
^
0
x
0
-
x
^
0
T
,
Q
^
0
=
Q
_
0
,
R
^
0
=
R
_
0
.
(II)
Iterating,k
=
1,2
,
…:
(a)
Time update:(23)
x
^
-
k
=
Φ
k
,
k
-
1
x
^
k
-
1
+
B
k
,
k
-
1
u
k
-
1
,
P
-
k
=
Φ
k
,
k
-
1
P
k
-
1
Φ
k
,
k
-
1
T
+
Q
^
k
-
1
.
(b)
Measurement update.
Estimate the statistical properties of measurement noise:(24)
z
~
=
z
k
-
H
k
x
^
-
k
,
R
^
k
=
1
-
d
k
R
^
k
-
1
+
d
k
z
~
k
z
~
k
T
-
H
k
P
-
k
H
k
T
.
Estimate the state value. After correction, calculate the a posteriori state variance:(25)
K
k
=
P
-
k
H
k
T
H
k
P
-
k
H
k
T
+
R
^
k
-
1
,
x
^
k
=
x
^
-
k
+
K
k
z
~
,
P
k
=
I
-
K
k
H
k
P
-
k
.
Estimate the statistical properties of the process noise according to(26)
Q
^
k
=
1
-
d
k
Q
^
k
-
1
+
d
k
·
K
k
z
~
k
z
~
k
T
K
k
T
+
P
k
-
Φ
k
,
k
-
1
P
k
-
1
Φ
k
,
k
-
1
T
,
whered
(
k
)
=
(
1
-
b
)
/
(
1
-
b
k
+
1
), b is the forgetting factor, and 0
<
b
<
1.Based on the SRUKF, traditional Maybeck’s estimator can be modified and extended to nonlinear systems. Thus, a new adaptive filtering approach for nonlinear systems with additive noise is developed. According to the three different structures of the nonlinear problem, the three new ASRUKF models are the following.(
1
) Both the process and the measurement equations are nonlinear (PNMN) according to(27)
x
k
=
f
x
k
-
1
,
u
k
-
1
+
w
k
-
1
,
z
k
=
h
x
k
+
v
k
,where f
(
·
) and h
(
·
) are nonlinear functions. The new ASRUKF for PNMN is the following.Initialize with(28)
x
^
0
=
E
x
0
,
S
0
=
chol
E
x
0
-
x
^
0
x
0
-
x
^
0
T
,
Q
^
0
=
S
0
,
R
^
0
=
chol
E
z
0
-
z
^
0
z
0
-
z
^
0
T
.Fork
=
1
,
2
,
…
∞, calculate the sigma points:(29)
χ
k
-
1
=
x
^
k
-
1
x
^
k
-
1
+
γ
S
k
-
1
x
^
k
-
1
-
γ
S
k
-
1
.The time update equations are the following:(30)
χ
^
∗
k
=
f
χ
k
-
1
,
u
k
-
1
,
x
^
-
k
=
∑
i
=
0
2
L
W
i
m
χ
^
i
∗
k
,
S
-
k
=
q
r
W
1
c
χ
^
1
:
2
L
∗
k
-
x
^
-
k
Q
^
k
-
1
,
S
-
k
=
cholupdate
S
-
k
,
χ
^
0
∗
k
-
x
^
-
k
,
W
0
c
.Estimate the square root of the measurement noise matrix:(31)
Z
^
∗
k
-
1
=
h
χ
^
k
-
1
,
z
^
∗
k
-
1
=
∑
i
=
0
2
L
W
i
m
Z
^
i
∗
k
-
1
,
z
~
∗
k
-
1
=
z
k
-
1
-
z
^
∗
k
-
1
,
R
∗
∗
=
cholupdate
1
-
d
k
R
^
k
-
1
,
z
~
∗
k
-
1
,
d
k
,
R
∗
=
cholupdate
R
∗
∗
,
Z
^
0
:
2
L
∗
k
-
1
-
z
^
∗
k
-
1
,
-
d
k
W
i
c
,
R
^
k
=
diag
⁡
diag
⁡
R
∗(augment sigma points),(32)
χ
^
-
k
=
x
^
-
k
x
^
-
k
+
γ
S
-
k
x
^
-
k
-
γ
S
-
k
,
Z
^
-
k
=
h
χ
^
-
k
,
z
^
-
k
=
∑
i
=
0
2
L
W
i
m
Z
^
i
-
k
,
z
~
k
=
z
k
-
z
^
-
k
.The measurement update equations are the following:(33)
P
x
z
k
=
∑
i
=
0
2
L
W
i
c
χ
^
i
-
k
-
x
^
-
k
Z
^
i
-
k
-
z
^
-
k
T
,
(34)
S
z
k
=
q
r
W
1
c
Z
^
1
:
2
L
-
k
-
z
^
-
k
R
^
k
,
S
z
k
=
cholupdate
S
z
k
,
Z
^
0
-
k
-
z
^
-
k
,
W
0
c
,
(35)
K
k
=
P
x
z
k
/
S
z
k
T
S
z
k
,
(36)
x
^
k
=
x
^
-
k
+
K
k
z
~
k
,
(37)
U
=
K
k
S
z
k
,
S
k
=
cholupdate
S
-
k
,
U
,
-
1
.Estimate the square root of the process noise matrix according to(38)
Q
∗
∗
=
cholupdate
Q
^
k
-
1
,
x
^
k
-
x
^
-
k
,
d
k
,
Q
∗
=
cholupdate
Q
∗
∗
,
U
,
-
d
k
,
Q
^
k
=
diag
⁡
diag
⁡
Q
∗
,where d
(
k
)
=
(
1
-
b
)
/
(
1
-
b
k
+
1
) and b is the forgetting factor, typically 0
<
b
<
1. The weights (W
i
(
m
) and W
i
(
c
)) of the mean and covariance are given by(39)
W
0
m
=
λ
L
+
λ
,
W
0
c
=
λ
L
+
λ
+
1
-
α
2
+
β
,
W
i
m
=
W
i
c
=
1
2
L
+
λ
,
i
=
1
,
…
,
2
L
,where λ
=
α
2
(
L
+
κ
)
-
L is a scaling parameter. The constant α determines the spread of the sigma points around the mean, which is typically set to a small, positive value (e.g., 1
0
-
4
≤
α
≤
1). The constant κ
≥
0 is a secondary scaling parameter. β
≥
0 is used to incorporate the prior knowledge of the distribution (for Gaussian distributions, β
=
2 is optimal) [4–6]. In addition, γ
=
L
+
λ.The new ASRUKF makes use of three powerful linear algebra techniques:Q
R decomposition (
q
r
), Cholesky factor updating (
c
h
o
l
u
p
d
a
t
e
), and efficient least squares (
/
), which are briefly reviewed in [6].Equations (31) and (38) are different from the standard SRUKF. These equations enhance the adaptive ability of the filter.(
2
) The process equation is linear, and the measurement equation is nonlinear (PLMN), which is represented by(40)
x
k
=
Φ
k
,
k
-
1
x
k
-
1
+
B
k
,
k
-
1
u
k
-
1
+
w
k
-
1
,
z
k
=
h
x
k
+
v
k
.Compared with PNMN, the new ASRUKF for PLMN can be described by replacing (30) with (41) as follows:(41)
x
^
-
k
=
Φ
k
,
k
-
1
x
^
k
-
1
+
B
k
,
k
-
1
u
k
-
1
,
S
-
k
=
q
r
Φ
k
,
k
-
1
S
k
-
1
Q
^
k
-
1
.(
3
) The process equation is nonlinear, and the measurement equation is linear (PNML), which is represented by(42)
x
k
=
f
x
k
-
1
,
u
k
-
1
+
w
k
-
1
,
z
k
=
H
k
x
k
+
v
k
.Compared with PNMN, the new ASRUKF for PNML can be described using (43) instead of (31)–(34) as follows:(43)
z
~
∗
k
-
1
=
z
k
-
1
-
H
k
-
1
x
^
k
-
1
,
R
∗
∗
=
cholupdate
1
-
d
k
R
^
k
-
1
,
z
~
k
-
1
,
d
k
,
R
∗
=
cholupdate
R
∗
∗
,
H
k
S
k
-
1
,
-
d
k
,
R
^
k
=
diag
⁡
diag
⁡
R
∗
,
z
~
k
=
z
k
-
H
k
x
^
-
k
,
P
x
z
k
=
S
-
k
S
-
k
T
H
k
T
,
S
z
k
=
q
r
H
k
S
-
k
R
^
k
.Application notes are as follows:(a)
IfQ is known, replace Q
^ with Q and only estimate R
^. In contrast, if R is known, replace R
^ with R and only estimate Q
^.
(b)
If bothQ and R are unknown, based on Franz’s scheme [20], estimate their square roots alternately.


## 2.1. Kalman Filter

The dynamical system considered here is a linear, discrete, stochastic sequence described by(1)
x
k
=
Φ
k
,
k
-
1
x
k
-
1
+
B
k
,
k
-
1
u
k
-
1
+
w
k
-
1
,
z
k
=
H
k
x
k
+
v
k
,where x
(
k
)
∈
R
L is the state vector, Φ
k
,
k
-
1
(
k
)
∈
R
L
×
L is the state transition matrix taking state x
(
k
-
1
) from time k
-
1 to time k, u
(
k
)
∈
R
r is a known exogenous input, B
k
,
k
-
1
∈
R
L
×
r is the gain of u
(
k
-
1
), z
(
k
)
∈
R
m is the observation vector, and H
(
k
)
∈
R
m
×
L is the observation mapping matrix. The process noise w
(
k
)
∈
R
L and the measurement noise v
(
k
)
∈
R
m are assumed to be additive, independent, white, and Gaussian with the following properties:(2)
E
w
k
=
0
,
E
w
k
w
j
T
=
Q
k
δ
k
j
,
E
v
k
=
0
,
E
v
k
v
j
T
=
R
k
δ
k
j
,
E
w
k
v
j
T
=
0
.The Kalman filter is represented by the following equations:(3)
x
^
-
k
=
Φ
k
,
k
-
1
x
^
k
-
1
+
B
k
,
k
-
1
u
k
-
1
,
P
-
k
=
Φ
k
,
k
-
1
P
k
-
1
Φ
k
,
k
-
1
T
+
Q
k
-
1
,
K
k
=
P
-
k
H
k
T
H
k
P
-
k
H
k
T
+
R
k
-
1
,
x
^
k
=
x
^
-
k
+
K
k
z
k
-
H
k
x
^
-
k
,
P
k
=
I
-
K
k
H
k
P
-
k
.As shown in (3), if the initial values x
^
(
0
) and P
(
0
) are given, according to z
(
k
) at time k, the state estimation x
^
(
k
)
(
k
=
1,2
,
…
) can be recursively calculated. The Kalman filter algorithm is shown in Figure 1.Figure 1
Kalman filtering process.The algorithm works in a two-step process, with a prediction step and an update step. As prior estimates, the current state variables are used in the update equations. The prior estimates are calibrated via the update equations to obtain the a posteriori estimates. After these two steps, the process repeats. The last a posteriori estimates are calculated as the next a priori estimates. This recursive process is one of the most attractive features of the Kalman filter and makes it easier to implement than other filters.


## 2.2. Extended Kalman Filter

The EKF system is a nonlinear, discrete, stochastic sequence described by(4)
x
k
=
f
x
k
-
1
,
u
k
-
1
+
w
k
-
1
,
z
k
=
h
x
k
+
v
k
,where f
(
x
) and h
(
x
) are nonlinear functions of the system. The other parameters are defined as in (1). The EKF can be expressed as(5)
x
^
-
k
=
f
x
^
k
-
1
,
u
k
-
1
,
P
-
k
=
Φ
k
,
k
-
1
P
k
-
1
Φ
k
,
k
-
1
T
+
Q
k
-
1
,
K
k
=
P
-
k
H
k
T
H
k
P
-
k
H
k
T
+
R
k
-
1
,
x
^
k
=
x
^
-
k
+
K
k
z
k
-
h
x
^
-
k
,
P
k
=
I
-
K
k
H
k
P
-
k
,where(6)
Φ
k
,
k
-
1
=
∂
f
∂
x
x
=
x
^
k
-
1
u
=
u
k
-
1
,
H
k
=
∂
h
∂
x
x
=
x
^
-
k
.The EKF algorithm uses the filter x
^
(
k
) and a Taylor series expansion to linearize the nonlinear functions f
(
x
) and h
(
x
). The higher order terms are omitted, and the linearization model is established. The EKF is a suboptimal filter, which is suitable only when the filtering error x
(
k
)
-
x
^
(
k
) and the prediction error x
(
k
)
-
x
^
-
(
k
) are small.


## 2.3. Unscented Kalman Filter

The UKF is a type of classic Kalman filter. According to the unscented transformation (UT), the UKF chooses sample points that exhibit the same statistical characteristics as the system state. These sample points are nonlinearly transformed through the nonlinear state equations and measurement equations. The mean and covariance can then be calculated using a weighted calculation. Finally, combined with the KF, the estimated system states can be recursively calculated.The UT uses the sample point setS (the so-called sigma points) as the input distribution. This algorithm addresses the entire system as a “black box” and does not depend on the specific nonlinear function or the calculation of the Jacobian matrix.The random vectorx (with dimension L) is nonlinearly transformed according to(7)
y
=
g
x
,where x
- is the mean and P
x is the variance. To obtain the statistical characteristics (mean and covariance) of y, a matrix χ is created. This matrix contains 2
L
+
1 sigma vectors represented by(8)
χ
0
=
x
-
,
χ
i
=
x
-
+
L
+
λ
P
x
i
,
i
=
1
,
…
,
L
,
χ
i
=
x
-
-
L
+
λ
P
x
i
-
L
,
i
=
L
+
1
,
…
,
2
L
,where (
(
L
+
λ
)
P
x
)
i is the ith column of the square root of the matrix (
L
+
λ
)
P
x. All weights that correspond to sigma points are defined according to(9)
W
0
m
=
λ
L
+
λ
,
W
0
c
=
λ
L
+
λ
+
1
-
α
2
+
β
,
W
i
m
=
W
i
c
=
1
2
L
+
λ
,
i
=
1
,
…
,
2
L
,where W
i
(
m
) is the weight of the mean, W
i
(
c
) is the weight of the covariance, and λ
=
α
2
(
L
+
κ
)
-
L is a scaling parameter. The constant α controls the distribution range of the sigma points around the mean x
-. Generally, α is a small, positive number (e.g., 1
0
-
4
≤
α
≤
1). The auxiliary scaling parameter is represented by κ, which is typically set at κ
=
0 to maintain a positive, semidefinite variance matrix. If β
≥
0, the prior distribution of the high-order matrix contains the random vector x.To obtain the mean and covariance of the output vectory, the sigma points (7) are obtained using the following nonlinear transformation:(10)
ζ
i
=
g
χ
i
,
i
=
0
,
…
,
2
L
.The mean and covariance of output vectory can then be obtained as follows:(11)
y
-
≈
∑
i
=
0
2
L
W
i
m
ζ
i
,
P
y
≈
∑
i
=
0
2
L
W
i
c
ζ
i
-
y
-
ζ
i
-
y
-
T
.The main advantage of UKF is that it does not use linearization to calculate the state predictions, covariance matrices. Instead, it uses a deterministic sampling approach to capture the mean and covariance estimates with a minimal set of2
L
+
1 sigma points based on a square-root decomposition of the prior covariance [23].According to the stochastic nonlinear discrete systems shown in (4), the UKF algorithm is the following:(I)
Initializing,k
=
0:(12)
x
^
0
=
E
x
0
,
P
0
=
E
x
0
-
x
^
0
x
0
-
x
^
0
T
.
(II)
Iterating,k
=
1,2
,
…:
(a)
Time update.
First, the sigma points are calculated, and the matrix is constructed:(13)
χ
k
-
1
=
x
^
k
-
1
x
^
k
-
1
+
L
+
λ
P
k
-
1
x
^
k
-
1
-
L
+
λ
P
k
-
1
.
The sigma point set is nonlinearly transformed by the state equations. The state and variance are then predicted:(14)
χ
^
∗
k
=
f
χ
k
-
1
,
u
k
-
1
,
x
^
-
k
=
∑
i
=
0
2
L
W
i
m
χ
^
i
∗
k
,
P
-
k
=
∑
i
=
0
2
L
W
i
c
χ
^
i
∗
k
-
x
^
-
k
χ
^
i
∗
k
-
x
^
-
k
T
+
Q
k
-
1
.
Then, the sigma points are calculated again:(15)
χ
-
k
=
x
^
-
k
x
^
-
k
+
L
+
λ
P
-
k
x
^
-
k
-
L
+
λ
P
-
k
.
The sigma point set is nonlinearly transformed by the measurement equations. The predicted output of system is calculated:(16)
Z
^
-
k
=
h
χ
^
-
k
,
z
^
-
k
=
∑
i
=
0
2
L
W
i
m
Z
^
i
-
k
.
(b)
Measurement update.
Calculate the filter gain:(17)
P
x
z
k
=
∑
i
=
0
2
L
W
i
c
χ
^
i
-
k
-
x
^
-
k
Z
^
i
-
k
-
z
^
-
k
T
,
P
z
k
=
∑
i
=
0
2
L
W
i
c
Z
^
i
-
k
-
z
^
-
k
Z
^
i
-
k
-
z
^
-
k
T
+
R
k
,
K
k
=
P
x
z
k
P
z
-
1
k
.
Estimate the state value after correction:(18)
x
^
k
=
x
^
-
k
+
K
k
z
k
-
z
^
-
k
.
Calculate the a posteriori state variance:(19)
P
k
=
P
-
k
-
K
k
P
z
k
K
k
T
.


## 2.4. AKF

In the AKF, the uncertain parameters that need to be adapted may be part of the system model throughΦ or H or the statistical information through R
/
Q [24]. The AKF based on Maybeck’s estimator is as follows.Initialize with(20)
x
^
0
=
E
x
0
,
P
0
=
E
x
0
-
x
^
0
x
0
-
x
^
0
T
,
Q
^
0
=
Q
_
0
,
R
^
0
=
R
_
0
.Fork
=
1,2
,
…
∞, Maybeck’s estimator is(21)
x
~
k
=
x
^
k
-
x
^
-
k
,
Q
^
k
=
Q
^
k
-
1
+
1
L
Q
x
~
k
x
~
k
T
+
P
k
-
Φ
k
,
k
-
1
P
k
-
1
Φ
k
,
k
-
1
T
-
Q
^
k
-
1
,
z
~
k
=
z
k
-
H
k
x
^
k
,
R
^
k
=
R
^
k
-
1
+
1
L
R
z
~
k
z
~
k
T
-
H
k
P
k
H
k
T
-
R
^
k
-
1
,where L
Q
/
L
R is the window size. Maybeck’s estimator sets the number of updates being averaged together or the window of past measurements that are averaged [20].


## 2.5. The New ASRUKF

The KF, EKF, and UKF assume that the statistical property of system noise is known. However, in the real world, the process noise covariance matrixQ or observation noise variance matrix R is often unknown. In addition, these parameters can vary with time. Hence, to adjust Q and R, an adaptive Kalman filter must be constructed, where it is important to improve the filtering accuracy and stability. Combined with the square-root filter algorithm, the traditional Sage-Husa adaptive filtering algorithm can be improved. The new ASRUKF can estimate the square root of Q or R in a straightforward manner such that positive semidefiniteness is guaranteed, which greatly improves the stability and accuracy of the filter.Based on (1), Sage and Husa proposed an adaptive filtering algorithm based on the maximum a posteriori estimation [25]. Based on this algorithm, the forgetting factor (sometimes called the “fading factor” [26, 27]) was introduced by Deng and Guo [28]. Subsequently, the algorithm has the ability to estimate the unknown time-varying noise. The specific calculation process is as follows:(I)
Initializing,k
=
0:(22)
x
^
0
=
E
x
0
,
P
0
=
E
x
0
-
x
^
0
x
0
-
x
^
0
T
,
Q
^
0
=
Q
_
0
,
R
^
0
=
R
_
0
.
(II)
Iterating,k
=
1,2
,
…:
(a)
Time update:(23)
x
^
-
k
=
Φ
k
,
k
-
1
x
^
k
-
1
+
B
k
,
k
-
1
u
k
-
1
,
P
-
k
=
Φ
k
,
k
-
1
P
k
-
1
Φ
k
,
k
-
1
T
+
Q
^
k
-
1
.
(b)
Measurement update.
Estimate the statistical properties of measurement noise:(24)
z
~
=
z
k
-
H
k
x
^
-
k
,
R
^
k
=
1
-
d
k
R
^
k
-
1
+
d
k
z
~
k
z
~
k
T
-
H
k
P
-
k
H
k
T
.
Estimate the state value. After correction, calculate the a posteriori state variance:(25)
K
k
=
P
-
k
H
k
T
H
k
P
-
k
H
k
T
+
R
^
k
-
1
,
x
^
k
=
x
^
-
k
+
K
k
z
~
,
P
k
=
I
-
K
k
H
k
P
-
k
.
Estimate the statistical properties of the process noise according to(26)
Q
^
k
=
1
-
d
k
Q
^
k
-
1
+
d
k
·
K
k
z
~
k
z
~
k
T
K
k
T
+
P
k
-
Φ
k
,
k
-
1
P
k
-
1
Φ
k
,
k
-
1
T
,
whered
(
k
)
=
(
1
-
b
)
/
(
1
-
b
k
+
1
), b is the forgetting factor, and 0
<
b
<
1.Based on the SRUKF, traditional Maybeck’s estimator can be modified and extended to nonlinear systems. Thus, a new adaptive filtering approach for nonlinear systems with additive noise is developed. According to the three different structures of the nonlinear problem, the three new ASRUKF models are the following.(
1
) Both the process and the measurement equations are nonlinear (PNMN) according to(27)
x
k
=
f
x
k
-
1
,
u
k
-
1
+
w
k
-
1
,
z
k
=
h
x
k
+
v
k
,where f
(
·
) and h
(
·
) are nonlinear functions. The new ASRUKF for PNMN is the following.Initialize with(28)
x
^
0
=
E
x
0
,
S
0
=
chol
E
x
0
-
x
^
0
x
0
-
x
^
0
T
,
Q
^
0
=
S
0
,
R
^
0
=
chol
E
z
0
-
z
^
0
z
0
-
z
^
0
T
.Fork
=
1
,
2
,
…
∞, calculate the sigma points:(29)
χ
k
-
1
=
x
^
k
-
1
x
^
k
-
1
+
γ
S
k
-
1
x
^
k
-
1
-
γ
S
k
-
1
.The time update equations are the following:(30)
χ
^
∗
k
=
f
χ
k
-
1
,
u
k
-
1
,
x
^
-
k
=
∑
i
=
0
2
L
W
i
m
χ
^
i
∗
k
,
S
-
k
=
q
r
W
1
c
χ
^
1
:
2
L
∗
k
-
x
^
-
k
Q
^
k
-
1
,
S
-
k
=
cholupdate
S
-
k
,
χ
^
0
∗
k
-
x
^
-
k
,
W
0
c
.Estimate the square root of the measurement noise matrix:(31)
Z
^
∗
k
-
1
=
h
χ
^
k
-
1
,
z
^
∗
k
-
1
=
∑
i
=
0
2
L
W
i
m
Z
^
i
∗
k
-
1
,
z
~
∗
k
-
1
=
z
k
-
1
-
z
^
∗
k
-
1
,
R
∗
∗
=
cholupdate
1
-
d
k
R
^
k
-
1
,
z
~
∗
k
-
1
,
d
k
,
R
∗
=
cholupdate
R
∗
∗
,
Z
^
0
:
2
L
∗
k
-
1
-
z
^
∗
k
-
1
,
-
d
k
W
i
c
,
R
^
k
=
diag
⁡
diag
⁡
R
∗(augment sigma points),(32)
χ
^
-
k
=
x
^
-
k
x
^
-
k
+
γ
S
-
k
x
^
-
k
-
γ
S
-
k
,
Z
^
-
k
=
h
χ
^
-
k
,
z
^
-
k
=
∑
i
=
0
2
L
W
i
m
Z
^
i
-
k
,
z
~
k
=
z
k
-
z
^
-
k
.The measurement update equations are the following:(33)
P
x
z
k
=
∑
i
=
0
2
L
W
i
c
χ
^
i
-
k
-
x
^
-
k
Z
^
i
-
k
-
z
^
-
k
T
,
(34)
S
z
k
=
q
r
W
1
c
Z
^
1
:
2
L
-
k
-
z
^
-
k
R
^
k
,
S
z
k
=
cholupdate
S
z
k
,
Z
^
0
-
k
-
z
^
-
k
,
W
0
c
,
(35)
K
k
=
P
x
z
k
/
S
z
k
T
S
z
k
,
(36)
x
^
k
=
x
^
-
k
+
K
k
z
~
k
,
(37)
U
=
K
k
S
z
k
,
S
k
=
cholupdate
S
-
k
,
U
,
-
1
.Estimate the square root of the process noise matrix according to(38)
Q
∗
∗
=
cholupdate
Q
^
k
-
1
,
x
^
k
-
x
^
-
k
,
d
k
,
Q
∗
=
cholupdate
Q
∗
∗
,
U
,
-
d
k
,
Q
^
k
=
diag
⁡
diag
⁡
Q
∗
,where d
(
k
)
=
(
1
-
b
)
/
(
1
-
b
k
+
1
) and b is the forgetting factor, typically 0
<
b
<
1. The weights (W
i
(
m
) and W
i
(
c
)) of the mean and covariance are given by(39)
W
0
m
=
λ
L
+
λ
,
W
0
c
=
λ
L
+
λ
+
1
-
α
2
+
β
,
W
i
m
=
W
i
c
=
1
2
L
+
λ
,
i
=
1
,
…
,
2
L
,where λ
=
α
2
(
L
+
κ
)
-
L is a scaling parameter. The constant α determines the spread of the sigma points around the mean, which is typically set to a small, positive value (e.g., 1
0
-
4
≤
α
≤
1). The constant κ
≥
0 is a secondary scaling parameter. β
≥
0 is used to incorporate the prior knowledge of the distribution (for Gaussian distributions, β
=
2 is optimal) [4–6]. In addition, γ
=
L
+
λ.The new ASRUKF makes use of three powerful linear algebra techniques:Q
R decomposition (
q
r
), Cholesky factor updating (
c
h
o
l
u
p
d
a
t
e
), and efficient least squares (
/
), which are briefly reviewed in [6].Equations (31) and (38) are different from the standard SRUKF. These equations enhance the adaptive ability of the filter.(
2
) The process equation is linear, and the measurement equation is nonlinear (PLMN), which is represented by(40)
x
k
=
Φ
k
,
k
-
1
x
k
-
1
+
B
k
,
k
-
1
u
k
-
1
+
w
k
-
1
,
z
k
=
h
x
k
+
v
k
.Compared with PNMN, the new ASRUKF for PLMN can be described by replacing (30) with (41) as follows:(41)
x
^
-
k
=
Φ
k
,
k
-
1
x
^
k
-
1
+
B
k
,
k
-
1
u
k
-
1
,
S
-
k
=
q
r
Φ
k
,
k
-
1
S
k
-
1
Q
^
k
-
1
.(
3
) The process equation is nonlinear, and the measurement equation is linear (PNML), which is represented by(42)
x
k
=
f
x
k
-
1
,
u
k
-
1
+
w
k
-
1
,
z
k
=
H
k
x
k
+
v
k
.Compared with PNMN, the new ASRUKF for PNML can be described using (43) instead of (31)–(34) as follows:(43)
z
~
∗
k
-
1
=
z
k
-
1
-
H
k
-
1
x
^
k
-
1
,
R
∗
∗
=
cholupdate
1
-
d
k
R
^
k
-
1
,
z
~
k
-
1
,
d
k
,
R
∗
=
cholupdate
R
∗
∗
,
H
k
S
k
-
1
,
-
d
k
,
R
^
k
=
diag
⁡
diag
⁡
R
∗
,
z
~
k
=
z
k
-
H
k
x
^
-
k
,
P
x
z
k
=
S
-
k
S
-
k
T
H
k
T
,
S
z
k
=
q
r
H
k
S
-
k
R
^
k
.Application notes are as follows:(a)
IfQ is known, replace Q
^ with Q and only estimate R
^. In contrast, if R is known, replace R
^ with R and only estimate Q
^.
(b)
If bothQ and R are unknown, based on Franz’s scheme [20], estimate their square roots alternately.


## 3. Simulation and Verification

To illustrate the advantages of the new ASRUKF, an example is presented, in which the univariate nonstationary growth model (UNGM) is estimated. This model was previously used as a benchmark [29]. In this case, the new ASRUKF resulted in better performance than that of the standard SRUKF. All codes were developed in MATLAB.The dynamic state-space model for UNGM can be written as(44)
x
k
=
α
x
k
-
1
+
β
x
k
-
1
1
+
x
2
k
-
1
+
γ
cos
⁡
1.2
k
-
1
+
w
u
k
,
z
k
=
x
2
k
20
+
v
u
k
,
k
=
1,2
,
…
,
N
,where w
u
(
k
)
~
N
(
0
,
σ
w
2
) and v
u
(
k
)
~
N
(
0
,
σ
v
2
). In this example, the parameters were set as follows: σ
w
=
0.3, σ
v
=
1, x
(
0
)
=
0.1, α
=
0.5, β
=
5, γ
=
8, and N
=
200:(1)
σ
v
=
1; estimate σ
w; initialize with σ
^
w
(
0
)
=
3;
(2)
σ
w
=
0.3; estimate σ
v; initialize with σ
^
v
(
0
)
=
0.1.The simulation results (Figures 2 to 5) show that if either σ
w or σ
v is known, the new ASRUKF can rapidly estimate the other one, which improves the filter performance.Figure 2
State tracking of SRUKF and ASRUKF (σ
v is known; σ
w is unknown).As shown in Figure2, the SRUKF estimated error and the new ASRUKF estimated error are almost the same during the primary stage. After the 15th step in the simulation, the state estimation for the ASRUKF can follow the true value accurately. The estimation error is not greater than 1. At the same stage, the state estimation for the SRUKF cannot follow the true value, where the estimation error can be larger than 7. Thus, the ASRUKF has a greater adaptive ability compared with that of the SRUKF.As shown in Figure3, at the 15th step in the simulation, the ASRUKF can identify σ
w, which is the standard deviation of the process noise. After the 30th step, σ
w of the ASRUKF is almost the same as the theoretical value. In contrast, the SRUKF cannot estimate the statistical characteristics of the unknown noise. Therefore, compared with the SRUKF, the ASRUKF has a better filtering ability.Figure 3
Theσ
w estimation with the ASRUKF (σ
v is known).Setting the initial value toσ
^
v
(
0
)
=
0.1, the SRUKF and ASRUKF are used to estimate state x. Then, 100 iterations were performed in MATLAB. The simulation results are shown in Figures 4 and 5.Figure 4
State tracking of the SRUKF and ASRUKF (σ
w is known; σ
v is unknown).Figure 5
Theσ
v estimation with the ASRUKF (σ
w is known).In Figure4, the UNGM is a highly nonlinear model, and, thus, the filtering performance of the ASRUKF is better than that of the SRUKF. After the 10th step in the simulation, the state estimation error of the ASRUKF is less than 1, whereas the error of the SRUKF is greater than 2.5.In Figure5, when the statistical characteristics of the system measurement noise are unknown, the SRUKF uses the initial assumption, which is one-tenth of the theoretical true value, to recursively estimate the system state. In contrast, at approximately the 15th step in the simulation, the ASRUKF can identify σ
v, which is the standard deviation of the measurement noise. Therefore, the accuracy of the ASRUKF is greater than that of the SRUKF.


## 4. Conclusions

In this paper, based on the standard SRUKF, the traditional Maybeck estimator is modified, creating a new adaptive filtering approach (i.e., the adaptive square-root Kalman filter). With the new filter, the positive semidefiniteness ofQ orR is guaranteed. Moreover, several of the shortcomings of the traditional Maybeck algorithm are overcome such that the stability and accuracy of the filter are greatly improved. In addition, based on three different nonlinear systems, three new adaptive filtering models were described in detail. Compared with conventional filtering algorithms, the algorithm proposed in this study effectively improves the self-adaptability of filters.This paper only discussed the adaptive filtering approaches for nonlinear system with additive noise. Further studies will address nonadditive noise.The developed filter in this study can estimate the square root ofQ or R in a straightforward manner such that positive semidefiniteness is guaranteed, which greatly improves the stability and accuracy of the filter. From this study, a valid mathematical tool is provided for the purpose of multisensory data fusion with uncertain process noise, where this tool could greatly help navigation techniques for autonomous mobile robots and intelligent vehicles.



---

*Source: 381478-2015-07-06.xml*