Filter Construction & Usage Walkthrough

This section will walk you through building and running a filter. To get a complete and runnable version of this code with simulated measurements and data being received from multiple sensors over time, look in the examples folder of NavToolkit for the straight_flight_example.cpp. The README has step-by-step instructions on how to run this example.

Setting Up a Filter

The following headers will be included for this walkthrough:

#include <navtk/filtering/containers/GaussianVectorData.hpp>
#include <navtk/filtering/fusion/StandardFusionEngine.hpp>
#include <navtk/filtering/processors/DirectMeasurementProcessor.hpp>
#include <navtk/filtering/stateblocks/FogmBlock.hpp>
#include <navtk/filtering/stateblocks/Pinson15NedBlock.hpp>
#include <navtk/utils/conversions.hpp>

And the following using statements are included for convenience:

using aspn_xtensor::to_type_timestamp;
using aspn_xtensor::TypeTimestamp;
using navtk::eye;
using navtk::Vector;
using navtk::Vector3;
using navtk::zeros;
using navtk::filtering::DirectMeasurementProcessor;
using navtk::filtering::GaussianVectorData;
using navtk::filtering::hg1700_model;
using navtk::filtering::NavSolution;
using navtk::filtering::Pinson15NedBlock;
using navtk::filtering::Pose;
using navtk::filtering::StandardFusionEngine;
using navtk::filtering::StateBlock;
using std::string;
using std::vector;
using xt::diag;

The first thing to do is choose the fusion engine and FusionStrategy type that we want and make them. NavToolkit currently can implement an unscented Kalman filter or an extended Kalman filter depending on the chosen FusionStrategy type. For now we’ll build a fusion engine which uses an EKF. Since the default FusionStrategy for StandardFusionEngine is EKF, the default constructor can be used here:

auto engine = StandardFusionEngine();

The fusion engine we just created is initially empty, containing zero states to estimate and no way to process any measurements. The next step is to fill in the fusion engine with the set of states we want. Once the fusion engine has had MeasurementProcessors and StateBlocks added to it, it can act as a navigation filter.

In NavToolkit, a set of \(N\) states is represented by a StateBlock. The StateBlock contains details of how those states propagate forward through time. With NavToolkit we provide a set of premade StateBlocks. For this walkthrough we’ll create a Pinson15NedBlock which contains 15 states that model the errors of an INS in the North-East-Down (NED) frame:

auto model = hg1700_model();
auto block = std::make_shared<Pinson15NedBlock>("pinson15", model);

Notice that when we created the block, we had to give it two parameters: a label (the name of the StateBlock, in this case we’re calling it "pinson15") and a model. Examining the constructor of Pinson15NedBlock we see that the label is a unique identifier for this block that we’ll use to refer to it later on, and the model is an instance of ImuModel which describes the grade of the INS. In the example above we used a preset model for the HG1700 inertial, but we could just as easily have built a custom ImuModel by passing in the needed parameters manually.

To add our newly created Pinson block to our fusion engine, we can simply call the add_state_block() method of the fusion engine:

engine.add_state_block(block);

At this point, the fusion engine will add the 15 states from block to its internal FusionStrategy, initializing them to zero. If the initial covariance of your states are non-zero, you can tell the fusion engine what you’d like the Pinson states initialized to. To do so, construct a 15x15 matrix of the initial \(P_0\) matrix and pass them into the fusion engine’s set_state_block_covariance() method:

auto s0 = Vector{3,
                 3,
                 3,
                 0.03,
                 0.03,
                 0.03,
                 0.0002,
                 0.0002,
                 0.0002,
                 model.accel_bias_sigma(0),
                 model.accel_bias_sigma(1),
                 model.accel_bias_sigma(2),
                 model.gyro_bias_sigma(0),
                 model.gyro_bias_sigma(1),
                 model.gyro_bias_sigma(2)};
auto P0 = diag(s0 * s0);
engine.set_state_block_covariance("pinson15", P0);

Notice that we passed in the label "pinson15". This is how the fusion engine knows that we are updating the covariance of the Pinson states. If our fusion engine had two blocks named "a" and "b", we could choose which one we wanted to update the covariance of by passing in either "a" or "b" here along with the matrix of values. If we wanted to set a block’s estimate (as opposed to its covariance) to something, we could use the set_state_block_estimate() method in a similar manner as we did here, but passing in a Vector of length 15 instead (since our state block has 15 states).

The fusion engine now has a state block loaded and it knows the initial conditions of the state block. For most state blocks, we would now be ready to propagate the fusion engine forward, estimating the states in "pinson15" as they change over time. However, the Pinson15NedBlock requires a bit of additional information in order to estimate inertial errors accurately. The errors of an INS change as a function of the vehicle’s motion. In particular, to estimate the errors the INS will contain, the Pinson block must approximately know the vehicle’s position, rotation, velocity, and specific force. In order to pass this sort of additional information to the Pinson block, we can call the fusion engine’s give_state_block_aux_data() method, passing in the label of the receiving StateBlock and the required data.

In order to know if a given state block needs aux data and what sort of aux data you need to give it, you can consult its documentation. In the case of the Pinson15NedBlock we can see that it requires an aux data of the type PinsonAux which contains specific force and a NavSolution, which is a structure containing the position, rotation, and current time. Thus we will pass the current pose and specific force to our Pinson block:

auto nav_sol =
    NavSolution{Vector3{0.0, 0.0, 0.0},  // Position in lat (rad), lon (rad), alt (m).
                Vector3{1, 0, 0},        // NED Velocity, m/s
                eye(3),                  // Attitude DCM
                aspn_xtensor::to_type_timestamp()};  // Time, seconds
auto aux_data = navtk::utils::to_inertial_aux(nav_sol, Vector3{0.0, 0.0, -9.81}, zeros(3));
engine.give_state_block_aux_data("pinson15", aux_data);

Propagating a Filter

The filter (via the fusion engine) now has all the information it needs to propagate forward estimates of inertial error over time. To do so, we call the fusion engine’s propagate() method:

engine.propagate(to_type_timestamp(1, 0));

The fusion engine starts out at time 0.0 by default, so the call above will propagate the fusion engine’s estimate one second into the future. If we wanted to get the estimate of our Pinson block after propagation, we could call

auto states     = engine.get_state_block_estimate("pinson15");
auto covariance = engine.get_state_block_covariance("pinson15");

which will return the 15-element Vector state estimate and 15x15 estimate covariance matrix, respectively.

A subsequent call to propagate(to_type_timestamp(3.0)) would propagate the filter to t=3 seconds, propagate(to_type_timestamp(5.0)) to t=5 seconds, and so on. The selected times do not need to be evenly spaced or integers as the state block will provide the information to the FusionStrategy on how to update the states for an arbitrary time. Note that the aux data should be refreshed as often as possible (by calling give_state_block_aux_data()) when using INS error states, as doing so is critical to get a correct estimate.

Updating a Filter with Measurements

By default, the filter knows nothing about how to process measurements coming from sensors. In order to learn, the fusion engine must be given one or more MeasurementProcessor, which are objects that contain the algorithms to process measurements coming from a particular sensor type. NavToolkit contains some off the shelf MeasurementProcessor implementations for common sensor types, and allows users to design custom ones for specific algorithms by implementing the MeasurementProcessor interface themselves. For this walkthrough, we’ll add a DirectMeasurementProcessor, configured to update the vehicle’s altitude, to the fusion engine. First we build an instance of the processor:

auto h_alt     = zeros(1, 15);
h_alt(0, 2)    = 1.0;
auto processor = std::make_shared<DirectMeasurementProcessor>("altimeter", "pinson15", h_alt);

The first parameter is the name (label) of the processor itself (so we can refer to it later on), the second parameter is the name of the state block it will relate its measurements to, and the third is a Matrix which maps the states to the measurement space. In this case, we want the measurement processor to take the altitude measurements it receives and update our estimate of the Pinson states, so we pass in the label of the Pinson states here and a 1x15 Matrix of zeros except for a one at the index corresponding to the vertical position state.

Next we add the processor to the fusion engine:

engine.add_measurement_processor(processor);

At this point the fusion engine has been informed of how to process altitude measurements from a sensor, so we can proceed with sending a measurement to the fusion engine:

auto reference_altitude   = nav_sol.pos[2];
auto measurement_altitude = 0.5;
engine.update(
    "altimeter",
    std::make_shared<GaussianVectorData>(to_type_timestamp(10, 0),
                                         Vector{reference_altitude - measurement_altitude},
                                         navtk::Matrix{{BARO_SIGMA * BARO_SIGMA}}));

We call the fusion engine’s update() method, which requires the label of the receiving MeasurementProcessor and the measurement (AspnBase) as parameters.

Thus, in our code above, we told the filter we have a measurement at time t=10.0, with a raw value of the reference altitude minus 0.5, and a measurement covariance of BARO_SIGMA squared. The processor_label passed to update was set to "altimeter" which is the name of the measurement processor that was previously added to the fusion engine.

It was necessary to difference the measured altitude against a reference altitude because we are updating an error-state filter in this case. An error state contains the estimate of the difference between a ‘true’ value and some system’s best guess at the same value- in this case it is estimating the level of error of our simulated reference altitude. To update this state, the MeasurementProcessor must be able to form a delta measurement, which is the difference between the measured altitude and the reference altitude.

When the fusion engine receives the above measurement, it will first note that the timestamp of 10.0 doesn’t match the current filter time (remember, we propagated to t=1.0 previously). The first thing it will do is propagate the estimate forward to the measurement’s time stamp.

Next, it will examine the processor_label and determine that the correct measurement processor to process the measurement is the "altimeter" processor we added earlier. The fusion engine will then pass the measurement to that processor, which will in turn generate the matrices and functions the FusionStrategy needs to perform the update. After the call to update(), the filter’s current estimate will have incorporated the information in the measurement, and calls to get_state_block_estimate() and get_state_block_covariance() will produce the post-update estimate and covariance respectively.

Adding a Bias State

The filter we just set up only contains the 15-state INS error model states. However, it is common to model a barometric altitude sensor as having a bias on the measurements it collects. Because NavToolkit is designed to be modular and (live) pluggable, it is straightforward to modify how we modeled the baro update without touching other parts of the filter (e.g. the inertial error states).

The first step is to add a new state block to the fusion engine that contains a single state representing our baro’s bias. We’ll model the bias here as a First-Order Gauss-Markov bias (FogmBlock):

auto baro_block = std::make_shared<navtk::filtering::FogmBlock>("baro_bias", 50.0, 10.0, 1);

Note that we’ve given this state block the name "baro_bias" with a time constant of 50, a state sigma of 10, and set it to one state. Now we add the new bias state block to the fusion engine:

engine.add_state_block(baro_block);

The fusion engine now contains two state blocks: the original Pinson15NedBlock and the FogmBlock. At this point you could set the new state block’s initial estimate and covariance as done with the Pinson15NedBlock above.

We previously added a DirectMeasurementProcessor to the fusion engine, which didn’t know anything about a bias state and thus won’t use our new "baro_bias" state. To rectify that situation, we’ll need to add another DirectMeasurementProcessor configured to update both the altitude and bias state:

auto h_alt_bias       = zeros(1, 16);
h_alt_bias(0, 2)      = 1.0;
h_alt_bias(0, 15)     = 1.0;
auto biased_processor = std::make_shared<DirectMeasurementProcessor>(
    "biased_altimeter", vector<string>{"pinson15", "baro_bias"}, h_alt_bias);
engine.add_measurement_processor(biased_processor);

Originally our filter only contained the 15 states from the Pinson15NedBlock, but now it has an additional bias state, bringing the total to 16 states. While the first MeasurementProcessor only mapped the third state (the Pinson15NedBlock vertical position state) to the measurement, this MeasurementProcessor also maps to the 16th state, which is the bias state we added. Also in contrast to the original processor, note that this one is constructed using a vector of StateBlock labels. The name (label) of the new processor is "biased_altimeter", which is how we’ll refer to this processor in the future.

We now have a filter that is capable of processing biased altimeter measurements. When we send a new measurement to the fusion engine, we decide which measurement processor the measurement is destined for. For example, if we write:

reference_altitude   = nav_sol.pos[2];
measurement_altitude = 0.5;
engine.update(
    "biased_altimeter",
    std::make_shared<GaussianVectorData>(to_type_timestamp(11, 0),
                                         Vector{reference_altitude - measurement_altitude},
                                         navtk::Matrix{{BARO_SIGMA * BARO_SIGMA}}));

we would be telling the fusion engine that a new biased measurement of altitude is available at time t=11.0 and that it should be sent to the "biased_altimeter" measurement processor for processing. However, if we received an unbiased altimeter measurement at time t=15 (say from some other altimeter sensor that didn’t have a bias) we could still send it to the previously added "altimeter" processor:

reference_altitude   = nav_sol.pos[2];
measurement_altitude = 0.5;
engine.update(
    "altimeter",
    std::make_shared<GaussianVectorData>(to_type_timestamp(15, 0),
                                         Vector{reference_altitude - measurement_altitude},
                                         navtk::Matrix{{BARO_SIGMA * BARO_SIGMA}}));

All processors added to the fusion engine will remain available for the lifetime of the fusion engine, which is also true of all state blocks added to the fusion engine (unless you remove them). Thus you can add blocks and processors at will to model any number of states or measurements that you need.

Next Steps

In this walkthrough we showed the basics of building a filter, propagating it, and updating it with measurements. In the StraightFlightExample source code example, we expand on this walkthrough code to demonstrate a filter that runs on simulated measurements that are fed over time, with three different measurement types optionally enabled.

The Adding State/Sensor Support (C++) and Adding State/Sensor Support (Python) sections illustrate how to make your own custom StateBlocks and MeasurementProcessors in C++ and Python, respectively.