Struct fast_slam::FastSLAM [−][src]
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>>>, }
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
Creates a FastSLAM with empty feature map.
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>,
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.
Forget all feature information, feature no can be reused for a new feature
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>,
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>,
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
Auto Trait Implementations
impl<N, DL, const DF: usize> !RefUnwindSafe for FastSLAM<N, DL, DF>
impl<N, DL, const DF: usize> !UnwindSafe for FastSLAM<N, DL, DF>
Blanket Implementations
Mutably borrows from an owned value. Read more
type Output = T
type Output = T
Should always be Self
The inverse inclusion map: attempts to construct self from the equivalent element of its
superset. Read more
pub fn is_in_subset(&self) -> bool
pub fn is_in_subset(&self) -> bool
Checks if self is actually part of its subset T (and can be converted to it).
pub fn to_subset_unchecked(&self) -> SS
pub fn to_subset_unchecked(&self) -> SS
Use with care! Same as self.to_subset but without any property checks. Always succeeds.
pub fn from_subset(element: &SS) -> SP
pub fn from_subset(element: &SS) -> SP
The inclusion map: converts self to the equivalent element of its superset.