Reference filter (quaternion)

Third-order reference filter

The underlying filter is the third-order model (Fossen, 2021):

\dot{x}_d = A_d x_d + B_d r

where

A_d = \begin{bmatrix}
0_{n\times n} & I_n & 0_{n\times n} \\
0_{n\times n} & 0_{n \times n} & I_n \\
-\Omega^3 & -(2 \Delta + I_n) \Omega^2 & -(2 \Delta + I_n) \Omega
\end{bmatrix}

and

B_d = \begin{bmatrix}
0_{n \times n} \\
0_{n \times n} \\
\Omega^3
\end{bmatrix}.

The state is integrated using forward Euler: $x_{i+1} = x_i + \dot{x}_i \cdot dt$.

Error-state formulation

This package avoids the singularity issues of Euler angles by using a quaternion-based error state.

Nominal pose — a Pose (position + quaternion) representing the current best estimate of the reference trajectory. This is the actual output.

Error state — the 18D filter state, where the first 6 elements are small errors relative to the nominal:

x = \begin{bmatrix} \delta p \\ \delta \phi \\ \dot{\eta} \\ \ddot{\eta} \end{bmatrix} \in \mathbb{R}^{18}

Step cycle

Each time step performs three operations:

1. Compute reference error — The 6D reference $r$ is the error between the waypoint goal and the nominal pose:
   • $r_{0:3} = p_{goal} - p_{nominal}$
   • $r_{3:6} = \text{quaternion_error}(q_{nominal},\ q_{goal})$

   The quaternion_error function returns $2 \cdot \text{vec}(q_{nominal}^{-1} \otimes q_{goal})$, which for small angles approximates the rotation vector from nominal to goal.

2. Integrate — The standard filter step:

   \dot{x} = A_d x + B_d r, \quad x \leftarrow x + \dot{x} \cdot dt
   

1. Reset — Absorb position and orientation errors into the nominal pose, then zero them:
   • $p_{nominal} \leftarrow p_{nominal} + \delta p, \quad \delta p \leftarrow 0$
   • $q_{nominal} \leftarrow q_{nominal} \otimes \exp(\delta \phi), \quad \delta \phi \leftarrow 0$

   where $\exp(\delta \phi)$ converts the rotation vector to a quaternion via AngleAxis.

The reset step keeps $\delta p$ and $\delta \phi$ near zero at all times, ensuring the linearized quaternion error remains accurate. The velocity and acceleration states ($x_{6:18}$) are not reset — they carry over to provide smooth, continuous motion.

Output message (ReferenceFilterQuat)

Waypoint modes

Each waypoint has a mode that determines which degrees of freedom are controlled by the reference filter. The mode also determines how convergence is measured.

For all modes, convergence is reached when the position error norm is below the waypoint’s position_tolerance [m] and the orientation error norm below its orientation_tolerance [rad]. A tolerance left at 0 uses the parameter default_position_tolerance / default_orientation_tolerance. A tolerance for a part the mode does not control (for example orientation_tolerance with ONLY_POSITION) has no effect.

The tolerances must hold continuously for the waypoint’s hold_time_sec (default 0, converge at once) before the goal succeeds. A new goal sent while another is executing keeps the filter state and only changes the target and tolerances; the first goal after startup starts from the measured pose and twist.

Action Server

The action server handles goal requests and publishes guidance commands. The server always prioritizes new goal requests, aborting ongoing requests when a new one arrives.

File truncated at 100 lines see the full file