State Modeling Plugin

The StateModelingPlugin is a factory plugin that provides components used in state modeling. State modeling is a catch-all term for how we represent quantities to be estimated (states), sensor measurements, and how they all relate to one another.

State modeling in pntos-python uses a multi-tiered factory structure that begins with the StateModelingPlugin. The plugin is used to generate one or more StateModelProviders. Each StateModelProvider is a collection of state modeling components that can be used to populate a fusion engine. Currently, pntos-python has one API-defined StateModelProvider called the StandardStateModelProvider. This class is also a factory, and as its name suggests, provides state modeling components for the pntOS Standard Model. This model includes:

StandardStateBlocks which define a set of states and provide a means to propagate them via the StandardDynamicsModel
StandardMeasurementProcessors which relate measurements to a set of states
VirtualStateBlocks which relate one set of states to another (transformation)

The next few sections will discuss particular points of the API and the existing Standard Model implementations in more depth.

State Modeling API

StateModelingPlugin

As mentioned above, the StateModelingPlugin itself is essentially a generator of StateModelProviders. It, like many of the elements described in this document, use the factory pattern. In addition to everything inherited from CommonPlugin, there are two additional functions: one that generates a new StateModelProvider of a specified type…

    @abstractmethod
    def new_state_model_provider(
        self, fusion_type: type[StateModelProviderType]
    ) -> StateModelProviderType | None:

…and one that reports if the plugin can generate a model of a given type:

    @abstractmethod
    def is_fusion_type_supported(
        self, fusion_type: type[StateModelProviderType]
    ) -> bool:

Both of these are generics that depend on the TypeVar StateModelProviderType:

StateModelProviderType = TypeVar(
    'StateModelProviderType', StandardStateModelProvider, Any
)

Note

There is no base class StateModelProvider that provides core elements required by any fusion approach. Rather a StateModelProvider is any class that:

Can provide a set of state modeling elements that satisfies the needs of some FusionEngine
Is a member of the TypeVar StateModelProviderType

Class entries in StateModelProviderType do not have to be related at all.

StateModelProvider

Just as the StateModelingPlugin is a factory that provides one or more types of StateModelProviders, a StateModelProvider is a factory that provides one or more elements that represent a piece of the fusion puzzle. For instance, the StandardStateModelProvider can provide StandardStateBlocks that represent states in a filter and their dynamics. Any fusion engine that understands StandardStateBlocks (or anything else a StandardStateModelProvider generates) may make use of any providers of this type.

StateModelProviders use the same typing approach as the StateModelingPlugin to describe the set of available models:

StateModelProviderType = TypeVar(
    'StateModelProviderType', StandardStateModelProvider, Any
)

Currently, the only explicitly defined StateModelProviderType is the StandardStateModelProvider. Each provider will be designed to work with a specific type of fusion approach and thus must be described individually. The StandardStateModelProvider is covered in the next section.

StandardStateModelProvider

The StandardStateModelProvider generates 3 types of objects that enable sensor fusion for the EKF and similarly structured estimators:

The StandardStateBlock models a fixed-size block of related values that need to be estimated, known as states. It implicitly defines the frames and units associated with each state via documentation. It is also a factory that provides, through the StandardDynamicsModel, the dynamics terms an EKF or similar estimator requires to propagate its states in time.
The StandardMeasurementProcessor is a factory that produces StandardMeasurementModels that relate a measurement to one or more StateBlocks, used in the filter update step.
The VirtualStateBlock is an optional convenience class that performs transforms of state vectors and covariances.

For each of the 3 types of objects the StandardStateModelProvider generates, it has a list of labels that refer to a specific implementation of that object and a bespoke generator function. For instance, the set of available MeasurementProcessors is described by the class member:

    processor_identifiers: list[str] | None

New processors are generated by evoking:

    @abstractmethod
    def new_processor(
        self,
        processor_index: int,
        engine: StandardFusionEngine | None,
        label: str,
        state_block_labels: list[str],
        config_group: str | None,
    ) -> StandardMeasurementProcessor | None:

Note that this function takes an int for the first argument to define which processor should be generated, rather than a str label. The proper argument is the location of the processor name in the processor_identifiers list, found via StandardStateModelProvider.processor_identifiers.index('my processor') or similar.

The other two objects are created in a like manner and their instantiation is not covered further here. Next, we’ll discuss the three objects the StandardStateModelProvider generates: StandardStateBlock, StandardMeasurementProcessor and VirtualStateBlock.

StandardStateBlock

The StandardStateBlock is responsible for defining a specific set of jointly Gaussian states (described by a state estimate vector and covariance matrix) and how those terms change with respect to time and other system inputs. Examples include a scalar temperature, a 3 dimensional point in space, or a collection of error terms associated with a physical sensor. Each StandardStateBlock has a fixed label that is used to refer to it and a num_states field that describes the length of the state vector associated with the StateBlock.

The primary use of a StandardStateBlock is to generate the StandardDynamicsModel that propagates a set of states forward in time.

    @abstractmethod
    def generate_dynamics(
        self,
        gen_x_and_p_func: GenXandP,
        time_from: TypeTimestamp,
        time_to: TypeTimestamp,
    ) -> StandardDynamicsModel | None:

Note

All discrete time equations in this document use the subscript shorthand \(t_k \rightarrow k\) for legibility.

In terms of the EKF propagation equations

\[ \begin{align}\begin{aligned} x_{k + 1} =x_k + \int^{k + 1}_k f(x_k, u_{k, k + 1}) dt = g(x_k, u_{k, k + 1})\\P_{k + 1} = \Phi_k P_k \Phi_k^T + \int^{k + 1}_k \Phi_k M_k Q_k M_k^T \Phi_k^T dt = \Phi_k P_k \Phi_k^T + Q_d \end{aligned}\end{align} \]

the StandardDynamicsModel provides \(g(x, u)\), \(\Phi\), and \(Qd\). The terms \(x_k\), \(t_k\) and \(t_{k + 1}\) all correspond to the arguments to generate_dynamics. Control (or any other) inputs \(u\) that \(g()\) requires are not explicitly passed to generate_dynamics but are provided through receive_aux_data():

    @abstractmethod
    def receive_aux_data(self, aux: list[Message | None]) -> None:

The StandardStateBlock is responsible for managing any aux data passed to it and ensuring the correct values are used in any given evaluation of generate_dynamics. Below is a diagram outlining construction of a StandardStateBlock, and data flow from the filter to the block in order to generate new StandardDynamicsModels.

StandardMeasurementProcessor

The StandardMeasurementProcessor is a class that defines how a measurement relates to one or more states being estimated. Similar to the StandardStateBlock, it features a label for identification and a receive_aux_data() to accept data required to generate its models. Additionally it has a state_block_labels field that tracks all the StateBlocks this processor can generate models against.

There are two important behaviors related to state_block_labels that are worth noting:

The order of the labels matters. If more than one label is present, then any model terms should be generated in the order that the labels appear in the list. The GenXandP functions must adhere to the same rule.
The list may be modified by the processor. For instance, a StandardMeasurementProcessor is free to add so-called nuisance states to the StandardFusionEngine via add_state_block(), in which case it should extend state_block_labels accordingly.

The StandardMeasuremementProcessor generates StandardMeasurementModels by way of the generate_model() function:

    @abstractmethod
    def generate_model(
        self, message: Message, gen_x_and_p_func: GenXandP
    ) -> StandardMeasurementModel | None:

In this case the function takes 2 inputs- a measurement of some kind, and a function that can provide the current estimate and covariance of the state blocks referred to in state_block_labels. If the processor can interpret the provided measurement and has been passed any additional required data through receive_aux_data then it may generate a StandardMeasurementModel which may be used to perform an update.

Note that it is permissible for a single MeasurementProcessor to process multiple types of measurement inputs; for example, generating an altitude update model from any ASPN measurement that contains an altitude or related value. It is also allowable to process similar measurements from multiple data sources. The only requirement is the measurement can be related to the states in state_block_labels. Care must be taken in this case if state_block_labels contains any sensor-specific states (e.g. lever arm, bias terms) that models do not incorrectly relate measurements to these states.

In terms of the EKF update equations

\[ \begin{align}\begin{aligned} K = PH^T(HPH^T + R)^{-1}\\x^+ =x^- + K(z - h(x^-))\\P^+ =P^- + KHP^- \end{aligned}\end{align} \]

the StandardMeasurementModel provides \(z\), \(h(x)\), \(H\) and \(R\).

Below is a diagram outlining construction of a StandardMeasurementProcessor, and data flow from the filter to the processor in order to generate new StandardMeasurementModels.

VirtualStateBlock

The purpose of a VirtualStateBlock is to provide a mapping between one standard state representation (Gaussian estimate and covariance) and another. Among other things, this allows measurement models that were written against a particular state representation to be used with other states, so long as a continuous mapping exists. For example, if a filter was tracking a state vector containing Earth-centered, Earth-fixed (ECEF) states, but a measurement model used latitude-longitude-altitude (LLA), a VirtualStateBlock that implemented the conversion from ECEF to LLA could be used to bridge the gap between them.

Mathematically speaking we are just decomposing functions into a compositions of functions. To illustrate, suppose a StandardMeasurementModel was available and provided the standard model terms with respect to some state vector x:

\[ \begin{align}\begin{aligned} z = h(x)\\H = \frac{\partial h}{\partial x}\rvert_{x=x0} \end{aligned}\end{align} \]

If the filter has a state representation y, and there exists a continuous and differentiable function that maps y to x

\[ x = g(y) \]

then the VirtualStateBlock can provide g(), which can be used to make h() a function of y:

\[ z = h(g(y)) \]

H can similarly be mapped using only the derivative of g(y) provided by the VirtualStateBlock due to the chain rule:

\[ \begin{align}\begin{aligned} \frac{\partial h}{\partial y} = \\\frac{\partial h}{\partial g(y)} \frac{\partial g}{\partial y} = \\\frac{\partial h}{\partial x} \frac{\partial g}{\partial y} = \\H\frac{\partial g}{\partial y}\rvert_{y=y0} = \\HG \end{aligned}\end{align} \]

Also due to the chain rule, multiple VirtualStateBlocks may be deployed in sequence to provide a series of mappings, e.g. \(x = g(f(e(y)))\)

To support these operations, a VirtualStateBlock must implement the following functions:

convert_estimate() maps one state vector representation to another; equivalent to \(g(y)\).

    @abstractmethod
    def convert_estimate(
        self, estimate: NDArray[float64], time: TypeTimestamp
    ) -> NDArray[float64]:

jacobian() returns the partial derivative of \(g(y)\) w.r.t. \(y\), a.k.a. \(G\):

    @abstractmethod
    def jacobian(
        self, estimate: NDArray[float64], time: TypeTimestamp
    ) -> NDArray[float64]:

convert() is a convenience function that not only maps \(x = g(y)\) but the associated covariance of \(y\) into \(x\)-space as well:

    @abstractmethod
    def convert(
        self,
        estimate_with_covariance: EstimateWithCovariance,
        time: TypeTimestamp,
    ) -> EstimateWithCovariance:

Finally, if \(g(y)\) requires any additonal inputs other than \(y\) to be evaluated, they may be passed in through the receive_aux_data() function:

    @abstractmethod
    def receive_aux_data(self, aux: list[Message | None]) -> None:

Note that use of VirtualStateBlocks is entirely optional and can contribute significant overhead in some cases. In resource-constrained applications, use of measurement processors that interact with non-virtual states directly is recommended.

Cobra Implementation: StandardStateModelingPlugin

The StandardStateModelingPlugin is a simple factory class that only provides objects the pntos.cobra.internal.StandardStateModelProvider, which is an implementation of the API class of the same name.

StandardStateModelProvider

As with the StandardStateModelingPlugin, the StandardStateModelProvider is just a factory that follows a pretty standard template. However, this class makes available a number of other classes that assist in modeling IMU errors. These are summarized in the following tables.

MeasurementProcessors

Class and Identifier	Accepted Measurements	Required StateBlocks	Description
`PinsonPositionMeasurementProcessor` pinson_position	MeasurementPosition (Geodetic)	pinson	Direct update of pinson position error states via the delta between input measurement and nominal (aux data) positions. Includes lever arm correction via configuration.
`PinsonVelocityMeasurementProcessor` pinson_velocity	MeasurementVelocity (NED)	pinson	Direct update of pinson velocity error states via the delta between input measurement and nominal (aux data) velocities. Measurement and nominal NED frames are assumed coincident.
`PinsonWithNedFogmPositionMeasurementProcessor` pinson_with_ned_fogm_position	MeasurementPosition (Geodetic)	pinson, fogm (3)	As `PinsonPositionMeasurementProcessor`, but with the addition of a 3-element FOGM to model arbitrary NED-frame measurement errors. Includes lever arm correction via configuration.
`AltitudeMeasurementProcessor` pinson_altitude	MeasurementPosition (Geodetic), MeasurementAltitude, MeasurementPositionVelocityAttitude(Geodetic)	pinson, fogm (1)	Generates an altitude error update via the delta between the measurement altitude and the nominal, with a FOGM-modeled altitude sensor bias in meters.
`PinsonWithLeverArmPositionMeasurementProcessor` pinson_with_lever_arm_position	MeasurementPosition (Geodetic)	pinson, fogm (3), fogm (3)	As `PinsonWithNedFogmPositionMeasurementProcessor`, but with the inclusion of an additional state block to estimate additional lever arm offset from the nominal value provided through configuration.
`PinsonBodyVelocityMeasurementProcessor` pinson_body_velocity	MeasurementVelocity (Sensor)	pinson	Updates pinson velocity error states using a velocity measurement in an arbitrary, platform-fixed sensor frame, where the lever-arm and orientation between the sensor and platform are known and provided through configuration.
`PinsonPosVelMeasurementProcessor` pinson_posvel	MeasurementPositionVelocityAttitude (Geodetic)	pinson	A processor that effectively joins `PinsonPositionMeasurementProcessor` and `PinsonVelocityMeasurementProcessor`.
`PositionMeasurementProcessor` position	MeasurementPosition (Geodetic)	PVA states, fogm (3)	A position update for whole-valued PVA states. Can be used with `pinson`-style error state blocks in conjunction with `PinsonErrorToStandard` VirtualStateBlock.
`Direction3DToPointsMeasurementProcessor` direction3D_to_points	MeasurementDirection3DToPoints	pinson	Updates pinson error states using the difference between the predicted and measured lateral and down units vectors pointing to features whose locations are known.

Note

In the above table

Required frames noted in parentheses refer to an ASPN23 frame, usually indicated by the reference_frame member of the message. If no frame is listed any are acceptable.
Numbers in parantheses indicate the required number of states a particular state block must have.
pinson refers to any state block that adheres to our typical pinson state layout, meaning 9 PVA related states followed by sensor error states. This means that one could provide an ‘extended’ pinson model that adds additional states to the end of the Pinson15NedBlock and the processor will still work.
PVA states means whole-valued states; see the linked processor for more details.

StateBlocks

Class	Identifier	Description
`Pinson15NedBlock`	pinson15	A state block that models INS error states.
`FogmBlock`	fogm	A configurable-length set of FOGM-modeled states.
`ClockBiasStateBlock`	clock_bias	Models a clock bias and 1-2 derivative terms.
`ConstantStateBlock`	constant	A configurable-length set of constant model states.

VirtualStateBlocks

Class	Identifier	Description
`PinsonErrorToStandard`	pinson_error_to_standard	Maps `pinson`-type state blocks to whole state estimates by combining the error states with the corresponding nominal PVA.
`StateExtractor`	state_extractor	Extracts a subset from a larger set of states, preserving frames, units etc.