javaslam.slam
Class InformationSLAMFilter

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

All Implemented Interfaces:

SLAMFilter

public class InformationSLAMFilter

extends LGSLAMFilter

An information filter for the Simultaneous Localization and Mapping (SLAM) problem.

Field Summary

protected InformationFilter

pf The thin junction tree used to represent the filtered belief state.

Fields inherited from class javaslam.slam.AbstractSLAMFilter

id2lm, lm2id, x

Constructor Summary

InformationSLAMFilter(double[] mu, double[][] sigma)
Constructor.

Method Summary

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.
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).
Gaussian	getRobotMarginal() Returns the filtered marginal potential (in the moment parameterization) over the robot's state.
void	measurement(int id, double[] z0, double[][] C, double[][] D, double[][] R, double[] z) Performs a linear-Gaussian landmark measurement update.
void	motion(double[] x0, double[][] A, double[][] Q) Performs a linear-Gaussian motion update.
void	odometry(double[] y0, double[][] B, double[][] S, double[] y) Performs a linear-Gaussian odometry update.

Methods inherited from class javaslam.slam.LGSLAMFilter

getLandmarkEstimate, getRobotEstimate

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

Field Detail

pf

protected InformationFilter pf

The thin junction tree used to represent the filtered belief state.

Constructor Detail

InformationSLAMFilter

public InformationSLAMFilter(double[] mu,
                             double[][] sigma)

Constructor.

Parameters:

mu - a n-vector containing the starting state of the robot

sigma - an n-by-n positive semidefinite covariance matrix giving the uncertainty in the robot's initial state

Method Detail

measurement

public 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.

Specified by:

measurement in class LGSLAMFilter

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 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.

Specified by:

motion in class LGSLAMFilter

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 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.

Specified by:

odometry in class LGSLAMFilter

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 Gaussian getRobotMarginal()

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

Specified by:

getRobotMarginal in class LGSLAMFilter

Returns:

the filtered marginal potential over the robot's state.

getLandmarkMarginal

public Gaussian getLandmarkMarginal(int id)

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

Specified by:

getLandmarkMarginal in class LGSLAMFilter

Returns:

the filtered marginal potential over a landmark's state

getRobotLandmarkMarginal

public Gaussian getRobotLandmarkMarginal(int id)

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

Specified by:

getRobotLandmarkMarginal in class LGSLAMFilter

Parameters:

id - the landmark's identifier

Returns:

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

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.

Overrides:

getLandmarkMarginals in class LGSLAMFilter

Parameters:

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

Returns:

a corresponding array of marginals 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, or a product-of-marginals approximation thereof.

Overrides:

getRobotLandmarkMarginals in class LGSLAMFilter

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)