# Vehicle Sideslip Angle Estimation Based on Hybrid Kalman Filter

Authors: Jing Li; Jiaxu Zhang
Journal: Mathematical Problems in Engineering
 (2016)

---

## Abstract

Vehicle sideslip angle is essential for active safety control systems. This paper presents a new hybrid Kalman filter to estimate vehicle sideslip angle based on the 3-DoF nonlinear vehicle dynamic model combined with Magic Formula tire model. The hybrid Kalman filter is realized by combining square-root cubature Kalman filter (SCKF), which has quick convergence and numerical stability, with square-root cubature based receding horizon Kalman FIR filter (SCRHKF), which has robustness against model uncertainty and temporary noise. Moreover, SCKF and SCRHKF work in parallel, and the estimation outputs of two filters are merged by interacting multiple model (IMM) approach. Experimental results show the accuracy and robustness of the hybrid Kalman filter.


---

## Body

## 1. Introduction

Active safety control systems can improve the handling and stability of vehicle effectively and then reduce the probability of traffic accidents. For instance, March and Shim [1] developed an integrated control system of an active suspension system (ASS) and active front steering (AFS) to enhance vehicle handling and stability in which the active suspension system was mainly used to control the normal force of the vehicle. Mashadi and Majidi [2] proposed a sliding-mode controller that cooperates with active front steering and direct yaw moment control (DYC) to prevent vehicle from spinning and drifting out of lane. However, on the premise that we can access the vehicle states information accurately, which include the longitudinal acceleration, the lateral acceleration, the vehicle sideslip angle, the yaw rate, and other states information, then the control logic of these vehicle active safety control systems can work effectively. Some of the required vehicle states information is easy to be measured by the sensors which are equipped on the mass production vehicle, but others are difficult to detect for both technical and economical reasons, such as the sideslip angle. As a consequence, estimation of the sideslip angle based on the measured vehicle states information has important theoretical and practical significance.Estimation of vehicle sideslip angle, or equivalently the lateral velocity, has been well investigated in the literature, and the estimation approach can be classified into two categories: kinematics-based approach designed according to kinematics motion and model-based approach designed according to the physical vehicle model. Tseng et al. [3] adopted integration method based on kinematical relationship of lateral velocity rate, longitudinal velocity, yaw rate, and lateral acceleration to estimate the lateral velocity. Farrelly and Wellstead [4] proposed a kinematics-based approach that integrated lateral and longitudinal kinematics motion with the pole placement method to estimate the lateral velocity. The kinematic model is unobservable when the yaw rate is zero, and so the resulting observer only functions when the yaw rate is nonzero. Ungoren et al. [5] combined the approach proposed by [4] with a model-based approach to avoid unobservability during near-zero yaw rate conditions. The kinematics-based approach is considered reliable for a transient manoeuvre, but its integration operation can inevitably cause progressive drifting issues resulting from sensor errors.Compared with kinematics-based approach, the model-based approach has the merit of being independent of sensor errors. Pi et al. [6] proposed a model-based approach based on a simplified single-track model with a linear adaptive tire-force model to estimate the sideslip angle. The vehicle lateral force is calculated by the linear adaptive tire-force model and fed into the extended Kalman filter (EKF), which provides the final estimation of the sideslip angle. Antonov et al. [7] established a planar two-track model with the empiric Magic Formula as a basis for the estimator design and utilized unscented Kalman filter (UKF) to estimate the longitudinal velocity and lateral velocity. Baffet et al. [8] presented a nonlinear cascaded observer method for vehicle sideslip angle and tire-road forces. The first block contains a sliding-mode observer whose principal role is to estimate tire-road forces, and then the estimated tire-road forces are used as input to the second block, in which the EKF estimates the sideslip angle. Ren et al. [9] proposed an UKF based on a 3-DoF vehicle model with the piecewise linear tire model to estimate the instantaneous vehicle speed. Shraim et al. [10] adopted a high order sliding-mode observer based on the dynamic model of the wheel for the estimation of the longitudinal forces, and then a sliding-mode observer based on the supertwisting algorithm for the estimation of sideslip angle and vehicle speed was proposed. Gadola et al. [11] proposed an EKF-based sideslip angle estimation using a simple single-track model and a set of curves which described the relationship of the lateral forces, the tire slip angle, and the vertical load. Xin et al. [12] proposed a cubature Kalman filter (CKF) based on 3-DoF vehicle model to estimate vehicle velocity, sideslip angle, and yaw rate. The model-based approach strongly depends on the accuracy of the physical vehicle model while the practical vehicle is a nonlinear time-varying high order dynamic system. Therefore, it is necessary to design the sideslip angle estimator which has robustness against model uncertainty and temporary disturbing noise.Square-root cubature Kalman filter (SCKF) presented by Arasaratnam and Haykin [13] is a nonlinear filter for high-dimensional state estimation. SCKF introduces a third-degree spherical-radial cubature rule to numerically approximate the multidimensional integrals encountered in the nonlinear Bayesian filter and has proper approximate accuracy and high convergence speed. However, the estimation accuracy and stability of SCKF may be reduced due to model uncertainty and temporary noise [14–16]. Square-root cubature based receding horizon Kalman filter (SCRHKF) has robustness against model uncertainty and temporary noise [17], while the convergence speed of this filter is worse than SCKF due to the use of a finite number of measurements. Namely, the two filters have complementary features with each other. This paper proposes the hybrid Kalman filter, which integrates the advantage of SCKF and SCRHKF and overcomes the disadvantage of the filters, to estimate the vehicle sideslip angle. In the hybrid Kalman filter, SCKF and SCRHKF work in parallel, and the estimation outputs of two filters are merged by interacting multiple model (IMM) approach. This approach can always make the estimation output of the hybrid Kalman filter smoothly track the estimation output of the filter with the smallest estimation error. Then, in the real vehicle experiment environment, the performance of the proposed method is verified under double lane change and slalom conditions.This paper is organized as follows. Section2 provides the 3-DoF vehicle dynamic model. Section 3 shows the proposed hybrid Kalman filter. Section 4 introduces our experiments and results. Finally, Section 5 draws the main conclusion of our work.


## 2. The Dynamic Model

The proposed method is based on a nonlinear 3-DoF vehicle body model with a precise nonlinear tire model “Magic Formula,” which consists of the longitudinal motion, lateral motion, and yaw motion. In order to reduce the size of the state vector, the following modeling assumptions are made:(i)
The vehicle body is modeled as being rigid and lumped at the mass center of gravity.
(ii)
The vehicle is moving on a flat horizontal plane.
(iii)
The heave, pitch, and roll motion are ignored.
(iv)
The aerodynamic drag forces are ignored.
(v)
The steer angles of the front left and front right wheels are equal.As shown in Figure1, O
x
y
z is the fixed coordinate on the ground, and B
x
y
z is the fixed coordinate on the vehicle, with x-axis in the vehicle longitudinal direction, y-axis in the lateral direction, z-axis in the vertical direction, and the origin at the vehicle center of gravity (CoG). The yaw angle around the z-axis is taken as positive in the anticlockwise direction. The differential equations of the longitudinal, lateral, and yaw motions are expressed as(1)
v
˙
x
=
a
x
+
r
v
y
,
a
x
=
1
m
F
x
f
l
+
F
x
f
r
cos
⁡
δ
f
-
F
y
f
l
+
F
y
f
r
sin
⁡
δ
f
+
F
x
r
l
+
F
x
r
r
,
v
˙
y
=
a
y
-
r
v
x
,
a
y
=
1
m
F
x
f
l
+
F
x
f
r
sin
⁡
δ
f
+
F
y
f
l
+
F
y
f
r
cos
⁡
δ
f
+
F
y
r
l
+
F
y
r
r
,
r
˙
=
1
I
z
a
F
x
f
l
+
F
x
f
r
sin
⁡
δ
f
+
a
F
y
f
l
+
F
y
f
r
cos
⁡
δ
f
-
b
F
y
r
l
+
F
y
r
r
-
c
2
F
x
f
l
-
F
x
f
r
cos
⁡
δ
f
+
c
2
F
y
f
l
-
F
y
f
r
sin
⁡
δ
f
-
c
2
F
x
r
l
-
F
x
r
r
,
β
=
a
r
c
t
a
n
v
y
v
x
,where v
x and v
y are the longitudinal and the lateral velocity at the CoG; a
x and a
y are the longitudinal and the lateral acceleration at the CoG; r is the yaw rate; m is the vehicle mass; δ
f is the steer angle of front wheels; I
z is the vehicle moment of inertia about z-axis; a is the distance from the front axle to CoG; b is the distance from the rear axle to CoG; c is the track width; β is the vehicle sideslip angle; and F
x
i
j and F
y
i
j are the longitudinal and lateral tire force, where i
j
∈
{
f
l
,
f
r
,
r
l
,
r
r
}. Meanwhile, the first subscript of ·
i
j denotes the front and rear axles, and the second denotes the left and right sides of the vehicle.Figure 1
Schematic diagram of 3-DOF vehicle dynamics model.The tire forces of each wheel are described by a nonlinear tire model “Magic Formula.” For the sake of simplicity, we neglect the aligning torque and the minor effects due to wheel camber. The tire model expresses the longitudinal forceF
x and the lateral force F
y as a function of the wheel longitudinal slip λ and the wheel sideslip angle α, respectively. The general form of the tire model is given by [18, 19] (2)
Y
ε
=
D
sin
⁡
C
arctan
⁡
B
ε
-
E
B
ε
-
a
r
c
t
a
n
B
ε
,where the input ε represents λ or α, which can be expressed as a function of δ
f,  v
x,  v
y,  r,  a
x, and a
y; the output variable Y
(
ε
) stands for either F
x or F
y; the coefficients B,  C,  D, and E are expressed as a function of the dynamic normal force F
z.Considering pitch and roll load transfer, the dynamic normal forces of each wheel are calculated by(3)
F
z
f
l
=
m
g
b
2
l
-
m
a
x
h
2
l
-
m
a
y
h
b
c
l
,
F
z
f
r
=
m
g
b
2
l
-
m
a
x
h
2
l
+
m
a
y
h
b
c
l
,
F
z
r
l
=
m
g
a
2
l
+
m
a
x
h
2
l
-
m
a
y
h
a
c
l
,
F
z
r
r
=
m
g
a
2
l
+
m
a
x
h
2
l
+
m
a
y
h
a
c
l
,where h is the CoG height and l is the wheel base.The sideslip angle of each wheel can be calculated by(4)
α
f
l
=
a
r
c
t
a
n
v
y
+
a
r
v
x
-
r
c
/
2
-
δ
f
,
α
f
r
=
a
r
c
t
a
n
v
y
+
a
r
v
x
+
r
c
/
2
-
δ
f
,
α
r
l
=
a
r
c
t
a
n
v
y
-
b
r
v
x
-
r
c
/
2
,
α
r
r
=
a
r
c
t
a
n
v
y
-
b
r
v
x
+
r
c
/
2
.The longitudinal speed of the wheel center can be calculated by(5)
v
x
f
l
=
v
x
-
r
c
2
cos
⁡
δ
f
+
v
y
+
r
a
sin
⁡
δ
f
,
v
x
f
r
=
v
x
+
r
c
2
cos
⁡
δ
f
+
v
y
+
r
a
sin
⁡
δ
f
,
v
x
r
l
=
v
x
-
r
c
2
,
v
x
r
r
=
v
x
+
r
c
2
.The wheel longitudinal slip is defined as(6)
λ
i
j
=
w
i
j
R
w
-
v
x
i
j
m
a
x
w
i
j
R
w
,
v
x
i
j
,where w
i
j is the angular velocity and i
j
∈
{
f
l
,
f
r
,
r
l
,
r
r
}; R
w is the effective rolling radius.In summary, we can calculate the input of the tire model that the wheel longitudinal slipλ, the wheel sideslip angle α, and the dynamic normal force F
z based on the states δ
f,  v
x,  v
y,  r,  a
x, and a
y are described by the nonlinear 3-DoF vehicle body model, and then the longitudinal force F
x and the lateral force F
y, which can be used as the input of the nonlinear 3-DoF vehicle body model, are calculated by the tire model. Hence, the tire model and the nonlinear 3-DoF vehicle body model are merged together into a complete vehicle model, which in the continuous state-space form reads as(7)
x
˙
t
=
f
~
x
t
,
u
t
,
z
t
=
h
x
t
,
u
twith the input, state, and measurement vectors given by(8)
u
t
=
δ
f
t
w
f
l
t
w
f
r
t
w
r
l
t
w
r
r
t
T
,
x
t
=
v
x
t
v
y
t
r
t
T
,
z
t
=
a
x
t
a
y
t
r
t
T
.To get the discrete form representation of the system, the zero-order-hold assumption for the system input vectoru
(
t
) during the sampling time Δ
T and the classical forward Euler integration are used. Meanwhile, introducing the stochastic process noise w
k and the stochastic measurement noise v
k, we obtain the discrete form of the system for the filter design as follows:(9)
x
k
+
1
=
x
k
+
f
~
x
k
,
u
k
Δ
T
+
w
k
=
f
x
k
,
u
k
+
w
k
,
z
k
=
h
x
k
,
u
k
+
v
k
,where w
k and v
k are uncorrelated zero-mean white Gaussian noise processes and the covariance of the two processes is denoted by Q and R, respectively.


## 3. The Hybrid Kalman Filter

The hybrid Kalman filter is shown in Figure2. It consists of interaction/mixing, SCKF, SCRHKF, model probability update, and estimation fusion. In the hybrid Kalman filter, the SCKF and SCRHKF are processed to estimate the vehicle sideslip angle based on the nonlinear 3-DoF vehicle model, respectively, and the state estimate and its covariance of the two filters are combined by interacting multiple model (IMM) approach. Assuming m
1 denotes the model-based SCKF, m
2 denotes the model-based SCRHKF, and M
=
{
m
1
,
m
2
} denotes the model set consisting of m
1 and m
2.Figure 2
The architecture of the hybrid Kalman filter, whereu
k
-
1
i
∣
j  (
i
∣
j
=
1
∣
2,2
∣
1
) is the model mixing probability;  x
^
k
-
1
∣
k
-
1
i  (
i
=
1,2
) and P
k
-
1
∣
k
-
1
i  (
i
=
1,2
) are the estimation states and the covariance matrices for m
i  (
i
=
1,2
) at time k
-
1; u
k
i  (
i
=
1,2
) is for m
i  (
i
=
1,2
) at time k; Λ
k
i  (
i
=
1,2
) is the likelihood function for m
i  (
i
=
1,2
) at time k; x
^
k
∣
k
i  (
i
=
1,2
) and P
k
∣
k
i  (
i
=
1,2
) are the estimation states and the covariance matrices for m
i  (
i
=
1,2
) at time k;  z
k is the measurement at time k;  x
^
k
∣
k and p
k
∣
k are the combined state estimate and its covariance.


### 3.1. Square-Root Cubature Kalman Filter

SCKF is a square-root extension of the standard CKF and uses the least-squares method for the Kalman gain and triangularization for covariance updates to improve the numerical stability. SCKF introduces a third-degree spherical-radial cubature rule to calculate the cubature pointξ
i and the corresponding weight ω
i: (10)
ξ
i
=
m
2
1
i
,
ω
i
=
1
m
,
i
=
1,2
,
…
,
m
,where m
=
2
n and n is the system state dimension; [
1
] is the set of points composed of the full array of the unit vector e
=
[
1,0
,
…
,
0
]
T’s elements, and [
1
]
i is the ith point from set [
1
].Assume at timek
-
1 that the Cholesky decomposition of the covariance P
k
-
1
∣
k
-
1 is known; that is, P
k
-
1
∣
k
-
1
=
S
k
-
1
∣
k
-
1
S
k
-
1
∣
k
-
1
T; the steps of SCKF are presented as follows [13].(
1) Time Update
Evaluate the cubature point:(11)
X
i
,
k
-
1
∣
k
-
1
=
S
k
-
1
∣
k
-
1
ξ
i
+
x
^
k
-
1
∣
k
-
1
.
Evaluate the propagated cubature points through the state function:(12)
X
i
,
k
∣
k
-
1
∗
=
f
X
i
,
k
-
1
∣
k
-
1
,
u
k
-
1
.
Estimate the predicted state:(13)
x
^
k
∣
k
-
1
=
1
m
∑
i
=
1
m
X
i
,
k
∣
k
-
1
∗
.
Estimate the square-root factor of the predicted error covariance:(14)
S
k
∣
k
-
1
=
T
r
i
a
χ
k
∣
k
-
1
∗
S
Q
,
k
-
1
,
whereT
r
i
a
(
·
) denotes a general triangularization algorithm (e.g., the QR decomposition) and is a lower triangular matrix; S
Q
,
k
-
1 denotes a square-root factor of Q
k
-
1 such that Q
k
-
1
=
S
Q
,
k
-
1
S
Q
,
k
-
1
T and the weighted, centered matrix (15)
χ
k
∣
k
-
1
∗
=
1
m
X
1
,
k
∣
k
-
1
∗
-
x
^
k
∣
k
-
1
X
2
,
k
∣
k
-
1
∗
-
x
^
k
∣
k
-
1
⋯
X
m
,
k
∣
k
-
1
∗
-
x
^
k
∣
k
-
1
.(
2) Measurement Update
Evaluate the cubature points:(16)
X
i
,
k
∣
k
-
1
=
S
k
∣
k
-
1
ξ
i
+
x
^
k
∣
k
-
1
.
Evaluate the propagated cubature points through the measurement function:(17)
Z
i
,
k
∣
k
-
1
=
h
X
i
,
k
∣
k
-
1
,
u
k
.
Estimate the predicted measurement:(18)
z
^
k
∣
k
-
1
=
1
m
∑
i
=
1
m
Z
i
,
k
∣
k
-
1
.
Estimate the square-root of the innovation covariance matrix:(19)
S
z
z
,
k
∣
k
-
1
=
T
r
i
a
Z
k
∣
k
-
1
S
R
,
k
,
whereS
R
,
k denotes a square-root factor of R
k such that R
k
=
S
R
,
k
S
R
,
k
T and the weighted, centered matrix (20)
Z
k
∣
k
-
1
=
1
m
Z
1
,
k
∣
k
-
1
-
z
^
k
∣
k
-
1
Z
2
,
k
∣
k
-
1
-
z
^
k
∣
k
-
1
⋯
Z
m
,
k
∣
k
-
1
-
z
^
k
∣
k
-
1
.
Estimate the cross-covariance matrix:(21)
P
x
z
,
k
∣
k
-
1
=
χ
k
∣
k
-
1
Z
k
∣
k
-
1
T
,
where the weighted, centered matrix(22)
χ
k
∣
k
-
1
=
1
m
X
1
,
k
∣
k
-
1
-
x
^
k
∣
k
-
1
X
2
,
k
∣
k
-
1
-
x
^
k
∣
k
-
1
⋯
X
m
,
k
∣
k
-
1
-
x
^
k
∣
k
-
1
.
Estimate the Kalman gain:(23)
W
k
=
P
x
z
,
k
∣
k
-
1
/
S
z
z
,
k
∣
k
-
1
T
S
z
z
,
k
∣
k
-
1
.
Estimate the updated state:(24)
x
^
k
∣
k
=
x
^
k
∣
k
-
1
+
W
k
z
k
-
z
^
k
∣
k
-
1
.
Estimate the square-root factor of the corresponding error covariance:(25)
S
k
∣
k
=
T
r
i
a
χ
k
∣
k
-
1
-
W
k
Z
k
∣
k
-
1
W
k
S
R
,
k
.


### 3.2. Square-Root Cubature Based Receding Horizon Kalman Filter

SCRHKF has an FIR structure, and the current estimation states of SCRHKF are based on a finite number of measurements over the recent time horizon [20]. Figure 3 shows the concept of the SCRHKF, which consists of the hidden horizon and the active horizon. The sizes of the receding horizon and receding interval are set up as N which is the system dimension. The RHKF is processed in the hidden horizon. The states and the inverse covariance matrix at time t
N
(
i
+
1
), which are used as the initial values of the active horizon from t
N
(
i
+
1
) to t
N
(
i
+
2
), can be estimated based on the final estimation states at time t
N
i and the measurements from t
N
i to t
N
(
i
+
1
) in this horizon. After processing the hidden horizon, the active horizon and another hidden horizon are carried out. In the active horizon, the SCKF is processed to calculate the final estimation states from t
N
(
i
+
1
) to t
N
(
i
+
2
) based on the outputs of the hidden horizon at time t
N
(
i
+
1
). The steps of SCRHKF are presented as follows.Figure 3
Concept of SCRHKF.(1) Initialization of Hidden Horizon
Initialize the states, the inverse covariance, and the pseudo error state:(26)
x
N
i
∗
=
x
^
N
i
,
Ω
N
i
=
0
n
×
n
,
ξ
^
N
i
=
0
n
×
1
.(2) Time Update of Hidden Horizon
Estimate the predicted inverse covariance:(27)
Ω
j
-
=
I
-
F
j
-
1
-
T
Ω
j
-
1
F
j
-
1
-
1
Q
-
1
+
F
j
-
1
-
T
Ω
j
-
1
F
j
-
1
-
1
-
1
F
j
-
1
-
T
Ω
j
-
1
F
j
-
1
-
1
,
whereF
j
=
(
∂
f
/
∂
x
)
x
=
x
j
∗ and j
=
N
i
+
η  (
η
∈
R
,
1
≤
η
≤
N
).
Estimate the predicted pseudo error state:(28)
ξ
^
j
-
=
I
-
F
j
-
1
-
T
Ω
j
-
1
F
j
-
1
-
1
Q
-
1
+
F
j
-
1
-
T
Ω
j
-
1
F
j
-
1
-
1
-
1
F
j
-
1
-
T
ξ
^
j
-
1
.(3) Measurement Update of Hidden Horizon
Estimate the update inverse covariance:(29)
Ω
j
=
Ω
j
-
+
H
j
T
R
-
1
H
j
,
whereH
j
=
(
∂
h
/
∂
x
)
x
=
x
j
∗.
Estimate the update pseudo error state:(30)
ξ
^
j
=
ξ
^
j
-
+
H
j
T
R
-
1
z
j
-
h
x
j
∗
.(4) Initialization of Active Horizon
Initialize the states and the covariance:(31)
P
N
i
+
1
=
Ω
N
i
+
1
-
1
,
x
N
i
+
1
∗
=
x
N
i
+
1
∗
+
Ω
N
i
+
1
-
1
ξ
^
N
i
+
1
.(5) Estimation State of Active Horizon
Estimate the final estimation states by formulas (11)~(25).


### 3.3. Interacting Multiple Model Approach

The IMM approach carries out a “soft switching” between the model-based SCKF and the model-based SCRHKF by the model probability. The steps of IMM are presented as follows [21].(
1) Model Mixing Probability Calculation
Calculate the model mixing probability:(32)
u
k
-
1
i
/
j
=
p
i
j
u
k
-
1
i
μ
k
∣
k
-
1
j
,
whereμ
k
-
1
i
=
p
{
m
k
-
1
i
∣
y
k
-
1
} is the model probability at time k
-
1; p
i
j
=
p
{
m
k
j
∣
m
k
-
1
i
} is the Markov transition probability from m
i to m
j;  μ
k
∣
k
-
1
j is the predicted probability; and μ
k
∣
k
-
1
j
=
∑
i
=
1
2
p
i
j
μ
k
-
1
i.(
2) Interaction/Mixing
Consider the following:(33)
x
^
k
-
1
∣
k
-
1
0
j
=
∑
i
=
1
2
u
k
-
1
i
/
j
x
^
k
-
1
∣
k
-
1
i
,
P
k
-
1
∣
k
-
1
0
j
=
∑
i
=
0
2
u
k
-
1
i
/
j
P
k
-
1
∣
k
-
1
i
+
x
^
k
-
1
∣
k
-
1
i
-
x
^
k
-
1
∣
k
-
1
0
j
x
^
k
-
1
∣
k
-
1
i
-
x
^
k
-
1
∣
k
-
1
0
j
T
.(
3) Filtering
The SCKF and SCRHKF are processed to estimate the vehicle sideslip angle, respectively.(
4) Model Probability Update
Update the probability model:(34)
μ
k
j
=
μ
k
∣
k
-
1
j
Λ
k
j
∑
i
=
1
2
μ
k
∣
k
-
1
i
Λ
k
i
,
whereΛ
k
j
=
e
x
p
-
(
1
/
2
)
z
k
j
-
z
^
k
∣
k
-
1
j
T
P
z
k
z
k
j
z
k
j
-
z
^
k
∣
k
-
1
j
/
2
π
P
z
k
z
k
j.(
5) Estimation Fusion
Consider the following:(35)
x
^
k
∣
k
=
∑
j
=
1
2
μ
k
j
x
^
k
∣
k
j
,
P
k
∣
k
=
∑
j
=
1
2
μ
k
j
P
k
∣
k
j
+
x
^
k
∣
k
j
-
x
^
k
∣
k
x
^
k
∣
k
j
-
x
^
k
∣
k
T
.


## 3.1. Square-Root Cubature Kalman Filter

SCKF is a square-root extension of the standard CKF and uses the least-squares method for the Kalman gain and triangularization for covariance updates to improve the numerical stability. SCKF introduces a third-degree spherical-radial cubature rule to calculate the cubature pointξ
i and the corresponding weight ω
i: (10)
ξ
i
=
m
2
1
i
,
ω
i
=
1
m
,
i
=
1,2
,
…
,
m
,where m
=
2
n and n is the system state dimension; [
1
] is the set of points composed of the full array of the unit vector e
=
[
1,0
,
…
,
0
]
T’s elements, and [
1
]
i is the ith point from set [
1
].Assume at timek
-
1 that the Cholesky decomposition of the covariance P
k
-
1
∣
k
-
1 is known; that is, P
k
-
1
∣
k
-
1
=
S
k
-
1
∣
k
-
1
S
k
-
1
∣
k
-
1
T; the steps of SCKF are presented as follows [13].(
1) Time Update
Evaluate the cubature point:(11)
X
i
,
k
-
1
∣
k
-
1
=
S
k
-
1
∣
k
-
1
ξ
i
+
x
^
k
-
1
∣
k
-
1
.
Evaluate the propagated cubature points through the state function:(12)
X
i
,
k
∣
k
-
1
∗
=
f
X
i
,
k
-
1
∣
k
-
1
,
u
k
-
1
.
Estimate the predicted state:(13)
x
^
k
∣
k
-
1
=
1
m
∑
i
=
1
m
X
i
,
k
∣
k
-
1
∗
.
Estimate the square-root factor of the predicted error covariance:(14)
S
k
∣
k
-
1
=
T
r
i
a
χ
k
∣
k
-
1
∗
S
Q
,
k
-
1
,
whereT
r
i
a
(
·
) denotes a general triangularization algorithm (e.g., the QR decomposition) and is a lower triangular matrix; S
Q
,
k
-
1 denotes a square-root factor of Q
k
-
1 such that Q
k
-
1
=
S
Q
,
k
-
1
S
Q
,
k
-
1
T and the weighted, centered matrix (15)
χ
k
∣
k
-
1
∗
=
1
m
X
1
,
k
∣
k
-
1
∗
-
x
^
k
∣
k
-
1
X
2
,
k
∣
k
-
1
∗
-
x
^
k
∣
k
-
1
⋯
X
m
,
k
∣
k
-
1
∗
-
x
^
k
∣
k
-
1
.(
2) Measurement Update
Evaluate the cubature points:(16)
X
i
,
k
∣
k
-
1
=
S
k
∣
k
-
1
ξ
i
+
x
^
k
∣
k
-
1
.
Evaluate the propagated cubature points through the measurement function:(17)
Z
i
,
k
∣
k
-
1
=
h
X
i
,
k
∣
k
-
1
,
u
k
.
Estimate the predicted measurement:(18)
z
^
k
∣
k
-
1
=
1
m
∑
i
=
1
m
Z
i
,
k
∣
k
-
1
.
Estimate the square-root of the innovation covariance matrix:(19)
S
z
z
,
k
∣
k
-
1
=
T
r
i
a
Z
k
∣
k
-
1
S
R
,
k
,
whereS
R
,
k denotes a square-root factor of R
k such that R
k
=
S
R
,
k
S
R
,
k
T and the weighted, centered matrix (20)
Z
k
∣
k
-
1
=
1
m
Z
1
,
k
∣
k
-
1
-
z
^
k
∣
k
-
1
Z
2
,
k
∣
k
-
1
-
z
^
k
∣
k
-
1
⋯
Z
m
,
k
∣
k
-
1
-
z
^
k
∣
k
-
1
.
Estimate the cross-covariance matrix:(21)
P
x
z
,
k
∣
k
-
1
=
χ
k
∣
k
-
1
Z
k
∣
k
-
1
T
,
where the weighted, centered matrix(22)
χ
k
∣
k
-
1
=
1
m
X
1
,
k
∣
k
-
1
-
x
^
k
∣
k
-
1
X
2
,
k
∣
k
-
1
-
x
^
k
∣
k
-
1
⋯
X
m
,
k
∣
k
-
1
-
x
^
k
∣
k
-
1
.
Estimate the Kalman gain:(23)
W
k
=
P
x
z
,
k
∣
k
-
1
/
S
z
z
,
k
∣
k
-
1
T
S
z
z
,
k
∣
k
-
1
.
Estimate the updated state:(24)
x
^
k
∣
k
=
x
^
k
∣
k
-
1
+
W
k
z
k
-
z
^
k
∣
k
-
1
.
Estimate the square-root factor of the corresponding error covariance:(25)
S
k
∣
k
=
T
r
i
a
χ
k
∣
k
-
1
-
W
k
Z
k
∣
k
-
1
W
k
S
R
,
k
.


## 3.2. Square-Root Cubature Based Receding Horizon Kalman Filter

SCRHKF has an FIR structure, and the current estimation states of SCRHKF are based on a finite number of measurements over the recent time horizon [20]. Figure 3 shows the concept of the SCRHKF, which consists of the hidden horizon and the active horizon. The sizes of the receding horizon and receding interval are set up as N which is the system dimension. The RHKF is processed in the hidden horizon. The states and the inverse covariance matrix at time t
N
(
i
+
1
), which are used as the initial values of the active horizon from t
N
(
i
+
1
) to t
N
(
i
+
2
), can be estimated based on the final estimation states at time t
N
i and the measurements from t
N
i to t
N
(
i
+
1
) in this horizon. After processing the hidden horizon, the active horizon and another hidden horizon are carried out. In the active horizon, the SCKF is processed to calculate the final estimation states from t
N
(
i
+
1
) to t
N
(
i
+
2
) based on the outputs of the hidden horizon at time t
N
(
i
+
1
). The steps of SCRHKF are presented as follows.Figure 3
Concept of SCRHKF.(1) Initialization of Hidden Horizon
Initialize the states, the inverse covariance, and the pseudo error state:(26)
x
N
i
∗
=
x
^
N
i
,
Ω
N
i
=
0
n
×
n
,
ξ
^
N
i
=
0
n
×
1
.(2) Time Update of Hidden Horizon
Estimate the predicted inverse covariance:(27)
Ω
j
-
=
I
-
F
j
-
1
-
T
Ω
j
-
1
F
j
-
1
-
1
Q
-
1
+
F
j
-
1
-
T
Ω
j
-
1
F
j
-
1
-
1
-
1
F
j
-
1
-
T
Ω
j
-
1
F
j
-
1
-
1
,
whereF
j
=
(
∂
f
/
∂
x
)
x
=
x
j
∗ and j
=
N
i
+
η  (
η
∈
R
,
1
≤
η
≤
N
).
Estimate the predicted pseudo error state:(28)
ξ
^
j
-
=
I
-
F
j
-
1
-
T
Ω
j
-
1
F
j
-
1
-
1
Q
-
1
+
F
j
-
1
-
T
Ω
j
-
1
F
j
-
1
-
1
-
1
F
j
-
1
-
T
ξ
^
j
-
1
.(3) Measurement Update of Hidden Horizon
Estimate the update inverse covariance:(29)
Ω
j
=
Ω
j
-
+
H
j
T
R
-
1
H
j
,
whereH
j
=
(
∂
h
/
∂
x
)
x
=
x
j
∗.
Estimate the update pseudo error state:(30)
ξ
^
j
=
ξ
^
j
-
+
H
j
T
R
-
1
z
j
-
h
x
j
∗
.(4) Initialization of Active Horizon
Initialize the states and the covariance:(31)
P
N
i
+
1
=
Ω
N
i
+
1
-
1
,
x
N
i
+
1
∗
=
x
N
i
+
1
∗
+
Ω
N
i
+
1
-
1
ξ
^
N
i
+
1
.(5) Estimation State of Active Horizon
Estimate the final estimation states by formulas (11)~(25).


## 3.3. Interacting Multiple Model Approach

The IMM approach carries out a “soft switching” between the model-based SCKF and the model-based SCRHKF by the model probability. The steps of IMM are presented as follows [21].(
1) Model Mixing Probability Calculation
Calculate the model mixing probability:(32)
u
k
-
1
i
/
j
=
p
i
j
u
k
-
1
i
μ
k
∣
k
-
1
j
,
whereμ
k
-
1
i
=
p
{
m
k
-
1
i
∣
y
k
-
1
} is the model probability at time k
-
1; p
i
j
=
p
{
m
k
j
∣
m
k
-
1
i
} is the Markov transition probability from m
i to m
j;  μ
k
∣
k
-
1
j is the predicted probability; and μ
k
∣
k
-
1
j
=
∑
i
=
1
2
p
i
j
μ
k
-
1
i.(
2) Interaction/Mixing
Consider the following:(33)
x
^
k
-
1
∣
k
-
1
0
j
=
∑
i
=
1
2
u
k
-
1
i
/
j
x
^
k
-
1
∣
k
-
1
i
,
P
k
-
1
∣
k
-
1
0
j
=
∑
i
=
0
2
u
k
-
1
i
/
j
P
k
-
1
∣
k
-
1
i
+
x
^
k
-
1
∣
k
-
1
i
-
x
^
k
-
1
∣
k
-
1
0
j
x
^
k
-
1
∣
k
-
1
i
-
x
^
k
-
1
∣
k
-
1
0
j
T
.(
3) Filtering
The SCKF and SCRHKF are processed to estimate the vehicle sideslip angle, respectively.(
4) Model Probability Update
Update the probability model:(34)
μ
k
j
=
μ
k
∣
k
-
1
j
Λ
k
j
∑
i
=
1
2
μ
k
∣
k
-
1
i
Λ
k
i
,
whereΛ
k
j
=
e
x
p
-
(
1
/
2
)
z
k
j
-
z
^
k
∣
k
-
1
j
T
P
z
k
z
k
j
z
k
j
-
z
^
k
∣
k
-
1
j
/
2
π
P
z
k
z
k
j.(
5) Estimation Fusion
Consider the following:(35)
x
^
k
∣
k
=
∑
j
=
1
2
μ
k
j
x
^
k
∣
k
j
,
P
k
∣
k
=
∑
j
=
1
2
μ
k
j
P
k
∣
k
j
+
x
^
k
∣
k
j
-
x
^
k
∣
k
x
^
k
∣
k
j
-
x
^
k
∣
k
T
.


## 4. Experimental Results

The performance of the proposed estimator has been verified by a rear-wheel-driveHongqi H7 test vehicle. The vehicle is additionally equipped with the Oxford Technical Solutions RT3000 measurement unit and the rotary potentiometer coupled with the steering column using a simple belt and pulley system. Different driving manoeuvres are performed on a flat and dry asphalt surface for testing the estimator performance. All the geometrical and physical data of the car are listed in Table 1.Table 1
Vehicle parameters.
Vehicle parameters
Symbols
Values
Vehicle mass
m
1993 (kg)
Distance from the front axle to CoG
a
1.376 (m)
Distance from the rear axle to CoG
b
1.608 (m)
The track width
c
1.615 (m)
The CoG height
h
0.552 (m)
Vehicle moment of inertia
I
z
3668 (kg⋅m2)
Effective rolling radius of the tire
R
w
0.312 (m)As previously explained, the signals used for testing the proposed estimator include (i) the longitudinal and lateral vehicle velocity, the longitudinal and lateral vehicle acceleration, the vehicle sideslip angle, and the yaw rate acquired from the RT3000 unit, (ii) the steering angle captured by the rotary potentiometer, and (iii) front and rear wheel speeds obtained from the vehicle CAN bus system. The covariance matrices of the stochastic process noisew
k and the stochastic measurement noise v
k are set to Q
=
0.001
∗
I
3
×
3 and R
=
0.05
∗
I
3
×
3, and the matrix of the Markov transition probability is (36)
Φ
=
0.98
0.02
0.02
0.98
.The initial model probabilities for the model-based SCKF and the model-based SCRHKF are identical withμ
0
1
=
μ
0
2
=
0.5. The initial states of the estimator are identical to the measured values from the RT3000 unit, and the initial covariance matrix is chosen as P
0
=
0
∗
I
3
×
3.First, the double lane change manoeuvre is implemented and the lateral acceleration is more than 0.8 g, which is shown in Figure4. Figures 5(a)–5(d) show a comparison of the estimated results versus the measured values for the manoeuvre. The results based on the hybrid Kalman filter follow the trend of measured values quite well and they are always within an acceptable range, while the estimated lateral velocity and sideslip angle using SCKF generate some large errors due to the modeling mismatch in nonlinear region and sensor noise. Moreover, in order to quantitatively evaluate the effectiveness of the proposed method, the root mean-squared error between the estimated and measured values is computed. According to the statistical results shown in Table 2, the proposed method has the maximum root mean-squared error 0.0777 and mean values 0.0258, while the maximum and mean root mean-squared errors of the SCKF are 0.1840 and 0.0690, respectively.Table 2
Root mean-square error of state estimation.
Manoeuvre
State
RMSE
SCKF
Hybrid Kalman filter
Double lane change
Longitudinal velocity
0.0315
0.0149
Lateral velocity
0.1840
0.0777
Sideslip angle
0.0099
0.0023
Yaw rate
0.0505
0.0082
Slalom
Longitudinal velocity
0.0177
0.0146
Lateral velocity
0.1271
0.0739
Sideslip angle
0.0075
0.0040
Yaw rate
0.0260
0.0081Figure 4
Estimation input under double lane change manoeuvre.Figure 5
Double lane change manoeuvre results: (a) longitudinal velocity, (b) lateral velocity, (c) vehicle sideslip angle, and (d) yaw rate.
(a)
(b)
(c)
(d)Second, the slalom manoeuvre is implemented and the vehicle undergoes the fast and large steering angle action, which is shown in Figure6. Figures 7(a)–7(d) show a comparison of the estimated results versus the measured values for the manoeuvre and similar results can be obtained compared with double lane change manoeuvre. The SCKF is influenced by the modeling mismatch and sensor noise and shows some large errors for the estimated lateral velocity and sideslip angle. However, the hybrid Kalman filter has high convergence speed for the fast and large vehicle lateral motion and great robustness against model mismatch and sensor noise, and therefore the filter can accurately track the measured values except the peak region of the lateral acceleration, in which the vehicle reaches the limit of tire-road adhesion and the estimated lateral velocity and sideslip angle of the hybrid Kalman filter have acceptable errors. According to the statistical results shown in Table 2, the proposed method has the smallest maximum 0.0739 and mean values 0.0251; the maximum and mean values of the SCKF are 0.1271 and 0.0446, respectively.Figure 6
Estimation input under slalom manoeuvre.Figure 7
Slalom manoeuvre results: (a) longitudinal velocity, (b) lateral velocity, (c) vehicle sideslip angle, and (d) yaw rate.
(a)
(b)
(c)
(d)


## 5. Conclusions

This paper has presented novel estimation methods based on the nonlinear 3-DoF vehicle model with the tire model “Magic Formula” and making use of typical sensory information found in stock ESC sensor packs to estimate the vehicle sideslip angle accurately. The proposed method combines the merit of SCKF that has high convergence speed and numerical stability with the merit of SCRHKF that has great robustness against model uncertainty and temporary noise. Then, the real vehicle test is implemented to validate the performance of the proposed method. The comparison with SCKF shows that it can provide more accurate estimation for the active safety control systems.In future works, the proposed method can be improved to adapt to various road conditions. Moreover, active safety control systems based on the proposed method should be researched.



---

*Source: 3269142-2016-07-18.xml*