javaslam.slam
Class LGSLAMFilter

java.lang.Object
  |
  +--javaslam.slam.AbstractSLAMFilter
        |
        +--javaslam.slam.LGSLAMFilter

All Implemented Interfaces:

SLAMFilter

Direct Known Subclasses:

InformationSLAMFilter, KalmanSLAMFilter, TJTSLAMFilter

public abstract class LGSLAMFilter

extends AbstractSLAMFilter

A linear-Gaussian filter for the Simultaneous Localization and Mapping (SLAM) problem.

Field Summary

Fields inherited from class javaslam.slam.AbstractSLAMFilter

id2lm, lm2id, x

Constructor Summary

LGSLAMFilter(int dim)
Constructor.

Method Summary

double[]	getLandmarkEstimate(int id) Returns the filtered estimate of a landmark's state.
abstract Gaussian	getLandmarkMarginal(int id) Returns the filtered marginal potential over a landmark's state (in the moment parameterization).
Gaussian[]	getLandmarkMarginals(int[] ids) Given a collection of landmark state variables, this method returns the set of filtered marginal potentials over each individual landmark's state.
double[]	getRobotEstimate() Returns the filtered estimate of the robot's state.
abstract Gaussian	getRobotLandmarkMarginal(int id) This method computes the joint filtered distribution over the states of the robot and a landmark.
Gaussian[]	getRobotLandmarkMarginals(int[] ids) Given a set of landmarks { l1, ..., lk}, this method computes a set of potentials over (x, l1), ..., (x, lk).
abstract Gaussian	getRobotMarginal() Returns the filtered marginal potential over the robot's state (in the moment parameterization).
abstract void	measurement(int id, double[] z0, double[][] C, double[][] D, double[][] R, double[] z) Performs a linear-Gaussian landmark measurement update.
abstract void	motion(double[] x0, double[][] A, double[][] Q) Performs a linear-Gaussian motion update.
abstract void	odometry(double[] y0, double[][] B, double[][] S, double[] y) Performs a linear-Gaussian odometry update.

Methods inherited from class javaslam.slam.AbstractSLAMFilter

addLandmark, contains, getLandmarkId, getLandmarkIds, getLandmarkVariable, getNumLandmarks, getRobotVariable

Methods inherited from class java.lang.Object

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

Constructor Detail

LGSLAMFilter

public LGSLAMFilter(int dim)

Constructor.

Parameters:

dim - the dimension of the robot state variable

Method Detail

measurement

public abstract void measurement(int id,
                                 double[] z0,
                                 double[][] C,
                                 double[][] D,
                                 double[][] R,
                                 double[] z)

Performs a linear-Gaussian landmark measurement update. The parameters z0, C, D, and R define the measurement equation as follows:

z=z0 + Cx + Dlm + w

where x is the robot state variable, lm is a landmark variable, and w is a white-noise variable with covariance R.

Parameters:

id - the identifier of the landmark that generated this observation; if it is not currently in the belief state, then it is first added with an uninformative prior.

z0 - a k-vector giving the constant term

C - a k by n matrix that defines the linear coefficient for the robot's state

D - a k by m matrix that defines the linear coefficient for the landmark's state

R - a k by k symmetric positive definite matrix giving the covariance of the measurement white noise

z - the measurement k-vector

Throws:

IllegalArgumentException - if there are any dimension mismatches

motion

public abstract void motion(double[] x0,
                            double[][] A,
                            double[][] Q)

Performs a linear-Gaussian motion update. The parameters A and Q define the state evolution equation as follows:

x_{t + 1} = x0 + Ax_t+v

where v is a white-noise variable with covariance Q.

Parameters:

x0 - an n-vector giving the constant term of the state evolution equation

A - an n by n evolution matrix that defines the linear evolution model

Q - an n by n symmetric positive definite matrix giving the covariance of the evolution white noise

Throws:

IllegalArgumentException - if there are any dimension mismatches

odometry

public abstract void odometry(double[] y0,
                              double[][] B,
                              double[][] S,
                              double[] y)

Performs a linear-Gaussian odometry update. The parameters y0, B, and S define the odometry measurement equation as follows:

y_{t + 1}=y0 + Bx_{t + 1}+u

where u is a white-noise variable with covariance S.

Parameters:

y0 - an h-vector giving the constant term of the odometry equation

B - an h by n evolution matrix that is linear term of the odometry equation

S - an h by h symmetric positive definite matrix giving the covariance of the odometry white noise

y - an h-vector giving the odometry measurement

Throws:

IllegalArgumentException - if there are any dimension mismatches

getRobotMarginal

public abstract Gaussian getRobotMarginal()

Returns the filtered marginal potential over the robot's state (in the moment parameterization).

Returns:

the filtered marginal potential over the robot's state.

getLandmarkMarginal

public abstract Gaussian getLandmarkMarginal(int id)

Returns the filtered marginal potential over a landmark's state (in the moment parameterization).

Returns:

the filtered marginal potential over a landmark's state

getLandmarkMarginals

public Gaussian[] getLandmarkMarginals(int[] ids)

Given a collection of landmark state variables, this method returns the set of filtered marginal potentials over each individual landmark's state.

Parameters:

ids - an array of landmark identifiers (or null to indicate all landmarks)

Returns:

a corresponding array of marginals in the moment parameterization

getRobotLandmarkMarginal

public abstract Gaussian getRobotLandmarkMarginal(int id)

This method computes the joint filtered distribution over the states of the robot and a landmark.

Parameters:

id - the landmark's identifier

Returns:

the joint filtered distribution over the states of the robot and a landmark (in the moment parameterization)

getRobotLandmarkMarginals

public Gaussian[] getRobotLandmarkMarginals(int[] ids)

Given a set of landmarks { l₁, ..., l_k}, this method computes a set of potentials over (x, l₁), ..., (x, l_k). Each potential is either the filtered marginal over the robot and landmark states.

Parameters:

ids - an array of landmark identifiers (or null to indicate all landmarks)

Returns:

an array of (perhaps approximate) marginal potentials over the corresponding landmark and robot state variables (in the moment parameterization)

getRobotEstimate

public double[] getRobotEstimate()

Returns the filtered estimate of the robot's state.

Specified by:

getRobotEstimate in interface SLAMFilter

Specified by:

getRobotEstimate in class AbstractSLAMFilter

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

Specified by:

getLandmarkEstimate in class AbstractSLAMFilter

Parameters:

id - the identifier of the landmark

Returns:

the filtered estimate of the landmark's state