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

Widjiantoro, Bambang Lelono; Indriawati, Katherin; Alexander Buyung, T. S. N.; Wahyuadnyana, Kadek Dwi

doi:10.2478/ijssis-2024-0002

Figures & Tables

The scenario of physical world and measurement.

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

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

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.

The EKF parameter details_

Parameter	Information
β^k− \hat \beta_k^-	The estimated state
f	Non-linear model system
h	Measurement model system
v_k	Measurement noise
z_k	IMU measurement
W_{k − 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
Q_k	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]
H_k	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]
R_k	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]
F_k	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

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

Figures & Tables

Figure 1:

Figure 2:

Figure 3:

Figure 4:

Figure 5:

Figure 6:

Figure 7:

Figure 8:

The EKF parameter details_

The steps of the EKF-SLAM Algorithm_

The comparison of the encoder results and the reference in X-axis_

The details of robot parameters_

Comparison between the EKF positions and the reference_

Accelerometer measurements_

The maximum and minimum distance values of the LiDAR data_

Gyroscope orientation measurements_

The comparison of the encoder results and the reference in Y-axis_

Paradigm

My account