From a fiducial marker system, I am getting measurements of the position and orientation of a tag (as a 3D vector and quaternion pair) at uneven intervals with some zero-mean measurement noise. I want to fit it to a linear model, with constant velocity and angular velocity, so I can predict its motion at some time in the future. For the positions I used the following equation, where the system parameters are velocity and initial position (subscript 0), and the data is set by the non-zero subscripts.
$$ \begin{pmatrix} x_1 & y_1 & z_1 \\ x_2 & y_2 & z_2 \\ \vdots & \vdots & \vdots \end{pmatrix} = \begin{pmatrix} t_1 & 1 \\ t_2 & 1 \\ \vdots & \vdots \end{pmatrix} \begin{pmatrix} v_x & v_y & v_z \\ x_0 & y_0 & z_0 \end{pmatrix} $$
Using pseudo-inverse I can calculate the velocities and initial positions, and extrapolate to the future. But I can't figure out how to do the same for rotations. Since they are in SO(3), I don't think this method or standard linear regression apply.
I have a following equation for the rotation matrix at some time $t_N$ as a result of the rotation matrix at $t_0$ and a constant angular velocity.
$$R_N = \exp\left([\hat{w}]_\times t_N \right) R_0$$
The issue happens with turning this into a form that can be used with linear regression. Since the initial orientation ($R_0$) and the rotation caused by the angular velocity are not along the same axis, I can't convert to the following form.
$$ \hat{k} \theta_N = \hat{w} t_N + \hat{k} \theta_0 $$
Is there some algorithm or method for finding the constant angular velocity and predicting the orientation at some arbitrary time using the given data? I want to avoid running optimizers since this needs to run real time.
Best Answer
Since we have $R_{t+1} = R_t \exp([\hat \omega] \Delta t)$, for small enough $\Delta t$ we get $R_{t+1} \approx R_t (1 + [\hat \omega]\Delta t)$ which can be re-written to $$R_{t+1} - R_t \approx R_t [\hat \omega] \Delta t$$ $$\frac {R_t^{-1}} {\Delta t }(R_{t+1} - R_t) \approx [\hat \omega] $$
The above is a least squares problem in $[\hat \omega]$. If you parameterize $[\hat \omega]$ in $R^3$ by choosing basis matrices $V_1, V_2, V_3$ for $\frak{so}_3$, then you can write
$$ \min_{c \in R^3} \sum_t||\frac {R_t^{-1}} {\Delta t }(R_{t+1} - R_t) - (c_1 v_1 + c_2 v_2, + c_3 v_3) ||^2$$
An alternative formulation would be to formulate the problem as an EKF/UKF, by packaging the orientation and rotational velocity into one state vector $x_t = (R_t, [\hat \omega_t]$). Your observation matrix would only observe the $R_t$ part of the state, and the transition function $x_{t+1} \leftarrow f(x_t)$ would be $(R_{t+1}, [\hat \omega_{t+1}]) \leftarrow (\exp([\omega_t] \Delta t) R_t, \, [\hat \omega_t])$