search

sfwa / ukf

Unscented Kalman Filter library for state and parameter estimation
http://au.tono.my/log/20130531-kalman-filter.html
MIT License
461 stars 172 forks source link

error in initialization of quaternions #34

Closed spaghetti- closed 7 years ago

spaghetti- commented 7 years ago

@danieldyer apologies for opening an issue here on an old revision (however I could not find your email to mail you directly)

https://github.com/sfwa/ukf/blob/f69d985755605d5dc621f5f0578b191414fd0264/src/state.cpp#L66

I read reading through the code here and I saw that you were initializing a quaternion using Quaterniond(x.segment<4>(9)) which would initialize it in the form Quaterniond(x, y, z, w) because of your state definition (JPL style quaternion) however Eigen uses Hamilton quaternions (w, x, y, z. May I know if this was intended or a bug?

danieldyer commented 7 years ago

@spaghetti- no worries – so this is actually intended, because of an inconsistency in the way Eigen represents quaternions. Despite the fact that the quaternion constructor Quaternion (const Scalar &w, const Scalar &x, const Scalar &y, const Scalar &z) which takes four scalar arguments takes them in the order (w, x, y, z), Eigen actually seems to represent quaternions internally with the components in the order (x, y, z, w), which is why the quaternion part of the state vector in the code you linked is defined that way. In any case, when initialising a quaternion from something like an Eigen::Matrix<double, 4, 1>, the elements are interpreted as being in the order (x, y, z, w). You can confirm this for yourself using the following:

Eigen::Quaterniond a = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
Eigen::Quaterniond b = Eigen::Quaterniond(Eigen::Matrix<real_t, 4, 1>(0.0, 0.0, 0.0, 1.0));
std::cout << a.w() << " " << a.vec().transpose() << std::endl;
std::cout << b.w() << " " << b.vec().transpose() << std::endl;

The output will be as follows:

1 0 0 0
1 0 0 0

Indicating that the way the two constructors have been used is equivalent.

spaghetti- commented 7 years ago

@danieldyer I apologize for opening the issue before checking myself. In fact I did check but I only checked the case where I did Eigen::Quaterniond(1, 0, 0, 0). The Eigen guys did say that they aware of the inconsistency but are unable to do anything about it.

spaghetti- commented 7 years ago

@danieldyer I also had a few questions regarding the calculation of the state vector derivate as you did here over equation 27 in A quaternion based unscented Kalman Filter by E. Kraft. Should I open a new issue to ask that or can I just email you if that is okay?

danieldyer commented 7 years ago

@spaghetti- Sure, no problem. My email address is visible on my profile now.