- #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:
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_tfor 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)<br />
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
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_tfor 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)<br />
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
