A Rao-Blackwellized point-mass filter: fixed points for one block of the state, such as a coarse position, each carrying a Kalman filter over the rest. More...

#include <EmrysFilter/Filters/RbPointMassFilter.h>

Inheritance diagram for Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >:

Public Types
using	GridType = Grid_
	the fixed points
using	Scalar = typename Grid_::Scalar
	the scalar type
using	SpaceType = typename Grid_::SpaceType
	the state space
using	ModelType = Model_
	the process model
using	MethodType = Method_
	the prediction method
using	StateType = GridGaussianParams<Grid_, Covariance_, typename internal::CellComponentOf<Grid_, Model_>::type>
	The state the filter owns: a Gaussian per cell, or a hybrid under a switching model.
using	ConditionalType = typename StateType::ConditionalType
	a cell's distribution over the rest
using	allocator_type = typename StateType::allocator_type
	the state's allocator
using	UpdateType = RbPointMassFilterUpdate<Scalar, allocator_type>
	The result of update(): the cells and per-cell arrays from the filter's allocator.

Public Member Functions
	RbPointMassFilter (std::type_identity_t< Model_ > model, Grid_ grid, ConditionalType const &conditional, Covariance_={}, MethodFor< Model_, Method_ > method={})
	A filter with process model model over the rest of the state, points grid, conditional as every cell's Gaussian over the rest, or its hybrid under a switching model, with every cell equally probable, prediction method method, which a fixed-step model carries instead, and upkeep upkeep, which maintain() runs.
	RbPointMassFilter (std::type_identity_t< Model_ > model, Grid_ grid, ConditionalType const &conditional, Covariance_, MethodFor< Model_, Method_ > method, std::type_identity_t< Upkeep_ > upkeep)
	As RbPointMassFilter(model, grid, conditional, covariance, method), with upkeep upkeep, which maintain() runs.
	RbPointMassFilter (std::type_identity_t< Model_ > model, Grid_ grid, ConditionalType const &conditional, std::type_identity_t< Upkeep_ > upkeep)
	As RbPointMassFilter(model, grid, conditional), with upkeep upkeep, which maintain() runs.
	RbPointMassFilter (std::type_identity_t< Model_ > model, Grid_ grid, ConditionalType const &conditional, Covariance_ covariance, std::type_identity_t< Upkeep_ > upkeep)
	As RbPointMassFilter(model, grid, conditional, covariance), with upkeep upkeep, which maintain() runs.
	RbPointMassFilter (Grid_ grid, Covariance_={})
	A filter whose points cover the whole state (see PointMassFilter), with every cell equally probable.
	RbPointMassFilter (Grid_ grid, Covariance_, std::type_identity_t< Upkeep_ > upkeep)
	As RbPointMassFilter(grid, covariance), with upkeep upkeep, which maintain() runs.
	RbPointMassFilter (Grid_ grid, std::type_identity_t< Upkeep_ > upkeep)
	As RbPointMassFilter(grid), with upkeep upkeep, which maintain() runs.
allocator_type	get_allocator () const
StateType const &	state () const
StateType &	state ()
void	setState (StateType state)
	Replaces the state, as when reinitializing, with one whose rest space the process model's writes partition, as the constructor's must.
Model_ const &	model () const
Model_ &	model ()
Method_ const &	method () const
template<typename... Inputs> requires (!isFixedStep<Model_>)
void	predict (TimeStep< Scalar > auto const &dt, Inputs const &... inputs)
	Moves each live cell's Gaussian forward by dt, any TimeStep (see Time steps), through the process model, with inputs passed as in Emrys::predict().
template<ExecutionPolicy P, typename... Inputs> requires (!isFixedStep<Model_>)
void	predict (P &&policy, TimeStep< Scalar > auto const &dt, Inputs const &... inputs)
	predict(dt, inputs...) under policy, a for-each over the live cells, so the result is Sequential's (see Execution policies).
template<typename... Inputs> requires (isFixedStep<Model_> && !internal::leadsWithPolicy<Inputs...> && !internal::leadsWithSteps<Inputs...>)
void	predict (Inputs const &... inputs)
	Moves each live cell's Gaussian forward by the fixed-step model's step, with inputs passed as in Emrys::predict().
template<ExecutionPolicy P, typename... Inputs> requires (isFixedStep<Model_> && !internal::leadsWithSteps<Inputs...>)
void	predict (P &&policy, Inputs const &... inputs)
	predict(inputs...) under policy, as predict(policy, dt, inputs...) runs.
template<typename... Inputs> requires isFixedStep<Model_>
void	predict (Steps steps, Inputs const &... inputs)
	Moves each live cell's Gaussian forward by steps of the fixed-step model, as that many calls of predict(inputs...) do, with inputs at every step.
template<ExecutionPolicy P, typename... Inputs> requires isFixedStep<Model_>
void	predict (P &&policy, Steps steps, Inputs const &... inputs)
	predict(steps, inputs...) under policy, as that many calls of predict(policy, inputs...) run.
template<internal::WeighsPoints M>
std::expected< UpdateType, NumericError >	update (M const &measurement)
	Conditions each live cell's Gaussian on measurement given the cell's center, by the measurement's own method, and multiplies each cell's probability by \( p(z \mid c) \).
template<ExecutionPolicy P, internal::WeighsPoints M>
std::expected< UpdateType, NumericError >	update (P &&executionPolicy, M const &measurement)
	update() under executionPolicy: each cell's update is a for-each and the evidence a reduction, which matches Sequential's to rounding (see Execution policies).
template<internal::WeighsPoints M, typename ResultAllocator>
std::expected< void, NumericError >	update (M const &measurement, RbPointMassFilterUpdate< Scalar, ResultAllocator > &result)
	As update(measurement), but into result, whose storage is reused: an update into a result kept across steps allocates nothing once it has held as many cells.
template<ExecutionPolicy P, internal::WeighsPoints M, typename ResultAllocator>
std::expected< void, NumericError >	update (P &&executionPolicy, M const &measurement, RbPointMassFilterUpdate< Scalar, ResultAllocator > &result)
	update(measurement, result) under executionPolicy, as update(executionPolicy, measurement) runs.
template<internal::WeighsPoints M, typename Policy> requires (!Shared)
std::expected< UpdateType, NumericError >	update (M const &measurement, Policy const &policy)
	As update(measurement), with PerPointCovariance, but with each cell's branches reduced by policy (Collapse, Select, or a Gate); a cell whose update the policy rejects is removed.
template<ExecutionPolicy P, internal::WeighsPoints M, typename Policy> requires (!Shared)
std::expected< UpdateType, NumericError >	update (P &&executionPolicy, M const &measurement, Policy const &policy)
	update(measurement, policy) under executionPolicy: each cell's Kalman update and reduction by policy is a for-each, and the evidence a reduction, which matches Sequential's to rounding (see Execution policies).
template<internal::WeighsPoints M, typename Policy, typename ResultAllocator> requires (!Shared)
std::expected< void, NumericError >	update (M const &measurement, Policy const &policy, RbPointMassFilterUpdate< Scalar, ResultAllocator > &result)
	As update(measurement, policy), but into result, whose storage is reused (see update(measurement, result)).
template<ExecutionPolicy P, internal::WeighsPoints M, typename Policy, typename ResultAllocator> requires (!Shared)
std::expected< void, NumericError >	update (P &&executionPolicy, M const &measurement, Policy const &policy, RbPointMassFilterUpdate< Scalar, ResultAllocator > &result)
	update(measurement, policy, result) under executionPolicy, as update(executionPolicy, measurement, policy) runs.

Detailed Description

template<typename Grid_, typename Model_, typename Covariance_ = internal::PointCovarianceFor<Model_>, typename Method_ = DefaultMethodFor<Model_>, typename Upkeep_ = upkeep::None>
class Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >

A Rao-Blackwellized point-mass filter: fixed points for one block of the state, such as a coarse position, each carrying a Kalman filter over the rest.

The state is a GridGaussianParams, whose density, a GridGaussian, is \( p(x) = \sum_c P(c) \, \delta(x_p - p_c) \, \mathcal{N}(x_r; \mu_c, \Sigma_c) \): the block \( x_p \) named by the grid's path sits at the cell's center \( p_c \), and the rest \( x_r \) is Gaussian given the cell. It is a Rao-Blackwellized particle filter whose particles never move: the points are fixed, the process model moves only the rest, and no probability passes between cells. predict() moves each cell's Gaussian through the process model, which covers the rest of the state. update() updates each cell's Gaussian given its center (see update(prior, measurement, given)) and multiplies each cell's probability by \( p(z \mid c) \), so measurements nonlinear in the fixed block, such as ranges or a terrain map, weigh the cells exactly, while the rest gets Kalman updates.

With Covariance_ SharedCovariance, every cell has the same covariance: one transform, at the most probable cell, gives the innovation covariance and gain for all of them, and each cell costs an evaluation of the measurement function and a mean update. This is exact when the dynamics and the measurement are affine in the rest of the state with the same Jacobian in every cell; the measurement may depend on the fixed block arbitrarily. Its noise must be one Gaussian. With PerPointCovariance, each cell is its own Kalman filter, its measurement transformed and its branches reduced by a policy (Collapse, Select, or a Gate) as a KalmanFilter's are. A measurement known only by its likelihood of the fixed block (a model with logLikelihood(g) and no function) multiplies each cell's probability by it and leaves the Gaussians alone.

A cell's covariance about its center (see PointGrid) enters each update as an independent uncertain input: it widens the innovation by the measurement's sensitivity to the fixed block, which the measurement's method computes, so a measurement nonlinear in that block needs Linearized or Unscented, as any measurement does. The offset from the center is not tracked, so successive innovations are treated as independent although the offset they share correlates them. Cells with covariances of their own (PointGrid with PerPointCovariance) widen each cell's innovation differently, so they need a PerPointCovariance filter. With SharedCovariance, one cell covariance shared by all is exact only if the measurement's sensitivity to the fixed block is the same in every cell; otherwise the most probable cell's stands for all.

Only live cells are stored, and a cell whose probability becomes zero is removed. The caller keeps the number of live cells in bounds through state() (see Upkeep): PruneBelow, PruneTail, and PruneToSize remove the improbable cells, and Subdivide splits every live cell of a regular grid, to resolve the fixed block more finely where the probability has gathered.

With Model_ a SwitchingModel over the rest, each cell holds a HybridGaussian over the rest, an IMM filter's state, so each cell is an IMM filter given its center: predict() mixes each cell's modes, predicts each through its mode's model, and steps the chain, and update() updates each cell as ImmRecursion::update(state, measurement, given, policy) does and multiplies each cell's probability by \( p(z \mid c) = \sum_m P(m \mid c) \, p(z \mid m, c) \), so noise by mode axis (ModeNoise) updates each mode with the component its value names, as in an ImmFilter. The covariance form is then PerPointCovariance, and the cells' steps share the recursion's scratch, so they run one at a time: predict() and update() take Sequential alone, except an update by a likelihood alone, which leaves the hybrids as they were. state().modes() sums the cells' modes. No publication found states this form: it composes the IMM filter, given a cell's center, with the Rao-Blackwellized point-mass filter, and is one of two exceptions to [published-algorithms-only].

The Rao-Blackwellized point-mass filter of Šmídl and Gašperin (2013) and Duník et al. (2019), as given in Pocoma et al. (2023), §II-C and §II-D: a grid over one block and a Kalman filter per cell. The update given each cell is the Rao-Blackwellized update the particle filters share (see RbUpdate.h); where the measurement does not depend on the rest it reduces to Pocoma et al.'s correction, Algorithm 1, equations (17) and (18). It departs from the published filter in its prediction, which moves only the rest: the published one also moves probability between cells, with the rest as noise on the block (§III-D) [rb-point-mass]. The cell covariance about each center has no source and is the other exception to [published-algorithms-only].

The state and the filter's scratch come from the grid's allocator (see Allocation), and the scratch is reused, so steps allocate nothing once one has run with as many live cells, except that an update of hybrid cells allocates the IMM result it reuses from cell to cell.

using State = Composite<Position, ClockBias>;
auto const grid = regularGrid<State, Position>(first, spacing, {200, 200});
auto filter = RbPointMassFilter{RandomWalk<ClockBias>{0.01}, grid, initialBias};
filter.predict(dt);
auto const updated = filter.update(range);  // reads Position and ClockBias
filter.state().marginal<GridAxis<Position, 0>>();
filter.state().toDensity()->collapsed();    // the whole state as one GaussianParams
upkeep::pruneBelow(1e-12)(filter.state());

Template Parameters

Grid_	the fixed points (see PointGrid)
Model_	the process model over the rest of the state, such as a ProcessModel, or a FixedStepModel over the grid's restSpace(), with which predict() takes no time step; a SwitchingModel gives each cell a hybrid
Covariance_	SharedCovariance or PerPointCovariance; PerPointCovariance under a SwitchingModel
Method_	the prediction method; a FixedStepModel's own; under a SwitchingModel, that of each mode whose model carries none of its own (see withMethod()), and ModelsMethod with fixed-step models
Upkeep_	the upkeep maintain() runs on the state (see Upkeep), upkeep::None unless given one, such as pruning and subdividing

See also: GridGaussianParams, PointGrid, update(prior, measurement, given), Filters

Constructor & Destructor Documentation

◆ RbPointMassFilter()

template<typename Grid_, typename Model_, typename Covariance_ = internal::PointCovarianceFor<Model_>, typename Method_ = DefaultMethodFor<Model_>, typename Upkeep_ = upkeep::None>

Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >::RbPointMassFilter	(	std::type_identity_t< Model_ >	model,
		Grid_	grid,
		ConditionalType const &	conditional,
		Covariance_	= {},
		MethodFor< Model_, Method_ >	method = {} )

inline

A filter with process model model over the rest of the state, points grid, conditional as every cell's Gaussian over the rest, or its hybrid under a switching model, with every cell equally probable, prediction method method, which a fixed-step model carries instead, and upkeep upkeep, which maintain() runs.

The model is not deduced from here, so that the deduction guide below, which binds fixedStep()'s result to the rest of the state, deduces the filter's arguments.

Member Function Documentation

◆ get_allocator()

template<typename Grid_, typename Model_, typename Covariance_ = internal::PointCovarianceFor<Model_>, typename Method_ = DefaultMethodFor<Model_>, typename Upkeep_ = upkeep::None>

allocator_type Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >::get_allocator ( ) const

inlinenodiscard

Returns: the allocator of the state and scratch: the grid's

◆ method()

template<typename Grid_, typename Model_, typename Covariance_ = internal::PointCovarianceFor<Model_>, typename Method_ = DefaultMethodFor<Model_>, typename Upkeep_ = upkeep::None>

Method_ const & Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >::method ( ) const

inlinenodiscard

Returns: the prediction method

◆ model() [1/2]

template<typename Grid_, typename Model_, typename Covariance_ = internal::PointCovarianceFor<Model_>, typename Method_ = DefaultMethodFor<Model_>, typename Upkeep_ = upkeep::None>

Model_ & Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >::model ( )

inlinenodiscard

Returns: the process model, to change its parameters

◆ model() [2/2]

template<typename Grid_, typename Model_, typename Covariance_ = internal::PointCovarianceFor<Model_>, typename Method_ = DefaultMethodFor<Model_>, typename Upkeep_ = upkeep::None>

Model_ const & Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >::model ( ) const

inlinenodiscard

Returns: the process model

◆ predict() [1/3]

template<typename Grid_, typename Model_, typename Covariance_ = internal::PointCovarianceFor<Model_>, typename Method_ = DefaultMethodFor<Model_>, typename Upkeep_ = upkeep::None>

template<typename... Inputs>
requires (isFixedStep<Model_> && !internal::leadsWithPolicy<Inputs...> && !internal::leadsWithSteps<Inputs...>)

void Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >::predict ( Inputs const &... inputs )

inline

Moves each live cell's Gaussian forward by the fixed-step model's step, with inputs passed as in Emrys::predict().

The points and their probabilities don't change.

◆ predict() [2/3]

template<typename Grid_, typename Model_, typename Covariance_ = internal::PointCovarianceFor<Model_>, typename Method_ = DefaultMethodFor<Model_>, typename Upkeep_ = upkeep::None>

template<ExecutionPolicy P, typename... Inputs>
requires (!isFixedStep<Model_>)

void Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >::predict	(	P &&	policy,
		TimeStep< Scalar > auto const &	dt,
		Inputs const &...	inputs )

inline

predict(dt, inputs...) under policy, a for-each over the live cells, so the result is Sequential's (see Execution policies).

The process model's function then runs for several cells at once. A PointMassFilter, whose points cover the whole state, has nothing to move. Hybrid cells take Sequential alone.

◆ predict() [3/3]

template<typename Grid_, typename Model_, typename Covariance_ = internal::PointCovarianceFor<Model_>, typename Method_ = DefaultMethodFor<Model_>, typename Upkeep_ = upkeep::None>

template<typename... Inputs>
requires (!isFixedStep<Model_>)

void Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >::predict	(	TimeStep< Scalar > auto const &	dt,
		Inputs const &...	inputs )

inline

Moves each live cell's Gaussian forward by dt, any TimeStep (see Time steps), through the process model, with inputs passed as in Emrys::predict().

The points and their probabilities don't change.

◆ setState()

template<typename Grid_, typename Model_, typename Covariance_ = internal::PointCovarianceFor<Model_>, typename Method_ = DefaultMethodFor<Model_>, typename Upkeep_ = upkeep::None>

void Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >::setState ( StateType state )

inline

Replaces the state, as when reinitializing, with one whose rest space the process model's writes partition, as the constructor's must.

The scratch follows the new state's allocator and its number of live cells.

◆ state() [1/2]

template<typename Grid_, typename Model_, typename Covariance_ = internal::PointCovarianceFor<Model_>, typename Method_ = DefaultMethodFor<Model_>, typename Upkeep_ = upkeep::None>

StateType & Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >::state ( )

inlinenodiscard

Returns: the state, to change where it is between steps, as upkeep such as pruning or subdividing does. It must keep its space and allocator; setState() replaces it with another, and checks it.

◆ state() [2/2]

template<typename Grid_, typename Model_, typename Covariance_ = internal::PointCovarianceFor<Model_>, typename Method_ = DefaultMethodFor<Model_>, typename Upkeep_ = upkeep::None>

StateType const & Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >::state ( ) const

inlinenodiscard

Returns: the live cells, their probabilities, and their Gaussians

◆ update() [1/4]

template<typename Grid_, typename Model_, typename Covariance_ = internal::PointCovarianceFor<Model_>, typename Method_ = DefaultMethodFor<Model_>, typename Upkeep_ = upkeep::None>

template<internal::WeighsPoints M>

std::expected< UpdateType, NumericError > Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >::update ( M const & measurement )

inlinenodiscard

Conditions each live cell's Gaussian on measurement given the cell's center, by the measurement's own method, and multiplies each cell's probability by \( p(z \mid c) \).

With SharedCovariance, the measurement's noise must be one Gaussian; with PerPointCovariance, each cell's branches are collapsed, as update(measurement, policy) does with Collapse. If every cell has likelihood zero, the state is unchanged.

A measurement whose model supplies its own logLikelihood(g), with or without a function, instead multiplies each cell's probability by that likelihood at the cell's center and leaves every cell's Gaussian as it was (decisions.md [likelihood-measurements]). Every block it reads must lie in the grid's block, which is checked at compile time: given the rest, the rest would no longer be Gaussian. Its result has NIS NaN and an innovation of size 0. A cell's covariance about its center is ignored, so a likelihood narrower than the cells needs finer cells, from Subdivide.

Returns: the cells updated, each one's log-likelihood and NIS, the total log-likelihood, and whether the measurement was accepted (see RbPointMassFilterUpdate), in storage from the filter's allocator; or, if a cell's update failed, as one whose innovation covariance is not positive definite does, its error (decisions.md [innovation-cholesky]), with the state unchanged, or with a hybrid per cell, which is updated where it is, with the cells before the failed one updated and the probabilities as they were [update-outcomes]

◆ update() [2/4]

template<typename Grid_, typename Model_, typename Covariance_ = internal::PointCovarianceFor<Model_>, typename Method_ = DefaultMethodFor<Model_>, typename Upkeep_ = upkeep::None>

template<internal::WeighsPoints M, typename Policy>
requires (!Shared)

std::expected< UpdateType, NumericError > Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >::update	(	M const &	measurement,
		Policy const &	policy )

inlinenodiscard

As update(measurement), with PerPointCovariance, but with each cell's branches reduced by policy (Collapse, Select, or a Gate); a cell whose update the policy rejects is removed.

A measurement weighted by its own likelihood has no branches, so it takes no policy.

◆ update() [3/4]

template<typename Grid_, typename Model_, typename Covariance_ = internal::PointCovarianceFor<Model_>, typename Method_ = DefaultMethodFor<Model_>, typename Upkeep_ = upkeep::None>

template<internal::WeighsPoints M, typename Policy, typename ResultAllocator>
requires (!Shared)

std::expected< void, NumericError > Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >::update	(	M const &	measurement,
		Policy const &	policy,
		RbPointMassFilterUpdate< Scalar, ResultAllocator > &	result )

inlinenodiscard

As update(measurement, policy), but into result, whose storage is reused (see update(measurement, result)).

Returns: nothing, or the error update(measurement, policy) would return

◆ update() [4/4]

template<typename Grid_, typename Model_, typename Covariance_ = internal::PointCovarianceFor<Model_>, typename Method_ = DefaultMethodFor<Model_>, typename Upkeep_ = upkeep::None>

template<internal::WeighsPoints M, typename ResultAllocator>

std::expected< void, NumericError > Emrys::RbPointMassFilter< Grid_, Model_, Covariance_, Method_, Upkeep_ >::update	(	M const &	measurement,
		RbPointMassFilterUpdate< Scalar, ResultAllocator > &	result )

inlinenodiscard

As update(measurement), but into result, whose storage is reused: an update into a result kept across steps allocates nothing once it has held as many cells.

Returns: nothing, or the error update(measurement) would return

The documentation for this class was generated from the following files:

EmrysFilter/Distributions/GridGaussian.h
EmrysFilter/Filters/RbPointMassFilter.h

Public Types

Public Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ RbPointMassFilter()

Member Function Documentation

◆ get_allocator()

◆ method()

◆ model() [1/2]

◆ model() [2/2]

◆ predict() [1/3]

◆ predict() [2/3]

◆ predict() [3/3]

◆ setState()

◆ state() [1/2]

◆ state() [2/2]

◆ update() [1/4]

◆ update() [2/4]

◆ update() [3/4]

◆ update() [4/4]