javaslam.slam
Class LinearizedSLAMFilter

java.lang.Object
  |
  +--javaslam.slam.LinearizedSLAMFilter

All Implemented Interfaces:

NonlinearSLAMFilter, SLAMFilter

public class LinearizedSLAMFilter

extends Object

implements NonlinearSLAMFilter

A nonlinear filter for the Simultaneous Localization and Mapping SLAM problem that uses a linear-Gaussian filter with a technique for linearizing nonlinear motion and measurement models. For example, by coupling a KalmanSLAMFilter with ExtendedTransformation linearizations, one obtains the traditional Extended Kalman filter technique for SLAM.

Field Summary

protected LGSLAMFilter	filter The underlying linear-Gaussian SLAM filter.
protected LinearizationFactory	linearization The factory for creating linearizations.

Constructor Summary

LinearizedSLAMFilter(LGSLAMFilter filter)
Constructor.

LinearizedSLAMFilter(LGSLAMFilter filter, LinearizationFactory linearization)
Constructor.

Method Summary

Variable	addLandmark(int id, int dim) Adds a new landmark to the map and returns its state variable.
boolean	contains(int id) Return true if this filter contains the landmark with the supplied identifier.
double[]	getLandmarkEstimate(int id) Returns the filtered estimate of a landmark's state.
int	getLandmarkId(Variable lm) Gets the identifier associated with the supplied landmark state variable.
int[]	getLandmarkIds() Returns the IDs of landmarks known to this filter.
Variable	getLandmarkVariable(int id) Gets the state variable associated with the landmark with the supplied identifier.
LGSLAMFilter	getLGFilter() Returns the underlying linear-Gaussian filter, which can be used to access the filtered belief state.
int	getNumLandmarks() Returns the number of landmarks known to this filter.
double[]	getRobotEstimate() Returns the filtered estimate of the robot's state.
Variable	getRobotVariable() Gets the state variable associated with the robot.
void	measurement(NoisyVectorFunction h, NoisyVectorFunction hInv, int id, double[] z) Performs a landmark measurement update assuming a known data association.
void	motion(NoisyVectorFunction f) Performs a motion update.
void	odometry(NoisyVectorFunction g, double[] y) Performs an odometry update.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Detail

filter

protected LGSLAMFilter filter

The underlying linear-Gaussian SLAM filter.

linearization

protected LinearizationFactory linearization

The factory for creating linearizations.

Constructor Detail

LinearizedSLAMFilter

public LinearizedSLAMFilter(LGSLAMFilter filter,
                            LinearizationFactory linearization)

Constructor.

Parameters:

filter - the underlying linear-Gaussian SLAM filter used for filtering

linearization - a linearization factory used to create linearizations of nonlinear models

See Also:

ExtendedTransformation.getFactory(), UnscentedTransformation.getFactory()

LinearizedSLAMFilter

public LinearizedSLAMFilter(LGSLAMFilter filter)

Constructor. The unscented transformation is used for linearization.

Parameters:

filter - the underlying linear-Gaussian SLAM filter used for filtering

See Also:

UnscentedTransformation

Method Detail

motion

public void motion(NoisyVectorFunction f)

Performs a motion update.

Specified by:

motion in interface NonlinearSLAMFilter

Parameters:

f - the stochastic motion model

odometry

public void odometry(NoisyVectorFunction g,
                     double[] y)

Performs an odometry update.

Specified by:

odometry in interface NonlinearSLAMFilter

Parameters:

g - the noisy odometry measurement model

y - the odometry measurement

measurement

public void measurement(NoisyVectorFunction h,
                        NoisyVectorFunction hInv,
                        int id,
                        double[] z)

Performs a landmark measurement update assuming a known data association.

Specified by:

measurement in interface NonlinearSLAMFilter

Parameters:

h - the stochastic landmark measurement model

hInv - the inverse of h with respect to the landmark state

id - the identifier of the landmark that was observed

z - the landmark measurement

getLGFilter

public LGSLAMFilter getLGFilter()

Returns the underlying linear-Gaussian filter, which can be used to access the filtered belief state.

Returns:

the underlying linear-Gaussian filter

getRobotVariable

public Variable getRobotVariable()

Gets the state variable associated with the robot.

Specified by:

getRobotVariable in interface SLAMFilter

getLandmarkVariable

public Variable getLandmarkVariable(int id)

Gets the state variable associated with the landmark with the supplied identifier.

Specified by:

getLandmarkVariable in interface SLAMFilter

getLandmarkId

public int getLandmarkId(Variable lm)

Gets the identifier associated with the supplied landmark state variable.

Specified by:

getLandmarkId in interface SLAMFilter

getNumLandmarks

public int getNumLandmarks()

Returns the number of landmarks known to this filter.

Specified by:

getNumLandmarks in interface SLAMFilter

addLandmark

public Variable addLandmark(int id,
                            int dim)

Adds a new landmark to the map and returns its state variable.

Specified by:

addLandmark in interface SLAMFilter

getLandmarkIds

public int[] getLandmarkIds()

Returns the IDs of landmarks known to this filter.

Specified by:

getLandmarkIds in interface SLAMFilter

contains

public boolean contains(int id)

Return true if this filter contains the landmark with the supplied identifier.

Specified by:

contains in interface SLAMFilter

getRobotEstimate

public double[] getRobotEstimate()

Returns the filtered estimate of the robot's state.

Specified by:

getRobotEstimate in interface SLAMFilter

Returns:

the filtered estimate of the robot's state

getLandmarkEstimate

public double[] getLandmarkEstimate(int id)

Returns the filtered estimate of a landmark's state.

Specified by:

getLandmarkEstimate in interface SLAMFilter

Parameters:

id - the identifier of the landmark

Returns:

the filtered estimate of the landmark's state