# Lesson E: Hello tubes¶

Mobile robot motion is usually described by an evolution function \(\mathbf{f}\) involved in a **differential equation**. The interval state estimation then amounts to estimating the set of **feasible trajectories** that are solutions of this differential equation. The difficulty is to propagate the uncertainties in time, and to consider observations from asynchronous measurements in the same time. We will see that these difficulties are easily handled with constraint propagations on **tubes**.

Content of this lesson

## Formalism¶

We now address the following problem:

We recall that

\(\mathbf{x}\in\mathbb{R}^n\) is the state vector

\(\mathbf{u}\in\mathbb{R}^m\) is some input vector

\(\mathbf{y}\in\mathbb{R}^p\) is a measurement

and \(\mathbf{f}\), \(\mathbf{g}\) are respectively

*evolution*and*observation*functions, possibly non-linear or uncertain

The \(\mathbf{x}\), \(\mathbf{u}\) and \(\mathbf{y}\) values continuously evolve with time. We will now handle them as **trajectories**.

What is a trajectory

A trajectory \(x(\cdot):[t_0,t_f]\to\mathbb{R}\) is a set of real values defined over some temporal domain \([t_0,t_f]\), called *t*-domain.
They are aimed at **representing temporal evolutions**. We implement them with the `Trajectory`

class (or the `TrajectoryVector`

class for trajectories \(\mathbf{x}(\cdot):[t_0,t_f]\to\mathbb{R}^n\)).

These trajectories correspond to a new kind of variable in the constraint programming approach. We will consider them together with other values such as reals of vectors, as we did in the previous lessons.

New type of variables \(\implies\) new type of domain

Because we will deal with a new kind of variable (trajectories \(x(\cdot)\)), we also define a new type of domain to handle them with contractors: **tubes** \([x](\cdot)\).

A tube \([x](\cdot)\) is defined over a temporal *t*-domain \([t_0,t_f]\) as an envelope of trajectories that are defined over the same *t*-domain. We speak about an *envelope* as it may exist trajectories enclosed in the tube that are not solutions of our problem. A trajectory \(x(\cdot)\) belongs to the tube \(\left[x\right](\cdot)\) if \(\forall t\in[t_0,t_f], x\left(t\right)\in\left[x\right]\left(t\right)\).

Note

**Important:** we assume that all the tubes and/or the trajectories involved in a given resolution process share the same *t*-domain \([t_0,t_f]\).

The `Tube`

class allows to create a tube. See more on: Tubes: sets of trajectories.

## Using trajectories¶

Let us consider a robot following a Lissajous curve from \(t_0=0\) to \(t_f=5\). Its exact trajectory is given by:

The following code creates the actual (but unknown) trajectory \(\mathbf{x}^*(\cdot)\):

```
dt = 0.01
tdomain = Interval(0,5) # temporal domain [t0,tf]
actual_x = TrajectoryVector(tdomain, TFunction("(2*cos(t) ; sin(2*t))"), dt)
```

```
double dt = 0.01;
Interval tdomain(0,5); // temporal domain [t0,tf]
TrajectoryVector actual_x(tdomain, TFunction("(2*cos(t) ; sin(2*t))"), dt);
```

`TFunction`

object depicts a temporal function. It is similar to the `Function`

objects we have seen previously, but the `t`

variable is already defined. In this example, the output of the function is two-dimensional (defined with parenthesis).`dt`

converts the analytical function into a discretized signal with \(\delta=0.01\).Exercise

**E.1.** In a new project, create the above trajectory \(\mathbf{x}^*(\cdot)\), named `actual_x`

.

**E.2.** Create a landmark \(\mathbf{b}=(0.5,1)\).

**E.3.** In a `VIBesFigMap`

figure, project the 2d trajectory in the view with the `.add_trajectory()`

method.
The arguments respectively refer to the object to display, its name in the view, the x-index component and the y-index component:

```
fig_map = VIBesFigMap("Map")
fig_map.set_properties(100, 100, 600, 300)
fig_map.add_trajectory(actual_x, "x*", 0, 1)
fig_map.add_beacon(b, 0.1) # 0.1: landmark width
fig_map.axis_limits(-2.5,2.5,-0.1,0.1, True)
fig_map.show(0.5) # argument is robot size
```

```
VIBesFigMap fig_map("Map");
fig_map.set_properties(100, 100, 600, 300);
fig_map.add_trajectory(&actual_x, "x*", 0, 1);
fig_map.add_beacon(b, 0.1); // 0.1: landmark width
fig_map.axis_limits(-2.5,2.5,-0.1,0.1, true);
fig_map.show(0.5); // argument is robot size
```

**E.4.** The robot continuously measures its distance to the landmark \(\mathbf{b}\).

Classical mathematical functions such as \(+\), \(\cos\), \(\exp\), \(\sqrt{\cdot}\), can be used on trajectories.
Compute the distance signal `actual_y`

between the robot and the landmark as one `Trajectory`

object.

**E.5.** Display the `actual_y`

trajectory in another dedicated view with a `VIBesFigTube`

object:

```
fig_dist = VIBesFigTube("Distance to the landmark")
fig_dist.set_properties(100, 100, 600, 300)
fig_dist.add_trajectory(actual_y, "y*")
fig_dist.show()
```

```
VIBesFigTube fig_dist("Distance to the landmark");
fig_dist.set_properties(100, 100, 600, 300);
fig_dist.add_trajectory(&actual_y, "y*");
fig_dist.show();
```

You should obtain this figure:

## Enclosing the feasible trajectories of a robot¶

The state estimation will be done by enclosing the feasible trajectories in a tube submitted to contractors.

*t*-domain \([t_0,t_f]\). A parameter \(\delta\) allows to numerically represent a tube as a set of temporal slices (we will use \(\delta\) =

`dt`

= 0.01).```
dt = 0.01
tdomain = Interval(0,5)
a = Tube(tdomain, dt, TFunction("cos(t*2)"))
```

```
dt = 0.01;
Interval tdomain(0,5);
Tube a(tdomain, dt, TFunction("cos(t*2)"));
```

The same function can be used to express uncertainties with respect to time. For instance:

```
b = Tube(tdomain, dt, TFunction("cos(t*2)+abs(t*2-5)*[-0.1,0.1]"))
```

```
Tube b(tdomain, dt, TFunction("cos(t*2)+abs(t*2-5)*[-0.1,0.1]"));
```

In the following figure, the tubes \([a](\cdot)\) and \([b](\cdot)\) are respectively drawn in dark gray and light gray.

Tubes can also be built from trajectories. In this example, we could have defined \([a](\cdot)\) by:

```
actual_a = Trajectory(tdomain, TFunction("cos(t*2)"))
a = Tube(actual_a, dt) # [a](·) will wrap the trajectory
```

```
Trajectory actual_a(tdomain, TFunction("cos(t*2)"));
Tube a(actual_a, dt); // [a](·) will wrap the trajectory
```

Exercise

**E.6.** Using the class `TubeVector`

, create a 2d tube \([\mathbf{x}](\cdot)\) enclosing \(\mathbf{x}^*(\cdot)\). You may refer to the manual for more information on `TubeVector`

.

**E.7.** Using the `.inflate()`

method on `x`

, inflate the tube so that \(\forall t,[\mathbf{x}](t)=\mathbf{x}^*(t)+[-0.2,0.2]\).

**E.8.** Display the tube in the previous figure with:

```
fig_map.add_tube(x, "x", 0, 1)
fig_map.show() # this method must be called to display the added objects
```

```
fig_map.add_tube(&x, "x", 0, 1);
fig_map.show(); // this method must be called to display the added objects
```

Is the actual trajectory \(\mathbf{x}^*(\cdot)\) enclosed in \([\mathbf{x}](\cdot)\) at any time?

**E.9.** Create a tube \([y](\cdot)\) for enclosing the actual trajectory \(y^{*}(\cdot)\) of distances between the robot and the landmark. For now, we will not consider uncertainties on \(y^{*}(\cdot)\). Therefore, the tube \([y](\cdot)\) should enclose \(y^{*}(\cdot)\) in a minimal way according to the discretization step `dt`

.
Note that all the tubes of this lesson have to share the same `tdomain`

and `dt`

parameters.

## Involving tubes in a Contractor Network¶

We can now perform a localization based on range-only measurements. We will use again the distance contractor \(\mathcal{C}_{\textrm{dist}}\) developed in Lesson B: Static range-only localization but now in a dynamical context.

The contractors that act on boxes can be used on tubes without loss of generality and in a very transparent way. They will contract the tubes, slice by slice for each time in \([t_0,t_f]\).

For instance, one can contract three tubes \([a](\cdot)\), \([b](\cdot)\), \([c](\cdot)\), in order to be consistent with the \(\mathcal{L}_{+}\) constraint:

```
# a = Tube(... (creating tubes)
# b = Tube(...
# c = Tube(...
# The constraint "x1+x2=x3" is equivalent to "x1+x2-x3=0":
ctc_add = CtcFunction(Function("x1", "x2", "x3", "x1+x2-x3"), Interval(0))
cn = ContractorNetwork()
cn.add(ctc_add, [a,b,c]) # the tubes are listed according to the constraint definition
cn.contract()
```

```
// a = Tube(... (creating tubes)
// b = Tube(...
// c = Tube(...
// The constraint "x1+x2=x3" is equivalent to "x1+x2-x3=0":
CtcFunction ctc_add(Function("x1", "x2", "x3", "x1+x2-x3"), Interval(0));
ContractorNetwork cn;
cn.add(ctc_add, {a,b,c}); // the tubes are listed according to the constraint definition
cn.contract();
```

Exercise

We recall the problem:

**E.10.** We first focus on the observation equation \(\mathbf{y}(t)=\mathbf{g}\big(\mathbf{x}(t)\big)\). Build a contractor network and contract the tube \([\mathbf{x}](\cdot)\) with the distance contractor, that expresses \(\mathbf{g}\). Note that this contractor is already defined in the library. You developed your own version as an exercise in sec-tuto-01, but you can also use:

```
ctc.dist # object already created, as for ctc.polar
```

```
ctc::dist // object already created, as for ctc::polar
```

You should obtain something like this:

As one can see, the contraction is reliable: the actual trajectory (in dark blue) is kept in the tube. However, the contraction is not efficient. We need to also consider the differential equation \(\dot{\mathbf{x}}(t)=\mathbf{f}\big(\mathbf{x}(t),\mathbf{u}(t)\big)\).

## Dealing with a differential equation¶

The equation \(\dot{\mathbf{x}}(t)=\mathbf{f}\big(\mathbf{x}(t),\mathbf{u}(t)\big)\) depicts the evolution of the robot. In this lesson, we assume that its actual speed \(\mathbf{v}^*(\cdot)\) is given in the absolute reference frame by:

Note that there is no difficulty to handle datasets instead of analytical functions: once the tube is defined (from functions or datasets), then the constraints will act on its bounds, in the same manner.

Then, the evolution equation amounts to \(\dot{\mathbf{x}}(t)=\mathbf{v}(t)\).

Exercise

**E.11.** Enclose \(\mathbf{v}^*(\cdot)\) in a tube \([\mathbf{v}]=\mathbf{v}^*(\cdot)+[-0.01,0.01]\).

**E.12.**A contractor exists to deal with the differential constraint:

It takes as input a tube \([\mathbf{x}](\cdot)\) and a tube containing the set of feasible derivatives: \([\mathbf{v}](\cdot)\). This will *smooth* \([\mathbf{x}](\cdot)\).

This contractor is also predefined in the library:

```
ctc.deriv # object already created, as for ctc.polar
```

```
ctc::deriv // object already created, as for ctc::polar
```

## (optional) Adding noise on the measurements¶

The `RandTrajectory`

class allows to create a trajectory with random values.

```
# Random values in [-0.1,0.1] at each dt=0.01
n = RandTrajectory(tdomain, 0.01, Interval(-0.1,0.1))
```

```
// Random values in [-0.1,0.1] at each dt=0.01
RandTrajectory n(tdomain, 0.01, Interval(-0.1,0.1));
```

Exercise

**E.13. (optional)** Add a noise, bounded by \([-0.05,0.05]\), to the distance trajectory:

**E.14. (optional)** What should be the uncertainties on \([y](\cdot)\) in order to reliably enclose the actual values despite the noise?

**E.15. (optional)** Compute the state estimation taking into account these errors.