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 | Science Articles, Homework Help, Discussion**

Dismiss Notice

Join Physics Forums Today!

The friendliest, high quality science and math community on the planet! Everyone who loves science is here!

The friendliest, high quality science and math community on the planet! Everyone who loves science is here!

# Extended Kalman Filter - Process noise and covariance

**Physics Forums | Science Articles, Homework Help, Discussion**