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 State Update

The Kalman update step is performed in the typical fashion. The predicted measurement is

\begin{align} \hat{\boldsymbol{z}} = \begin{bmatrix} \quatHat{B}{C} (\quatHat{L}{B} (\poseHat{F}{L} - \poseHat{B}{L}) - \poseHat{C}{B}) \\ \quatHat{B}{C} \quatHat{L}{B} \quatHat{F}{L} \end{bmatrix} \end{align}

The measurement residual is

\begin{align} \boldsymbol{y} = \boldsymbol{z} - \hat{\boldsymbol{z}} \end{align}

The resultant observation matrix is

\begin{align} \boldsymbol{H} = \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0s \end{bmatrix} \end{align}

The typical Kalman update equations are used for the remainder of the update

\begin{align} \boldsymbol{S} = \boldsymbol{H} * \boldsymbol{P}_{k|k-1} * \boldsymbol{H}^T + \boldsymbol{R} \end{align}

\begin{align} \boldsymbol{K} = \boldsymbol{P}_{k|k-1} * \boldsymbol{H}^T * \boldsymbol{S}^{-1} \end{align}

\begin{align} \boldsymbol{x}_{k|k} = \boldsymbol{K} * \boldsymbol{x}_{k|k} \end{align}

\begin{align} \boldsymbol{P}_{k|k} = (\boldsymbol{I} - \boldsymbol{K} * \boldsymbol{H}) * \boldsymbol{P}_{k|k-1} * (\boldsymbol{I} - \boldsymbol{K} * \boldsymbol{H})^T + \boldsymbol{K} * \boldsymbol{R} * \boldsymbol{K}^T; \end{align}

Fiducial Error Model

The simulation 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 simulation 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