Given a set of 3d positions and timestamps, what is the best numerical method to estimate the velocity of an object at the last position?

Question

I am working on a VR game where the player is able to grab an object and throw it. I would like to estimate the object velocity at the moment of release. I want to base this estimation on a recording of 3D positions and timestamps, and I want to use a numerical method in order to deal with signal noise.

The following picture describes my question. Given a set of points [(p1, t1), (p2, t2), ...] what is the velocity V at the last point?

Here is an example of the data I have recorded:

points_data = [[2.83043766 1.9541018  2.95477581]
               [2.96888566 1.88202035 3.03757858]
               [3.03159261 1.80428386 3.07880282]
               [3.07919741 1.76630378 3.11723733]
               [3.11075521 1.74352813 3.13725162]]

points_time = [[ 0.      ]
               [-0.019358]
               [-0.0394  ]
               [-0.057304]
               [-0.074427]]

The vector array points_data contains 3d (x, y, z) coordinates in meters, points_time contains time in seconds relative to a moment of release (t = 0).

• I have tried simple differentiation y1 - y0 / dt at the last point, but the result is rather noisy since VR data is also noisy.
• I then tried exponential smoothing to filter out the noise, but this results in a lag in the resulting velocity estimation.
• Yesterday I implemented least squares regression to estimate a cubic polynomial through the point data and use it to estimate the velocity. The result is pretty good, but I think I need a higher order estimation to make it 'feel' right to the player.
• This morning I was thinking about finite differences, and I think something like a higher order backward finite difference could work very well, but it looks like it is only applicable to a fixed time step. In VR, some frames take longer than others.

Any ideas are appreciated! Thanks!

Answer 1

There exists recursive polynomial approximation algorithm for N points: \begin{array} {rl} INPUT: &[(t_0,(x,y,z))], where f(t_0)=(x,y,z)\\ PROCESS:& P([(t_0, (x,y,z))]) \rightarrow f(t)\\ OUTPUT:& P([]) = (0,0,0)\\ & P(pair : rest) = (t_0,(x,y,z))=pair,\\ & f(t)=(t-t_0)*P(rest)+(x,y,z)\\ \end{array}

This algorithm gives you $f(t) : R\times R\times R$, which is polynomial approximation $f:(t)\rightarrow(x,y,z)$.

Then velocity can be calculated on point $t_0$ using the definition of derivative once the function is available: $$f'(t) = \lim_{k\to 0} \frac{f(t+k)-f(t)}{k}$$

THEOREM: if $f(a)=b$, then $f(x)-b=0$ equation has a root $x=a$.

THEOREM: when equation $f(x)=0$ has root $x=a$, then $(x-a)$ is a component of the polynomial describing f.

THEOREM: if polynomial has $N$ roots $r=[r_1,r_2,..,r_n]$, then a polynomial in form $f(x) = (x-r_1)*(x-r_2)*...*(x-r_n)$ fully describes the polynomial function.