# Estimate angular velocity and angular acceleration from a point cloud sequence

Lets say I have the a set of points $P = \{p_1, p_2, ...\}, p_i \in R^3$ that change position with time. These points are part of a rigid body and I record these positions in order to estimate its properties. Also, I know the linear velocity and acceleration of these points.

I was able to estimate the transformation matrix $T$ between two consequent time instances $t_1, t_2$, that best maps $P_i(t_2) = T P_i(t_1), \forall i$, meaning I know the orientation of the body. Now I need to estimate the angular velocity and angular acceleration with respect to some point $O$.

What are my options? I want to avoid numerical differentiation if possible.

If you know that your transform was caused by a rotation, then what you're looking at is a class Aerospace engineering problem called as Wahba's Problem which comes up in ADCS (attitude determination and control systems)

Wahba's problem asks for an orthogonal matrix $T$ (orthogonal because they represent rotations in 3D), that best minises the error function

$$\delta \theta = \sum_{i=1}^{n}|P_i(t + 1) - w_i T P_i(t)|$$

where $P_i(t)$ are unit reference vectors, $w_i$ are their weights that reflect our "belief" in the correctness of the $P_i(t)$

The "best" algorithm that I am aware of is QUEST (QUaternion ESTimation). Here is a link to an explanation and historical overview of the algorithm by the creator of the algorithm himself.

Note that the problem cannot have a "perfect" solution by definition, since the system is forced to be either over or under damped.

This is because to specify a rotation, we require 3 parameters (commonly thought of as Euler angles)

However, since the $P(t)$ vectors are unit vectors, they are determined by just 2 components (say $x$ and $y$. The third component is derived from $z = \sqrt{x^2 + y^2}$. Hence, one vector provides 2 data points, while 2 vectors provide 4. We can never have 3 data points (which is what the problem essentially asks for).

• Thank you for your answer. Actually, I have solved the problem of estimating the rotation matrix through SVD. My problem now is how to estimate the angular velocity and acceleration. – crow May 7 '16 at 12:55