Struct fast_slam::FastSLAM

pub struct FastSLAM<N: RealField, DL: Dim, const DF: usize> where
    DefaultAllocator: Allocator<N, DL, DL> + Allocator<N, DL> + Allocator<N, Const<DF>, Const<DF>> + Allocator<N, Const<DF>>,  {
    pub loc: SampleState<N, DL>,
    pub map: HashMap<u32, FeatureCondMap<N, Const<DF>>>,
}

Fast SLAM state.

A sample state of the location with augmented Kalman states of map features.

Fields

loc: SampleState<N, DL>

Location as a sample state.

map: HashMap<u32, FeatureCondMap<N, Const<DF>>>

Hashmap of augmented map feature states.

Implementations

impl<N: RealField + ToPrimitive, DL: Dim, const DF: usize> FastSLAM<N, DL, DF> where
    DefaultAllocator: Allocator<N, DL, DL> + Allocator<N, DL> + Allocator<N, Const<DF>, Const<DF>> + Allocator<N, U1, Const<DF>> + Allocator<N, Const<DF>>, 

pub fn new_equal_likelihood(loc: SampleState<N, DL>) -> Self

Creates a FastSLAM with empty feature map.

pub fn observe_new(
    &mut self,
    feature: u32,
    obs_model: impl Fn(&OVector<N, DL>) -> Feature<N, Const<DF>>
)

SLAM new feature observation (overwrite).

Assumes there is no prior information about the feature (strictly a uniform un-informative prior) This implies: a) There has no information about location so no resampling is requires b) Feature state estimates comes directly from the location and observation

pub fn observe<DZ: Dim>(
    &mut self,
    feature: u32,
    innovation_model: impl Fn(&OVector<N, DL>, &OVector<N, Const<DF>>) -> (OVector<N, DZ>, CorrelatedNoise<N, DZ>),
    hx: &OMatrix<N, DZ, Const<DF>>
) where
    DefaultAllocator: Allocator<N, U1, Const<DF>> + Allocator<N, DZ, DZ> + Allocator<N, DZ, Const<DF>> + Allocator<N, Const<DF>, DZ> + Allocator<N, DZ>, 

SLAM Feature observation. Uses Extended Fast_SLAM observation equations Note: Mathematically only weight ratios are important. Numerically however the range should be restricted. The weights are computed here using the simplest form with common factor Ht removed.

pub fn forget(&mut self, feature: u32) -> Option<FeatureCondMap<N, Const<DF>>>

Forget all feature information, feature no can be reused for a new feature

pub fn update_resample(
    &mut self,
    resampler: &mut Resampler,
    roughener: &mut Roughener<N, DL>
) -> Result<(u32, f32), &'static str>

Resampling Update.

Resample using weights. Propagate resampling to All features. Only resamples if weights have been updated.

pub fn statistics_compressed(&self, kstat: &mut KalmanState<N, Dynamic>) where
    DefaultAllocator: Allocator<N, U1, DL>, 

Compute sample mean and covariance statistics of filter.

kstat elements are filled first with Location statistics and then the Map feature statistics. Feature statistics are are computed in feature number order and only for those for which there is space in kstat.

Precond: kstat must have space for location statistics

pub fn statistics_sparse(&self, kstat: &mut KalmanState<N, Dynamic>) where
    DefaultAllocator: Allocator<N, U1, DL>, 

Compute sample mean and covariance statistics of filter.

kstat elements are filled first with Location statistics and then the Map feature statistics. Feature statistics are are computed in feature number as index (after location) and only for those for which there is space in kstat.

Precond: kstat must have space for Location statistics