Extended Kalman Filter - Process noise and covariance

[Rares]

Dear all,

I'm trying to implement an Extended Kalman Filter for position, velocity and orientation tracking of a rigid body and I am using quaternions for representing the orientation in the state vector. As this is the first time I have to work with a Kalman Filter, and the project is on a tight time-schedule, I didn't have time to dig in too deep into Kalman Filter theory, or to let the knowledge I've acquired so far sink in ... Below is a (detailed) description of my problem:


The inputs to the system come from an IMU, in the form of angular velocities and linear accelerations - in the body reference frame, while the measurements are of the position and orientation in the global reference frame.

For now I am trying to figure out how to get the process noise vector and its covariance. The state is:

$$x(p^{e},\dot{p}^e,q^{be})$$

The state transition functions are:

$$p^{e}_{t+1} = p^e_t + T\dot{p}^e_t + \frac{T^2}{2}\ddot{p^e_t}$$
$$\dot{p}^e_{t+1} = \dot{p}^e_t + T \ddot{p}^e_t$$
$$q^{be}_{t+1} = e^{-\frac{T}{2}\omega^b_t}\odot q_T^{be}$$

with $$\odot$$ being the quaternion product and T the sampling interval.

The inputs to the system are: $$(u_{acc},u_{\omega})$$ - in the body reference frame and they translate to $$(\ddot{p},\omega )$$ through the following equations:

$$\ddot{p}^e_t = R^{be}u^b_{acc,t} + g^e - R^{be}_t e^b_{acc}$$
$$\omega^{b}_t = u^b_{\omega} - e^b_{\omega}$$

where $$g^e$$ is the gravity vector in the global reference frame, and $$e_{acc}, e_{\omega}$$ are zero mean Gaussian noise (these I am given from the datasheet of the IMU). I am trying to derive the functions needed for the second equation of the EKF:

$$P^{-}_t = A_tP_{t-1}A_t^T + W_tQ_{t-1}W_t^T$$

with $$A_{[i,j]} = \frac{\partial f_{<i>}}{\partial x_{[j]}}\left( \hat{x}_{t-1},u_{t-1},0\right)</i>$$
$$W_{[i,j]} = \frac{\partial f_{<i>}}{\partial w_{[j]}}\left( \hat{x}_{t-1},u_{t-1},0\right)</i>$$

with $$f$$ being the state transition function and $$w$$ being the process noise. I've computed the Jacobian $$A_t$$for the state transition function $$f$$ as (I think this is correct):

$$A_t=\left(<br /> \begin{array}{ccc}<br /> I_3 & TI_3 & 0 \\<br /> 0 & I_3 & 0\\<br /> 0& 0 & e^{-\frac{T}{2}\omega^b_t} \\<br /> \end{array}<br /> \right)$$

I've been trying for some time now to figure out how to relate the sensor noises to the state and how to compute the $$Q$$ and $$W$$ matrices.
Any help is greatly appreciated.
Thanx in advance,
Rares

 

[D H]

Using quaternions to represent the rotational state in a Kalman filter is a bit problematic for two reasons:

1. Quaternions have four elements, but there are only three degrees of freedom. As a result the covariance matrix is inherently singular.
2. The unit quaternions are a double map of SO(3). In other words, -Q represents exactly the same rotation as does Q.

There are some ways to work around these issues and still use quaternions. For example, use the Moore-Penrose pseudoinverse and normalize the propagated/updated quaternion. You still have to take care with the normalization process because of that double map thing.

Alternatives to quaternions are to use a representation of orientation that only uses three parameters such as Euler angles (yech), Rodrigues parameters (singularities at multiples of 180 degrees), and modified Rodrigues parameters (singularities at multiples of 360 degrees).

A lot of people are moving toward using modified Rodrigues parameters to describe the attitude error. A vehicle that has a 360 degree attitude error has much bigger problems than a non-convergent Kalman filter. In other words, the singularity at 360 degrees probably won't ever become an issue.

A paper on MRPs for attitude estimation:
http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19960035754_1996055864.pdf

 

[Dave_fr]

DH,

I'm very interested in the modified Rodrigues parameter method but one question arises. Can you tell us why it seems to be concidered as a much better approach than working with Euler angle directly in the EKF especially concidering that singularities happen at 360°. Is this singularitiy similar to the gimble lock ?

 

[D H]

http://www.google.com/search?q="Euler+angles+are+evil"". Gimbal lock is just one reason. Rodrigues parameters and MRPs (modified Rodrigues parameters) are much more closely allied with quaternions than with Euler angles.

Any rotation in three-space can always be represented as a single rotation by some angle θ about some eigenaxis ê. The unit quaternion corresponding to this eigenrotation is {cos(θ/2), -sin(θ/2)ê} (left quaternion) or {cos(θ/2), sin(θ/2)ê} (right quaternion)^1[/color]. Quaternions have some very nice features, but also one misfeature: They overspecify the problem. Quaternions have four elements, but rotations in three space have only three degrees of freedom. Both Rodrigues parameters and MRPs combine the eigenangle θ and eigenaxis ê to form a three vector, so neither Rodrigues parameters nor MRPs overspecify the problem. Rodrigues parameters use êtan(θ/2) while MRPs use êtan(θ/4). The downside of Rodrigues parameters is that tan(θ/2) becomes infinite at θ=180°; MRPs have a similar problem at 360°.

This paper, http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19960035754_1996055864.pdf, describes an extended Kalman filter that uses MRPs for attitude estimation. MRPs are also described in some aeronautics engineering texts. For example, Schaub & Junkins, Analytical mechanics of space systems. See http://books.google.com/books?id=qXvESNWrfpUC&pg=PA107&lpg=PA107#v=onepage&q&f=false.


--------------------

^1[/color] Regarding left versus right quaternions.
Consider a similarity transformation of a matrix. http://mathworld.wolfram.com/SimilarityTransformation.html" uses P^-1AP. So which is the "right" way to represent a similarity transformation: PAP^-1 or P^-1AP? The thing that is wrong is insisting that one way is correct and the other wrong. Both are valid.

A very similar issue exists with quaternions. Given quaternions A and Q, this expression, Q^-1AQ, represents a rotation of A via the quaternion Q. So does QAQ^-1. If Q is a unit quaternion these become Q^*AQ and QAQ^*. Both representations are valid. The only distinguishing factor is that the quaternion Q appears in its unconjugated form on the left in QAQ^* but on the right in Q^*AQ. Hence the names left quaternion and right quaternion.

 

[Dave_fr]

Thank you very much for your answer.
I have red the NASA paper which is very interesting. Now, what I'm interested in is creating an attitude estimator for an UAV. I have already written one using quaternions and a PI corrector which works fine for me but apparently is not as good as a kalman filter algorithm would be.
I have sort of understood the concept of MRP.What I have to do is to work out euler angles from the "origin" quaterion which is "flat" and pointing north in an Earth refrence frame. Now I'm thinking that if I use MRP, the 3 MRP variables will overflow when heading south right ? The other solution I though of was to set the origin quaternion flat but pointing down that way, no singularities unless the airplane pitches up the point where the nose is vertical (theta=360°) which shouldn't happen.
You see I'm trying to fing a method that does not take too much computional power and is quite robust but I don't know what to go for...

 

[D H]

Regarding your quaternion-based filter,

• How are you propagating the quaternion? A simple Euler propagator, RK4, or something more exotic?
• Are you normalizing the quaternion with each step? If you aren't you had better use the quaternion inverse rather than the quaternion complex. Normalizing is also a bit suspect. It is ad-hoc and introduces false physics. Some have used Lagrange multipliers as a better approach. I don't have time to look for papers for you.

Regarding MRPs, the singularity is at 360 degrees. Force the angle to be between -180 and +180 and you will be fine (well, almost fine; this forcing will introduce some problems as well).

 

[Dave_fr]

Yes, I'm propagating the state forward using quaternion. I do also once every X time steps normalize the quaternions.
Do you think that working MRP and set the orign as the "down direction" would be the way to go (cf previous post) ?

Can you also tell me what are the real dangers to work with Euler angle in EKF directly ?

Thank you very much,

David

 

[pvm]

bit of a delay on this thread but...

I've implemented an indirect Kalman Filter for attitude estimation, where the KF estimates the error in orientation and error in gyro bias, rather than the orientation and bias itself (these are kept external to the filter). Works pretty well - no problems with singular covariance matrix from using quaternions in the state etc.

However, i only get to model the gyro bias (using qerrdot = -gyro x qerr - 0.5 * bias). I'd like to model the gyro scaling parameters too, perhaps even including cross axis (i.e. a full 3x3 scaling matrix). This makes it all non-linear though.

Don't suppose anyone has seen this worked though into a set of EKF equations, or can point me to any papers of people doing this...?