1
$\begingroup$

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!

$\endgroup$
0

1 Answer 1

2
$\begingroup$

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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.