# Simultaneous Localization And Mapping (SLAM) using RTAB-Map

Sagarnil Das

###### Abstract

This paper implements Simultaneous Localization and Mapping (SLAM) technique
to construct a map of a given environment. A Real Time Appearance Based
Mapping (RTAB-Map) approach was taken for accomplishing this task. Initially,
a 2d occupancy grid and 3d octomap was created from a provided simulated
environment. Next, a personal simulated environment was created for mapping as
well. In this appearance based method, a process called Loop Closure is used
to determine whether a robot has seen a location before or not. In this paper,
it is seen that RTAB-Map is optimized for large scale and long term SLAM by
using multiple strategies to allow for loop closure to be done in real time
and the results depict that it can be an excellent solution for SLAM to
develop robots that can map an environment in both 2d and 3d.

###### Index Terms:

Robot, SLAM, RTAB-Map.

##  1 Introduction

In SLAM (Simultaneous Localization and Mapping), a robot must construct a map
of the environment, while simultaneously localizing itself relative to this
map. This problem is more challenging than localization or mapping, since
neither the map nor the robot poses are provided making this problem a
’chicken or a egg’ problem. With noise in the robot’s motion and measurements,
the map and robot’s pose will be uncertain, and the errors in the robot’s pose
estimates and map will be correlated. The accuracy of the map depends on the
accuracy of the localization and vice versa. Given a series of sensor
observations over discrete time steps , the SLAM problem is to compute an
estimate of the agent’s location and a map of the environment. In this paper,
two simulation environments were provided where SLAM was performed. The robot
was successfully able to localize itself and map the 3d world. The benchmark
environment is called kitchen-dining (Figure 1) and the second environment is
that of a cafeteria called sagar-cafe (Figure 2).

Figure 1: Kitchen-Dining World Figure 2: Sagar-Cafe World

##  2 Background

SLAM algorithms generally fall into 5 categories:

  1. 1.

Extended Kalman Filter SLAM (EKF)

  2. 2.

Sparse Extended Information Filter (SEIF)

  3. 3.

Extended Information Form (EIF)

  4. 4.

FastSLAM

  5. 5.

GraphSLAM

The two most useful approaches to SLAM are Grid based FastSLAM[1] and
GraphSLAM[2] and these two algorithms will be discussed here.

###  2.1 Grid based FastSLAM

The FastSLAM algorithm uses a custom particle filter approach to solve the
full SLAM problem with known correspondence. Using particles, FastSLAM
estimates a posterior over the robot’s path along with the map. Each of these
particles hold the robot’s trajectory which gives an advantage to SLAM to
solve the problem of mapping with known poses. In addition to the trajectory,
each particle holds a map and each feature of the map is represented by a
local Gaussian.

With the FastSLAM algorithm, the problem is now divided into two separate
independent problems, each of which aims to solve the problem of estimating
features of the map. To solve these independent mini-problems, FastSLAM will
use the low dimensional Extended Kalman Filter. While map features are treated
independently, dependency only exists between robot pose uncertainty. This
custom approach of representing the posterior with particle filter and
Gaussian is known by Rao-Blackwellized particle filter approach[3]. The Grid
based FastSLAM is really an extension of FastSLAM and it adapts FastSLAM to
grid maps.

With grid mapping algorithm, the environment can be modeled using grid maps
without predefining any landmark position. So by extending the FastSLAM
algorithm to occupancy grid maps, the SLAM problem can now be solved in an
arbitrary environment. While mapping the real world environment, mobile robots
equipped with range sensors can be used and the FastSLAM algorithm can be
extended to solve the SLAM problem in terms of grid maps.

$P(x_{0:t},m|z_{1:t},u_{1:t})=P(x_{0:t}|z_{1:t},u_{1:t})*P(m|x_{1:t},z_{1:t})$
(1)

The first term in the RHS represents the robot trajectory where just as in
FastSLAM, with the grid based FastSLAM, each particle holds a guess of the
robot’s trajectory.

The second term represents a map where each particle maintains its own. The
grid based FastSLAM algorithm will update each particle by solving the mapping
with known poses problem using the Occupancy grid mapping algorithm.

####  2.1.1 Grid based FastSLAM techniques

Adapting the FastSLAM algorithm to grid maps is altered in the grid based
FastSLAM algorithm. Since the grid based FastSLAM algorithm uses a particle
filter approach and represents the world in terms of grid maps, both MCL
(Monte Carlo Localization) and Occupancy Grid Mapping algorithm are combined.
Now three different techniques are needed which are represented by 3
probability functions to adapt FastSLAM to grid mapping. These techniques are
known as:

  1. 1.

Sampling Motion $P(x_{t}|x\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[k]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t-1}},u_{t})$:
Estimates the current pose given the $k^{th}$ particle’s previous pose and
controls u (MCL)

  2. 2.

Map Estimation $P(m_{t}|z_{t},x\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[k]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}},m\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[k]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t-1}})$:
Estimates the current map given the current measurements, the current $k^{th}$
particle’s pose and the previous $k^{th}$ particle map (use Occupancy Grid
Mapping)

  3. 3.

Importance Weight $P(z_{t}|x\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[k]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}},m^{[k]})$:
Estimates the current likelihood of the measurement given the current $k^{th}$
particle pose and the current $k^{th}$ particle map (MCL).

The sampling motion, map estimation and importance weight techniques are the
essence of the grid based FastSLAM algorithm. Grid based FastSLAM implements
them to estimate both the map and the robot’s trajectory, given the
measurements and the control. The grid based FastSLAM algorithm looks very
similar to Monte Carlo localization algorithm with some additional statements
concerning the map estimation.

Algorithm 1 Grid based FastSLAM

1:procedure Grid based FastSLAM($X_{t-1},u_{t},z_{t}$)

2: $\bar{X_{t}}=X_{t}=\phi$

3: for $k$ = 1 to $M$ do

4: $x\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[k]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}}$
= sample-motion-model($u_{t},x\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[k]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t-1}}$)

5: $w\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[k]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}}$
= measurement-model-map($z_{t},x\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[k]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}},m\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[k]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t-1}}$)

6: $m\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[k]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}}$
= updated-occupancy-grid($z_{t},x\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[k]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}},m\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[k]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t-1}}$)

7: $\bar{X_{t}}=\bar{X_{t}}+<x\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[k]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}},m\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[k]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}},w\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[k]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}}>$

8: for $k$ = 1 to $M$ do

9: draw i with probability $\propto{w\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[i]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}}}$

10: add $<x\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[i]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}},m\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}[i]}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}}>toX_{t}$

11: Return $X_{t}$

###  2.2 Graph SLAM

Graph SLAM is a SLAM algorithm that solves the full SLAM problem. This means
that the algorithm recovers the entire path and map, instead of just the
recent pose and map. This difference allows it to consider dependencies
between current and previous poses. One of the benefits of graph SLAM is the
reduced need for significant on-board processing capability. Another is graph
SLAM’s increased accuracy over fast SLAM. Fast SLAM uses particles to estimate
the robot’s most likely pose. However, at any point in time, it is possible
that there is not a particle in the most likely location. In fact, chances are
slim to none especially in large environments. Since graph SLAM solves the
full SLAM problem, this means that it can work with all of the data at once to
find the optimal solution.

In graph SLAM, the idea is to organize information in a graph. A node in the
graph represents either a robot pose $x_{t}$ at a specific time step t or the
location of a feature in the environment denoted as $m^{(i)}$ with i = 1. . .
. α . An edge in the graph represents either a measurement constraint between
a pose and a feature or a motion constraint between two successive poses.
Since the spatial constraint are soft, they can be considered as springs
connecting two masses. In this analogy, the full SLAM problem can be solved as
a global graph optimization problem. The optimal graph configuration is the
one where the springs are relaxed, and the forces on each of the nodes are
minimized.

The Maximum Likelihood Principle (MLE) is used to optimize the graph. When
applied to SLAM, likelihood tries to estimate the most likely configuration of
state and feature locations given the motion and measurement observations. The
measurement update at time step t is given by

$\bar{z^{t}}:=x_{t}+m\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}(i)}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}}$
(2)

which represents for instance a laser range finder measuring the distance to
the landmark m (i) . Equivalently, a motion update can be defined as

$\bar{x^{t}}:=x_{t-1}+u_{t}$ (3)

which could be realized as a control command instructing the robot to move a
certain distance $u_{t}$ . The update are assumed to have Gaussian noise. The
corresponding probability distributions are given by

$p_{u}(x_{t})=\frac{1}{{\sigma_{m}\sqrt{2\pi}}}e^{{{-\left({z_{t}-\bar{z_{t}}}\right)^{2}}\mathord{\left/{\vphantom{{-\left({z_{t}-\bar{z_{t}}}\right)^{2}}{2\sigma_{m}^{2}}}}\right.\kern-1.2pt}{2\sigma_{m}^{2}}}}$
(4)

$p_{m}(z_{t})=\frac{1}{{\sigma_{u}\sqrt{2\pi}}}e^{{{-\left({x_{t}-\bar{x_{t}}}\right)^{2}}\mathord{\left/{\vphantom{{-\left({x_{t}-\bar{x_{t}}}\right)^{2}}{2\sigma_{u}^{2}}}}\right.\kern-1.2pt}{2\sigma_{u}^{2}}}}$
(5)

In some simple cases it is possible to find an analytical solution to MLE by
converting the target function to the negative log-likelihood form

$J_{GraphSLAM}=\sum_{t}\left(\frac{z_{t}-\bar{z_{t}}}{\sigma_{m}}\right)^{2}+\sum_{t}\left(\frac{x_{t}-\bar{x_{t}}}{\sigma_{u}}\right)^{2}$
(6)

trying to minimize the sum of all constraints. In more complex realistic
scenarios, approximate numerical solutions are needed, for instance by
applying gradient descent techniques.

In real world, most systems are multi-dimensional and to tackle such
scenarios, matrices and covariances must be used. The state and measurement
are given by $x_{t}$ and $z_{t}$. The constraints are given by

$v_{t}:=z_{t}-h(x_{t},m_{t})$ (7)

$w_{t}:=x_{t}-g(x_{t-1},u_{t})$ (8)

where h() and g() represent the measurement and motion functions and $Q_{t}$
and $R_{t}$ are the covariances of the measurement and motion noise. The
multidimensional formula for the sum of all constraints is given by

$J_{GraphSLAM}=x\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}T}}\hss}\textsubscript{{\color[rgb]{0,0,1}0}}\Omega
x_{0}+\sum_{t}\left(w\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}T}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}}*R\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}-1}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}}*w_{t}+v\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}T}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}}*Q\hbox
to0.0pt{\textsuperscript{{\color[rgb]{1,0,0}-1}}\hss}\textsubscript{{\color[rgb]{0,0,1}t}}*v_{t}\right)$
(9)

The goal of graph SLAM is to create a graph of all robot poses and features
encountered in the environment and the most likely robot’s path and map of the
environment. Th is task can be broken down into two sections. The front-end
and the back-end.

####  2.2.1 Front End vs Back End

The front end of graph SLAM looks at how to construct the graph, using the
odometry and sensory measurements collected by the robot. This includes
interpreting sensory data, creating the graph and continuing to add nodes and
edges to it as the robot traverses the environment. Naturally the front end
can differ greatly from application to application depending on the desired
goal, including accuracy, the sensor used and other factors e.g. the front end
of a mobile robot applying SLAM in the office using a Laser Range finder would
differ greatly from the front end of a vehicle operating on a large outdoor
environment and using a stereo camera. The front end of graph SLAM also has
the challenge of solving the data association problem. In simpler terms, this
means accurately identifying whether features in the environment have been
previously seen.

The back end of graph SLAM is where the magic happens. The input to the back
end is the completed graph with all of the constraints and the output is the
most probable configuration of robot poses and map features. The back end is
an optimization process that takes all of the constraints and find the system
configuration that produces the smallest error. The back end is a lot more
consistent across applications. The front end and the back end can be
completed in succession or can be performed iteratively, with a back end
feeding an updated graph to the front end for further processing.

####  2.2.2 Using RTAB-Map for 3D Graph SLAM

RTAB-Map (Real Time Appearance Based Mapping)[4] is a graph based SLAM
approach. Appearance based SLAM means that the algorithm uses data collected
from vision sensors to localize the robot and map the environment. In
appearance based methods, a process called Loop Closure is used to determine
whether the robot has seen a location before. As the robot travels to new
areas in its environment, the map is expanded and the number of images that
each new image must be compared to increases. This causes the loop closure to
take longer with the complexity increasing linearly. RTAB-Map is optimized for
large scale and long term SLAM by using multiple strategies to allow for loop
closure to be done in real time. Figure 3 shows the block diagram of the front
end and the back end.

Figure 3: RTAB-Map Front end and Back End block diagram

##  3 Scene and Robot configurations

The ROS package slam_project deploys the RTAB-Map to perform SLAM on two
environments. The first environment is an environment provided by Udacity as a
part of this research project and is named Kitchen-Dining. The second
environment is a custom made environment of a cafeteria named sagar_cafe. Just
like other robotics project, this project has been organized into different
folders containing the meshes, Gazebo SDF files, scripts, robot model URDFs,
configuration files and launch files.

The URDF folder contains the files sagar_bot.xacro defining the links and
joints of the robot model used for physics simulation and visualization as
well as the file sagar_bot.gazebo specifying the Gazebo plugins for
differential drive, RGB-D camera and laser range finder. Section 3.1 goes into
more details about the robot model. Besides the provided kitchen-dining world,
the worlds directory contains a file named sagar_cafe.world defining a custom
indoor cafeteria environment in SDF format. Section 3.2 discusses this in more
details. The image and mesh files necessary to model the Hokuyo laser and
Kinect camera are downloaded from the Gazebo model database and stored in the
meshes folder. The launch folder contains four ROS node launch configurations,
as detailed in section 3.3. The config directory contains the RViz
configuration file, and a script for tele-operating the rover can be found in
scripts.

###  3.1 Robot model

Figure 1 and 2 depicts the robot model inside the Gazebo simulation
environments of both the provided and the custom made one. The URDF
specification can be found in the file sagar_bot.xacro. The transform tree
associated with the robot is shown in Figure 4.

Figure 4: Transform Tree of the Robot - sagar_bot

For this project, the robot from [5] was taken as a starting point. The xacro
file provides the shape and size of the robot in macro format. For the
sagar_bot, a fixed base is used. A single link, with the name defined as
”chassis”, encompassed the base as well as the caster wheels. Each link has
specific elements, such as the inertial or the collision elements. The chassis
is a cuboidal (or box), whereas the casters are spherical as denoted by their
”geometry” tags. Each link (or joint) has an origin (or pose) defined as well.
Every element of that link or joint will have its own origin, which will be
relative to the link’s frame of reference.

For this base, as the casters are included as part of the link (for stability
purposes), there is no need for any additional links to define the casters,
and therefore no joints to connect them. The casters do, however, have
friction coefficients defined for them, and are set to 0, to allow for free
motion while moving.

Two wheels were attached to the robot. Each wheel is represented as a link and
is connected to the base link (the chassis) with a joint. For each wheel, a
”collision”, ”inertial” and ”visual” elements are present. The joint type is
set to ”continuous” and is similar to a revolute joint but has no limits on
its rotation. It can rotate continuously about an axis. The joint will have
it’s own axis of rotation, some specific joint dynamics that correspond to the
physical properties of the joint like ”friction”, and certain limits to
enforce the maximum ”effort” and ”velocity” for that joint. The limits are
useful constraints in regards to a physical robot and can help create a more
robust robot model in simulation as well. To enable the robot to perform
appearance based mapping using visual odometry, the generic RGB camera of the
original model is upgraded to a Kinect RGB-D camera. The camera is mounted to
the front of the chassis to allow for unobstructed view, facing in forward
direction. The mesh files for the Kinect camera model are downloaded from the
Gazebo model database and included in the slam_project/meshes folder. Like the
original model, the rover is fitted with a Hokuyo 2D laser range finder. The
corresponding hokuyo link is mounted with a fixed joint on the top of the
chassis, to let the laser beans rotate without hitting any part of the robot.
the laser range finder provides more precise localization and thereby refines
geometric constraints The differential drive plugin is configured in the
sagar_bot.gazebo file to publish control commands to the /cmd_vel topic and
odometry messages to the /odom topic. The camera plugin is configured to
publish raw RGB images to /camera/rgb/image_raw and raw depth images to
/camera/depth/image_raw. The laser plugin is configured to publish messages of
type LaserScan to the /scan topic. A graphical view of the ROS topics and
nodes is shown in figure 5 and a closeup of the robot model is depicted in
Figure 6.

Figure 5: RQT graph of the topics after all the nodes are launched Figure 6:
Close up view of the robot model

###  3.2 Design of the World

As the second part of the project, the custom world is created in Gazebo. This
world is based on the cafe model inside Gazebo database. The base model is
customized with different objects like tables, beer can, people, trees etc.
These objects serve as distinctive elements in the base world for the robot to
distinguish and map. In this world, the kitchen cannot be entered by the
robot. A bird’s eye view of this world is provided in Figure 7.

Figure 7: Bird’s eye view of Sagar cafe

###  3.3 Launch file configuration

Four launch files are required for a successful mapping of the environments in
simulation. The gazebo simulation environment is (kitchen-dining or cafe)
specified in the world.launch file. The teleop.launch file launches the teleop
keyboard which is required for moving the robot in the simulation world. The
mapping.launch file is used to start the RTAB-Map node. This node is used for
loop closure detection using the ORB-SLAM algorithm. ORB-SLAM is a versatile
and accurate Monocular SLAM solution able to compute in real-time the camera
trajectory and a sparse 3D reconstruction of the scene in a wide variety of
environments, ranging from small hand-held sequences to a car driven around
several city blocks. It is able to close large loops and perform global re-
localization in real-time and from wide baselines. Finally, the rviz.launch
file starts visualization of the rover, sensor data, as well as map and camera
topics in RViz.Figure 8 depicts RViz view of the world at the starting point.

Figure 8: Starting point of the robot as seen in RViz

During the mapping of the environment, the mapping data is saved in the
rtabmap.db database. The localization.launch file can be started in order to
localize the robot during the run.

##  4 Results

The mapping was done by the robot controlled by the teleop keyboard. In order
to be able to have more than 3 loop closure detection, which was the project’s
benchmark, the robot was navigated through the full environment of both the
worlds so that it could collect more images.

###  4.1 Kitchen-Dining World

The mapping run in the provided world ended with 66 global loop closures. This
file has a size of 315 MB and is named as rtabmap_kitchen_dining.db. Figure 9
shows the robot’s trajectory as well as the 2d occupancy grid map of the
kitchen_dining world.

Figure 9: Robot’s trajectory and 2d occupancy grid map of the Kitchen-Dining
world

At the end of the multiple passes, a well structured 3d point cloud map was
created by using the Export 3d Clouds functionality. Figure 10 depicts the
reconstructed point cloud data. It can be seen that most features in the world
like the chairs and tables are reconstructed properly and are distinctive.
Figure 11 shows the RViz result of the same world after the end of the mapping
task.

Figure 10: Reconstructed Point cloud data in RTAB-Map viewer of the Kitchen-
Dining world Figure 11: Rviz view of the Kitchen-dining world

At the end of the map, the loop closures can be seen in the
rtabmap_kitchen_dining.db. Figure 12 shows one of them.

Figure 12: Loop Closure detection

###  4.2 Sagar-cafe World

In the custom made world - sagar_cafe.world, the robot performed well. As the
robot is very short, some of the taller objects like the people, trees are not
fully mapped. The kitchen also could not be traveled by the robot. Figure 13
and 14 depicts the RTAB-map view and the RViz view of this world respectively
at the end of the mapping.

Figure 13: Reconstructed Point cloud data in RTAB-Map viewer in the sagar_cafe
world Figure 14: RViz view of the sagar_cafe world

##  5 Discussion

In both the environments, successful mapping was performed in order to
identify the ground truth and the distinctive features of the environment
like, walls, tables, chairs, trees, people etc. In the sagar_cafe world, the
robot could not enter the kitchen area, which can be seen in right lower
corner of Figure 13 and Figure 14. One of the possible explanations might be
that there is a height difference of the floor between the living room and the
kitchen. Another possible explanation might be that there is a transparent
door separating those two rooms and hence the robot couldn’t pass through.

The generated 2d and 3d maps can be improved by doing more mapping runs which
cover the environment in a more complete manner and by optimizing the loop
closure detection further.

##  6 Conclusion / Future work

An interesting future work would be to explore the RTAB-Map package’s
visualization section in more details. The obstacle detection feature can be
deployed in order to extracts obstacles and the ground from your point cloud.
With this information in hand, these obstacles can be avoided when executing a
desired path. Another potential area would be Wifi signal strength mapping.
This feature allows the user to visualize the strength of your robot’s WiFi
connection. This can be important in order to determine where the robot may
lose its signal, therefore dictating it to avoid certain areas. The situation
can also be remedied with larger antennas.

##  7 End sections

###  7.1 Acknowledgements

The author sincerely thanks Udacity for their support and critiques throughout
the project. The kitchen-dining world was provided by them.

## References

  * [1] M. Montemerlo, S. Thrun, D. Koller and B. Wegbreit, “FASTSLAM: A FAC- TORED SOLUTION TO THE SIMULTANEOUS LOCALIZATION AND MAPPING PROBLEM,” In Proceedings of the AAAI National Conference on Arti- ficial Intelligence, pp. 593-598” 2002, AAAI.
  * [2] S. Thrun and M. Montemarlo, “THE GRAPH SLAM ALGORITHM WITH APPLICATIONS TO LARGE SCALE MAPPING OF URBAN STRUC- TURES,” The International Journal of Robotics Research volume 25, issue 5-6, pp. 403-429, 2006, IJRR.
  * [3] G. Grisetti, C. Stachniss and Wolfram Burgard, “IMPROVED TECHNIQUES FOR GRID MAPPING WITH RAO-BLACKWELLIZED PARTICLE FIL- TERS,” IEEE Transactions on robotics, volume 23, issue 1, pp. 34-46, 2007, (IEEE transactions).
  * [4] M. Labbe and F. Michaud, “ONLINE GLOBAL LOOP CLOSURE DETEC- TION FOR LARGE SCALE MULTI-SESSION GRAPH BASED SLAM,” IEEE/RSJ International Conference on Intelligent Robots and systems (2014), pp. 2661-2666, doi: 10.1109/IROS.2014.6942926.
  * [5] S. Das, “ROBOT LOCALIZATION IN A MAPPED ENVIRONMENT USING ADAPTIVE MONTE CARLO ALGORITHM,” unpublished, https://drive.google.com/open?id=13pkunfwA80RmJE8vxIkLJ8PTaOI6DR.


# Automatic dimensionality reduction of Twin-in-the-Loop Observers

Giacomo Delcaro, Federico Dettù, Simone Formentin,
and Sergio Matteo Savaresi The authors are with the Electronics, Computer
Science and Bioengineering Department (Dipartimento di Elettronica,
Informazione e Bioingegneria), Politecnico di Milano, Milan, Italy (email:
<EMAIL_ADDRESS>
###### Abstract

State-of-the-art vehicle dynamics estimation techniques usually share one
common drawback: each variable to estimate is computed with an independent,
simplified filtering module. These modules run in parallel and need to be
calibrated separately. To solve this issue, a unified Twin-in-the-Loop (TiL)
Observer architecture has recently been proposed: the classical simplified
control-oriented vehicle model in the estimators is replaced by a full-fledged
vehicle simulator, or digital twin (DT). The states of the DT are corrected in
real time with a linear time invariant output error law. Since the simulator
is a black-box, no explicit analytical formulation is available, hence
classical filter tuning techniques cannot be used. Due to this reason,
Bayesian Optimization will be used to solve a data-driven optimization problem
to tune the filter. Due to the complexity of the DT, the optimization problem
is high-dimensional. This paper aims to find a procedure to tune the high-
complexity observer by lowering its dimensionality. In particular, in this
work we will analyze both a supervised and an unsupervised learning approach.
The strategies have been validated for speed and yaw-rate estimation on real-
world data.

###### Index Terms:

vehicle dynamics, observer design, bayesian optimization, twin-in-the-loop,
dimensionality reduction.

##  I Introduction

###  I-A Background

Since the 80s, car manufacturers and their tier-one vendors have been
developing vehicle dynamics controllers that require unmeasured signals. In
light of both physical and economic limitations, the measurement of these
signals is impractical; thus, it becomes necessary to develop observers that
estimate the missing variables. A thorough review on the topic is presented in
[1]: in Table 1 the authors compare 33 state-of-the-art filters, which share
one common drawback: they are only able to estimate a subset of states, such
as tire forces, tire slip angles, vehicle sideslip and/or vehicle lateral
velocity, etc. The adopted models are quite simple, _e.g._ single-track model
to describe the lateral dynamic or quarter-car model for the vertical one. Due
to the lack of one unified full-state filter, many observers are executed in
parallel (see Figure 1 in [2]), often running in dedicated ECUs.
In recent times, the computational power available onboard vehicles has
increased to the point where we are able to run a multibody vehicle simulator
in real-time111See, e.g., https://www.vi-grade.com/en/products/autohawk.. We
can adopt this virtual vehicle inside state estimators and advanced control
architectures, as in the recent works by Riva et al. [3] and Dettù et al. [4,
5, 6] and Delcaro et al. [7]. Since the vehicle simulator (or Digital Twin DT)
is adopted inside the observer or the control architecture, we will refer to
these schemes as Twin-in-the-Loop (TiL) systems. In [3, 5], the authors have
already proven that the TiL architecture is able to outperform existing
benchmarks on experimental data.

###  I-B Motivations

In this paper, we will consider the TiL observer scheme, which exploits the DT
as the model of the system, and corrects the virtual vehicle’s states in real
time in order to track the real car with the simulated one.
Key advantages of using such an architecture are:

  1. 1.

carmakers already use vehicle simulators in order to prototype and develop the
vehicle, hence they do not need to develop a different model;

  2. 2.

all vehicle’s states and variables of interest can be estimated simultaneously
with one single observer. This means that a single observer has to be tuned.

Due to the intrinsic complexity of the vehicles’ simulator, we have to
consider the DT as a black-box model whose inputs are the driver’s commands.
This is not the first time a unified observer for the entire vehicle has been
put forth: the authors in [8] and [9] propose observers based on full-state
white-box vehicle models, corrected in a Kalman-filter-like fashion. Despite
being more accurate than the canonical singe-dynamic observers, these filters
still lack the fidelity of the simulation-based environment: the DT can easily
deal with complex phenomena such as aerodynamic and floor effects, which play
a fundamental role for both traction and stability when dealing with high-
performing vehicles at high speeds.
The white-box approach is alternative to the TiL one, and poses other
challenges, such as the tuning of the state and output covariance matrices,
which have hundreds of entries.
As analyzed in [3], [5] and [7] the main disadvantage of the TiL observer
architecture is the black-box nature of the DT: there is no explicit
analytical formulation of the model as a dynamic system. Therefore, we cannot
adopt classical filter-tuning theory, such as (extended or unscented) Kalman
filtering [10, 11, 12] or sliding-mode-observer tuning [13]. The correction
matrix $K$ can be tuned by solving a black-box optimization problem that aims
at minimizing the difference between the real and the estimated states. In
order to solve the optimization problem with statistical accuracy in spite of
a small number of data samples, we will use Bayesian optimization [14],
discussed in section II-C. Matrix $K$ relates the system outputs $y$ to its
states $x$ (see Figure 1), hence it has $n_{x}\times n_{y}$ entries, where
$n_{x}$ is the number of states and $n_{y}$ the number of outputs. Since we
are dealing with the entire vehicle at once, $n_{x}$ and $n_{y}$ are in
general large, and so is $K$. Optimizing a large number of parameters can be
complex, due to this reason the authors in [3] select a-priori just 5
parameters out of the original 252 entries and focus on optimizing them,
setting the remaining ones to 0.
The choice of the parameters to optimize can be suboptimal and represents one
of the most critical aspects of the previous literature.

###  I-C Related work

This paper focuses on introducing sparsity to an optimization problem.
Bayesian optimization is very well known to be able to optimize only a handful
of parameters, hence reducing the problem complexity significantly eases the
optimization procedure. In [15, 16, 17, 18], the authors state that BO is
limited, in practice, at 10-20 optimization variables; in [19] the authors
even limit its capabilities to 5-6 variables. In general, all black-box
optimization algorithms suffer from the curse of dimensionality: in order to
find convergence in large-scale black-box optimizations we need to have a lot
of samples. One of the reasons behind this is that the higher the dimensions,
the higher the average distance between samples: the expected distance between
two random points in $[0,1]^{d}$ approaches $\sqrt{d/6}$ [20]. Due to
computational limits, the number of samples for our case study will be
limited, and we will show that the optimization is far from convergence and
overfitting.
A typical way to decrease the optimization complexity lies in reducing the
dimensionality of the problem. This can be realized by feature selection or
feature extraction [21]. Feature extraction is usually more efficient than
feature selection, since it does not discard the original features but finds a
map from feature space into a new reduced subspace. Linear and non-linear
methods for building the map exist. Examples of linear mapping [22] are
Principal Component Analysis, Linear Discriminant Analysis, Factor Analysis;
non-linear mappings [21] include kernel linear mapping, non-negative matrix
factorization, manifold learning, etc.
Another approach for complexity reduction is the formulation of a sparse
optimization (SO) problem. Sparse optimization [23] (SO) has been introduced
in the last years in multiple fields: compressive sensing, image processing,
matrix completion, and more in general information extraction. All the
representations share the underlying rationale: minimizing the cardinality of
one variable while satisfying some constraints. One of the milestones that
made the development in this field possible is the 2006 theorem by Candès
[24], which allowed to turn the NP-hard sparsity-imposing problem based on the
$\ell_{0}$-norm into a convex problem that adopts the $\ell_{1}$-norm (under
some Restricted Isometric Property or Uniform Uncertainty Principle).

One of the fields of research in SO is matrix completion [25], which aims at
imposing sparsity to matrices by minimizing their rank (or, thanks to Candès’
theorem, by minimizing their nuclear norm [26]). While this theory may appear
to align closely with our objectives, it’s important to clarify that our
primary aim is not to minimize the rank of the matrix but to to alleviate the
burden on the optimizer by focusing on reducing the cardinality of the
matrices.

###  I-D Main contributions

In the following, we will discuss two data-driven procedures to reduce the
dimensionality of the optimization problem without introducing any prior to
the system.

  1. 1.

The first Dimensionality Reduction (DR) algorithm will be based on
$\ell_{1}$-norm regularization and will be labeled Supervised DR (SDR). This
data-driven technique consists of a two-stage optimization: the first black-
box optimization sorts the optimization variables based on their impact on the
estimator performance. The second stage of this algorithm is to optimize the
sole performance of the filter, using the reduced set of variables.

  2. 2.

The second DR algorithm has already been analyzed in one of our previous
publications [7]. It is based on Principal Component Analysis (PCA) and is
defined Unsupervised DR (UDR). In this work, a combined approach of SDR and
UDR will be discussed.

All the results of the previously mentioned algorithms will be compared to a
model-based DR (MBR).
We finally suggest a generic heuristic algorithm for dimensionality reduction.

###  I-E Article structure

The paper is structured as follows. The problem statement is reported in
Section II. Section III is devoted to building a Model-Based Reduction (MBR)
benchmark. Section IV focuses on the Supervised Dimensionality Reduction (SDR)
approach, and Section V covers the Unsupervised Dimensionality Reduction (UDR)
approach, while also showing SDR and UDR can be combined. We conclude the
paper with some final remarks in Section VI.

##  II PROBLEM STATEMENT

###  II-A TiL Architecture

In Figure 1 the architecture of the TiL observer is reported.

Figure 1: Twin-in-the-Loop Observer architecture.

The scheme resembles the classical observer schemes, such as Luenberger
observers or Kalman filters: the difference $e_{y}=y(t)-\hat{y}(t)$ between
the model outputs and the measured ones is turned into a state perturbation
$e_{x}$ via matrix $K$. In discrete time this becomes a two-step procedure:

  1. 1.

prediction step, performed internally by the DT:

$\begin{cases}\hat{x}^{-}(t)\,=\,f(\hat{x}(t-1),\;u(t))\\\
\hat{y}^{-}(t)\,=\,h(\hat{x}(t-1),\;u(t))\end{cases}$ (1)

being $\hat{x}(t)$ the estimated state, $u(t)$ the driver input and $y(t)$ the
measured signal. $f(\cdot)$ and $h(\cdot)$ model the DT’s behavior and are
unknown to the user.

  2. 2.

correction step:

$\hat{x}(t)=\hat{x}^{-}(t)+K(y(t)-\hat{y}^{-}(t))$ (2)

In Figure 1, $e_{x}(t)=\hat{x}(t)-\hat{x}^{-}(t)$ and
$e_{y}(t)=y(t)-\hat{y}^{-}(t)$. The sampling frequency of our sensors is 100
$Hz$, hence we will be correcting the DT every 10 $ms$.
Since the model of the system is highly non-linear, according to extended
Kalman filters theory the correction law should be time-varying. In [3], the
effectiveness of using just a linear, time invariant correction law has
already been proven, hence we can adopt the same approach and optimize just
one constant matrix $K$. The rationale behind this choice is the fact that the
DT’s outputs should be very close to the real ones, hence $e_{y}(t)$ will be
very small, and the corrective term $e_{x}(t)$ will be small too (but
necessary to track the real vehicle with the simulator). A time-varying matrix
$K$ would not considerably improve the results, but it would make the tuning
problem much more complicated.

###  II-B Performance optimization

Let us now define the procedure to tune matrix $K$ in a data-driven fashion.
The first step is to gather some data. In order to prove the capabilities of
the TiL scheme, we will choose a scenario in which single-dynamic observers do
not usually perform well: a high-performing vehicle in a racing circuit, where
multiple dynamics are excited simultaneously. Moreover, the driver was asked
to drive as aggressively as possible.
In the preliminary data acquisition, we need to measure both the vehicle’s
outputs $y_{meas}(t)$ and some states $x_{meas}(t)$. This is necessary since
we will need to minimize the difference between the estimated states
$\hat{x}(t)$ and the measured ones $x_{meas}(t)$. Note that the measurement of
the states is only necessary in the initial phase to tune $K$ and will not be
required any further. In order to minimize the difference between measured and
estimated states, a generic loss function $L_{K}$ can be defined:

$\displaystyle
L_{K}(x_{meas},\hat{x})=\sum_{i=1}^{n_{x}}w_{x_{i}}rms(x_{{meas}_{i}}-\hat{x}_{i})$
(3)
$\displaystyle\text{being:}\qquad\textstyle{rms(e_{i})=\sqrt{\frac{1}{L}\sum_{l=1}^{L}e_{i}(l)^{2}}}$

$w_{x_{i}}$ are the weights given to each state, which are used to both
normalize the different magnitudes of the states and sort the states’
importance. $e_{i}(l)$ is the l-th sample of signal $e_{i}$, which has $L$
total samples.
Thus, we can minimize the estimation error by selecting $K$ such that $L_{K}$
is minimized. We can formulate this as an optimization problem:

$\displaystyle K=\operatorname*{arg\,min}_{K}L_{K}(x_{meas},\,\hat{x})$ (4a)
$\displaystyle s.t.\quad
k_{i,j}\,\in\,[\,\underline{k}_{i,j},\,\overline{k}_{i,j}\,]\qquad\begin{matrix}\forall
i=1,\cdots,n_{x}\\\ \forall j=1,\cdots,n_{y}\end{matrix}$ (4b)

Being $k_{i,j}$ element $(i,j)$ of matrix $K$. Equation (4b) is a constraint
on the range of all entries of $K$. $\overline{k}_{i,j}$ and
$\underline{k}_{i,j}$ are respectively the upper and lower bounds for the
optimization variables $k_{i,j}$.

###  II-C Parallel bayesian optimization

Let us now select the optimization algorithm to solve problem (4). The cost
function depends on $\hat{x}$, which in turn depends on the black-box internal
behavior. Therefore, we do not have any prior knowledge on $L_{K}$, which can
in general be non-convex. Moreover, in order to evaluate the cost function on
a selected matrix $K$, we must simulate the DT, which is computationally
expensive. Due to these reasons, local optimizers or optimizers which require
a large number of objective function evaluations cannot be used (see section
2.3 in [7]). One of the most efficient classes of global optimizers is the one
based on surrogate functions [27, 28, 29]. This class of algorithms computes
the next point to evaluate by minimizing an acquisition function $a(K)$, which
finds a trade-off between exploring the unknown regions and exploiting the
current knowledge of the position of the minimum. In order to estimate the
unknown cost function $L(K)$, at each iteration $i$ these optimizers use the
previously $i-1$ sampled points to build a surrogate or meta model
$\hat{L}_{i}(K)$.
We will adopt one of the most used in this class of optimizers: the Bayesian
Optimization (BO) algorithm [14]. BO assumes $L_{K}$ as a realization of a
prior Gaussian Process (GP) [30] with zero mean and $\kappa(K,\,K^{\prime})$
covariance (or kernel): $L_{K}\sim\mathcal{GP}(0,\;\kappa(K,\,K^{\prime}))$.
At iteration $i$, we know the previously $i-1$ sampled points:
$\mathcal{D}_{i-1}=(K_{1,\ldots,i-1},\;L_{K_{1,\ldots,i-1}})$, hence we can
condition the prior GP and get a posterior GP:
$L_{K}|\,\mathcal{D}_{i-1}\sim\mathcal{GP}(m_{post}(K),\;\kappa_{post}(K,\,K^{\prime}))$.
$L_{K}|\,\mathcal{D}_{i-1}$ can be used as a meta-model of the unknown cost
function $L_{K}$. This step is known as Gaussian Process Regression (GPR).
We will adopt one of the most frequently used kernel functions for GP
regression: the Matérn $5/2$ function [30].

$\kappa(K_{m},\,K_{n})=\sigma_{f}^{2}\left(1+\sqrt{5}r+5r^{2}/3\right)e^{-\sqrt{5}r}$
(5a)

Where $r$ is the notion of distance between $K_{m}$ and $K_{n}$. In
particular, to optimize the distance in a high-dimensional optimization, we
can use the Automatic Relevance Determination (ARD) version of Matérn
function, introduced in [31]. In ARD, each dimensions is weighted by the
charateristic length-scales $\ell_{1},\dots,\ell_{d}$:

$r=\sqrt{(K_{m}^{vec}-K_{n}^{vec})^{T}M(K_{m}^{vec}-K_{n}^{vec})}$ (5b)

where $M=diag([\ell_{1},\cdots,\ell_{d}])^{-2}$ is a diagonal matrix and
$K_{*}^{vec}$ represents the flattening of matrix $K_{*}$ into an
$\mathbb{R}^{d}$ vector. The higher $\ell_{i}$, the smaller the impact of the
$i$-th dimension on the GP. Thus, we are able to remove irrelevant dimensions.
ARD is also known as Sparse Bayesian Learning (SBL) and has been proven
effective in several fields (see [32]).
As acquistion function, the expected improvement function [15] is chosen.
Since we aim at tuning an observer, we do not know a priori whether the chosen
$K_{i}$ at iteration $i$ will be asymptotically stable or not. We can cope
with this problem by adding a coupled constraint (as in [33]): a constraint
that can be evaluated only by sampling the objective function. The constraint
is modelled as an additional GP and considered in the acquisition function
$a_{i}$, to avoid sampling points in known unstable regions (see equation (8)
in [34]).
For the correct initialization, the optimization algorithm needs $n_{seed}$
initial points to start computing the meta-model. The algorithm terminates
when reaching a fixed number of iterations, defined as $N$.
As can be deduced from the preceding discussion, surrogate-function-based
optimizers adopt a sequential optimization approach: we first need to sample a
point, then build the surrogate model in order to determine the next point to
evaluate. We can speed up this procedure using parallel Bayesian optimization
[35]: we can use multiple parallel workers to sample different points
simultaneously. When a worker is free, parallel BO assigns it a new point to
evaluate by using all the previously sampled data enlarged with the points
which are currently being sampled by other workers. In order to enlarge the
dataset the algorithm requires an imputation method. We will use the model-
prediction method (or kriging believer heuristic in [35]), which imputes the
value of the objective function $L(K)$ as the mean of the GP at point $K$.
Since we are dealing with high-dimensional models, which can make overly
optimistic predictions, the imputed value is then saturated so as not to be
smaller than the minimum sampled point so far. This method is known as
clipped-model-prediction.
We can sum up parallel BO as in Algorithm 1.

1 Select $N$ number of total iterations, $n_{seed}$ number of initial points
($n_{seed}<N$) and $P$ parallel workers

2 Sample $n_{seed}$ seed points using all workers

3 $i\leftarrow n_{seed}+1$

4 while  _$i\leq N$_ do

5 if  _at least one worker is free_ then

6 If some workers are busy evaluating points, impute the values of $L_{K}$ in
those points

7 Using the available (actual and imputed) data, build a GP model for the
objective function and the constraints

8 Compute the acquisition function $a_{i}(K)$

9 $K_{i}\leftarrow\operatorname*{arg\,min}_{K}a_{i}(K)$

10 Evaluate $L(K_{i})$ on a free worker

11 $i\leftarrow i+1$

12

13 end if

14

15 end while

16Update the surrogate function

return _best evaluated point and best predicted feasible point_

Algorithm 1 Pseudo-code for parallel BO

###  II-D Case study

To validate the observer’s effectiveness, it is essential to thoroughly assess
and challenge its capabilities through stress testing. For this reason, the
driver was instructed to push the car to its limits, aiming to excite all
vehicle dynamics concurrently. The data presented in this paper refers to a
high-performing vehicle assessed on a racing track. In particular, two
datasets will be used. The optimization dataset lasts 30 seconds and is
reported in grey in Figure 2. The driving style is very aggressive, with
sideslip angles higher than 23 degrees at more than 100 $km/h$. The validation
dataset is quite aggressive too and consists of an entire lap of the track.
The available sensors onboard the vehicle are the following:

  * •

6-dof IMU (3D accelerometer and gyroscope) (6 signals);

  * •

wheel encoders (4 signals).

Overall, there are 10 measured signals. In order to know the ground truth of
our experiments, a Real-time-kinematic positioning system (RTK-GPS) was
installed on the vehicle. Note that the GPS data will not be used as input to
the observer, since it was mounted for testing purposes only. Besides, in
typical driving scenarios GPS sensors suffer from availability and signal
quality problems, caused for example by tall buildings or tunnels.
Due to the absence of suspension data, we will not deal with the vehicle’s
vertical dynamics. We will only make the DT track the real longitudinal and
lateral dynamics. In order to do so, the cost function in (II-B) must weigh
states which influence both dynamics. It is straightforward to verify that
without one of the two, the DT will not be able to track the real vehicle. The
chosen cost function is the following:

$L_{K}(x_{meas},\,\hat{x})=\underbrace{rms(v_{x}-\hat{v}_{x})}_{\text{long.
dynamic}}+\underbrace{rms(\omega_{z}-\hat{\omega}_{z})}_{\text{lat. dynamic}}$
(6)

Where the yaw rate $\omega_{z}$ is expressed in $deg/s$ while the longitudinal
velocity $v_{x}$ is in $km/h$. The first term in (6) is necessary to track the
longitudinal dynamic of the real vehicle, while $\omega_{z}$ allows the DT to
track the lateral movement of its real counterpart. Only one state per dynamic
is used to keep the cost function as simple as possible.
VI-CarRealTime (CRT)222https://www.vi-grade.com/en/products/vi-carrealtime/
from VI-Grade will be used as off-the-shelf black-box vehicle simulator. CRT
is commercially available and quite often used by car manufacturers for
vehicle prototyping. To simulate the vehicle, CRT adopts 28 internal states:

  * •

3D position, orientation, linear and angular velocities;

  * •

wheels’ positions and velocities;

  * •

suspensions’ stroke lengths and velocities.

Since CRT can run on Simulink, we will use MATLAB function
bayesopt333https://it.mathworks.com/help/stats/bayesopt.html.
Correcting all 28 states with all 10 measured signals would generate a
correction matrix with $n_{x}\times n_{y}=280$ entries, which is far too
highly dimensional for BO. Since running the digital twin is a computationally
expensive task, we decided to limit the number of iterations to $N=1000$.
Imposing sparsity in the problem while maintaining the number of iterations
can be beneficial: if the number of dimensions is too high the optimizer may
not converge to the absolute minimum, while a reduced number of variables is
more likely to converge to their absolute minimum. Nonetheless, it is to be
noted that the absolute minimum for the original problem is better than (or,
at least, exactly equal to) the reduced one, so the reduced problem may have a
better convergence to an overall worse result.
In order to reduce the dimensionality of our case study, only a subset of
states and outputs will be used in this paper. In particular, since no GPS
data is available, we will not correct the vehicle’s pose or its orientation.
Since the front and rear wheels are highly coupled, only the velocity of one
wheel for each pair will be corrected. As mentioned, no data from the
suspensions is available, hence the vehicle’s vertical dynamics will not be
considered. For the same reason, roll and pitch rates and the vertical
velocity will not be corrected. Finally, for the lateral dynamic we will only
weigh $\omega_{z}$, since it is directly measured and is involved in the cost
function equation (6).
To sum up, we will only correct the following 4 states:

  * •

$v_{x}$: longitudinal velocity

  * •

$\omega_{z}$: yaw rate

  * •

$\omega_{fl}$: FL wheel speed

  * •

$\omega_{rr}$: RR wheel speed

Finally, we need to select the outputs used to correct the aforementioned
states. Since three of the four states are directly measured, we will use only
the following outputs:

  * •

$\omega_{z}$: yaw rate

  * •

$\omega_{fl}$: FL wheel speed

  * •

$\omega_{rr}$: RR wheel speed

We are now dealing with a problem with cardinality $n_{x}\times n_{y}=12$.
Optimizing such a number of parameters with a surrogate-function-based
optimizer probably will not converge to the best solution, but at least the
problem is now manageable. We will now discuss how to tackle such a problem,
and how reducing its dimensionality affects the performance, convergence, and
computational times. The aim of this work does not reside in the pursuit of
the best possible observer or the estimation of the most complex dynamics,
which was already proven in [3], but rather in formulating methodologies for
the reduction of dimensionality of the filter.

##  III Benchmark: model-based reduction

Before introducing the data-driven dimensionality reduction techniques
(sections IV and V), we need to build a benchmark to compare the future
results to.
Matrix $K$ has 12 entries. Each element $k_{y_{j}\to x_{i}}$indicates that
state $x_{i}$ is corrected using the error on the measurement $y_{j}$.
We can leverage our prior knowledge of the system to eliminate certain
optimization variables. For this reason, we will define this reduction
Physics-Inspired Reduction or Model-Based Reduction (MBR). Furthermore,
through parameter sorting, we can implement varying degrees of reduction,
allowing us to investigate the impact of different reduction levels on the
resultant outcomes. The most important parameters are the auto-correlated ones
(i) (the ones in the lower diagonal of matrix $K$). The second and third class
of parameters are respectively made of the parameters that relate the wheel
speeds to the longitudinal velocity (ii) and cross-correlate the wheel speeds
(iii). The last class relates lateral and longitudinal dynamics (iv).

  1. i)

$k_{\omega_{z}\to\omega_{z}}$

  2. i)

$k_{\omega_{fl}\to\omega_{fl}}$

  3. i)

$k_{\omega_{rr}\to\omega_{rr}}$

  4. ii)

$k_{\omega_{fl}\to v_{x}}$

  5. ii)

$k_{\omega_{rr}\to v_{x}}$

  6. iii)

$k_{\omega_{fl}\to\omega_{rr}}$

  7. iii)

$k_{\omega_{rr}\to\omega_{fl}}$

  8. iv)

$k_{\omega_{z}\to v_{x}}$

  9. iv)

$k_{\omega_{z}\to\omega_{fl}}$

  10. iv)

$k_{\omega_{z}\to\omega_{rr}}$

  11. iv)

$k_{\omega_{fl}\to\omega_{z}}$

  12. iv)

$k_{\omega_{rr}\to\omega_{z}}$

###  III-A Optimization ranges heuristic

If we aim at solving equation (4), we need to know the optimization intervals
for each variable (4b). Finding the range for the optimization variables can
be one of the most difficult tasks for data-driven optimization algorithms. We
will now briefly describe the rationale behind our choice of the ranges. The
parameters will be split into three categories: auto-correlated ones (1) and
cross-correlated wheels (2) and all the remaining ones (4).

  1. 1.

Auto-correlated parameters: $k_{\omega_{z}\to\omega_{z}}$,
$k_{\omega_{fl}\to\omega_{fl}}$ and $k_{\omega_{rr}\to\omega_{rr}}$. Since the
dynamics of the system are quite slower than the DT update frequency (100
$Hz$), we can safely state that the states can be approximated as constants
(they slowly change): $x_{i}(t+1)\approx x_{i}(t)$. If we plug this solution
into the previously mentioned filter equations (1) and (2), we obtain:
$\hat{x}_{i}(t+1)\approx\hat{x}_{i}(t)+k_{i\to i}(x_{i}(t+1)-\hat{x}_{i}(t))$,
which is asymptotically stable if and only if: $k_{i\to i}\in(0,\;2)$. In our
tests, we have found that being close to 2 can lead to unstable filters. Thus,
for safety reasons, we reduce the limit to $k_{i\to i}\in(0,\;1.5)$.

  2. 2.

Cross-correlated wheel parameters: $k_{\omega_{fl}\to\omega_{rr}}$ and
$k_{\omega_{rr}\to\omega_{fl}}$. They are of the same nature of the previous
$k_{\omega_{fl}\to\omega_{fl}}$, thus we will keep the span of 1.5 but center
it in 0.

  3. 3.

All remaining 7 parameters. The order of magnitude of the intervals is roughly
given by range($x_{i}$)/range($y_{j}$)444The range $x(t)$ is defined as:
$\text{range}(x(t))=\max_{t}(x(t))-\min_{t}(x(t))$. Instead of using the
entire dataset to compute the range of each variable, the dataset was cut in
equally long intervals and the final span of each optimization variable was
set to be $1.5\cdot\text{mean}(\text{range}(x_{i})/\text{range}(y_{j}))$. All
intervals will be centered in 0, except for $k_{\omega_{fl}\to v_{x}}$ and
$k_{\omega_{rr}\to v_{x}}$, which will only be positive.

The optimization variable ranges are reported in Table I.

Parameter | $\boldsymbol{\underline{k}}$ |  $\boldsymbol{\overline{k}}$
---|---|---
$k_{\omega_{z}\to v_{x}}$ | -1.5 | 1.5
$k_{\omega_{z}\to\omega_{z}}$ | 0 | 1.5
$k_{\omega_{z}\to\omega_{fl}}$ | -1.5 | 1.5
$k_{\omega_{z}\to\omega_{rr}}$ | -1.5 | 1.5
$k_{\omega_{fl}\to v_{x}}$ | 0 | 0.45
$k_{\omega_{fl}\to\omega_{z}}$ | -0.06 | 0.06

Parameter | $\boldsymbol{\underline{k}}$ |  $\boldsymbol{\overline{k}}$
---|---|---
$k_{\omega_{fl}\to\omega_{fl}}$ | 0 | 1.5
$k_{\omega_{fl}\to\omega_{rr}}$ | -0.75 | 0.75
$k_{\omega_{rr}\to v_{x}}$ | 0 | 0.45
$k_{\omega_{rr}\to\omega_{z}}$ | -0.06 | 0.06
$k_{\omega_{rr}\to\omega_{fl}}$ | -0.75 | 0.75
$k_{\omega_{rr}\to\omega_{rr}}$ | 0 | 1.5

TABLE I: Optimization variables ranges.

###  III-B Optimization stack

We possess all the essential elements to solve problem (4). Let us recap the
optimization:

$\displaystyle K=$
$\displaystyle\operatorname*{arg\,min}_{K}\;rms(v_{x}-\hat{v}_{x})+rms(\omega_{z}-\hat{\omega}_{z})$
(7a) $\displaystyle s.t.\quad$ $\displaystyle
k_{i,j}\,\in\,[\,\underline{k}_{i,j},\,\overline{k}_{i,j}\,]\qquad\begin{matrix}\forall
i=1,\cdots,n_{x}\\\ \forall j=1,\cdots,n_{y}\end{matrix}$ (7b)
$\displaystyle\mathcal{G}(K)\leq 0$ (7c)

Where constraint (7c) represents the set of $K$ for which the filter is
asymptotically stable. $\mathcal{G}(K)$ is not known and must be estimated
alongside the objective function: it is a coupled constraint. In our
optimization setup (see Algorithm 1), $K_{i}$ is considered not asymptotically
stable when the simulation drifts away or diverges.
The optimizer adopted to solve the problem is Parallel Bayesian Optimization
(algorithm 1), with $n_{seed}=500$ and $N=1000$. Both $n_{seed}$ and $N$ are
referred to feasible iterations: iterations in which the sampled $K_{i}$ is
unstable are not counted. The number of parallel workers $P$ is set to 2.
We can now run the optimizations. We will run 4 types of optimization which
differ in the number of variables according to the previously mentioned
importance classes: the original 12-parameter problem, then 3 reduced problems
with 7, 5 and 3 parameters. The results of the optimizations are reported in
Figure 2. The state estimates of the different filters (colored lines) are
compared to the ground truth (grey line). The variables reported in Figure 2
are $v_{x}$ and $\omega_{z}$, which represent respectively the longitudinal
and lateral dynamic of the system and are equally weighted in the cost
function. The validation dataset is reported in Figure 3. The filters seem to
be able to generalize quite well and do not overfit the optimization dataset.
A zoomed segment of the validation dataset is shown in Figure 4. It can be
noted that the noise of the high dimensional optimization (blue line) is not
present in the lower dimensional optimizations. Furthermore, the smaller the
number of parameters, the smoother the estimated signals. The extreme case is
the 3-parameter optimization, which is too smooth and loses a lot of
information.

Figure 2: MBR - Optimization dataset comparison. Filters with different
numbers of parameters (colored lines) are compared against the ground truth
(grey line). Figure 3: MBR - Validation lap comparison. Filters with different
numbers of parameters (colored lines) are compared against the ground truth
(grey line). Figure 4: MBR - Zoomed segment of Figure 3.

###  III-C Statistical analysis

In order to assess whether these properties statistically hold, each type of
optimization must be repeated multiple times. We repeated each optimization 12
times. The solutions of the optimizations were all far from the variable range
limits except for the 3-dimensional one, which sets all three auto-correlated
parameters to their upper bound of 1.5.
The statistical results for the validation lap are reported in Figure 5. For
each optimization type, 2 boxplots are depicted, one representing the root-
mean-square-error of $v_{x}$ and the other one the computational time taken by
BO. The boxes represent the 25th and 75th percentiles of the distributions of
the points, while the horizontal black line represents the median. The smaller
dashed black lines illustrate the minimum and maximum recorded values. The
cost $rmse(\omega_{z})$ is not reported since $\omega_{z}$ is directly
measured: it is weighted in the objective function to make the DT track the
real system, but its performance of concern.
We can draw some conclusions on Figure 5:

  * •

the loss function line (blue line in Figure 5) slightly decreases from 12 to 5
parameters, but it increases again for the 3-dimensional case. In the context
of the original high-dimensional optimization problem (depicted on the left),
achieving convergence to the optimal solution presents a challenge: only
suboptimal solutions can be found. Conversely, excessively aggressive
reduction (on the right), proves to be counterproductive due to the loss of
critical degrees of freedom. The optimal balance, resulting in the best-
performing solutions, is typically found between these two extremes;

  * •

the variance tends to decrease as the dimensionality is reduced. This is due
to the better convergence to the global minimum for the simplified problems;

  * •

the computational time (orange line in 5) decreases with the number of
parameters. An in depth analysis of this phenomenon is reported in section
III-D.

In summary, pruning dimensions is beneficial in many ways: smaller
computational effort, better convergence, better performance. The clear
downside is that a too-aggressive reduction can lead to unsatisfactory
performance.

Figure 5: MBR - Validation lap boxplot. Blue boxes report the distributions of
the loss function for different degrees of reduction. Orange boxes represent
the distribution of computational effort. The optimizations are ordered in
decreasing order of optimization variables from left to right.

###  III-D Computational times

As shown in Figure 5, the computational times increase as the dimensionality
of the process increases. At any iteration, BO first needs to estimate a GP
meta-model of the objective function (step 1 in Algorithm 1), and later to
minimize the acquisition function (step 1 in Algorithm 1).
Let us first analyze the computational times of building the surrogate model.
GP regression has a complexity cubic with the number of input samples [36]: at
iteration $i$ fitting the GP has complexity $\mathcal{O}(i^{3})$. If we sum
all the computational times to build the GP at all iterations, we get an
overall complexity of $\mathcal{O}(N^{4})$, being $N$ the number of BO
iterations.
The covariance matrix of the GP depends on a kernel function, which depend on
some notion of distance between samples [30]. Thus, the GP regression has a
complexity that is linear in the dimensions $d$: $\mathcal{O}(d)$.
Once the GP model has been computed, the acquisition function $a_{i}(K)$ must
be minimized. Due to its non-convexity, the minimization process can be
difficult and its complexity is largely determined by the adopted solver. In
general, the computational effort is impacted by the dimensions $d$ and by the
complexity of the function to minimize.
To sum up, the increase in optimization time of Figure 5 is justified by the
increases in complexity of both the GP regression step and the minimization of
the acquisition function.

##  IV Supervised dimensionality reduction

In section III we have built the benchmark reduction (MBR) using apriori
knowledge of the system. Our goal now is to find an automatic way of reducing
the dimensionality of the problem from data. We will define this procedure as
Supervised Dimensionality Reduction (SDR).

###  IV-A Sparsity requirement

In problem-solving, Sparse Optimization theory (SO) [37, 23] analyzes how the
reduction of dimensionality can be beneficial for a problem. In particular, SO
is used to find sparse solutions to undetermined linear systems. In our case,
all the hypotheses that are typical of linear systems do not hold, but we can
adopt the main ideas of sparse optimization. There exist three sparse problem
formulations:

  * •

$\ell_{0}$-constrained problem formulation:

$K=\operatorname*{arg\,min}_{K}L_{K}(x_{meas},\,\hat{x})\quad
s.t.\;\;\|K\|_{0}\leq T$ (8a)

  * •

function-constrained formulation:

$K=\operatorname*{arg\,min}_{K}\|K\|_{0}\quad
s.t.\;\;L_{K}(x_{meas},\,\hat{x})\leq\overline{L}$ (8b)

  * •

weighted formulation:

$K=\operatorname*{arg\,min}_{K}L_{K}(x_{meas},\,\hat{x})+\lambda\|K\|_{0}$
(8c)

Where $\|K\|_{0}$ is the $\ell_{0}$ norm of matrix $K$:
$\;\|K\|_{0}\;=\allowbreak=\text{cardinality}\\{k_{i,j}\,:\;k_{i,j}\neq 0\\}$.
$T$ in equation (8a) is the maximum allowed number of parameters different
from 0 and $\overline{L}$ in equation (8b) is a constraint on the maximum cost
allowed. Finally, $\lambda$ in equation (8c) is a hyperparameter of the
optimization. According to the Lagrange multiplier theorem, a $\lambda$ can be
found such that problem (8c) is equivalent to (8a) or (8b).
The main issue behind equations (8) is the discontinuity given by the
$\ell_{0}$ norm: BO cannot correctly estimate highly discontinuous objective
functions or constraints. In order to solve the problem we can adopt the same
technique used in SO: we resort to $\ell_{1}$-norm minimization.

$\textstyle{\|K\|_{1}=\sum_{i,j}|k_{i,j}|}$ (9)

The most straightforward way of imposing sparsity to $K$ would be to use
formulation (8a) by adding the sparsity constraint to problem (7).
Unfortunately, while it is easy to assign $T$ for the $\ell_{0}$-norm
constraint, it is difficult to choose it for the $\ell_{1}$ norm. Adopting
formulation (8c) can be challenging: the hyperparameter $\lambda$ does not
have physical meaning. In order to tune it, we could use cross-validation
techniques, but this would require repeating many times a high-dimensional
optimization, without any guarantee of convergence. Thus, we will use
formulation (8b).
We can now formalize our sparse optimization problem, which aims at sorting
the optimization variables by importance, exactly as we did in section III
using our prior knowledge of the system. We will refer to this optimization as
Structure optimization.

$\displaystyle K=$
$\displaystyle\operatorname*{arg\,min}_{K}\;\|\widetilde{K}\|_{1}$ (10a)
$\displaystyle s.t.\quad$ $\displaystyle
k_{i,j}\,\in\,[\,\underline{k}_{i,j},\,\overline{k}_{i,j}\,]\qquad\begin{matrix}\forall
i=1,\cdots,n_{x}\\\ \forall j=1,\cdots,n_{y}\end{matrix}$ (10b)
$\displaystyle\mathcal{G}(K)\leq 0$ (10c) $\displaystyle
rms(\omega_{z}-\widehat{\omega}_{z})\,<\,\text{UB}_{\omega_{z}}$ (10d)
$\displaystyle rms(v_{x}-\widehat{v}_{x})\,\,<\,\text{UB}_{v_{x}}$ (10e)

In the objective function (10a), $\widetilde{K}$ contains all the normalized
parameters $\widetilde{k}_{i,j}$. Using the normalized parameters and not the
original ones is crucial due to the varying orders of magnitude.
The normalization is computed as follows:

$\begin{split}\widetilde{k}_{i,j}\,=\,\begin{cases}\left\lvert\displaystyle{{{k}_{i,j}}\,/\,{\overline{k}_{i,j}}}\right\lvert,&\text{if
}k_{i,j}\,>\,0\\\
\left\lvert\displaystyle{{k_{i,j}}\,/\,{{\underline{k}_{i,j}}}}\right\lvert,&\text{otherwise}\end{cases}\\\
\forall\,i\,=\,1,\dots,n_{x},\quad j\,=\,1,\dots,n_{y}\\\ \text{(}\,\text{by
construction }\underline{k}_{i,j}\leq 0\text{ and }\overline{k}_{i,j}\geq
0\,\text{)}\end{split}$ (11)

Constraint (10b) guarantees that the parameters are within the given
optimization ranges and (10c) represents the set of $K$ for which the filter
is asymptotically stable.
Constraints (10d) and (10e) guarantee a satisfactory performance on both
lateral (10d) and longitudinal (10e) dynamics. Thus, their role is to mimic
the longitudinal and lateral dynamic terms in (7a). The Upper Bounds on the
root-mean-square errors $\text{UB}_{\omega_{z}}$ and $\text{UB}_{v_{x}}$ have
physical meaning, hence they are much easier to select than the hyperparameter
$\lambda$ seen in formulation (8c). Differently from (10b), these constraints
share the same nature of (10c) and are unknown a priori.
In section III, the best optimizations had $rmse(v_{x})\approx 1.1\;km/h$ and
$rmse(\omega_{z})\approx 0.6\;deg/s$. By setting $\text{UB}(v_{x})=1.5\;km/h$
and $\text{UB}(\omega_{z})=1.5\;deg/s$ we can still have very good performance
for $v_{x}$ and acceptable one for $\omega_{z}$.
Note that we are now trying to solve a problem which seems to be more
complicated than the original one (7): it has the same number of optimization
variables but additional constraints. However, we are not seeking the true
global optimum, but one feasible solution that tends to be the optimal one: if
the performance constraints (10e) and (10d) are sufficiently strict, we are
already guaranteed that the solution will be good enough. Once the solution of
(10) has been found (or even a suboptimal solution), we know which are the
most influential parameters for the optimization problem and we can run a
lower dimensional optimization to only improve the performance (problem (7)).
Thanks to the structure optimization, we manage to turn a non-converging high
dimensional performance problem (7) into a probably non-converging sparse
optimization (10) followed by a reduced performance problem with a much
greater probability of convergence to a better solution.

###  IV-B Structure optimization procedure

We are now able to run structure optimization (10), whose results are reported
in Table II.

Parameter | $\boldsymbol{\widetilde{k}_{i,j}}$ |  $\boldsymbol{k_{i,j}}$
---|---|---
$k_{\omega_{rr}\to\omega_{rr}}$ | 56.9% | 0.85
$k_{\omega_{z}\to\omega_{rr}}$ | 55.6% | -0.83
$k_{\omega_{z}\to\omega_{z}}$ | 42.2% | 0.63
$k_{\omega_{fl}\to\omega_{fl}}$ | 41.5% | 0.62
$k_{\omega_{z}\to v_{x}}$ | 35.2% | 0.53
$k_{\omega_{fl}\to v_{x}}$ | 26.2% | 0.11

Parameter | $\boldsymbol{\widetilde{k}_{i,j}}$ |  $\boldsymbol{k_{i,j}}$
---|---|---
$k_{\omega_{rr}\to\omega_{fl}}$ | 7.7% | 0.058
$k_{\omega_{fl}\to\omega_{rr}}$ | 5.5% | -0.041
$k_{\omega_{rr}\to\omega_{z}}$ | 3.8% | 0.0023
$k_{\omega_{fl}\to\omega_{z}}$ | 3.5% | -0.0021
$k_{\omega_{rr}\to v_{x}}$ | 2.1% | 0.0095
$k_{\omega_{z}\to\omega_{fl}}$ | 2.0% | 0.030

TABLE II: Result of the structure optimization (10).

In order to remove some of the optimization variables we need to select a
threshold $\delta$: all the optimal normalized $\tilde{k}_{i,j}$ which are
smaller than $\delta$ will be pruned. Finally, the remaining parameters must
be optimized according to the performance optimization problem (7).
In section III, we have selected varying degrees of reduction; to replicate
the same effect three different thresholds $\delta$ are chosen: 0.05, 0.10 and
0.40. The resulting optimization classes will have 8, 6 and 4 parameters.

Figure 6: SDR - Validation lap comparison. Filters with different numbers of
parameters (colored lines) are compared against the ground truth (grey line).
Figure 7: SDR - Zoomed on Figure 6.

###  IV-C Statistical analysis

As in section III-C, to evaluate the statistical validity of these results we
repeated 12 times each optimization class. The optimal $K$ returned by each
optimization was far from the variable range limits, with the only exception
being the 4-dimensional case, where all three correlated parameters were set
to their upper bound of 1.5.
The boxplots containing performance results for the validation lap are
reported in Figure 8. Optimizations are ordered from left to right in
decreasing order of variables. We can draw similar conclusions to the MBR case
(Figure 5):

  * •

there is a clear parabolic trend in the average filter performance (blue
dashed line): the high-dimensional optimization (on the left) is not
converging to its global optimum, while the low-dimensional one (on the right)
does not have all the degrees of freedom to converge to a well-performing
solution;

  * •

the variance in performance and computational times decrease with the number
of parameters. The ”+15h” label indicates the structure optimization time.

Overall, we can conclude that the dimensionality reduction is beneficial to
improve the performance. The best solution (8-parameter one) is able to
improve on average by 35% with respect to the original optimization. Note that
this improvement comes with a computational cost: while the original
optimization takes about 10 hours, the 8-dimensional one takes about $6+15=21$
hours! The lack of prior information results in increased computational
overhead.

Figure 8: SDR - Validation lap boxplot. Blue boxes report the distributions of
the loss function for different degrees of reduction. Orange boxes represent
the distribution of computational effort. The optimizations are ordered in
decreasing order of optimization variables from left to right.

##  V Unsupervised dimensionality reduction

In this section, we will deal with numerically intractable problems. For the
moment we have seen problems in which the solver is always able to find at
least a ”good enough” solution (as in the 12-dimensional case). If we
considered correcting more states or using more system outputs, we would
increase considerably the number of entries of $K$. This exact case is treated
in our previous work [7]: 4 outputs and 4 states are considered, for a total
of 16 optimization variables. We will now highlight the main contributions of
[7] in order to be able to compare this method with the supervised method. The
input space is now too highly-dimensional for BO: the problem is numerically
intractable with our solver.
As mentioned in section I-C, we will now adopt a different way of imposing
sparsity: we will use feature extraction to reduce the dimensions of the input
to the observer (which are the system outputs). A visual representation of
this is presented in Figure 9, where the TiL-Filtering scheme of Figure 1 is
expanded with a feature-reducing map. The new correction matrix $K_{red}$ has
a number of inputs $\tilde{n}_{y}<n_{y}$.
In [7], the authors adopt Principal Component Analysis (PCA) to reduce the
dimensionality of $e_{y}$ from 4 to 3 dimensions, turning a 16-dimensional
problem into a 12-dimensional one. Since the dimensionality reduction step is
blind to the real performance objective, we will define this method
Unsupervised Dimensionality Reduction (UDR).
The main issue which arises from the feature extraction step comes from the
optimization variables upper and lower bounds (7b): they need to be specified
in terms of $K_{red}$ and not $K$. Theorem 1 in [7] finds a sufficient but not
necessary condition that is able to turn the limits on $K$ into limits on
$K_{red}$ as long as the feature extraction step is linear.

Figure 9: Twin-in-the-Loop Observer architecture with input reduction.

###  V-A Statistical analysis

Since the optimization and validation datasets employed in [7] are the same as
the ones adopted in this work, we will directly present the statistical
analysis of the results. Refer to [7] for any doubt in the methodology. Figure
10 reports the boxplots that compare the 16-dimensional optimization reduced
to 12 using PCA and the native 12-dimensional benchmark.
Note that PCA is able to get better results on average (blue line), but at the
cost of an increased variance and doubled computational effort. These
drawbacks are not surprising: Theorem 1 [7] is only sufficient, hence the
limits in $K_{red}$ are intrinsically larger than their $K$ counterpart. This
introduces more complex unstable regions, which increase the number of
unfeasible BO iterations, thus, the computational effort.

Figure 10: UDR - Validation lap boxplot. Blue boxes report the distributions
of the loss function for different degrees of reduction. Orange boxes
represent the distribution of computational effort. The optimizations on the
left are from the 16-dimensional case reduced to 12-dimensional using PCA; the
ones on the right are the native 12-dimensional benchmark.

###  V-B Combined SDR and UDR approach

Let us now study the combination of the previous supervised and unsupervised
approaches.
In the previous section, we have analyzed a 12-dimensional problem reduced
from a 16 one. We have stated multiple times that BO is not able to find the
true optimal solution in such a high-dimensional space. Thus, let us apply SDR
to our PCA-reduced solution. We will choose two values of $\delta$: 0.07 and
0.1, which respectively correspond to removing 2 and 4 variables. More severe
reductions (i.e. for $\delta>0.1$) proved to be unstable.
The validation laps for each optimization are reported in Figure 11.
The statistical results obtained by repeating each optimization type 12 times
are reported in Figure 12. The parabolic trend present in 8 is also evident
here. Due to the complexity of the feature extraction layer, the variance of
the lower-dimensional case is not the smallest. This is even more evident if
we remove more parameters, since the observer becomes unstable.

Figure 11: Combined SDR and UDR approach - Validation lap comparison. Filters
with different numbers of parameters (colored lines) are compared against the
ground truth (grey line). Figure 12: Combined SDR and UDR approach -
Validation lap boxplot. Blue boxes report the distributions of the loss
function for different degrees of reduction. Orange boxes represent the
distribution of computational effort. The optimizations are ordered in
decreasing order of optimization variables from left to right. Figure 13:
Result comparison boxplot. This figure summarizes Figures 5, 8 and 10.

###  V-C Overall comparison

Let us now compare the three techniques (MBR, SDR, and UDR). Figure 13 reports
all the statistical analyses seen in Figures 5, 8 and 10 (Figure 12 is here
not reported for the sake of simplicity). In the abscissae, the optimization
types are ordered in decreasing number of optimization variables. Starting
from the left, the first red column represents the native 16-dimensional
problem reduced to 12-dimensional using PCA. The second column is the native
12-dimensional problem. The remaining 6 columns represent different reductions
using MBR (green) and SDR (yellow). Note that the computational times (orange
lines) are evidently decreasing with the complexity of the optimizations: this
was studied in detail in section III-D. The computing effort for the
unsupervised case is more than doubled with respect to all other
optimizations, due to the additional complexity given by the PCA layer.

##  VI Final remarks and conclusions

###  VI-A Heuristics for dimensionality reduction

Figure 14 represents a control flow diagram showing a heuristic on how to
tackle problems as the one presented in the paper. When approaching a large-
scale black-box optimization problem, we need to ask ourselves whether we can
exploit any prior knowledge on the system. If the problem is not tractable –
which is a vague definition, but we consider it to be so if the solver is not
able to find a good solution in a reasonable time – we can only use UDR. Once
the problem is again tractable, we can decide to reduce its dimensionality
using SDR or directly run the performance optimization. The determination of
which approach to take relies once again on the solver and the specific
application and solver: it’s crucial to ascertain a deep knowledge of the
number of variables that can be effectively optimized.

ptSTARTPriorknowledge? Model-based reductionNumericallytractable?Too
manyparameters? Unsupervised  reduction  Performance optimization Supervised
reduction ENDptYptNptYptNptNptY

Figure 14: Control flow diagram

###  VI-B Conclusions

In this paper, we have studied two dimensionality reduction techniques for
non-converging large-scale black-box optimization. In section III we have
built a model-based benchmark (MBR), and in sections IV and V two different
reduction approaches have been proposed: SDR and UDR. A combined approach of
the two techniques has been analyzed in section V-B. All the reduction
techniques have been tested on the performance optimization (Section 7) on a
challenging scenario: a high-performing vehicle on a race track.
Thanks to the heuristic proposed in section III-A we were able to find all the
parameters we needed to run optimization (7) with minimal prior knowledge on
the system.

## Acknowledgments

The authors are grateful to company VI-Grade GmbH for their technical support.

## References

  * [1] K. B. Singh, M. A. Arat, and S. Taheri, “Literature review and fundamental approaches for vehicle and tire state estimation,” _Vehicle System Dynamics_ , vol. 57, no. 11, pp. 1643–1665, 2019.
  * [2] M. Viehweger, C. Vaseur, S. van Aalst, M. Acosta, E. Regolin, A. Alatorre, W. Desmet, F. Naets, V. Ivanov, A. Ferrara, and A. Vittorino, “Vehicle state and tyre force estimation: demonstrations and guidelines,” _Vehicle System Dynamics_ , vol. 59, no. 5, pp. 675–702, 2021.
  * [3] G. Riva, S. Formentin, M. Corno, and S. M. Savaresi, “Twin-in-the-loop state estimation for vehicle dynamics control: theory and experiments,” 2022. [Online]. Available: https://arxiv.org/abs/2204.06259
  * [4] F. Dettù, S. Formentin, and S. M. Savaresi, “The twin-in-the-loop approach for vehicle dynamics control,” _IEEE/ASME Transactions on Mechatronics_ , pp. 1–12, 2023.
  * [5] F. Dettù, S. Formentin, and S. M. Savaresi, “Joint vehicle state and parameters estimation via twin-in-the-loop observers,” 2023. [Online]. Available: https://arxiv.org/abs/2309.01461
  * [6] F. Dettù, S. Formentin, and S. M. Savaresi, “Robust tuning of twin-in-the-loop vehicle dynamics controls via randomized optimization,” in _Proc. 22nd IFAC World Congress_ , 2023.
  * [7] G. Delcaro, F. Dettù, S. Formentin, and S. M. Savaresi, “Dealing with the curse of dimensionality in twin-in-the-loop observer design,” in _Proc. 22nd IFAC World Congress_ , 2023.
  * [8] A. J. Rodríguez, E. Sanjurjo, R. Pastorino, and M. Ángel Naya, “State, parameter and input observers based on multibody models and kalman filters for vehicle dynamics,” _Mechanical Systems and Signal Processing_ , vol. 155, p. 107544, 2021.
  * [9] L.-Y. Hsu and T.-L. Chen, “Vehicle full-state estimation and prediction system using state observers,” _IEEE Transactions on Vehicular Technology_ , vol. 58, no. 6, pp. 2651–2662, 2009.
  * [10] R. E. Kalman, “A New Approach to Linear Filtering and Prediction Problems,” _Journal of Basic Engineering_ , vol. 82, no. 1, pp. 35–45, 1960.
  * [11] M. I. Ribeiro, “Kalman and extended kalman filters: Concept, derivation and properties,” _Institute for Systems and Robotics_ , vol. 43, no. 46, pp. 3736–3741, 2004.
  * [12] E. Wan and R. Van Der Merwe, “The unscented kalman filter for nonlinear estimation,” in _Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium (Cat. No.00EX373)_ , 2000, pp. 153–158.
  * [13] S. K. Spurgeon, “Sliding mode observers: a survey,” _International Journal of Systems Science_ , vol. 39, no. 8, pp. 751–764, 2008.
  * [14] B. Shahriari, K. Swersky, Z. Wang, R. P. Adams, and N. de Freitas, “Taking the human out of the loop: A review of bayesian optimization,” _Proceedings of the IEEE_ , vol. 104, no. 1, pp. 148–175, 2016.
  * [15] P. I. Frazier, “A tutorial on bayesian optimization,” 2018. [Online]. Available: https://arxiv.org/abs/1807.02811
  * [16] D. Eriksson and M. Jankowiak, “High-dimensional Bayesian optimization with sparse axis-aligned subspaces,” in _Proceedings of the Thirty-Seventh Conference on Uncertainty in Artificial Intelligence_ , ser. Proceedings of Machine Learning Research, C. de Campos and M. H. Maathuis, Eds., vol. 161\. PMLR, 27–30 Jul 2021, pp. 493–503. [Online]. Available: https://proceedings.mlr.press/v161/eriksson21a.html
  * [17] R. Moriconi, M. P. Deisenroth, and K. Sesh Kumar, “High-dimensional bayesian optimization using low-dimensional feature spaces,” _Machine Learning_ , vol. 109, pp. 1925–1943, 2020.
  * [18] M. Binois and N. Wycoff, “A survey on high-dimensional gaussian process modeling with application to bayesian optimization,” _ACM Transactions on Evolutionary Learning and Optimization_ , vol. 2, no. 2, pp. 1–26, 2022.
  * [19] L. Sabug, F. Ruiz, and L. Fagiano, “SMGO-$\Delta$: Balancing caution and reward in global optimization with black-box constraints,” _Information Sciences_ , vol. 605, pp. 15–42, 2022. [Online]. Available: https://www.sciencedirect.com/science/article/pii/S0020025522004376
  * [20] M. Köppen, “The curse of dimensionality,” in _5th online world conference on soft computing in industrial applications (WSC5)_ , vol. 1, 2000, pp. 4–8.
  * [21] W. Jia, M. Sun, J. Lian, and S. Hou, “Feature dimensionality reduction: a review,” _Complex & Intelligent Systems_, vol. 8, no. 3, pp. 2663–2693, 2022.
  * [22] J. P. Cunningham and Z. Ghahramani, “Linear dimensionality reduction: Survey, insights, and generalizations,” _The Journal of Machine Learning Research_ , vol. 16, no. 1, pp. 2859–2900, 2015.
  * [23] S. Wright, “Sparse optimization methods,” in _Conference on Advanced Methods and Perspectives in Nonlinear Optimization and Control_ , 2010.
  * [24] E. J. Candès, J. K. Romberg, and T. Tao, “Stable signal recovery from incomplete and inaccurate measurements,” _Communications on Pure and Applied Mathematics_ , vol. 59, no. 8, pp. 1207–1223, 2006.
  * [25] E. Candès and B. Recht, “Exact matrix completion via convex optimization,” _Commun. ACM_ , vol. 55, no. 6, p. 111–119, jun 2012. [Online]. Available: https://doi.org/10.1145/2184319.2184343
  * [26] E. J. Candes and Y. Plan, “Matrix completion with noise,” _Proceedings of the IEEE_ , vol. 98, no. 6, pp. 925–936, 2010.
  * [27] N. V. Queipo, R. T. Haftka, W. Shyy, T. Goel, R. Vaidyanathan, and P. Kevin Tucker, “Surrogate-based analysis and optimization,” _Progress in Aerospace Sciences_ , vol. 41, no. 1, pp. 1–28, 2005.
  * [28] A. I. Forrester and A. J. Keane, “Recent advances in surrogate-based optimization,” _Progress in Aerospace Sciences_ , vol. 45, no. 1, pp. 50–79, 2009.
  * [29] A. Bemporad, “Global optimization via inverse distance weighting and radial basis functions,” _Computational Optimization and Applications_ , vol. 77, no. 2, pp. 571–595, 2020.
  * [30] C. K. Williams and C. E. Rasmussen, _Gaussian processes for machine learning_. MIT press Cambridge, MA, 2006, vol. 2, no. 3.
  * [31] D. J. MacKay and R. M. Neal, “Automatic relevance determination for neural networks,” in _Technical Report in preparation._ Cambridge University, 1994.
  * [32] S. H. Rudy and T. P. Sapsis, “Sparse methods for automatic relevance determination,” _Physica D: Nonlinear Phenomena_ , vol. 418, p. 132843, 2021\. [Online]. Available: https://www.sciencedirect.com/science/article/pii/S0167278921000014
  * [33] M. Khosravi, C. König, M. Maier, R. S. Smith, J. Lygeros, and A. Rupenyan, “Safety-aware cascade controller tuning using constrained bayesian optimization,” _IEEE Transactions on Industrial Electronics_ , vol. 70, no. 2, pp. 2128–2138, 2023.
  * [34] M. A. Gelbart, J. Snoek, and R. P. Adams, “Bayesian optimization with unknown constraints,” 2014.
  * [35] D. Ginsbourger, R. Le Riche, and L. Carraro, “A multi-points criterion for deterministic parallel global optimization based on gaussian processes,” 2008\.
  * [36] H. Liu, Y.-S. Ong, X. Shen, and J. Cai, “When gaussian process meets big data: A review of scalable gps,” _IEEE Transactions on Neural Networks and Learning Systems_ , vol. 31, no. 11, pp. 4405–4423, 2020.
  * [37] Z. Zhang, Y. Xu, J. Yang, X. Li, and D. Zhang, “A survey of sparse representation: Algorithms and applications,” _IEEE Access_ , vol. 3, pp. 490–530, 2015.

|  Giacomo Delcaro received the B.Sc. and M.Sc. degrees (cum laude) in
Automation and Control Engineering at Politecnico di Milano, Milan, Italy in
2020 and 2022, respectively. In September 2020, he joined the mOve Research
Group as a M.Sc. student in the setting of the IndyAutonomousChallenge. In
2021, he spent four months in Indianapolis (USA) as software developer and
vehicle engineer for the above-mentioned competition. In November 2022, he
joined the mOve Research Group as a Ph.D. Student in Information Technology,
Systems and Control area, at the Department of Electronics, Computer Sciences
and Bioengineering (DEIB) of Politecnico di Milano. His research focuses on
innovative data-driven estimation and control methods automotive systems.
---|---
|  Federico Dettù received the B.Sc. degree and the M.Sc. degree (cum laude)
in automation and control engineering from Politecnico di Milano, Milan,
Italy, in 2017 and 2020, respectively. In 2020, he was a Visiting Researcher
with Stanford University, Stanford, USA. In November 2020, he joined the mOve
Research Group as a Ph.D. Student in Information Technology, Systems and
Control area, at the Department of Electronics, Computer Sciences and
Bioengineering (DEIB) of Politecnico di Milano. His research interests include
data-based estimation and control approaches for automotive systems.
---|---
|  Simone Formentin was born in Legnano, Italy, in 1984. He received the B.Sc.
and M.Sc. degrees (cum laude) in automation and control engineering from
Politecnico di Milano, Milan, Italy, in 2006 and 2008, respectively. He
received the Ph.D. degree (cum laude) in information technology within a joint
program between Politecnico di Milano and Johannes Kepler University of Linz,
Linz, Austria, in 2012. After that, he held two postdoctoral appointments with
the Swiss Federal Institute of Technology of Lausanne (EPFL), Lausanne,
Switzerland and the University of Bergamo, Bergamo, Italy. Since 2014, he has
been with Politecnico di Milano, first as an Assistant Professor, then as an
Associate Professor. His research interests include system identification and
data-driven control with a focus on automotive and financial applications. Dr.
Formentin is the Chair of the IEEE Technical Committee on System
Identification and Adaptive Control, the social media representative of the
IFAC Technical Committee on Robust Control, and a Member of the IFAC Technical
Committee on Modelling, Identification and Signal Processing. He is an
Associate Editor for Automatica and the European Journal of Control.
---|---
|  Sergio Matteo Savaresi (Senior Member, IEEE) received the M.Sc. degree in
electrical engineering from Politecnico di Milano, Milan, Italy, in 1992, and
the M.Sc. degree in applied mathematics from Catholic University, Brescia,
Italy, in 2000, and the Ph.D. degree in systems and control engineering from
Politecnico di Milano, in 1996. After Ph.D., he was a Management Consultant
with McKinsey & Company, Milan, Italy. Since 2006, he has been a Full
Professor in automatic control at Politecnico di Milano, where he is Deputy
Director and Chair of the Systems and Control Section of the Department of
Electronics, Computer Sciences and Bioengineering (DEIB). He has been a
Manager and Technical Leader of more than 400 research projects in cooperation
with private companies. He is the Co-Founder of eight high-tech startup
companies. He is the author of more than 500 scientific publications. His main
research interests include vehicle control, automotive systems, data analysis
and system identification, nonlinear control theory, and control publications,
with a special focus on smart mobility.
---|---