Trajectory interpolation with known coordinates and heading angles.

Question

I have a set of $n$ tuples $(t_i,x_i,y_i,r_i)$ for $i \in \{0,...,n\}$, where $t_i$ is a timestamp, $x_i$ and $y_i$ location coordinates and $r_i$ a heading angle of a moving object in 2D space. Note that I do not know the velocity, only the heading angle.

I would like a continuous interpolation for the time interval $[t_0, t_N]$, i.e. the trajectory of said object. What would be a smart way to do this?

Here are the steps to my current approach:

1. Estimate velocities $v_i$ using only time and location coordinates:
   1. Fit natural cubic splines $S_x(t)$ to points $(t_i,x_i)$.
   2. Fit natural cubic splines $S_y(t)$ to points $(t_i,y_i)$.
   3. Calculate $x'_i$ and $y'_i$, which are the first derivatives of $S_x$ and $S_y$ at $t_i$, respectively.
   4. Define $v_i$ as $\lVert(x'_i,y'_i)\rVert_2$. Note that vector $(x'_i,y'_i)$ does not point in the direciton of $r_i$.
2. Adjust derivatives, so they point in direction $r_i$
   1. $\tilde{x}'_i := \cos(r_i)*v_i$
   2. $\tilde{y}'_i := \sin(r_i)*v_i$
   3. Vector $(\tilde{x}'_i, \tilde{y}'_i)$ has length $v_i$ and points in desired direction $r_i$.
3. Estimate overall trajectory
   1. Fit cubic splines $\tilde{S_x}$ using $(t_i,x_i,\tilde{x}'_i$), where the splines should be consistent with $x_i$ and have derivatives $\tilde{x}'_i$ at timestamps $t_i$.
   2. Fit cubic splines $\tilde{S_y}$ using $(t_i,y_i,\tilde{y}'_i$), analogous to $\tilde{S_x}$.
   3. Note that (unlike in the previous cubic splines) we no longer enforce that the second derivative is the same for two connected splines. Instead we enforce specific first derivative values.

Result: $\tilde{S_x}$ and $\tilde{S_y}$ define a continuous trajectory, which are consistent with all $(t_i,x_i,y_i,r_i)$.