Extended Kalman Filter - Process noise and covariance

In summary, the author is trying to implement an Extended Kalman Filter for position, velocity and orientation tracking of a rigid body and he is using quaternions for representing the orientation in the state vector. As he is the first time he has to work with a Kalman Filter, and the project is on a tight time-schedule, he did not have time to delve into Kalman Filter theory or to let the knowledge he has acquired so far sink in. He has provided a summary of the content.
  • #1
Rares
1
0
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:

[tex]x(p^{e},\dot{p}^e,q^{be})[/tex]

The state transition functions are:

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

with [tex]\odot[/tex] being the quaternion product and T the sampling interval.

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

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

where [tex]g^e [/tex] is the gravity vector in the global reference frame, and [tex]e_{acc}, e_{\omega}[/tex] 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:

[tex] P^{-}_t = A_tP_{t-1}A_t^T + W_tQ_{t-1}W_t^T [/tex]

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

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

[tex]A_t=\left(
\begin{array}{ccc}
I_3 & TI_3 & 0 \\
0 & I_3 & 0\\
0& 0 & e^{-\frac{T}{2}\omega^b_t} \\
\end{array}
\right)
[/tex]

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 [tex]Q[/tex] and [tex]W[/tex] matrices.
Any help is greatly appreciated.
Thanx in advance,
Rares
 
Engineering news on Phys.org
  • #2
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
 
  • #3
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 ?
 
  • #4
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. 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 Regarding left versus right quaternions.
Consider a similarity transformation of a matrix. http://mathworld.wolfram.com/SimilarityTransformation.html" [Broken] 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.
 
Last edited by a moderator:
  • #5
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...
 
  • #6
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).
 
  • #7
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
 
  • #8
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...?
 

1. What is an Extended Kalman Filter (EKF)?

An Extended Kalman Filter is a state estimation algorithm used to estimate the state of a nonlinear system by incorporating a linearization process. It is an extension of the Kalman Filter, which is only applicable to linear systems.

2. How does an EKF handle process noise and covariance?

An EKF uses a process noise vector and a covariance matrix to account for the uncertainty in the system's dynamics. The process noise vector represents the unknown disturbances that affect the system, while the covariance matrix represents the uncertainty in the estimation of the system's state.

3. How are the process noise and covariance values determined for an EKF?

The process noise and covariance values are typically determined empirically or based on prior knowledge of the system. They can also be tuned during the estimation process to improve the filter's performance.

4. Can an EKF handle both additive and multiplicative process noise?

Yes, an EKF can handle both additive and multiplicative process noise. Additive noise is directly added to the state vector, while multiplicative noise is incorporated into the state transition matrix.

5. What are the limitations of an EKF in handling process noise and covariance?

An EKF assumes that the system is Gaussian and that the process noise and covariance are constant. This may not always be the case in real-world systems, leading to suboptimal performance. Additionally, if the process noise and covariance values are not accurately estimated, the filter's performance may be affected.

Similar threads

  • Engineering and Comp Sci Homework Help
Replies
1
Views
710
  • Engineering and Comp Sci Homework Help
Replies
1
Views
774
  • MATLAB, Maple, Mathematica, LaTeX
Replies
2
Views
2K
  • General Engineering
Replies
2
Views
2K
Replies
1
Views
758
Replies
23
Views
1K
  • High Energy, Nuclear, Particle Physics
Replies
3
Views
845
  • Advanced Physics Homework Help
Replies
9
Views
1K
Replies
1
Views
533
  • Classical Physics
Replies
1
Views
1K
Back
Top