Using Virtual State Blocks

What is a Virtual State Block?

A VirtualStateBlock (VSB) is a class that converts a state estimate and covariance from one representation to another. These alternative state representations can then be used to simplify some operations that a user would normally need to do ‘manually’, such as producing solutions for output, forming error state updates, or doing lever arm and frame corrections between different points on a vehicle.

In NavToolkit, a StateBlock is a class that is used to represent some value being estimated, but it does not directly hold the actual estimate \(\mathbf{\hat{x}}\) and covariance \(\mathbf{P}\). Rather, these values are managed internally by a fusion engine like StandardFusionEngine and the estimate and covariance corresponding to a StateBlock are obtained by the user by (for example) querying the engine’s get_state_block_estimate() function using the StateBlock’s label.

A VirtualStateBlock enables the engine to represent existing states in a different way, by providing a mapping function \(\mathbf{f(\hat{x})}\) ( VirtualStateBlock::convert_estimate) and its Jacobian \(\mathbf{J}\) ( VirtualStateBlock::jacobian), the partial derivative of \(\mathbf{f(\hat{x})}\) evaluated at \(\mathbf{\hat{x}}\), \(\frac{\partial f(\mathbf{\hat{x}})}{\delta \mathbf{\hat{x}}}|_{\mathbf{\hat{x}}}\). Using these functions, the fusion engine computes the current value of the VirtualStateBlock's states on demand, based on the value of other states it’s estimating.

Comparing and Contrasting State Blocks and Virtual State Blocks

Neither StateBlocks nor VirtualStateBlocks actually contain a state estimate (\(\mathbf{\hat{x}}\)) or covariance (\(\mathbf{P}\)) – these come from the engine’s get_state_block_estimate() and get_state_block_covariance() respectively – but both implicitly define what ‘their’ state vector must look like via their respective functions’ outputs. For example, the Phi produced by generate_dynamics() implies the length, order, and units of the associated state vector by virtue of the length, order, and units of its diagonal elements. A similar argument applies to VirtualStateBlocks jacobian(), though two state vectors are involved in this case: the state vector to be transformed, and the result.

Both StateBlock and VirtualStateBlock are not usually interacted with directly after creation and passing to an engine, but are instead referenced using a string label.

While a StateBlock is referenced using the single label attached to it, a VirtualStateBlock has two labels:

1. current (for the source representation) referring to the StateBlock (or another VirtualStateBlock) that provides the input \(\mathbf{\hat{x}}\) and \(\mathbf{\hat{P}}\) to be converted.

2. target to refer to the converted values. When a filter implementation fully supports VirtualStateBlock, a user is able to retrieve the VirtualStateBlocks state and covariance representations \(\mathbf{\hat{x}_{vsb}}\) and \(\mathbf{P_{vsb}}\) by querying the filter using the target label.

The ‘core’ of each is a function that transforms the state vector, but the transformations are different. A StateBlock transforms the state vector over time; its g function takes an estimate \(x_t\) at a given time and returns \(g(x_t) \rightarrow x_{t+1}\), the expected estimate at a future time, keeping the semantic meaning of the states intact, including units and frames. For the VirtualStateBlock, the convert_estimate() function changes units, frame, or other semantic meaning of the states, but keeps the time of validity \(t\) intact.

How to Configure a Virtual State Block

To properly use a VirtualStateBlock:

1. Figure out what units/frames your desired VirtualStateBlock states need to be in.

2. Identify an estimate and covariance source to form the basis of the VirtualStateBlock. This can be from an existing StateBlock or another VirtualStateBlock.

3. Determine the current label to use. This will either be a StateBlock’s label or another VirtualStateBlock’s target and derived from the selection made in step 2.

4. Determine if an existing VirtualStateBlock can perform the conversion; if not, implement one.

5. Create the VirtualStateBlock and add it to the filter.

6. Get the VirtualStateBlock values as you would with a normal StateBlock, using the target label to retrieve them from the engine.

We have a number of off-the-shelf VirtualStateBlock s that handle common conversions relating to inertial error states, lever arm corrections and unit conversions, some of which are described in the examples below. You can also implement your own VSB.

Aside: Chaining

As the only prerequisite to form a VirtualStateBlock is that an existing basis state and covariance must be available, it is possible to ‘chain’ VirtualStateBlock conversions together. This allows a user to break down a more complicated conversion into smaller, more tractable constituents which has the additional benefit of making more VSB representations available. No special actions are needed to perform chaining outside of the steps outlined above; one just needs to ensure that VSB labels ‘link’ together and the output states of one match the expected input states of the other:

auto vsb1 = VirtualStateBlock("a", "b", ...);

// After added to filter can call filter.get_state_block_estimate("c"),
// which will return the result of vsb2.convert_estimate(vsb1.convert_estimate(filter.get_state_block_estimate("a")))
// (convert_estimate aspn_xtensor::TypeTimestamp arguments removed for clarity)
auto vsb2 = VirtualStateBlock("b", "c", ...);

Also, it is acceptable to build multiple VirtualStateBlocks off of the same source block, but having multiple ‘paths’ to a VirtualStateBlock is currently not supported:

auto vsb1 = VirtualStateBlock("pinson", "pva_at_imu", ...);

// OK
auto vsb2 = VirtualStateBlock("pva_at_imu", "just_position_states", ...);

// Cannot do this if vsb2 exists, because more than one path would
// provide the label just_position_states.
auto vsb3 = VirtualStateBlock("pinson", "just_position_states"...);

In other words, if one were to draw a graph of all StateBlock labels, VirtualStateBlock targets and VirtualStateBlock currents that are linked via chained VSBs, the graph may branch, but may not form a closed loop.

Fig. 1 Visualizing a chain of virtual state blocks as a graph. vsb3 is not allowed because vsb2 already exists.

Implementing a Custom Virtual State Block

A subclass of VirtualStateBlock must define mapping function \(\mathbf{f(\hat{x})}\) and the Jacobian \(\mathbf{J}\) by overriding the virtual methods convert_estimate() and jacobian(), respectively. These will be used by the fusion engine to generate the VirtualStateBlock’s state estimate \(\mathbf{\hat{x}_{vsb}}\) and covariance \(\mathbf{P_{vsb}}\).

Given an original estimate \(\mathbf{\hat{x}}\) and covariance \(\mathbf{P}\), which will be the fusion engine’s estimate and covariance for the current label, the VirtualStateBlock representations \(\mathbf{\hat{x}_{vsb}}\) and \(\mathbf{P_{vsb}}\) are calculated as:

\[\mathbf{\hat{x}_{vsb}} = \mathbf{f(\hat{x})} \label{eqn:vsb_fx}\]

\[\mathbf{P_{vsb}} = \mathbf{JPJ}^T \label{eqn:vsb_jac}\]

Implementing the convert_estimate() and jacobian() functions is similar to implementing a MeasurementProcessor::generate_model function. There, you define a function h and its Jacobian H that map a state estimate and covariance to a measurement vector z; with VirtualStateBlocks you define a function and Jacobian that maps a state estimate and covariance to an alternative state representation.

The convenience class FirstOrderVirtualStateBlock can be used instead of VirtualStateBlock and allows the user to supply only the mapping function \(\mathbf{f(\hat{x})}\). It uses numerical methods to calculate the Jacobian.

Some Examples

Scalar Unit Conversion

Assume we have a single state that is estimating speed over ground in units of meters-per-second (m/s), and a sensor that measures the same quantity but in feet-per-second (ft/s). We would normally just enable an update with this measurement by creating a new MeasurementProcessor and defining:

\[\begin{split}\mathbf{\hat{z}} = 3.28084\mathbf{\hat{x}} \\ H = 3.28084\end{split}\]

If the real StateBlock has a label "speed_meters", we can implement a new VirtualStateBlock as:

#include <memory>
#include <string>

#include <navtoolkit.hpp>

// Define a new VirtualStateBlock
class MetersToFeetVirtualStateBlock : public navtk::filtering::VirtualStateBlock {
public:
	MetersToFeetVirtualStateBlock(std::string current, std::string target)
	    : VirtualStateBlock(std::move(current), std::move(target)) {}

	navtk::Matrix jacobian(const navtk::Vector&, const aspn_xtensor::TypeTimestamp&) override {
		return navtk::Matrix{{3.28084}};
	}

	navtk::Vector convert_estimate(const navtk::Vector& x,
	                               const aspn_xtensor::TypeTimestamp&) override {
		return 3.28084 * x;
	}

	navtk::not_null<std::shared_ptr<VirtualStateBlock>> clone() override {
		return std::make_shared<MetersToFeetVirtualStateBlock>(*this);
	}
};

int main() {
	// Then create elsewhere
	auto vsb = MetersToFeetVirtualStateBlock("speed_meters", "speed_feet");
}

Or, alternatively using the FirstOrderVirtualStateBlock:

	std::function<navtk::Vector(const navtk::Vector&)> fx = [](const navtk::Vector& x) {
		return 3.28084 * x;
	};

	std::function<navtk::Matrix(const navtk::Vector&)> jx = [&](const navtk::Vector& x) {
		return navtk::Matrix{{3.28084 * x[0]}};
	};

	auto vsb = std::make_shared<navtk::filtering::FirstOrderVirtualStateBlock>(
	    "speed_meters", "speed_feet", fx, jx);

and then provide it to the filter:

	filter.add_virtual_state_block(vsb);

This now enables us to reference either the StateBlock using the "speed_meters" label, or the VirtualStateBlock with the target "speed_feet". Using the latter lets us use the off-the-shelf DirectMeasurementProcessor as the state and measurement units are the same:

	auto proc =
	    navtk::filtering::DirectMeasurementProcessor("proc", "speed_feet", navtk::Matrix{{1.0}});

In this case, the use of a VirtualStateBlock is likely overkill, but a more complicated relationship between the state and the measurement can benefit greatly from being broken into simple, composable transformations.

Vector Unit Conversion with FirstOrderVirtualStateBlock

We start by assuming that we have a StateBlock modeling 3 position states: latitude, longitude and altitude in radians, radians and meters. We wish to convert these to Earth-Centered-Earth-Fixed (ECEF) coordinates. We identify an existing navutils function, llh_to_ecef(), that can do this conversion. Luckily, it also accepts and returns Vector, which is the exact format we need (otherwise, we’d have to wrap the navutils function in a lambda or similar that accepts/returns Vector). We will avoid calculating the Jacobian by hand as it is fairly involved and just use the FirstOrderVirtualStateBlock:

auto vsb = navtk::filtering::FirstOrderVirtualStateBlock(
    "llh_pos", "ecef_pos", navtk::navutils::llh_to_ecef);

Error-State to Whole-State

A more interesting example occurs when we have an error state filter. In this case we are estimating the errors in some reference process, such as the position, velocity, attitude (PVA) and sensor errors in a ‘raw’ inertial integration. However, the error states themselves are not really the end goal of the process–we are usually interested in the ‘best’ PVA available, which is the reference inertial PVA solution with the estimated errors removed.

Taking our Pinson15NedBlock as an example state representation and our BufferedImu class as the source of the reference PVA, calculating the ‘best’ PVA at the IMU requires the following operations:

1. Get the reference PVA at the filter time

2. Convert the NED position errors in meters to LLH errors in radians, radians and meters, and add these to the reference position

3. Add the velocity error states to the reference velocity directly, as the units/frames already match

4. Form a skew-symmetric matrix from the NED tilt error states and use these to correct the reference NED-to-IMU DCM.

This combination of error state and reference PVA is going to be a pretty common operation–it’ll be the first step in generating output, will form the basis of many update functions, will be calculated in order to perform inertial feedback, etc. Because of this we provide the PinsonErrorToStandard VirtualStateBlock. However, evaluating this conversion relies on an outside source of data (the reference PVA), so we need to package up that source in a format the PinsonErrorToStandard can use:

	// Assume we have a starting MeasurementPositionVelocityAttitude 'pva' previously calculated

	// Create the filter and add the basic StateBlock
	auto engine       = navtk::filtering::StandardFusionEngine(pva.get_time_of_validity());
	auto imu_model    = navtk::filtering::sagem_primus200_model();
	auto pinson_block = std::make_shared<navtk::filtering::Pinson15NedBlock>("pinson", imu_model);
	engine.add_state_block(pinson_block);

	// Create the BufferedImu that we will use to process IMU data and provide
	// a reference PVA
	auto ins = navtk::inertial::BufferedImu(pva);

	// The reference PVA source is provided to the VSB as a generic function, so
	// the user is not beholden to the BufferedImu class. As such, we capture
	// the reference to the BufferedImu and convert its output to proper format
	auto nav_fun = [&](const aspn_xtensor::TypeTimestamp& time) {
		return navtk::utils::to_navsolution(*ins.calc_pva(time));
	};

	// We create the VSB using the label to the Pinson15NedBlock, the label we want
	// to associate with the VSB output, and the function that provides the reference PVA
	auto vsb1 = std::make_shared<navtk::filtering::PinsonErrorToStandard>(
	    "pinson", "standard_at_imu", nav_fun);

	// Then we add it to the filter
	engine.add_virtual_state_block(vsb1);

This example illustrates why convert_estimate() and related functions also accept a TypeTimestamp input; some conversions may rely on being able to sync up the state estimates and covariance at a given time with other time-varying sources of information.

A Typical Inertial Navigation Application

The final example pulls together the error-to-whole state and unit conversions that we’ve already examined with lever arm corrections and state-space reduction to provide a position-only state representation at a sensor frame origin different from the IMU sensor frame the Pinson error states are estimated in. This example assumes we have a vehicle outfitted with an IMU fixed inside the vehicle (a ‘strapdown’ inertial) and a GPS receiver with the antenna mounted on the roof. A semi-arbitrary ‘platform frame’ has been defined and the relative location of both the IMU and the GPS antenna are both constant and known with respect to this frame. We are using an error state filter to estimate the errors in the IMU generated PVA and using the GPS positions as updates to this filter.

To provide an accurate update to the error states, the input position measurement from the GPS must be differenced with a reference position of the same location. Normally the GPS measurement would be ‘shifted’ to correspond to the IMU frame, but a virtually equivalent measurement can be generated by moving the reference PVA to the GPS antenna frame. The steps required to do so are as follows:

1. Convert the error state units to match the reference PVA units

2. Add the error states to the reference PVA

3. Shift this corrected PVA to the platform frame. This usually involves doing vector addition in a Cartesian frame; we’ll use ECEF

4. Shift the PVA again from the platform frame to the GPS frame/antenna phase center

5. Convert the ECEF position back to LLH (or whatever units the GPS receiver provides)

6. (Optional) Extract just the position states

We’ve already covered steps 1 and 2 in the previous example using the PinsonErrorToStandard VSB. We also have existing VirtualStateBlock implementations to handle each other step in this process:

	// Assume we have previously created an instance of BufferedImu called "ins" and
	// StandardFusionEngine called "engine".

	std::function<navtk::filtering::NavSolution(const aspn_xtensor::TypeTimestamp& time)> nav_fun =
	    [&](const aspn_xtensor::TypeTimestamp& time) {
		    return navtk::utils::to_navsolution(*ins.calc_pva(time));
	    };

	// Define the lever arms/sensor rotations between the platform frame and the two sensors
	// Each is tagged with the target label of the VSB it is associated with
	navtk::Vector quat{1, 0, 0, 0};
	auto mount1 = aspn_xtensor::TypeMounting(
	    navtk::Vector3{1.0, -2.0, 0.0}, navtk::zeros(3), quat, navtk::zeros(3, 3));
	auto mount2 = aspn_xtensor::TypeMounting(
	    navtk::Vector3{0.0, 0.0, 4.0}, navtk::zeros(3), quat, navtk::zeros(3, 3));

	// Create a StateBlock and add it to the fusion engine.
	auto imu_model    = navtk::filtering::sagem_primus200_model();
	auto pinson_block = std::make_shared<navtk::filtering::Pinson15NedBlock>("pinson", imu_model);
	engine.add_state_block(pinson_block);

	// Create all of the VSBs. All of the less-important conversions are given single-letter 'dummy'
	// tags; using more meaningful signifiers is a good idea

	// This block is similar to PinsonErrorToStandard, but with a quaternion attitude representation
	auto vsb1 =
	    std::make_shared<navtk::filtering::PinsonErrorToStandardQuat>("pinson", "A", nav_fun);
	// Converts from LLH 'Standard' representation to ECEF units
	auto vsb2 = std::make_shared<navtk::filtering::StandardToEcefQuat>("A", "B");
	// Shifts the PVA to the platform frame
	auto vsb3 = std::make_shared<navtk::filtering::SensorToPlatformEcefQuat>("B", "C", mount1);
	// Shifts the platform frame PVA to the GPS sensor frame
	auto vsb4 = std::make_shared<navtk::filtering::PlatformToSensorEcefQuat>("C", "D", mount2);
	// Converts ECEF units back to LLH
	auto vsb5 = std::make_shared<navtk::filtering::EcefToStandardQuat>("D", "E");
	// Pulls out just the states of interest--the first 3. Must know the number of input states
	// (16- 3 pos, 3 vel, 4 quaternion, and 6 IMU sensor error)
	auto vsb6 = std::make_shared<navtk::filtering::StateExtractor>(
	    "E", "pos_at_gps", 16, std::vector<navtk::Size>{0, 1, 2});

	// Add all the VSBs to the filter
	engine.add_virtual_state_block(vsb1);
	engine.add_virtual_state_block(vsb2);
	engine.add_virtual_state_block(vsb3);
	engine.add_virtual_state_block(vsb4);
	engine.add_virtual_state_block(vsb5);
	engine.add_virtual_state_block(vsb6);

	// Get the ECEF PVA at the platform frame
	engine.get_state_block_estimate("C");

	// Get the position covariance at the gps frame
	engine.get_state_block_covariance("pos_at_gps");

We can now avoid having to write a specialized MeasurementProcessor to do all of the above in generate_model() to form an update. The fusion engine will take the model generated against the "pos_at_gps" VSB and ‘unwind’ it to form an equivalent model that works with the original StateBlock.

Pitfalls

VSBs are handy, but do have limitations. Some of them follow.

1. A VirtualStateBlock is not a full-up mimic of a StateBlock; a lot of filter-side functionality that works on StateBlocks will not support VirtualStateBlocks. Generally only estimate/covariance getter functions will work:

   • get_state_block_estimate()

   • get_state_block_covariance()

   • get_state_block_est_and_cov()

   • generate_x_and_p()

2. All VSBs are tied to a real StateBlock; removing said StateBlock from the filter will invalidate any VirtualStateBlock using it as a base.

3. Conversions that may have discontinuities must be implemented carefully to avoid unexpected behavior. If the derivative does not exist the Jacobian will be incorrect; ideally the implementation would be written in such a way as to produce a Jacobian that’s ‘close enough’. Numerically-derived Jacobians are especially prone to this pitfall; special handling of step sizes may be required. Be sure to document any failure cases that cannot be worked around.

4. Using a VirtualStateBlock is often slower and more computationally expensive than other methods. This is most pronounced when performing updates against a VirtualStateBlock, and is amplified when crossing programming language boundaries. If performance becomes an issue it may be better to push the conversion back into the MeasurementProcessor::generate_model function.

5. We often refer to being able to use a DirectMeasurementProcessor with VirtualStateBlock, but this is not always suitable. For instance, one would not do a direct update of roll, pitch, yaw Euler angles without additional logic (if one of the states is \(0\) and the update is \(2\pi\), these are equivalent and the state estimate shouldn’t be changed by the update, but using DirectMeasurementProcessor would cause the state to jump somewhere between \(0\) and \(2\pi\), depending on the covariances associated with state and measurement). Use of a VirtualStateBlock will not make issues such as these disappear.

6. As the VirtualStateBlock operates at the base level on raw Vector and Matrix inputs, clear documentation of state units, frames and ordering on VirtualStateBlock inputs and outputs is absolutely critical. This also applies to what a VirtualStateBlock does with ‘extra’ input states. For instance, most of our VSBs are written with a Pinson15NedBlock in mind as the ‘base state’, and this StateBlock has 15 states associated with it. The first nine are PVA related, and the last 6 are sensor errors. Only the first 9 are typically targets of any conversion, so what does one do with the 6 ‘trailing’ states? Whether they are kept or dropped during the conversion should be made obvious to potential users.