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} = q_{\mathrm{err}}(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 error metric drops below the convergence_threshold specified in the action goal.

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.

• Action name: /reference_filter
• Goal type: Waypoint (pose + mode + convergence threshold)

File truncated at 100 lines see the full file