danieldyer
commented
9 years ago Hi Brian,
Thanks for the kind words – I hope you’re able to find it useful.
There are a set of comments in ‘state.h’ describing the composition of the state vector – the attitude quaternion is defined in such a way as to describe a rotation from the North/East/Down frame to the vehicle body frame.
So, if the vehicle was heading directly West, a magnetic field vector of say [1, 0, 0] in the NED frame would end up as [0, 1, 0] in the body frame – a rotation of 90 degrees clockwise, looking down along the Z-axis – and the corresponding quaternion is [0.707, 0, 0, 0.707].
However, the angular velocity around the Z-axis required to spin the vehicle counter-clockwise from the North direction to the West direction is negative, which is the opposite sign to the imaginary part of the quaternion which describes that orientation.
I must confess I got myself confused about coordinate systems a number of times developing the filter, and in hindsight this might not have been the most intuitive choice of attitude representation, so let me know if this explanation makes sense!
Cheers, Daniel
Hi -- First I'd like to say this is a very nice UKF implementation. For the most part, it makes a lot of sense.
I'm having a bit of trouble, but I can't tell whether it's a misunderstanding on my part or an actual bug. While I'm reasonably familiar with Kalman filters, I'm having trouble with the coordinate system conventions and some apparent sign issues.
In order to understand what is going on inside the filter, I implemented the test function "AccelerometerConstantAngularVelocity" so that it prints out the state vector to a file at each step. Then I can plot components, etc., and watch the behavior.
I find that the UKF properly estimates the angular velocity about the y axis to be +1. However, the q2 component of the quaternion is decreasing. Shouldn't they have the same sign? Unless, of course, the interpretation of the attitude quaternion is the rotation required to take the body frame back to the world frame?
The rotation through 10 radians nets out to 212 degrees. If I understand your body coordinate system (forward/left/up), then a positive rotation about the y-axis "points the nose down", leading to a quaternion (w=0.283662, xyz = 0, 0.-95892), which is the conjugate of the test assertion. The test assertion is indeed what I get when I run the code.
I'd love to discuss further. I'm trying to implement this for Raspberry Pis and Phidget Spatial sensors. I can be reached at brian, and then the at sign, and then kairosaerospace.com.
Thanks!