State prediction for insfilterMARG

1 view (last 30 days)
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.

Accepted Answer

Brian Fanous
Brian Fanous on 9 Aug 2021
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.
  1 Comment
Pere Garau Burguera
Pere Garau Burguera on 9 Aug 2021
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.

Sign in to comment.

More Answers (0)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!