Need Help with Calculating Quaternion Derivatives for Rigid Body Dynamics?

[jstluise]

Quaternions are new to me, so I constructed a simple model to help grasp the concept. I have a very simple dynamic model that used Euler's equations for the rigid body dynamics. The model only considers attitude; translational motion is ignored. I am making use of quaternions to describe the attitude of the body, but I am having problems with the rate quaternion. Using quaternions for performing transformations is straightforward and I understand that.

I've ran across a few different equations for calculating dq/dt, but the difference is mainly how you write the quaternion and whether or not you use quaternion math. Anyways, the form I've had the most luck with is dq(t)/dt = 1/2*W(t)*q(t), where W(t) is the angular velocity vector (composed as a quaternion) of the body wrt the fixed coordinates.

Found here: http://www.euclideanspace.com/physics/kinematics/angularvelocity/QuaternionDifferentiation2.pdf

The problem I am having is that none of my resulting quaternions are unity, which they should be. I've looked everywhere for calculating the derivative (including Kuipers), and I can't seem to find anything. The dynamics seem to behave correctly, but I know something is off.

Can someone point me in the right direction?

 

[mfb]

What happens if you use your formula with some simple examples? Like q=1, q=i, ..., W in one direction and so on. In which cases do you get deviations from unity?

Does it work properly if you normalize your values afterwards?

 

[jstluise]

What happens if you use your formula with some simple examples? Like q=1, q=i, ..., W in one direction and so on. In which cases do you get deviations from unity?

Does it work properly if you normalize your values afterwards?

I tried some simple examples like you said and I still get errors. Setting the initial quaternion and then applying different torques on at a time. The initial attitude does not matter, but the errors grow the farther away from this initial attitude. For example, if I apply a small torque, the quaternion norm may only grow to 1.03. Applying larger and larger torques, which results in larger changed in attitude, causes the quaternion norm to grow and grow.

I've seen some things about using lagrange multipliers to ensure unity, but wasn't sure if that applied to this and even how to implement it. All the sources I found regarding the quaternion derivative made no mention of having to normalize it.

I'm not sure normalizing the quaternions AFTER the numerical integration will do anything, since the dynamics depend on the qauternions as they are propagated in time. I would think they would have to be normalized during the integration process...?

 

[jstluise]

After a little more searching around, it seems normalization is required. From one source:

"As a special note, the quaternion that represents the inertial frame to body frame transformation (attitude) should be re-normalized periodically to avoid divergence of the simulation ..."

I am just using ode45 in Matlab for the numerical integration, so it looks like I'll have to tweak my code a little bit so I can normalize the quaternion every so often (ie manually run ode45 for each time step so I can see/change the output throughout the simulation).


Another thing I found that really bugs me is the way to calculate dq/dt. Referring to the paper I linked to earlier, the following are equivalent:

dq/dt = 1/2*wf*q. (wf = angular velocity transformed to fixed frame)

dq/dt = 1/2*q*wb. (wb = angular velocity in body frame)

...because wf = q*wb*inv(q)

Anyways, even though they are equivalent (I verified in Matlab), I get different results in my simulation depending on which one I use. Maybe its a numerical rounding error? Frustrating...

 

[mfb]

"As a special note, the quaternion that represents the inertial frame to body frame transformation (attitude) should be re-normalized periodically to avoid divergence of the simulation ..."

That's what I expected. The rotations just don't care about the fourth degree of freedom the quaternions have (the magnitude), as it has no physical meaning anyway.

Anyways, even though they are equivalent (I verified in Matlab), I get different results in my simulation depending on which one I use. Maybe its a numerical rounding error? Frustrating...

Or a wrong wf or wb or a wrong implementation of something.

 

[jstluise]

I think I found the right answer to what I was looking for. The following was in the help content for Matlab's aerospace toolbox:

The integration of the rate of change of the quaternion vector is given below. The gain K drives the norm of the quaternion state vector to 1.0 should ε become nonzero. You must choose the value of this gain with care, because a large value improves the decay rate of the error in the norm, but also slows the simulation because fast dynamics are introduced. An error in the magnitude in one element of the quaternion vector is spread equally among all the elements, potentially increasing the error in the state vector.

I remember seeing this in one source, but never saw it again. All the other sources I found had no mention of it. It's weird because other places in Matlab where the rate quaternion is discussed, it is shown as:

which is what I've seen almost everywhere else. I dunno...right now I am just normalizing the quaternion during the numerical integration and my dynamics look right and I haven't had any issues. But still, I'd like to know what I am missing...ideas?

 

[D H]

The old tried but not true approach is to simply normalize the quaternion after each integration step. That this is necessary is because a unit quaternion and it's derivative are normal to one another. (Any constant length vector and its time derivative are normal to one another.) An additive step in the direction of that derivative necessarily takes the quaternion away from the unit 3-sphere.

A much better approach is to use Lie group integration techniques. Here's a nice summary paper: http://arxiv-web3.library.cornell.edu/abs/1207.0069v1. The basic idea is that instead of formulating q(t+Δt) via some variant on Euler's method q(t)+ Δt*dq/dt, one formulates q(t+Δt) via some variant on exp(Δt*ω/2)*q(t). That generalization of the Euler step makes the integrated quaternion always lie on the unit 3-sphere, to within numerical limits.

Word of warning: All of the papers I've found use orthonormal matrices to represent rotation. I've yet to find one that uses quaternions. I've adapted those Lie group techniques to quaternion-based representations of SO(3). My NASA sponsor wants me to write a paper on what I've done. If I have time.

 

[jstluise]

The old tried but not true approach is to simply normalize the quaternion after each integration step. That this is necessary is because a unit quaternion and it's derivative are normal to one another. (Any constant length vector and its time derivative are normal to one another.) An additive step in the direction of that derivative necessarily takes the quaternion away from the unit 3-sphere.

A much better approach is to use Lie group integration techniques. Here's a nice summary paper: http://arxiv-web3.library.cornell.edu/abs/1207.0069v1. The basic idea is that instead of formulating q(t+Δt) via some variant on Euler's method q(t)+ Δt*dq/dt, one formulates q(t+Δt) via some variant on exp(Δt*ω)*q(t). That generalization of the Euler step makes the integrated quaternion always lie on the unit 3-sphere, to within numerical limits.

Word of warning: All of the papers I've found use orthonormal matrices to represent rotation. I've yet to find one that uses quaternions. I've adapted those Lie group techniques to quaternion-based representations of SO(3). My NASA sponsor wants me to write a paper on what I've done. If I have time.

Thanks for the clarification. Do you have anything to say about the discrepancies I keep finding about some sources mentioning normalizing and some not? Or is it just implied that it should be a unit quaternion and it is up to the user to keep it that way?

 

[D H]

Normalizing what? The quaternion or its derivative? You should *not* normalize the time derivative of a unit quaternion because that time derivative is not a unit quaternion. When you use the quaternion derivative to propagate the quaternion in time, you should normalize the integrated quaternion. How often depends on how big your time steps are and on the integration technique you are using. When in doubt, always normalize.

 

[jstluise]

Normalizing what? The quaternion or its derivative? You should *not* normalize the time derivative of a unit quaternion because that time derivative is not a unit quaternion. When you use the quaternion derivative to propagate the quaternion in time, you should normalize the integrated quaternion. How often depends on how big your time steps are and on the integration technique you are using. When in doubt, always normalize.

Sorry I wasn't clear. I was referring to the quaternion, not the derivative. I am normalizing the integrated quaternion like you say and it works fine.

What I was getting at was how some sources just list the quaternion derivative as dq/dt = 1/2*W*q, where as some list is at dq/dt = 1/2*W*q + K*e*q, where the latter uses K to correct for quaternion not being unity (see my post #6). The sources that list the former make no mention of having to correct for the changing quaternion norm, which I find odd since it seems to be important. In the end, I guess it doesn't matter but I was just curious if there was a special case I wasn't aware of or something..

Thanks again!

 

[D H]

That K*ε*q looks like a complete hack to compute the derivative in the case that the quaternion is not normalized. That fudge factor will be zero if the quaternion is unitary. A much bigger problem is that if you are using standard formulae such as computing the corresponding rotation matrix from the quaternion or using the quaternion to transform a vector, those standard formulae implicitly assume a unit quaternion. Normalize.

Or better yet, use Lie group techniques.