Unscented (dual) quaternion kalman filter for pose estimation

duality-theoremskalman filterquaternions

This paper Unscented Dual Quaternion Particle Filter for SE(3) Estimation shows a method to use Unscented Kalman Filter (UKF) on dual quaternions in the pose estimation problem.
It proposes a projection/retraction method to project/retract sigma points on the dual-hypersphere to Euclidean space back and forth. The following is the projection equation. enter image description here
enter image description here
enter image description here

A is a 4×3 matrix, which is also the last 3 columns of Q (4×4) matrix. By simple matrix multiplication, the numerator of tau is 0, which doesn't make sense, because it means all the projected points tau, regardless what x is, is 0. Do I miss something here?

Best Answer

I suspect that the paper might have a typo in those equations. Namely, instead of $\mathcal{A}_{x_r}^{\circ}$ and $\mathcal{Q}_{x_r}^{\circ}$ I think one is supposed to use $\mathcal{A}_{v_r}^{\circ}$ and $\mathcal{Q}_{v_r}^{\circ}$ respectively. However, when using this and substituting $\tau_r = \mathcal{P}_{v_r}\!(x_r)$ in $x_r = \mathcal{U}_{v_r}\!(\tau_r)$ I get that

$$ \mathcal{U}_{v_r}\!(\tau_r) = x_r \frac{|v_r^\top x_r|}{v_r^\top x_r} = \pm x_r. $$

This would also make more sense for the notation of $\mathcal{U}_{v_r}\!(\tau_r)$, which otherwise would also be directly a function of $x_r$, instead of just $v_r$ and $\tau_r$. It can be noted that negating a unit quaternion represents the same attitude. Therefore, this possible change in sign might not matter for the rest of the algorithm.

This sign issue doesn't happen when one keeps using $\mathcal{A}_{x_r}^{\circ}$ and $\mathcal{Q}_{x_r}^{\circ}$, but that would indeed yield $\tau_r = 0$ which is quite a boring projection. However, in order to be completely sure about whether this is indeed a typo you could try to contact one of the authors.