Understanding Quartenion Errors and Integration for Attitude Estimation

[Alpha2005]

I have two questions on the quaternion which I hope could be clarified.

Homework Statement

Q1)
The multiplicative error quaternion has equation:

q_aposteriori = [δqτ √(1-δqτ * δq)]τ χ q_apriori

I have some data where δqτ * δq > 1 and thus √(1-δqτ * δq) give me an imaginary number, which is supposed to be the scalar fourth component of quaternion. Am I doing anything wrong here, and how can I solve it?Q2) For numerical integration, can I just integrate quaternion like I will normally do with dynamic equations?

as per usual propagation:
x = x + w*dt

for quartenion:
q = q + qdot*dt
then normalize(q)
is this correct?

Homework Equations

The multiplicative error quaternion has equation:

q_aposteriori = [δqτ √(1-δqτ * δq)]τ χ q_apriori

where:
q_aposteriori = quartenion after update
q_aposteriori = quartenion before update
τ = transpose
χ = quaternion multiplication

The Attempt at a Solution

as above

 

[D H]

First, a huge caveat. There are multiple ways of using quaternions to represent orientation. Three key issues are

• Rotation vs. transformation.
  A similar issue arises with using matrices. A rotation matrix (quaternion) physically rotates a vector while a transformation matrix (quaternion) transforms the same vector from one frame of reference to another. Rotation and transformation are conjugate operations.
• Left versus right quaternions.
  Do you rotate/transform a vector v via v'=qvq^τ ("left quaternion") or via v'=q^τvq ("right quaternion")? Both forms are valid. A similar issue but less frequently encountered issue arises in matrices. If you use column vectors you rotate/transform via v'=Mv; with row vectors you use v'=vM.
• Representation as four vectors.
  Is the identity quaternion [0,0,0,1] or [1,0,0,0]?

You need to be aware of these issues when going through the literature because (jaded personal view) many authors are not aware of them. They just assume that there is only one possible answer to each question and move on. There are in fact 2×2×2 sets of answers.


Another caveat applies with regard to using quaternions to represent orientation/attitude in a Kalman filter, and this has nothing to do with representation. The problem is that rotation has three degrees of freedom, but quaternions are represented with four numbers. This means the innovation matrix, at least in theory, must be singular. In practice, you won't get a singular matrix. You'll just get an ill-formed matrix. Naively taking the inverse of this can lead to *big* problems. To avoid this, it is best to use a pseudo inverse rather than a simple matrix inverse. Decompose the innovation matrix into eigenvalues and eigenvectors, S=PDP^τ. The pseudo inverse of the diagonal matrix D is formed by computing A_ii = 1/D_ii if |D_ii| is greater than some threshold, 0 otherwise (in short, 1/small number ≈ 0 rather than a big number). The pseudo inverse of S is computed via PA^τ. This alone might be the solution to your first question.


Regarding your first question, some say to use 1/√(1+δq^τδq) when δq^τδq is greater than one. You can try that, but you might also want to investigate why δq^τδq is not small.

One reason it might not be small is because you are using a standard matrix inversion to invert your innovation matrix. Suppose your observable is more or less parallel to an eigenvector corresponding to a small eigenvalue. This will lead to a huge δq. A better way to look at it is that those eigenvectors are not truly observable.

Another thing that can make δq^τδq large is your quaternion normalization. The scalar part of a unit quaternion is non-negative in canonical form. Do not do that. q and -q are indistinguishable when it comes to rotating/transforming a vector. To your integration and update algorithm, q and -q are worlds apart. When you normalize the quaternion, just scale by 1/||q||. If you have a negative scalar part, let it stay negative.


With regard to your second question, using x = x + xdot*dt implicitly assumes a constant value for xdot over the time interval dt. Using q = q + qdot*dt implicitly assumes a constant value for qdot *and* implicitly uses the small angle approximation cos(ω*dt/2)≈1, sin(ω*dt/2)≈ω*dt/2. The after-the-fact normalization is a kludge to get around errors induced by the small angle approximation. It's a useful kludge, but it is a kludge.

 

[Alpha2005]

Thanks D H for the tips.

For clarification, I'm dealing with transformation and the unit quaternion is [0;0;0;1], albeit MATLAB uses [1 0 0 0]

I'm also curious as to why δqτδq is not small. I have tested with an ideal case (no measurement noise) and the error in attitude is zero for 50 min, but after that it goes haywire. (kalman sampling interval of 1 second). The update equation is as given before:

qAposteriori = qError χ qApriori;
where qError = [δqτ √(1-δqτ * δq)]τ


I have then tried with another update equation and it works good. The equation is :

qAposteriori = qApriori + ∅(qApriori)/2 *qError
where ∅(Apriori) = [qApriori(4)*eye(3)+skew(qApriori(1:3));-qApriori(1:3)']

I'm not sure what when wrong with the first equation.

I'm also curious with the pseudoinverse that you mentioned. Are you referring to the kalman gain K where there's a inverse term for the innovation? S^-1 as shown:

Thanks for clarifying the second question. I have seen some literature mention numerical integration for propagation, which I assumed to be the q=q+qdot*dt along with kinematics equation. I have also came across a paper which propagate using:

where

Are the essentially the same? The second equation seems to normalize the propagated quaternion.

 

[D H]

With regard to $\boldsymbol K_k = \boldsymbol P_{k|k-1}\boldsymbol H^T_k \boldsymbol S^{-1}_k$: That's the problem child in attitude estimation.

Attitude+attitude rate live in the the non-abelian space $\mathrm{SO}(3)\times\mathbb R^3$. That space has six degrees of freedom. You have seven parameters, four for the quaternion and three for rate. That's one too many parameters given the number of degrees of freedom. If you were true to the math of the space, your matrix S would be singular. It probably isn't singular because you start with a non-singular matrix, but after fifty minutes of running it might be getting close to it.

With regard to $\hat{\boldsymbol q}^-_{k+1} = \bar{\Omega}\bigl(\hat{\omega}^+_k\bigr) \hat{\boldsymbol q}^+_k$, pre-multiplying by that matrix is identical to post-multiplying by the quaternion $[\sin\frac{\omega \Delta t} 2 \boldsymbol e_{\omega}, \cos\frac{\omega \Delta t} 2]$ where $\omega=||\hat{\omega}^+_k||,\ \boldsymbol e_{\omega} = \hat{\omega}^+_k / ||\hat{\omega}^+_k||$. That quaternion in turn is the quaternion exponential of the pure imaginary quaternion $[\frac{\hat{\omega}^+_k \Delta 2}2, 0]$.

So where does this come from? A translation vector $\boldsymbol x \in \mathbb R^3$ with a constant velocity $\boldsymbol v$ updates via $\boldsymbol x(t+\Delta t) = \boldsymbol x(t) + \boldsymbol v\Delta t$. That is what justifies the additive form you typically use for integrating position.

This is not true for orientation. Suppose an object is rotating at a constant angular velocity. If you represent orientation as a right transformation quaternion (which is what you are apparently using), the equation of motion for that object rotating at a constant rate is $\boldsymbol q(t+\Delta t) = \boldsymbol q(t)\exp\bigl(\frac{\omega \Delta 2}2\bigr)$. The reason you can use the naive $\boldsymbol q(t+\Delta t) = \boldsymbol q(t) + \dot{\boldsymbol q}(t)\Delta t$ is because of the small angle approximation.


Some things to think about:
- You might want to look at using a pure imaginary error quaternion as opposed to a quaternion. This keeps the state size to six.
- Alternatively, you might want to look at using modified Rodrigues parameters for representing orientation. This too will keep the state size to six.


If you haven't already read it, I suggest you read this survey paper, Crassidis, J.L., Markley, F.L., and Cheng, Y., “A Survey of Nonlinear Attitude Estimation Methods,” AIAA Journal of Guidance, Control, and Dynamics, Vol. 30, No. 1, Jan.-Feb. 2007, http://www.acsu.buffalo.edu/~johnc/att_survey07.pdf.