State prediction for insfilterMARG

Hi,

In insfilterMARG, I am trying to understand how the state update equations work. Specifically, the 4 first elements of the state, which are the 4 elements of the quaternion representing orientation. How are these update equations derived? The previous orientation [q0...q3] is updated with the delta angle, but how are these elements updated by multiplying them to the delta angle? If we have the orientation and add a delta to it (another quaternion), then the resulting orientation is defined by the product of the two quaternions, but I do not see how those equations represent that, I do not see how can the quaternion elements be multiplied directly by angles.

Am I missing something, or is it that the deltas are in some way already quaternion elements?

q0_new = q0*q0' - q1*q1' - q2q2' - q3q3' (' for prime, not transpose)

q1_new = ...

If my (q')s are the elements of the delta orientation, then they are defined based on the angles (with cos and sin combinations), but they are not the angles themselves, is what I am trying to say. Not sure how the result on the page is reached.

Answer 1

[Brian Fanous]

The documentation is a bit incorrect or not detailed enough. Here’s what is going on:

The gyroscope signal is integrated to create a “delta angle,” i.e. a change in angle. The delta angle bias is tracked as a state and subtracted from the delta angle. The delta angle bias is a result of the gyroscope bias.

This delta angle minus bias quantity is treated as a rotation vector and converted to a delta quaternion. That is the key piece that is missing from these equations. The current orientation estimate (states 1:4, representing a quaternion) is multiplied by this delta quaternion for a new orientation quaternion estimate.

Also not shown in the equations is that this new orientation quaternion estimate is then normalized and the real part is forced to be positive.

It’s hard to capture all this in simple equations, but I’ll make sure the documentation is updated to reflect these details.

[Pere Garau Burguera]

Thanks for your answer, it helps clarify things. It will sure be nice if the documentation is updated so that it is more clear to any potential reader.