# Track 1 Paper: Good Usability Practices
in Scientific Software Development

Francisco Queiroz
Sandor Brockhauser  Tecgraf Institute, PUC-Rio
Department of Arts & Design, PUC-Rio
Rio de Janeiro, Brazil  European XFEL GmbH
Schenefeld, Germany  Raniere Silva  Software Sustainability Institute UK
School of Computer Science
University of Manchester
Manchester, United Kingdom  Jonah Miller
Hans Fangohr Licensed under a CC-BY-4.0 license. DOI:
10.6084/m9.figshare.5331814. Perimeter Institute of Theoretical Physics
Waterloo, Canada  University of Guelph
Guelph, Canada  University of Southampton
United Kingdom
European XFEL GmbH
Schenefeld, Germany

###### Abstract

Scientific software often presents very particular requirements regarding
usability, which is often completely overlooked in this setting. As
computational science has emerged as its own discipline, distinct from
theoretical and experimental science, it has put new requirements on future
scientific software developments. In this paper, we discuss the background of
these problems and introduce nine aspects of good usability. We also highlight
best practices for each aspect with an emphasis on applications in
computational science.

###### Index Terms:

Best Practices, Usability, Scientific Software, Computational Science,
Software for Science.

##  I Introduction

Scientific software development is a field of growing importance but lacks a
widespread methodology. Scientists generally have little or no training in
software engineering but tend to be main developers of computational science
codes. They face a number of challenges including: quickly changing
requirements due to the research nature of the work, competition between
maintainable and performance code, and lack of metrics that would reward
investment into sustainable software [1, 2]. Of particular detriment is the
pressure to rapidly produce scientific publications [3, 4]. It may be possible
to overcome this publication pressure when funding agencies are convinced that
it is worth investing directly in software software for computationally
intensive fields. The Science and Technology Facilities Council (STFC) in the
UK Collaborative Computational Projects (http://www.ccp.ac.uk/about.html) sets
a good example.

In this work we focus on _usability_ , a particular aspect of software
development and design. Usability is one of the attributes of sustainable
software and can be defined as “the extent to which a product can be used by
specified users to achieve specified goals with effectiveness, efficiency, and
satisfaction in a specified context of us” [5, p.3]. Without proper usability,
a software cannot be distributed and applied even within its targeted domain.
More importantly, its unusable software can easily result in non-reproducible
science and the violation of the FAIR principles [6]. Unfortunately, usability
is often neglected in scientific software development [7], and is of mixed
perceived importance to users and developers [8, 9]. Scientific software usage
and development present many challenges for usability design that can be
related to development models, user-base needs and specialization,
professional practices, technical constraints, and scientific demands [10].
Computational science is therefore an idiosyncratic field with unique and,
occasionally, counterintuitive usability requirements. There are,
nevertheless, a significant number of informative case studies and guidelines
on the subject [11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 7, 22, 23, 24].
Supported by those references and informed by first-hand experience, we
discuss usability challenges and how to address them.

##  II Good Practices

###  II-A Think Beyond Graphical User Interfaces

Graphical user interfaces (GUIs) have made software user-friendly and arguably
fosteed the popularization of software in general. However, scientific
software might require alternatives that, if not more intuitive, are more
appropriate and efficient depending on the user’s needs — especially if they
involve entering a large amounts of data, and reading the data from many
files, or running on a shared or distributed architectures. Command-Line
Interfaces (CLIs) are popular in computational science because they often
allow for quick repetition of tasks [25] and scriptability. The analysis of
large datasets can be significantly easier and more productive when done
through command-line input than through visual-based interfaces [12].111Of
course using a CLI does not guarantee ease of use. One must still follow
usability best practices when designing the CLI. Moreover, GUIs can be
extremely cumbersome on distributed infrastructures such as supercomputers. To
implement a GUI for distributed code, a graphical frontend must connect via
network to a distributed backend. Although many scientific visualization tools
such as VisIt [26] and Paraview [27, 28] have implemented this scheme, full
rendering via GUI can be impractical and computationally intensive renders are
often performed “headless” without the GUI [26]. Because of these
difficulties, most distributed scientific codes completely lack a graphical
frontend or separate computation and visualization into separate and
subsequent stages in the workflow. Similarly, complex experimental protocols
combining data analysis and scientific instrumentation control can be designed
with separate User Interaction points in the complete workflow [29].

Even for software where daily use relies on a GUI, such as text processors and
web browsers, there are times when having a CLI for some tasks is a time
saver. For example, users of LaTeX, or the LibreOffice or Chrome CLIs can
convert a text document into PDF format from the command line.

###  II-B Keep UI Code Separate From Scientific Calculation

Simulation (or any scientific calculation) should not be embedded in User
Interface (UI) code [30]. This rule is particularly true for scientific
software primarily because, as previously stated, scientific software should
be usable via a number of alternative interfaces, such as GUI and CLI.
Moreover, it should be possible to access these interfaces both locally and
over a network (e.g., via ssh). Keeping the scientific calculation code
wrapped into functions that are called by the UI should make reconfiguration
and customization more convenient [31], make porting the functionalities to
another UI easier and make integration with other software simpler.

###  II-C Keep the Configuration in a File

Some tasks requires researchers to provide a long list of parameters to define
their computational problem, and the software they are using may not provide
default values for the parameters, or the default parameters need to be
overriden. In these situations, it is very handy to have the ability to store
some or all of the parameters in a configuration file that the software can
read at the begining of every execution. Alternatively, if a command line tool
asks for input parameters from the standard input, which requires continuous
user interaction, it can be modified to be scriptable. In a script, the
configuration parameters are stored next to the execution command itself [32].

Configuration files have the advantage of being declarative and automatically
verifiable. The file defines a state which the program will start from or try
to achieve, rather than a procedure which leads to that state. Moreover, a
parser can automatically check to see if state is valid. The former is good
for reproducibility because it is (ideally) unambiguous even years later [33].
The latter is good for accuracy because the code can check if the parameters
in the file are sensible [23]. This state-based approach can also make
parallelism easier to automatically reason about [34] and some parallel
runtime environments have made use of this property [35, 36].

Domain Specific Languages (DSLs), on the other hand, provide additional
flexibility not present in a plain configuration file. They allow the user to
programmatically define new behaviour for the code. This can be a major
advantage since it often enables the code to be extended to unforeseen use-
cases without major rewrites. DSLs can also allow the user to interact with
the code at runtime, which can be helpful for debugging, prototyping, and
visualization [23]. The syntax and rules of the DSL can also provide the same
error checking as a parameter file.

While defining a new domain specific language (DSL) requires the development
of a parser for the language, this extra effort can be avoided by _embedding_
the domain specific language in an existing general purposes language. This
has been demonstrated by a number of projects recently, and Python is a common
choice as the general purpose language. In this context, the domain specific
language is given through a Python module that the user imports into their
generic Python program, and which provides commands, objects and operations
that are specific to the domain in question. The Python program then becomes
the (very flexible) configuration file for the computational problem.

However, some care is required when designing such DSLs: the elements of the
DSL must be constructed so that users cannot combine them in ways that would
take the tool outside its range of applicability. This could be achieved
through explicit assert statements in the DSL’s implementation or appropriate
(often Object Oriented) design. If the code author lacks the experience or
time available to achieve this, it is important to document the assumptions
made for use of the DSL so that it is not used incorrectly inadvertently by
others in the future. For example, the yt project [37] is a DSL for scientific
visualization and data analysis built in Python. If the data fed into yt
doesn’t satisfy the correct assumptions, yt could produce spurious
visualization artifacts or incorrectly integrate a quantity over the domain.
Therefore the authors of yt take extreme care to document their API, sanitize
their inputs and throw informative error messages when incorrect data is fed
into the tool. A major part of this process is unit testing the DSL’s
functionality

Some codes combine both plain configuration files and DSLs. For example, the
Einstein Toolkit [38],222For which one of us is a developer. a code for
relativistic astrophysics, uses configuration files for day-to-day
simulations. However, it also provides a low-level DSL, Kranc [39], for
defining systems of equations to solve.

###  II-D Design for Small, Incremental Changes

Making incremental changes is considered a best practice for scientific
software development [25], and the same principle applies to user interfaces.
Ideally, _UIs should be planned for extensibility and frequent changes_ as new
requisites emerge. Through incremental changes, software is more likely to
_stay attuned to users’ needs_ , not forcing them to radically change the way
they work [19].

Regarding constant updates and addition of new functionalities, UI components
that can be easily extended might offer interesting solutions. This is the
case for map3D, a scientific visualization software for displaying and editing
three-dimensional models and associated data [40]. During development, pop-up
menus were implemented for providing the necessary flexibility, allowing
developers to add new commands and submenus as the software development and
requisites evolved [11]. It is worth mentioning that web-based applications
might take advantage of the modularity allowed by frontend design
methodologies such as Atomic Design [41], making it easier to configure user
interfaces as the project advances.

The parameter files and DSLs described in section II-C are particularly good
for satisfying this design constraint. For example, the Einstein Toolkit [38]
packages low-level code in modules. Each module must declare which
functionality it adds, which relevant parameters can be set in the parameter
file, and how these parameters depend on other modules. The parameter file
parser then automatically adds these options to the parameter file at compile
time. The yt project [37] provides a DSL for scientific visualization. This
DSL interacts with the low-level code only through function calls and so
functionality can easily be incrementally added by the introduction of new DSL
language features or functions.

###  II-E Facilitate and Register User Activity and Environment

There are a number of ways through which usability can be enhanced based on
_past_ user activity. First, providing access to a list of recent commands and
allowing users to re-execute them can help users save time. This is a major
reason for the popularity of command-line interfaces [25]. A very popular
implementation of this concept is the ability to access previously typed
commands by pressing the up arrow key or do a reverse search on the history of
executed commands. Users can also press the right and left arrow keys to
navigate through a previous command and edit it to suit their needs. Second,
it might be a good idea to give users quick access to frequently used commands
[42]. In some environments, the tab key is used to roll among frequent used
commands or to auto complete a command. Third, logging user activity might
help users identify and support research reproducibility [24] by exporting the
history to a file.

After registering user activity, developers can go further and log the user
environment, i.e. compiled binary, configuration files, input files and output
files, used when running the program. This is useful in scientific software
since the output of any experiment can be different because of different
implementations (or compiler optimizations) of the Basic Linear Algebra
Subprograms (BLAS), LAPACK (Linear Algebra Package) or any other library used
when performing the experiment. This automatic logging not only provides users
quick access to their exact configuration for debugging purposes but also
allows the computation to be reproduced years after it was run for the first
time. One example framework is Formaline [43]. For software developers working
with Python, we mention the related packages ReciPy [44] and Sumatra [45].

###  II-F Learn About How Users Work

Guidelines and case studies often recommend the adoption of a user-centered
design process that seeks to develop a firm understanding of how scientists do
their work before developing a piece of software. This understanding can be
acquired by learning the meanderings of scientific work [12], or through a
participatory design approach in which users are actively involved in the
design process [16, 46, 47, 48]. It is also important to _analyze the
scientific work within the environment where it actually takes place_ [13] and
evaluate existing tools which are already in use [14]. In this last case it
might be advantageous to adopt preexisting industry standards (e.g.: keyboard
shortcuts for common functionalities, iconography, etc.).

When designing user interfaces for scientific software, it is a good idea to
_address specific users or user-bases_ rather than aim for a general solution
[14, 19]. Ideally, GUIs should be open to user customization and adjustable to
personal preferences and professional specialization [49, 14]. However, users
should not be overwhelmed by an excessive number of customizable parameters —
some of which can be unimportant or meaningless to their specific case.
Instead, there should be an additional section for setting advanced parameters
[24].

As a user base grows, users may have suggestions for improving the UI or the
underlying scientific code. If the code is open source, it can be extremely
advantageous to transform these users into developers so that they can bring
their user experience and domain expertise to bear [50]. Additionally, it is
advisable that scientific domain experts are brought into the design process
for informing _domain best practices_ [15, 21] and evaluating the tool [20].

###  II-G Be Minimalistic, but Look Out for Exceptional Needs

Designers should be attentive to information that is particularly relevant in
scientific software, but that could be eluded otherwise. _Metadata_ , for
instance, is often required to be readable and easy to access [17, 51, 18, 20,
25, 47].

Also, despite recent trends favoring flat design over skeuomorphism (i.e.:
visual design that imitates the appearance of real-world objects), software
versions of physical instruments might benefit from adopting the looks of
their real-world counterparts [52], making it easier for users to recognize
and learn about their functioning. An example for that approach is LabViEW’s
set of GUI components mimicking dials, knobs and meters [53].

Finally, minimalism should _emphasize, rather than conceal, critical
information_ such as system malfunctioning [54], emergency information [55],
and situations where awareness and response under time pressure are essential.
For instance, the Sky software for astronomical visualization reduces users’
cognitive load by simplifying three-dimensional visualization data as a two-
dimensional projection [46].

###  II-H Design for Precision

In order to achieve satisfying results, scientific work often demands
precision regarding user’s input. A possible means for that would be
continuously constraining users’ input and providing feedback on it. The
Dynamic Dragging Interface, for instance, makes user of force-feedback input
devices to help users selecting sections of 3D brain visualization [20].
Another solution would be not accepting the input when the input device, such
as a stylus or mouse, moves too fast.

It can be advantageous to have two input modes for the same action — one
designed for accuracy, and another for speed. In map3D, for instance,
geometric models could be moved, rotated and scaled through dial boxes
(accurate, but slower) or, alternatively, via mouse (less accurate, but
faster) [11]. Another approach would be to give users a way to switch between
fast and precise working modes. For instance, by activating a ’snap’ mode
where the mouse cursor snaps to a gridline, objects or other elements on
screen.

###  II-I Contextualize User Actions

Work in scientific software can involve a number of different and/or
sequential tasks to be performed by users. In those cases, it is desirable to
contextualize users’ actions, facilitating their access to functions that are
relevant to their current tasks and preventing their access to functions that
are not.

The Petri Net Toolbox for MATLAB, for instance, features a button that toggles
between _Draw Mode_ , for creating and editing models, and _Explore Mode_ ,
for simulation and analysis [42]. When switching between modes, GUI elements
are displayed or hidden depending on their relevance to the selected mode.
This approach is known as a design pattern named _Disabled Irrelevant Things_
[56].

Another possible approach is the _Window Per Task_ [56] design pattern, in
which tasks are distributed across individual screens containing the
appropriate commands for that task only. Pharmaceutical biology software
Lipid-Pro makes use of this design pattern by organizing its tasks into
separate panels [7].

##  III Summary

Throughout the previous section, we have presented a nonexhaustive set of good
practices in usability for scientific software, taking in consideration
challenging aspects of scientific software development and use such as the
lack of attention to software engineering; the need for reproducibility; the
handling of large amounts of data; the complexity of actions and parameters
involved in scientific work; frequent changes in requirements; particularities
of scientific work and its environment; the need for accessing and responding
to critical information; and the importance of precision.

By adopting the presented practices, developers should be able to deliver
applications that are more usable, robust and more appropriate for scientific
work.

## Acknowledgment

This project began during the Fourth Annual Workshop on Sustainable Software
for Science: Practices and Experiences (WSSSPE4), where four of the authors
established the Working Group on Software best practices for undergraduates
[57]. The authors would therefore like to thank the organizers of WSSSPE4 for
facilitating this conversation. J. Miller and F. Queiroz would like to thank
travel grants from the National Science Foundation of the USA and The Gordon
and Betty Moore Foundation, which made attendance possible. J. Miller also
acknowledges support from the Natural Sciences and Engineering Research
Council of Canada and from the National Science Foundation of the USA (OCI
0905046, PHY 1212401). Research at Perimeter Institute is supported by the
Government of Canada through the Department of Innovation, Science and
Economic Development and by the Province of Ontario through the Ministry of
Research and Innovation. F. Queiroz acknowledges support from PUC-Rio and
Tecgraf Institute. H. Fangohr and R. Silva acknowledge support from the
Software Sustainability Institute and Engineering and Physical Sciences
Research Council (EPSRC) in the UK.

## References

  * [1] J. Segal, “Some problems of professional end user developers,” in _Proceedings of the IEEE Symposium on Visual Languages and Human-Centric Computing_ , ser. VLHCC ’07. Washington, DC, USA: IEEE Computer Society, 2007, pp. 111–118. [Online]. Available: http://dx.doi.org/10.1109/VLHCC.2007.17
  * [2] D. F. Kelly, “A software chasm: Software engineering and scientific computing,” _IEEE Softw._ , vol. 24, no. 6, pp. 120–119, Nov. 2007. [Online]. Available: http://dx.doi.org/10.1109/MS.2007.155
  * [3] G. Wilson, “Where’s the Real Bottleneck in Scientific Computing?” _American Scientist_ , vol. 94, no. 1, pp. 5+, 2006. [Online]. Available: http://dx.doi.org/10.1511/2006.1.5
  * [4] S. Killcoyne and J. Boyle, “Managing chaos: Lessons learned developing software in the life sciences,” _Computing in Science Engineering_ , vol. 11, no. 6, pp. 20–29, Nov 2009. [Online]. Available: http://dx.doi.org/10.1109/MCSE.2009.198
  * [5] C. Venters, L. Lau, M. K. Griffiths, V. Holmes, R. R. Ward, and J. Xu, “The blind men and the elephant: Towards a software sustainability architectural evaluation framework,” figshare, Tech. Rep. 790758, 2013, http://dx.doi.org/10.6084/m9.figshare.790758.
  * [6] M. D. Wilkinson, M. Dumontier, I. J. Aalbersberg, G. Appleton, M. Axton, A. Baak, N. Blomberg, J.-W. Boiten, L. B. da Silva Santos, P. E. Bourne, J. Bouwman, A. J. Brookes, T. Clark, M. Crosas, I. Dillo, O. Dumon, S. Edmunds, C. T. Evelo, R. Finkers, A. Gonzalez-Beltran, A. J. G. Gray, P. Groth, C. Goble, J. S. Grethe, J. Heringa, P. A. C. ’t Hoen, R. Hooft, T. Kuhn, R. Kok, J. Kok, S. J. Lusher, M. E. Martone, A. Mons, A. L. Packer, B. Persson, P. Rocca-Serra, M. Roos, R. van Schaik, S.-A. Sansone, E. Schultes, T. Sengstag, T. Slater, G. Strawn, M. A. Swertz, M. Thompson, J. van der Lei, E. van Mulligen, J. Velterop, A. Waagmeester, P. Wittenburg, K. Wolstencroft, J. Zhao, and B. Mons, “The FAIR Guiding Principles for scientific data management and stewardship,” _Scientific Data_ , vol. 3, pp. 160 018+, Mar. 2016. [Online]. Available: http://dx.doi.org/10.1038/sdata.2016.18
  * [7] Z. Ahmed, S. Zeeshan, and T. Dandekar, “Developing sustainable software solutions for bioinformatics by the butterfly paradigm [version 2; referees: 2 approved],” _F1000Research_ , vol. 3, no. 71, 2014. [Online]. Available: http://dx.doi.org/10.12688/f1000research.3681.2
  * [8] L. Nguyen-Hoan, S. Flint, and R. Sankaranarayana, “A survey of scientific software development,” in _Proceedings of the 2010 ACM-IEEE International Symposium on Empirical Software Engineering and Measurement_ , ser. ESEM ’10. New York, NY, USA: ACM, 2010, pp. 12:1–12:10. [Online]. Available: http://doi.acm.org/10.1145/1852786.1852802
  * [9] M. Hucka and M. J. Graham, “Software search is not a science, even among scientists,” _CoRR_ , vol. abs/1605.02265, 2016. [Online]. Available: http://arxiv.org/abs/1605.02265
  * [10] F. Queiroz and R. Spitz, “The lens of the lab: Design challenges in scientific software,” _International Journal of Design Management and Professional Practice_ , vol. 10, no. 3, pp. 17–45, 2016. [Online]. Available: https://doi.org/10.18848/2325-162X/CGP/v10i03/17-45
  * [11] R. S. MacLeod, C. R. Johnson, and M. A. Matheson, “Visualization of cardiac bioelectricity-a case study,” in _Visualization, 1992. Visualization ’92, Proceedings., IEEE Conference on_ , Oct 1992, pp. 411–418. [Online]. Available: http://dx.doi.org/10.1109/VISUAL.1992.235178
  * [12] R. R. Springmeyer, “Applying observations of work activity in designing prototype data analysis tools,” in _Visualization, 1993. Visualization ’93, Proceedings., IEEE Conference on_ , Oct 1993, pp. 228–235. [Online]. Available: https://doi.org/10.1109/VISUAL.1993.398873
  * [13] C. Pancake, “Improving the usability of numerical software through user-centered design,” Corvallis, OR, USA, Tech. Rep., 1996. [Online]. Available: https://web.engr.oregonstate.edu/~pancake/papers/ImpUsab.pdf
  * [14] H. Javahery, A. Seffah, and T. Radhakrishnan, “Beyond power: Making bioinformatics tools user-centered,” _Commun. ACM_ , vol. 47, no. 11, pp. 58–63, Nov. 2004. [Online]. Available: http://doi.acm.org/10.1145/1029496.1029527
  * [15] m. c. schraefel, G. V. Hughes, H. R. Mills, G. Smith, T. R. Payne, and J. Frey, “Breaking the book: Translating the chemistry lab book into a pervasive computing lab environment,” in _Proceedings of the SIGCHI Conference on Human Factors in Computing Systems_ , ser. CHI ’04. New York, NY, USA: ACM, 2004, pp. 25–32. [Online]. Available: http://doi.acm.org/10.1145/985692.985696
  * [16] C. Letondal and W. E. Mackay, “Participatory programming and the scope of mutual responsibility: Balancing scientific, design and software commitment,” in _Proceedings of the Eighth Conference on Participatory Design: Artful Integration: Interweaving Media, Materials and Practices - Volume 1_ , ser. PDC 04. New York, NY, USA: ACM, 2004, pp. 31–41. [Online]. Available: http://dx.doi.org/10.1145/1011870.1011875
  * [17] T. Talbott, M. Peterson, J. Schwidder, and J. D. Myers, “Adapting the electronic laboratory notebook for the semantic era,” in _Proceedings of the 2005 International Symposium on Collaborative Technologies and Systems, 2005._ , May 2005, pp. 136–143. [Online]. Available: http://dx.doi.org/10.1109/ISCST.2005.1553305
  * [18] C. Macaulay, D. Sloan, X. Jiang, P. Forbes, S. Loynton, J. R. Swedlow, and P. Gregor, “Usability and user-centered design in scientific software development,” _IEEE Softw._ , vol. 26, no. 1, pp. 96–102, Jan. 2009. [Online]. Available: http://dx.doi.org/10.1109/MS.2009.27
  * [19] D. D. Roure and C. Goble, “Software design for empowering scientists,” _IEEE Software_ , vol. 26, no. 1, pp. 88–95, Jan 2009. [Online]. Available: http://dx.doi.org/10.1109/MS.2009.22
  * [20] D. F. Keefe, “Integrating visualization and interaction research to improve scientific workflows,” _IEEE Computer Graphics and Applications_ , vol. 30, no. 2, pp. 8–13, March 2010. [Online]. Available: http://doi.org/10.1109/MCG.2010.30
  * [21] P. De Matos, J. A. Cham, H. Cao, R. Alcantara, F. Rowland, R. Lopez, and C. Steinbeck, “The Enzyme Portal: a case study in applying user-centred design methods in bioinformatics,” _BMC Bioinformatics_ , vol. 14, p. 103, 2013. [Online]. Available: http://dx.doi.org/10.1186/1471-2105-14-103
  * [22] H. Fangohr, M. Albert, and M. Franchin, “Nmag micromagnetic simulation tool: Software engineering lessons learned,” in _Proceedings of the International Workshop on Software Engineering for Science_ , ser. SE4Science ’16. New York, NY, USA: ACM, 2016, pp. 1–7. [Online]. Available: http://doi.acm.org/10.1145/2897676.2897677
  * [23] M. Beg, R. A. Pepper, and H. Fangohr, “User interfaces for computational science: a domain specific language for oommf embedded in python,” _AIP Advances_ , vol. 7, p. 056025, 2017. [Online]. Available: http://doi.org/10.1063/1.4977225
  * [24] M. List, P. Ebert, and F. Albrecht, “Ten simple rules for developing usable software in computational biology,” _PLOS Computational Biology_ , vol. 13, no. 1, pp. 1–5, 01 2017. [Online]. Available: http://dx.doi.org/10.1371%2Fjournal.pcbi.1005265
  * [25] G. Wilson, D. A. Aruliah, C. Titus Brown, N. P. Chue Hong, M. Davis, R. T. Guy, S. H. D. Haddock, K. D. Huff, I. M. Mitchell, M. D. Plumbley, B. Waugh, E. P. White, and P. Wilson, “Best practices for scientific computing,” _PLoS Biol_ , vol. 12, no. 1, Jan. 2014. [Online]. Available: https://doi.org/10.1371/journal.pbio.1001745
  * [26] H. Childs, E. Brugger, B. Whitlock, J. Meredith, S. Ahern, D. Pugmire, K. Biagas, M. Miller, C. Harrison, G. H. Weber, H. Krishnan, T. Fogal, A. Sanderson, C. Garth, E. W. Bethel, D. Camp, O. Rübel, M. Durant, J. M. Favre, and P. Navrátil, “VisIt: An End-User Tool For Visualizing and Analyzing Very Large Data,” in _High Performance Visualization–Enabling Extreme-Scale Scientific Insight_ , Oct 2012, pp. 357–372.
  * [27] J. Ahrens, B. Geveci, and C. Law, “Paraview: An end-user tool for large data visualization,” _The Visualization Handbook_ , vol. 717, 2005. [Online]. Available: https://doi.org/10.1016/B978-012387582-2/50038-1
  * [28] U. Ayachit, _The Paraview Guide (Full Color Version): A Parallel Visualization Application_. Kitware, Incorporated, 2015. [Online]. Available: https://books.google.com/books?id=Z8wHrgEACAAJ
  * [29] S. Brockhauser, O. Svensson, M. W. Bowler, M. Nanao, E. Gordon, R. M. F. Leal, A. Popov, M. Gerring, A. A. McCarthy, and A. Gotz, “The use of workflows in the design and implementation of complex experiments in macromolecular crystallography,” _Acta Crystallographica Section D_ , vol. 68, no. 8, pp. 975–984, Aug 2012. [Online]. Available: https://doi.org/10.1107/S090744491201863X
  * [30] D. Kelly, D. Hook, and R. Sanders, “Five recommended practices for computational scientists who write software,” _Computing in Science Engineering_ , vol. 11, no. 5, pp. 48–53, Sept 2009. [Online]. Available: https://doi.org/10.1109/MCSE.2009.139
  * [31] B. F. Bastos, V. M. Moreira, and A. T. A. Gomes, “Rapid prototyping of science gateways in the brazilian national hpc network.” in _IWSG_ , ser. CEUR Workshop Proceedings, T. Kiss, Ed., vol. 993. CEUR-WS.org, 2013. [Online]. Available: http://dblp.uni-trier.de/db/conf/iwsg/iwsg2013.html#BastosMG13
  * [32] E. Potterton, P. Briggs, M. Turkenburg, and E. Dodson, “A graphical user interface to the CCP4 program suite,” _Acta Crystallographica Section D_ , vol. 59, no. 7, pp. 1131–1137, Jul 2003. [Online]. Available: https://doi.org/10.1107/S0907444903008126
  * [33] J. W. Lloyd, “Practical advtanages of declarative programming,” in _1994 Joint Conference on Declarative Programming, GULP-PRODE’94 Peñiscola, Spain, September 19-22, 1994, Volume 1_ , 1994, pp. 18–30.
  * [34] M. M. Chakravarty, “On the massively parallel execution of declarative programs,” Ph.D. dissertation, Berlin Institute of Technology, 1997. [Online]. Available: http://www.cse.unsw.edu.au/~chak/papers/diss.ps.gz
  * [35] A. Buss, Harshvardhan, I. Papadopoulos, O. Pearce, T. Smith, G. Tanase, N. Thomas, X. Xu, M. Bianco, N. M. Amato, and L. Rauchwerger, “Stapl: Standard template adaptive parallel library,” in _Proceedings of the 3rd Annual Haifa Experimental Systems Conference_ , ser. SYSTOR ’10. New York, NY, USA: ACM, 2010, pp. 14:1–14:10. [Online]. Available: http://doi.acm.org/10.1145/1815695.1815713
  * [36] M. Bauer, S. Treichler, E. Slaughter, and A. Aiken, “Legion: Expressing locality and independence with logical regions,” in _Proceedings of the International Conference on High Performance Computing, Networking, Storage and Analysis_ , ser. SC ’12. Los Alamitos, CA, USA: IEEE Computer Society Press, 2012, pp. 66:1–66:11. [Online]. Available: http://dl.acm.org/citation.cfm?id=2388996.2389086
  * [37] M. J. Turk, B. D. Smith, J. S. Oishi, S. Skory, S. W. Skillman, T. Abel, and M. L. Norman, “yt: A Multi-code Analysis Toolkit for Astrophysical Simulation Data,” _ApJS_ , vol. 192, p. 9, Jan. 2011. [Online]. Available: http://dx.doi.org/10.1088/0067-0049/192/1/9
  * [38] F. Löffler, J. Faber, E. Bentivegna, T. Bode, P. Diener, R. Haas, I. Hinder, B. C. Mundim, C. D. Ott, E. Schnetter, G. Allen, M. Campanelli, and P. Laguna, “The Einstein Toolkit: A Community Computational Infrastructure for Relativistic Astrophysics,” _Class. Quantum Grav._ , vol. 29, no. 11, p. 115001, 2012. [Online]. Available: http://dx.doi.org/10.1088/0264-9381/29/11/115001
  * [39] S. Husa, I. Hinder, and C. Lechner, “Kranc: a Mathematica application to generate numerical codes for tensorial evolution equations,” _Comput. Phys. Commun._ , vol. 174, pp. 983–1004, 2006. [Online]. Available: http://dx.doi.org/10.1016/j.cpc.2006.02.002
  * [40] CIBC, 2016, map3d: Interactive scientific visualization tool for bioengineering data. Scientific Computing and Imaging Institute (SCI), Download from: http://www.sci.utah.edu/cibc/software.html.
  * [41] B. Frost, “Designing systems — atomic design by brad frost,” 2016, (Accessed on 08/17/2017). [Online]. Available: http://atomicdesign.bradfrost.com/chapter-1/
  * [42] J. Júlvez, M. H. Matcovschi, and O. Pastravanu, “Matlab tools for the analysis of petri net models,” in _Proceedings of the 2014 IEEE Emerging Technology and Factory Automation (ETFA)_ , Sept 2014, pp. 1–12. [Online]. Available: https://doi.org/10.1109/ETFA.2014.7005053
  * [43] C. Schnetter, E. Ott, “Formaline: The provenance of computational astrophysics simulations and their results (part i),” http://blowingupstars.blogspot.ca/2016/06/formaline-provenance-of-computational.html, 2016, online Access. http://blowingupstars.blogspot.ca/2016/06/formaline-provenance-of-computational.html. Accessed Julu 5, 2017.
  * [44] e. a. Robin Wilson, “Recipy,” https://github.com/recipy/recipy, 2015, accessed: 2017-07-04.
  * [45] A. P. Davison, M. Mattioni, D. Samarkanov, and B. Teleńczuk, “Sumatra: A Toolkit for Reproducible Research,” in _Implementing Reproducible Research_ , V. Stodden, F. Leisch, and R. D. Peng, Eds. Chapman & Hall, 2014, ch. 3, pp. 57–78, https://pypi.python.org/pypi/Sumatra.
  * [46] C. R. Aragon, S. S. Poon, G. S. Aldering, R. C. Thomas, and R. Quimby, “Using visual analytics to maintain situation awareness in astrophysics,” in _2008 IEEE Symposium on Visual Analytics Science and Technology_ , Oct 2008, pp. 27–34. [Online]. Available: https://doi.org/10.1109/VAST.2008.4677353
  * [47] A. K. Thomer, M. B. Twidale, J. Guo, and M. J. Yoder, “Co-designing scientific software: Hackathons for participatory interface design,” in _Proceedings of the 2016 CHI Conference Extended Abstracts on Human Factors in Computing Systems_ , ser. CHI EA ’16. New York, NY, USA: ACM, 2016, pp. 3219–3226. [Online]. Available: http://doi.acm.org/10.1145/2851581.2892549
  * [48] D. R. Luna, D. A. R. Lede, C. M. Otero, M. R. Risk, and F. G. B. de Quirós, “User-centered design improves the usability of drug-drug interaction alerts: Experimental comparison of interfaces,” _Journal of Biomedical Informatics_ , vol. 66, pp. 204 – 213, 2017. [Online]. Available: https://doi.org/10.1016/j.jbi.2017.01.009
  * [49] M. W. Gertz, D. B. Stewart, and P. K. Khosla, “A human machine interface for distributed virtual laboratories,” _IEEE Robotics Automation Magazine_ , vol. 1, no. 4, pp. 5–13, Dec 1994. [Online]. Available: https://doi.org/10.1109/100.388265
  * [50] M. J. Turk, “Scaling a code in the human dimension,” in _Proceedings of the Conference on Extreme Science and Engineering Discovery Environment: Gateway to Discovery_ , ser. XSEDE ’13. New York, NY, USA: ACM, 2013, pp. 69:1–69:7. [Online]. Available: http://doi.acm.org/10.1145/2484762.2484782
  * [51] S. M. Baxter, S. W. Day, J. S. Fetrow, and S. J. Reisinger, “Scientific software development is not an oxymoron,” _PLoS Comput Biol_ , vol. 2, no. 9, pp. 1–4, 09 2006. [Online]. Available: https://dx.doi.org/10.1371%2Fjournal.pcbi.0020087
  * [52] K. R. Foster, “Software tools [technology 1998 analysis and forecast],” _IEEE Spectrum_ , vol. 35, no. 1, pp. 52–56, Jan 1998. [Online]. Available: https://doi.org/10.1109/6.645980
  * [53] “Ni pxi and labview deliver unrivaled performance, flexibility, and value for automated test,” 2013. [Online]. Available: http://www.ni.com/white-paper/14300/en/
  * [54] H. Morais, P. Vancraeyveld, A. H. B. Pedersen, M. Lind, H. Jóhannsson, and J. Østergaard, “Sospo-sp: Secure operation of sustainable power systems simulation platform for real-time system state evaluation and control,” _IEEE Transactions on Industrial Informatics_ , vol. 10, no. 4, pp. 2318–2329, Nov 2014. [Online]. Available: https://doi.org/10.1109/TII.2014.2321521
  * [55] H. T. Ferguson, S. Gesing, and J. Nabrzyski, “Measuring usability in decision tools supporting collaborations for environmental disaster response,” in _2016 49th Hawaii International Conference on System Sciences (HICSS)_ , Jan 2016, pp. 2872–2881. [Online]. Available: https://doi.org/10.1109/HICSS.2016.360
  * [56] Z. Ahmed, “Designing flexible GUI to increase the acceptance rate of product data management systems in industry,” _CoRR_ , vol. abs/1103.1134, 2011. [Online]. Available: http://arxiv.org/abs/1103.1134
  * [57] D. S. Katz, K. E. Niemeyer, S. Gesing, L. Hwang, W. Bangerth, S. Hettrick, R. Idaszak, J. Salac, N. Chue Hong, S. Núñez Corrales, A. Allen, R. S. Geiger, J. Miller, E. Chen, A. Dubey, and P. Lago, “Report on the Fourth Workshop on Sustainable Software for Science: Practice and Experiences (WSSSPE4),” _ArXiv e-prints_ , May 2017\. [Online]. Available: https://arxiv.org/abs/1705.02607v2


#  Constrained Covariance Steering Based Tube-MPPI

Isin M. Balci  Efstathios Bakolas  Bogdan Vlahov  Evangelos A. Theodorou This
research has been supported in part by NSF awards CMMI-1937957, CMMI-1936079,
and ECCS-1924790. I. M. Balci (PhD student) and E. Bakolas (Associate
Professor) are with the Department of Aerospace Engineering and Engineering
Mechanics, The University of Texas at Austin, Austin, Texas 78712-1221, USA,
Email<EMAIL_ADDRESS><EMAIL_ADDRESS>B. Vlahov (PhD
student) and E. Theodorou (Associate Professor) are with the Department of
Aerospace Engineering, Georgia Tech, Atlanta, Georgia, USA, Email:
<EMAIL_ADDRESS>
###### Abstract

In this paper, we present a new trajectory optimization algorithm for
stochastic linear systems which combines Model Predictive Path Integral (MPPI)
control with Constrained Covariance Steering (CSS) to achieve high performance
with safety guarantees (robustness). Although MPPI can be used to solve
complex nonlinear trajectory optimization problems, it may not always handle
constraints effectively and its performance may degrade in the presence of
unmodeled disturbances. By contrast, CCS can handle probabilistic state and /
or input constraints (e.g., chance constraints) by controlling uncertainty
which implies that CCS can provide robustness against stochastic disturbances.
CCS, however, suffers from scalability issues and cannot handle complex cost
functions in general. We argue that the combination of the two methods yields
a class of trajectory optimization algorithms that can achieve high
performance while ensuring safety with high probability. The efficacy of our
algorithm is demonstrated in an obstacle avoidance problem and a path
generation problem with a circular track.

##  I Introduction

Many real-world tasks for autonomous systems can be cast as finite horizon
stochastic trajectory optimization problems in the presence of model
uncertainties, and random exogenous inputs from the environment. The main goal
of these problems is to find control policies that minimize the expected value
of a given cost function while satisfying state and input constraints with a
given confidence level.

In this work, we present a novel algorithm for constrained stochastic
trajectory optimization problems subject to safety constraints. Our proposed
algorithm combines Constrained Covariance Steering (CCS) theory for discrete-
time stochastic linear systems with Model Predictive Path Integral (MPPI) to
achieve robustness to uncertainties and variations of the different parameters
of the proposed controllers as well as improved performance, and scalability.

Literature Review: Optimization-based methods treat the stochastic trajectory
optimization problem as a nonlinear program (NLP) which can be solved by
specialized NLP solvers. However, these NLP based approaches rely on a good
initial guess to achieve high performance and may suffer from the lack of
convergence guarantees [1]. Successive convexification-based methods provide
convergence guarantees, but they may still suffer from scalability issues, if
the underlying system dynamics are stochastic [2, 3, 4].

Dynamic programming-based algorithms have been proposed for unconstrained
stochastic trajectory optimization problems to alleviate the scalability issue
in [5, 6] and for constrained problems in [7, 8]. These methods, however, lack
safety guarantees and their applicability is limited to smooth objective
functions. On the other hand, sampling-based stochastic optimization
algorithms deal with non-smooth objective functions but they often cannot
handle unmodeled disturbances and model mismatch [9, 10].

The two methods that are most closely related to our approach are discussed in
[11], [12]. In [11], the authors use Covariance Steering for path planning for
linear systems with chance constraints for obstacle avoidance. However, the
approach in [11] does not scale well due to the fact that the number of
decision variables increases quadratically with the problem horizon because of
the feedback terms. Furthermore, integer variables that are used to encode
obstacle avoidance constraints add to computational complexity. In [12], the
authors use unconstrained covariance steering to take sample trajectories from
the low cost regions of the state space to enhance the performance and to
avoid local minima. However, this method requires the terminal mean and
covariance as design parameters, which can be hard to tune, and the safety
constraints are not explicitly enforced.

Main Contributions: This paper presents a novel trajectory optimization
algorithm for stochastic linear systems with a non-convex safe state space
(CCSMPPI). The proposed algorithm achieves high performance with safety
guarantees by combining the standard MPPI with the CCS. In particular, MPPI is
used to generate a reference trajectory, which is then used to generate a
convex safe region, by solving an unconstrained stochastic trajectory
optimization problem whereas CCS generates a control policy which minimizes
the divergence from the reference trajectory while satisfying the safety
constraints. In this way, CCSMPPI endows the MPPI algorithm with robustness to
stochastic disturbances by leveraging the framework of CCS.

The other improvement of the CCSMPPI over the standard MPPI is the robustness
and the safety guarantees against poorly designed cost functions and incorrect
tuning of algorithm parameters. The CCS procedure of the proposed algorithm
filters the unsafe inputs that are computed by MPPI and corrects them by means
of a feedback control law. This technique makes the cost function design and
parameter tuning tasks less time-consuming.

Finally, the practicality of CCSMPPI is demonstrated in two different
trajectory optimization problems in which we compare our results with those
obtained by using the standard MPPI [9] and tube-MPPI [10]. It is shown in
numerical experiments that our approach is superior to standard MPPI and tube-
MPPI in terms of providing safety against both stochastic disturbances and
poorly designed cost functions.

##  II Problem Statement and Preliminaries

###  II-A Notation

We denote by $\mathbb{R}^{n}$ the set of $n$-dimensional real vectors. We use
$\mathbb{E}\left[\cdot\right]$ and $\mathbb{P}(E)$ to denote the expectation
functional and the probability of the random event $E$, respectively. Given a
vector $x$, its 2-norm is denoted by $\lVert x\rVert_{2}$ and given a matrix
$\mathbf{A}\in\mathbb{R}^{n\times m}$, we denote its Frobenius norm by
$\lVert\mathbf{A}\rVert_{F}$ and its trace by $\operatorname{tr}(\mathbf{A})$.
We use $\bm{0}$ and $I_{n}$ to denote the zero matrix and the $n\times n$
identity matrix, respectively. We will denote the convex cone of $n\times n$
symmetric positive semi-definite (symmetric positive definite) matrices by
$\mathbb{S}^{+}_{n}$ ($\mathbb{S}^{++}_{n}$). We write $\mathrm{bdiag}(A_{1},$
$\dots,A_{\ell})$ to denote the block diagonal matrix formed by the matrices
$A_{i}$, $i\in\\{1,\dots,\ell\\}$. $\cup_{i=1}^{N}\mathcal{O}_{i}$ to denote
the union of sets $\mathcal{O}_{i}$ indexed by $i\in\\{1,\dots,N\\}$. We
denote by $\mu_{z}$ and $\mathrm{var}_{z}$ the mean and the variance of a
random vector $z$, respectively. We write $x\sim\mathcal{N}(\mu,\Sigma)$ to
represent the random variable $x$ is normally distributed with mean $\mu$
covariance $\Sigma$.

###  II-B Problem Statement

The motion of the agent is described by a discrete-time stochastic linear
system:

$\displaystyle x_{k+1}=A_{k}x_{k}+B_{k}u_{k}+w_{k},$ (1)

where $A_{k}\in\mathbb{R}^{4\times 4}$, $B_{k}\in\mathbb{R}^{4\times 2},$
$x_{k}\in\mathcal{X}\subseteq\mathbb{R}^{4}$ is the state of the agent which
is decomposed as $x_{k}=[p_{k}^{\mathrm{T}},v_{k}^{\mathrm{T}}]^{\mathrm{T}}$,
where $p_{k}=[p^{x}_{k},p^{y}_{k}]^{\mathrm{T}}\in\mathbb{R}^{2}$ is the
position and $v_{k}=[v^{x}_{k},v^{y}_{k}]^{\mathrm{T}}\in\mathbb{R}^{2}$ is
the velocity, $u_{k}=[u_{k}^{x},u_{k}^{y}]^{\mathrm{T}}\in\mathbb{R}^{2}$ is
the control input and $w_{k}\in\mathbb{R}^{4}$ is the disturbance. We assume
that $w_{k}\sim\mathcal{N}(\bf{0},\mathbf{W}_{k})$ and
$\mathbb{E}[w_{k}w_{l}^{\mathrm{T}}]=\bm{0}$ for all $k\neq l$.

The choice of the objective function will be determined by the particular
application. However, in this paper, we use the objective function utilized in
the formulation of the information-theoretic MPPI [9], which is defined as
follows:

$\displaystyle\mathcal{L}(X^{N},U^{N-1})=\Phi(x_{N})+\sum_{k=0}^{N-1}\left(q(x_{k})+\lambda
u_{k}^{\mathrm{T}}R_{k}u_{k}\right),$ (2)

where $X^{N}=\\{x_{0},x_{1},\dots,x_{N}\\}$ is the state sequence,
$U^{N-1}=\\{u_{0},u_{1},\dots,u_{N-1}\\}$ is the control input sequence,
$q:\mathbb{R}^{n}\rightarrow\mathbb{R}$ is the state-dependent term of the
running cost function, $\Phi:\mathbb{R}^{2n}\rightarrow\mathbb{R}$ is the
terminal cost function, $R_{k}\in\mathbb{S}_{n}^{+}$ and $\lambda\geq 0$.
While the term $\lambda u_{k}^{\mathrm{T}}R_{k}u_{k}$ penalizes the control
input, $q(x_{k})$ and $\Phi(x_{N})$ are task dependent and each of them can
either be a smooth function as in [9] or a sum of indicator functions for
obstacle avoidance as in [10].

We assume that the position space is populated by $N_{\mathrm{obs}}$
obstacles. The $i$-th obstacle is parametrized by its position,
$s_{i}\in\mathbb{R}^{n}$, and its radius, $r_{i}\geq 0$, and the region it
occupies is denoted as $\mathcal{O}_{i}:=\\{p\in\mathbb{R}^{n}|\lVert
x-s_{i}\rVert_{2}\leq r_{i}\\}$. The position space is defined as
$\mathcal{X}\subseteq\mathbb{R}^{n}$ and the safe region is defined as
$\mathcal{X}_{\mathrm{safe}}:=\mathcal{X}\backslash\mathcal{O}$ where
$\mathcal{O}=\cup_{j=1}^{N_{\mathrm{obs}}}\mathcal{O}_{j}$. Then, the safe
trajectory optimization problem can be formally stated as follows:

$\displaystyle\underset{X^{N},\;U^{N-1}}{\textrm{minimize}}$
$\displaystyle\quad\mathbb{E}[\mathcal{L}(X^{N},U^{N-1})]$ (3a) subject to
$\displaystyle\quad x_{0}=\bar{x}_{0},\leavevmode\nobreak\
\leavevmode\nobreak\ \eqref{eq:linear-dynamics}$
$\displaystyle\quad\mathbb{P}(x_{k}\in\mathcal{X}_{\mathrm{safe}})\geq
1-\mathrm{P}_{\mathrm{fail}},\leavevmode\nobreak\ \leavevmode\nobreak\ \forall
k\in\mathcal{I}^{N}$ (3b)

where $\mathcal{I}^{N}=\\{0,1,\dots,N\\}$, $\bar{x}_{0}\in\mathbb{R}^{2n}$ and
$\mathrm{P}_{\mathrm{fail}}\in(0,0.5]$ is the acceptable level of the
probability of the failure.

Since the system dynamics in (1) includes the random noise term $w_{k}$, the
state sequence $X^{k}$ is a random process thus the problem in 3 is a
stochastic trajectory optimization problem.

##  III Brief Review of MPPI and TUBE-MPPI

MPPI is a sampling-based stochastic Model Predictive Control (MPC) algorithm
[9]. It works by taking $K$ samples of control sequences from a Gaussian
distribution, and finding the corresponding state trajectories and costs. Each
sequence is then weighted by an exponential transform of its cost and the
optimal control sequence is found as as weighted sum:
$\bm{u}^{\mathrm{MPPI}}=\frac{1}{\eta}\sum_{i=1}^{K}\omega_{i}\bm{u}^{(i)}$,
where $\eta=\sum_{i=1}^{K}\omega_{i}$ ,
$\omega_{i}=\exp{-\frac{1}{\lambda}(C_{i}-\beta)},$
$\beta=\min_{i=1,\dots,K}C_{i}$, $\bm{u}^{(i)}=\bm{v}+\bm{\epsilon}^{(i)}$ and
$\bm{\epsilon}^{(i)}=[\epsilon^{(i)}_{0},\dots,\epsilon^{(i)}_{T_{\mathrm{MPPI}}}]$,
$\epsilon^{(i)}_{k}\sim\mathcal{N}(\bm{0},\nu I)$ and $C_{i}$ is defined as
follows:

$\displaystyle
C_{i}=\Phi(x^{(i)}_{T_{\mathrm{MPPI}}})+\sum_{k=0}^{T_{\mathrm{MPPI}}}\Big{(}q(x_{k}^{(i)})+\frac{1}{2}v^{\mathrm{T}}_{k}R_{k}v_{k}$
$\displaystyle\qquad+\frac{1}{2}\big{(}v_{k}^{\mathrm{T}}R_{k}\epsilon^{(i)}_{k}+(1-\nu^{-1})\epsilon_{k}^{(i)\mathrm{T}}R_{k}\epsilon_{k}^{(i)}\big{)}\Big{)},$
(4)

$C_{i}$ is the total path cost induced by $i$th sample trajectory,
$\bm{v}=[v_{0},\dots,v_{T_{\mathrm{MPPI}}-1}]^{\mathrm{T}}$ is the optimal
control sequence obtained by the previous iteration of the MPPI,
$\bm{\epsilon}^{(i)}$ is the control sampling noise, $u^{(i)}$ is the $i$th
control sequence sample and $\nu\geq 0$ is the control sampling covariance
parameter. At the next iteration, the previous optimal control sequence is
shifted in time and used as the mean of the Gaussian distribution to sample
controls from.

MPPI may not perform well when the system has a disturbance that causes the
control sampling distribution to only sample high cost trajectories. Tube-MPPI
[10] addresses this by running MPPI from two starting states, a nominal and a
real state. The real state is taken from the state of the real system as
before whereas the nominal state is found by propagating the previous nominal
state and control through the noise-free dynamics model without the
disturbance term, $w_{k}$. Once the MPPI optimization step is done, the
optimal trajectory of the real system is pushed to follow the optimal
trajectory of the nominal system through the use of a feedback controller.
This procedure makes the trajectory of the real system converge to the
trajectory of the nominal system which has a lower cost.

##  IV Linear Covariance Steering Theory

The main objective of the constrained covariance steering (CCS) problem is to
steer the mean and the covariance of a stochastic linear system to desired
values while minimizing the expected value of an objective function subject to
state and/or input constraints [13, 14]. The general form of the discrete-time
CCS problem can be formally stated as follows:

$\displaystyle\underset{\pi\in\Pi}{\textrm{minimize}}$
$\displaystyle\leavevmode\nobreak\ \leavevmode\nobreak\
J(\bm{x},\bm{u})=\mathbb{E}\left[\sum_{k=0}^{N-1}x_{k}^{\mathrm{T}}Q_{k}x_{k}+u_{k}^{\mathrm{T}}R_{k}u_{k}\right]$
(5a) subject to $\displaystyle\leavevmode\nobreak\ \leavevmode\nobreak\
x_{k+1}=A_{k}x_{k}+B_{k}u_{k}+w_{k},$ (5b) $\displaystyle\leavevmode\nobreak\
\leavevmode\nobreak\ \mathbb{P}[x_{k}\in\mathcal{X}]\geq
1-\epsilon_{x},\leavevmode\nobreak\ \mathbb{P}[u_{k}\in\mathcal{U}]\geq
1-\epsilon_{u}$ (5c) $\displaystyle\leavevmode\nobreak\ \leavevmode\nobreak\
x_{0}\sim\mathcal{N}(\mu_{0},\Sigma_{0}),\quad
x_{N}\sim\mathcal{N}(\mu_{\mathrm{d}},\Sigma_{\mathrm{d}})$ (5d)

where $\Pi$ denotes the set of causal policies,
$\mathcal{X}\subseteq\mathbb{R}^{n_{x}}$ and
$\mathcal{U}\subseteq\mathbb{R}^{n_{u}}$ are arbitrary sets corresponding to
state and input constraints. The constraints in (5d) represents the initial
and the desired state distributions. The terminal covariance constraint in
(5d) which can be dropped in our CCS formulation since it is not useful to
specify a desired covariance for safety as long as constraints in (5c) are
satisfied.

Discrete-time formulations of the CCS problems can be cast as finite-
dimensional deterministic optimization problems by restricting the class of
admissible policies to those which admit the affine state history feedback
[15] or the disturbance feedback parametrization [16]. In this work, we will
be utilizing the latter according to which the control input at each discrete
stage can be expressed as follows:

$\displaystyle
v_{k}=\left\\{\begin{matrix}\bar{v}_{0}+H_{0}(x_{0}-\mu_{0})&k=0,\\\
\bar{v}_{k}+H_{k}(x_{0}-\mu_{0})+K_{k-1}w_{k-1}&k>0,\end{matrix}\right.$ (6)

where $H_{k},K_{k}\in\mathbb{R}^{n\times n}$ and
$\bar{v}_{k}\in\mathbb{R}^{n}$ for all $k\in\\{0,1,\dots,N-1\\}$. This
parametrization allows us to cast the covariance steering problem as a finite-
dimensional (deterministic) optimization problem in terms of the following
decision variables: $\\{\bar{v}_{k},H_{k},K_{k}\\}_{k=0}^{N-1}$. In order to
do that, the decision variables are represented in a more compact form as
follows:
$\bm{\mathcal{H}}:=[H_{0}^{\mathrm{T}},H_{1}^{\mathrm{T}},\dots,H_{N-1}^{\mathrm{T}}]^{\mathrm{T}}$,
$\mathbf{K}=\mathrm{bdiag}(K_{0},K_{1},\dots,K_{N-2},\bm{0}^{n\times n})$,
$\bm{\mathcal{K}}:=\left[\begin{smallmatrix}\bm{0}&\bm{0}\\\
\mathbf{K}&\bm{0}\end{smallmatrix}\right]$ and
$\bm{\bar{v}}=[\bar{v}_{0}^{\mathrm{T}},\bar{v}^{\mathrm{T}}_{1},\dots,\bar{v}^{\mathrm{T}}_{N-1}]^{\mathrm{T}}$.

Now, let $\bm{x}$, $\bm{v}$ and $\bm{w}$ be concatenated state, input and
noise vectors. Then, it follows from (1) that

$\displaystyle\bm{x}$
$\displaystyle:=\bm{\Gamma}x_{0}+\mathbf{G}_{\bm{u}}\bm{v}+\mathbf{G}_{\bm{w}}\bm{w},$
(7a) $\displaystyle\bm{v}$
$\displaystyle:=\bm{\bar{v}}+\bm{\mathcal{H}}\tilde{x}_{0}+\bm{\mathcal{K}}\bm{w},$
(7b) where $\tilde{x}_{0}:=x_{0}-\mu_{0}$. Equations (7a)-(7b) are derived
from (1) and (6) respectively. The reader can refer to [15, 16] for the
details of the previous derivations.

We can compute the mean of the vectors $\bm{x}$ and $\bm{u}$ by taking the
expectation of both sides of (7a) and (7b). Then, we compute the deviation of
concatenated vectors $\bm{\tilde{x}}=\bm{x}-\mu_{\bm{x}}$ and
$\bm{\tilde{v}}:=\bm{v}-\mu_{\bm{v}}$. Finally, we compute the variances
$\mathrm{var}_{\bm{x}}:=\mathbb{E}[\bm{\tilde{x}}\bm{\tilde{x}}^{\mathrm{T}}]$
and
$\mathrm{var}_{\bm{v}}:=\mathbb{E}[\bm{\tilde{v}}\bm{\tilde{v}}^{\mathrm{T}}]$
as follows:

$\displaystyle\mu_{\bm{x}}$
$\displaystyle=\bm{\Gamma}\mu_{0}+\mathbf{G}_{\bm{u}}\bm{\bar{u}},$ (8a)
$\displaystyle\mathrm{var}_{\bm{x}}$
$\displaystyle=(\bm{\Gamma}+\mathbf{G}_{\bm{u}}\bm{\mathcal{H}})\Sigma_{0}(\bm{\Gamma}+\mathbf{G}_{\bm{u}}\bm{\mathcal{H}})^{\mathrm{T}}$
$\displaystyle\leavevmode\nobreak\ \leavevmode\nobreak\
+(\mathbf{G}_{\bm{w}}+\mathbf{G}_{\bm{u}}\bm{\mathcal{K}})\mathbf{W}(\mathbf{G}_{\bm{w}}+\mathbf{G}_{\bm{u}}\bm{\mathcal{K}})^{\mathrm{T}},$
(8b) $\displaystyle\mu_{\bm{v}}$ $\displaystyle=\bm{\bar{v}},$ (8c)
$\displaystyle\mathrm{var}_{\bm{v}}$
$\displaystyle=\bm{\mathcal{H}}\Sigma_{0}\bm{\mathcal{H}}^{\mathrm{T}}+\bm{\mathcal{K}}\mathbf{W}\bm{\mathcal{K}}^{\mathrm{T}},$
(8d)

where $\mu_{0}=\mathbb{E}[x_{0}]$,
$\Sigma_{0}=\mathbb{E}[\tilde{x}_{0}\tilde{x}_{0}^{\mathrm{T}}]$ and
$\mathbf{W}=\operatorname{bdiag}(\mathbf{W}_{0},\dots,\mathbf{W}_{N-1})$.
Furthermore, the state and the control at the discrete stage $k$ can be
recovered from the concatenated state and input vectors as
$x_{k}=F^{x}_{k}\bm{x}$ and $v_{k}=F^{v}_{k}\bm{v}$, where $F^{x}_{k}$ and
$F^{v}_{k}$ denote the block matrices whose $k$th block is equal to the
identity matrix and the other blocks are equal to zero. Thus, the mean and the
covariance of $x_{k}$ and $v_{k}$ are given by
$\mu_{x_{k}}=F^{x}_{k}\mu_{\bm{x}}$,
$\mathrm{var}_{x_{k}}=F^{x}_{k}\mathrm{var}_{\bm{x}}F_{k}^{x\mathrm{T}}$,
$\mu_{v_{k}}=F^{v}_{k}\mu_{\bm{v}}$,
$\mathrm{var}_{v_{k}}=F^{v}_{k}\mathrm{var}_{\bm{v}}F_{k}^{u\mathrm{T}}$.

So, it follows from (8) that the mean of $x_{k}$ and $v_{k}$ can be expressed
as affine functions of the decision variable $\bm{\bar{v}}$ whereas the
covariance matrices of $x_{k}$ and $v_{k}$ can be expressed as convex
quadratic functions of the decision variables $\bm{\mathcal{H}}$ and
$\bm{\mathcal{K}}$. This allows us to cast various forms of the (constrained)
covariance steering problem as convex optimization problems which can be
solved with highly efficient solvers. The reader can refer to [15, 16, 17, 18]
for more details.

##  V Main Algorithm

The main components of the algorithm are the MPPI controller, the half-space
generator, and the Constrained Covariance Steering module. The MPPI controller
solves the unconstrained stochastic trajectory optimization problem and
returns a state and an input sequence of length $T_{\mathrm{MPPI}}$. The state
sequence generated by the MPPI module is used to generate the half-space
constraints. The state and the input sequences and half-space constraints are
used in the CCS module to solve for a policy that is guaranteed to be safe
with high probability.

Require : $T_{\mathrm{max}}$, $T_{\mathrm{CS}}$, $T_{\mathrm{MPPI}}$,
$\mathcal{M}_{\mathrm{obs}},\\{A_{k},B_{k},\mathbf{W}_{k}\\}_{k=0,\dots,T_{\mathrm{max}}},\sigma_{\mathrm{max}}$

1

2$\bar{x}_{0}\leftarrow x_{0}$;

3 $\Sigma_{k}\leftarrow\bm{0}$;

4 for  _$k\in\\{0,1,\dots,T_{\mathrm{max}}\\}$_ do

5 $X^{T_{\mathrm{MPPI}}},U^{T_{\mathrm{MPPI}}}\leftarrow$ MPPI($\bar{x}_{k}$);

6 $\mathcal{S}_{k}^{\mathrm{obs}}\leftarrow$
HSGen($X^{T_{MPPI}},\mathcal{M}_{\mathrm{obs}}$) ;

7

8 $\mu_{0}\leftarrow\bar{x}_{k}$; $\Sigma_{0}\leftarrow\Sigma_{k}$ ;

9

10 $\bm{\bar{v}},\bm{\mathcal{H}},\bm{\mathcal{K}}\leftarrow$
CCS($\mu_{0},\Sigma_{0},X^{T_{\mathrm{MPPI}}},U^{T_{\mathrm{MPPI}}},\mathcal{S}_{k}^{\mathrm{obs}}$);

11 $\bar{u}_{k}\leftarrow\bar{v}_{0}$, $L_{k}\leftarrow H_{0}$ ;

12

13 $u_{k}\leftarrow\bar{u}_{k}+L_{k}(x_{k}-\bar{x}_{k})$ ;

14 SendToActuators($u_{k}$);

15 $\bar{x}_{k+1}\leftarrow A_{k}\bar{x}_{k}+B_{k}\bar{u}_{k}$ ;

16
$\Sigma_{k+1}\leftarrow(A_{k}+B_{k}L_{k})\Sigma_{k}(A_{k}+B_{k}L_{k})^{\mathrm{T}}+\mathbf{W_{k}}$
;

17

18 if  _$\lambda_{\mathrm{max}}(\Sigma) >\sigma_{\mathrm{max}}$_ then

19 $\bar{x}_{k+1}\leftarrow x_{k}$; $\Sigma_{k+1}\leftarrow\bm{0}$;

20

Algorithm 1 CCSMPPI

###  V-A MPPI

In this paper, we follow the procedures described in [9] to use MPPI
algorithm. The MPPI module requires system dynamics and initial state $x_{0}$
to sample trajectories. The MPPI horizon $T_{\mathrm{MPPI}}$, the input
sampling covariance parameter $\nu$, and the number of trajectory samples $K$
are required as algorithm parameters.

###  V-B Half-space Generation

Safe half-spaces are generated by the “HSGen” procedure. It takes the first
$T_{\mathrm{CS}}$ states of the sequence $X^{T_{\mathrm{MPPI}}}$ and projects
the position vectors $p_{\ell}$ onto each obstacle $\mathcal{O}_{j}$. Then, it
computes the supporting hyperplane
$\mathcal{H}_{\ell,j}:=\\{p\in\mathbb{R}^{2}\leavevmode\nobreak\
|\leavevmode\nobreak\ a_{\ell,j}^{\mathrm{T}}p-b_{\ell,j}=0\\},$ at the point
of projection such that $a_{\ell,j}^{\mathrm{T}}p_{\ell}-b_{\ell,j}\geq
0\Rightarrow p_{\ell}\notin\mathcal{O}_{j}.$ The projection of position
$p_{\ell}$ at time $\ell$ onto the obstacle $\mathcal{O}_{j}$ is denoted as
$z_{\ell,j}$ and is defined as $z_{\ell,j}:=s_{j}+h_{\ell,j}r_{j}$, where
$h_{\ell,j}:=(p_{\ell}-s_{j})/\lVert p_{\ell}-s_{j}\rVert_{2}$ is the unit
normal vector to obstacle $\mathcal{O}_{j}$ at the point $z_{\ell,j}$ towards
$p_{\ell}$. We set $a_{\ell,j}=h_{\ell,j}$ and,
$b_{\ell,j}=a_{\ell,j}^{\mathrm{T}}z_{\ell,j}$. So, we can express
$a_{\ell,j}$ and $b_{\ell,j}$ in terms of $p_{l},s_{j}$ and $r_{j}$ as
follows:

$\displaystyle a_{\ell,j}$ $\displaystyle=\frac{(p_{\ell}-s_{j})}{\lVert
p_{\ell}-s_{j}\rVert_{2}},$ $\displaystyle b_{\ell,j}$
$\displaystyle=\frac{(p_{\ell}-s_{j})^{\mathrm{T}}s_{j}}{\lVert
p_{\ell}-s_{j}\rVert_{2}}+r_{j}.$ (9)

The halfspace generation process is repeated for each obstacle
$\mathcal{O}_{j}$ and every time step $\ell$. The halfspace parameters are
gathered in the set of tuples
$\mathcal{S}_{k}^{\mathrm{obs}}=\\{(a_{\ell,j},b_{\ell,j})\\}_{\ell=0,\dots,T_{MC}}^{j=1,\dots,N_{\mathrm{obs}}}$
to be used in Constrained Covariance Steering. The condition
$p_{\ell}\notin\mathcal{O}_{j}$ is not necessary for the half-space generation
procedure. Even if $p_{\ell}\in\mathcal{O}_{j}$, the procedure described by
equations in (9) generates a half-space $\mathcal{H}_{\ell,j}$ such that
$a_{\ell,j}^{\mathrm{T}}p-b_{\ell,j}\leq 0$ holds for all
$p\in\mathcal{O}_{j}$.

###  V-C Constrained Covariance Steering

The goal of the Constrained Covariance Steering module is to minimize the
deviation of the actual state and control sequence from the reference state
and control sequences which are computed by the MPPI algorithm while
satisfying the safety constraints. This problem can be formally stated as the
following stochastic optimal control problem:

$\displaystyle\underset{\pi\in\Pi}{\textrm{minimize}}$ $\displaystyle\quad
J(\pi):=\mathbb{E}\Big{[}\sum_{\ell=0}^{T_{CS}-1}\delta
x_{\ell}^{\mathrm{T}}Q_{\ell}\delta x_{\ell}+\delta
u_{\ell}^{\mathrm{T}}R_{\ell}\delta u_{\ell}$
$\displaystyle\qquad\qquad\quad+\delta x_{T_{CS}}^{\mathrm{T}}Q_{T_{CS}}\delta
x_{T_{CS}}\Big{]}$ (10a) subject to $\displaystyle\quad
x_{\ell+1}=A_{\ell}x_{\ell}+B_{\ell}u_{\ell}+w_{\ell},\quad\forall\ell\in\mathcal{I}_{t}$
(10b) $\displaystyle\quad
u_{\ell}=\pi(x_{0},\dots,x_{\ell}),\qquad\qquad\;\forall\ell\in\mathcal{I}_{\mathrm{t}}$
(10c)
$\displaystyle\quad\mathbb{P}\left[a_{\ell,j}^{\mathrm{T}}p_{\ell}-b_{\ell,j}\geq
0\right]\geq 1-P_{\mathrm{fail}},\quad$
$\displaystyle\qquad\qquad\qquad\qquad\qquad\qquad\forall\\{\ell,j\\}\in\mathcal{I}$
(10d)

where $\Pi$ denotes the set of all admissible control policies, $\delta
x_{\ell}=x_{\ell}-x_{\ell}^{\mathrm{MPPI}}$, $\delta
u_{\ell}=u_{\ell}-u_{\ell}^{\mathrm{MPPI}}$,
$\mathcal{I}_{\mathrm{t}}:=\\{0,\dots,T_{CS}\\}$,
$\mathcal{I}_{\mathrm{o}}:=\\{1,\dots,N_{\mathrm{obs}}\\}$,
$\mathcal{I}=\mathcal{I}_{\mathrm{t}}\times\mathcal{I}_{\mathrm{o}}$. The
stochastic optimal control problem defined in (10) can be cast as a
deterministic optimization problem by fixing the policy as in (6) as explained
in Section IV. The resulting finite-dimensional deterministic optimization
problem is given by:

$\displaystyle\underset{\bm{\bar{u}},\bm{\mathcal{H}},\bm{\mathcal{K}}}{\textrm{minimize}}$
$\displaystyle\quad\mathcal{J}(\bm{\bar{u}},\bm{\mathcal{H}},\bm{\mathcal{K}}):=\delta\bm{\bar{x}}^{\mathrm{T}}\bm{Q}\delta\bm{\bar{x}}+\delta\bm{\bar{u}}^{\mathrm{T}}\bm{\mathcal{R}}\delta\bm{\bar{u}}$
$\displaystyle\qquad\qquad+\operatorname{tr}(\bm{\mathcal{Q}}\mathrm{var}_{\bm{x}})+\operatorname{tr}(\bm{\mathcal{R}}\mathrm{var}_{\bm{u}})$
(11a) subject to $\displaystyle\quad
a_{\ell,j}^{\mathrm{T}}\mathbf{P}_{\ell}\mu_{\bm{x}}-b_{\ell,j}\geq$
$\displaystyle\qquad\qquad\alpha\lVert\bm{\zeta}^{\mathrm{T}}\mathbf{P}_{\ell}^{\mathrm{T}}a_{\ell,j}\rVert_{2},\leavevmode\nobreak\
\leavevmode\nobreak\ \forall\\{\ell,j\\}\in\mathcal{I}$ (11b)
$\displaystyle\quad\bm{\zeta}=\left[(\mathbf{G}_{0}+\mathbf{G_{u}}\bm{\mathcal{H}})\leavevmode\nobreak\
(\mathbf{G_{w}}+\mathbf{G_{w}}\bm{\mathcal{K}})\right]\mathbf{R}$ (11c)

where
$\mathbf{R}\mathbf{R}^{\mathrm{T}}=\operatorname{bdiag}(\Sigma_{0},\mathbf{W})$,
$\bm{x}$ and $\bm{u}$ are defined as in Section IV,
$\delta\bm{\bar{x}}=\mu_{\bm{x}}-\bm{x}^{\mathrm{MPPI}}$,
$\delta\bm{\bar{u}}=\mu_{\bm{u}}-\bm{u}^{\mathrm{MPPI}}$,
$\bm{\mathcal{Q}}=\operatorname{bdiag}(Q_{0},\dots,Q_{T_{CS}})$,
$\bm{\mathcal{R}}=\operatorname{bdiag}(R_{0},\dots,R_{T_{CS}-1})$.
$\mathbf{P}_{\ell}$ is defined such that $p_{\ell}=\mathbf{P}_{\ell}\bm{x}$
and $\alpha=\varphi^{-1}(1-P_{\mathrm{fail}})$ where $\varphi$ is the
cumulative density function of normal random variable with zero mean and unit
variance. Finally, $\mu_{\bm{x}}$, $\mu_{\bm{u}}$, $\mathrm{var}_{\bm{x}}$ and
$\mathrm{var}_{\bm{u}}$ are defined as in (8a)-(8d). In addition, we observe
that $\mathrm{var}_{\bm{x}}=\bm{\zeta}\bm{\zeta}^{\mathrm{T}}$ where
$\bm{\zeta}$ is given by (11c).

To see the equivalence of optimization problems in (11) and (10), first,
observe that the objective function in (10) can be written as
$\mathbb{E}[\delta\bm{x}^{\mathrm{T}}\bm{\mathcal{Q}}\delta\bm{x}+\delta\bm{u}^{\mathrm{T}}\bm{\mathcal{R}}\delta\bm{u}]$
where $\delta\bm{x}=\bm{x}-\bm{x}^{\mathrm{MPPI}}$,
$\delta\bm{u}=\bm{u}-\bm{u}^{\mathrm{MPPI}}$. Using the cyclic permutation
property of trace operator, the linearity of expectation and the equalities
$\mathrm{var}_{\delta\bm{x}}=\mathrm{var}_{\bm{x}}$ and
$\mathrm{var}_{\delta\bm{u}}=\mathrm{var}_{\bm{u}}$, it follows that the
objective functions in (10a) and (11a) are equivalent. Proposition 1 along
with the expressions $\mu_{p_{\ell}}=\mathbf{P}_{\ell}\mu_{x}$ and
$\mathrm{var}_{p_{\ell}}=\mathbf{P}_{\ell}\bm{\zeta}\bm{\zeta}^{\mathrm{T}}\mathbf{P}_{\ell}^{\mathrm{T}}$
show that (11b), (11c) $\Leftrightarrow$ (10d).

######  Proposition 1.

Let $p\sim\mathcal{N}(\mu_{p},\Sigma_{p})$, where
$a,\mu_{p}\in\mathbb{R}^{n}$, $b\in\mathbb{R}$,
$\Sigma_{p}\in\mathbb{S}_{n}^{+}$, and $P\in(0,1/2]$. Then,
$\mathbb{P}[a^{\mathrm{T}}p-b\geq 0]\geq 1-P$ if and only if
$a^{\mathrm{T}}\mu_{p}-b\geq\varphi^{-1}(1-P)\lVert\mathbf{R}^{\mathrm{T}}a\rVert_{2}$
where $\varphi:\mathbb{R}\rightarrow(0,1)$ is the cumulative density function
of normally distributed random variable with zero mean and unit variance and,
finally $\mathbf{R}$ is such that
$\mathbf{R}\mathbf{R}^{\mathrm{T}}=\Sigma_{p}$.

The problem in (11) has a convex quadratic objective function and the
constraint in (11c) is affine. Also, the constraint in (11b) corresponds to a
second-order cone constraint since
$\alpha=\varphi^{-1}(1-P_{\mathrm{fail}})\geq 0$ for all
$P_{\mathrm{fail}}\in(0,0.5]$ [19]. Thus, problem 11 can be solved for global
optimal solution
$(\bm{\bar{v}}^{\star},\bm{\mathcal{H}}^{\star},\bm{\mathcal{K}}^{\star})$
using off-the-shelf solvers such as [20].

###  V-D Discussion

First, the algorithm is initialized by setting $\bar{x}_{0}=x_{0}$ and
$\Sigma_{0}=\bm{0}$ where $\bar{x}_{0}$ and $\Sigma_{0}$ represent the initial
nominal state and initial covariance respectively. Then, using $\bar{x}_{k}$
as the initial state, MPPI generates a pair of reference state and input
sequences $(X^{T_{\mathrm{MPPI}}},U^{T_{\mathrm{MPPI}}})$. The state sequence
is used to generate a safe convex region over which the constraints (10d) are
satisfied based on the technique that will be described in Section V-B. Then,
we formulate a corresponding CCS problem that seeks for a control policy in
the form of (6) that will guarantee the satisfaction of chance constraints
while minimizing the deviation from reference state and control sequences. If
the largest eigenvalue of the computed covariance $\Sigma_{k+1}$ exceeds a
predetermined threshold $\sigma_{\mathrm{max}}$, then the nominal state
$\bar{x}_{k}$ is set equal to the real state $x_{k}$ and covariance
$\Sigma_{k}$ is set to $\bm{0}$. Next, the nominal state $\bar{x}_{k+1}$ and
covariance matrix $\Sigma_{k+1}$ will be updated as described in lines 9-10 in
Algorithm 1.

It is worth mentioning that the CCS module uses the disturbance noise
covariance in its formulation. However, this information is usually unknown in
real-world scenarios. But, this can easily be handled by over-approximating
the noise covariance, that is, by taking
$\mathbf{W}_{k}\succeq\mathbf{W}_{k}^{\mathrm{real}}$, where
$\mathbf{W}_{k}^{\mathrm{real}}$ is the actual noise covariance that is acting
on the system. This allows the CCS module to find a policy that satisfies the
safety constraints. Furthermore, this condition implies that the state will
stay in the safe region with probability greater than $1-P_{\mathrm{fail}}$.
Although this approach may generate overly conservative policies, system
identification techniques can be used to learn the actual noise covariance
[21] and hence reduce conservativeness.

The final component of our algorithm is the use of nominal dynamics, which is
the noise-free version of the real dynamics. Since the CCS module assures that
chance constraints are satisfied and it uses the nominal state as the initial
mean state in its formulation, the safety margins of the nominal state will be
greater than the real state. Also, by computing the covariance in line 10 in
Algorithm 1, we compute the high probability region where the real state lies.
Then, this covariance value is used as initial covariance in the CCS procedure
in the next step to guarantee the satisfaction of the chance constraints.

##  VI Numerical Experiments

In our numerical experiments, we consider a double integrator dynamics
described by (1) with euler discretization scheme with $dt=0.05$, and
$\\{w_{k}\\}$ is taken to be a white noise process with
$w_{k}\sim\mathcal{N}(\bm{0},\mathbf{W}_{k})$ where the noise covariance
matrix $\mathbf{W}_{k}$ varies depending on different problem instances. We
show the efficacy of our approach in two trajectory optimization problems: an
obstacle avoidance problem and a path generation problem in a circular track.

Obstacle Avoidance: In the obstacle avoidance case, we compare the
performances of CCSMPPI with tube-MPPI [10] under high noise that is acting
upon the system to show the robustness of our approach. In the tube-MPPI
formulation, an LQG tracking controller is used to track nominal state and
input sequences. In our experiments, the LQG cost function parameters
$Q^{\mathrm{LQG}}_{k},R^{\mathrm{LQG}}_{k}$ are chosen to be equal to the cost
function parameters used in the CCS formulation given in (10a). Also, the
failure parameter $P_{\mathrm{fail}}$ that is shown in (10d) is taken to be
$0.01$.

We consider the running cost function $q_{\mathrm{hard}}(p_{k})=\lVert
p_{k}-p_{\mathrm{des}}\rVert_{2}^{2}+5000\sum_{j=1}^{N_{\mathrm{obs}}}\mathbb{I}_{\mathcal{O}_{j}}(p_{k})$
and $\Phi_{\mathrm{hard}}(x_{T})=0$ where
$\mathbb{I}_{\mathcal{O}_{j}}:\mathbb{R}^{2}\rightarrow\\{0,1\\}$ is the
indicator function of set $\mathcal{O}_{j}$.

The parameters of the MPPI algorithm used in these experiments are
$T_{\mathrm{MPPI}}=40$, $K=100$, $\lambda=0.1$, $\nu=0.1$ and
$\epsilon_{k}\sim\mathcal{N}({\bm{0},0.001I})$. In addition, the problem
horizon parameter $T_{\mathrm{max}}=200$ and the noise covariance matrix
$\mathbf{W}_{k}=\operatorname{bdiag}(0.0,0.0,5.0,5.0)$. The state-dependent
term of the running cost function was taken to be
$q(x_{k})=10\,q_{\mathrm{hard}}(x_{k})$ and the desired final position
$p_{\mathrm{des}}=[2.0,10.0]^{\mathrm{T}}$.

(a) CCSMPPI

(b) Tube-MPPI

(a) CCSMPPI - $q_{\mathrm{t,s}}$

(b) CCSMPPI - $q_{\mathrm{t,h}}$

(c) Tube-MPPI - $q_{\mathrm{t,s}}$

(d) Tube-MPPI - $q_{\mathrm{t,h}}$

(e) MPPI - $q_{\mathrm{t,s}}$

(f) MPPI - $q_{\mathrm{t,h}}$

Figure 2: CCSMPPI, Tube-MPPI and standard MPPI in different scenarios

Figure 1(a) illustrates 10 randomly sampled trajectories induced by the
CCSMPPI algorithm. Although the intensity of the noise that is acting upon the
system is quite high compared to the sampling distribution parameter $\nu$,
the CCSMPPI is successfully avoiding obstacles. Figure 1(b) shows 10 randomly
sampled trajectories of the system running under the tube-MPPI algorithm. It
can be seen that the agent reaches the goal position but fails to avoid
obstacles even though the trajectories that lead to collisions are heavily
penalized. In this case, tube-MPPI fails to handle uncertain disturbances and
generate safe control inputs.

Circular Track: In this scenario, the goal is to keep the position of the
system in a circular track with inner and outer radius
$R_{\mathrm{in}},R_{\mathrm{out}}=R_{\mathrm{c}}\mp 0.125$ where
$R_{\mathrm{c}}=2$ while maintaining a desired speed $v_{\mathrm{des}}$ in
counter-clockwise direction. In the numerical experiments, this goal is
encoded as 2 different running cost functions $q_{\mathrm{t,s}}(x_{k})$ and
$q_{\mathrm{t,h}}(x_{k})$:

$\displaystyle q_{\mathrm{t,s}}(x_{k})$ $\displaystyle:=(\lVert
v_{k}\rVert_{2}-v_{\mathrm{des}})^{2}+\lVert(p_{k}^{x}v^{y}_{k}-v^{x}_{k}p_{k}^{y})-R_{\mathrm{c}}v_{\mathrm{des}}\rVert$
$\displaystyle+100\left(\sqrt{p_{k}^{x2}+p_{k}^{y2}}-R_{\mathrm{c}}\right)^{2}$
(12a) $\displaystyle q_{\mathrm{t,h}}(x_{k})$ $\displaystyle:=(\lVert
v_{k}\rVert_{2}-v_{\mathrm{des}})^{2}+\lVert(p_{k}^{x}v^{y}_{k}-v^{x}_{k}p_{k}^{y})-R_{\mathrm{c}}v_{\mathrm{des}}\rVert$
$\displaystyle+5000\mathbb{I}_{\lnot C}(p_{k})$ (12b)

where $C:=\\{p\in\mathbb{R}^{2}\,|\,R_{\mathrm{c}}-0.125\leq\lVert
p\rVert_{2}\leq R_{\mathrm{c}}+0.125\\}$, $\mathbb{I}_{\lnot
C}:\mathbb{R}^{2}\rightarrow\\{0,1\\}$ is the indicator function such that
$\mathbb{I}_{\lnot C}(p)=1$ if $p\notin C$ and $\mathbb{I}_{\lnot C}(p)=0$ if
$p\in C$.

The safety criterion in this example is to stay within the circular track
which is formally defined by set $C$. This condition is encoded in
$q_{\mathrm{t,s}}(x_{k})$ in (12a) by penalizing the deviation of the position
$p_{k}$ from the mid radius $R_{\mathrm{c}}$. This choice makes
$q_{t,s}(x_{k})$ a smooth function. On the other hand, the safety criterion is
encoded by using indicator functions in (12b) which is a more clear encoding
of the safety constrained however non-smoothness of $q_{\mathrm{t,h}}(x_{k})$
makes this problem harder to solve.

In Figure 2, trajectories generated by CCSMPPI, Tube-MPPI and standard MPPI
are shown from the top to the bottom. Figures 2(a), 2(c), 2(e) show the
results with running cost $q(x_{k})=100\,q_{\mathrm{t,s}}(x_{k})$ and Figures
2(b), 2(d), 2(f) show the results with
$q(x_{k})=100\,q_{\mathrm{t,h}}(x_{k})$. The parameters of the MPPI are
$T_{\mathrm{MPPI}}=20$, $K=200$, $\lambda=0.1$, $\nu=1.0$, and
$\epsilon\sim\mathcal{N}(\bm{0},0.001I)$. In addition, the problem horizon
$T_{\mathrm{max}}=300$,
$\mathbf{W_{k}}=\operatorname{bdiag}(0.001,0.001,1.0,1.0)$ and
$v_{\mathrm{des}}=6.0$.

When $q_{\mathrm{t,s}}(x_{k})$ is used as a running cost function, the
trajectories induced by tube-MPPI and standard MPPI fail to meet the safety
criterion (shown in Figures 2(c), 2(e)) due to the poor design of the cost
function. On the other hand, it is shown in Figure 2(a) that the CCSMPPI
manages to keep the position within the track. When the running cost is
switched to $q_{\mathrm{t,h}}(x_{k})$, the non-smoothness of the cost function
causes standard MPPI to fail as shown in Figure 2(f). Even though tube-MPPI
seems to keep the position within the circular track in Figure 2(d), there are
more violations of the safety constraints than CCSMPPI as is shown in Figure
2(b).

In Table I, we compare the performance of CCSMPPI, tube-MPPI and standard MPPI
by sampling $N_{\mathrm{sim}}=15$ trajectories for both experiments #1 and #2.
The running cost function is taken as $100q_{t,s}(x_{k})$ and
$100q_{t,h}(x_{k})$ in experiments #1 and #2, respectively. Also,
$T_{\mathrm{max}}$ is taken as $200$ and $300$ in experiments #1 and #2,
respectively. In both experiments, $\mathbf{W_{k}}$ are chosen to be equal to
$\operatorname{bdiag}(0.005,0.005,0.5,0.5)$ and $v_{\mathrm{des}}=6.0$. Prfail
represents the probability of failure and it is computed by dividing the
number of trajectories that leave the circular track at least once
($N_{\mathrm{fail}}$) by the total number of trajectories $N_{\mathrm{sim}}$.

It can be seen from the results of experiment #1 in Table I that standard MPPI
performs better in terms of minimizing the cost than both tube-MPPI and
CCSMPPI and reaches higher speeds. However, this is due to the poor design of
the cost function, and the fact that the control inputs that are corrected by
CCS module to guarantee safety are not optimal with respect to the used cost
function. When $q_{\mathrm{t,h}}(x_{k})$ is used as the running cost in
experiment #2, standard MPPI performs worse than both tube-MPPI and CCSMPPI
due to the presence of random noise $w_{k}$. In these experiments, the safety
of the trajectory is the first priority, as encoded in the running cost
$q_{\mathrm{t,h}}(x_{k})$. Although tube-MPPI reaches higher speeds, it fails
to reach the safety levels of CCSMPPI.

It should be highlighted that the probability of violating the constraint in
(10d) at every time step $k$ is less than $P_{\mathrm{fail}}=0.01$ but still
greater than 0. This means that as $T_{\mathrm{max}}\rightarrow\infty$, the
failure probability approaches 1. This is the reason why Prfail is non-zero
for CCSMPPI in both experiments. Prfail can be reduced by lowering the safety
threshold $P_{\mathrm{fail}}$, however it is not possible to make it 0 since
$w_{k}$ is assumed to be normally distributed and thus unbounded.

Exp. #1 | Av. Speed | Max Speed | Prfail | Cost
---|---|---|---|---
MPPI | 2.46 $\pm$ 0.31 | 3.42 $\pm$ 0.35 | 1.0 | 44.1 $\pm$ 7.0
Tube-MPPI | 2.37 $\pm$ 0.32 | 2.95 $\pm$ 0.36 | 0.87 | 47.9 $\pm$ 8.2
CCSMPPI | 2.33 $\pm$ 0.31 | 3.03 $\pm$ 0.37 | 0.13 | 58.6 $\pm$ 8.5
Exp. #2 | Av. Speed | Max Speed | Prfail | Cost
MPPI | 1.66 $\pm$ 0.24 | 3.53 $\pm$ 0.32 | 1.0 | 259.6 $\pm$ 73.8
Tube-MPPI | 1.77 $\pm$ 0.26 | 2.85 $\pm$ 0.35 | 0.67 | 95.6 $\pm$ 20.9
CCSMPPI | 1.65 $\pm$ 0.27 | 2.67 $\pm$ 0.29 | 0.07 | 66.1 $\pm$ 8.3
TABLE I: Performance Comparision Statistics

##  VII Conclusion

In this paper, we presented a novel framework for safe trajectory optimization
for stochastic linear systems. We used Model Predictive Path Integral (MPPI)
control for stochastic optimization and a projection-based linearization
method for the generation of safe convex regions. In addition, we used a
Constrained Covariance Steering algorithm to safeguard against unmodeled noise
disturbances that the MPPI algorithm may not always handle. Our numerical
simulations have demonstrated that our approach can guarantee safety against
unmodeled noise uncertainties as well as unsafe outputs generated by the
stochastic optimization algorithm. In our future work, we plan to extend our
proposed framework to trajectory generation problems for uncertain nonlinear
systems based on model-free trajectory optimization algorithms while
guaranteeing safety.

## References

  * [1] S. Gros, M. Zanon, R. Quirynen, A. Bemporad, and M. Diehl, “From linear to nonlinear MPC: bridging the gap via the real-time iteration,” International Journal of Control, vol. 93, no. 1, pp. 62–80, 2020.
  * [2] D. Malyuta, T. P. Reynolds, M. Szmuk, T. Lew, R. Bonalli, M. Pavone, and B. Açıkmeşe, “Convex optimization for trajectory generation,” arXiv preprint arXiv:2106.09125, 2021.
  * [3] Y. Mao, M. Szmuk, and B. Açıkmeşe, “Successive convexification of non-convex optimal control problems and its convergence properties,” in 2016 IEEE 55th Conference on Decision and Control (CDC), pp. 3636–3641, 2016.
  * [4] F. Augugliaro, A. P. Schoellig, and R. D’Andrea, “Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach,” in 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 1917–1922, 2012.
  * [5] E. Theodorou, Y. Tassa, and E. Todorov, “Stochastic differential dynamic programming,” in Proceedings of the 2010 American Control Conference, pp. 1125–1132, IEEE, 2010.
  * [6] E. Todorov and W. Li, “A generalized iterative LQG method for locally-optimal feedback control of constrained nonlinear stochastic systems,” in Proceedings of the 2005, American Control Conference, 2005., pp. 300–306, IEEE, 2005.
  * [7] Y. Aoyama, G. Boutselis, A. Patel, and E. A. Theodorou, “Constrained differential dynamic programming revisited,” in 2021 IEEE International Conference on Robotics and Automation (ICRA), pp. 9738–9744, 2021.
  * [8] Z. Xie, C. K. Liu, and K. Hauser, “Differential dynamic programming with nonlinear constraints,” in 2017 IEEE International Conference on Robotics and Automation (ICRA), pp. 695–702, 2017.
  * [9] G. Williams, N. Wagener, B. Goldfain, P. Drews, J. M. Rehg, B. Boots, and E. A. Theodorou, “Information theoretic MPC for model-based reinforcement learning,” in 2017 IEEE International Conference on Robotics and Automation (ICRA), pp. 1714–1721, IEEE, 2017.
  * [10] G. Williams, B. Goldfain, P. Drews, K. Saigol, J. M. Rehg, and E. A. Theodorou, “Robust sampling based model predictive control with sparse objective information.,” in Robotics: Science and Systems, 2018.
  * [11] K. Okamoto and P. Tsiotras, “Optimal stochastic vehicle path planning using covariance steering,” IEEE Robotics and Automation Letters, vol. 4, no. 3, pp. 2276–2281, 2019.
  * [12] J. Yin, Z. Zhang, E. Theodorou, and P. Tsiotras, “Improving model predictive path integral using covariance steering,” arXiv preprint arXiv:2109.12147, 2021.
  * [13] Y. Chen, T. T. Georgiou, and M. Pavon, “Optimal steering of a linear stochastic system to a final probability distribution, part I,” IEEE Transactions on Automatic Control, vol. 61, no. 5, pp. 1158–1169, 2015.
  * [14] A. Hotz and R. E. Skelton, “Covariance control theory,” International Journal of Control, vol. 46, no. 1, pp. 13–32, 1987.
  * [15] E. Bakolas, “Finite-horizon covariance control for discrete-time stochastic linear systems subject to input constraints,” Automatica, vol. 91, pp. 61–68, 2018.
  * [16] I. M. Balci and E. Bakolas, “Covariance control of discrete-time gaussian linear systems using affine disturbance feedback control policies,” in 2021 60th IEEE Conference on Decision and Control (CDC), pp. 2324–2329, 2021\.
  * [17] E. Bakolas, “Covariance control for discrete-time stochastic linear systems with incomplete state information,” in 2017 American Control Conference (ACC), pp. 432–437, IEEE, 2017.
  * [18] J. Pilipovsky and P. Tsiotras, “Chance-constrained optimal covariance steering with iterative risk allocation,” in 2021 American Control Conference (ACC), pp. 2011–2016, IEEE, 2021.
  * [19] S. Boyd, S. P. Boyd, and L. Vandenberghe, Convex optimization.  Cambridge university press, 2004.
  * [20] MOSEK ApS, MOSEK Optimizer API for Python 9.2.40, 2019.
  * [21] B. Feng, M. Fu, H. Ma, Y. Xia, and B. Wang, “Kalman filter with recursive covariance estimation—sequentially estimating process noise covariance,” IEEE Transactions on Industrial Electronics, vol. 61, no. 11, pp. 6253–6263, 2014.