javaslam.slam
Class KalmanSLAMFilter

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

All Implemented Interfaces:

SLAMFilter

public class KalmanSLAMFilter

extends LGSLAMFilter

A Kalman filter for the Simultaneous Localization and Mapping (SLAM) problem.

This class records counts of all floating point operations using Flops.count(long) (except those used in the service of debugging and avoiding numerical errors).

Field Summary

protected KalmanFilter

kf The underlying Kalman filter.

Fields inherited from class javaslam.slam.AbstractSLAMFilter

id2lm, lm2id, x

Constructor Summary

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

Method Summary

KalmanFilter	getKalmanFilter() Returns the underlying Kalman filter used by this SLAM filter.
Gaussian	getLandmarkMarginal(int id) Returns the filtered marginal potential over a landmark's state (in the moment parameterization).
Gaussian	getRobotLandmarkMarginal(int id) This method computes the joint filtered distribution over the states of the robot and a landmark.
Gaussian	getRobotMarginal() Returns the filtered marginal potential 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, getLandmarkMarginals, getRobotEstimate, getRobotLandmarkMarginals

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

kf

protected KalmanFilter kf

The underlying Kalman filter.

Constructor Detail

KalmanSLAMFilter

public KalmanSLAMFilter(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; if id identifies a new landmark, then D must be invertible

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

getKalmanFilter

public KalmanFilter getKalmanFilter()

Returns the underlying Kalman filter used by this SLAM filter.

Returns:

the underlying Kalman filter used by this SLAM filter