Dear all,(adsbygoogle = window.adsbygoogle || []).push({});

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

**Physics Forums - The Fusion of Science and Community**

# Extended Kalman Filter - Process noise and covariance

Have something to add?

- Similar discussions for: Extended Kalman Filter - Process noise and covariance

Loading...

**Physics Forums - The Fusion of Science and Community**