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.

Formalism

We are now addressing the following problem:

\[\begin{split}\left\{ \begin{array}{lll} \dot{\mathbf{x}}(t)=\mathbf{f}\big(\mathbf{x}(t),\mathbf{u}(t)\big) & & \textrm{(evolution equation)}\\ \mathbf{y}_i=\mathbf{g}\big(\mathbf{x}(t_i)\big) & & \textrm{(observation equation)} \end{array}\right.\end{split}\]

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`in[t_0,t_f]\).

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

\[\begin{split}\mathcal{L}_{\textrm{eval}}~:~\left\{~\begin{array}{r}y_i=z(t_i)\\\dot{z}(\cdot)=w(\cdot)\end{array}\right.\end{split}\]

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]\).

Codac 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:

\[\begin{split}\mathbf{v}^*(t) \in \left(\begin{array}{c}-2\sin(t)\\2\cos(2t)\end{array}\right)+[-0.03,0.03]\end{split}\]

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:

\[\begin{split}\left\{ \begin{array}{l} \dot{\mathbf{x}}(\cdot)=\mathbf{v}(\cdot)\\ \mathbf{y}_i=\mathbf{x}(t_i) \end{array}\right.\end{split}\]

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:

PythonC++

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

You should obtain a figure similar to this one:

Fig. 42 The orange point depicts the observation at time \(t=2\).

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:

\[\begin{split}\left\{ \begin{array}{l} \dot{\mathbf{x}}(t)=\mathbf{f}\big(\mathbf{x}(t),\mathbf{u}(t)\big)\\ y_i=g\big(\mathbf{x}(t_i),\mathbf{b}_i\big) \end{array}\right.\end{split}\]

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

\[\begin{split}\mathbf{f}(\mathbf{x},\mathbf{u})=\left( \begin{array}{c} \vartheta\cos(\psi) \\ \vartheta\sin(\psi) \\ u_1 \\ u_2 \end{array}\right)=\dot{\mathbf{x}}.\end{split}\]

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

\[\begin{split}\mathbf{x}^*(t)=\left( \begin{array}{c}x^*_1\\x^*_2\\\psi^*\\\vartheta^*\end{array}\right)= \left( \begin{array}{l} 10\cos(t)+t \\ 5\sin(2t)+t \\ \textrm{atan2}\big((10\cos(2t)+1),(-10\sin(t)+1)\big) \\ \sqrt{(-10\sin(t)+1)^2+(10\cos(2t)+1)^2} \end{array}\right)\end{split}\]

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.

We also consider that \(t\in[0,3]\).

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:

Fig. 43 In blue: the reliable set of feasible positions. The three yellow robots illustrate the three instants \(t_i\) of observation. The white line is the unknown truth \(\mathbf{x}^*(t)\).

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.

End of third step!

That’s about all for this step!

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