|
EmrysFilter 0.1.0
State estimation utilities in C++23 using Eigen
|
A Rao-Blackwellized particle filter: particles for one block of the state, each carrying a Kalman filter over the rest. More...
#include <EmrysFilter/Filters/RbParticleFilter.h>
Public Types | |
| using | Scalar = Scalar_ |
| the scalar type | |
| using | SpaceType = Space_ |
| the state space | |
| using | ModelType = Model_ |
| the process model | |
| using | MethodType = Method_ |
| the prediction method | |
| using | StateType |
| The state the filter owns: a Gaussian per particle, or a hybrid under a switching model. | |
| using | ConditionalType = typename StateType::ConditionalType |
| a particle's distribution over the rest | |
| using | allocator_type = Allocator_ |
| the state's allocator | |
| using | UpdateType = RbParticleFilterUpdate<Scalar, Allocator_> |
| The result of update(): the per-particle arrays from the filter's allocator. | |
Public Member Functions | |
| RbParticleFilter (std::type_identity_t< Model_ > model, StateType state, MethodFor< Model_, Method_ > method={}, std::type_identity_t< Upkeep_ > upkeep={}) | |
| A filter with process model model over the whole state, initial state state, prediction method method, which a fixed-step model carries instead, and upkeep upkeep, which maintain() runs. | |
| RbParticleFilter (std::type_identity_t< Model_ > model, StateType state, std::type_identity_t< Upkeep_ > upkeep) | |
| A filter with process model model over the whole state, initial state state, the default prediction method, and 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 space the process model's writes partition, as the constructor's must. | |
| Model_ const & | model () const |
| Model_ & | model () |
| Method_ const & | method () const |
| template<std::uniform_random_bit_generator G, typename... Inputs> requires (!isFixedStep<Model_>) | |
| auto | predict (G &rng, TimeStep< Scalar_ > auto const &dt, Inputs const &... inputs) |
| Moves each particle and its Gaussian forward by dt, any TimeStep (see Time steps): predicts the whole state given the particle through the process model, with inputs passed as in Emrys::predict(), draws the particle's new value from the prediction's marginal on the sampled block with rng, and conditions the rest on it. | |
| template<ExecutionPolicy P, std::uniform_random_bit_generator G, typename... Inputs> requires (!isFixedStep<Model_>) | |
| auto | predict (P &&policy, G &rng, TimeStep< Scalar_ > auto const &dt, Inputs const &... inputs) |
| predict(rng, dt, inputs...) under policy: the draws are taken first, as the sequential prediction takes them, and each particle and its Gaussian then move as a for-each, so the result is Sequential's (see Execution policies). | |
| template<std::uniform_random_bit_generator G, typename... Inputs> requires (isFixedStep<Model_> && !internal::leadsWithSteps<Inputs...>) | |
| auto | predict (G &rng, Inputs const &... inputs) |
| Moves each particle and its Gaussian forward by the fixed-step model's step, as predict(rng, dt, inputs...) does, with the model's cached \( Q \). | |
|
template<ExecutionPolicy P, std::uniform_random_bit_generator G, typename... Inputs> requires (isFixedStep<Model_> && !internal::leadsWithSteps<Inputs...>) | |
| auto | predict (P &&policy, G &rng, Inputs const &... inputs) |
| predict(rng, inputs...) under policy, as predict(policy, rng, dt, inputs...) runs. | |
| template<std::uniform_random_bit_generator G, typename... Inputs> requires isFixedStep<Model_> | |
| auto | predict (G &rng, Steps steps, Inputs const &... inputs) |
| Moves each particle and its Gaussian forward by steps of the fixed-step model, as that many calls of predict(rng, inputs...) do, with inputs at every step and the draws in the same order. | |
|
template<ExecutionPolicy P, std::uniform_random_bit_generator G, typename... Inputs> requires isFixedStep<Model_> | |
| auto | predict (P &&policy, G &rng, Steps steps, Inputs const &... inputs) |
| predict(rng, steps, inputs...) under policy, as that many calls of predict(policy, rng, inputs...) run. | |
| template<internal::WeighsPoints M> | |
| std::expected< UpdateType, NumericError > | update (M const &measurement) |
| Conditions each particle's Gaussian on measurement given the particle, by the measurement's own method, and multiplies each particle's weight by \( p(z \mid i) \). | |
| template<ExecutionPolicy P, internal::WeighsPoints M> | |
| std::expected< UpdateType, NumericError > | update (P &&executionPolicy, M const &measurement) |
| update() under executionPolicy: each particle'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, RbParticleFilterUpdate< 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 particles. | |
| template<ExecutionPolicy P, internal::WeighsPoints M, typename ResultAllocator> | |
| std::expected< void, NumericError > | update (P &&executionPolicy, M const &measurement, RbParticleFilterUpdate< 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 particle's branches reduced by policy (Collapse, Select, or a Gate); a particle whose update the policy rejects keeps its Gaussian and gets weight zero. | |
|
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 particle'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, RbParticleFilterUpdate< 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, RbParticleFilterUpdate< Scalar, ResultAllocator > &result) |
| update(measurement, policy, result) under executionPolicy, as update(executionPolicy,
measurement, policy) runs. | |
A Rao-Blackwellized particle filter: particles for one block of the state, each carrying a Kalman filter over the rest.
The state is a ParticleGaussianParams, whose density, a ParticleGaussian, is \( p(x) = \sum_i w_i \, \delta(x_p - p_i) \, \mathcal{N}(x_r; \mu_i, \Sigma_i) \). The process model covers the whole state, and the sampled block's motion may depend on the rest, as a position moves with a velocity kept Gaussian.
predict() moves each particle with its Gaussian: it predicts the whole state given the particle by the filter's method, with the process noise, which gives a Gaussian over the new state; draws the particle's new value from that Gaussian's marginal on the sampled block; and conditions the rest on the draw. The draw's offset tells the rest about the block's motion, as a measurement would: a velocity is learned from the positions it moves. The weights don't change. update() conditions each particle's Gaussian on a measurement given the particle (see update(prior, measurement, given)) and multiplies its weight by \( p(z \mid i) \).
With Covariance_ SharedCovariance, every particle's Gaussian has the same covariance: one transform, at the heaviest particle, serves every particle in predict() and in update(), and each particle costs a propagation or a measurement 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 at every particle, as in the classic mixed linear/nonlinear model; the measurement may depend on the sampled block arbitrarily. Its noise must be one Gaussian. With PerPointCovariance, each particle is its own Kalman filter, predicted and updated by its own transform, and an update's branches are reduced by a policy (Collapse, Select, or a Gate) as a KalmanFilter's are. A measurement known only by its likelihood of the sampled block (a model with logLikelihood(g) and no function) multiplies each particle's weight by it and leaves the Gaussians alone.
With Model_ a SwitchingModel over the whole state, each particle holds a HybridGaussian over the rest, an IMM filter's state, so each particle is an IMM filter given its value. predict() mixes each particle's modes through the chain, predicts the whole state given the particle through each mode's model, draws a mode from the predicted mode probabilities and the particle's new value from that mode's prediction, and conditions every mode's Gaussian on the draw, with each mode's probability multiplied by the density of the draw under that mode's prediction: the draw tells the modes apart, as it tells the rest about the block's motion. The weights don't change. update() updates each particle as ImmRecursion::update(state, measurement, given, policy) does and multiplies its weight by \( p(z \mid i) = \sum_m P(m \mid i) \, p(z \mid m, i) \), 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 particles' 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 particles' modes.
The filter never resamples on its own; the caller applies upkeep to state() (see Upkeep), as with ParticleFilter.
The Rao-Blackwellized particle filter of Doucet et al. (2000) and the marginalized particle filter of Schön, Gustafsson, and Nordlund (2005), whose §IV gives the prediction that draws the nonlinear substate from the predicted joint and conditions the linear one on the draw.
Its scratch (the draws, the posterior Gaussians, and the per-particle likelihoods) comes from the state's allocator and is reused, so its steps allocate nothing once it has run one at a steady number of particles (see Allocation), except that an update of hybrid particles allocates the IMM result it reuses from particle to particle.
| Space_ | the whole state space |
| Path_ | the block the particles sample, as a tag or a TagPath |
| Model_ | the process model over the whole state, a ProcessModel or a FixedStepModel; with a FixedStepModel, predict() takes no time step; a SwitchingModel gives each particle a hybrid |
| Covariance_ | SharedCovariance or PerPointCovariance; PerPointCovariance under a SwitchingModel |
| Method_ | the prediction method: Linearized or Unscented; 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 |
| Scalar_ | scalar type, double by default |
| Allocator_ | the state's allocator (see Allocation) |
| Upkeep_ | the upkeep maintain() runs on the state (see Upkeep), upkeep::None unless given one |
| using Emrys::RbParticleFilter< Space_, Path_, Model_, Covariance_, Method_, Scalar_, Allocator_, Upkeep_ >::StateType |
The state the filter owns: a Gaussian per particle, or a hybrid under a switching model.
|
inline |
A filter with process model model over the whole state, initial state state, prediction method method, which a fixed-step model carries instead, and upkeep upkeep, which maintain() runs.
Its scratch comes from the state's allocator, with room for the state's capacity if it has one. The model is not deduced from here, so that the deduction guide below, which binds fixedStep()'s result to the state's space, deduces the filter's arguments.
|
inlinenodiscard |
|
inlinenodiscard |
|
inlinenodiscard |
|
inlinenodiscard |
|
inlinenodiscard |
Moves each particle and its Gaussian forward by the fixed-step model's step, as predict(rng, dt, inputs...) does, with the model's cached \( Q \).
Under a cached transition and with no inputs, the prediction given each particle is \( F_r P_i F_r^T + Q \), with \( F_r \) the transition's columns for the rest of the state, rather than a transform; with a hybrid per particle, each mode predicts by its model's transform.
|
inlinenodiscard |
Moves each particle and its Gaussian forward by steps of the fixed-step model, as that many calls of predict(rng, inputs...) do, with inputs at every step and the draws in the same order.
|
inlinenodiscard |
Moves each particle and its Gaussian forward by dt, any TimeStep (see Time steps): predicts the whole state given the particle through the process model, with inputs passed as in Emrys::predict(), draws the particle's new value from the prediction's marginal on the sampled block with rng, and conditions the rest on it.
The weights are unchanged.
The draws are taken first, one standard normal vector per particle. With a hybrid per particle, each particle then draws its mode with one uniform draw, in order.
std::expected with NotPositiveDefinite if a mode the chain reaches predicts the sampled block with a covariance that is not positive definite, so that the draw has no density under it, with the state unchanged
|
inlinenodiscard |
predict(rng, dt, inputs...) under policy: the draws are taken first, as the sequential prediction takes them, and each particle and its Gaussian then move as a for-each, so the result is Sequential's (see Execution policies).
The process model's propagate() then runs for several particles at once. Hybrid particles take Sequential alone.
|
inline |
Replaces the state, as when reinitializing, with one whose space the process model's writes partition, as the constructor's must.
The scratch follows the new state's allocator and capacity.
|
inlinenodiscard |
|
inlinenodiscard |
|
inlinenodiscard |
Conditions each particle's Gaussian on measurement given the particle, by the measurement's own method, and multiplies each particle's weight by \( p(z \mid i) \).
With SharedCovariance, the measurement's noise must be one Gaussian; with PerPointCovariance, each particle's branches are collapsed, as update(measurement, policy) does with Collapse. If no particle can explain the observation, the state is unchanged.
A measurement whose model supplies its own logLikelihood(g), with or without a function, instead multiplies each particle's weight by that likelihood at the particle and leaves every particle's Gaussian as it was (decisions.md [likelihood-measurements]). Every block it reads must lie in the sampled 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.
|
inlinenodiscard |
As update(measurement), with PerPointCovariance, but with each particle's branches reduced by policy (Collapse, Select, or a Gate); a particle whose update the policy rejects keeps its Gaussian and gets weight zero.
A measurement weighted by its own likelihood has no branches, so it takes no policy.
|
inlinenodiscard |
As update(measurement, policy), but into result, whose storage is reused (see update(measurement, 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 particles.