Attitude Determination Method by Fusing Single Antenna GPS and

Attitude Determination Method by Fusing Single Antenna GPS and
Hindawi
Mathematical Problems in Engineering
Volume 2017, Article ID 4517673, 14 pages
https://doi.org/10.1155/2017/4517673
Research Article
Attitude Determination Method by Fusing Single
Antenna GPS and Low Cost MEMS Sensors Using Intelligent
Kalman Filter Algorithm
Lei Wang, Bo Song, Xueshuai Han, and Yongping Hao
Research Centre of Weaponry Science and Technology, Shenyang Ligong University, Shenyang, Liaoning, China
Correspondence should be addressed to Lei Wang; [email protected]
Received 2 March 2017; Revised 10 May 2017; Accepted 12 June 2017; Published 17 July 2017
Academic Editor: Kalyana C. Veluvolu
Copyright © 2017 Lei Wang et al. This is an open access article distributed under the Creative Commons Attribution License, which
permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.
For meeting the demands of cost and size for micronavigation system, a combined attitude determination approach with
sensor fusion algorithm and intelligent Kalman filter (IKF) on low cost Micro-Electro-Mechanical System (MEMS) gyroscope,
accelerometer, and magnetometer and single antenna Global Positioning System (GPS) is proposed. The effective calibration
method is performed to compensate the effect of errors in low cost MEMS Inertial Measurement Unit (IMU). The different
control strategies fusing the MEMS multisensors are designed. The yaw angle fusing gyroscope, accelerometer, and magnetometer
algorithm is estimated accurately under GPS failure and unavailable sideslip situations. For resolving robust control and characters
of the uncertain noise statistics influence, the high gain scale of IKF is adjusted by fuzzy controller in the transition process and
steady state to achieve faster convergence and accurate estimation. The experiments comparing different MEMS sensors and fusion
algorithms are implemented to verify the validity of the proposed approach.
1. Introduction
Attitude estimation is essential information for control of
micronavigation system, such as Micro Autonomous Vehicle,
Micro Aerial Vehicle (MAV), and Guided Munition Shell
[1–4]. In these specific applications, the size and cost are
limited. Low cost and small size MEMS IMU, which consists
of MEMS gyroscope and accelerometer, could be used in
micronavigation system. Lower price, smaller sized MEMS
IMU is less accurate, has large biases, and is more noisy. The
errors of MEMS sensors will lead to a great impact on the
calculation performance and reduce the estimation accuracy
significantly [5, 6]. Thus, the effective calibration and error
compensation method is essential to evaluate and improve
the performance of MEMS IMU before attitude estimation
[7–9].
Nowadays, the different attitude estimation algorithms
have been developed, which are employed to Micro IMUs
and integrated with magnetic sensors, Global navigation
satellite system (GNSS) receiver, and other sensors [1–4,
10–17]. Among them, representative researches like multiinformation fusion technique utilize the different MEMS
sensor [10–12], different Kalman filtering methods for attitude estimation [3, 13–15], and methodology combining data
fusion with filtering strategies [16, 17]. Utilizing barometer
and low cost IMU, the data fusion method via the complementary filter is proposed to fuse altitude data for unmanned
aerial vehicle (UAV) system [10]. A quaternion-based complementary filter for estimating the attitude of a smartphone
using IMU and magnetic sensor is developed. The optimal
tuning correction factors of the algorithm across all the body
movements were also identified [11]. An attitude estimation
method is proposed using low cost gyroscope, magnetometer,
and GPS receiver. The pseudoattitude, magnetic attitude, and
gyroscope measurement are fused based on Euler angle. The
simulation and flight test verified the proposed method [12].
For small UAVs, an attitude estimation method fusion of
gyroscopes and single antenna GPS with quaternion-based
2
EKF is proposed. The results of the simulations and the
flight experiment demonstrate the ability of the proposed
algorithm [3]. The sideslip angle is estimated in Kalman
filter using the velocity signals from GPS and angular signals
from magnetometer in case that no longitudinal tire force
and no pitch angle exist. Road tests are implemented [13].
According to practical application of quadcopter UAV, the
adaptive gain parameter in EKF filter was tuned [14]. The
attitude estimation algorithm is developed by incorporating
a nonlinear observer in order to fuse measurements from
both the gyroscope and accelerometer. The attitude rotation
matrix is evaluated with estimated the gyro bias and tilt
error [15]. A wireless IMU with the smallest volume and
weight is designed. The data fusion is implemented before a
Kalman filter combined with the zero velocity update. The
methodology for wireless IMU is adopted to track a person
in an indoor area [16]. Integrating double-antenna Global
Positioning System (DA-GPS) with other low cost in-vehicle
sensors, two parallel extended Kalman filters (EKFs) are
adopted to estimate the roll angle firstly; then vehicle sideslip
and yaw angles are estimated through fusion methodology
[17].
The above works on attitude estimation algorithm lie in
following problems. Some attitude estimation methods could
provide reliable estimation, but it has to rely on the use of
multiple antennas, which incurs a complex structure, large
volume, and high cost of the system [1, 17]. It is restricted
in such micro navigation system. For traditional extended
Kalman filter and other improved Kalman filter methods for
attitude estimation, it would cause the inaccurate estimation
or even divergence because the statistical properties of process and measurement noise cannot be predicted accurately,
especially for low cost MEMS-based sensors [3, 13, 14]. Some
complicated nonlinear filter and optimal control methods
could obtain optimal estimate. On the other hand, the main
drawback lies in introducing a heavy computational burden
that leads to difficulty of application in real time navigation
system [15, 18].
For meeting the demands of cost and size for micro
navigation system, such as spinning shell, Missiles, and
aircrafts, the scheme of attitude estimation combining single
antenna GPS with low cost MEMS IMU and magnetometer is
designed. This paper concentrates on developing a combined
fusion methodology with intelligent Kalman filter, which
realizes the fusion of Micro IMU, GPS, and magnetometer.
It could continue to provide reliable information, particularly in larger sideslip angle, GPS failure situation. Testing
experiments using the proposed approach are presented by
comparison with different fusion algorithms and filter methods. The novel aspects of this paper can be summarized as
follows.
To utilize the strengths of the fusion estimation algorithm
and adaptive filter approaches, the combined approach has
been proposed. The different control strategies fusing the
MEMS multi-inertial sensors are adopted under different
driving situations.
The high gain scale in IKF is tuned by fuzzy controller
to resolve the problem of robust control and characters of
the uncertain noise statistics influence for low micro IMU.
Mathematical Problems in Engineering
GPS-measured yaw angle for Micro IMU/GPS system is
afforded as measurement to enhance the observability of IKF.
Besides, the fusion information of yaw angle using gyroscope,
accelerometer, and magnetometer fusion algorithm is added
into IKF observer, which could provide reliable information,
particularly under unavailable larger sideslip angle, GPS
failure situation. Integrating data fusion algorithm with
IFK method, the proposed methodology could significantly
improve the estimation performance.
2. Methodology
2.1. Gyroscope Model and Attitude Algorithm. MEMS inertial
sensors of low cost, small size, and low power consumption
are now widely used in micronavigation system and guidance
system of tactical weapons. The application of these sensors
is restricted for low precision and bias instability. It is
essential to test, calibrate, and compensate for these sensors.
The systematic error sources for MEMS inertial sensors
mainly embody in biases, scale factors, nonorthogonality, or
misalignment errors. The output model of a triad of MEMS
gyro sensor in matrix form can be represented as
̃𝑏 = 𝑠 ⋅ 𝜔𝑏 + 𝑏.
𝜔
𝑖𝑏
𝑖𝑏
(1)
In (1), the definition of angular rates is based on the following conventions. The upper index defines the referenced
coordinate system, whereas the lower index refers to the
described value. The rotation refers to the relative movement
of coordinate system. Used indices are 𝑒-Earth frame, 𝑛Navigation frame, 𝑖-inertial frame, and 𝑏-body frame, where
̃𝑏 is the raw measurement angular rate of the gyroscope; 𝜔𝑏
𝜔
𝑖𝑏
𝑖𝑏
is the true angular rate; 𝑏 is the output bias. The scale factor
and misalignment matrix 𝑠 is given by
𝑘𝑥 𝜎𝑥𝑦 𝜎𝑥𝑧
[
]
]
𝑠=[
[𝜎𝑦𝑥 𝑘𝑦 𝜎𝑦𝑧 ] .
[𝜎𝑧𝑥 𝜎𝑧𝑦 𝑘𝑧 ]
(2)
The main diagonal elements 𝑘 are scale factors that
account for the sensitivities of the individual sensors by
scaling the outputs. The variable 𝜎 represents the nonorthogonality and misalignment of the triaxial gyroscope sensors.
The biases, scale factors, nonorthogonality, or misalignment
errors are main systematic error or deterministic sources for
MEMS inertial sensors. The body angler rates with respect to
the navigation frame 𝜔𝑛𝑏 are given by the difference between
the body angler rates, 𝜔𝑖𝑏 , and the navigation frame rotation,
𝜔𝑖𝑛 .
𝑇
𝑏
𝑏
𝑛
= 𝜔𝑖𝑏
− (𝐶𝑏𝑛 ) 𝜔𝑖𝑛
.
𝜔𝑛𝑏
(3)
The coordinate transformation from navigation frame to
body fixed frame is based on the three angles, namely, pitch,
roll, and yaw, denoted by the symbols 𝜃, 𝜙, and 𝜓. The attitude
matrix 𝐶𝑏𝑛 from navigation frame and body frame by three
Euler angles is given by
Mathematical Problems in Engineering
𝐶𝑏𝑛
3
cos 𝜃 cos 𝜓 sin 𝜙 sin 𝜃 cos 𝜓 − cos 𝜙 sin 𝜓 cos 𝜙 sin 𝜃 cos 𝜓 + sin 𝜙 sin 𝜓
[ cos 𝜃 sin 𝜓 sin 𝜙 sin 𝜃 sin 𝜓 + cos 𝜙 cos 𝜓 cos 𝜙 sin 𝜃 sin 𝜓 − sin 𝜙 cos 𝜓]
=[
].
[ − sin 𝜃
sin 𝜙 cos 𝜃
The propagation of the Euler angles with time yields is
derived in
𝑏
𝜔𝑛𝑏𝑥
𝜙̇
1 sin 𝜙 tan 𝜃 cos 𝜙 tan 𝜃
[
]
[ ] [
𝑏 ]
[ 𝜃̇ ] = [0
cos 𝜙
− sin 𝜙 ]
(5)
][
[𝜔𝑛𝑏𝑦 ] .
[ ]
𝑏
[𝜓̇ ] [0 sin 𝜙 sec 𝜃 cos 𝜙 sec 𝜃 ] [𝜔𝑛𝑏𝑧 ]
By (3), neglecting of the earth rate and the angular
velocity of the local-level frame for the spherical earth model,
𝑏
𝑏
𝑏
are approximately equal as 𝜔𝑖𝑏
≈ 𝜔𝑛𝑏
.
the gyroscope rates 𝜔𝑖𝑏
2.2. Accelerometer Model and Attitude Algorithm. Define the
̃𝑏 as the raw measurements of accelerometer in body frame.
𝑓
The vector 𝑓𝑏 is the true output of accelerometer. Considering
the scale factor error and fixed error, the relation is given by
̃𝑏 = (𝐼 + [𝛿𝐾 ]) (𝐼 + [𝛿𝐴]) 𝑓𝑏 .
𝑓
𝐴
(6)
The matrix [𝛿𝐾𝐴] representing scale factor error for
accelerometer and the skew symmetric matrix [𝛿𝐴] representing the fixing error of accelerometer are given by
𝛿𝐾𝐴𝑥 0
[ 0 𝛿𝐾
[𝛿𝐾𝐴 ] = [
𝐴𝑦
[ 0
0
[
[𝛿𝐴] = [
[−𝛿𝐴𝑧
0
0
0 ]
],
𝛿𝐾𝐴𝑧 ]
𝛿𝐴𝑧 −𝛿𝐴𝑦
(7)
]
𝛿𝐴𝑥 ]
].
0 ]
0
[ 𝛿𝐴𝑦 −𝛿𝐴𝑥
The parameters of matrix [𝛿𝐾𝐴 ] and [𝛿𝐴] are computed
by calibration method. ∇𝑏 is the error vectors of accelerometer. Define 𝑓𝑛 as the value in the navigation frame. The
relation between the output of accelerometer 𝑓𝑏 and 𝑓𝑛 is in
𝑓𝑏 = 𝐶𝑛𝑏 𝑓𝑛 = 𝐶𝑛𝑏 (𝑎𝑛 + 𝑔𝑛 + ∇𝑛 ) = 𝑎𝑏 + 𝐶𝑛𝑏 𝑔𝑛 + ∇𝑏 ,
(8)
𝑏
where 𝑔 is the gravity. The vector 𝑎 is the vehicle acceleration. The following two cases are considered in (8). One is
that the vehicle is in stationary or low dynamic condition,
accelerometer measures gravity mainly. The accelerometer
output in navigation frame is given by
𝑇
𝑛
𝑓 = [0 0 𝑔] .
𝜙 = arctan (
𝑓𝑥𝑏
),
𝑔
𝑓𝑦𝑏
𝑔 ⋅ cos 𝜃
(10)
).
]
On the other side, when the vehicle is accelerated, the
accelerometer measurement reflects not only gravity but also
the acceleration of the vehicle. The vehicle acceleration, 𝑎𝑏 ≠
0, is determined by Costa theorem. The vector 𝜐𝑏 is the vehicle
velocity in body frame
𝑎𝑏 = 𝜐𝑏̇ + 𝜔𝑏 × 𝜐𝑏 .
(11)
2.3. Magnetometer Model and Attitude Algorithm. Magnetometer sensor is designed and manufactured by making
use of alloy resistance having sensitive response to direction
magnetic field. The performance of magnetometer sensor is
distorted by hard iron and soft iron effects. Intrinsic and cross
sensor calibration of three-axis magnetometer is indispensable to eliminate scale factor, cross coupling, and bias errors.
Thus, the magnetometer’s output can be modeled by
̃𝑏 = 𝐶 𝑚𝑏 + 𝜀.
𝑚
𝑒
(12)
̃𝑏 represents the raw measurement in body
The matrix 𝑚
𝑏
frame. 𝑚 represents the calibrated magnetometer output.
Here, 𝐶𝑒 can be decomposed as 𝐶𝑒 = 𝐶sf 𝐶no 𝐶sm , where 𝐶sf ,
𝐶no , and 𝐶sm matrix represent effect of sensor imperfection
and magnetic disturbance. The subscript “sf” means the scale
factor error, “no” means nonorthogonality and misalignment
error, and “sm” means the soft iron effect. The vector 𝜀
represents the noise of magnetic disturbance. Define 𝑚𝑛 as
the local magnetic vector in 𝑛-frame. The relationship of
magnetic vectors in 𝑏-frame and 𝑛-frame is given by
𝑚𝑏 = 𝐶𝑛𝑏 𝑚𝑛 .
(13)
Considering the horizontal plane projection of magnetic
field 𝑚𝑝 , corresponding representation is given by
𝑚𝑥𝑏
[
]
[ 𝑝] [ 0
𝑏]
cos 𝜙
− sin 𝜙 ]
]⋅[
[𝑚𝑦 ] = [
[𝑚𝑦 ] . (14)
𝑝
[𝑚𝑧 ] [− sin 𝜃 sin 𝜙 cos 𝜃 cos 𝜙 cos 𝜃] [𝑚𝑧𝑏 ]
𝑚𝑥𝑝
cos 𝜃 sin 𝜙 sin 𝜃 cos 𝜙 cos 𝜃
Eventually, yaw angle 𝜓𝑚 is calculated in
(9)
Thus, the roll and pitch angles are determined by the
following equations, respectively,
𝜃 = − arctan (
cos 𝜙 cos 𝜃
(4)
𝜓𝑚 = arctan (
𝑚𝑦𝑝
𝑝).
𝑚𝑥
(15)
2.4. Different Sensor Attitude Fusion Estimation Algorithm.
For the scheme of attitude estimation combining low cost
MEMS IMU, magnetometer with single antenna GPS, Strapedown Inertial Navigation system (SINS) of MEMS gyroscope, and accelerometer could provide angles utilizing the
differential equation for quaternion vector but suffer from
4
Mathematical Problems in Engineering
accumulating errors induced by gyroscopes bias drift. The
differential equations of the quaternion vector are expressed
as
1
𝑏
𝑏
(16)
− 𝜔𝑖𝑛
).
𝑞 ̇ = 𝑞 ⊗ (𝜔𝑖𝑏
2
Here, 𝑞 is the error quaternion vector, 𝑞
=
𝑇
[𝑞0 𝑞1 𝑞2 𝑞3 ] . The operator “⊗” denotes the quaternion
𝑏
𝑛
= 𝐶𝑏𝑛 𝜔𝑖𝑛
.
multiplication. The vector 𝜔𝑖𝑛
Accelerometers can provide the pitch and roll angles by
measuring the gravity vectors if the vehicle is stationary or in
low dynamic environment in (10). Triaxis magnetometers are
used for calculating the yaw angle in (14) and (15). Its accuracy
is heavily affected by local magnetic environments.
Derived from the single antenna GPS, the pseudoattitude
method [19, 20] is adopted. The velocity vector 𝑉𝑛 =
𝑇
[𝑉𝐸𝑛 𝑉𝑁𝑛 𝑉𝐷𝑛 ] is the single antenna GPS-measured velocity
vector in the east, north, and down directions of the navigation frame, respectively. The lift acceleration vector 𝐿 is
equivalent to the difference between the normal acceleration
component 𝑎𝑛 and gravitational acceleration component 𝑔𝑛 .
Then 𝐿 is given by 𝐿 = 𝑎𝑛 − 𝑔𝑛 . A horizontal reference vector
𝑇
𝑃 is derived from 𝑉𝐷 = [0 0 𝑉𝐷𝑛 ] and 𝑉𝑛 , in the form
𝐷
𝑛
𝑃 = 𝑉 × 𝑉 . The Euler angle denoting pseudoattitude algorithm is determined in
𝜃𝑠 = arctan (
𝜙𝑠 = arcsin [
𝑉𝐷𝑛
(√𝑉𝑁𝑛 2 + 𝑉𝐸𝑛 2 )
),
(𝐿 ⋅ 𝑃)
],
(|𝐿| |𝑃|)
𝜓𝑠 = arctan (
(17)
𝑉𝐸𝑛
).
𝑉𝑁𝑛
It has been verified that GPS could reliably provide the
estimation. However, some common limitations are that GPS
failure is not considered or larger sideslip angle could not be
estimated for single antenna GPS.
For attitude estimation algorithm by fusing data from
MEMS inertial sensors, complementary filter (CF) is usually
utilized to estimate attitudes due to the complementary
characteristic of gyroscopes and accelerometer [10, 11]. The
classical form of the complementary filters can be reformulated.
̂1 (𝑠) + 𝐺2 (𝑠) 𝑋
̂2 (𝑠) .
̂ (𝑠) = 𝐺1 (𝑠) 𝑋
𝑋
(18)
̂
̂1 (𝑠)
𝑋(𝑠)
is the estimation of the complementary filter. 𝑋
̂2 (𝑠) are the estimation by gyro and accelerometer,
and 𝑋
respectively. The transfer function 𝐺1 (𝑠) is first-order high
pass filter and 𝐺2 (𝑠) = 1 − 𝐺1 (𝑠) is first-order low pass filter.
The transfer functions of the filters are as follows
𝑠
𝐺1 (𝑠) =
,
(𝑠 + 𝐾)
(19)
𝐾
𝐺2 (𝑠) =
.
(𝑠 + 𝐾)
For attitude algorithm, gyroscopes can provide angles
but suffer from accumulating errors induced by gyroscopes
bias drift. High pass filter could eliminate the low-frequency
random noise of the Euler angle integrated by gyroscope,
while the low pass filter could eliminate the high frequency
random noise of calculation by accelerometer.
2.5. IKF Fusion Algorithm for Attitude Estimation. According
to the micro navigation system of low cost Micro IMU,
magnetometer, and a single antenna GPS receiver, the aforementioned attitude estimation approaches utilizing different
sensors separately are difficult to provide reliable estimation
due to sensor errors and dynamic environmental variations.
The single fusion algorithm employing complementary filter
is also difficult to achieve good robustness for different
driving situations and complementary filter parameters. In
practice cases, small adjustment in fusion and control strategy
may finally affect the estimation performance. In addition, for
trying to solve the attitude estimation utilizing conventional
EKF or modified EKF methods, the uncertain noise for low
micro IMU would not be estimated accurately.
To overcome the drawbacks of above-mentioned methods, it is preferable to develop a reliable approach to estimate
attitude angles using affordable sensors in micro navigation system. Utilizing the advantage of the fusion estimation algorithm and adaptive filter approach, the combined
approach has been also developed. The proposed method
could estimate the yaw angle utilizing affordable sensors
under different driving situations. It could accommodate GPS
failure or the situation of unavailable sideslip, which could
continue to provide reliable information. In IKF module,
the adaptive gain scale is adjusted by the fuzzy controller
to compensate the uncertainty of measurement and process
noise. Furthermore, the control strategies of different measurements considering sensor failure are adopted to solve the
lack of observability of yaw angle. The switching criterion
is determined according to GPS satellite number, yaw error
estimate result, and the vehicle acceleration measurements
to make a distinction between normal driving condition,
vehicle turning, and GPS outage. One side, besides measurements from accelerometer, GPS-measured heading angle
for micronavigation system is afforded as the measurement
to enhance the observability in normal driving condition.
Additionally, in case of GPS failure or the situation of unavailable sideslip, the fusion information of yaw angle using
gyroscope, accelerometer, and magnetometer fusion algorithm is added into IKF observer. The scheme of the proposed
methodology is shown in Figure 1.
This scheme is actually a combined fusion algorithm
and filtering approach architecture. It comprises two parts:
the multisensor fusion module and IKF method. The multisensor fusion module consists of Micro IMU, magnetometer, and single antenna GPS. Micro IMU includes
three single-axis MEMS gyroscopes and accelerometers. The
deterministic errors are compensated by calibration process
before fusion algorithm. Different fusion strategies provide
efficient solutions to enhance the accuracy of the attitude
estimation. To realize further accurate estimation, the IKF is
designed.
Mathematical Problems in Engineering
5
GPS outage or unavailable sideslip
Magnetometer
b
m
Calibration &
mb
error
compensation
Attitude
determination
based
magnetometer
Micro IMU
MEMS
gyroscope
MEMS
accelerometer
Fusion algorithm
m
Roll & pitch
estimation
b
ib

Calibration & b
ib
error
compensation
Attitude
determination
based
gyroscope
fb
Calibration & fb
error
compensation
Attitude
determination +
based
accelerometer
_
g
Intelligent
Kalman
filter
algorithm
(IKF)
Control
strategy
N E D

Measurement
updates
E, k
G
 , 
,
Pseudoattitude
algorithm
GPS works normally

Fuzzy
controller
ab
Vehicle acceleration
calculation
Single antenna
GPS receiver
Yaw
estimation
bx , by , bz
Figure 1: The scheme of combing fusion algorithm with IKF for low cost Miro IMU and single antenna GPS.
According to difference equation of the Euler angles in
(5), the state vector is chosen of three attitude angles and
triaxis gyroscope random bias vector.
𝑇
𝑥 = [𝜑 𝜃 𝜓 𝑏𝑥 𝑏𝑦 𝑏𝑧 ] .
(20)
The state model is thus
̃𝑏 ) = (
𝑥̇ = 𝑓 (𝑥, 𝜔
𝑖𝑏
̃𝑏 − 𝑏)
Γ ⋅ 𝑠−1 (𝜔
𝑖𝑏
).
1
− 𝜀𝑟 + 𝑤𝑟
𝛽
(21)
The time varying bias component 𝜀𝑟 will be modeled as
a first-order Gauss-Markov process. 𝑤𝑟 is a Gaussian white
process. 𝛽 is the time constant. 𝑏 is the constant bias.
With
1 sin 𝜙 tan 𝜃 cos 𝜙 tan 𝜃
[0
cos 𝜙
− sin 𝜙 ]
Γ=[
],
(22)
[0 sin 𝜙 sec 𝜃 cos 𝜙 sec 𝜃 ]
̃𝑏 is the measurement angular rate signal of the
where 𝜔
𝑖𝑏
gyroscope and 𝑠 is the misalignment and scale factor matrix
in (1), the GPS and accelerometer with high accuracy provide
measurements. The measurement model is chosen as follows:
∇𝐺
𝜓𝐺
𝜓
] [
[ 𝑏]
[ 𝑏
𝑏
]
]
[ ]
𝑧=[
[𝑓𝑥 − 𝑎𝑥 ] = [ − sin 𝜃 ⋅ 𝑔 ] + [ ∇𝑥 ] ,
𝑏
𝑏
𝑏
[𝑓𝑦 − 𝑎𝑦 ] [sin 𝜙 cos 𝜃 ⋅ 𝑔] [ ∇𝑦 ]
(23)
where 𝜓𝐺 is the GPS-measured yaw angle, ∇𝜓 is the zero
Gaussian white noise with variance 𝜎𝐺2 , 𝑓𝑏 is the acceleration
signal of the accelerometer, and ∇𝑏 is the measurement noise
of accelerometer.
When single antenna GPS system could provide normal
satellite signals from GPS receiver, the yaw angle is calculated
by velocity of GPS. If the single antenna GPS signal is lock,
the GPS-measured yaw angle is unavailable. A compensation
method integrating the MEMS inertial sensors is presented.
The Euler angles can be estimated by means of measurement
of gyroscopes according to
𝑏
𝑏
cos 𝜙 − 𝜔𝑖𝑏𝑧
sin 𝜙,
𝜃𝑔̇ = 𝜔𝑖𝑏𝑦
𝑏
𝑏
𝑏
𝜙𝑔̇ = 𝜔𝑖𝑏𝑥
+ 𝜔𝑖𝑏𝑦
sin 𝜙 tan 𝜃 + 𝜔𝑖𝑏𝑧
cos 𝜙 tan 𝜃,
(24)
𝑏
𝑏
𝜓̇𝑔 = 𝜔𝑖𝑏𝑦
sin 𝜙 sec 𝜃 + 𝜔𝑖𝑏𝑧
cos 𝜙 sec 𝜃.
The yaw angle 𝜓(𝑡) is calculated in (25); correspondingly,
the corrected yaw angle 𝜓(𝑡) is used as measurement vector
in intelligent Kalman filter.
𝜓 (𝑡) = (1 − 𝛼1 ) ⋅ 𝜓 (𝑡 − 𝑇) + 𝛼1 ⋅ 𝜓𝑚 (𝑡) + 𝛼2 ⋅ 𝜓̇𝑔 (𝑡) , (25)
where 𝛼1 = (𝑘1 ⋅ 𝑇)/(𝑘1 ⋅ 𝑇 + 1), 𝛼2 = 𝑇/(𝑘2 ⋅ 𝑇 + 1).
Here T is the sampling time length and 𝜓(𝑡 − 𝑇) is the
estimated value of last sampling time. 𝜓𝑚 (𝑡) is the estimated
value by magnetometer sensor in (15). 𝜓𝑔 (𝑡) is the estimated
value by gyro sensor in (24). It is worthwhile to note that
the roll and pitch angles estimated by accelerometer are prior
to calculating horizontal plane projection of magnetic field
𝑚𝑝 with reference to (14). Then the magnetometer-based yaw
6
Mathematical Problems in Engineering
angle could be determined with reference to (15). 𝛼1 and 𝛼2
are the weight coefficients of the first-order low pass and
high pass filter; 𝑘1 is cutoff bandwidth of low pass filter; 𝑘2 is
cutoff bandwidth of high pass filter. Considering the higher
frequency for noises of magnetometers, the high frequency
noises of magnetometer are eliminated in low pass filter.
Furthermore, the high pass filter is preferred to eliminate
the lower frequency noises of gyroscope. In this way, the
yaw angle could be estimated accurately depending on fusion
of gyroscope and magnetometer in case that GPS signals is
unavailable. The switching criterion is determined through
detecting the vehicle acceleration and GPS-measured velocity. Different measurements are provided in IKF. The initial
system noise statistics 𝑄 matrix can be determined based on
Allan standard variance of angle random walk about system
model. The statistic characteristic of measurement noise
could be determined by using Allan variance method; angular
velocity random walk and bias instabilities were crucial noise
in MEMS sensors. The initial measurement noise statistics
R matrix can be determined based on the observation noise
of measurement acceleration bias instabilities. 𝐵𝑥 and 𝐵𝑦 are
bias instabilities of accelerometer. The initial values of 𝑅 and
𝑄 are given by
03×3 03×3
],
𝑄=[
03×3 𝐶3×3
−2
𝐶3×3 = 10
5.693 8.485 3.059
[8.485 1.265 4.559]
∗[
],
[3.059 4.559 1.644]
0
0
0
[
]
]
𝑅=[
[0 𝐵𝑥 ⋅ 𝐵 𝐵𝑥 ⋅ 𝐵𝑦 ]
[0 𝐵𝑦 ⋅ 𝐵𝑥 𝐵𝑦 ⋅ 𝐵𝑦 ]
−4
= 10
(26)
0 0
0
[0 4.942 4.175]
∗[
].
[0 4.175 3.528]
The IKF algorithm from transition process to steady state
would adjust the magnitude of 𝑄 matrix by high gain scale,
and not only depending on initial value.
2.6. The Tuning of High Gain Scale. In order to resolve the
problem of robust control and characters of the uncertain
noise statistics influence, an intelligent Kalman filter (IKF)
is proposed and it, just like the traditional EKF, does not
require a constant time step, nor synchronous measurements.
Define 𝑥𝑘 as the state vector, 𝑧𝑘 as the vector of measurement,
and 𝜔𝑘 and ]𝑘 as the process and measurement noises whose
covariance is given by 𝑄𝑘 and 𝑅𝑘 , respectively. The transition
and measurement models are expressed as follows:
𝑥̂𝑘,𝑘−1 = 𝑓 (𝑥̂𝑘−1 , 𝜔𝑘 ) ,
𝑃𝑘,𝑘−1 = Φ𝑘,𝑘−1 𝑃𝑘−1 Φ𝑇𝑘,𝑘−1 + 𝛾𝑘 L𝑘 𝑄𝑘−1 L𝑘 ,
−1
𝐾𝑘 = 𝑃𝑘,𝑘−1 𝐻𝑘𝑇 (𝐻𝑘 𝑃𝑘,𝑘−1 𝐻𝑘𝑇 + o𝑘 𝑅𝑘 o𝑘 ) ,
𝑥̂𝑘 = 𝑥̂𝑘,𝑘−1 + 𝐾𝑘 [𝑧𝑘 − ℎ (𝑥̂𝑘,𝑘−1 )] ,
𝑃𝑘 = 𝑃𝑘,𝑘−1 − 𝐾𝑘 𝐻𝑘 𝑃𝑘,𝑘−1 .
(27)
The state vector x has 6 elements. The matrices L𝑘
and o𝑘 are diagonal. The variable 𝛾𝑘 is the high gain scale
(1 ≤ 𝛾𝑘 ≤ 𝛾max ). Define 𝐽𝜏,𝑘 as the innovation sequence,
and 𝜏 is the calculation length of moving window. The
parameter 𝑛 is the first sampling time with a sampling period.
𝐸𝜏,𝑘 is the variation between the innovation sequence and
threshold value at sample time k, which reflects the influence
of measurement noise
𝑘
󵄩
󵄩2
𝐽𝜏,𝑘 = ∑ (𝑡𝑖+1 − 𝑡𝑖 ) 󵄩󵄩󵄩𝑧 (𝑡𝑖 ) − 𝑧̃ (𝑡𝑖 )󵄩󵄩󵄩 ,
(28)
𝐸𝜏,𝑘 = 𝐽𝜏,𝑘 − 𝐽threshold .
(29)
𝑖=𝑛
The high gain scale is adjusted by fuzzy controller according to the innovation sequence 𝐽𝜏,𝑘 and variation 𝐸𝜏,𝑘 .
𝛾𝑘 = Rfuzzy (𝐽𝜏,𝑘 ) .
(30)
The following analysis is given concerning the adjustments of high gain scale 𝛾𝑘 . The performance of the extended
Kalman filter applied to nonlinear systems is highly dependent on measurement and process noise variance values. In
practical application of the Kalman filter a priory information
about measurement noise statistics (R matrix) is usually
known, because it depends on the quality of measurement
sensors used. The information about system noise statistics
(𝑄 matrix) is quite subjective, because it depends on the
model representation created by a developer. Kalman filter
error does not depend on the selection of 𝑄 or 𝑅 absolute
levels but on their relationship [21]. As a result, the reasonable
way to analyze the noise statistics influence on the estimation
errors is based on 𝑄 matrix influence interpretation when
𝑅 matrix is fixed. The estimation process of the Kalman
filter contains two stages. The first stage is a transition
process of the estimation; the second one is a steady state. In
practical applications, the true behavior of some state vector
components is unknown. In order to select the reasonable
magnitude of 𝑄 matrix, the following procedure would be
suggested.
During the first stage of Kalman filter, the larger value of
variation 𝐸𝜏,𝑘 is obtained. The high gain scale 𝛾𝑘 should tend
to increase its value until it tends to maximum value 𝛾max .
With reference to (27), it is equivalent to increase elements of
Q matrix in first stage. Correspondingly, the magnitude of the
elements of 𝑃𝑘,𝑘−1 and 𝐾𝑘 increase; it means that the weight of
measurements in estimation process has to be high in order
to reduce the influence of input noise on current estimates.
In this way, the effect of faster convergence could be realized;
meanwhile, the Kalman filter is in steady state from transition
process.
Furthermore, the Kalman filter provides the estimates
with constant accuracy at the steady state, which cannot be
improved with time. Corresponding, the variation 𝐸𝜏,𝑘 would
be close to small value from transition process to steady
Mathematical Problems in Engineering
7
Table 1: Fuzzy control rules for the output 𝑈.
𝐸
𝑈
NB
VS
NS
S
Z
M
PS
B
PB
VB
process. The high gain scale 𝛾𝑘 should tend to reduce and
tend to small constant value; the smaller elements of 𝑄 matrix
should be chosen. Then the magnitude of the elements of
𝑃𝑘,𝑘−1 and 𝐾𝑘 reduced. Consequently, it is expectable that the
gain matrix norm ‖𝐾𝑘 ‖ in the Kalman filter has to be close to
small value for the effective measurement noise smoothing.
A fuzzy controller with one-dimension is developed to
adjust high gain scale 𝛾𝑘 . The inputs of the fuzzy controller
are the variation 𝐸𝜏,𝑘 and its output is high gain scale 𝛾𝑘 .
According to above analysis of Kalman filter adjustment in
transition process and steady state, a set of fuzzy rules to
adjust high gain scale 𝛾𝑘 can be produced. The linguistic
variables assigned to the variation 𝐸𝜏,𝑘 and high gain scale 𝛾𝑘
are 𝐸 and 𝑈, respectively. The corresponding fuzzy subsets are
𝐴 and 𝐵. The set 𝐴 comprises NB, NS, Z, PS, and PB, which
denote negative big, negative small, zero, positive small, and
positive big, respectively. The set 𝐵 comprises VS, S, M, B, and
VB, which denote very small, small, medium, big, and very
big, respectively. The fuzzy rules for input 𝐸 and output 𝑈 are
shown in Table 1.
The fuzzy rule table of high gain scale is generated
according to fuzzy rules offline. The output of fuzzy controller
is got using online enquiry rule table for real time control.
3. Experimental Results
3.1. Prototype of Navigation System. The prototype of navigation system in our lab, which was composed of DSP/FPGA
microcontroller, power module, and MEMS IMU, was developed. The MEMS sensors comprise MEMS IMU of three
single-axis gyroscopes, accelerometers, and triaxis magnetometer. The design of prototype was shown in Figure 2.
Six layers for FPGA/DSP microcontroller and a fourlayer PCB for integration of all MEMS sensors are developed in lab. The FPGA controller is responsible for data
resolution, processing of a single antenna GPS receiver, and
MEMS IMU. The DSP controller is used for processing the
calculation of navigation algorithms in real time. The relating
algorithms were implemented on both Ti TMS320C6713B
and Altera EP2C35F484C6, which were necessary for real
time operation. The parallel bus interface is used for data
communication between DSP and FPGA controller. The size
of embedded navigation system is 𝜙 80 × 30 mm. Micro
IMU provides a higher sensor performance after calibration
procedure and error compensation.
3.2. Calibration Method and Testing. Due to the limitation
of processing technology level, detecting circuit realization,
the low cost Micro IMU is restricted to low precision. MEMS
gyroscope has larger biases and is less accurate. It is sensitive
to environmental effects. The performances of the low cost
MEMS IMU are worse than that of the conventional fibreoptic-based or ring laser gyro inertial sensors. It is essential
MEMS IMU PCB
DSP/FPGA PCB
Figure 2: The prototype of navigation system.
that the calibration and error compensations of Micro IMU
are implemented before attitude estimation. The deterministic errors of low cost Micro IMU, which are composed of
bias, scale factors, nonorthogonality, or misalignment errors,
should be removed. The raw data from Micro IMU could
be preprocessed by means of effective calibration method to
eliminate the effects of these errors.
Firstly, the Micro IMU device was mounted inside of
a metal cylinder. The deterministic error parameters of the
gyroscope were determined depending on calibration experiments by mounting the Micro IMU to single rotation table
or three-axes rotation table in Figure 3. The different biases in
each setting angular speed are calculated by the positive and
negative cancellation method. The scale factor and misalignment errors are also estimated through a similar method.
After the calibration process offline, the deterministic errors
of the raw measurements of gyroscope are compensated
online. For the calibration of accelerometer, the sensitive
axis of accelerometer was aligned in 18 positions, which
comprise 6 orthogonal positions to the ground and the other
12 declined positions. Before calibration, the maximal error
of raw measurement for the accelerometer is within 3𝑔. After
calibration, the bias has been removed utilizing the method
of mean value and the least square method. The compensated
norm of three-axis accelerometer using least square method
has better than method of mean value. Its norm or calibrated
measurements of accelerometer remain close to 1𝑔.
The calibration experiment for magnetometer was also
implemented [22]. The calibration and compensation procedure for the deterministic errors could be done offline by
using rate table before applications. The sensor characteristics
of stochastic errors in low MEMS gyro and accelerometer
could be identified using Allan variance [23]. The raw sensor
data of low cost micro IMU are captured with a sample rate
of more than 100 Hz. Utilizing the comparison of the loglog graph of Allan variance, the random walk that appears
with a gradient of −0.5 is dominant noise for MEMS gyro
and accelerometer. The root mean square random drift errors
of Allan variance are calculated, which would be provided to
IKF as initial process and measurement noises.
3.3. Experimental Setup and Performance Testing. The experimental platform, which comprises the prototype of DSP/
FPGA micro-controller, low cost MEMS sensors, a single
8
Mathematical Problems in Engineering
MEMS IMU
MEMS IMU
Cylinder
45∘
Inclined Block
Three axes rotation table
Figure 3: Calibration experiment of MEMS IMU.
Navigation prototype
Mti-G
Sec. 1
Sec. 2
Satellite antenna
Sec. 3
End
Figure 4: Experimental platform of navigation system.
antenna GPS receiver, and the Xsens Mti-G, is set up. The
vehicle experiment is performed to verify the effect of the
proposed IKF attitude estimation method. It is shown in
Figure 4.
The Xsens Mti-G device from the Xsens Motion Technologies [24], which is a commercially available Attitude
Heading Reference System (AHRS) composed of MEMS
IMU and magnetometer, is used to provide reference attitude
calculation results for comparison. The maximum sampling
frequency 100 Hz is set to collect raw data from Xsens
Mti-G. The GPS receiver works in autonomous mode and
measurements are provided at 10 Hz sampling frequency.
The experimental devices are mounted in car roof. The
experimental data are collected by means of a laptop PC with
the USB port from converting the RS232 serial port.
Vehicle tests were performed to verify the performance
of the proposed method. The experiment was carried out on
the cases that single antenna GPS worked normally, vehicle
was in turning with large sideslip angle, and GPS signals were
affected and lost by obstruction. The Vehicle test routing was
Start
Figure 5: Test field trajectory with single antenna GPS.
chosen in a district of countryside outskirt of city. The test
trajectory is presented in Figure 5.
The start point is located at 123.4479∘ E and 41.6744∘ N.
The results of on-vehicle test are presented from 0 s to100 s.
The performances of traveling in straight and turning sections
are compared considering that the accuracy of attitude
calculation suffered from accelerometer’s signals distortion
noise disturbance and large sideslip angle. The proposed
algorithms are implemented in DSP/FPGA development
prototype. The consuming period of calculation in a sampling
time is less than 10 ms. The vehicle traveled through two
turns during two intervals of 43.3–55.5 s and 68–83 s marked
in yellow line in section 1 and section 2. The GPS signal
was blocked subjectively by man-made near 89 s in order to
verify the effect of the proposed algorithm in GPS outage.
It is marked in red line from 89 to 95 s in section 3. The
Mathematical Problems in Engineering
9
5
40
0
30
Pitch angle (degree)
Roll angle (degree)
−5
−10
−15
−20
−25
10
0
−10
−20
−30
−35
20
0
10
20
30
40
50 60
Time (s)
70
80
90
100
−30
0
20
30
40 50 60
Time (s)
100
70
80
90
100
90
100
Acc
Gyro-Euler
Ref
Gyro-Quaternion
Acc
Gyro-Euler
Ref
Gyro-Quaternion
10
6
5
Yaw angle error (degree)
Yaw angle (degree)
50
0
−50
4
3
2
1
0
−1
−100
0
10
20
30
40
50
60
70
80
90
100
−2
0
10
20
30
Time (s)
Ref
Gyro-Quaternion
40
50
60
70
80
Time (s)
Gps
Gyro-Euler
GPS
Gyro-Euler
Gyro-Quaternion
Figure 6: Attitude estimates from different sensors.
attitudes estimated separately by different sensors, which
comprise MEMS gyroscope, accelerometer, and GPS receiver,
are shown in Figure 6. Data denoted as “Ref,” “GyroQuaternion,” “Acc,” “Gyro-Euler,” and “GPS,” respectively,
are derived from referenced Mti-G, gyroscope integration
based on quaternion algorithm, accelerometer, gyroscope
integration based on Euler algorithm, and the yaw angle
measured by GPS-pseudoattitude algorithm.
Figure 6 shows that the gyro-based attitudes derived from
quaternion and Euler algorithm have short-term accuracy,
which suffers from long-term accumulated errors caused by
gyroscope bias drift with reference to (5) and (16). The attitude algorithm by accelerometer could provide roll and pitch
estimation of long-term accuracy but suffers from sensor
errors and vibration noises due to inevitable disturbances and
dynamic motions with reference to (10). The pseudoattitude
algorithm based on GPS could provide heading angle as
measurement yaw angle via GPS-measured velocity with
reference to (17); its maximal measured error of yaw even
reaches 4.65 degree in turning section. It is seen that the pitch
and roll estimate by accelerometer is better than others. The
error of yaw estimate based on GPS is smaller than others in
straight line (0–43 s). The statistics of attitude angle estimation errors using different sensors are summarized in Table 2.
The estimate attitude accuracy only depending on single
type sensor is not satisfied. Then, it is very vital to fuse these
sensors and design the fusion filtering methods to improve
effectively the estimation performance. According to the
results estimated, the attitude algorithm from accelerometer
could provide pitch and roll angle in low dynamic condition.
The attitude algorithm from gyroscope could estimate Euler
angles for only short-term accuracy. In addition, the method
fusing the yaw angle derived from single antenna GPS and
magnetometer has long-term accuracy.
For attitude estimate of adopting fusion algorithms for
MEMS INS/GPS system, complementary filter (CF) method
based on Wahba’s problem is frequently used for attitude
fusion solution in order to achieve a trade-off between static
and dynamic performances [25]. In most practical cases, the
independent fusion information of different sensors is not
10
Mathematical Problems in Engineering
Table 2: Attitude angle estimation errors using different sensors.
Different sensors
Roll RMS
Pitch RMS
Yaw RMS
Roll STD
Pitch STD
Yaw STD
Gyro-Quaternion
Gyro-Euler
Acc.
GPS
11.7087
15.9387
0.6693
2.7292
12.6745
17.3281
0.6486
3.4792
2.5189
2.9679
8.4046
8.0346
0.3127
0.5783
12.1537
8.5878
0.2969
0.4084
1.6373
1.5683
1.7906
1.535
Table 3: The STD in IKF and EKF.
STD
Sec. 1
Roll
Pitch
Yaw
EKF
IKF
EKF
IKF
EKF
IKF
0.556494
0.526692
0.644948
0.335493
1.317089
0.48039
Sec. 2
1.251228
0.377351
0.782464
0.546662
1.078504
0.283883
Sec. 3
Total
0.469554
0.6502
0.176453
0.3035
0.580741
0.7171
0.116373
0.3347
0.723493
1.4932
0.649624
1.1556
sufficient for accurate estimation. Therefore, Kalman filter
(KF) is proposed in order to take full advantage of attitude
differential equation for reliable estimation. To realize better
refusion estimation, the combining methodology of “Fusion
+ KF” is widely adopted in the literature. The representative methods are categorized as follows. For example, the
federated Kalman filters are designed to the dynamic condition to realize global fusion estimation [17]. It has obvious
advantages over the traditional centralized EKF algorithm,
but heavy calculation burden could not satisfy the demands
of real time processing. The other most widely researched
method is that the attitude estimate using CF fusion algorithm is utilized as the state coefficient [26] or measurement
vector [16, 17, 25] in modified Kalman filter, which could
be regarded as one benchmark algorithm in the field of
GPS and INS data fusion. Among this category algorithm,
the quaternion estimation algorithm (QUEST) [3, 11, 25] is
usually used as attitude algorithm for Micro IMU, which
could remove the adverse effect by linear acceleration of the
navigation system to attain good dynamic performance.
In order to verify the effect of the propose method, the
fusion algorithm based complementary filter for gyroscope
and accelerometer (Fusion Acc&Gyro) and the conventional
EKF method of fix gain are compared in Figure 7. Data
denoted as “Ref,” “Fusion-Complementary,” “EKF-FixGain,”
and “IKF&Fusion,” respectively, are derived from referenced
Mti-G, the CF fusion algorithm for fusing gyroscope and
accelerometer, the conventional EKF method of fix gain, and
the proposed method.
For CF fusion algorithm of gyroscope and accelerometer,
it can be seen that the complementary filter is able to
provide accurate roll and pitch estimate in low dynamic
condition utilizing characters of accelerometers that has longterm accuracy and gyroscope that that has only short-term
accuracy. The accuracy of the pitch and roll angle is corrupted
by the forward acceleration and lateral acceleration. The STD
of the pitch and roll angle is 0.63∘ and 0.64∘ , respectively, at
all times. Considering yaw angle is unable to be estimated by
complementary filter for fusing gyroscope and accelerometer,
it is calculated taking advantage of roll and pitch estimates
of complementary filter in (24). The estimate yaw angle
introduces large error originated from low cost gyro bias
drifts and estimate errors of roll and pitch.
Moreover, the conventional EKF method of fix gain is
implemented in experiments. The same state and measurement equations are adopted in EKF and IKF. The differences
lie in control strategies to deal with vehicle turning and GPS
outage. Additionally, the fix gain is adopted in EKF. The
process and measurement noise covariance matrices are not
tuned in transition process. As observed from yaw estimate
error, the yaw estimation error results of the IKF and EKF
are similar to the reference system in straight line section
(0–43 s), but the EKF has large errors in vehicle turning and
GPS outage. The maximal yaw error of vehicle turning in EKF
is about 4.5 degree due to the influence of large sideslip angle.
The STD of EKF is 0.65∘ , 0.71∘ , and 1.49∘ . The STD of the
proposed IKF for sections 1, 2, and 3 is 0.48∘ , 0.28∘ , and 0.65∘ .
Table 3 lists the STD of estimate errors in EKF and IKF.
The high gain scale is in fast adjustment in transition
process by fuzzy controller in IKF. It is illustrated in Figure 8.
It is shown that the high gain scale 𝛾𝑘 is increased from
initial value 1 to maximum 9.52 in transition process and it
is decreased in steady process. The sampling time 𝑛 in (28)
is assigned to 10, which is used to make sure that all the
measurement data could be given in fuzzy controller. The
high gain scale is adjusted thrice in one innovation. After
the transition process is ended, its value is not adjusted and
stable at 1.32 in steady process. Thus, the IKF algorithm has
the benefits of adaptability which increases the value of 𝛾𝑘
according to the innovation and thus improves its convergence capabilities.
Compared with the above two methods, the proposed
IKF can take the advantages of different fusion strategies
and provide more accurate and reliable estimation whether
in normal condition, unavailable sideslip, and GPS failure.
On the straight trajectory of driving situation, GPS-measured
yaw angle could be reliable to provide accurate measurement
for IKF ignoring the effect of sideslip. In turning sections
(43.3–55.5 s, 68–83 s), the valid values of GPS-measured
vehicle velocity are not convenient to separate from large
Mathematical Problems in Engineering
11
−0.5
5.0
−1.0
4.5
−1.5
4.0
Roll angle (degree)
−2.0
Pitch angle (degree)
−2.5
−3.0
−3.5
−4.0
−4.5
−5.0
3.5
3.0
2.5
2.0
1.5
−5.5
1.0
−6.0
0.5
−6.5
0
10
20
30
40
50
60
Time (s)
Ref
Fusion-Complementary
70
80
90
0.0
100
0
10
20
30
40
50
60
70
80
90
100
Time (s)
Ref
Fusion-Complementary
EKF-Fix Gain
IKF & Fusion
100
EKF-Fix Gain
IKF & Fusion
6
5
Yaw angle error (degree)
Yaw angle (degree)
50
0
−50
4
3
2
1
0
−1
−100
0
10
20
30
40
50
60
70
80
90
100
−2
0
20
Time (s)
Ref
Fusion-Complementary
40
60
80
100
Time (s)
EKF-Fix Gain
IKF & Fusion
EKF-Fix Gain
IKF & Fusion
Fusion-Complementary
Figure 7: Attitude estimates from different fusion algorithms and filters.
10
8
k
6
4
2
0
0
5
10
15
20
Iterative times
25
30
Figure 8: The regulating process of the high gain scale in IKF.
noises due to the decreasing velocity in turning section.
Its value is not accurate. Besides, the larger sideslip angle
occurred, which is the difference between the velocity heading and the vehicle heading. It is not calculated for only
single antenna GPS. Depending on switching fusion strategy,
the yaw estimate by fusing accelerometer, gyroscope, and
magnetometer is used for substituting the GPS-measured yaw
angle as measurement in IKF. The same strategy is adapted
to GPS outage section (89–95 s). The accurate estimation is
provided in turning section or GPS outage.
In order to further validate the proposed method, the
above-mentioned benchmark algorithm based on QUEST is
selected for comparison. The quaternion vector 𝑞 and the
bias of gyroscope 𝑏𝑡 are chosen as state vector considering
the effect of the model error in benchmark algorithm. The
𝑇
state vectors are expressed in 𝑥 = [𝑞 𝑏𝑡 ]7×1 . Depending
on the concept of basic structure of the refusion attitude
information “CF + KF”, the attitude estimates based on CF
fusion algorithm are utilized as filter measurement to correct
the state quaternion vector 𝑞 and the bias of gyroscope
12
Mathematical Problems in Engineering
1.5
1.0
0.5
Pitch angle error (degree)
Roll angle error (degree)
1.0
0.5
0.0
−0.5
−1.0
0.0
−0.5
−1.0
−1.5
−1.5
0
20
40
60
80
100
0
20
40
Time (s)
60
80
100
Time (s)
Benchmark algorithm
IKF & Fusion
Yaw angle error (degree)
Benchmark algorithm
IKF & Fusion
5.5
5.0
4.5
4.0
3.5
3.0
2.5
2.0
1.5
1.0
0.5
0.0
−0.5
−1.0
−1.5
0
20
40
60
80
100
Time (s)
Benchmark algorithm
IKF & Fusion
Figure 9: Attitude estimate errors of the proposed method and benchmark algorithm.
𝑏𝑡 in EKF. The fusion attitudes 𝜙fusion , 𝜃fusion , which have
been calculated by CF method based on gyroscope and
accelerometer, would be chosen as the measurement vector of
filter. Due to the reliable yaw estimation by the GPS receiver,
the GPS-measured yaw angle 𝜓GPS by the pseudoattitude
method is also chosen as measurement vector in filter. Here,
𝑇
the measurement vector 𝑧 = [𝜙fusion 𝜃fusion 𝜓GPS ] . Figure 9
displays the attitude estimation errors.
It is inferred from Figure 9, due to the reliable refusion
information as the measurements (𝜙fusion , 𝜃fusion ), selected
benchmark algorithms based on QUEST and the proposed
method have similar low dynamic performance for estimating the roll and pitch angle. The RMS of roll and pitch in
IKF is 0.31∘ and 0.33∘ , respectively. The RMS of roll and
pitch in benchmark algorithm is 0.66∘ and 0.6∘ , respectively.
The precision of the proposed method is slightly better
than benchmark algorithm. For yaw estimation, benchmark
algorithm becomes worse in turning sections (43.3–55.5 s,
68–83 s) owing to the effect of gyro drift in quaternion calculation. In the GPS outage section (89–95 s), the estimated
value of last sampling time 𝜓̂𝑘−1 is used for replacing the 𝜓GPS
in the measurement process. The precision of benchmark
algorithm is unaffected due to short GPS outage time. RMS
of benchmark algorithm is 2∘ and RMS of independent CF
method is 2.97∘ . Benchmark algorithm attains better performance than only CF method for yaw estimation. Compared
with the performance of turning sections and GPS outage
section, the proposed IKF gets remarkable improvement
due to the tuning of high gain scale in filter. It maintains
satisfactory estimation performance in whole section. The
STD and RMS for the various methods tested in attitude
estimation experiments are summarized in Table 4.
Compared with different sensors, fusion algorithm of
complementary filter, the conventional EKF, and benchmark
algorithm, it can provide relatively accurate and reliable
attitude estimation. The STD of the yaw estimation errors is
Mathematical Problems in Engineering
13
Table 4: Attitude angle estimation errors using fusion and filter.
Fusion algorithm
EKF-Fix Gain
Fusion-Complementary (CF)
Benchmark algorithm
IKF and Fusion
Roll RMS
0.6764
0.6377
0.6603
0.3059
Pitch RMS
0.7324
0.6442
0.6001
0.3337
almost within 1.15∘ ; roll and pitch estimation errors are almost
within 0.33∘ in whole section.
4. Conclusions
The combined attitude determination approach with fusion
estimation algorithm and intelligent Kalman filter on low cost
MEMS IMU and magnetometer and single antenna GPS is
proposed. The deterministic errors of low cost MEMS sensors
are eliminated effectively before attitude estimation through
calibration and error compensation. The different control
strategies fusing the MEMS multisensors are implemented in
case of normal driving, GPS failure, and unavailable sideslip.
An independently development prototype is designed for
experimental tests. The proposed method provide reliable
yaw estimate as measurement for IKF observer, particularly
unavailable larger sideslip angle and GPS failure.
The performance of the proposed methodology has been
evaluated and verified through compared experiments for
different sensors attitude algorithms and fusion methods. The
proposed method is compared with traditional representative
methods, which comprises the gyro based on quaternion,
the gyro based on Euler, accelerometer, GPS-pseudoattitude
algorithm and fusion algorithm based complementary filter,
benchmark algorithm based on QUEST, and so on. The
experimental results indicate that the proposed method
exhibits reliable and satisfactory performance.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work is supported by Shenyang Science and Technology
Plan Project, China. Corresponding experimental tests are
carried out at Research Centre of Weaponry Science and
Technology in Shenyang Ligong University. The authors
would like to thank their colleagues for their support in
implementing the experiments.
References
[1] Z. Wu, M. Yao, H. Ma, W. Jia, and F. Tian, “Low-cost antenna
attitude estimation by fusing inertial sensing and two-antenna
GPS for vehicle-mounted satcom-on-the-move,” IEEE Transactions on Vehicular Technology, vol. 62, no. 3, pp. 1084–1096, 2013.
[2] J. F. Vasconcelos, C. Silvestre, and P. Oliveira, “INS/GPS aided
by frequency contents of vector observations with application to
autonomous surface crafts,” IEEE Journal of Oceanic Engineering, vol. 36, no. 2, pp. 347–363, 2011.
Yaw RMS
1.7355
2.9679
2.006
1.3049
Roll STD
0.6502
0.2359
0.2636
0.3035
Pitch STD
0.7171
0.2539
0.2912
0.3347
Yaw STD
1.4932
1.5691
1.6940
1.1556
[3] Y.-C. Lai and S.-S. Jan, “Attitude estimation based on fusion of
gyroscopes and single antenna GPS for small UAVs under the
influence of vibration,” GPS Solutions, vol. 15, no. 1, pp. 67–77,
2011.
[4] W. Chou, B. Fang, L. Ding, X. Ma, and X. Guo, “Two-step optimal filter design for the low-cost attitude and heading reference
systems,” IET Science, Measurement and Technology, vol. 7, no.
4, pp. 240–248, 2013.
[5] F. Qin, X. Zhan, and L. Zhan, “Performance assessment of a lowcost inertial measurement unit based ultra-tight global navigation satellite system/inertial navigation system integration for
high dynamic applications,” IET Radar, Sonar and Navigation,
vol. 8, no. 7, pp. 828–836, 2014.
[6] H. Huang, X. Chen, and J. Zhang, “Weight Self-adjustment
Adams Implicit Filtering Algorithm for Attitude Estimation
Applied to Underwater Gliders,” IEEE Access, vol. PP, no. 99,
2016.
[7] R. Zhang, F. Hoflinger, and L. M. Reind, “Calibration of an IMU
using 3-D rotation platform,” IEEE Sensors Journal, vol. 14, no.
6, pp. 1778–1787, 2014.
[8] Z. Zhang and G. Yang, “Calibration of miniature inertial and
magnetic sensor units for robust attitude estimation,” IEEE
Transactions on Instrumentation & Measurement, vol. 63, no. 3,
pp. 711–718, 2014.
[9] P. Aggarwal, Z. Syed, X. Niu, and N. El-Sheimy, “A standard
testing and calibration procedure for low cost MEMS inertial
sensors and units,” The Journal of Navigation, vol. 61, no. 2, pp.
323–336, 2008.
[10] S. Wei, G. Dan, and H. Chen, “Altitude data fusion utilising differential measurement and complementary filter,” IET Science,
Measurement & Technology, vol. 10, no. 8, pp. 874–879, 2016.
[11] M. B. Del Rosario, N. H. Lovell, and S. J. Redmond, “Quaternion-Based Complementary Filter for Attitude Determination
of a Smartphone,” IEEE Sensors Journal, vol. 16, no. 15, pp. 6008–
6017, 2016.
[12] H. No, A. Cho, and C. Kee, “Attitude estimation method for
small UAV under accelerative environment,” GPS Solutions, vol.
19, no. 3, pp. 343–355, 2014.
[13] J.-H. Yoon, S. Eben Li, and C. Ahn, “Estimation of vehicle
sideslip angle and tire-road friction coefficient based on magnetometer with GPS,” International Journal of Automotive Technology, vol. 17, no. 3, pp. 427–435, 2016.
[14] K. D. Sebesta and N. Boizot, “A real-time adaptive high-gain
EKF, applied to a quadcopter inertial navigation system,” IEEE
Transactions on Industrial Electronics, vol. 61, no. 1, pp. 495–503,
2014.
[15] Z. Wu, Z. Sun, W. Zhang, and Q. Chen, “A Novel Approach
for Attitude Estimation Based on MEMS Inertial Sensors Using
Nonlinear Complementary Filters,” IEEE Sensors Journal, vol.
16, no. 10, pp. 3856–3864, 2016.
[16] F. Hoflinger, J. Muller, and M. Tork, “A wireless micro inertial
measurement unit (IMU),” IEEE Transactions on Instrumentation & Measurement, vol. 62, no. 9, pp. 2583–2595, 2013.
14
[17] X. Li, C.-Y. Chan, and Y. Wang, “A Reliable Fusion Methodology for Simultaneous Estimation of Vehicle Sideslip and Yaw
Angles,” IEEE Transactions on Vehicular Technology, vol. 65, no.
6, pp. 4440–4458, 2016.
[18] M. M. Teshnizi and A. Shirazi, “Attitude estimation and sensor
identification utilizing nonlinear filters based on a low-cost
mems magnetometer and sun sensor,” Aerospace & Electronic
Systems Magazine, vol. 30, no. 12, pp. 20–33, 2015.
[19] H. Jie, H. Xianlin, and W. Guofeng, “Design and application
of single-antenna GPS/accelerometers attitude determination
system,” Journal of Systems Engineering and Electronics, vol. 19,
no. 2, pp. 220–227, 2008.
[20] R. P. Kornfeld, H. R. John, and J. J. Deyst, “Single-antenna GPSbased aircraft attitude determination,” Navigation: Journal of the
Institute of Navigation, vol. 45, no. 1, pp. 51–60, 1998.
[21] O. S. Salychec, Applied Inertial Navigation: Problem and Solutions, BMSTU Press, 2004.
[22] L. Wang and F. Wang, “Intelligent calibration method of low
cost mems inertial measurement unit for an FPGA-based navigation system,” International Journal of Intelligent Engineering
& Systems, vol. 4, no. 2, pp. 32–41, 2011.
[23] N. El-Sheimy, H. Y. Hou, and X. J. Niu, “Analysis and modeling
of inertial sensors using allan variance,” IEEE Transactions on
Instrumentation and Measurement, vol. 57, no. 1, pp. 140–149,
2008.
[24] http://www.xsens.com/products/mit-g-710.
[25] H. Rong, J. Lv, C. Peng et al., “Dynamic Regulation of the
Weights of Request Based on the Kalman Filter and an Artificial
Neural Network,” IEEE Sensors Journal, vol. 16, no. 23, pp. 8597–
8607, 2016.
[26] D. Nemec, A. Janota, M. Hrubos, and V. Simak, “Intelligent
Real-Time MEMS Sensor Fusion and Calibration,” IEEE Sensors
Journal, vol. 16, no. 19, pp. 7150–7160, 2016.
Mathematical Problems in Engineering
Advances in
Operations Research
Hindawi Publishing Corporation
http://www.hindawi.com
Volume 2014
Advances in
Decision Sciences
Hindawi Publishing Corporation
http://www.hindawi.com
Volume 2014
Journal of
Applied Mathematics
Algebra
Hindawi Publishing Corporation
http://www.hindawi.com
Hindawi Publishing Corporation
http://www.hindawi.com
Volume 2014
Journal of
Probability and Statistics
Volume 2014
The Scientific
World Journal
Hindawi Publishing Corporation
http://www.hindawi.com
Hindawi Publishing Corporation
http://www.hindawi.com
Volume 2014
International Journal of
Differential Equations
Hindawi Publishing Corporation
http://www.hindawi.com
Volume 2014
Volume 2014
Submit your manuscripts at
https://www.hindawi.com
International Journal of
Advances in
Combinatorics
Hindawi Publishing Corporation
http://www.hindawi.com
Mathematical Physics
Hindawi Publishing Corporation
http://www.hindawi.com
Volume 2014
Journal of
Complex Analysis
Hindawi Publishing Corporation
http://www.hindawi.com
Volume 2014
International
Journal of
Mathematics and
Mathematical
Sciences
Mathematical Problems
in Engineering
Journal of
Mathematics
Hindawi Publishing Corporation
http://www.hindawi.com
Volume 2014
Hindawi Publishing Corporation
http://www.hindawi.com
Volume 2014
Volume 2014
Hindawi Publishing Corporation
http://www.hindawi.com
Volume 2014
#HRBQDSDĮ,@[email protected]
Journal of
Volume 201
Hindawi Publishing Corporation
http://www.hindawi.com
Discrete Dynamics in
Nature and Society
Journal of
Function Spaces
Hindawi Publishing Corporation
http://www.hindawi.com
Abstract and
Applied Analysis
Volume 2014
Hindawi Publishing Corporation
http://www.hindawi.com
Volume 2014
Hindawi Publishing Corporation
http://www.hindawi.com
Volume 2014
International Journal of
Journal of
Stochastic Analysis
Optimization
Hindawi Publishing Corporation
http://www.hindawi.com
Hindawi Publishing Corporation
http://www.hindawi.com
Volume 2014
Volume 2014
Was this manual useful for you? yes no
Thank you for your participation!

* Your assessment is very important for improving the work of artificial intelligence, which forms the content of this project

Download PDF

advertisement