Using Unscented Transformation with Quaternions (Unscented Kalman Filter)

282 Views Asked by At

I have a standard UKF with quaternions. Lets suppose that in the state vector I have mixed coordinates of elements from the "normal" vector-space (v) and not (quaternion q representing rotation). Once I have my sigma points I can propagate them through the model, and take the average (for quaternions I have to take barycentric mean). Then I get my covariance matrix (P), which describes the dependency of the coordinates on each other and start the computation again.

For the generation of the normal vector points (X) we kind of take (depending on generation method):


Eqn. (1): X = x+S*k = x + delta_x(S)


where S ~ sqrt(P) represents roughly the "square root" of the covariance matrix.

For quaternions however, it is generally not possible to sum them up, because they are not closed for addition (at least when they represent a rotation), so the quaternion product has to be taken.


Eqn. (2): Q = q (x) delta_q


where (x) denotes quaternion multiplication.

In the available literature Khoder et. al., Kraft there are several methods to model the quaternion deviation (delta_q).

Sometimes it is the part of the state-space model, sometimes they calculate it depending on S. Common in the generation is, that they usually handle the sigma point independently for the quaternions and "normal" points. That means that for (1) and (2) they take the correspoinding submatrix of S and they do something with it. So the whole sigma point is:


Eqn. (3): X_vq = [X(S_x), Q(S_q)]


where S_x and S_q is the corresponding submatrix of S.

But here is exactly my problem, because then I loose the cross-covariance terms between the corresponding coordinates (e.g v_1, q_1).

Question: is there a correct mathematical formulation to overcome it? Or does it even make sense to express the covariance of things which are not represented in the same space?