# Omnidirectional kinematic model in robot_localization's EKF

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!

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.