Operations on trajectories

Main author: Simon Rohou

Arithmetic and calculus

Operations such as x1+x2 or sin(x1) compute values at the stored timestamps. The resulting SampledTraj interpolates linearly between these values. Arithmetic between two sampled trajectories requires identical timestamps.

For a scalar or vector trajectory \(x\), primitive(dt) approximates the integral

\[p(t)=\int_{t_0}^{t}x(\tau)\,d\tau,\qquad p(t_0)=0,\]

using the trapezoidal rule, with integration step dt. For a SampledTraj, primitive() instead integrates over its existing sample intervals:

\[p(t_{i+1})=p(t_i)+\frac{t_{i+1}-t_i}{2}(x_i+x_{i+1}).\]

derivative() approximates the derivative of a sampled trajectory by finite differences. These numerical operations do not provide guaranteed bounds.

PythonC++

speed = SampledTraj({0.:0, 1.:1, 2.:2})
acceleration = speed.derivative() # numerical derivative
distance = speed.primitive()      # trapezoidal integral, p(0)=0
doubled_speed = 2. * speed        # pointwise arithmetic

Cartesian products

traj_cart_prod(x1,x2,...) combines scalar or vector sampled trajectories into a vector trajectory. The timestamps must be identical. Use sampled_as() to align trajectories when necessary.

PythonC++

x1 = SampledTraj({0.:0, 1.:1})
x2 = SampledTraj({0.:2, 1.:3})
x = traj_cart_prod(x1, x2)

Unwrapping periodic signals

continuous_traj() removes apparent jumps in a sampled scalar signal, for instance a heading crossing its angular discontinuity.

PythonC++

heading = SampledTraj({0.:6.1, 1.:0.1, 2.:0.2})
continuous_heading = continuous_traj(heading)

The function detects jumps between consecutive samples and corrects them using a period estimated from the observed values. This is particularly useful for trajectories representing angles. Since the period is estimated rather than specified, the correction may be approximate.

Applying analytic functions

f.traj_eval(x1,x2,...) evaluates an AnalyticFunction on sampled trajectories at the timestamps of the first input, interpolating the other inputs as needed. In Python, inputs are all scalar trajectories or all vector trajectories, while C++ also accepts mixed inputs. For interval enclosures on tubes, see Evaluating analytic functions on tubes.

For example, given a heading \(\theta(t)\) and a speed \(v(t)\), an analytic function computes the Cartesian velocity components:

\[\begin{split}\mathbf{u}(t)=\begin{pmatrix}v(t)\cos\theta(t)\\v(t)\sin\theta(t)\end{pmatrix}.\end{split}\]

PythonC++

theta = SampledTraj({0.:0, 1.:0.1, 2.:0.2})
v = SampledTraj({0.:1., 1.:2., 2.:3.})
theta_var, v_var = ScalarVar(), ScalarVar()
f = AnalyticFunction([theta_var, v_var], [v_var*cos(theta_var), v_var*sin(theta_var)])
u = f.traj_eval(theta, v)

For a vector-valued input trajectory, use a VectorVar:

PythonC++

p = SampledTraj({0.: Vector([3,4]), 1.: Vector([0,5])})
p_var = VectorVar(2)
f = AnalyticFunction([p_var], sqr(p_var[0]) + sqr(p_var[1]))
result = f.traj_eval(p)

Using a trajectory in an analytic expression

as_function() makes a trajectory available inside an analytic expression of time. This is useful when an expression depends on an existing time series.

PythonC++

measurements = SampledTraj({0.:1, 1.:2, 2.:1.5})
measurement_fn = measurements.as_function()
t = ScalarVar()
energy = AnalyticFunction([t], sqr(measurement_fn(t)))
estimated_energy = energy.eval(0.5)

The trajectory operator currently supports forward evaluation but not general backward contraction or derivative propagation through the expression.

Converting trajectories and tubes

SlicedTube(td,traj) encloses a trajectory on a temporal partition. To enclose an unknown signal, additional measurement and interpolation uncertainty may be needed. Conversely, tube.mid() gives a numerical midpoint trajectory without retaining the tube’s uncertainty.

PythonC++

t = ScalarVar()
traj = AnalyticTraj([0,2], AnalyticFunction([t],sin(t)))
td = create_tdomain([0,2], 0.1)
tube = SlicedTube(td, traj)
tube.inflate(0.05) # encloses the true signal if its error is bounded by 0.05