GPS/INS integrated navigation system for autonomous robot

ar estimator. It combines the INS es-
timation results with the GPS results to estimate the
position, velocity, attitude error and the IMU sensor’s
error. The second is the GPS filter. It uses the pseu-
dorange and Doppler measurement values from GPS
module to determine the position, velocity. Figure 3
shows the diagram of loosely coupled model.
In today’sGPSmodules, there is usually a built-inGPS
data processor, which can calculate position, velocity
and some other information from GPS raw data. In
the LC model, position and velocity are fed into the
nonlinear filter. The filter used in this paper is the Ex-
tended Kalman filter, which is suitable for nonlinear
systems. Measurement values from IMU sensor (an-
gular rate and acceleration) after being computed us-
ing the Euler angles estimation and INS mechaniza-
tion, will be compared with the position and veloc-
ity of the GPS. The difference between two results is
used as the input of the EKF. The integrated system
works in closed loop, the estimated error values are
fed back to adjust the state of the INS system and to
compensate for the IMU measurements. This close-
loop model is suitable for MEMS IMU, which has
large disturbance.
The error state vector dx of the EKFfilter in thismodel
is composed of the position error d rn, the velocity er-
ror dvn, the attitude error e , the acceleration bias er-
ror dba and the gyroscope bias error dbg. Derive the
INS mechanization function and take the first order
elements3, we have the process model equation:
d :x= F:dx+G:u (4)
where:
F =
2666664
Frr Frv 03x3 03x3 03x3
Fvr Fvv ( f nx) Cnb 03x3
Fer Rev (wninx) 03x3 Cnb
03x3 03x3 03x3 1=tba 03x3
03x3 03x3 03x3 03x3 1=tbg
3777775 (5)
G=
2666664
03x3 03x3 03x3 03x3
Cnb 03x3 03x3 03x3
03x3 Cnb 03x3 03x3
03x3 03x3 I3x3 03x3
03x3 03x3 03x3 I3x3
3777775 (6)
In matrix F; tba and tbg are the correlation time vec-
tors of accelerometers and gyroscopes, determined
based on the Gauss-Markov model. The components
of vector u are white noises, with the covariance de-
termined by the formula:
Q= diag
h
qa qg qba qbg
i
; q= 2s
2
t (7)
In the above formula, s is the standard deviation of
the Gauss-Markov noise. Matrix Q is called the spec-
tral density matrix and its component, respectively,
are covariance accelerometer, gyroscope, accelerom-
eter bias and gyroscope bias. These values can be de-
termined in the datasheet of the sensor5.
The measurement model of the EKF is the difference
of the INS results (position and velocity) and GPS re-
sults:
d z=
"
rnINS rnGPS
vnINS vnGPS
#
= H:dx+ e (8)
In the above equation, symbol e is the measurement
noise. Its covariance matrix R can be obtained from
GPS processing. The activation of the EKF is divided
into 2 stages: update and prediction. TheKalman gain
is computed first in the update stage. Then state vari-
ables (dx) and error covariance (P) are updated based
on prior estimates and its error covariance. After each
correction, the error state vector should be reset to
zero.
When there is a GPS outage, we can use velocity con-
straints (Figure 4) to estimate errors4. Vehicles essen-
tially move in forward direction. If the vehicle does
not jump off the ground nor slide on the ground, its
velocity in the axes perpendicular to the forward di-
rection (y-axis and z-axis) is almost zero. So we have
two velocity constraints:(
vby  0
vbz  0
(9)
SIMULATION RESULTS
In simulation, we use FlightGear simulation
software7 to create the data file and use MAT-
LAB/Simulink to process it. The GPS signal is
disturbed with noise to research about noise suppres-
sion of the estimator. The standard deviation of noise
is 2.5 m in each horizontal axis and 5 m in vertical
axis. Simulations were made in two cases: with and
without the Euler angles estimator. We have the
result Table 1:
From the above table, we can see that with the rotation
angles estimator, the results are better. The horizontal
SI31
Science & Technology Development Journal – Engineering and Technology, 2(SI1):29-37
Figure 3: Loosely-coupledmodel with 15-state vector.
Table 1: Attitude, position and velocity errors
Error Without Euler angles estimation With Euler angles estimation
Attitude
(r-p-y)
(degree)
24.7 48.6 14.8 2.0 2.0 2.0
Position
NED (m)
0.69 1.54 1.62 0.43 0.47 2.99
Velocity
NED (m/s)
1.80 3.46 4.61 0.05 0.05 0.04
accuracy is about 0.64 meters. The velocity error is
within 0.1 m/s. We can conclude that the estimator
has good filtering capability. Next, we will examine
the quality of the system when the GPS signal is lost
in intervals of 3, 5 and 10 seconds.
From Table 2, we can conclude that when there is a
GPS outage, the error of the systemwill be larger than
the normal case (GPS fix). In addition, if the GPS lost
time is longer, the horizontal error is larger. Using an
Euler angles estimator helps to make smaller errors.
EXPERIMENTAL RESULTS AND
DISCUSSION
Hardware development
We built an experimental system to verify the imple-
mented algorithm. The hardware (Figure 5) consists
of the IMU sensorADIS16405 fromAnalogDevices 8,
the GPS module from U-blox9 and the microcon-
troller STM32F407 (ARM Cortex-M4) from STMi-
croelectronics10.
The reference system is the GNSS/INS system from
Xsens Technology. The MTi-G-70011 can give rota-
tion angles estimate with a 1degree accuracy, position
error of 2 meters and velocity error of 0.05 m/s.
The update rate of INS and GPS are 100 Hz and 10
Hz, respectively. In each INS cycle of 10 milliseconds,
the STM32F407 microcontroller reads data from the
IMU sensor and the GPS module. Then update the
error estimator, using Extended Kalman filter algo-
rithm. Navigation data is sent to the computer via
COM/RS232 port or via SD card.
Results
For MEMS IMU sensors, the amplitude of its noise is
huge, so if we do not use the Euler angles estimator,
the result is bad, the attitude, position, velocity errors
are enormous. The estimated trajectory (red dots in
Figure 6) does not have the same shape with the ref-
erence one (black line). In contrast, when we use the
angles estimator, the errors are smaller, the accuracy
is higher. The horizontal error of our GPS/INS sys-
tem is 1.69 m, while the error of the individual GPS
system is 1.93 m. For this reason, the GPS/INS algo-
rithm can reduce over 10% of the error. On the other
hand, the update rate of GPS is only 10 Hz. The in-
tegrated GPS/INS update rate is 10 times larger (100
Hz). We can see the effective of high update rate in
Figure 6. Because the GPS has the low update rate
of 10 Hz, there are visible spaces between the green
dots (GPS-only). And if the vehicle moves very fast,
the GPS cannot describe the vehicle’s trajectory accu-
rately. Differently, the blue dots (GPS/INS) approx-
SI32
Science & Technology Development Journal – Engineering and Technology, 2(SI1):29-37
Table 2: Horizontal accuracy during GPS outages
GPS outage Without Euler angles estimation With Euler angles estimation
3 seconds 15.0 m 0.88 m
5 seconds 35.8 m 0.92 m
10 seconds 171.4 m 1.33 m
Figure 4: EKF with velocity constraints.
Figure 5: GPS/INS system in experiments.
Table 3: Attitude, position and velocity errors
Error Without Euler angles estimation With Euler angles estimation
Attitude
(r-p-y)
(degree)
70 29 83 1.31 1.07 16.7
Position
NED (m)
11 7.7 313 1.58 0.58 4.42
Velocity
NED (m/s)
10 6.4 85 0.27 0.26 0.46
SI33
Science & Technology Development Journal – Engineering and Technology, 2(SI1):29-37
Figure 6: Estimated 2D position.
imately form a continuous line. From the above re-
sults, it can be concluded that the angles estimator can
improve the accuracy of the navigation system and the
integrated GPS/INS system performs better than the
single GPS system (Table 3).
Next, assuming the GPS signal is lost for a period of 5
seconds, we will analyze the accuracy of implemented
GPS/INS system in cases with andwithout speed con-
straints. We will simulate GPS outages in two cases:
GPS lost in straight line and in curved line (Table 4).
From the above results, we can see in the normal case
of GPS fix, the velocity constraints can still reduce the
horizontal error of the system. When there is a GPS
outage, using speed constraints can either increase or
decrease the system’s accuracy. However, it restricts
the trajectory from divergence. The blue dots in Fig-
ures 7 and 8 can follow the reference trajectory, while
the red dots are diverging. Thus, velocity constraints
are also a tool that can improve the accuracy of the
integrated GPS/INS navigation system.
CONCLUSIONS
In this paper, we have implemented a loosely coupled
GPS/INS integrated navigation system. The main al-
gorithm in this system is the Extended Kalman filter.
We combined the EKF with Euler angles estimator
and velocity constraints to improve accuracy.
The rotation angles estimator helps us to determine
the Euler angles correctly, thereby increasing the
quality of the position and velocity estimation. In
practice, the accuracy of roll and pitch angle is 2 de-
grees, the error of yaw angle is still large.
The achieved horizontal accuracy is 2mwhen theGPS
signal is stable and 3m when the GPS signal is lost in
a short period. Compared with individual GPS, the
error of the integrated system is about 10% smaller.
In addition, the positive point of the GPS/INS is its
update rate reaches 100 Hz, which is 10 times larger
than the initial system. When there is a long-period
GPS outage, the LC algorithm’s result is very bad, so
we need to use the tightly coupled model. In the fu-
ture, we will research about this model, point out its
advantages and disadvantages, and compare with the
original model. After that, we will find the optimal
switching method between two models.
SI34
Science & Technology Development Journal – Engineering and Technology, 2(SI1):29-37
Table 4: Horizontal accuracy when GPS lost, with and without velocity constraints
Horizontal error (m)
GPS fix 5 seconds GPS lost,
straight line
5 seconds GPS lost, curved
line
Without velocity con-
straints
1.69 2.23 2.60
With velocity con-
straints
1.59 2.76 1.96
Figure 7: GPS outage – straight line.
Figure 8: GPS outage – Curved line.
SI35
Science & Technology Development Journal – Engineering and Technology, 2(SI1):29-37
ACKNOWLEDGEMENT
This research is supported by National Key Labora-
tory of Digital Control and System Engineering (DC-
SELAB), HCMUT and funded by Vietnam National
University Ho Chi Minh city (VNU-HCM) under
grant number C2018-20b-02.
CONFLICT OF INTERESTS
The author declares that this paper has no conflict of
interests.
AUTHORS’ CONTRIBUTIONS
Tran Ngoc Huy has developed the proposed algo-
rithm and wrote the manuscript. Le Manh Cam and
NguyenThanh Nam implemented simulation, exper-
iment and wrote the manuscript.
ABBREVIATIONS
USV: Unmanned Surface Vehicle
UAV: Unmanned Aerial Vehicle
AUV: Autonomous Underwater Vehicle
GPS: Global Positioning System
INS: Inertial Navigation System
LC: Loosely Coupled
TC: Tightly Coupled
EKF: Extended Kalman Filter
REFERENCES
1. Kim K. GPS/INS with non-linear filters. ICCAS 2011, KINTEX,
Gyeonggi-do, Korea. 2011;.
2. Schmidt GT, Phillips RE. INS/GPS Integration Architectures.
RTO Lecture Series RTO-EN-SET-116. 2010;.
3. Shin EH. Accuracy Improvement of LowCost INS/GPS for Land
Applications. University of Calgary. 2001;.
4. Angrisano A. GNSS/INS IntegrationMethods. PhD Thesis, The
Parthenope University of Naples, Italy. 2010;.
5. Nguyen HD, et al. Implementation of a GPS/INS tightly-
coupled system in urban environment. The 3rd Vietnam Con-
ference on Control and Automation, Ho Chi Minh city. 2015;.
6. Tran NH, Le MC. Orientation estimation using extended
Kalman filter. The 4th Conference on Science and Technology,
HCMC University of Transport. 2018;.
7. FlightGear Flight Simulator ;Available from: 
flightgear.org/.
8. AnalogDevices. ADIS16405 - Triaxial Inertial SensorwithMag-
netometer. 2017;.
9. u-blox AG. NEO-M8, u-blox M8 concurrent GNSS modules.
2016;.
10. STMicroelectronics, Datasheet STM32F405xx - STM32F407xx.
2016;.
11. Xsens Technologies, MTi User Manual: MTi 10-series and MTi
100-series 5th generation. 2018;.
SI36
Tạp chí Phát triển Khoa học và Công nghệ – Kĩ thuật và Công nghệ, 2(SI1):SI29-SI37
Open Access Full Text Article Bài Nghiên cứu
1Trường Đại học Bách khoa,
ĐHQG-HCM
2Phòng Thí nghiệm Trọng điểm Điều
khiển số và Kỹ thuật Hệ thống,
ĐHQG-HCM
Liên hệ
Trần Ngọc Huy, Trường Đại học Bách khoa,
ĐHQG-HCM
Email: tnhuy@hcmut.edu.vn
Lịch sử
 Ngày nhận: 15/10/2018
 Ngày chấp nhận: 25/12/2018
 Ngày đăng: 31/12/2019
DOI : 10.32508/stdjet.v3iSI1.720
Bản quyền
© ĐHQG Tp.HCM. Đây là bài báo công bố
mở được phát hành theo các điều khoản của
the Creative Commons Attribution 4.0
International license.
Xây dựng hệ thống định vị tích hợp GPS/INS cho robot tự hành
Trần Ngọc Huy1,*, Lê Mạnh Cầm1, Nguyễn Thanh Nam2
Use your smartphone to scan this
QR code and download this article
TÓM TẮT
Các loại robot tự hành có khả năng thay thế con người làm những công việc nặng nhọc, ở những
môi trường khó khăn, nguy hiểm, vì vậy lĩnh vực này hiện đang rất phát triển. Một trong những vấn
đề quan trọng trong việc điều khiển các loại robot tự hành đó là xác định vị trí hiện tại của robot.
Hệ thống định vị toàn cầu (GPS) được sử dụng rộng rãi trong lĩnh vực này, tuy nhiên có một số
nhược điểm như tốc độ cập nhật thấp hoặc đôi khi mất tín hiệu từ các vệ tinh. Một hệ thống điều
hướng khác là Hệ thống dẫn đường quán tính (INS) có thể cho phép ta xác định vị trí, vận tốc và
góc xoay của robot. Tuy nhiên, INS khi tính toán sẽ làm sai số tích lũy theo thời gian. Một phương
pháp hiệu quả là tích hợp GPS với INS, trong đó trái tim của hệ thống là công cụ ước tính phi tuyến
(ví dụ: bộ lọc Kalmanmở rộng) từ đó có thể khắc phục được khuyết điểm của hệ thống GPS và INS
so với khi tích hợp riêng rẽ. Bài báo này giới thiệu về phương pháp tích hợp lỏng GPS/INS, sử dụng
bộ ước lượng góc xoay ba trục và ràng buộc vận tốc để cải thiện độ chính xác. Giải thuật được thực
nghiệm kiểm chứng trên cảm biến quán tính giá rẻ gồm con quay hồi chuyển, cảm biến gia tốc,
từ trường ba trục và cảm biến GPS. Kết quả cho thấy sai số của góc roll và pitch là 2 độ, sai số của
góc yaw vẫn còn lớn. Độ chính xác theo phương ngang đạt được là 2m khi tín hiệu GPS ổn định
và 3 m khi tín hiệu GPS bị mất trong một khoảng thời gian ngắn. So với hệ thống GPS riêng lẻ, sai
số của hệ thống tích hợp nhỏ hơn khoảng 10%.
Từ khoá: Robot tự hành, Tích hợp định vị/hệ thống định vị quán tính, thiết bị đo lường quán tính
Trích dẫn bài báo này: Huy T N, Cầm L M, Nam N T. Xây dựng hệ thống định vị tích hợp GPS/INS cho
robot tự hành. Sci. Tech. Dev. J. - Eng. Tech.; 2(SI1):SI29-SI37.
SI37