[acronym]long-short

# idSTLPy: A Python Toolbox for Active Perception and Control
††thanks: This work was supported in part by the National Science Foundation
under Grant IIS-1724070, Grant CNS-1830335 and Grant IIS-2007949.

Rafael Rodrigues da Silva1 Kunal Yadav1, and Hai Lin1 1 All authors are with
Department of Electrical Engineering, University of Notre Dame, Notre Dame, IN
46556, USA<EMAIL_ADDRESS><EMAIL_ADDRESS><EMAIL_ADDRESS>
###### Abstract

This paper describes a Python toolbox for active perception and control
synthesis of probabilistic signal temporal logic (PrSTL) formulas of switched
linear systems with additive Gaussian disturbances and measurement noises. We
implement a counterexample-guided synthesis strategy that combines Bounded
Model Checking, linear programming, and sampling-based motion planning
techniques. We illustrate our approach and the toolbox throughout the paper
with a motion planning example for a vehicle with noisy localization. The code
is available at https://codeocean.com/capsule/0013534/tree.

##  I Introduction

The recent decade has seen more intelligent systems in our day-to-day lives,
but many of them are still pre-programmed or only work well in controlled
environments. Next-generation intelligent systems need to recognize their
surrounding environments, make predictions of the environment behavior, and
purposely take actions to improve confidence in their belief of environment
states. This process is known as an active perception: the intelligent system
explicitly explores the environment to collect more information about the
environmental behavior [1].

Since the process of active perception involves both actions and perceptions,
we propose to specify an active perception task as probabilistic signal
temporal logic (PrSTL) formulas, which combine real-time temporal logic with
chance constraints. Then the active perception problem can be solved as a
controller design for a given PrSTL specification with uncertain and
differential constraints.

Existing PrSTL controller synthesis methods include mixed-integer Second-Order
Cone Programming (SOCP) [2, 3], sampling-based optimization [4], and
heuristic-search based [5]. SOCP and sampling-based methods provide satisfying
controllers for a convex fragment of PrSTL, but do not incorporate a
perception model in the system dynamics. Thus these algorithms cannot
synthesize controls to gather more information, and therefore are not
considered active perception methods.

In this paper, we introduce idSTLPy: a software toolbox for active perception
and control developed based on our recent work in [6, 7]. This toolbox is an
open-source software package for designing the trajectory of an intelligent
system with active perception from temporal specification and hybrid dynamics.
Unlike other methods, this toolbox synthesizes controllers that consider the
effects of observation on the belief dynamics. Hence, the planned trajectory
includes motions that reduce the uncertainty about the state variables to
achieve the task, i.e., active perception.

Our current development is inspired by several toolboxes for symbolic control
in the literature, such as TuLip [8], Linear Temporal Logic MissiOn Planning
(LTLMoP) [9] and Open Motion Planning Library [10], which support the design
of controllers for deterministic hybrid systems from Linear Temporal Logic
(LTL) formulas. However, to our best knowledge, idSTLPy would be the first
toolbox that tackles active perception and control for stochastic systems.

The current version of idSTLPy models the stochastic system behavior as a
switched linear system with Gaussian noises. This model allows us to inherit
the computational efficiency and soundness of Kalman filtering. Additionally,
these systems help to represent complex behaviors of physical systems
interacting with logical rules or controllers. Therefore, these switched
systems allow us to model several real-life problems.

The software is written in Python. Our basic idea is to combine Bounded Model
Checking (BMC) with sampling-based motion planning to separate logical and
dynamical constraints. We propose abstractions that approximate the belief
dynamics during the planning and permit us to use these techniques. We show
through a simple example that the system can track the planned trajectory
during the execution. Therefore, the main goal of this paper is to introduce
the newly developed toolbox through a motion planning example under uncertain
localization.

This paper is structured as follows: Section II briefly describes the
preliminaries, Sections III and IV gives the overview of the toolbox with an
example. Finally, Section V concludes the work.

##  II Preliminaries

###  II-A System

We consider switched linear control systems as follows:

$\displaystyle\bm{x}_{k+1}=$ $\displaystyle
A_{q_{k}}\bm{x}_{k}+B_{q_{k}}\bm{u}_{k}+W_{q_{k}}\bm{W}_{k},$
$\displaystyle\bm{W}_{k}\sim\mathcal{N}(0,I)$ (1) $\displaystyle\bm{y}_{k}=$
$\displaystyle C_{q_{k}}\bm{x}_{k}+n_{q_{k}}(\bm{x}_{k})\bm{V}_{k},$
$\displaystyle\bm{V}_{k}\sim\mathcal{N}(0,I),$

where $\bm{x}_{k}\in\mathbb{R}^{n}$ are the state variables,
$\bm{u}_{k}\in\mathcal{U}\subseteq\mathbb{R}^{m}$ are the input variables,
$\mathcal{U}\subseteq\mathbb{R}^{m}$ is a polytope,
$\bm{y}_{k}\in\mathbb{R}^{p}$ are the output variables. Each system location
$q\in Q=\\{1,2,\dots,N\\}$ is defined by a noise function
$n_{q}:\mathbb{R}^{n}\rightarrow\mathbb{R}^{n}$, and constant matrices
$A_{q}\in\mathbb{R}^{n\times n}$, $B_{q}\in\mathbb{R}^{n\times m}$, and
$C_{q}\in\mathbb{R}^{p\times n}$ with proper dimensions. We assume that the
system is subject to mutually uncorrelated zero-mean stationary Gaussian
additive disturbances $\bm{V}_{k}\sim\mathcal{N}(0,I_{n})$ and
$\bm{W}_{k}\sim\mathcal{N}(0,I_{p})$, where $I_{n}$ is the identity matrix
with dimension $n$. Note that this dynamical system can arise from
linearization and sampling of a more general continuous system. In such a
case, we denote the sampling period as $T_{s}$, where $T_{s}=t_{k+1}-t_{k}$
for all $k\in\mathbb{N}_{\geq 0}$. We assume that the uncertainty is stable,
meaning that the uncertainty does not increase infinitely over time.

###  II-B Trajectory

The system in Eq. 1 is probabilistic. This means that the dynamics result into
a random process $\bm{X}_{k}$ that represent probabilities over the state
variables $prob(\bm{X}_{k}=\bm{x}_{k}$). We call this random process as belief
state. A belief trajectory $\bm{\beta}$ is defined as a sequence
$\bm{X}_{0}\xrightarrow{q_{0},\bm{u}_{0},\bm{y}_{1}}\bm{X}_{1}\dots$. A
transition $\bm{X}_{k}\xrightarrow{q_{k},\bm{u}_{k},\bm{y}_{k}}\bm{X}_{k+1}$
represents the process of applying a command $q_{k}\in Q$ and input
$\bm{u}_{k}\in\mathcal{U}$ at instant $k$ and waiting for an observation
$\bm{y}_{k+1}$ at instant $k+1$ to update the next belief state
$\bm{X}_{k+1}$.

###  II-C Probabilistic Signal Temporal Logic

We specify the requirements of a system belief trajectory using PrSTL
formulas. These formulas are defined recursively according to the following
grammar:

$\displaystyle\phi:=$
$\displaystyle\pi^{\mu}_{\epsilon}|\pi^{\mathbb{Q}}|\pi^{\mathbb{Q}_{1}}\vee\pi^{\mathbb{Q}_{2}}|\phi_{1}\wedge\phi_{2}$
$\displaystyle\varphi:=$
$\displaystyle\phi|\varphi_{1}\wedge\varphi_{2}|\varphi_{1}\vee\varphi_{2}|\varphi_{1}\bm{U}_{[a,b]}\varphi_{2}|\varphi_{1}\bm{R}_{[a,b]}\varphi_{2},$

where $\pi$ is a predicate, $\varphi$, $\varphi_{1}$, and $\varphi_{2}$ are
PrSTL formulas, and $\phi$, $\phi_{1}$, and $\phi_{2}$ are PrSTL state
formulas. Predicates can be one of two types: atomic and probabilistic. An
atomic predicate $\pi^{\mathbb{Q}}$ is a statement about the system locations
and is defined by a subset $\mathbb{Q}\subseteq Q$ of locations. A
probabilistic predicate $\pi^{\mu}_{\epsilon}$ is a statement about the belief
$\bm{X}_{k}$ defined by a linear function
$\mu:\mathbb{R}^{n}\rightarrow\mathbb{R}$ and a a tolerance
$\epsilon\in[0,0.5]$. The operators $\wedge,\vee$ are Boolean operators
conjuntion, and disjunction, respectively. The temporal operators $\bm{U}$ and
$\bm{R}$ are LTL operators until and release, respectively. In PrSTL, these
operators are defined by an interval $[a,b]\subseteq\mathbb{N}_{\geq 0}$. We
assume that PrSTL state formulas forms a full-dimensional region in the state
space $\mathbb{R}^{n}$.

We denote the fact that a belief trajectory $\bm{\beta}$ satisfies an PrSTL
formula $\varphi$ with $\bm{\beta}\vDash\varphi$. Furthermore, we write
$\bm{\beta}\vDash_{k}\varphi$ if the trajectory
$\bm{X}_{k}\xrightarrow{q_{k},\bm{u}_{k},\bm{y}_{k+1}}\bm{X}_{k+1}\dots$
satisfies $\varphi$. Formally, the following semantics define the validity of
a formula $\varphi$ with respect to the trajectory $\bm{\beta}$:

  * •

$\bm{\beta}\vDash_{k}\pi^{\mathbb{Q}}$ if and only if $k=0$ or
$q_{k-1}\in\mathbb{Q}$,

  * •

$\bm{\beta}\vDash_{k}\pi^{\mu}_{\epsilon}$ if and only if
$p\big{(}\mu(\bm{x}_{k})\leq 0\big{)}\geq 1-\epsilon$,

  * •

$\bm{\beta}\vDash_{k}\varphi_{1}\wedge\varphi_{2}$ if and only if
$\bm{\beta}\vDash_{k}\varphi_{1}$ and $\bm{\beta}\vDash_{k}\varphi_{2}$,

  * •

$\bm{\beta}\vDash_{k}\varphi_{1}\vee\varphi_{2}$ if and only if
$\bm{\beta}\vDash_{k}\varphi_{1}$ or $\bm{\beta}\vDash_{k}\varphi_{2}$,

  * •

$\bm{\beta}\vDash_{k}\varphi_{1}\bm{U}_{[a,b]}\varphi_{2}$ if and only if
$\exists k^{\prime}$ s.t. $k+a\leq k^{\prime}\leq k+b$,
$\bm{\beta}\vDash_{{k^{\prime}}}\varphi_{2}$, and
$\bm{\beta}\vDash_{{k^{\prime\prime}}}\varphi_{1}$, $\forall k+a\leq
k^{\prime\prime}<k^{\prime}$;

  * •

$\bm{\beta}\vDash_{k}\varphi_{1}\bm{R}_{[a,b]}\varphi_{2}$ if and only if
$\exists k^{\prime}$ s.t. $k+a\leq k^{\prime}\leq k+b$,
$\bm{\beta}\vDash_{{k^{\prime}}}\varphi_{1}$, and
$\bm{\beta}\vDash_{{k^{\prime\prime}}}\varphi_{2}$, $\forall k+a\leq
k^{\prime\prime}\leq k^{\prime}$, or
$\bm{\beta}\vDash_{{k^{\prime}}}\varphi_{2}$, $\forall k+a\leq k^{\prime}\leq
k+b$,

  * •

$\bm{\beta}\vDash\varphi$ if and only if $\bm{\beta}\vDash_{0}\varphi$,

where the temporal operators are indexed by its delay $a\in\mathbb{N}_{\geq
0}$ and deadline $b\in\mathbb{N}_{\geq 0}:a<b\leq\infty$. We can derive other
operators such as _true_ ($\top=\pi^{Q}$), _false_ ($\perp=\pi^{\emptyset}$),
_always_ ($\square_{[a,b]}\varphi=\perp\bm{R}_{[a,b]}\varphi$) and eventually
($\Diamond_{[a,b]}\varphi=\top\bm{U}_{[a,b]}\varphi$).

###  II-D Problem Formulation

A practical problem definition for active perception and control synthesis
from PrSTL specification is a feasibility problem of the form,

find $\displaystyle\bm{\xi}$ (2) s.t. $\displaystyle\bm{\xi}\vDash\varphi,$
$\displaystyle
prob(\bm{X}_{0}=\bar{\bm{x}})\sim\mathcal{N}(\bar{\bm{x}},\bar{\Sigma}^{x}),$
$\displaystyle\bm{X}_{k+1}=A_{q_{k}}\bm{X}_{k}+B_{q_{k}}\bm{u}_{k}+W_{q_{k}}\bm{W}_{k},$
$\displaystyle\bm{Y}_{k}=C_{q_{k}}\bm{X}_{k}+n_{q_{k}}(\bm{x}_{k})\bm{V}_{k},$
$\displaystyle\bm{y}_{k+1}=\operatorname*{arg\,max}_{\bm{y}_{k+1}}prob(\bm{Y}_{k+1}=\bm{y}_{k+1}|\bm{X}_{k},q_{k},\bm{u}_{k}),$
$\displaystyle q_{k}\in
Q,\bm{u}_{k}\in\mathcal{U},\bm{W}_{k}\sim\mathcal{N}(0,I_{n}),\bm{V}_{k}\sim\mathcal{N}(0,I_{p}),$

where $\bm{\xi}$ is a belief trajectory, $\varphi$ is a PrSTL formula,
$prob(\bm{X}_{0}=\bar{\bm{x}})$ is the initial condition (a priori belief),
and
$\operatorname*{arg\,max}_{\bm{y}_{k+1}}prob(\bm{Y}_{k+1}=\bm{y}_{k+1}|\bm{X}_{k},q_{k},\bm{u}_{k})$
is a practical approximation called Maximum Likelihood Observation (MLO) [11,
7].

##  III idSTLPy Overview

Our toolbox implements the approach in [7] illustrated in Fig. 1. The basic
idea is to construct deterministic abstractions (i.e., $\widehat{TS}$ and
$\widetilde{TS}$) and to use counterexample-guided synthesis [12, 13] to
satisfy both the PrSTL specification $\varphi$ and the dynamics of System (1).
Two interacting layers, discrete and continuous, work together to overcome
nonconvexities in the logical specification $\varphi$ efficiently. At the
discrete layer, a discrete planner acts as a proposer, generating discrete
plans by solving a BMC [14, 15] for the given specification (i.e.,
$(\varphi)_{LTL}$):
$\widetilde{TS}\times\breve{TS}_{fair,1}\times\dots\times\breve{TS}_{fair,N}\vDash\bm{E}(\varphi)_{LTL}$.
We use an iterative deepening search to search first for shorter satisfying
plans, thus minimizing undue computation. We pass the satisfying discrete
plans to the continuous layer, which acts as a teacher. In the continuous
layer, a sampling-based search is applied to check whether a discrete plan is
feasible. If the feasibility test does not pass, we construct a counterexample
(i.e., $\breve{TS}_{fair,i}$) to discard infeasible trajectories. Then we add
this counterexample to the discrete planner and repeat this process until we
find a solution or no more satisfying plans exist.

In this approach, we proposed a SPARSE-RRT [16] –a sampling-based motion
planning– variant for active perception. The execution of this method is
defined by a timeout in seconds ($rrt\\_timeout$), a distance to consider that
two states are near ($delta\\_near$), a distance to drain near states
($delta\\_drain$), a goal bias ($goal\\_bias$), a minimum
($min\\_num\\_of\\_steps$) and a maximum ($max\\_num\\_of\\_steps$) number of
steps for each iteration. Intuitively, for each candidate solution, we execute
the proposed RRT for $rrt\\_timeout$ seconds. During the execution, we
randomly sample a state and take an existing trajectory that the last state is
sufficient near ($delta\\_near$) and has less uncertainty (i.e., active
perception). Next, we randomly select a target state with probability
$goal\\_bias$ to be in the goal (i.e., task planning) and synthesize control
inputs for an horizon between $min\\_num\\_of\\_steps$ and
$max\\_num\\_of\\_steps$. If the new trajectory has the last state with less
uncertainty than other near ($delta\\_drain$) trajectories, we remove the
latter from the existing trajectory. If we find at least one trajectory that
satisfies the PrSTL formula, we finish the search. Otherwise, we create a
counter-example and find another candidate solution.

Construct an Approximated System$\text{System
}(\ref{eq:prsystem}),\mathcal{N}(\bar{\bm{x}},\bar{\Sigma}^{x}),\varphi$Abstract
the Approximated System and the Specification$\widehat{TS},\varphi$ Bounded
Model Checking (BMC)
---
$\widetilde{TS}\times\breve{TS}_{fair,1}\times\dots\times\breve{TS}_{fair,N}\vDash\bm{E}(\varphi)_{LTL}$
$\widetilde{TS},(\varphi)_{LTL}$Dynamical Feasibility CheckExample
$\tilde{\bm{\beta}}_{K,L}$satCounter-example $\widetilde{TS}_{cex,i}$infeas a
trajectory that
---
satisfies the specification
No solution Figure 1: Pictorial representation of proposed approach.

##  IV User Guide for idSTLPy

We will illustrate the main idea of the design methods though a simple motion
planning example where the robot position in the workspace is uncertain. In
this scenario, we assume that the robot localization depends on the amount of
light at its current position as shown in Fig. 7. We can model the robot
dynamics in the workspace plane, i.e., $\bm{x}\in\mathbb{R}^{2}$, as a first-
order system controlled by velocity, $\bm{u}\in\mathbb{R}^{2}$:
$\bm{x}_{k+1}=\bm{x}_{k}+0.25\bm{u}_{k}$. The observation function is the
identity $\bm{y}_{k}=\bm{x}_{k}+n(\bm{x}_{k})\bm{v}_{k}$ with a zero-mean
Gaussian noise function as follows:

$n(\bm{x})=0.1(5-x_{1})^{2}+const,$ (3)

where $\bm{x}=[x_{1},x_{2}]^{\intercal}$.

We do not know the robot’s position in the workspace. However, our initial
belief is an isotropic Gaussian distribution centered at position
$[0,2.5]^{\intercal}$ with covariance $diag(0.1,0.1)$.

We can specify the motion planning requirements as an PrSTL formula as
follows:

$\varphi=safe\bm{U}_{[0,240]}\square_{[0,40]}target,$ (4)

where
$safe=\pi_{0.01}^{-x_{1}-1}\wedge\pi_{0.01}^{x_{1}-5}\wedge\pi_{0.01}^{-x_{2}-1}\wedge\pi_{0.01}^{x_{2}-4}$
and
$target=\pi_{0.05}^{-x_{1}-0.25}\wedge\pi_{0.05}^{x_{1}-0.25}\wedge\pi_{0.05}^{-x_{2}-0.25}\wedge\pi_{0.05}^{x_{2}-0.25}$.
In plain English, the robot must satisfy each safety boundary with $99\%$ of
confidence until it achieves the target region with $95\%$ confidence within
$240$ time instants and stays in the target for $40$ time instants.

We can solve this problem using idSTLPy toolbox as shown in LABEL:lst:light-
dark. In the subsequent sections, we will explain this code in more detail.

⬇

1import idstlpy as stl

2import numpy as np

3

4q = stl.mk_variable(size=1, dtype=int)

5x = stl.mk_variable(size=2, dtype=float)

6u = stl.mk_variable(size=2, dtype=float)

7problem = stl.Problem(

8 switched_system=stl.mk_switched_sys([

9 stl.dynamical_system.mk_lbs(

10 A=np.identity(x.size), B=0.25 * np.eye(x.size, u.size), W=np.zeros((2, 2)),

11 C=np.identity(2), V=lambda state: ((1 / 10) * (5 - state[0]) ** 2 + 0.001)
* np.identity(2))

12 ]),

13 control_domain=stl.logical_and(u[0] >= -1.0, u[0] <= 1.0, u[1] >= -1.0,
u[1] <= 1.0).region,

14 initial_state=stl.to_belief(mean=np.array([0, 2.5]), cov=np.diag([0.1,
0.1])),

15 stl_formula=stl.until(

16 stl.logical_and(q == 0, stl.prob(x[0] >= -1) >= 1 - 0.01, stl.prob(x[0] <=
5) >= 1 - 0.01,

17 stl.prob(x[1] >= -1) >= 1 - 0.01, stl.prob(x[1] <= 4) >= 1 - 0.01,
name=’free_space’),

18 0, 240,

19 stl.always(0, 40,

20 stl.logical_and(q == 0, stl.prob(x[0] >= -0.25) >= 1 - 0.05, stl.prob(x[0]
<= 0.25) >= 1 - 0.05,

21 stl.prob(x[1] >= -0.25) >= 1 - 0.05, stl.prob(x[1] <= 0.25) >= 1 - 0.05,

22 name=’target’)))

23)

24solution = problem.solve(rrt_timeout=60, delta_near=2, delta_drain=0.5,
goal_bias=0.25,

25 min_num_of_steps=3, max_num_of_steps=15)

26

27for i in range(problem.switched_system.system_modes.size):

28
problem.switched_system.system_modes[i].compute_finite_horizon_lqr(horizon=5,
Q_final=np.identity(2), Q=np.identity(2), R=0.05*np.identity(2))

29xi_sim, xi_real = problem.switched_system.simulate(

30 reference_trajectory=solution, num_of_steps=solution.num_of_steps,
real_initial_state=np.array([0.5, 2.75]),

31 real_system=stl.mk_switched_sys([stl.mk_lcs(A=np.identity(2), B=0.25 *
np.identity(2))])

32)

Listing 1: Motion Planning Example

###  IV-A Systems

A system (a.k.a., switched system) is composed of a finite set of system
modes. Each system mode is also a dynamical system that inherit the behavior
of a linear control system (LCS, i.e., $\bm{x}_{k+1}=A\bm{x}_{k}+B\bm{u}_{k}$)
as illustrated in Fig. 2. We can have three types of dynamical behavior. If
the output dimension is zero, i.e., $p=0$, the system mode is a linear belief
system (LBS). In this behavior, there is no active perception because we
assume no observation. Otherwise, if the output dimension is non-zero (i.e.,
$p>0$), we have a partially observable linear belief system (POLBS). In turn,
a POLBS can have a linear noise function (POLBSWithLinNoise, i.e.,
$n(\bm{x})=V$, where $V\in\mathbb{R}^{p\times p}$ is a constant matrix) or a
nonlinear noise function (POLBSWithNonLinNoise).

We can create any one of the linear belief systems using the function
$mk\\_lbs(A,B,W,C=None,V=None)$, where $C$ and $V$ are optional parameters,
and $V$ can be a constant matrix or a function that returns a matrix.
Similarly, we can construct a switched system using the function
$sys.mk\\_switched\\_sys(system\\_modes)$, where $system\\_modes$ is a list of
linear belief systems. We declare a dynamical system in our example in
LABEL:lst:light-dark lines 8-12.

$DynamicalSystem$$LCS$$LBS$$POLBS$$POLBSWithLinNoise$$POLBSWithNonLinNoise$$SwitchedSystem$
Figure 2: Class Diagram representation of dynamical system types and their
inheritance.

###  IV-B Variables

Due the hybrid nature of switched systems, we have two variable types: real-
valued and discrete. As illustrated in Fig. 3, a real-valued variable is also
a linear expression. A linear expression over a variable
$\bm{x}\in\mathbb{R}^{n}$ is a multiplication between a constant vector
$\bm{h}\in\mathbb{R}^{n}$ and the variable plus a constant $c\in\mathbb{R}$:
$\bm{h}^{\intercal}\bm{x}+c$. For example, variable $\bm{x}\in\mathbb{R}^{2}$
is also a vector of linear expressions:
$\big{(}(1,0)^{\intercal}\bm{x}+0.0,(0,1)^{\intercal}\bm{x}+0.0\big{)}$. A
discrete variable is a variable which can assume a finite set of values. For
example, in LABEL:lst:light-dark, $q$ is a discrete variable that have one
valid value (i.e., $q\in\\{0\\}$).

We can declare a variable by the function $mk\\_variable(size,dtype)$, where
$dtype$ is one of of these two types: $float$ for real-valued variables, $int$
for discrete variables. If $dtype$ is $float$, $size$ is the variable
dimension. If $dtype$ is $int$, $size$ is the cardinality of the variable set
of values. We declare variables in our example in LABEL:lst:light-dark lines
4-6.

$LinearExpression$$RealValuedVariable$$DiscreteVariable$ Figure 3: Class
Diagram representation of variable types and their inheritance.

###  IV-C Constraints

An PrSTL formula atom is a constraint over the discrete and real-valued
variables. We illustrate the inheritance of different constraint objects in
Fig. 4. We call a constraint defined as an equality over discrete variables
$q=\alpha$ as discrete predicate (DiscretePredicate implements
$\pi^{\\{\alpha\\}}$), where $\alpha\in\mathbb{N}$. On the other hand, if the
constraint is defined over a real-valued variable, this constraint is convex
(ConvexConstraint). Particularly, a linear inequality over the variable (i.e.,
$\bm{h}^{\intercal}\bm{x}+c\leq 0$) is a linear predicate (LinearPredicate).
Similarly, a inequality over the probability of a linear predicate (i.e.,
$prob(\bm{h}^{\intercal}\bm{x}+c\leq 0)>=1-\epsilon$) is a probabilistic
linear predicate (ProbabilisticLinearPredicate implements
$\pi^{\bm{h}^{\intercal}\bm{x}+c}_{\epsilon}$).

$ConvexConstraint$$LinearPredicate$$ProbabilisticLinearPredicate$$DiscretePredicate$$ConvexRegion$
Figure 4: Class Diagram representation of variable types and their
inheritance.

We can apply the Boolean conjunction operator $\wedge$ over linear and
probabilistic linear predicates (i.e., the function
$logical\\_and(*args)$111In Python language, $*args$ means a variable number
of arguments.). The resulting constraint is a convex region over the state (if
arguments are LinearPredicates) or belief state space (if arguments are
ProbabilisticLinearPredicate). A convex region has a property that defines a
polytope or a belief cone as illustrated in Fig. 5. We declare a convex region
and take its region that defines the input domain (a polytope) in our example
in LABEL:lst:light-dark line 13.

$Polytope$$FullDimensionalPolytope$$BeliefCone$$FullDimensionalBeliefCone$
Figure 5: Class Diagram representation of polytope types and their
inheritance.

####  IV-C1 Polytopes

A _polytope_ $\mathcal{X}\subseteq\mathbb{R}^{n}$ is a set in $\mathbb{R}^{n}$
defined by the intersection of a finite number of closed half-spaces, i.e,
$\mathcal{X}:=\cap_{i}\\{\mu_{i}(\bm{x})\leq 0\\}$, where $\mu_{i}\in
H(\mathcal{P})$ is a linear function
$\mu_{i}:\mathbb{R}^{n}\rightarrow\mathbb{R}$ such that
$\mu_{i}(\bm{x}):=\bm{h}_{i}^{\intercal}\bm{x}+c_{i}$, $H(\mathcal{P})$ is the
set of linear functions that defines the polytope $\mathcal{P}$,
$\bm{h}_{i}\in\mathbb{R}^{n}$ and $c_{i}\in\mathbb{R}$ are constants. We can
also represent a compact polytope $\mathcal{X}\subset\mathbb{R}^{n}$ as the
convex hull of its vertices, i.e.,
$\mathcal{X}=\text{conv}\big{(}V(\mathcal{X})\big{)}$, where $V(\mathcal{X})$
is a set of vertices.

####  IV-C2 Belief Cone

We call the reciprocal of polytopes in the belief state space as belief cones.
These cones $\mathcal{B}\subseteq\mathbb{R}^{n(n+1)}$ are intersection of a
finite set of second order cones about the multivariate Gaussian distribution
parameters (i.e., mean $\hat{\bm{x}}\in\mathbb{R}^{n}$ and covariance
$\Sigma^{x}\in\mathbb{R}^{n\times n}$) that satisfies a conjunction of a
finite set of probabilistic linear predicates (i.e,
$\bigwedge_{i}prob(\mu_{i}(\bm{x})\leq 0)\geq 1-\epsilon_{i}$). For
simplicity, we will denote that a Gaussian random variable
$\bm{X}_{k}\sim\mathcal{N}(\hat{x}_{k},\Sigma_{k}^{x})$ satisfy a
probabilistic linear predicate $prob(\mu_{i}(\bm{x})\leq 0)\geq
1-\epsilon_{i}$ by $\bm{X}_{k}\vDash prob(\mu_{i}(\bm{x})\leq 0)\geq
1-\epsilon_{i}$. Therefore, a belief cone is defined as:

$\displaystyle\mathcal{B}:=$
$\displaystyle\\{\bm{b}\in\mathbb{R}^{n(n+1)}:\bigwedge_{i}\bm{X}\vDash
prob(\bm{h}_{i}^{\intercal}\hat{\bm{x}}_{k}+c_{i}\leq 0)\geq
1-\epsilon_{i}\\}$ (5) $\displaystyle:=$
$\displaystyle\cap_{i}\\{\bm{h}_{i}^{\intercal}\hat{\bm{x}}_{k}+c_{i}+\Phi^{-1}(1-\epsilon_{i})\sqrt{\bm{h}_{i}^{\intercal}\Sigma_{k}^{x}\bm{h}_{i}}\leq
0\\},$

where $\bm{b}\in\mathbb{R}^{n(n+1)}$ is the Gaussian distribution parameter
variable, $\bm{h}_{i}^{\intercal}\in\mathbb{R}^{n}$, $c_{i}\in\mathbb{R}$ and
$\epsilon_{i}\in[0,0.5]$ are constants, $\Phi(v)$ and $\Phi^{-1}(p)$ are the
cumulative distribution and quantile functions of the standard Gaussian
distribution $V\sim\mathcal{N}(0,1)$, i.e., $\Phi(v)=prob(V\leq v)$ and
$\Phi^{-1}(p)\leq v$ if and only if $p\leq\Phi(v)$. We can easily see that
$\bm{X}\vDash prob(\bm{h}_{i}^{\intercal}\hat{\bm{x}}_{k}+c_{i}\leq 0)\geq
1-\epsilon_{i}$ if and only if
$\bm{h}_{i}^{\intercal}\hat{\bm{x}}_{k}+c_{i}+\Phi^{-1}(1-\epsilon_{i})\sqrt{\bm{h}_{i}^{\intercal}\Sigma_{k}^{x}\bm{h}_{i}}\leq
0$ from Gaussian distribution properties such as linear transformation and the
quantile function definition.

###  IV-D PrSTL formula

We implement a PrSTL formula as classes shown in Fig. 6. An
STLAtomicProposition implements a PrSTL state formula $\phi$, meaning that it
represents the conjunction (i.e., $logical\\_and(*args)$) over a list of
ProbabilisticLinearPredicate and Boolean formula (i.e., using
$logical\\_and(*args)$ or $logical\\_or(*args)$) over DiscretePredicate,
meaning that it is defined by a convex region and a set of valid system modes.
In plain English, a trajectory $\bm{\xi}$ satisfies an STLAtomicProposition at
instant $k$ if it reaches a belief state in the convex region using one of the
valid system modes. We declare two STLAtomicProposition in our example in
LABEL:lst:light-dark lines 16-17 and lines 20-21.

An STLAnd object represent the conjunction (i.e., $logical\\_and(*args)$) of a
list of STLFormulas containing at most one STLAtomicProposition. On the other
hand, an STLOr object is a disjunction (i.e., $logical\\_or(*args)$) of a list
STLFormulas but containing an arbitrary number of STLAtomicProposition. An
STLUntil object is an PrSTL formula with until operator
$\varphi_{1}\bm{U}_{[a,b]}\varphi_{2}$, and STLRelease object is an PrSTL
formula with until operator $\varphi_{1}\bm{R}_{[a,b]}\varphi_{2}$. We obtain
these formulas using the functions: $until(\varphi_{1},a,b,\varphi_{2})$,
$eventually(a,b,\varphi)$, $release(\varphi_{1},a,b,\varphi_{2})$,
$always(a,b,\varphi)$. We declare a formula in our example in LABEL:lst:light-
dark lines 15-22.

$STLFormula$$STLTemporalFormula$$STLAtomicProposition$$STLAnd$$STLOr$$STLUntil$$STLRelease$
Figure 6: Class Diagram representation of PrSTL formula types and their
inheritance.

###  IV-E Solution

The object Problem wraps the implementation of the approach presented in
Section III. A solution could be empty ($None$ in Python) if the algorithm did
not find a trajectory that satisfies the specification. However, if such a
trajectory is found, the algorithm returns an object that implements this
trajectory. Since the solution is a trajectory of the approximated transition
system [7], the returned trajectory is a ProbabilisticTSTrajectory. This
object implements methods to extract data from the trajectory such as:
$get\\_mean$, $get\\_cov$, $get\\_control$, $get\\_action$ that returns the
belief state mean, the belief state covariance, the control, and the action
from the trajectory. We can use these methods to execute a trajectory tracking
strategy. We declare a problem in our example in LABEL:lst:light-dark lines
7-23 and solve it in LABEL:lst:light-dark lines 24-25.

We can simulate the execution of this planned trajectory. The planned
trajectory is the result of an approximated belief dynamics where the
observations are the MLO. Hence, we propose to use a linear feedback law to
adjust the belief state during the execution to track the planned trajectory.
Specifically, we implemented a receding horizon control (RHC) strategy with a
finite horizon discrete time Linear Quadratic Regulator (LQR) to track the
belief mean values. We initialize this strategy by calling the method
$compute\\_finite\\_horizon\\_lqr(horizon,Q\\_final,Q,R)$ for each system
mode, where $Q_{final},Q\in\mathbb{R}^{n\times n}$ are positive-semi-definite
contrant matrices and $R\in\mathbb{R}^{m\times m}$ is a positive definite
matrix. Next, we call the method $simulate$ from SwitchedSystem object. This
method simulates the system $real\\_system$ initialized at
$real\\_initial\\_state$ for $num\\_of\\_steps$ steps while using the linear
feedback law to track the planned trajectory $reference\\_trajectory$. In the
running example, we track the mean of the estimated belief with cost function
$J=\hat{\bm{x}}_{h}^{\intercal}Q_{final}\hat{\bm{x}}_{h}+\sum_{k=0}^{h-1}\hat{\bm{x}}_{k}^{\intercal}Q\hat{\bm{x}}_{k}+\bm{u}_{k}^{\intercal}R\bm{u}_{k}$,
where $Q=I_{2}$ and $R=0.05I_{2}$ and the horizon $h=5$, as shown in
LABEL:lst:light-dark lines 27-32.

The result is shown in Fig. 7. The blue trajectory is the planned trajectory
in the belief space. This trajectory approximates the observation as maximum
likelihoods. However, we observe a very different observation during the
execution, which is the purple trajectory in the figure. As a result, the
belief trajectory during the execution is slightly different, the orange
trajectory in the figure. However, this trajectory satisfies the
specification, and the resulting state (in red) also is within the expected
result. Since the maximum likelihood approximation is close to the most likely
belief trajectory, a simple tracking strategy is, in general, sufficient to
enforce the planned trajectory during execution.

Figure 7: Light-dark example. The shade in the workspace represents the amount
of light at that position and the black box is the target.

##  V Conclusions and future work

In this work, we presented a Python toolbox for controller synthesis from
PrSTL specifications. We considered problems with a switched linear system
with Gaussian noises. We illustrated our approach on a simulation of robot
motion planning under noisy localization. In this example, we showed that the
planned trajectory satisfied both the task and active perception requirements.

We will focus on two directions in future work. One direction is to extend to
other probabilistic hybrid systems and also consider probabilistic switching.
Another direction is to drop the MLO approximation during the planning without
a conservative assumption.

## References

  * [1] R. Bajcsy, “Active perception,” _Proceedings of the IEEE_ , vol. 76, no. 8, pp. 966–1005, 1988.
  * [2] D. Sadigh and A. Kapoor, “Safe Control under Uncertainty with Probabilistic Signal Temporal Logic,” in _Proceedings of Robotics: Science and Systems_ , AnnArbor, Michigan, jun 2016.
  * [3] K. Zhong, P. Jain, and A. Kapoor, “Fast second-order cone programming for safe mission planning,” in _2017 IEEE International Conference on Robotics and Automation (ICRA)_ , may 2017, pp. 79–86.
  * [4] D. Dey, D. Sadigh, and A. Kapoor, “Fast Safe Mission Plans for Autonomous Vehicles,” 2016.
  * [5] C. Yoo and C. Belta, “Control with probabilistic signal temporal logic,” _arXiv preprint arXiv:1510.08474_ , 2015.
  * [6] R. R. da Silva, V. Kurtz, and H. Lin, “Active perception and control from temporal logic specifications,” _IEEE Control Systems Letters_ , vol. 3, no. 4, pp. 1068–1073, 2019.
  * [7] ——, “Active perception and control from prstl specifications,” 2021 (to appear in arXiv).
  * [8] I. Filippidis, S. Dathathri, S. C. Livingston, N. Ozay, and R. M. Murray, “Control design for hybrid systems with tulip: The temporal logic planning toolbox,” in _2016 IEEE Conference on Control Applications (CCA)_ , 2016, pp. 1030–1041.
  * [9] C. Finucane, G. Jing, and H. Kress-Gazit, “Ltlmop: Experimenting with language, temporal logic and robot control,” in _2010 IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2010, pp. 1988–1993.
  * [10] K. He, M. Lahijanian, L. E. Kavraki, and M. Y. Vardi, “Towards manipulation planning with temporal logic specifications,” in _Robotics and Automation (ICRA), 2015 IEEE International Conference on_. IEEE, 2015, pp. 346–352.
  * [11] R. Platt Jr, R. Tedrake, L. Kaelbling, and T. Lozano-Perez, “Belief space planning assuming maximum likelihood observations,” 2010.
  * [12] R. Alur, R. Bodik, G. Juniwal, M. M. K. Martin, M. Raghothaman, S. A. Seshia, R. Singh, A. Solar-Lezama, E. Torlak, and A. Udupa, “Syntax-guided synthesis,” in _Formal Methods in Computer-Aided Design (FMCAD), 2013_. IEEE, 2013, pp. 1–8.
  * [13] A. Reynolds, M. Deters, V. Kuncak, C. Tinelli, and C. Barrett, “Counterexample-guided quantifier instantiation for synthesis in SMT,” in _International Conference on Computer Aided Verification_. Springer, 2015, pp. 198–216.
  * [14] A. Biere, K. Heljanko, T. Junttila, T. Latvala, and V. Schuppan, “Linear encodings of bounded ltl model checking,” _arXiv preprint cs/0611029_ , 2006\.
  * [15] R. R. da Silva, V. Kurtz, and H. Lin, “Automatic trajectory synthesis for real-time temporal logic,” _IEEE Transactions on Automatic Control_ , 2021\.
  * [16] Z. Littlefield, Y. Li, and K. E. Bekris, “Efficient sampling-based motion planning with asymptotic near-optimality guarantees for systems with dynamics,” in _Intelligent Robots and Systems (IROS), 2013 IEEE/RSJ International Conference on_. IEEE, 2013, pp. 1779–1785.