Adding State/Sensor Support (C++)

While NavToolkit has a few measurement processors and state blocks available off the shelf, you’ll most likely want to build your own for custom states or sensors. This section will discuss the process of creating one of each of these in C++. For more information on creating a state block or measurement processor from Python, see Adding State/Sensor Support (Python).

Creating the StateBlock

The StateBlock represents a block of one or more states, along with their dynamics over time. To create a new state block, simply subclass StateBlock with your own implementation.

When a fusion engine is asked to propagate() forward its estimates in time, it will in turn ask each of the state blocks that have been added to it for information on how the block’s states should be propagated. It does this by calling the block’s generate_dynamics() function, which produces the information the fusion engine needs to provide to the FusionStrategy to perform the propagation.

Let’s take a look at an example block that we might write. Suppose we wanted a StateBlock with one state which has constant dynamics. Then we might write BiasBlock.hpp:

#pragma once

#include <memory>

#include <navtk/filtering/GenXhatPFunction.hpp>
#include <navtk/filtering/containers/StandardDynamicsModel.hpp>
#include <navtk/filtering/stateblocks/StateBlock.hpp>
#include <navtk/not_null.hpp>
#include <navtk/tensors.hpp>

class BiasBlock : public navtk::filtering::StateBlock<> {
public:
	virtual ~BiasBlock() = default;

	BiasBlock(const std::string& label) : navtk::filtering::StateBlock<>(1, label) {}

	navtk::not_null<std::shared_ptr<StateBlock<>>> clone() override {
		return std::make_shared<BiasBlock>(get_label());
	}

	// This function takes in the current state estimate and time
	// and produces the needed dynamics equation parameters (g, Phi, Qd)
	navtk::filtering::StandardDynamicsModel generate_dynamics(
	    navtk::filtering::GenXhatPFunction,
	    aspn_xtensor::TypeTimestamp time_from,
	    aspn_xtensor::TypeTimestamp time_to) override {

		// Define the g(x) propagation function as g(x_(k)) = x_(k-1)
		auto g = [](navtk::Vector x) { return x; };

		auto delta_time = to_seconds(time_to - time_from);

		navtk::Matrix Phi = {{1}};
		navtk::Matrix Qd  = {{delta_time}};

		return navtk::filtering::StandardDynamicsModel(g, Phi, Qd);
	}
};

The StateBlock constructor takes in two parameters: the number of states you have and the label of your block. Here we pass it 1, label for those parameters respectively. We receive the label from the user in our own constructor and pass it through to the super constructor.

You’ll also notice our implementation of StateBlock has two methods implemented:

  • clone(): This is a method which is generally only used for advanced functionality, but it can also be used as a convenient way to duplicate StateBlocks. Note that since it returns a copy of itself with the same label, we would either have to change the label of the cloned StateBlock or add it to a different fusion engine.

  • generate_dynamics(): When the fusion engine needs to propagate forward the states this block represents, it will call this method. The method is given the current state estimate (xhat), the time we are propagating from, and the time we are propagating to. It is required to return an instance of StandardDynamicsModel, which is a structure containing all the information the fusion engine needs to calculate the propagated estimates. Recalling the formulation of the problem from the Introduction, dynamics contains the function g (which returns the matrix product of xhat and a 1x1 identity matrix), the first-order Taylor series approximation of g named \(\Phi\), and the covariance matrix of \(v_k\) named \(Q_d\). In the code above, we fill out the dynamics struct with a function g that returns its input, \(\Phi\) as the identity matrix, and a covariance equal to the elapsed time on the dynamics noise.

Other things to note:

  • Our BiasBlock requires a unique label associated with it. This is important as its uniqueness makes it possible to query the fusion engine for blocks by label. If a fusion engine has multiple BiasBlocks added to it they should each have a different label.

  • All of the matrices generate_dynamics() inputs or outputs are only with respect to the states that belong to the StateBlock. For example, the xhat argument is guaranteed to be a 1-length vector consisting of only the bias state, even if other states are in the fusion engine from other sensors. This approach makes it easier to write modules in isolation, as adding new modules to a fusion engine won’t affect the input/output of the individual modules. This also means that it is generally recommended that any states that have coupled dynamics be a part of the same StateBlock.

  • If our StateBlock required data to be brought in through a side channel, we could have implemented receive_aux_data()

Creating the MeasurementProcessor

A MeasurementProcessor contains the information a FusionStrategy needs to process a specific type of raw measurement that it receives. When someone calls a fusion engine’s update() method, the fusion engine will look at the processor_label parameter and try to find a MeasurementProcessor with the same label that it can send the measurement to for processing. If it finds one, the fusion engine will send the measurement to the processor, calling its generate_model() method which returns the information the FusionStrategy needs to incorporate the measurement. The processor’s job is then to receive the measurement from the fusion engine and extract the information from it that the FusionStrategy needs to perform an update.

Let’s look at an example of a simple processor implementation. We’ll create a processor object for a measurement of the bias state, called BiasMeasurementProcessor.hpp:

#pragma once

#include <memory>

#include <navtk/aspn.hpp>
#include <navtk/filtering/GenXhatPFunction.hpp>
#include <navtk/filtering/containers/GaussianVectorData.hpp>
#include <navtk/filtering/processors/MeasurementProcessor.hpp>
#include <navtk/not_null.hpp>

class BiasMeasurementProcessor : public navtk::filtering::MeasurementProcessor<> {

public:
	navtk::Matrix measurement_matrix;

	BiasMeasurementProcessor(std::string label, const std::string &state_block_label)
	    : MeasurementProcessor(std::move(label), state_block_label),
	      measurement_matrix(navtk::Matrix{{1.0}}) {}

	std::shared_ptr<navtk::filtering::StandardMeasurementModel> generate_model(
	    std::shared_ptr<aspn_xtensor::AspnBase> measurement, navtk::filtering::GenXhatPFunction) {

		std::shared_ptr<navtk::filtering::GaussianVectorData> gvd =
		    std::dynamic_pointer_cast<navtk::filtering::GaussianVectorData>(measurement);

		navtk::Vector z = gvd->estimate;
		navtk::Matrix H = measurement_matrix;
		navtk::Matrix R = gvd->covariance;

		return std::make_shared<navtk::filtering::StandardMeasurementModel>(
		    navtk::filtering::StandardMeasurementModel(z, H, R));
	}

	navtk::not_null<std::shared_ptr<MeasurementProcessor<>>> clone() {
		return std::make_shared<BiasMeasurementProcessor>(get_label(), get_state_block_labels()[0]);
	}
};

Implementing a MeasurementProcessor subclass requires you to implement two methods:

  • clone(): This is a method which is generally only used for advanced functionality, but it can also be used as a convenient way to duplicate MeasurementProcessors. Note that since it returns a copy of itself with the same label, we would either have to change the label of the cloned MeasurementProcessors or add it to a different fusion engine.

  • generate_model(): This method is passed in the measurement coming from the sensor along with the current snapshot of the FusionStrategy’s estimate and estimate covariance. It is required to produce the processed measurement (\(\mathbf{z}\)) and measurement model (\(\mathbf{h,H,R}\)) which will be be passed into the FusionStrategy. In our example, we’ll not need to process the measurement further so we’ll simply pass through the raw measurement.

Other things to note:

  • Our BiasMeasurementProcessor requires a label to uniquely identify it, as well as an array of labels of StateBlocks it relates to. When generate_model() is called, its input/output will be the set of concatenated states from the set of states in this list. This allows measurement processors to be written independently of the other sensors in the fusion engine, but still be able to relate the measurement to any states as needed.

  • If our BiasMeasurementProcessor required data to be brought in through a side channel, we could have implemented receive_aux_data().

Using the StateBlock and MeasurementProcessor with a Fusion Engine

Once we have a library of modules for the states and measurements we are interested in, using them with a fusion engine is simple. Consider the file bias_example_with_update.cpp:

#include <memory>

#include <spdlog/fmt/fmt.h>
#include <spdlog/fmt/ostr.h>
#include <spdlog/spdlog.h>

#include <navtk/factory.hpp>
#include <navtk/filtering/fusion/StandardFusionEngine.hpp>
#include <navtk/tensors.hpp>

#include "BiasBlock.hpp"
#include "BiasMeasurementProcessor.hpp"

using aspn_xtensor::to_type_timestamp;

int main() {
	// Create our state block, measurement processor, and fusion engine instance. All together these
	// act as a navigation filter.
	auto block     = std::make_shared<BiasBlock>("my_bias_block");
	auto processor = std::make_shared<BiasMeasurementProcessor>("my_processor", "my_bias_block");
	auto engine    = navtk::filtering::StandardFusionEngine();

	// Add the block and processor to the engine/filter
	engine.add_state_block(block);
	engine.add_measurement_processor(processor);

	// Get and print the initial state estimate and covariance
	auto out     = engine.get_state_block_estimate("my_bias_block");
	auto out_cov = engine.get_state_block_covariance("my_bias_block");
	spdlog::info("Initial:");
	spdlog::info("The state estimate is {}", out);
	spdlog::info("The state covariance is {}\n", out_cov);

	// Propagate to 0.1 seconds
	engine.propagate(to_type_timestamp(0.1));

	// Get and print the state estimate and covariance after propagation
	out     = engine.get_state_block_estimate("my_bias_block");
	out_cov = engine.get_state_block_covariance("my_bias_block");
	spdlog::info("After propagation:");
	spdlog::info("The state estimate is {}", out);
	spdlog::info("The state covariance is {}\n", out_cov);

	// Make a measurement at 0.1 seconds of sensed value 1.0 with cov 1.001
	aspn_xtensor::TypeTimestamp time_validity = to_type_timestamp(0.1);
	navtk::Vector measurement_data            = {1.0};
	navtk::Matrix measurement_covariance      = {{1.001}};
	auto raw_meas = std::make_shared<navtk::filtering::GaussianVectorData>(
	    time_validity, measurement_data, measurement_covariance);

	// Update the filter estimate with the measurement
	engine.update("my_processor", raw_meas);

	// Get and print the state estimate and covariance after the update
	out     = engine.get_state_block_estimate("my_bias_block");
	out_cov = engine.get_state_block_covariance("my_bias_block");
	spdlog::info("After update:");
	spdlog::info("The state estimate is {}", out);
	spdlog::info("The state covariance is {}", out_cov);

	return EXIT_SUCCESS;
}

If you’d like to try this example out you can find it (as well as the StateBlock and MeasurementProcessor) in the examples/bias_with_update directory. It can be compiled and run by:

ninja -C build run_bias_example_with_update