pub struct FastSLAM<N: RealField, DL: Dim, const DF: usize>where
DefaultAllocator: Allocator<DL, DL> + Allocator<DL> + Allocator<Const<DF>, Const<DF>> + Allocator<Const<DF>>,{
pub loc: SampleState<N, DL>,
pub map: HashMap<u32, FeatureCondMap<N, Const<DF>>>,
}
Expand description
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§
source§impl<N: RealField + Copy + FromPrimitive + ToPrimitive, DL: Dim, const DF: usize> FastSLAM<N, DL, DF>
impl<N: RealField + Copy + FromPrimitive + ToPrimitive, DL: Dim, const DF: usize> FastSLAM<N, DL, DF>
sourcepub fn new(loc: SampleState<N, DL>) -> Self
pub fn new(loc: SampleState<N, DL>) -> Self
Creates a FastSLAM with empty feature map.
sourcepub fn observe_new(
&mut self,
feature: u32,
obs_model: impl Fn(&OVector<N, DL>) -> Feature<N, Const<DF>>,
)
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 required b) Feature state estimates comes directly from the location and observation
sourcepub 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>>,
)
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>>, )
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.
sourcepub fn forget(&mut self, feature: u32) -> Option<FeatureCondMap<N, Const<DF>>>
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
sourcepub fn update_resample(
&mut self,
resampler: &mut Resampler,
roughener: &mut Roughener<N, DL>,
rng: &mut dyn RngCore,
) -> Result<(u32, f32), &str>
pub fn update_resample( &mut self, resampler: &mut Resampler, roughener: &mut Roughener<N, DL>, rng: &mut dyn RngCore, ) -> Result<(u32, f32), &str>
Resampling Update.
Resample using weights. Propagate resampling to All features. Only resamples if weights have been updated.
sourcepub fn statistics_compressed(&self, kstat: &mut KalmanState<N, Dyn>)
pub fn statistics_compressed(&self, kstat: &mut KalmanState<N, Dyn>)
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 computed in feature number order and only for those for which there is space in kstat.
Precond: kstat must have space for location statistics
sourcepub fn statistics_sparse(&self, kstat: &mut KalmanState<N, Dyn>)
pub fn statistics_sparse(&self, kstat: &mut KalmanState<N, Dyn>)
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 computed with the 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
Auto Trait Implementations§
impl<N, DL, const DF: usize> Freeze for FastSLAM<N, DL, DF>where
DefaultAllocator: Sized,
impl<N, DL, const DF: usize> !RefUnwindSafe for FastSLAM<N, DL, DF>
impl<N, DL, const DF: usize> !Send for FastSLAM<N, DL, DF>
impl<N, DL, const DF: usize> !Sync for FastSLAM<N, DL, DF>
impl<N, DL, const DF: usize> !Unpin for FastSLAM<N, DL, DF>
impl<N, DL, const DF: usize> !UnwindSafe for FastSLAM<N, DL, DF>
Blanket Implementations§
source§impl<T> BorrowMut<T> for Twhere
T: ?Sized,
impl<T> BorrowMut<T> for Twhere
T: ?Sized,
source§fn borrow_mut(&mut self) -> &mut T
fn borrow_mut(&mut self) -> &mut T
source§impl<SS, SP> SupersetOf<SS> for SPwhere
SS: SubsetOf<SP>,
impl<SS, SP> SupersetOf<SS> for SPwhere
SS: SubsetOf<SP>,
source§fn to_subset(&self) -> Option<SS>
fn to_subset(&self) -> Option<SS>
self
from the equivalent element of its
superset. Read moresource§fn is_in_subset(&self) -> bool
fn is_in_subset(&self) -> bool
self
is actually part of its subset T
(and can be converted to it).source§fn to_subset_unchecked(&self) -> SS
fn to_subset_unchecked(&self) -> SS
self.to_subset
but without any property checks. Always succeeds.source§fn from_subset(element: &SS) -> SP
fn from_subset(element: &SS) -> SP
self
to the equivalent element of its superset.