Omnidirectional kinematic model in robot_localization's EKF

Question

I am looking for a more detailed mathematical background of robot_localization's kinematic model used for nonlinear kalman filtering.

From the documentation, the robot state used is:

$$\mathcal{X} = [p^W, q^W, \dot{p}^L, \dot{q}^L, \ddot{p}^L]$$

Where $W$ is the world reference frame and $L$ the robot local reference frame.

Of course,

• $p^\top = [x,y,z]$ is the 3d position.
• $\dot{p}$, $\ddot{p}$ its first and second time derivative (vel, acc).
• $q^\top = [\phi, \psi, \theta]$ are the usual roll, pitch and yaw euler angles.
• $\dot{q}$ its time derivative.

By looking at the code for the position transition function, it seems it's implemented something of this fashion:

$$ p_{t+1}^W = p_t^W + \dot{p}_t^W\Delta t + \frac{1}{2} \ddot{p}_t^W \Delta t^2, \qquad $$

In this equation, linear velocity $\dot{p}$ and acceleration $\ddot{p}$ vectors are rotated from the robot's state variables, i.e. via a rotation matrix from robot frame $L$ to world frame derived from the estimated orientation of the robot $\hat{q}$, .

$$ p_{t+1}^W = p_t^W + R_t^{WL}(\hat{q}) \dot{p}_t^L\Delta t + \frac{1}{2}R_t^{WL}(\hat{q}) \ddot{p}_t^L \Delta t^2$$

The other transition funtions for the other states are: $$\dot{p}_{t+1} = \dot{p}_t + \ddot{p}_t\Delta t$$ $$\ddot{p}_{t+1} = \ddot{p}_t$$ $$\dot{q}_{t+1} = \dot{q}_t$$

However I am missing the reasoning behind euler angles $q$ evolution from angular rates $\omega$.

At first, I thought euler angles were simply integrated from a gyroscope's angular rates

$$ q_{t+1}^W = q_t^W + \omega_t^?\Delta t $$

But clearly it's not what's being used there.

I cant' explain these relations:

    transferFunction_(StateMemberRoll, StateMemberVroll) = delta;
transferFunction_(StateMemberRoll, StateMemberVpitch) = sr * tp * delta;
transferFunction_(StateMemberRoll, StateMemberVyaw) = cr * tp * delta;
transferFunction_(StateMemberPitch, StateMemberVpitch) = cr * delta;
transferFunction_(StateMemberPitch, StateMemberVyaw) = -sr * delta;
transferFunction_(StateMemberYaw, StateMemberVpitch) = sr * cpi * delta;
transferFunction_(StateMemberYaw, StateMemberVyaw) = cr * cpi * delta;

Thanks for the help!

Answer 1

I forgot that Euler angle rates $\dot{q}$ are of course NOT EQUAL to body angular rates $\omega$. $$\dot{q} \neq \omega$$ Their relation, given RPY rotation convention, is

$$ \begin{bmatrix} \dot{\phi}\\ \dot{\psi}\\ \dot{\theta} \end{bmatrix} = \overbrace{\begin{bmatrix} 1 & \sin(\phi) \tan(\psi) & \cos(\phi)\tan(\psi)\\ 0 & \cos(\phi) & -\sin(\phi) \\ 0 & \frac{\sin(\phi)}{\cos(\psi)} & \frac{\cos(\phi)}{\cos(\psi)} \end{bmatrix}}^{T} \begin{bmatrix} \omega_x\\ \omega_y\\ \omega_z \end{bmatrix} $$

The derivation implemented in the code is correct.

Although numerically unstable for pitch $\psi = \pi/2$ , should be fine for UGVs..

Calling $T$ this matrix, we have

$$q_{t+1}^W = q_t^W + T\omega_t^L \Delta t$$

However, robot_localization does NOT sample $\omega$ with a gyroscope.