Lesson D: SLAM

Main authors: Simon Rohou

We propose to apply interval tools to solve a classical state estimation problem of Simultaneous Localization And Mapping (SLAM).

A tank robot \(\mathcal{R}\), whose state vector \(\mathbf{x}\in\mathbb{R}^3\) describes its position \((x_1,x_2)^\intercal\) and its heading \(x_3\), is evolving among a set of landmarks \(\mathbf{b}^k\in\mathbb{R}^2\) with a constant speed \(v=10\). It is equipped with a compass for measuring its heading \(x_3\) with bounded uncertainty.

Formalism

We are addressing the following state estimation problem:

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

The evolution of the state is given by:

(2)\[\begin{split}\dot{\mathbf{x}}= \mathbf{f}(\mathbf{x},u)=\left( \begin{array}{c} 10\cos(x_3) \\ 10\sin(x_3) \\ u \end{array} \right),\end{split}\]

and the system input \(u\) (rotation rate) is expressed as:

\[u(t) = 3\sin^2(t)+\frac{t}{100}.\]

The observation function \(g\) is the distance function between a position \((x_1,x_2)^\intercal\) and a landmark \((b_1^k,b_2^k)^\intercal\).

Exercise

D.1. Update your current version of Codac. In Python, for instance:

pip install codac --upgrade --pre
# Option --pre has to be set because Codac v2 is only available in pre-release

To verify that you have the correct version, you can use:

python -m pip show codac

> Name: codac
  Version: 2.0.0.dev27

The required version for this tutorial is 2.0.0.dev27 or later.

The following instructions are given in Python, but feel free to use C++ or Matlab if you prefer.

Simulation

The following code provides a simulation of the actual but unknown trajectory \(\mathbf{x}^*(\cdot)\), without uncertainties. This code can be used as a starting point for your project.

PythonC++Matlab

from codac import *
import random
import numpy as np

dt = 0.02 # temporal discretization
t0tf = Interval(0,15) # temporal domain [t0,tf]

# System input
t = ScalarVar()
# Input u(.) is given as an analytic trajectory
u = AnalyticTraj(AnalyticFunction([t],3*sqr(sin(t))+t/100), t0tf).sampled(dt)

# Implementing manually the evolution function (Eq. (2))
truth_heading = u.primitive()
truth_px = (cos(truth_heading)*10.).primitive()
truth_py = (sin(truth_heading)*10.).primitive()

# Actual state trajectory
# Note that this trajectory is unknown to the estimation process
s1,s2,s3 = ScalarVar(),ScalarVar(),ScalarVar()
f_concat = AnalyticFunction([s1,s2,s3], [s1,s2,s3])
truth_x = f_concat.traj_eval(truth_px,truth_py,truth_heading)

DefaultFigure.draw_trajectory(truth_x)
DefaultFigure.draw_tank(truth_x(t0tf.ub()), 1., [Color.dark_gray(),Color.yellow()])

Using VIBes, you should visualize these states:

Unknown actual trajectory, to be estimated.

Deadreckoning

The robot knows its initial state: \(\mathbf{x}(0)=(0,0,0)^\intercal\). Deadreckoning consists in estimating the robot’s subsequent positions without exteroceptive measurements (i.e. without distances from the landmarks). In this section, we will compute the set of feasible positions of \(\mathcal{R}\), considering only heading measurements and the evolution function \(\mathbf{f}\).

Exercise

D.2. Domains. The set of feasible positions along time is a tube (interval of trajectories). We create a tube \([\mathbf{x}](\cdot)\) using:

PythonC++Matlab

tdomain = create_tdomain(t0tf, dt) # temporal discretization over [t0,tf]
x = SlicedTube(tdomain, IntervalVector(3))

where tdomain is the temporal discretization over \([t_0,t_f]\), and dt is the discretization parameter. Here, x is a 3d tube.

At this point, \(\forall t\in[t_0,t_f],~ [\mathbf{x}](t)=[-\infty,\infty]^3\): the states are completely unknown.

This can be verified, for instance at \(t=0\), with:

PythonC++Matlab

print(x(0.))

Exercise

D.3. Heading measurements. In practice, the headings \(x_3(\cdot)\) are measured with some uncertainties known to be bounded in \([-0.03,0.03]\). We set these bounded measurements in the last component of the tube vector \([\mathbf{x}](\cdot)\) with:

PythonC++Matlab

x = tube_cart_prod(
  SlicedTube(tdomain, IntervalVector(2)),
  # Heading measurement with bounded uncertainties:
  SlicedTube(tdomain, truth_heading).inflate(0.03)
)

Exercise

D.4. Initial condition. The initial state \(\mathbf{x}(0)\) (position and heading) is known, which can be implemented in the tube with:

PythonC++Matlab

x.set([0,0,0], 0.) # setting a vector value (0,0,0) at t=0
print(x(0.))

Now that a domain (a tube) has been defined for enclosing the estimates together with their uncertainties, it remains to define contractors for narrowing their bounds.

In deadreckoning, only Eq. (2) is considered: \(\dot{\mathbf{x}}(\cdot)=\mathbf{f}(\mathbf{x}(\cdot))\). This can be processed with two contractors, one for dealing with \(\mathbf{v}(\cdot)=\mathbf{f}(\mathbf{x}(\cdot))\), and one for \(\dot{\mathbf{x}}(\cdot)=\mathbf{v}(\cdot)\).

The new tube \([\mathbf{v}](\cdot)\) will enclose the feasible derivatives of the possible states in \([\mathbf{x}](\cdot)\).

Exercise

D.5. Derivatives of the state. As for \([\mathbf{x}](\cdot)\), create another 3D SlicedTube for \([\mathbf{v}](\cdot)\), called v.

Exercise

D.6. Contractors. Then, create a contractor for \(\mathbf{v}(\cdot)=\mathbf{f}(\mathbf{x}(\cdot))\). The associated constraint is expressed as an implicit form \(\mathbf{f}(\mathbf{x}(\cdot),\mathbf{v}(\cdot))=\mathbf{0}\).

PythonC++Matlab

vx,vv = VectorVar(3),VectorVar(3)
f_evol = AnalyticFunction([vx,vv], [
    vv[0]-10*cos(vx[2]),
    vv[1]-10*sin(vx[2]),
  ])

ctc_f = CtcInverse(f_evol, [0,0]) # [0,0] stands for the implicit form f(..)=0

Create a contractor for \(\dot{\mathbf{x}}(t)=\mathbf{v}(t)\):

PythonC++Matlab

ctc_deriv = CtcDeriv()

We recall that contractors are algorithms for reducing sets of feasible values (intervals, boxes, tubes..).

Exercise

D.7. Deadreckoning estimation. At this point, you can implement an algorithm for deadreckoning, by calling the previously defined contractors.

PythonC++Matlab

x,v = ctc_f.contract(x,v)
x,v = ctc_deriv.contract(x,v)

Exercise

D.8. Display. Finally, the contracted tube \([\mathbf{x}](t)\) can be displayed with:

PythonC++Matlab

DefaultFigure.draw_tube(x, [Color.light_gray(),Color.light_gray()])
DefaultFigure.draw_trajectory(truth_x)

You should obtain the following result:

In gray: tube \([\mathbf{x}](t)\) enclosing the estimated trajectories of the robot. The actual but unknown trajectory is depicted in black. With interval methods, the computations are guaranteed: the actual trajectory cannot be outside the tube. However, the tube may be large in case of poor localization, as is the case so far without state observations.

Range-only localization

The obtained tube grows with time, illustrating a significant drift of the robot. We now rely on distances measured from known landmarks to reduce this drift. This amounts to a non-linear state estimation problem: function \(g\) in System (1) is a distance function. Non-linearities can be difficult to solve with conventional methods.

Exercise

D.9. Landmarks. Define five landmarks with:

PythonC++Matlab

b = [(6,12),(-2,-5),(-3,20),(3,4),(-10,0)]

Exercise

D.10. Range observations. In a loop, for each \(t_i\in\{0,1,\dots,15\}\), compute the distance measured from a random landmark:

PythonC++Matlab

Y = []
for ti in np.arange(0,15):
  k = random.randint(0,len(b)-1) # a random landmark is perceived
  d = sqrt(sqr(truth_x(ti)[0]-b[k][0])+sqr(truth_x(ti)[1]-b[k][1]))
  Y.append([k,ti,d])

Exercise

D.11. Measurement uncertainty. The measurements come with some errors (not computed here) that are known to be bounded within \([-0.03,0.03]\). We use intervals for enclosing the observations:

PythonC++Matlab

  d += Interval(-0.03,0.03)
  # or equivalently: d.inflate(3e-2)

As in the previous lessons, a fixpoint function can be used to manage the contractors automatically. In another for loop, we will combine contractors using a fixpoint method, in order to improve the localization of the robot.

Exercise

D.12. Observation contractors. The state observations are added to the set of constraints by means of a new distance contractor linking the state to the position of a landmark:

PythonC++Matlab

vb = VectorVar(2)
vd = ScalarVar()
f_dist = AnalyticFunction([vx,vb,vd], sqrt(sqr(vx[0]-vb[0])+sqr(vx[1]-vb[1]))-vd)
ctc_dist = CtcInverse(f_dist, 0) # also expressed in a implicit form g(x,b,d)=0

Exercise

D.13. Range-only localization. The contractor network is now as follows:

PythonC++Matlab

def contractors_list(x,v):
  x,v = ctc_deriv.contract(x,v)
  x,v = ctc_f.contract(x,v)
  for yi in Y: # for each range-only measurement
    ti = yi[1]
    pi = x(ti)
    bi = IntervalVector(b[yi[0]])
    di = yi[2]
    pi,bi,di = ctc_dist.contract(pi,bi,di)
    x.set(pi, ti)
  return x,v

x,v = fixpoint(contractors_list, x,v)

Exercise

D.14. Display the contracted tube.

PythonC++Matlab

DefaultFigure.draw_tube(x, ColorMap.blue_tube())
DefaultFigure.draw_trajectory(truth_x)
DefaultFigure.draw_tank(truth_x(t0tf.ub()), 1., [Color.dark_gray(),Color.yellow()])

You should obtain the following result:

In gray: the former tube without state observations. In blue, the new contracted tube, considering distance measurements from the landmarks.

Exercise

D.15. Unknown initial condition?

What if we have no knowledge about the initial position of the robot?

Try to remove the condition set in Exercise D.4., and zoom towards the initial position.

Challenge: Simultaneous Localization and Mapping (SLAM)

Exercise

D.16. Range-only SLAM.

Adapt the script in order to deal with a SLAM problem, in which the positions of the landmarks are a priori unknown, but then estimated together with the positions of the robot.

For this estimation, we will need more observations: we now assume that distance measurements are obtained every 0.1s.

Note that in SLAM, the initial condition set in Exercise D.4. is mandatory.