When available, GPS can provide a drift-free global position measurement.

Since these measurements are produced in a global frame, it is necessary to have a global to local frame transformation to utilize these measurements in a localization system.

Local Frame Initialization

A least squares method is used to initialize the global to local frame transformation. This requires a prior estimate of the GPS antenna in the body frame \( \pose{A}{B} \) as well as some form of odometry to predict or estimate body positions in the local frame without GPS-aiding.

Before the frame transformation is initialized, each GPS measurement is saved off along with the current estimate of the body position in the local frame. These measurement/estimate pairs are then used for the following routine.

Given desired translational and rotational errors thresholds, \( \epsilon_t \) and \( \epsilon_r \), it is possible to determine when it is best to perform a least squares estimate of the global to local frame rotation. Estimates of the translational and rotational errors are

Once these thresholds are reached, the Kabsch-TODO method of frame-to-frame estimation is used.

Online Calibration

Errors in the initial local frame heading estimate can cause issues with estimates of intrinsic sensor parameters over long trajectories. Therefore, it is also desirable to develop an online estimate of just the local frame heading to further improve accuracy with large distances traveled by the body. Additionally, it is desirable to further refine the estimated location of the GPS antenna in the body frame. As such, the local frame heading is incorporated into the state estimate alongside each antenna position in the body frame.

\begin{align} \boldsymbol{x}_G = \begin{bmatrix} \ang{G}{L} & \pose{A_1}{B} & \hdots & \pose{A_{N_G}}{B} \end{bmatrix} \end{align}

GPS Update Equations

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

\begin{align} \hat{\boldsymbol{z}} = \begin{bmatrix} \poseHat{L}{G} + \quatHat{L}{G} (\poseHat{B}{L} + \quatHat{B}{L} (\poseHat{A}{B})) \end{bmatrix} \end{align}

The measurement residual is

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

The resultant observation matrix is

GPS Error Model

The GPS error model used is

\begin{align} \begin{bmatrix} \phi_m \\ \lambda_m \\ h_m \end{bmatrix} = \begin{bmatrix} \phi \\ \lambda \\ h \end{bmatrix} + \begin{bmatrix} n_\phi \\ n_\lambda \\ n_h \end{bmatrix} \end{align}

where

\(\phi_m\) is the measured latitude,
\(\lambda_m\) is the measured longitude,
\(h_m\) is the measured altitude,
\(\phi\) is the true latitude,
\(\lambda\) is the true longitude,
\(h\) is the true altitude, and
\(n_\phi\), \(n_\lambda\), and \(n_h\) are Gaussian, white noise processes