javaslam.slam
Class TJTSLAMFilter

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

All Implemented Interfaces:

SLAMFilter

public class TJTSLAMFilter

extends LGSLAMFilter

A thin junction tree filter for the Simultaneous Localization and Mapping (SLAM) problem.

See Also:

"Thin Junction Tree Filters for Simultaneous Localization and Mapping. Mark Paskin. Technical Report UCB/CSD-02-1198, University of California, Berkeley, 2002."

Field Summary

protected ThinJunctionTree	jt The thin junction tree used to represent the filtered belief state.
protected int	overlap When the active cluster is cloned to admit a new landmark variable, this is the size of its separator.
protected int	width The cluster size limit of the thin junction tree.

Fields inherited from class javaslam.slam.AbstractSLAMFilter

id2lm, lm2id, x

Constructor Summary

TJTSLAMFilter(int width, int overlap, double[] mu, double[][] sigma)
Constructor.

Method Summary

JunctionTree	getJunctionTree() Gets the junction tree representation of the belief state.
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

jt

protected ThinJunctionTree jt

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

width

protected int width

The cluster size limit of the thin junction tree. If this is -1, then the junction tree is not thinned.

overlap

protected int overlap

When the active cluster is cloned to admit a new landmark variable, this is the size of its separator.

Constructor Detail

TJTSLAMFilter

public TJTSLAMFilter(int width,
                     int overlap,
                     double[] mu,
                     double[][] sigma)

Constructor.

Parameters:

width - the maximum number of variables permitted in a single cluster of the thin junction tree; this must be two or greater

overlap - the size of separators for clones; this must be positive and less than width

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

getJunctionTree

public JunctionTree getJunctionTree()

Gets the junction tree representation of the belief state.

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. Given the actual measurement z, this method updates the belief state as follows:

1. A measurement potential is constructed over the state variable of the robot state and that of the observed landmark.
2. If the landmark has not yet been observed and the active cluster is full (meaning its size is the width limit), then:
   1. the active cluster is cloned;
   2. the clone is made the active cluster
   3. the robot state variable is contracted to the active cluster
   4. the active cluster is thinned until its separator contains overlap variables
   Otherwise, the cluster closest to the active cluster that contains the state variable of the observed landmark is made the active cluster.
3. The measurement potential is multiplied into the active cluster.

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. This method updates the belief state as follows:

1. An evolution potential is constructed over the robot state variables at the current and the next time step.
2. This robot's state variable at the current time step is contracted to the active cluster.
3. The evolution potential is multiplied into the active cluster.
4. This robot's state variable at the current time step is marginalized out of the active cluster.
5. The robot's state variable at the next time step is renamed to be the robot's state variable at the current time step.

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. An odometry measurement potential over x is constructed and multiplied into the active cluster.

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. The distribution may be a product-of-marginals approximation if it is cheaper to compute.

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)