Introduction

What is NavToolkit?

NavToolkit (navtk) is a modular navigation software library, designed to assist users in the creation of navigation filters in an efficient, pluggable, agile manner. To do so, NavToolkit exposes an API for developing pluggable filter components:

  • State Blocks: A stochastic model for a set of unknown quantities of interest, represented in state-space form.

  • Measurement Processors: A description of the error model of a measurement from a sensor and instructions on how to relate the measurement to the state blocks.

  • Fusion Strategies: Contains the state estimates of the filter and uses models provided by StateBlocks and MeasurementProcessors to propagate and update the state estimates.

  • Fusion Engines: An online iterative sensor fusion engine which can take in a list of one or more StateBlocks and MeasurementProcessors as well as the raw measurements (as AspnBases) from a sensor and produce an estimate of all the states in every StateBlock at current, past, and (via error-propagation) future times. It is the master component which the user interacts with. It does bookkeeping, passes measurements to MeasurementProcessors, and the models from StateBlocks and MeasurementProcessors to the FusionStrategy.

To build a filter, you select a fusion engine, (e.g. StandardFusionEngine), a FusionStrategy (e.g. EkfStrategy), one or more StateBlocks (to represent all the states modeled by your filter), and one or more MeasurementProcessors (one for each kind of measurement you will be receiving). These components assembled together comprise your filter. The fusion engine which coordinates the components of the filter is then able to accept measurements from the sensors and your filter can start estimating your chosen states (jump straight to the Adding State/Sensor Support (C++) section to see an example).

How is NavToolkit Different than AFIT Scorpion or pntOS?

For an in-depth explanation of the differences between these systems, please see the Frequently Asked Questions.

Formal Description

NavToolkit is designed to estimate a set of time-varying values \(\mathbf{x}\) given a set of observations \(\mathbf{z}\) which contain information about \(\mathbf{x}\). Let \(\mathbf{x}_k\) be the Mx1 vector representing the value of \(\mathbf{x}\) at time \(t_k\) and \(\mathbf{z}_k\) be the Nx1 set of observations collected at \(t_k\). Then NavToolkit assumes that the way \(\mathbf{x}\) changes from one time epoch to the next is well-modeled by

\[\mathbf{x}_{k}=\mathbf{g}(\mathbf{x}_{k-1})\mathbf{+w}_{k},\ \ \ \mathbf{w}_{k}\overset{\mathrm{iid}}{\sim}N(0,\sigma_{w})\]

where \(\mathbf{g}\) is the discrete-time propagation function, and \(\mathbf{w}\) is a white Gaussian noise source. It also assumes that the observations are related to the state vector \(\mathbf{x}\) by

\[\mathbf{z}_{k}\mathbf{=h}(\mathbf{x}_{k})\mathbf{+v}_{k},\ \ \ \mathbf{v}_{k}\overset{\mathrm{iid}}{\sim}N(0,\sigma_{v})\]

where \(\mathbf{h}\) is the measurement model function, and \(\mathbf{v}\) is a white Gaussian noise source.

Estimators that are able to estimate problems modeled as above are known as filters. NavToolkit’s goal is to build filters that estimate \(\mathbf{x}\) at \(t_k\) (\(\mathbf{x}_{k})\) given the measurement at \(t_k\) (\(\mathbf{z}_{k}\)) along with all prior received measurements (\(\mathbf{z}_{k-1},\ldots)\). As measurements come in, we can continually refine our estimate of \(\mathbf{x}\) using the new information, calculating a new estimate of \(\mathbf{x}_{k+1}\) when the measurement \(\mathbf{z}_{k+1}\) is received, and so forth. The primary task a user of NavToolkit must complete is defining and describing \(\mathbf{g},\mathbf{w},\mathbf{h}\), and \(\mathbf{v}\) for their particular problem of interest.

In the special case of a linear model, these equations can be rewritten as:

\[\mathbf{x}_{k}=\mathbf{\Phi}_{k-1}\mathbf{x}_{k-1}\mathbf{+w}_{k},\ \ \ \mathbf{w}_{k}\overset{\mathrm{iid}}{\sim}N(0,\sigma_{w})\]

where \(\mathbf{\Phi}_{k-1}\) is the Jacobian matrix of \(\mathbf{g}(\mathbf{x}_{k-1})\), and the covariance of \(\mathbf{w}_{k}\) is \(\mathbf{Qd}_{k}\) (the discrete-time process noise covariance matrix), defined as \(\mathbf{Qd}_{k}= E[\mathbf{w}_{k} \mathbf{w}_{k}^{T} ]\).

\[\mathbf{z}_{k}=\mathbf{H}_{k}\mathbf{x}_{k}\mathbf{+v}_{k},\ \ \ \mathbf{v}_{k}\overset{\mathrm{iid}}{\sim}N(0,\sigma_{v})\]

where \(\mathbf{H}_{k}\) is the Jacobian matrix of \(\mathbf{h}(\mathbf{x}_{k})\), and the covariance of \(\mathbf{v}_{k}\) is \(\mathbf{R}_{k}\) (the measurement noise covariance matrix), defined as \(\mathbf{R}_{k}= E[\mathbf{v}_{k} \mathbf{v}_{k}^{T} ]\).

Targeted Problems

NavToolkit aims to be flexible enough to solve a large portion of real-world complimentary navigation problems, including:

  • Classic GPS/INS fusion.

  • Problems with highly non-linear \(\mathbf{h}\) functions, solved via Monte-Carlo methods.

  • Simultaneous Localization and Mapping (SLAM) problems, including live state insertion, deletion, and maintenance.

  • Post-processed non-causal solutions, including forward/backward smoothing.

  • Both inertial-present (error state) and inertial-free (direct state) sensor fusion approaches.