Fiducial markers can be excellent resources in localization and mapping. When visible, they can provide relative position and orientation measurements. While fiducial markers remain remain visible and stationary, they can provide a quick and moderately robust SLAM solution.

Issues can arise, however, when performing online calibration of IMU and Camera sensors or when there are sufficiently large errors in the position and orientation of the fiducial.

As such, it can be beneficial to simultaneously estimate the pose of the fiducial alongside the localization of the moving body. This is done by introducing the fiducial pose states into the overall filter as

\begin{equation} \boldsymbol{x}_F = \begin{bmatrix} \pose{F}{L} & \quat{F}{L} \end{bmatrix} \end{equation}

Fiducial Error Model

The error model used for a fiducial position measurement is

\begin{align} \pose{f}{C} = R(\quat{C}{B})^T \left( R(\quat{B}{L})^T \left[ \pose{f}{L} - \pose{B}{L} \right] - \pose{C}{B} \right) + n_p \end{align}

where

\( \pose{f}C \) is the measured position of the fiducial in the camera frame,
\( \quat{C}{B} \) is the rotation from the camera frame to the body frame,
\( \quat{B}{L} \) is the rotation from the body frame to the local frame,
\( \pose{f}L \) is the position of the fiducial in the local frame,
\( \pose{C}L \) is the position of the camera in the local frame, and
\( n_p \) is the position Gaussian white noise process.

The error model used for a fiducial angular measurement is

\begin{align} \quat{f}{C} = \begin{bmatrix} \cos(n_\alpha) & -\sin(n_\alpha) & 0 \\ \sin(n_\alpha) & \cos(n_\alpha) & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \cos(n_\beta) & 0 & \sin(n_\beta) \\ 0 & 1 & 0 \\ -\sin(n_\beta) & 0 & \cos(n_\beta) \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos(n_\gamma) & -\sin(n_\gamma) \\ 0 & \sin(n_\gamma) & \cos(n_\gamma) \end{bmatrix} \quat{C}{B}^{-1} \quat{B}{L}^{-1} \quat{F}{L} \end{align}

where

\( \quat{f}{C} \) is the measured fiducial frame to camera frame rotation
\( n_\alpha \) is the yaw Gaussian white noise,
\( n_\beta \) is the pitch Gaussian white noise,
\( n_\gamma \) is the roll Gaussian white noise,
\( \quat{C}{B} \) is the rotation from the camera to the body frame,
\( \quat{B}{L} \) is the rotation from the body to the local frame, and
\( \quat{F}{L} \) is the rotation from the fiducial frame to the local frame