Fuzzy-Based WIVO State Estimation
Fuzzy-Based WIVO State Estimation
Abstract: This paper investigates the odometry drift problem in differential-drive indoor mobile
robots and proposes a multi-sensor fusion approach utilizing a Fuzzy Inference System (FIS) within
a Wheel-Inertial-Visual Odometry (WIVO) framework to optimize the 6-DoF localization of the
robot in unstructured scenes. The structure and principles of the multi-sensor fusion system are
developed, incorporating an Iterated Error State Kalman Filter (IESKF) for enhanced accuracy. An
FIS is integrated with the IESKF to address the limitations of traditional fixed covariance matrices in
process and observation noise, which fail to adapt effectively to complex kinematic characteristics and
visual observation challenges such as varying lighting conditions and unstructured scenes in dynamic
environments. The fusion filter gains in FIS-IESKF are adaptively adjusted for noise predictions,
optimizing the rule parameters of the fuzzy inference process. Experimental results demonstrate
that the proposed method effectively enhances the localization accuracy and system robustness of
differential-drive indoor mobile robots in dynamically changing movements and environments.
Citation: Huang, G.; Huang, H.; Zhai,
Y.; Tang, G.; Zhang, L.; Gao, X.; Keywords: multi-sensor fusion; wheel-inertial-visual odometry; iterative error state Kalman filter
Huang, Y.; Ge, G. Multi-Sensor Fusion (IESKF); system noise covariance; fuzzy inference system (FIS)
for Wheel-Inertial-Visual Systems
Using a Fuzzification-Assisted
Iterated Error State Kalman Filter.
Sensors 2024, 24, 7619. https://
1. Introduction
doi.org/10.3390/s24237619
Position estimation in 6-DoF is important for determining the position of an au-
Academic Editors: Chee Kiat Seow tonomous navigation robot and its further actions in Simultaneous Localization and Map-
and Henrik Hesse
ping (SLAM) technology [1]. Proprioceptive and exteroceptive sensors are employed to
Received: 7 November 2024 accomplish localization and map-building tasks, which include obtaining external sensory
Revised: 25 November 2024 information (i.e., the surrounding environment of visual light intensity or distance) and
Accepted: 26 November 2024 proprioceptive information (i.e., the robot’s internal values of speed and/or orientation) [2].
Published: 28 November 2024 Proprioceptive sensors, such as wheel encoders, inertial measurement units (IMUs), cam-
eras and LiDAR are mounted on the body of the robot to provide relative positioning
information. In contrast, exteroceptive sensors including the Global Navigation Satellite
System (GNSS) and Ultra-Wideband (UWB) systems are located outside the body of the
Copyright: © 2024 by the authors.
robot to supply absolute position data.
Licensee MDPI, Basel, Switzerland.
The odometry system utilizes the relative position of each frame provided by pro-
This article is an open access article
prioceptive sensors to estimate the instantaneous position and orientation of the robot
distributed under the terms and
relative to its starting point. This is a crucial module in SLAM and autonomous navigation
conditions of the Creative Commons
Attribution (CC BY) license (https://
technologies, especially for providing positioning and orientation data when exteroceptive
creativecommons.org/licenses/by/
sensors are unavailable. However, in the actual operation of indoor mobile robots, GNSS
4.0/). is limited in its ability to provide accurate indoor positioning due to signal blockages [3].
Although UWB enables localized positioning by deploying base station units to cover
specific areas, expanding coverage requires additional base stations, which fails to meet the
needs of large-area inspection tasks for indoor mobile robots [4]. Thus, using propriocep-
tive sensors in indoor environments is advantageous for real-time positioning of mobile
robots. Nonetheless, it is generally recognized that single-sensor odometry performance is
influenced by sensor characteristics, environmental disturbances, and vehicle state, often
resulting in suboptimal localization accuracy. This limitation has led to extensive research
on multi-sensor odometry approaches [5], such as Wheel-Inertial Odometry (WIO) [6],
Visual-Inertial Odometry (VIO) [7], Wheel-Visual Odometry (WVO) [8], and LiDAR-Inertial
Odometry (LIO) [9].
The focus of this study is a Wheel-Inertial-Visual Odometry (WIVO) with sensor
fusion method to optimize the localization accuracy of a mobile robot. The wheel odometer
(WO) calculates the rotation angle and linear displacement of a single wheel, the linear
velocity of the left and right wheels, the linear velocity along the x-axis, and the angular
velocity about the z-axis based on the number of pulses obtained by the wheel encoder,
and finally provides translational and rotational motion information with six degrees of
freedom through the Dead Reckoning method [10]. However, WO accuracy is affected by
errors in rotational information and scale, especially under conditions like wheel slip, idling,
wear, and uneven ground friction, impacting localization precision. Inertial Odometry (IO)
employs a tri-axis gyroscope, accelerometer, and magnetometer. Since the accelerometer
is susceptible to high-frequency noise (e.g., mechanical vibration and electromagnetic in-
terference) [11] and the gyroscope to low-frequency noise (e.g., offset drift and random
walk) [12], integrating acceleration and angular velocity over time provides translational
and rotational information but lacks reliability. Visual Odometry (VO) offers small diver-
gence in six-degree-of-freedom motion estimation between frames, but its sensitivity to
environmental features can lead to global odometry divergence [13]. Therefore, the WIVO
system proposed in this study includes two systems: Wheel-Inertial Odometry (WIO) and
Wheel-Visual Odometry (WVO). The WIO subsystem estimates six degrees of freedom in
movement using the linear velocity from wheel encoders and the angular velocity from
the tri-axis gyroscope of IMU, addressing the rotational information errors in WO and the
translational information errors in IO. The WVO subsystem combines WO information
with two-frame VO, mitigating scale errors from wheel wear and uneven ground friction
and reducing divergence in global VO. The odometry information from the WIO and WVO
subsystems is integrated through sensor fusion to form the WIVO system.
To achieve more accurate robot localization, sensor fusion methods based on filtering
algorithms are commonly used in back-end optimizations of SLAM multi-sensor fusion.
Among these, Kalman filter variants are the most classic approach. The Kalman Filter
(KF) was the first filtering method applied to SLAM sensor fusion, primarily based on the
assumptions that the system is linear and that the noise follows a Gaussian distribution [14].
However, SLAM sensor fusion often requires filtering in nonlinear systems, which limits the
application of the KF in SLAM. The Extended Kalman Filter (EKF) removes the assumption
of linearity, using local linearization and Taylor series expansion to address the filtering
challenges of nonlinear systems [15]. The Iterated Extended Kalman Filter (IEKF) is a
modification of the EKF which adds an iterator for the observation step [16]. However,
EKF and its variants linearize the measurement model using a Taylor series, which can
cause significant errors in highly nonlinear systems, leading to biased state estimation in
their performance [17]. The Unscented Kalman Filter (UKF) further optimizes the KF by
avoiding the linearization process in the EKF and using the Unscented Transform (UT) to
sidestep errors introduced by linearization [18]. However, the UKF suffers from arbitrary
parameters necessary for sigma point placement, potentially causing it to perform poorly
in nonlinear problems [19]. Besides, the Error State Kalman Filter (ESKF) is an indirect
optimization approach compared to EKF, and it is particularly suitable for extended Kalman
filtering on state spaces that lie on manifolds. By introducing error states, ESKF constrains
the prediction and update steps to the manifold, allowing for more accurate handling of
Sensors 2024, 24, 7619 3 of 33
nonlinear problems [20]. The Iterated error-state Kalman Filter (IESKF) inherits the high
speed from the Kalman filter, and the error state has smaller nonlinear errors than the
original state during state estimation as with the ESKF. Additionally, it has been proved
in [16,21] that its iterative update exhibits the same solving ability as the optimization
method. With regards to the performance of IESKF, LINS [22] was the first method to
employ the Iterated Error State Kalman Filter (IESKF) for tightly-coupled LiDAR and
IMU-based robot motion estimation. It recursively corrected the robot’s estimated position
using LiDAR-extracted features, preventing filter divergence during long-term operation
while maintaining computational speed.
However, a fundamental issue remains in the previously mentioned Kalman filter-
based fusion methods: the system noise covariance generally includes both the process
noise covariance matrix and the observation noise covariance matrix [23], and the assump-
tion that system noise covariance follows a Gaussian distribution with a mean of zero and
constant variance is often unrealistic [24]. In general, methods for addressing this issue
mainly fall into two categories: robust and adaptive approaches. Robust methods handle
bounded uncertainties by solving a minimax problem to maintain stable performance un-
der worst-case scenarios. However, they are often overly conservative, which compromises
motion estimation accuracy and makes tuning difficult, such as the H∞ method [25] and
Bayes KF method [26]. On the other hand, adaptive methods adjust algorithms in real
time to adapt to changes, typically estimating the system noise covariance matrix based on
innovation or residuals [27]. These methods rely on a sliding window that assumes con-
sistency in errors, making them suitable only for slowly varying systems and inadequate
for handling sudden disturbances and sensor failures. In addition to robust and adaptive
approaches, the Fuzzy Inference System (FIS) has been extensively applied to adaptive
control in complex systems with dynamic uncertainties [28]. It has been demonstrated to
effectively establish nonlinear relationships between inputs and outputs with the support of
human expertise and can be used in the field of dynamic prediction of system noise, which
can quickly adapt to covariance prediction of robot motion and working environment in
dynamic changes without mathematical models.
During the localization of a two-wheel differential speed mobile (TWDSM) robot,
various complex kinematic challenges arise [29]. For WO, these challenges often occur in
demanding scenarios, such as uneven wheel-to-ground friction or movements at signif-
icantly varying speeds during straight or turning motions. Under such conditions, the
actual system noise does not conform to the predetermined process noise covariance matrix.
For VO, similar issues are due to factors like varying lighting conditions and unstructured
scenes (e.g., glass in the camera’s field of view) [30]. These factors dynamically affect
the number of key points and reprojection error, rendering the actual observation noise
incompatible with the predefined observation noise covariance matrix.
To address these problems, this paper proposes a multi-fusion method using a Fuzzy
Inference System (FIS) to optimize the process and observe noise covariance in the Iterative
Error State Kalman Filter (IESKF). The covariance matrices for process noise and obser-
vation noise are challenging to measure directly and are typically approximated through
experience, system modeling and/or experimentation. However, the limitation of empirical
estimation lies in its inability to effectively adapt to changes in system states or varying
environmental scenarios. The proposed IESKF incorporates FIS and is built to adaptively
adjust the position noise component of the covariance matrix in response to different motion
states and environmental scenarios. The proposed method offers several advantages: (1)
the IESKF filtering method applied in our paper not only inherits the high speed of the KF
algorithm, but also utilizes error states and iterators to better solve nonlinear optimization
problems; (2) incorporating FIS enables the system to quickly and dynamically adapt to
covariance prediction of robot motion and working environment without relying on explicit
mathematical models. The remainder of this paper offers the following:
- The models of Wheel-Inertial Odometry (WIO) and Wheel-Visual Odometry (WVO)
are formulated. Using the characteristics of WO and IO in translational and rotational
Sensors 2024, 24, 7619 4 of 33
motion, the WIO system is built. Additionally, the relatively stable interframe odome-
try information from VO and global translational odometry from WO are utilized to
construct a WVO system. The formulated WIO and WVO models are then applied to
the prediction and observation steps in IESKF, respectively.
- An Iterative Error State Kalman Filter (IESKF) is applied based on WIO and WVO, ulti-
mately forming a wheel-inertial-visual odometry (WIVO) multi-sensor fusion system.
The key steps, including initialization, prediction, observation, error compensation
and error reset, are demonstrated.
- A Fuzzy Inference System is built based on WO (WO-FIS), focusing on the complex
kinematic characteristics of a two-wheel differential robot under various motion
scenarios such as forward movement, slow turning and high-speed spinning. By
using the WO-estimated velocity differences, z-axis angular velocity, and x-axis linear
velocity of the robot, an adaptive fuzzy inference is applied to the process noise
covariance matrix in the prediction step of the IESKF, enabling a real-time adaptation
of process noise covariance gain.
- A Fuzzy Inference System is developed based on VO (VO-FIS), addressing conditions
such as contrast reduction in input images due to varying lighting conditions and
unstructured scenes (e.g., glass in the camera’s field of view). By using the information
of visual keypoints and reprojection error, adaptive fuzzy inference is applied to the
observed noise covariance matrix in the observation step of IESKF, enabling a real-time
adaptation of observed noise covariance gain.
Figure2.2.Formulation
Figure Formulationof
ofthe
theWheel-Inertial
Wheel-InertialOdometry
Odometryin
inTWDSM
TWDSMrobot.
robot.
yy
h i
The rotation
The rotation angle θθim = [θθxim
im =
x ,,θ
im imθ ,,θ
im imθ
zz
im ] of
of the
therobot
robotisisderived
derivedfrom froma acombination
combination of
y
triaxial linear acceleration data ai = [axi ,hai x, azi ],y magnetometer
i B = [B x, By, Bz], and gyro-
of triaxial linear acceleration
y data ai = a , a , aiz , magnetometer B = [Bx , By , Bz ], and
scope wi = [wxi , w h i , wi ]. Theirotation anglesi ini the
z
x and y directions are obtained using
x y z
gyroscope
Equation (4). = withe
wi Since , wimagnetometer
, wi . The rotation measures angles the x and
theinstrength of ythe
directions are obtained
Earth’s magnetic field
alongEquation
using three orthogonal
(4). Sinceaxes, the pitch and roll
the magnetometer anglesthe
measures obtained
strength inof(4)the
allow for magnetic
Earth’s the deter-
mination
field alongofthree
the magnetometer’s
orthogonal axes,horizontal the pitch and magnetic field Bobtained
roll angles h and vertical
in (4)magnetic
allow forfield
the
B . Then, the yaw angle can be determined by Equations (5)
determination of the magnetometer’s horizontal magnetic field Bh and vertical magnetic
v and (6).
field Bv . Then, the yaw angle can be determined by Equations (5) and (6).
θ x = arctan ay /az
im
ri i
!
y y 2 2 (4)
x z
θim = arctan − ai / ai + ai
x x sin θ y x cos θ y Bx
cos θim sin θim sin θim
Bh im im By
= y y (5)
Bv 0 cos θim − sin θim B z
z
θim = arctan( Bv /Bh ) (6)
To mitigate the attitude estimation errors caused by high-frequency noise in accelerom-
eter data and low-frequency noise in gyroscope data, a complementaryh filter is iapplied in
x , θ , θz y
this paper. This filter combines the low-frequency signals θim = θim from the
him im i
y
accelerometer and magnetometer with the high-frequency signals wi = wix , wi , wzi from
the gyroscope, resulting in the filtered rotation angle information as shown in Equation (7).
where λ is the filter coefficient used to adjust the weighting between the gyroscope and the
accelerometer/magnetometer for attitude estimation.
Sensors 2024, 24, 7619 7 of 33
Subsequently, the x-axis linear velocity vex and the yaw angle θiz from the inertial
odometry can be used to solve the WIO of the TWDSM robot. The translational motion
of the TWDSM robot is generally calculated using the Dead Reckoning method. The
principle of this algorithm is to estimate the current position of translational motion by
continuously integrating the displacement with the known linear velocity and yaw angle
during movement [35].
As shown in Figure 2, d represents the wheelbase, {W} denotes the world coordinate
system, and {Bi } and {Bj } are the robot base coordinate systems at times i and j, respectively.
The x-axis linear velocity vex is obtained from the wheel encoder, while ∆θ ez is the yaw angle
change from time i to j as given in Equation (4). Similarly, ∆θ iz is the yaw angle change
obtained from the IMU through complementary filtering from time i to j. In general, when
using the Dead Reckoning method for translational motion calculation, the TWDSM robot
relies solely on wheel odometry data. As can be seen from Equations (1)–(4), this approach
results in inaccuracies in the linear displacement S calculation when the robot encounters
conditions like turning slippage or uneven ground friction, leading to inaccuracies in the
real-time yaw angle calculation ∆θ ez and restricting its effectiveness under actual ground
conditions. When the computed linear displacement S does not accurately reflect the
robot’s actual motion, additional errors may propagate into the x-axis velocity vex derived
from the wheel encoder. However, compared to using the IMU triaxial acceleration data
directly for second-order integration, which is more affected by high-frequency noise, the
x-axis linear velocity vex obtained from the wheel encoder is more reliable.
Therefore, the proposed method for constructing the translational motion WIO primarily
uses vex as the x-axis linear velocity and ∆θ iz as the yaw angle change in Equation (8).
where PWIO represents the position information from the WIO odometry translational
z
motion, dt represents the differential of time t, and pWIO is set to zero assuming the robot is
moving on a flat indoor surface.
The rotational motion in WIO is obtained by converting the rotation angles through
complementary filtering into quaternion orientation:
y x y z
x
θWIO
θWIO
z
θWIO θWIO θ θ
cos 2 cos 2 cos 2 + sin 2 sin WIO2 sin WIO2
w y x y z
qWIO x
θWIO
θ
z
θ θ θ θ
q x sin 2
cos WIO2 cos WIO 2 − cos WIO 2 sin WIO2 sin WIO2
WIO
y = y y (9)
q x z x z
cos θWIO sin θWIO cos θWIO + sin θWIO cos θWIO sin θWIO
WIO
z
qWIO 2 2 2 2 2 2
y x y z
θ x
θ θ z
θ θ θ
cos WIO 2 cos WIO2 sin WIO 2 − sin WIO 2 sin WIO2 cos WIO2
where θ WIO is the rotational motion and qWIO is the quaternion orientation.
k
The linear acceleration {aWIO k
}k=x,y,z and angular acceleration {αWIO }k=x,y,z for WIO need
to be calculated using (10) by performing second-order differentiation on the translational
and rotational motion information, respectively.
2
d k
p
k
aWIO 2 WIO
k = dtd2 , k = x, y, z (10)
αWIO 2 θ
k
dt WIO
In summary, the WIO in the proposed method can by constructed by (4)–(10). Since
the accuracy of WO heavily depends on ground conditions, issues such as uneven ground
friction or wheel wear can cause scale discrepancies and odometry drift over time. While
VO does not rely on ground conditions, it can suffer from long-term drift due to lighting
Sensors 2024, 24, 7619 8 of 33
changes and unstructured scenes. To address the translational and rotational motion
calculation errors in the wheel odometry and the IMU, a WVO is constructed in this paper.
The construction of VO involves three main steps [36]: image feature detection and
matching, camera position estimation, and visual odometry formation. The ORB feature
is used for feature detection and matching which includes a FAST keypoints extractor
and BRIEF descriptors. After detecting FAST keypoints, BRIEF descriptors and Hamming
distance [37] are used to generate feature descriptors and match features accordingly.
The method is applied for 2D feature points in the image coordinate frame. The EPnP
method [38] is then used to estimate the position of a camera by solving the correspondence
between known 3D points and 2D feature points, where the 3D points can be obtained
from the depth values of the RGBD camera. Finally, the ICP method [39] is used to form
the visual odometry, resulting in 3-DoFDoF translational information of the robot’s base
coordinate {B} relative to the world coordinate {W} for VO. This is then fused with the
3-DoF wheel odometry to construct the WVO.
To construct the WVO, the first step is to establish the 3-DoF wheel odometry transla-
tional information. This is done by performing the Dead Reckoning method based on the
robot’s linear velocity along the x-axis and angular velocity around the z-axis.
The translational motion information of the WVO is obtained by linearly combining
the WO and interframe VO, as shown in Equation (11). As the second step, the 3-DoF
interframe VO is derived by calculating the difference between the visual odometry values
k }
of two consecutive frames {pVO k=x,y,z , thus capturing the positional change between frames
k
{∆pVO }k=x,y,z .
+ vex · cos(R ωez · ds) · dt + ∆pVO
x x
R x
pWVO = ′ pWO
y y y
p = ′ pWO + ve · sin( ωe · ds) · dt + ∆pVO
x z (11)
WVOz
pWVO =0
where {′ pWOk }
k=x,y,z represents the wheel odometry’s translational position along the X,
Y and Z axes of the last frame, and {p WVO k }k=x,y,z represents the translational motion
information of the WVO of the current frame. The linear and angular R velocity (vex , wez )
along the x-axis and z-axis can be obtained by Equation (3). The term ωe ·ds denotes the z
yaw angle calculated by integrating z-axis angular velocity over time. The second dt is
k
to update the status of location for the WO. {∆p VO }k=x,y,z is the inter-frame translational
position information of VO, which is calculated by the subtraction value between two
frames of {p VO k } k
k=x,y,z . {p VO }k=x,y,z is constructed through image feature detection, camera
position estimation and visual odometry formation. The position of WVO is computed
by linearly superimposing the WO and VO position obtained from the above steps. The
z
pWVO component is set to zero under the assumption that the robot operates on a flat
surface, where vertical movement (along the z-axis) is negligible. This simplification is
valid for the specific application scenario addressed in this study and helps to improve the
computational efficiency without impacting the overall accuracy of the system.
To minimize random noise from lighting disturbances, a sliding window averaging
filter [40] is applied to the WVO. The concept of this filter is to compute the average of data
within a moving window, thereby smoothing the signal and reducing noise.
1 M −1 k
M i∑
k
p̂WVO [n] = pWVO [n − i ], k = x, y, z (12)
=0
where M represents the number of data points in each averaging calculation, n is the index
k
of the current time or data point, and p̂WVO [n] is the filtered WVO value at time n constant.
With the formulations in (11) and (12), the WVO is constructed. The 6-DoF position
information obtained from both WIO and WVO will be applied to the prediction and
observation stages of the IESKF system, respectively, to achieve multi-sensor data fusion
and enhance the accuracy and robustness of position estimation.
Sensors 2024, 24, 7619 9 of 33
Status Types
Status Variable True Status Nominal Status Error Status
(xt ) (x) (δx)
Position pt p δp
Velocity vt v δv
Orientation θt θ δθ
Linear acceleration bias bat ba δba
Angular acceleration bias bαt bα δbα
Gravitational acceleration gt g δg
In Table 1, the rotation angle θ in the nominal state is derived from the quaternion-
based attitude qWIO of the WIO. The rotation angle θ and rotation matrix R are conversed
through Euler angles in the IESKF, reflected in various state types such as nominal state,
true state, and error state.
Figure 3 illustrates the flowchart of the IESKF system. The proposed IESKF has an
added iterator for the observation step compared to the ESKF, as shown in Figure 3, where
N[i] represents the current number of iterations, Nmax is the maximum number of iterations
and ε is the minimum value of iterations.
In the IESKF initialization, position from the WO is retrieved and set as the initial true
position variable pt with an approximation of initial state covariance matrix defined as P̂t .
ptx
x
pWO
y y
pt = pt = pWO and P̂t = diag σ̂p2 I3×3 , σ̂v2 I3×3 , σ̂θ
2 2
I3×3 , σ̂ba 2
I3×3 , σ̂bα I3×3 , σ̂g2 I3×3 (13)
pzt z
pWO
where σ̂2P , σ̂2v , σ̂2θ , σ̂2ba , σ̂2bα , σ̂2g denotes the initial noise variances for position, veloc-
ity, rotation angle, linear acceleration bias, angular acceleration bias and gravitational
acceleration, respectively. I3×3 represents the 3 × 3 identity matrix.
In the IESKF prediction step, the WIO data is incorporated in the nominal kinematic
equations of the TWDSM robot in (14), which is then transformed into the error-state kinematic
Equation (15). These can then be converted to a state-space form as (16) and (17) for the prior
estimate valuable δx− and an approximation of prior estimate covariance matrix Pt− .
δx− = Fx δx (16)
P− T T
t = Fx Pt Fx + Fi QFi (17)
where δx = f (δx, u, w) = f (δx, u) + w, w ∼ N (0, Q), (18)
Q = diag(03 , Cov(ηv ), Cov(ηθ ), Cov(ηa ), Cov(ηα ), 03 )
12×12 (19)
= diag 03 , σ̂v2 I3×3 , σ̂θ2 I3×3 , σ̂ba
2 I 2
3×3 , σ̂bα I3×3 , 03 ∈ R
I I∆t 0 0 0 0
0 I −R(a − ba )∧ ∆t −R∆t 0 I∆t
0 0 Exp(−(α − bα )∆t)
∂f 0 −I∆t 0 ∈ R18×18
Fx = = (20)
∂δx x,u 0 0 0 I 0 0
0 0 0 0 I 0
0 0 0 0 0 I
0 0 0 0
I 0 0 0
∂f 0 I 0 0
∈ R18×12
Fi = = (21)
∂i x,u 0 0 I 0
0 0 0 I
0 0 0 0
In (14)–(21), u = (a, ba , a, bα ) is the control input values from WIO data including
acceleration a and angular velocity α along with their respective biases ba and bα . w
denotes the process noise, assumed to follow a normal distribution with a mean of 0 and
an approximation of process noise covariance matrix Q as shown in Equation (18). Q
is calculated as a diagonal matrix composed of various noise components (ηv , ηθ , ηa ,
ηα ) shown in Equation (19). (ηv , ηθ , ηa , ηα ) represent velocity, rotation angle, linear
acceleration and angular acceleration noise, respectively. ∆t is the time interval. Fx is the
error-state transition matrix, and Fi is the process noise control matrix.
In the IESKF observation step, the observation z refers to the data obtained from
sensors or measurement devices. It represents a measurement of the system state and
is used to update the filter’s estimate. The observation z is related to the system state x
through the measurement model h(x), typically expressed as Equations (22) and (23) below.
Figure3.3. Flowchart
Figure Flowchart of
of the
theIESKF
IESKFsystem.
system.
In
Inathe
traditional EKF, the observation
IESKF initialization, positionequation from theis WO linearized is retrieved with respect and set to as thethe
nominal
initial
state, obtainingvariable
true position the Jacobian pt withfor Kalman filter updates.
an approximation of In the IESKF,
initial state both covariance the nominal status
matrix de-
and the
xfined as error
Pt . status δx are used. Thus, the Jacobian of the observation equation H with
respect to the error state x is computed in (24), which executed the linearization operation for
pt that pthe linearization operation may cause certain errors, therefore
x
b ( t + Δt ) = b ( t )
α α
estimated by the nominal kinematicgequations shown in (14), the superimposition between
( t + Δt ) = g ( t )
x and δx is performed to derive the true status xt by (28). It should be noted that for
linear states (e.g., position, velocity), direct linear addition is sufficient. However, for
nonlinear states (e.g., rotation, attitude), specific nonlinear operations, such as quaternion
Sensors 2024, 24, 7619 12 of 33
multiplication or Lie algebra mapping, are required. Finally, the error status variable is
reset as δx = 018×1 .
pt = p + δp
vt = v + δv
Rt = Reδθ
xt = x ⊕ δx ⇒ (28)
bat = ba + δbα
b = bα + δbα
αt
gt = g + δg
Table 2. The proposed fuzzy set and the running effects on the robot.
Figure 4. Cont.
Sensors 2024, 24, 7619 13 of 33
Figure 4. The physical meanings of the fuzzy inference system utilizing wheel odometry and visual
Figure 4. The physical meanings of the fuzzy inference system utilizing wheel odometry and visual
odometry information.(a) Slip turning in left or right, (b) Straight forward, (c) Turn with slow or
odometry information.(a) Slip turning in left or right, (b) Straight forward, (c) Turn with slow or
quick angular acceleration, (d) Spin turning or stop, (e) Unstructured scene and varying light scene
quick angular
and (f) Structured scene acceleration,
and stable lighting (d) Spin turning or stop, (e) Unstructured scene and varying light scene
scene.
and (f) Structured scene and stable lighting scene.
In Table 2, each variable is used within the proposed FIS to interpret different oper-
In Table 2, each variable is used within the proposed FIS to interpret different operating
ating conditions, such as slippage during turning, straight-line motion, or spin turning.
conditions, such as slippage during turning, straight-line motion, or spin turning. These
These conditions are set by
conditions arerelevant fuzzy rules
set by relevant fuzzythat adaptively
rules adjust noise
that adaptively adjustcovariance
noise covariance values
values based on the current
based on the kinematic state andstate
current kinematic visual features
and visual of the robot.
features Therobot.
of the ρ in Table
The ρ in Table 2 is
2 is defined as defined
the wheel as speed
the difference
wheel speed between
differencethe right
between andtheleft wheels
right and in a
left TWDSM.
wheels in a TWDSM. vex ,
vxe , ωze represents
z the linear velocity along the x-axis and z-axis, respectively. N u denotes
ωe represents the linear velocity along the x-axis and z-axis, respectively. Nu denotes the
the number ofnumber
ORB feature
of ORB points detected
feature pointsin the VO.inEthe
detected r is the ORB feature reprojection
VO. Er is the ORB feature reprojection error,
error, indicating the consistency of feature tracking.
indicating the consistency of feature tracking.
As shown in Figure 4, the physical interpretation of the wheel odometry-based fuzzy
inference system (WO-FIS) and Visual Odometry Fuzzy Inference System (VO-FIS) includes
different operational effects:
(a) Left/right turn slippage (A1, A3): In Figure 4a, slippage during left or right turns is
represented by the wheel speed difference ρ. When ρ ∈ A1, the linear speed of the
right wheel significantly exceeds that of the left wheel. This speed difference implies
that the right wheel surpasses its friction limit with the ground, causing the right
wheel to lose traction and slip. Conversely, when ρ ∈ A3, slippage occurs on the left
wheel as the left wheel speed exceeds the friction threshold.
Sensors 2024, 24, 7619 14 of 33
(b) Forward motion (A2): As shown in Figure 4b, during forward motion ρ ∈ A2, the left
and right wheel speeds are equal, resulting in no wheel slippage.
(c) Slow/Quick turn (A4, A5): In Figure 4c, a small angular velocity wez (small) around
the z-axis represents a slow turn, where as a larger wez (large) indicates a fast turn.
(d) Stationary/Spin-in-place (A6): Figure 4d demonstrates the robot’s stationary or spin-
in-place conditions. When vex (0) and both wheel speeds are zero, the robot is stationary.
When vex ∈ A6[1] but the left and right wheels have equal and opposite speeds vex ∈
A6[2], the robot rotates in place with an angular velocity wez . However, if vex ∈ A6[1],
and no angular velocity exists, the robot remains stationary without rotation.
(e) Small set of feature points/Large reprojection error (B1, B4): Figure 4e demonstrates
the deficiency of ORB feature points and the significant error in ORB reprojection
in visual information when facing a scene with reflective glass or changing lighting.
In unstructured scenes, such as glass surfaces, due to the lack of sufficient texture
and detail on these surfaces, visual feature point detection algorithms have difficulty
detecting a large number of feature points, which also leads to a decrease in fea-
ture points used for feature matching and camera position calculation, resulting in
increased reprojection errors.
(f) Large set of feature points/Small reprojection error (B2, B3): Figure 4f demonstrates
the visual information has sufficient ORB feature points and a smaller ORB reprojec-
tion error when facing a no-glass scene or stable lighting scene, which is opposite to
the situation illustrated in Figure 4e.
By utilizing the Wheel Odometry Fuzzy Inference System (WO-FIS) and Visual Odom-
etry Fuzzy Inference System (VO-FIS), the system performs fuzzy calculations to predict
∼2
the noise values σ̂2P and σP . These predicted noise values will be applied in real time to
an approximation of the process noise covariance Q and observation noise covariance V
in (29) and (30), respectively. This approach dynamically adapts the covariance values to
better reflect the current operational state, thereby enhancing the accuracy and robustness
of sensor fusion in complex environments.
The fuzzification and setting fuzzy rules step is extremely crucial for FIS, and it is
necessary to complete its numerical verification through pre-experiments before building
the FIS system. Section 3.1 will provide a detailed description for the fuzzification and
fuzzy rules determined by combining a priori experimental data.
After that, aggregation is performed to combine these outputs into a fuzzy value [43].
In this paper, after obtaining the fuzzy output values triggered by each rule, the maximum
method is used to classify all activated rules. This classification yields the combined fuzzy
output values WR (ω) for WO-FIS and VR (v) for VO-FIS. The classification criterion is based
on the output membership function associated with a given rule, where the maximum
output fuzzy value for each membership function is calculated and then linearly combined.
WR[1] ( x ), WR[2] ( x ), WR[3] ( x ),
WR(w) = max WR[6] ( x ), WR[7] ( x ) + max (31)
WR[4] ( x ), WR[5] ( x ), WR[8] ( x )
VR(v) = max VR[1] ( x ) + max(VR3 ( x )) + max VR[2] ( x ), VR[4] ( x ) (32)
where WR(x) represents the fuzzy values for any two input variables under their correspond-
ing output membership functions (WH , WL ). Similarly, VR(x) refers to the fuzzy values for
any three input variables under their output membership functions (VH , VM , VL ).
WR [1] ( x ) , WR [2] ( x ) , WR [3] ( x ) ,
( )
WR ( w ) = max WR [6] ( x ) , WR [7] ( x ) + max
WR [ 4] ( x ) , WR [5] ( x ) , WR [8] ( x )
(31)
( ) (
V R ( v ) = m a x V R [1 ] ( x ) + m a x ( V R 3 ( x ) ) + m a x V R [ 2 ] ( x ) , V R [ 4 ] ( x ) ) (32)
Sensors 2024, 24, 7619 15 of 33
where represents the fuzzy values for any two input variables under their corre-
sponding output membership functions (WH, WL). Similarly, VR(x) refers to the fuzzy val-
ues for any three input variables under their output membership functions ∼2
(VH, VM, VL).
22
Inthe
In thedefuzzification
defuzzificationprocess
process[44],[44],the
theexact
exactvalues
valuesofofσσ̂ and σσ2PP are obtained
P Pand obtained using
using
the centroid
the centroid method
method inin (33).
(33).
R v ⋅ VR ( v) dv
2 p = Rv · VR( v ) dv
R
2 w ·wWR
2 R
⋅ WR(w( w)dw
) dw , σ 2
σ̂p =σˆ p = σp =
, e (33)
(33)
WR W(Rw()wdw) dw VR VR( v()vdv
)dv
where
where ω,
ω,vvrepresent
representthe
thefuzzy
fuzzyinput
inputvariables
variablesfor
forWO-FIS
WO-FISand andVO-FIS,
VO-FIS,respectively.
respectively. These
These
two
two variables are used in calculating the centroid for the final defuzzied outputs.
variables are used in calculating the centroid for the final defuzzied outputs.
2.4.
2.4. The
TheFIS-IESKF
FIS-IESKF Multi-Sensor
Multi-Sensor Fusion
Fusion Method
Method
The
The proposed FIS-IESKF multi-sensor fusion
proposed FIS-IESKF multi-sensor fusion method
method with
with the
the three
three systems
systems is
isshown
shown
in
in Figure
Figure 5.
5.
Figure
Figure 5.
5. The
Thediagram
diagramof
ofFIS-IESKF
FIS-IESKF multi-sensor
multi-sensor fusion
fusion method.
method.
The
The odometry
odometry fusion system system integrates
integratesdata datafrom
fromaawheel
wheelencoder,
encoder,ananIMU IMU andand vis-
visual
information
ual information from an RGBD
from an RGBD camera.
camera. TheTheWOWO supplies linear
supplies velocity
linear data,
velocity data,while
while thethe
IO
contributes
IO contributes angular
angular velocity.
velocity.These
Thesedata dataarearefused
fusedusing
usingaa Dead
Dead Reckoning method method to to
compute the
compute the translational
translational position
position of of the
the robot.
robot. The
The rotational
rotational information
information is is derived
derived by by
integrating the
integrating the angular
angular velocity
velocityfromfromthe theinertial
inertialodometry,
odometry, resulting
resultingin in six
six degrees-of-
degrees-of-
freedom (6-DoF) WIO data. For WVO, the WO provides 3-DoF
freedom (6-DoF) WIO data. For WVO, the WO provides 3-DoF position information, position information, while
the VOthe
while supplies frame-to-frame
VO supplies 3-DoF position
frame-to-frame as a gainas
3-DoF position term. These
a gain areThese
term. linearly arecombined
linearly
to form theto3-DoF
combined form theinformation of the WVO.
3-DoF information of the WVO.
The IESKF
The IESKF system
system is is responsible
responsible for for odometry
odometry estimation.
estimation. The
The WIO
WIO and and WVO
WVO data data
are used within the IESKF for precise odometry calculations. The
are used within the IESKF for precise odometry calculations. The WIO data are applied in WIO data are applied
in the
the prediction
prediction phase,
phase, wherewherethe the
statestate is predicted
is predicted based
based on kinematic
on the the kinematic equations
equations of
of the
the robot. The WVO data are used in the observation phase, where
robot. The WVO data are used in the observation phase, where the state of the robot is the state of the robot
is observed
observed according
according to the
to the observation
observation equation.
equation. During
During errorerror compensation,
compensation, the error
the error be-
between the observed and predicted states is used to adjust the state
tween the observed and predicted states is used to adjust the state estimate, completing estimate, completing
the odometry
the odometry calculations
calculations and and obtaining
obtaining the the updated
updated position.
position.
The fuzzy inference system is used to construct
The fuzzy inference system is used to construct the WO-FIS the WO-FIS andand VO-FIS.
VO-FIS. By Byinputting
inputting
data such as wheel speed difference, z-axis angular velocity, number
data such as wheel speed difference, z-axis angular velocity, number of visual feature of visual feature points,
and visual reprojection error into the proposed FIS, this system dynamically
points, and visual reprojection error into the proposed FIS, this system dynamically pre- predicts the
process noise covariance and observation noise covariance matrices
dicts the process noise covariance and observation noise covariance matrices for the Iter- for the Iterative Error
State Kalman filter. This optimization improves the accuracy of the robot’s odometry
ative Error State Kalman filter. This optimization improves the accuracy of the robot’s
calculation, yielding more precise position and transformation.
odometry calculation, yielding more precise position and transformation.
3. Results of Numerical Verification and Experimental Validation
In this paper, the proposed FIS-IESKF multi-sensor fusion method is verified numeri-
cally and validated experimentally with the TWDSM robot. The experimental hardware
includes an Aruco code positioning and measurement system, environmental objects, and
the TWDSM robot, as shown in Figure 6. The whole TWDSM robot system consists of
Kobuki base with a wheel encoder (YUJIN ROBOT Co., Ltd., Incheon, Republic of Korea),
3. Results of Numerical Verification and Experimental Validation
In this paper, the proposed FIS-IESKF multi-sensor fusion method is verified numer-
ically and validated experimentally with the TWDSM robot. The experimental hardware
includes an Aruco code positioning and measurement system, environmental objects, and
Sensors 2024, 24, 7619 16 of 33
the TWDSM robot, as shown in Figure 6. The whole TWDSM robot system consists of
Kobuki base with a wheel encoder (YUJIN ROBOT Co., Ltd., Incheon, Republic of Korea),
IMU (Shenzhen Yahboom Technology Co., Ltd., Shenzhen, China), RGBD camera(ASUS-
IMU (Shenzhen Inc.,
TeK Computer Yahboom Technology
Shanghai, China),Co., Ltd.,
Lidar Shenzhen,
(Shanghai China),Co.,
Slamtec RGBDLtd.,camera(ASUSTeK
Shanghai, China)
Computer Inc., Shanghai, China), Lidar (Shanghai Slamtec Co., Ltd.,
and Microcomputer (NVIDIA Corporation, Santa Clara, CA, USA). Meanwhile, Shanghai, China) and
the Aruco
Microcomputer (NVIDIA Corporation, Santa Clara, CA, USA). Meanwhile, the Aruco
code and other environment identification are supplied by TBEA Co., Ltd. from Shenyang, code
and other environment identification are supplied by TBEA Co., Ltd. from Shenyang, China.
China.
Figure6.
Figure 6. System
System hardware
hardware for
for the
the experiment.
experiment. (a) Experimental
Experimental environment
environment settings,
settings, and
and (b) trans-
formationrelationships
formation relationshipsof
ofhomogeneous
homogeneouscoordinate
coordinatesystems
systemsininvarious
varioussystems.
systems.
Figure6a
Figure 6aillustrates
illustratesthe
thehardware
hardware setup
setup and
and experimental
experimental environment
environment configura-
configuration.
tion.Aruco
The The Aruco code positioning
code positioning and measurement
and measurement systemsystem comprises
comprises 15 Aruco
15 Aruco codescodes
labeled la-
from
beledID-01
fromtoID-01
ID-15,toarranged in a 1-m horizontal
ID-15, arranged and verticaland
in a 1-m horizontal gridvertical
patterngrid
[45] pattern
throughout[45]
the entire experimental
throughout space. The red
the entire experimental circles
space. Themark the robot’s
red circles markstart and endstart
the robot’s points.
and {W}
end
denotes the world coordinate frame and {R} represents the robot’s coordinate
points. {W} denotes the world coordinate frame and {R} represents the robot’s coordinate frame, which
isframe,
fixed which
at the robot’s
is fixedchassis center. chassis
at the robot’s Duringcenter.
the experiment,
During thetheexperiment,
robot begins atrobot
the the starting
begins
point,
at themoves along
starting themoves
point, black dashed line black
along the and ultimately
dashed linereturns
and to the starting
ultimately point.toThis
returns the
setup enables continuous assessment of effectiveness across the movement path of the robot.
The primary function of this system is to provide reference positions for the predicted
positions of the robot calculated by the proposed FIS-IESKF multi-sensor fusion method,
which then serve as ground truth values for position measurements. These measured
positions are used to assess the accuracy of the predicted positions.
The environmental object setup includes various items, with a table shown as an
example in Figure 6b, placed to represent environmental objects other than the TWDSM
Sensors 2024, 24, 7619 17 of 33
robot. Since the RGBD camera acts as an external sensory device on the robot, additional
environmental objects aid in generating visual odometry through feature point recognition
and matching. The transformation relationships between the homogeneous coordinate
systems in various hardware setups is shown in Figure 6b. The robot is positioned between
Aruco Code ID-05 and Aruco Code ID-06. {W} represents the world coordinate frame.
{B} is the robot’s base coordinate frame, which aligns with {R} in Figure 6a. {I} denotes
the inertial navigation coordinate frame. {L} is the lidar coordinate frame. {C} stands
for the camera coordinate frame. {A05} and {A06} are the coordinate frames for Aruco
Codes ID-05 and ID-06, respectively. {V} represents the coordinate frame for environmental
objects, with a table as a generic reference for this coordinate system in Figure 6b. Each
homogeneous transformation of a hardware component and corresponding coordinate
system classification and meaning are summarized in Table 3.
In the proposed FIS-IESKF method of this paper, the predicted position derived from
the multi-sensor fusion system is obtained using (34).
W B B
p W p W V C L I p
=BT· = V T · C T · L T · I T · BT · (34)
1 1 1
where W p and B p are the position of the robot in the world coordinate {W} and base
coordinate {B}, respectively. W B T is the transformation matrix of the robot base coordinate
frame relative to the world coordinate frame.
Similarly, the measured position calculated by the Aruco code positioning system is
obtained using (35).
W B B
p p p
=W
B T · = W
A06 T · A06
C T · C
L T · L
I T · I
B T · (35)
1 1 1
where W A06 T is the transformation matrix of the Aruco code (ID-06) coordinate frame
relative to the world coordinate. Depending on the robot’s location within the experimental
space, the system will match different Aruco code IDs, which define the transformation
matrix between the Aruco code coordinate frame and the world coordinate frame, and
apply this to the position calculation.
Using Equations (34) and (35), the predicted position and the measured position at
each moment of the robot’s operation are calculated, corresponding to coordinates {P} and
{M}, respectively. This setup enables the system to obtain continuous position estimations
for comparison and refinement throughout the trajectory of the robot.
Sensors 2024, 24, x FOR PEER REVIEW 19 of 33
(V) For A5, which represents ωze (large), the membership function is set to 1 when ωze
Sensors 2024, 24, 7619 18 of 33
approaches positive or negative infinity and is set to 0 in all other cases. Therefore,
using the Anti-“Gbell” type membership function is appropriate for A5.
(VI) For A6, which represents vxe (0), the membership function is set to 1 when vxe ap-
3.1. Numerical
proaches Verification
0 and is setfor
tothe
0 inFIS
all other cases. Therefore, using the “Gauss” type mem-
Fuzzy
bershipmembership functions [46]
function is appropriate for play
A6. a role in defining fuzzy sets and achieving
(VII)fuzzification of fuzzy
For all the outputinput
fuzzyvariables
inferenceinfunctions
fuzzy logic systems. our
of WO-FIS, Figures 7 and
paper uses8the
illustrate the
“Triangle”
fuzzytype
membership
membership functions
functionof the
for Wheel Odometry
both high and lowFuzzy Inference
variance, as theSystem
process(WO-FIS)
noise co-
and Visual Odometry
variance matrix inFuzzy Inference
the system System
exhibits (VO-FIS) established
fluctuations in this paper.
and variations.
Figure 7.
Figure 7. WO-FIS
WO-FIS membership
membership functions.
functions. (a) (a)Input
InputWO-FIS
WO-FISmembership
membershipfunction
functionforfor,ρ,(b)
(b)Input
Input
z , (c) Input WO-FIS membership function for vx and (d) Out-
WO-FISmembership
WO-FIS membershipfunction
functionfor
forωω
z ,e(c) Input WO-FIS membership function for v x and
e (d) Output
e e
put WO-FIS membership function.
WO-FIS membership function.
Sensors2024,
Sensors 2024,24,
24,7619
x FOR PEER REVIEW 20 of 33
19 of 33
(a)
(b)
(c)
Figure 8.
8. VO-FIS
VO-FIS membership
membership functions.
functions. (a)
(a)Input
InputVO-FIS
VO-FISmembership
membershipfunction
functionfor
forNN
u, (b) Input
Figure u , (b) Input
VO-FIS membership function for E r and (c) Output VO-FIS membership function.
VO-FIS membership function for Er and (c) Output VO-FIS membership function.
Thefuzzy
The numerical results are
membership shownparameters
function in Figure 7 shown
and Table 4. As shown
in Figures 7 andin8 Figure 7a, when
are determined
ρ >collecting
by ρmax, the membership
and analyzing degree is 1, representing
the corresponding a wheelvalues
parameter speed ofdifference condition
various motion of ρ
condi-
(+∞).or
tions The result ofover
scenarios ρ ∈aA1 indicates
period thatthe
of time; the right wheel
parameter is slipping.
range In rule WR
can be determined of Table
to[1]facilitate
4, A1
the indicates
setting that the
of specific wheel
fuzzy speed difference
membership functionis in state ρ (+∞), meaning the robot is in a
values.
right-turn slipping state. A5 shows that
As shown in Figure 7, the input variables of WO-FISthe angular velocity
includearound
wheelthe z-axis
speed is in state
difference ρ,
z z x
we (large),
angular indicating
velocity around a sharp turn.ωe , and linear velocity along the x-axis ve . The output
the z-axis
variable is σ̂2P . Thewhen
Conversely, verticalρ <axis
ρmin,ofthe
themembership
fuzzy membershipdegree functions
is 1, indicating a wheel
represents the speed
degreedif-of
ference condition
membership, of ρ (−∞).
indicating the relationship ρ ∈ A3 indicates
The result ofbetween a specificthat the left
variable andwheel is slipping.
a fuzzy set across In
addition,
various whenTypically,
ranges. ρ→0, thethe membership
membership degree
degree is ranges
1, indicating
from 0 atowheel
1. speed difference
condition of ρ (0).
To define the The result
logical of ρ ∈ A2 implies
relationship between that nofuzzy
the wheelvalues
slip is occurring.
of input and output
variables, it is essential to set up fuzzyz rules for each z activated situation [47]. The fuzzy rule
Figure 7b shows that when ωe > 0.6 or ωe < −0.6 for the negative range, the mem-
in this paper
bership degreeis shown in Table 4.aW
is 1, indicating H and
slow W L represent
turning condition z
the high variance
we (small). and low
When −0.2variance
< ωze <
2 z
membership functions,
0.2, the membership respectively.
degree A higheravariance
is 1, representing fast-turningσ̂P indicates
conditiona w lower confidence
e (small).
level for the noise value in the process noise covariance matrix along the diagonal entries.
Sensors 2024, 24, 7619 20 of 33
The rules WR[1] –WR[8] are mainly constructed by pairing the data from ρ, ω ez , vex based on
the kinematic state of the robot.
Regarding the selection of functional types for WO-FIS membership function, the
following rules are observed in (I–VII).
(I) For A1, which represents ρ (+∞), the membership function is set to 1 when ρ ap-
proaches positive infinity and is set to 0 in all other cases. Therefore, using the “S”
type membership function is appropriate for A1.
(II) For A2, which represents ρ (0), the membership function is set to 1 when ρ approaches
0 and is set to 0 in all other cases. Therefore, using the “Gauss” type membership
function is appropriate for A2.
(III) For A3, which represents ρ (−∞), the membership function is set to 1 when ρ ap-
proaches negative infinity and is set to 0 in all other cases. Therefore, using the “Z”
type membership function is appropriate for A3.
(IV) For A4, which represents ωez (small), the membership function is set to 0 when ωez
approaches positive or negative infinity and is set to 1 in all other cases. Therefore,
using the “Gbell” type membership function is appropriate for A4.
(V) For A5, which represents ωez (large), the membership function is set to 1 when ωez
approaches positive or negative infinity and is set to 0 in all other cases. Therefore,
using the Anti-“Gbell” type membership function is appropriate for A5.
(VI) For A6, which represents vex (0), the membership function is set to 1 when vex ap-
proaches 0 and is set to 0 in all other cases. Therefore, using the “Gauss” type
membership function is appropriate for A6.
(VII) For all the output fuzzy inference functions of WO-FIS, our paper uses the “Triangle”
type membership function for both high and low variance, as the process noise
covariance matrix in the system exhibits fluctuations and variations.
The numerical results are shown in Figure 7 and Table 4. As shown in Figure 7a, when
ρ > ρmax , the membership degree is 1, representing a wheel speed difference condition of ρ
(+∞). The result of ρ ∈ A1 indicates that the right wheel is slipping. In rule WR[1] of Table 4,
A1 indicates that the wheel speed difference is in state ρ (+∞), meaning the robot is in a
right-turn slipping state. A5 shows that the angular velocity around the z-axis is in state wez
(large), indicating a sharp turn.
Conversely, when ρ < ρmin , the membership degree is 1, indicating a wheel speed
difference condition of ρ (−∞). The result of ρ ∈ A3 indicates that the left wheel is slipping.
In addition, when ρ→0, the membership degree is 1, indicating a wheel speed difference
condition of ρ (0). The result of ρ ∈ A2 implies that no wheel slip is occurring.
Figure 7b shows that when ωez > 0.6 or ωez < −0.6 for the negative range, the member-
ship degree is 1, indicating a slow turning condition wez (small). When −0.2 < ωez < 0.2, the
membership degree is 1, representing a fast-turning condition wez (small).
In Figure 7c, when vex →0, the membership degree is 1, representing a state of linear
velocity along the x-axis at vex (0), which implies the robot is either stationary or performing
a spinning movement.
Sensors 2024, 24, 7619 21 of 33
Figure 7d shows the output membership functions of WO-FIS, where the membership
degree of the WO-FIS’s WL function is 1 within the range of σ̂2P = 1/3, and the WH function
has a membership degree of 1 within the range σ̂2P = 2/3.
Regarding the setting of WO-FIS fuzzy rules, the following rules are followed:
i. For the WR[1] and WR[3] rules setting, these situations align with the kinematic state
of slipping turn with a large angular velocity, which indicates that the robot is in
a certain slip turning state with low confidence, and it is reasonable to give a high
variance to σ̂2P for Q.
ii. For the WR[2] , WR[4] and WR[5] rules setting, these situations align with the kinematic
state of going forward with a large angular velocity or slipping turn with a small
angular velocity, which indicates that the robot is in an uncertain motion state with
low confidence, and it is worth being cautious to give a high variance to σ̂2P for Q.
iii. For the WR[6] and WR[7] rules setting, these situations align with the kinematic state of
going forward with a small angular velocity or stopping with a small angular velocity,
which indicates that the robot is in a certain motion state with high confidence, and it
is reasonable to give a low variance to σ̂2P for Q.
iv. For the WR[8] rules setting, this situation aligns with the kinematic state of a spinning
turn with a large angular velocity, which represents that the robot is in a certain
motion state with low confidence, and it is reasonable to give a high variance to σ̂2P
for Q.
Figure 8 and Table 5 present the fuzzy membership functions and fuzzy rules of the
Visual Odometry Fuzzy Inference System (VO-FIS) established in this paper. The input
membership function variables for VO-FIS include the number of ORB feature points Nu
and the ORB feature point reprojection error Er . The output membership function variable
∼2
is σP . V H , V M , V L correspond to the high variance, medium variance, and low variance
∼2
membership functions in Figure 8, respectively. A higher variance σP indicates a lower
confidence level in the noise value for the observation noise covariance matrix’s diagonal
elements. The rules VR[1] –VR[4] are constructed by combining data from Nu and Er based
on the status of ORB feature points in the visual camera.
∼2
Rules Nu Er σP
VR[1] B2 B3 VL
VR[2] B2 B4 VH
VR[3] B1 B3 VM
VR[4] B1 B4 VH
Regarding the selection of functional types for VO-FIS membership functions, the
following rules are observed in (VIII–XII).
(VIII)For B1, which represents Nu (less), the membership function is set to 1 when Nu
approaches negative infinity and is set to 0 in all other cases. Therefore, using the “Z”
type membership function is appropriate for B1.
(IX) For B2, which represents Nu (more), the membership function is set to 1 when Nu
approaches positive infinity and is set to 0 in all other cases. Therefore, using the “S”
type membership function is appropriate for B2.
(X) For B3, which represents Er (small), the membership function is set to 1 when Er
approaches negative infinity and is set to 0 in all other cases. Therefore, using the “Z”
type membership function is appropriate for B3.
(XI) For B4, which represents Er (large), the membership function is set to 1 when Er
approaches positive or negative infinity and is set to 0 in all other cases. Therefore,
using the “S” type membership function is appropriate for B4.
Sensors 2024, 24, 7619 22 of 33
(XII) For all the output fuzzy inference functions of VO-FIS, our paper uses the “Triangle”
type membership function for high, medium and low variance, as the observed noise
covariance matrix in the system exhibits fluctuations and variations.
As shown in Figure 8a, the membership value of function B1 is 1 within the range Nu <
Numin , representing a condition where the number of ORB feature points is low Nu (less). In
contrast, the membership value of function B2 is 1 within the range Nu > Numax , indicating a
state where the ORB feature points are abundant Nu (more).
Figure 8b demonstrates that the membership value of function B3 is 1 when the ORB
reprojection error Er < Ermin , reflecting a low reprojection error for ORB feature points Er
(small). On the other hand, function B4 has a membership value of 1 within the range Er >
Ermax , indicating a high ORB feature point reprojection error Er (large).
In Figure 8c, the output membership functions of VO-FIS are shown. The membership
∼2 ∼2
function V L takes a value of 1 when σP = 1/4, V M takes a value of 1 when σP = 1/2, and
∼2
V H takes a value of 1 when σP = 3/4.
Regarding the setting of VO-FIS fuzzy rules, the following rules are followed:
i. For the VR[1] rules setting, this situation aligns with the visual information of a low
reprojection error with a large number of key points, which indicates that the robot is
in a high quality environment scene with high confidence, and it is reasonable to give
∼2
a low variance to σP for V.
ii. For the VR[3] rules setting, this situation aligns with the visual information of a low
reprojection error with a small number of key points, which indicates that the robot
is in a medium quality environment scene with medium confidence, and it is worth
∼2
being cautious to give a medium variance to σP for V.
iii. For the VR[2] and VR[4] rules setting, these situations align with the visual informa-
tion of a large reprojection error, which indicates that the robot is in a low quality
environment scene with low confidence, and it is worth being cautious to give a high
∼2
variance to σP for V.
Let WR[λ] [x] (λ = 1, 2 . . .8) and WR[τ] [x] (τ = 1, 2 . . .4) represent the output fuzzy
values for individual rules in WR[1] –WR[8] and VR[1] –VR[4] , respectively. The aggregated
output fuzzy value can be obtained for each individual rule in Table 6.
Therefore, the specific inputs in the proposed FIS system can be assigned correspond-
ing fuzzy values based on the membership functions established, thus enabling the fuzzifi-
cation process.
Sensors 2024, 24, 7619 23 of 33
Compared Methods
Experiment and the
Fuzzy Set FIS-IESKF Ground
EKF UKF
(Our Method) Truth
Straight Motion A2
Sharp Turning Motion A1/A3/A5
Slow Turning Motion A4
Spin Motion A6
Glass Scene
B1/B4
Changing Lighting Scene
Non-Glass Scene
B2/B3
Stable Lighting Scene
The criterion of Fréchet Distance [48] is used to assess the trajectory similarity in this
paper by (36)–(38).
DF ( A, B) = inf max | A(α(t)) − B( β(t))| (36)
α,β t∈[0,1]
where α(t), β(t), γ(t), ψ(t) are the reparameterization functions for trajectories A, B, C, D,
respectively. maxt∈[0, 1] denotes the maximum point-to-point distance between the two
curves at all times t under a certain parameterization condition. The objective is to find
a pair of parameterization functions (infα,β , infα,γ , infα,ψ ) that minimizes the maximum
distance across all possible pairs of parameterization functions in Figure 9. As illustrated
in Figure 9, trajectory A represents the ground truth, while trajectory B represents the
Sensors 2024, 24, x FOR PEER REVIEW 24 of 33
estimated trajectory from the FIS-IESKF, trajectory C represents the estimated trajectory
from the EKF, and trajectory D represents the estimated trajectory from the UKF.
Figure
Figure9.9.The
Themaximum
maximumvalue
valueofofthe
thepaths
pathsgenerated
generatedby bythe
theFIS-IESKF,
FIS-IESKF,EKF
EKF and
and UKF
UKF algorithms
algorithms
under
underoptimal
optimalparameterization
parameterizationcompared
compared toto the
the ground
ground truth.
truth.
The APE and RPE in results comparison is defined as (39) and (40).
A P E i =‖lo g (T re−f,1 i T e s ti, i )‖ (39)
(
RPE i =‖log Tgt−1, i Tgt , i + Δt f ) (T ) ‖
−1
−1
T
esti, i esti, i + Δ t f (40)
Sensors 2024, 24, 7619 24 of 33
Figure 9. The maximum value of the paths generated by the FIS-IESKF, EKF and UKF algorithms
underThe APEparameterization
optimal and RPE in results comparison
compared is defined
to the ground truth. as (39) and (40).
−1
The APE and RPE in resultsAPE i = ∥
comparison
log isTref,i esti,i ∥
defined
T as (39) and (40). (39)
A P E i =‖lo g (T re−f,1 i T e s ti, i )‖ (39)
−1
−1 −1
∥RPE ∥
RPEi = gt,i TTgt,i
log =‖Tlog
( ) ( −T
)
Testi,i +∆t f (40)
−1
− 1 + ∆t 1
gt , i Tgt , i + Δ t f Testi, i Testi, i + Δ t f ‖
i
f esti,i (40)
where
where APE is used to to compute
computethe theabsolute
absoluteposition
positionerror
errorfor
foreach
eachframe
frame [49],
[49], while
while RPERPE
calculates
calculates the error between adjacent frames
between adjacent frames [50]. [50].
Figure 10 presents
presents trajectory
trajectorysimilarity
similaritycomparison
comparisonresults
resultsforfordifferent
different scenarios:
scenarios:
straight
straight motion, spin turning,
turning,slow
slowturning
turningand
andsharp
sharpturning.
turning.AsAs shown
shown inin Figure
Figure 10,10,
thethe
trajectory
trajectory estimated
estimated by by the
the FIS-IESKF
FIS-IESKF algorithm
algorithm closely
closelyfollows
followsthetheground
groundtruth
truthtrajectory.
trajec-
In contrast,
tory. the trajectory
In contrast, estimated
the trajectory by the by
estimated EKFthealgorithm gradually
EKF algorithm exhibitsexhibits
gradually yaw drift over
yaw
time
drift during
over timelow-speed straight and
during low-speed slow and
straight turning
slowmotions. During spin
turning motions. Duringturning
spin and sharp
turning
turning
and sharpmovements, significantsignificant
turning movements, yaw drift yaw
occurs primarily
drift at the turning
occurs primarily at thepoints,
turningleading
points, to
considerable global odometry
leading to considerable globaldisplacement. The trajectory
odometry displacement. Theestimated
trajectory byestimated
the UKF exhibited
by the
UKFyaw
less exhibited less time
drift over yaw during
drift over time during
low-speed low-speed
straight and slowstraight andmotions
turning slow turning
compared mo- to
tions
the compared
EKF. to the EKF.
Additionally, duringAdditionally,
spin turning during
and spin
sharpturning
turning and sharp turning
maneuvers, the maneu-
trajectory
vers, the trajectory
deviation of the UKF deviation of the UKF
was significantly was significantly
reduced compared reduced compared
to the EKF, although toitthe EKF,
remained
althoughtoitthat
inferior remained inferior to that of the FIS-IESKF.
of the FIS-IESKF.
Figure 10.
Figure 10. Trajectory
Trajectorycomparison
comparisondiagram
diagramofofstraight
straightmotion,
motion,sharp turning,
sharp slow
turning, turning
slow andand
turning spin-
spin-
turning. (a) Straight motion, (b) Spin motion, (c) Slow turning motion and (d) Sharp turning motion.
turning. (a) Straight motion, (b) Spin motion, (c) Slow turning motion and (d) Sharp turning motion.
Figures 11
Figures 11 and
and 12
12 illustrate
illustrate the
thetrajectory
trajectorycomparison
comparisonand andactual
actualscene
sceneimages
images forfor
scenarios with and without glass obstacles, which will prove the adaptability of
scenarios with and without glass obstacles, which will prove the adaptability of the FIS- the FIS-
IESKF algorithm
IESKF algorithm to
to unstructured
unstructuredenvironments.
environments.InInFigure
Figure11,
11,when
when encountering
encountering a scene
a scene
with glass, the number of paired feature points remains between 50 and 70.
with glass, the number of paired feature points remains between 50 and 70. In this In this case, it it
case,
exhibits substantial errors and minor error when the EKF and UKF algorithms
exhibits substantial errors and minor error when the EKF and UKF algorithms estimate the estimate
robot’s turning position, respectively, while the FIS-IESKF algorithm maintains a position
that is roughly consistent with the ground truth.
Sensors 2024, 24, x FOR PEER REVIEW 25 of 33
Figure11.
Figure 11.Trajectory
Trajectorycomparison
comparison in in a scene
a scene with
with glass.
glass. (a) Global
(a) Global motion
motion trajectory,
trajectory, (b) Local
(b) Local motionmo-
Figure
tion 11. Trajectory
trajectories at comparison
bends, (c) in feature
Visual a scene with
pointglass. (a) Global
situation at motion trajectory,
Observation A and (d)(b) Localfeature
Visual mo-
trajectories at bends, (c) Visual feature point situation at Observation A and (d) Visual feature point
tion trajectories
point situation atatObservation
bends, (c) Visual
B. feature point situation at Observation A and (d) Visual feature
situation at Observation B.
point situation at Observation B.
Figure
Figure 13
13 presents
presents the
the trajectory
trajectory comparisons and actual scene images images for for scenarios
scenarios
involving
involving both
both changing and stable lighting conditions, demonstrating the the adaptability
adaptability of of
the
the proposed
proposed FIS-IESKF
FIS-IESKF to varying lighting environments.
environments. To To minimize
minimize the the influence
influence of of
non-illumination factors, the
non-illumination factors, the experiments
experiments were
were conducted
conducted in in aa low-speed
low-speedmotionmotionmode.
mode.
As illustrated in Figure 13a, significant changes in the light source cause
As illustrated in Figure 13a, significant changes in the light source cause a sharp declinea sharp decline
in
in the numberof
the number ofORB
ORBfeature
featurepoints,
points, leading
leading to notable
to notable trajectory
trajectory fluctuations
fluctuations in theinesti-
the
estimations provided
mations provided by the
by the EKFEKF
and and
UKF.UKF. In contrast,
In contrast, the trajectory
the trajectory estimated
estimated by the bypro-
the
proposed FIS-IESKF
posed FIS-IESKF method
method remains
remains as consistent
as consistent as possible
as possible with the with the ground
ground truth.
truth. Under
Under stable lighting
stable lighting conditions,
conditions, shownshown in Figure
in Figure 13b,ofnone
13b, none of the algorithms
the algorithms experience
experience a sharpa
sharp drop in ORB feature points or significant trajectory fluctuations, indicating
drop in ORB feature points or significant trajectory fluctuations, indicating stable perfor- stable
performance. Therefore, the proposed FIS-IESKF is proved to have the
mance. Therefore, the proposed FIS-IESKF is proved to have the ability to ensure accurate ability to ensure
accurate
positioningpositioning
even undereven under environmental
environmental variability.
variability.
Figure 13.
Figure 13. Trajectory
Trajectory comparison
comparison in in changing
changing lighting
lighting scene
scene and
and stable
stable lighting
lighting scene.
scene. (a)
(a) Changing
Chang-
ing lighting
lighting scenescene
and and (b) Stable
(b) Stable lighting
lighting scene.
scene.
Table 8.
Table 8. Trajectory
Trajectory similarity
similarity comparison.
comparison.
Experiment DF (A, B) DF (A, C) DF (A, D) DF (A, C)/DF (A, B) DF (A, D)/DF (A, B)
Experiment DF (A, B) DF (A, C) DF (A, D) DF (A, C)/DF (A, B) DF (A, D)/DF (A, B)
Straight Motion 0.009156 0.013778 0.010814 1.504756288 1.181044745
Straight Motion 0.009156 0.013778 0.010814 1.504756288 1.181044745
Sharp Turning Motion 0.021493 0.103382 0.039939 4.809883826 1.858172395
Sharp Turning Motion 0.021493 0.103382 0.039939 4.809883826 1.858172395
Slow
Slow Turning
Turning Motion
Motion 0.015056
0.015056 0.057390
0.057390 0.0347510.034751 3.811694695
3.811694695 2.308055046
2.308055046
Spin Motion
Spin Motion 0.013638
0.013638 0.106934
0.106934 0.0318060.031806 7.840763444
7.840763444 2.332125942
2.332125942
Glass Scene
Glass Scene 0.009809
0.009809 0.075703
0.075703 0.0336090.033609 7.717601843
7.717601843 3.426273294
3.426273294
Non-Glass Scene 0.013581 0.092971 0.026781 6.845536477 1.971917062
Non-Glass Scene 0.013581 0.092971 0.026781 6.845536477 1.971917062
Changing Lighting
Changing Lighting Scene 0.006818
0.006818 0.035554
0.035554 0.0235310.023531 5.214722577
5.214722577 3.451254748
3.451254748
Scene
Stable
Stable Lighting
Lighting Scene
Scene 0.008489
0.008489 0.040613
0.040613 0.0284360.028436 4.784235885
4.784235885 3.349786191
3.349786191
It can be found from Table 8 that DF (A, B) < DF (A, D) < DF (A, C) holds true across
It can be found
all six motion fromindicating
categories, Table 8 that
thatDFthe B) < DF (A,
(A,trajectories D) < DF using
estimated (A, C)the
holds true across
proposed FIS-
all six motion categories, indicating that the trajectories estimated using
IESKF method exhibit a higher similarity to the ground truth. The ratio DF (A, C)/D the proposed
F (A,
FIS-IESKF method exhibit a higher similarity to the ground truth. The ratio DF (A, C)/DF
B) reveals that as the robot’s speed increases, along with the occurrence of turning maneu-
(A, B) reveals that as the robot’s speed increases, along with the occurrence of turning
vers, glass scenes and the changing lighting scene, the values of DF (A, C)/DF (A, B) rise
maneuvers, glass scenes and the changing lighting scene, the values of DF (A, C)/DF (A,
significantly. Furthermore, the ratio DF (A, D)/DF (A, B) indicates that the UKF maintains
B) rise significantly. Furthermore, the ratio DF (A, D)/DF (A, B) indicates that the UKF
trajectory-following stability even in sharp turning motion, spin motion, the glass scene
maintains trajectory-following stability even in sharp turning motion, spin motion, the
and the changing lighting scene. The average value mean (DF (A, C)/DF (A, B)) and mean
glass scene and the changing lighting scene. The average value mean (DF (A, C)/DF (A,
(DF (A, D)/DF (A, B)) reaches 5.316 and 2.485, respectively. This indicates that, in terms of
B)) and mean (DF (A, D)/DF (A, B)) reaches 5.316 and 2.485, respectively. This indicates
trajectory similarity assessment, the FIS-IESKF method enhances tracking performance
that, in terms of trajectory similarity assessment, the FIS-IESKF method enhances tracking
Sensors 2024, 24, x FOR PEER REVIEW 27 of 33
Sensors 2024, 24, 7619 27 of 33
relative to the
performance ground
relative totruth by approximately
the ground 531% and 248%
truth by approximately 531%compared
and 248%to the EKF to
compared and
UKF, respectively.
the EKF and UKF, respectively.
Theexperimental
The experimental results
results of the
of the global
global trajectory
trajectory position
position error
error and and
local local adjacent
adjacent frame
frame position
position error iserror
shownis shown in Figure
in Figure 14. The14. The TWDSM
TWDSM robot isrobot is conduct
set to set to conduct a closed-
a closed-loop
loop long-distance
long-distance motionmotion fromtostart
from start end. to end.
The The experimental
experimental trajectories
trajectories are compared
are compared by the
by the
APE andAPE
RPE.and RPE.
Figure14.
Figure 14.The
Theresult
resultofofclosed-loop
closed-loopmotion
motionevaluated
evaluatedbybyAPE
APEand
andRPE.
RPE.
Figure 14
Figure 14 illustrates
illustrates the results of the the closed-loop
closed-loop motion
motionassessment
assessmentof ofthe
therobot.
robot.In
thethe
In APEAPEevaluation, the FIS-IESKF
evaluation, the FIS-IESKF algorithm demonstrated
algorithm demonstratedgoodgood
adherence to theto
adherence ground
the
truth, exhibiting
ground a smaller
truth, exhibiting absolute
a smaller positionposition
absolute error. Conversely, the EKF
error. Conversely, algorithm
the showed
EKF algorithm
showed considerable
considerable fluctuations
fluctuations in absolute
in absolute position position error compared
error compared to the to the ground
ground truth,truth,
partic-
particularly
ularly evidentevident
duringduring turning
turning maneuvers.
maneuvers. Meanwhile,
Meanwhile, the trajectory
the trajectory estimated
estimated by
by UKF
UKF exhibited
exhibited moremore severe
severe global
global position
position errors,
errors, especially
especially during
during turning
turning maneuvers.
maneuvers. InInthe
the
RPERPE evaluation,
evaluation, thethe FIS-IESKF
FIS-IESKF algorithm
algorithm achieved
achieved relatively
relatively lowlow errors
errors compared
compared totothe
the ground
ground truth,
truth, confirming
confirming its its superior
superior inter-frame
inter-frame odometry
odometry performance.
performance. However,
However, the
the
EKF and UKF algorithm did not display more stable relative errors with respecttotothe
EKF and UKF algorithm did not display more stable relative errors with respect the
ground
groundtruth.
truth.
The
Theroot
rootmean
meansquare
squareerror (RMSE)
error (RMSE)serves as aas
serves crucial metric
a crucial for evaluating
metric the robot’s
for evaluating the ro-
motion position using the APE and RPE, reflecting the overall
bot’s motion position using the APE and RPE, reflecting the overall magnitudemagnitude of the errors
of in
the
Sensors 2024, 24, 7619 28 of 33
Tables 9 and 10. The evaluation results of APE and RPE encompass assessments of both
translational and rotational motion.
The results indicate that the translational estimation performance of the FIS-IESKF,
EKF, and UKF algorithms stays within a reasonable range. Specifically, the FIS-IESKF
shows the best translational accuracy with an RMSE of 0.009396 m, followed by the UKF
at 0.016228 m, and the EKF at 0.054120 m, which is notably higher. In terms of rotational
estimation, the EKF and UKF display significant deviations from the ground truth, with
RMSEs of 64.3963◦ and 81.3227◦ , respectively. On the other hand, the FIS-IESKF achieves
superior overall accuracy, with a global rotational RMSE of 0.149480◦ . Two probable reasons
contribute to this problem: (1) Nonlinear effects and error accumulation can cause the
EKF and UKF to gradually deviate from the actual values during long-duration rotations
or complex paths, leading to less accurate position estimation. (2) These algorithms may
struggle to accurately predict the covariance in highly nonlinear systems, which limits their
estimation performance.
Additionally, the FIS-IESKF demonstrates impressive local adjacent frame perfor-
mance, achieving a translational RMSE of 0.002347 m and a rotational RMSE of 0.201990◦ .
These results significantly outperform those of both the EKF and UKF algorithms, confirm-
ing the FIS-IESKF’s robustness in maintaining high trajectory accuracy under long-distance
closed-loop motion.
3.3. Computational Complexity Analysis for the FIS-IESKF Multi-Sensor Fusion Method
The computational complexity analysis method is an essential way to evaluate the
real-time performance of SLAM algorithms. The steps of the FIS-IESKF algorithm largely
follow the overall framework of the IESKF, with the key difference being that during the
IESKF prediction step, it incorporates covariance prediction using WO-FIS, and during the
observation step, it integrates covariance prediction using VO-FIS.
In terms of theoretical calculation for the IESKF method, our paper decomposes the
complexity of each key step and combines the properties of matrix operations to obtain
the overall asymptotic computational complexity as ⃝– 1 ⃝.
5 It should be noted in advance
that N represents the dimension of the state vector while M represents the dimension of
Sensors 2024, 24, 7619 29 of 33
the observation vector. Besides, due to the nominal status x and observation z having
dimensions of 18 × 1 and 3 × 1, the value of N and M is defined as 18 and 3, respectively.
⃝1 In the IESKF initialization step, the primary contribution to the asymptotic compu-
tational complexity comes from the covariance matrix initialization (P̂t .) in Equation (13).
This matrix consists of six diagonal blocks, each represented as a 3 × 3 matrix, which
involves asymptotic computational complexity of the IESKF initialization step which is
6 × O(3 × 3) = O(54).
⃝2 In the IESKF prediction step, the primary contribution to the asymptotic computa-
tional complexity comes from the prior estimate covariance calculation in Equation (17). Fx
and Pt both have a dimension of N × N, therefore the asymptotic computational complexity
of matrices Fx and Pt is O(N2 ) and matrix multiplication is performed in this formula. The
overall asymptotic computational complexity is O(N3 ) = O(5832).
⃝3 In the IESKF observation step, the Kalman gain (K) computation is the primary
contributor to the overall asymptotic computational complexity in Equation (25). H, V and
Pt− have dimensions of M × N, M × M and N × N, respectively, which involves calculating
HPt− through matrix multiplication with a complexity of O(MN2 ), and performing a matrix
inversion for HPt− HT + V, which has a complexity of O(M3 ). Combining these operations,
the overall asymptotic computational complexity of the observation step is O(MN2 + M3 ) =
O(999).
⃝4 In the IESKF error compensation step, the computation of error state compensation
is the primary contributor to the overall asymptotic computational complexity. Specifically,
when performing element-wise addition as described in Equation (28), the complexity is
O(N) = O(18).
⃝5 In the IESKF error reset step, the asymptotic computational complexity of setting
the error state to zero (δx = 018×1 ) is O(N) = O(18), as each of the N elements must be
individually assigned to zero.
In terms of theoretical calculation for WO-FIS and VO-FIS, the calculation of asymptotic
computational complexity follows Equation (41) as shown below [51]. Therefore, the asymp-
totic computational complexity of WO-FIS and VO-FIS are shown in⃝and 6 ⃝,
7 respectively.
Ninput
OFIS = O ∑ Fj + O R · Ninput + Noutput + O R · Noutput
(41)
j=1
where Ninput and Noutput represent the number of input/output variables, Fj represents the
number of membership functions for a single input, and R represents the number of rules
in the FIS system.
⃝6 In the WO-FIS, owing to Ninput = 3, Noutout = 1 and R = 8, meanwhile F1 = 3, F2 = 2 and
F3 = 1 (representing the number of membership functions for ρ, ωez , vex , respectively), the
asymptotic computational complexity of WO-FIS is OWO-FIS = O(F1 + F2 + F3 ) + O(R·(Ninput
+ Noutout )) + O(R·Noutout ) =O(6) + O(32) + O(8) = O(46).
⃝7 In the WO-FIS, owing to Ninput = 2, Noutout = 1 and R = 4, meanwhile F1 = 2 and F2
= 2 (representing the number of membership functions for Nu and Er , respectively), the
asymptotic computational complexity of VO-FIS is OVO-FIS = O (F1 + F2 ) + O (R·(Ninput +
Noutout )) + O(R·Noutout ) = O(4) + O(12) + O(4) = O(20).
As a result, the computational complexity of the FIS-IESKF method has been sum-
marized in Table 11. It is noted that Ts represents the time step. Due to the fact that, in
FIS-IESKF, Ts depends on the sampling frequency and system operation time after sensor
time synchronization is applied, when the sampling frequency is set to 20 Hz and the
system operation time is 100 s, Ts takes a value of 2000. Therefore, the time proportion of
each step in Table 11 was calculated using Equation (42).
where Ck represents each specific computational complexity value in Table 11, and Ctotal
represents the sum of each Ck .
IESKF
Name of IESKF IESKF Prediction IESKF Observation IESKF Error
Error
Steps Initialization with WO-FIS with VO-FIS Compensation
Reset
Steps ⃝
1 ⃝
2 ⃝
6 ⃝
3 ⃝
7 ⃝
4 ⃝
5
Asymptotic
Computational O(54) O(5832) O(46) O(999) O(20) O(18) O(18)
Complexity
Specific
Computational 1·O(54) Ts·O(5832) Ts·O(46) Ts·O(999) Ts·O(20) Ts·O(18) Ts·O(18)
Complexity
Time
0.00039% 84.11% 0.66% 14.41% 0.29% 0.26% 0.26%
Proportion
The computational complexity analysis shows that the IESKF prediction step domi-
nates the total time with 84.11%, followed by the IESKF observation step at 14.41%, as both
involve computationally intensive matrix operations (O(N3 ) and O(MN2 + M3 ), respec-
tively). The error compensation, error reset and other minor steps contribute minimally
(<1%) due to simpler operations like O(N2 ). Notably, the inclusion of the FIS system has
a negligible impact on the overall runtime of the FIS-IESKF system, as FIS-specific steps
such as prediction with FIS and observation with FIS contribute less than 1% of the total
time. This demonstrates that FIS integration does not significantly slow down the overall
performance of the algorithm.
4. Conclusions
A multi-sensor fusion method using a fuzzy inference system (FIS) in the wheel-
inertial-visual odometry (WIVO) system for position estimation of two-wheel differential
speed mobile (TWDSM) robots in indoor environments is established in this paper. Due to
the dynamic characteristics of the system, applying fixed covariance matrices for process
noise and observation noise in the iterative Kalman filter algorithm is insufficient to adapt
effectively to the complex kinematic characteristics faced by the robot, as well as the
intricate impacts of changing lighting conditions and unstructured scenes in the camera
model on visual observation models. This paper provides a metrology for inferring the
fusion Kalman filter gain using a fuzzy inference system, establishing fuzzy inference
systems for both wheeled odometry and visual odometry across various scenarios. This
approach allows for dynamic predictions of the system covariance matrix noise.
Through experiments across eight distinct scenarios (straight motion, sharp turning,
slow turning, spin motion, environments with and without glass, and changing or stable
lighting), we demonstrate the effectiveness of the FIS-IESKF method by comparing it to
the EKF, UKF and ground truth trajectories obtained through Aruco markers. In terms of
trajectory similarity assessment, the tracking performance of the FIS-IESKF algorithm is on
average approximately 542% better than that of the EKF algorithm relative to the ground
truth. This indicates that the FIS-IESKF method produces trajectories that more closely
match the ground truth across various motion types, including straight, turning and spin
motions, as well as in complex environments like glass and changing lighting scenes.
Additionally, results from closed-loop movement tests of the robot indicate that the
FIS-IESKF algorithm significantly enhances the performance of rotational estimation, with a
global rotational RMSE of 0.149◦ , far outperforming the EKF (64.3963◦ ) and UKF (81.3227◦ ).
The global translational RMSE of the FIS-IESKF is less than 0.01 m, while the EKF and UKF
show higher errors of 0.0541 m and 0.0162 m, respectively. The FIS-IESKF also achieves
superior local adjacent frame performance with a translational RMSE of 0.0023 m and
Sensors 2024, 24, 7619 31 of 33
rotational RMSE of 0.2020◦ , outperforming both EKF and UKF in maintaining trajectory
accuracy in long-distance closed-loop motion.
In terms of adaptability to complex environments, the FIS-IESKF method shows higher
robustness in varying environments. In glass scenes, FIS-IESKF’s position estimation remains
closely aligned with the ground truth, while both EKF and UKF exhibit significant deviations.
Similarly, in scenarios with changing lighting conditions, the FIS-IESKF demonstrates higher
consistency, while the EKF and UKF exhibit considerable fluctuations in trajectory.
In terms of analysis to computational complexity, the inclusion of the FIS system
has negligible impact on the runtime of the FIS-IESKF algorithm, with FIS covariance
prediction steps contributing less than 1% of the total time. Therefore, the proposed method
is validated to show improved robustness and accuracy of multi-sensor fusion localization
for two-wheeled differential drive indoor mobile robots.
Author Contributions: Conceptualization, G.H. and Y.H.; methodology, G.H., H.H. and Y.H.; soft-
ware, G.H., H.H. and Y.Z.; validation, G.H., Y.Z., G.G. and G.T.; formal analysis, G.H., L.Z. and G.T.;
writing—original draft preparation, G.H. and H.H.; writing—review and editing, G.H., G.G, X.G.
and Y.H.; visualization, G.H. and H.H.; funding acquisition, Y.H. and X.G. All authors have read and
agreed to the published version of the manuscript.
Funding: This research was funded in part by [National Natural Science Foundation of China]
grant number [52005119]; in part by the [Guangxi Key Technologies R&D Program] grant num-
ber [Guike-AB24010249] and the [Innovation Project of GUET Graduate Education] grant number
[YCSW2024350], and the [China Postdoctoral Science Foundation] grant number [2021MD703864].
Institutional Review Board Statement: Not applicable.
Informed Consent Statement: Not applicable.
Data Availability Statement: The proposed method with codes can be found in our GitHub repository
(https://github.com/botlowhao/FIS-IESKF, accessed on 7 November 2024). The experimental images
and the codes of the proposed multi-fusion method can be found in this publicly shared repository.
Acknowledgments: The authors sincerely thank Yanyang Dang and Weijiang Xiao from TBEA Co.,
Ltd. for their help in providing the testing environment for the mobile robot.
Conflicts of Interest: The authors declare no conflicts of interest.
References
1. Ullah, I.; Adhikari, D.; Khan, H.; Anwar, M.S.; Ahmad, S.; Bai, X. Mobile robot localization: Current challenges and future
prospective. Comput. Sci. Rev. 2024, 53, 100651. [CrossRef]
2. Bavle, H.; Sanchez-Lopez, J.L.; Cimarelli, C.; Tourani, A.; Voos, H. From SLAM to Situational Awareness: Challenges and Survey.
Sensors 2023, 23, 4849. [CrossRef] [PubMed]
3. Bhat, S.; Kavasseri, A. Multi-Source Data Integration for Navigation in GPS-Denied Autonomous Driving Environments. Int. J.
Electr. Electron. Res. 2024, 12, 863–869. [CrossRef]
4. Huang, S.-P.; Chen, C.-B.; Wei, T.-Z.; Tsai, W.-T.; Liou, C.-Y.; Mao, Y.-M.; Sheng, W.-H.; Mao, S.-G. Range-Extension Algorithms
and Strategies for TDOA Ultra-Wideband Positioning. Syst. Sens. 2023, 23, 3088. [CrossRef] [PubMed]
5. Chen, W.; Zhou, C.; Shang, G.; Wang, X.; Li, Z.; Xu, C.; Hu, K. SLAM Overview: From Single Sensor to Heterogeneous Fusion.
Remote Sens. 2022, 14, 6033. [CrossRef]
6. Nagata, T.; Ishigami, G. Gyro-based odometry associated with steering characteristics for wheeled mobile robot in rough terrain.
Adv. Robot. 2016, 30, 1495–1508. [CrossRef]
7. Campos, C.; Elvira, R.; Rodríguez, J.J.G.; Tardós, J.D. ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual–Inertial,
and Multimap SLAM. IEEE Trans. Robot. 2021, 37, 1874–1890. [CrossRef]
8. Wang, J.; Shi, Z.; Zhong, Y. Visual SLAM incorporating wheel odometer for indoor robots. In Proceedings of the 2017 36th Chinese
Control Conference (CCC), Dalian, China, 26–28 July 2017; IEEE: Piscataway, NJ, USA; pp. 5167–5172.
9. Ye, H.; Chen, Y.; Liu, M. Tightly Coupled 3D Lidar Inertial Odometry and Mapping. In Proceedings of the 2019 International
Conference on Robotics and Automation (ICRA), Montreal, QC, Canada, 20–24 May 2019; IEEE: Piscataway, NJ, USA, 2019;
pp. 3144–3150.
10. Guo, F.; Yang, H.; Wu, X.; Dong, H.; Wu, Q.; Li, Z. Model-Based Deep Learning for Low-Cost IMU Dead Reckoning of Wheeled
Mobile Robot. IEEE Trans. Ind. Electron. 2024, 71, 7531–7541. [CrossRef]
Sensors 2024, 24, 7619 32 of 33
11. Kowalczuk, Z.; Merta, T. Evaluation of position estimation based on accelerometer data. In Proceedings of the 2015 10th
International Workshop on Robot Motion and Control (RoMoCo), Poznan, Poland, 6–8 July 2015; IEEE: Piscataway, NJ, USA,
2015; pp. 246–251.
12. Hong, S.K.; Park, S. Minimal-Drift Heading Measurement using a MEMS Gyro for Indoor Mobile Robots. Sensors 2008, 8,
7287–7299. [CrossRef]
13. Liu, C.; Zhao, J.; Sun, N.; Yang, Q.; Wang, L. IT-SVO: Improved Semi-Direct Monocular Visual Odometry Combined with JS
Divergence in Restricted Mobile Devices. Sensors 2021, 21, 2025. [CrossRef]
14. Kalman, R.E. A New Approach to Linear Filtering and Prediction Problems. J. Basic Eng. 1960, 82, 35–45. [CrossRef]
15. Friedland, B.; Irwin, B. Estimation of the state of a nonlinear process in the presence of nongaussian noise and disturbances.
J. Frankl. Inst. 1966, 281, 455–480. [CrossRef]
16. Havlík, J.; Straka, O. Performance evaluation of iterated extended Kalman filter with variable step-length. J. Phys. Conf. Ser. 2015,
659, 012022. [CrossRef]
17. Ullah, I.; Su, X.; Zhu, J.; Zhang, X.; Choi, D.; Hou, Z. Evaluation of Localization by Extended Kalman Filter, Unscented Kalman
Filter, and Particle Filter-Based Techniques. Wirel. Commun. Mob. Comput. 2020, 2020, 1–15. [CrossRef]
18. Julier, S.J.; Uhlmann, J.K. New extension of the Kalman filter to nonlinear systems. Signal Process. Sens. Fusion Target Recognit. VI
1997, 3068, 182–193.
19. Turner, R.; Rasmussen, C.E. Model based learning of sigma points in unscented Kalman filtering. Neurocomputing 2012, 80, 47–53.
[CrossRef]
20. Madyastha, V.; Ravindra, V.; Mallikarjunan, S.; Goyal, A. Extended Kalman Filter vs. Error State Kalman Filter for Aircraft
Attitude Estimation. In Proceedings of the AIAA Guidance, Navigation, and Control Conference, Portland, OR, USA, 8–11
August 2011; American Institute of Aeronautics and Astronautics: Reston, VA, USA, 2011; p. 6615.
21. Bell, B.M.; Cathey, F.W. The iterated Kalman filter update as a Gauss-Newton method. IEEE Trans. Autom. Control 1993, 38,
294–297. [CrossRef]
22. Qin, C.; Ye, H.; Pranata, C.E.; Han, J.; Zhang, S.; Liu, M. LINS: A Lidar-Inertial State Estimator for Robust and Efficient Navigation.
In Proceedings of the 2020 IEEE International Conference on Robotics and Automation (ICRA), Paris, France, 1 June 2020; IEEE:
Piscataway, NJ, USA, 2020; pp. 8899–8906.
23. Zhao, G.; Ban, Y.; Zhang, Z.; Shi, Y.; Chen, B.; Liu, H. Improving the interpolation accuracy of optical encoders via noise
suppression and signal correction. Sens. Actuators A Phys. 2024, 368, 115122. [CrossRef]
24. Zhang, F.; Li, A.; Yuan, S. Variational Bayesian Based Adaptive CKF-SLAM Algorithm under Non-steady Noise. In Proceedings
of the 2024 WRC Symposium on Advanced Robotics and Automation (WRC SARA), Beijing, China, 23 August 2024; IEEE:
Piscataway, NJ, USA, 2024; pp. 216–221.
25. Zhang, L.; Yang, C.; Chen, Q.; Yan, F. Robust H-infinity CKF/KF hybrid filtering method for SINS alignment. IET Sci. Meas.
Technol. 2016, 10, 916–925. [CrossRef]
26. Jiancheng, F.; Sheng, Y. Study on innovation adaptive EKF for inflight alignment of airborne POS. IEEE Trans. Instrum. Meas. 2011,
60, 1378–1388. [CrossRef]
27. Lin, C.-M.; Hsueh, C.-S. Adaptive EKF-CMAC-based multisensor data fusion for maneuvering target. IEEE Trans. Instrum. Meas.
2013, 62, 2058–2066. [CrossRef]
28. Ali, J. Strapdown inertial navigation system/astronavigation system data synthesis using innovation-based fuzzy adaptive
Kalman filtering. IET Sci. Meas. Technol. 2010, 4, 246–255. [CrossRef]
29. Maulana, E.; Muslim, M.A.; Zainuri, A. Inverse kinematics of a two-wheeled differential drive an autonomous mobile robot.
In Proceedings of the 2014 Electrical Power, Electronics, Communicatons, Control and Informatics Seminar (EECCIS), Malang,
Indonesia, 27–28 August 2014; IEEE: Piscataway, NJ, USA, 2014; pp. 93–98.
30. Wang, K.; Zhao, G.; Lu, J. A Deep Analysis of Visual SLAM Methods for Highly Automated and Autonomous Vehicles in
Complex Urban Environment. IEEE Trans. Intell. Transport. Syst. 2024, 25, 10524–10541. [CrossRef]
31. Yang, X.; Chen, P.; Lu, H. Design of an Improved Autonomous Tracked Vehicle Based on SLAM Algorithm. Artif. Intell. Med. Eng.
Educ. IOS Press 2024, 48, 366–373.
32. Ojeda, L.; Borenstein, J. Non-GPS navigation with the personal dead-reckoning system. Unmanned Syst. Technol. IX 2007,
6561, 110–120.
33. Harle, R. A Survey of Indoor Inertial Positioning Systems for Pedestrians. IEEE Commun. Surv. Tutor. 2013, 15, 1281–1293.
[CrossRef]
34. Vega-Heredia, M.; Muhammad, I.; Ghanta, S.; Ayyalusami, V.; Aisyah, S.; Elara, M.R. Multi-Sensor Orientation Tracking for a
Façade-Cleaning Robot. Sensors 2020, 20, 1483. [CrossRef]
35. Wu, Y.; Kuang, J.; Niu, X. Wheel-INS2: Multiple MEMS IMU-Based Dead Reckoning System With Different Configurations for
Wheeled Robots. IEEE Trans. Intell. Transp. Syst. 2023, 24, 3064–3077. [CrossRef]
36. Mur-Artal, R.; Tardos, J.D. ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras. IEEE Trans.
Robot. 2017, 33, 1255–1262. [CrossRef]
37. Schlegel, D.; Grisetti, G. HBST: A Hamming Distance Embedding Binary Search Tree for Feature-Based Visual Place Recognition.
IEEE Robot. Autom. Lett. 2018, 3, 3741–3748. [CrossRef]
Sensors 2024, 24, 7619 33 of 33
38. Lepetit, V.; Moreno-Noguer, F.; Fua, P. EPnP: An Accurate O(n) Solution to the PnP Problem. Int. J. Comput. Vis. 2009, 81, 155–166.
[CrossRef]
39. Ji, Z.; Zhou, F.; Tian, X.; Jiang, R.; Chen, Y. Probabilistic 3D ICP algorithm based on ORB feature. In Proceedings of the 2013 IEEE
Third International Conference on Information Science and Technology (ICIST), Yangzhou, China, 23–25 March 2013; pp. 300–304.
40. Heo, S.; Cha, J.; Park, C.G. EKF-Based Visual Inertial Navigation Using Sliding Window Nonlinear Optimization. IEEE Trans.
Intell. Transp. Syst. 2019, 20, 2470–2479. [CrossRef]
41. He, J.; Sun, C.; Zhang, B.; Wang, P. Adaptive Error-State Kalman Filter for Attitude Determination on a Moving Platform. IEEE
Trans. Instrum. Meas. 2021, 70, 1–10. [CrossRef]
42. Begum, M.; Mann, G.K.I.; Gosine, R.G. Integrated fuzzy logic and genetic algorithmic approach for simultaneous localization and
mapping of mobile robots. Appl. Soft Comput. 2008, 8, 150–165. [CrossRef]
43. Jakovljevic, Z.; Petrovic, P.B.; Mikovic, V.D.; Pajic, M. Fuzzy inference mechanism for recognition of contact states in intelligent
robotic assembly. J. Intell. Manuf. 2014, 25, 571–587. [CrossRef]
44. Bobyr, M.V.; Milostnaya, N.A.; Kulabuhov, S.A. A method of defuzzification based on the approach of areas’ ratio. Appl. Soft
Comput. 2017, 59, 19–32. [CrossRef]
45. Poulose, A.; Han, D.S. Hybrid Indoor Localization Using IMU Sensors and Smartphone Camera. Sensors 2019, 19, 5084. [CrossRef]
46. Ren, Z.-L.; Wang, L.-G.; Bi, L. Improved Extended Kalman Filter Based on Fuzzy Adaptation for SLAM in Underground Tunnels.
Int. J. Precis. Eng. Manuf. 2019, 20, 2119–2127. [CrossRef]
47. Chatterjee, A.; Matsuno, F. A Neuro-Fuzzy Assisted Extended Kalman Filter-Based Approach for Simultaneous Localization and
Mapping (SLAM) Problems. IEEE Trans. Fuzzy Syst. 2007, 15, 984–997. [CrossRef]
48. Zhang, D.; Chang, S.; Zou, G.; Wan, C.; Li, H. A Robust Graph-Based Bathymetric Simultaneous Localization and Mapping
Approach for AUVs. IEEE J. Oceanic Eng. 2024, 49, 1350–1370. [CrossRef]
49. Garigipati, B.; Strokina, N.; Ghabcheloo, R. Evaluation and comparison of eight popular Lidar and Visual SLAM algorithms. In
Proceedings of the 2022 25th International Conference on Information Fusion (FUSION), Linköping, Sweden, 4–7 July 2022; IEEE:
Piscataway, NJ, USA, 2022; pp. 1–8.
50. Xie, X.; Qin, Y.; Zhang, Z.; Yan, Z.; Jin, H.; Xu, M.; Zhang, C. GY-SLAM: A Dense Semantic SLAM System for Plant Factory
Transport Robots. Sensors 2024, 24, 1374. [CrossRef]
51. Ross, T.J. Fuzzy Logic with Engineering Applications; John Wiley & Sons: Hoboken, NJ, USA, 2010.
Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual
author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to
people or property resulting from any ideas, methods, instructions or products referred to in the content.