# Lesson F: Localization with asynchronous measurements¶

This lesson is an application of the previous one. We will only introduce a new contractor for dealing with **evaluation constraints**, and try to localize a robot with the help of range-only measurements obtained at discrete times.

Content of this lesson

## Formalism¶

We are now addressing the following problem:

The difference with the previous lesson is that the measurements are not obtained continuously in time anymore. In the above equation, the observation vector \(\mathbf{y}_i\) is related to the time \(t_i\).

This observation equation, classically encountered in robotics, involves the so-called **evaluation constraint** (variable names are arbitrary):

As one can see, this constraint involves heterogeneous variables: a real \(t_i\) corresponding to the time of the evaluation, a real (or vector) \(y_i\) associated with the value of the evaluation, and two (vector) trajectories \(z(\cdot)\) and \(w(\cdot)\) corresponding respectively to the evaluated trajectory and its derivative.

Note

The presence of the derivative in this constraint is due to the variable \(t_i\) that may be uncertain: \(t_i\in[t_i]\). In this case, the derivative \(w(\cdot)\) is required in order to depict the evolution of \(z(\cdot)\) along the time interval \([t_i]\).

Tubex provides a contractor \(\mathcal{C}_{\textrm{eval}}\big([t_i],[y_i],[z](\cdot),[w](\cdot)\big)\) to deal with this constraint on intervals and tubes.

## Contract a tube at a given time¶

Before solving a complete range-only localization problem, let us come back to the example of the previous lesson.

Exercise

The robot follows a Lissajous curve, but we only know that its velocities (in the absolute reference frame) are known to belong to:

**F.1.** Enclose the velocities in a 2d tube. Create another 2d tube \([\mathbf{x}](\cdot)\) with same sampling \(\delta\) but without initialization. This means that \(\forall t,[\mathbf{x}](t)=[-\infty,\infty]^2\).

**F.2.** Create a Contractor Network for dealing with:

We will assume that a bounded measurement \([\mathbf{y}_i]=([-0.84,-0.83],[-0.76,-0.75])^\intercal\) is made at time \(t_i=2\).

Note that the \(\mathcal{C}_{\textrm{eval}}\) contractor is also predefined in the library. You may use the following object:

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

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

You should obtain a figure similar to this one:

What happened?

An evaluation constraint has narrowed the tube at \(t=2\): the contraction keeps the trajectories going through the box \([\mathbf{y}_i]\). This effect is propagated, backward and forward in time, from \(t=2\): the whole tube has been contracted, without other knowledge on \([\mathbf{x}](\cdot)\).

## Dynamic range-only localization¶

We now have all the material to deal with a complete problem.

Let us consider a localization with range-only measurements. A robot is described by the state vector \(\mathbf{x}=\{x_1,x_2,\psi,\vartheta\}^\intercal\) depicting its position, its heading and its speed. It evolves between three landmarks \(\mathbf{b}_1\), \(\mathbf{b}_2\), \(\mathbf{b}_3\) and measures distances \(y_i\) from these points. We assume that the position and the identity of the landmarks are known.

The problem is defined by:

where \(\mathbf{u}(t)\) is the input of the system. \(g\) is the distance function, and \(\mathbf{f}\) is defined by

The actual (but unknown) state trajectory \(\mathbf{x}^*(\cdot)\) is expressed by:

The input \(\mathbf{u}(\cdot)\) is unknown, but we assume that we have continuous measurements for the headings \(\psi(\cdot)\) and the speeds \(\vartheta(\cdot)\), with some bounded uncertainties defined by intervals \([e_\psi]=[-0.01,0.01]\), \([e_\vartheta]=[-0.01,0.01]\).

Finally, we consider three range-only observations \((t_i,y_i)\). They are summarized in the following table, with positions of the landmarks.

Time \(t_i\) |
Distance \(y_i\) |
Landmark \(\mathbf{b}_i\) |
---|---|---|

\(0.3\) |
\(1.9\) |
\((8,3)^\intercal\) |

\(1.5\) |
\(3.6\) |
\((0,5)^\intercal\) |

\(2.0\) |
\(2.8\) |
\((-2,1)^\intercal\) |

The measurements \(y_i\) are bounded by the interval \([e_y]=[-0.1,0.1]\).

Exercise

**F.3.** First, the problem requires a decomposition into elementary constraints. This will make appear intermediate variables.

**F.4.**Define the domains for all the variables involved in the problem.

**F.5.** In a figure, display the three landmarks and the related range-only bounded measurements.

**F.6.** Create the contractors related to the state equations.

**F.7.** Solve the problem using a Contractor Network.

You should obtain this figure:

Important

Using a constraint programming approach, you have been able to solve a non-linear state-estimation **without knowledge about initial condition**. This difficulty is hardly managed by classical methods dealing with state estimation, that need some initial vector to start correctly. Here, we only convert the equations and measurements into constraints and let the contractors reduce the feasible sets.

In addition, we did not perform **linearizations**: all the computations are guaranteed without approximations. This is useful for making proofs or for **providing safety** on complex systems.

## Submit your exercise¶

Next (and last) lessons will merge all the concepts that have been seen so far, in order to solve :

a complete problem of data association with tubes

a range-only SLAM