Open Access

Experimental Validation: Perception and Localization Systems for Autonomous Vehicles using the Extended Kalman Filter Algorithm

Bambang Lelono Widjiantoro
Bambang Lelono Widjiantoro
, 
Katherin Indriawati
Katherin Indriawati
, 
T. S. N. Alexander Buyung
T. S. N. Alexander Buyung
 and 
Kadek Dwi Wahyuadnyana
Kadek Dwi Wahyuadnyana
   | Feb 07, 2024
International Journal on Smart Sensing and Intelligent Systems's Cover Image
About this article
Previous Article
Next Article
Cite
Article Category: Article
Published Online: Feb 07, 2024
Page range: -
Received: Aug 28, 2023
DOI: https://doi.org/10.2478/ijssis-2024-0002
Keywords
© 2024 Bambang Lelono Widjiantoro et al., published by Sciendo
This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivatives 4.0 International License.

Figure 1:

The hardware wiring and schematics.
The hardware wiring and schematics.

Figure 2:

The physical look of a robot.
The physical look of a robot.

Figure 3:

The scenario of physical world and measurement.
The scenario of physical world and measurement.

Figure 4:

Encoders' schematic diagram.
Encoders' schematic diagram.

Figure 5:

The Localization and perception block diagram. ROS, robot operating system.
The Localization and perception block diagram. ROS, robot operating system.

Figure 6:

The map generated using the Hector-SLAM algorithm. SLAM, Simultaneous localization and mapping.
The map generated using the Hector-SLAM algorithm. SLAM, Simultaneous localization and mapping.

Figure 7:

Comparison of EKF position estimation results with references. During the experiment, we focus on two coordinates as the mobile robot moves—namely, the X and Y coordinates. Label A represents the robot's motion along the X coordinate, while label B corresponds to its movement along the Y coordinate. EKF, extended Kalman filter.
Comparison of EKF position estimation results with references. During the experiment, we focus on two coordinates as the mobile robot moves—namely, the X and Y coordinates. Label A represents the robot's motion along the X coordinate, while label B corresponds to its movement along the Y coordinate. EKF, extended Kalman filter.

Figure 8:

RViz visualization.
RViz visualization.

The EKF parameter details.

Parameter Information
β^k− \hat \beta_k^- The estimated state
f Non-linear model system
h Measurement model system
vk Measurement noise
zk IMU measurement
Wk − 1 Dynamic system noise
β^k−1+ \hat \beta_{k - 1}^+ The estimated updates state
V The linear velocity of the robot
Δt Time derivative
θ The steering angle of the robot
Qk Dynamic system noise matrix Q=[10−40000010−40000010−40000010−40000010−4] Q = \left[ {\matrix{{{{10}^{- 4}}} \hfill & 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill \cr 0 \hfill & {{{10}^{- 4}}} \hfill & 0 \hfill & 0 \hfill & 0 \hfill \cr 0 \hfill & 0 \hfill & {{{10}^{- 4}}} \hfill & 0 \hfill & 0 \hfill \cr 0 \hfill & 0 \hfill & 0 \hfill & {{{10}^{- 4}}} \hfill & 0 \hfill \cr 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill & {{{10}^{- 4}}} \hfill \cr}} \right]
Hk IMU measurement matrix Hodometry+IMU=[1000001000001000001000001] {H_{{\rm{odometry+}}IMU}} = \left[ {\matrix{1 \hfill & 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill \cr 0 \hfill & 1 \hfill & 0 \hfill & 0 \hfill & 0 \hfill \cr 0 \hfill & 0 \hfill & 1 \hfill & 0 \hfill & 0 \hfill \cr 0 \hfill & 0 \hfill & 0 \hfill & 1 \hfill & 0 \hfill \cr 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill & 1 \hfill \cr}} \right]
Rk Measurement noise matrix R=[10−40000010−40000010−40000010−40000010−4] R = \left[ {\matrix{{{{10}^{- 4}}} \hfill & 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill \cr 0 \hfill & {{{10}^{- 4}}} \hfill & 0 \hfill & 0 \hfill & 0 \hfill \cr 0 \hfill & 0 \hfill & {{{10}^{- 4}}} \hfill & 0 \hfill & 0 \hfill \cr 0 \hfill & 0 \hfill & 0 \hfill & {{{10}^{- 4}}} \hfill & 0 \hfill \cr 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill & {{{10}^{- 4}}} \hfill \cr}} \right]
Fk Jacobians matrix, Fk=[10−Vk−1ΔtsinθkΔtcosθk001Vk−1ΔtcosθkΔtsinθk00010Δt0001000001] {F_k} = \left[ {\matrix{1 \hfill & 0 \hfill & {- {V_{k - 1}}\Delta t\sin {\theta_k}} \hfill & {\Delta t\cos {\theta_k}} \hfill & 0 \hfill \cr 0 \hfill & 1 \hfill & {{V_{k - 1}}\Delta t\cos {\theta_k}} \hfill & {\Delta t\sin {\theta_k}} \hfill & 0 \hfill \cr 0 \hfill & 0 \hfill & 1 \hfill & 0 \hfill & {\Delta t} \hfill \cr 0 \hfill & 0 \hfill & 0 \hfill & 1 \hfill & 0 \hfill \cr 0 \hfill & 0 \hfill & 0 \hfill & 0 \hfill & 1 \hfill \cr}} \right]

The steps of the EKF-SLAM Algorithm.

EKF-SLAM Algorithm

The initial step is to initialize the previous estimated value (b_prev) and the previous covariance error (p_prev) with a value of 0.

Initialize the predicted state (b_new) based on Eq. (1)

Initialize the prediction error covariance (p_new) based on Eq. (2)

Obtain the optimal gain (K) based on Eq. (3)

Obtain an estimate of the state of the update (b) based on Eq. (4)

The estimation of the state of the update is the result of the KF displayed

Get the updated covariance error (K) based on Eq. (5)

The updated state estimate value is stored as the previous estimate, and then the state estimation algorithm returns to step 2

The error covariance update value is stored as the prior covariance, and then the error covariance algorithm returns to step 3.

The comparison of the encoder results and the reference in X-axis.

X position from odometry X reference Error
0.95 0.97 0.02
0.95 0.97 0.03
0.95 0.97 0.03
0.95 0.97 0.03
0.95 0.98 0.03
0.95 0.98 0.04
0.95 0.98 0.04
0.95 0.99 0.04
0.95 0.99 0.04
0.95 1.00 0.05
Error in average (%) 3

The details of robot parameters.

Parameter Value
Battery 12.6 V (DC)
Dimension 242.9 × 192.2 mm
Steering servo 9 kg/cm torque
Wheel motor 240 RPM
Wireless 2.4G/5G dual-band WIFI, Bluetooth 4.2
Driving type Ackerman steering dual gearmotor rear wheel drive

Comparison between the EKF positions and the reference.

X position (EKF) X reference Y position (EKF) Y reference Error X Error Y
0.97 0.97 0.02 0.02 0.00 0.00
0.98 0.97 0.02 0.02 0.01 0.00
0.98 0.97 0.02 0.02 0.01 0.01
0.98 0.97 0.02 0.02 0.01 0.01
0.99 0.98 0.02 0.02 0.01 0.02
0.99 0.98 0.02 0.02 0.01 0.04
1.00 0.98 0.02 0.02 0.02 0.04
1.01 0.99 0.02 0.02 0.02 0.04
1.01 0.99 0.02 0.02 0.02 0.04
1.02 1.00 0.02 0.02 0.02 0.05
Error in average (%) 1 3
RMSE 0.11 0.15

Accelerometer measurements.

X Y X reference Y reference X error Y error
0.02 −0.02 0 0 0.02 −0.02
0.01 −0.01 0 0 0.01 −0.01
−0.03 −0.02 0 0 −0.03 −0.02
−0.06 −0.02 0 0 −0.06 −0.02
−0.07 −0.02 0 0 −0.07 −0.02
−0.06 0.01 0 0 −0.06 0.01
−0.06 −0.03 0 0 −0.06 −0.03
0.01 0.01 0 0 0.01 0.01
−0.01 0.00 0 0 −0.01 0.00
−0.06 −0.02 0 0 −0.06 −0.02
Error in average (%) 4 2

The maximum and minimum distance values of the LiDAR data.

Maximum (m) Minimum (m)
Measured data 6.808 0.15
Actual value (reference) 6.54 0.09
Error (%) 4 67

Gyroscope orientation measurements.

X orientation Y orientation X reference Y reference X error Y error
−0.01 −0.01 0 0 −0.01 −0.01
−0.01 −0.01 0 0 −0.01 −0.01
−0.01 0.02 0 0 −0.01 0.02
−0.01 0.03 0 0 −0.01 0.03
−0.01 0.03 0 0 −0.01 0.03
0.00 0.03 0 0 0.00 0.03
−0.01 0.03 0 0 −0.01 0.03
0.00 0.00 0 0 0.00 0.00
0.00 0.00 0 0 0.00 0.00
−0.01 0.03 0 0 −0.01 0.03
Errors in average (%) 1 2

The comparison of the encoder results and the reference in Y-axis.

Y position from odometry Y reference Error
0.02 0.02 0.00
0.02 0.02 0.00
0.02 0.02 0.00
0.02 0.02 0.00
0.02 0.02 0.00
0.01 0.02 0.01
0.01 0.02 0.01
0.02 0.02 0.02
0.07 0.02 0.05
0.07 0.02 0.05
Error in average (%) 2
eISSN:
1178-5608
Language:
English
Publication timeframe:
Volume Open
Journal Subjects:
Engineering, Introductions and Overviews, other
Journal RSS Feed