On the Structure and Solution of the Simultaneous Localisation and Map Building Problem. Paul Michael Newman.
1999. Ph. D. thesis, Australian Centre for Field Robotics - The University of Sydney
// ground truth of num_landmarks landmarks in the world coordinate
double landmark[num_landmarks];// = { 200, 600, 400 }; // x-position
for( int n = 0; n < num_landmarks; n++ )
{
landmark[n] = width * uniform_random();
}
// set the initial state
double rob_pos = 25.0; // initial robot position
double rob_vel = 10.0; // initial robot velocity
// set the initial covariance of the state
double rob_pos_cov = 0.01; // covariance of noise to robot position
double rob_vel_cov = 0.01; // covariance of noise to robot velocity
double obs_cov = 900; // covarriance of noise to measurement of landmarks
double xGroundTruth, vGroundTruth;
xGroundTruth = rob_pos;
vGroundTruth = rob_vel;
// H matrix
int Hrow = num_landmarks;
int Hcol = num_landmarks + 2;
CvMat* H = cvCreateMat(Hrow, Hcol, CV_64FC1);
cvZero(H); // initialize H matrix
// set H matrix
for (int row = 0; row < Hrow; row++)
{
cvmSet(H, row, 0, -1.0);
cvmSet(H, row, row+2, 1.0);
}
displayMatrix(H, "H matrix");
// Q matrix ; covariance of noise to H ; uncertainty of control
CvMat* Q = cvCreateMat(Hrow, Hrow, CV_64FC1);
cvZero(Q); // initialize Q matrix
// set Q matrix
for (int row = 0; row < Q->rows; row++)
{
cvmSet(Q, row, row, obs_cov);
}
displayMatrix(Q, "Q matrix");
// G matrix // transition
int Grow = num_landmarks + 2;
int Gcol = Grow;
CvMat* G = cvCreateMat(Grow, Gcol, CV_64FC1);
cvZero(G); // initialize G matrix
// set G matrix
cvmSet(G, 0, 0, 1.0); // previous position
cvmSet(G, 0, 1, 1.0); // velocity
for (int row = 1; row < Grow; row++)
{
cvmSet(G, row, row, 1.0); // constance of velocity
}
displayMatrix(G, "G matrix");
// R matrix ; covariance of noise to H ; uncertainty of observation
CvMat* R = cvCreateMat(Grow, Grow, CV_64FC1); // 5x5
cvZero(R); // initialize R matrix
// set R matrix
cvmSet(R, 0, 0, rob_pos_cov);
cvmSet(R, 1, 1, rob_vel_cov);
displayMatrix(R, "R matrix");
CvMat* mu = cvCreateMat(num_dim, 1, CV_64FC1); // state vector to be estimated
CvMat* rob_ground = cvCreateMat(num_dim, 1, CV_64FC1); // state vector to be estimated
CvMat* mu_p = cvCreateMat(num_dim, 1, CV_64FC1); // state to be predicted
CvMat* u = cvCreateMat(1, 1, CV_64FC1); // control vector
cvmSet(u, 0, 0, 1.0); // set u(0,0) to 1.0, the constant velocity here
CvMat* sigma = cvCreateMat(num_dim, num_dim, CV_64FC1); // covariance to be updated
CvMat* sigma_p = cvCreateMat(num_dim, num_dim, CV_64FC1); // covariance to be updated
CvMat* z = cvCreateMat(num_landmarks, 1, CV_64FC1); // measurement vector
CvMat* K = cvCreateMat(num_dim, num_landmarks, CV_64FC1); // K matrix // Kalman gain
// initialize "mu" vector
cvmSet(mu, 0, 0, rob_pos + sqrt(rob_pos_cov)*gaussian_random()); // set mu(0,0) to "rob_pos"
cvmSet(mu, 1, 0, rob_vel + sqrt(rob_vel_cov)*gaussian_random()); // set mu(0,0) to "rob_vel"
for(int n = 0; n < num_landmarks; n++)
{
// cvmSet(mu, n+2, 0, landmark[n] + sqrt(obs_cov)*gaussian_random());
cvmSet(mu, n+2, 0, landmark[n]);
}
displayMatrix(mu, "mu vector");
// initialize "sigma" matrix <-- This is the most critical point in tuning
cvSetIdentity(sigma, cvRealScalar(obs_cov));
displayMatrix(sigma, "sigma matrix");
// set the initial covariance of the state uncertainty
double rob_px_cov = 0.01; // covariance of noise to robot's x-position
double rob_py_cov = 0.01; // covariance of noise to robot's y-position
double rob_vx_cov = 0.01; // covariance of noise to robot's x-velocity
double rob_vy_cov = 0.01; // covariance of noise to robot's y-velocity
// set the initial covariance of the measurement noise
double obs_x_cov = 900; // covarriance of noise to x-measurement of landmarks
double obs_y_cov = 900; // covarriance of noise to y-measurement of landmarks
// ground truth of state
double xGroundTruth = rob_x;
double yGroundTruth = rob_y;
double vxGroundTruth = rob_vx;
double vyGroundTruth = rob_vy;
// ground truth of num_landmarks landmarks in the world coordinate
double landmark[2*num_landmarks];
for( int n = 0; n < num_landmarks; n++ )
{
landmark[2*n] = width * uniform_random();
landmark[2*n+1] = height * uniform_random();
}
// H matrix, measurement matrix
CvMat* H = cvCreateMat(2*num_landmarks, num_dim, CV_64FC1);
cvZero(H); // initialize H matrix
// set H matrix
for (int r = 0; r < num_landmarks; r++)
{
cvmSet(H, 2*r, 0, -1.0); // robot's x-position
cvmSet(H, 2*r, 2*r+4, 1.0); // landmark's x-position
cvmSet(H, 2*r+1, 1, -1.0); // robot's y-position
cvmSet(H, 2*r+1, 2*r+5, 1.0); // landmarks's y-position
}
displayMatrix(H, "H matrix");
// Q matrix ; covariance of noise to H; uncertainty of control
CvMat* Q = cvCreateMat(2*num_landmarks, 2*num_landmarks, CV_64FC1);
cvZero(Q); // initialize Q matrix
// set Q matrix
for (int row = 0; row < Q->rows; row++)
{
cvmSet(Q, row, row, obs_x_cov);
}
displayMatrix(Q, "Q matrix");
// G matrix // transition
CvMat* G = cvCreateMat(num_dim, num_dim, CV_64FC1);
cvZero(G); // initialize G matrix
// set G matrix
cvmSet(G, 0, 0, 1.0); // previous x-position
cvmSet(G, 0, 2, 1.0); // x-velocity
cvmSet(G, 1, 1, 1.0); // previous y-position
cvmSet(G, 1, 3, 1.0); // y-velocity
for (int row = 2; row < G->rows; row++)
{
cvmSet(G, row, row, 1.0); // constance of velocity
}
displayMatrix(G, "G matrix");
// R matrix ; covariance of noise to G; uncertainty of observation
CvMat* R = cvCreateMat(num_dim, num_dim, CV_64FC1);
cvZero(R); // initialize R matrix
// set R matrix
cvmSet(R, 0, 0, rob_px_cov);
cvmSet(R, 1, 1, rob_py_cov);
cvmSet(R, 2, 2, rob_vx_cov);
cvmSet(R, 3, 3, rob_vy_cov);
displayMatrix(R, "R matrix");
CvMat* rob_ground = cvCreateMat(num_dim, 1, CV_64FC1); // ground truth of state
CvMat* mu = cvCreateMat(num_dim, 1, CV_64FC1); // state vector to be estimated
CvMat* mu_p = cvCreateMat(num_dim, 1, CV_64FC1); // state vector to be predicted
CvMat* sigma = cvCreateMat(num_dim, num_dim, CV_64FC1); // covariance to be updated
CvMat* sigma_p = cvCreateMat(num_dim, num_dim, CV_64FC1); // covariance to be updated
CvMat* z = cvCreateMat(2*num_landmarks, 1, CV_64FC1); // measurement vector
CvMat* K = cvCreateMat(num_dim, 2*num_landmarks, CV_64FC1); // K matrix // Kalman gain
// initialize "mu" vector
cvmSet(mu, 0, 0, rob_x); // set mu(0,0) to "rob_x"
cvmSet(mu, 1, 0, rob_y); // set mu(1,0) to "rob_y"
cvmSet(mu, 2, 0, rob_vx); // set mu(2,0) to "rob_vx"
cvmSet(mu, 3, 0, rob_vy); // set mu(3,0) to "rob_vy"
for (int n = 0; n < 2*num_landmarks; n++)
{
cvmSet(mu, n+4, 0, landmark[n]); // set mu(4,0) to "landmark[0]", ...
}
displayMatrix(mu, "mu vector");
/* // initialize "sigma" matrix
cvmSet(sigma, 0, 0, rob_px_cov);
cvmSet(sigma, 1, 1, rob_py_cov);
cvmSet(sigma, 2, 2, rob_vx_cov);
cvmSet(sigma, 3, 3, rob_vy_cov);
for (int r = 4; r < sigma->rows; r=r*2)
{
cvmSet(sigma, r, r, obs_x_cov);
cvmSet(sigma, r+1, r+1, obs_y_cov);
}
*/ // initialize "sigma" matrix <-- This is the most critical point in tuning
cvSetIdentity(sigma, cvRealScalar(obs_x_cov));
displayMatrix(sigma, "sigma matrix");
// matrices to be used in calculation
CvMat* Hx = cvCreateMat(H->rows, mu->cols, CV_64FC1);
CvMat* Gt = cvCreateMat(G->cols, G->rows, CV_64FC1);
cvTranspose(G, Gt); // transpose(G) -> Gt
CvMat* sigmaGt = cvCreateMat(sigma->rows, G->rows, CV_64FC1);
CvMat* GsigmaGt = cvCreateMat(G->rows, G->rows, CV_64FC1); // 10x10
// observation relative to robot's position
for( int n = 0; n < 2*num_landmarks; n++ )
{
cvmSet(obs, n, 0, cvmGet(mu,0,0) + cvmGet(z,n,0));
}
// update the state with Kalman gain (ref. L5, EKF algorithm, 42p)
cvMatMul(H, mu, Hmu); // H * mu -> Hmu
cvSub(z, Hmu, miss); // z - Hmu -> miss
cvMatMul(K, miss, adjust); // K * miss -> adjust
cvAdd(mu_p, adjust, mu); // mu_p + adjust -> mu
displayMatrix(mu, "mu vector");
Kalman filter
: estimates system states that can only be observed indirectly or inaccurately by the system itself.
: estimates the variables of a wide range of processes.
: estimates the states of a linear system.
: minimizes the variance of the estimation error
Linear system
x: state of the system
u: known input to the system
y: measured output
w: process noise
z: measurement noise
http://wiki.answers.com/Q/What_is_a_feedback_system
A feedback system, in general engineering terms, is a system whose
output if fed back to the input, and depending on the output, your
input is adjusted so as to reach a steady-state. In colloquial
language, you adjust your input based on the output of your system so
as to achieve a certain end, like minimizing disturbance, cancelling
echo (in a speech system) and so on.
Criteria of an Estimator
1) The expected value of the estimate should be equal to the expected value of the state.
2) The estimator should be with the smallest possible error variance.
Requirement of Kalman filter
1) The average value of w is zero and average value of z is zero.
2) No correlation exists between w and z. w_k and z_k are independent random variables.
Kalman filter equations
K matrix: Kalman gain
P matrix: estimation error covariance
http://en.wikipedia.org/wiki/Three_sigma_rule
In statistics, the 68-95-99.7 rule, or three-sigma rule, or empirical rule, states that for a normal distribution, nearly all values lie within 3 standard deviations of the mean.
"steady state Kalman filter"
- K matrix & P matrix are constant
"extended Kalman filter"
: an extension of linear Kalman filter theory to nonlinear systems
"Kalman smoother"
: to estimate the state as a function of time so to reconstruct the trajectory after the fact
H infinity filter
=> correlated noise problem
=> unknown noise covariances problem
spacecraft navigation for the Apollo space program
> applications
all forms of navigation (aerospace, land, and marine)
nuclear power plant instrumentation
demographic modeling
manufacturing
the detection of underground radioactivity
fuzzy logic and neural network training
Gelb, A. Applied Optimal Estimation. Cambridge, MA: MIT Press, 1974.
Anderson, B. and J. Moore. Optimal Filtering. Englewood Cliffs, NJ: Prentice-Hall, 1979.
Grewal, M. and A. Andrews. Kalman Filtering Theory and Practice. Englewood Cliffs, NJ: Prentice-Hall, 1993.
Sorenson, H. Kalman Filtering: Theory and Application. Los Alamitos, CA: IEEE Press, 1985.
Peter Joseph’s Web site @http://ourworld.compuserve.com/homepages/PDJoseph/
CvMat* state = cvCreateMat(2, 1, CV_64FC1); //states to be estimated
CvMat* state_p = cvCreateMat(2, 1, CV_64FC1); //states to be predicted
CvMat* velocity = cvCreateMat(2, 1, CV_64FC1); //motion controls to change states
CvMat* measurement = cvCreateMat(2, 1, CV_64FC1); //measurement of states
CvMat* cov = cvCreateMat(2, 2, CV_64FC1); //covariance to be updated
CvMat* cov_p = cvCreateMat(2, 2, CV_64FC1); //covariance to be predicted
CvMat* gain = cvCreateMat(2, 2, CV_64FC1); //Kalman gain to be updated
// temporary matrices to be used for estimation
CvMat* Kalman = cvCreateMat(2, 2, CV_64FC1); //
CvMat* invKalman = cvCreateMat(2, 2, CV_64FC1); //
CvMat* I = cvCreateMat(2,2,CV_64FC1);
cvSetIdentity(I); // does not seem to be working properly
// cvSetIdentity (I, cvRealScalar (1));
// check matrix
for(int i=0; i<2; i++)
{
for(int j=0; j<2; j++)
{
cout << cvmGet(I, i, j) << "\t";
}
cout << endl;
}
// set the initial state
cvmSet(state, 0, 0, 0.0); //x-value //set state(0,0) to 0.0
cvmSet(state, 1, 0, 0.0); //y-value //set state(1,0) to 0.0
// set the initital covariance of state
cvmSet(cov, 0, 0, 0.5); //set cov(0,0) to 0.5
cvmSet(cov, 0, 1, 0.0); //set cov(0,1) to 0.0
cvmSet(cov, 1, 0, 0.0); //set cov(1,0) to 0.0
cvmSet(cov, 1, 0, 0.4); //set cov(1,1) to 0.4
// set the initial control
cvmSet(velocity, 0, 0, 10.0); //x-direction //set velocity(0,0) to 1.0
cvmSet(velocity, 1, 0, 10.0); //y-direction //set velocity(0,0) to 1.0
for (int t=0; t<step; t++)
{
// retain the current state
CvMat* state_out = cvCreateMat(2, 1, CV_64FC1); // temporary vector
cvmSet(state_out, 0, 0, cvmGet(state,0,0));
cvmSet(state_out, 1, 0, cvmGet(state,1,0));
// predict
cvAdd(state, velocity, state_p); // state + velocity -> state_p
cvAdd(cov, transition_noise, cov_p); // cov + transition_noise -> cov_p
// measure
cvmSet(measurement, 0, 0, measurement_set[t]); //x-value
cvmSet(measurement, 1, 0, measurement_set[step+t]); //y-value
// estimate Kalman gain
cvAdd(cov_p, measurement_noise, Kalman); // cov_p + measure_noise -> Kalman
cvInvert(Kalman, invKalman); // inv(Kalman) -> invKalman
cvMatMul(cov_p, invKalman, gain); // cov_p * invKalman -> gain
// update the state
CvMat* err = cvCreateMat(2, 1, CV_64FC1); // temporary vector
cvSub(measurement, state_p, err); // measurement - state_p -> err
CvMat* adjust = cvCreateMat(2, 1, CV_64FC1); // temporary vector
cvMatMul(gain, err, adjust); // gain*err -> adjust
cvAdd(state_p, adjust, state); // state_p + adjust -> state
// update the covariance of states
CvMat* cov_up = cvCreateMat(2, 2, CV_64FC1); // temporary matrix
cvSub(I, gain, cov_up); // I - gain -> cov_up
cvMatMul(cov_up, cov_p, cov); // cov_up *cov_p -> cov
// update the control
cvSub(state, state_out, velocity); // state - state_p -> velocity
Techniques for Generating Consistent Maps
• Scan matching
• EKF SLAM
• Fast-SLAM
• Probabilistic mapping with a single map and a posterior about poses Mapping + Localization
• Graph-SLAM, SEIFs
Approximations for SLAM
• Local submaps
[Leonard et al.99, Bosse et al. 02, Newman et al. 03]
• Sparse links (correlations)
[Lu & Milios 97, Guivant & Nebot 01]
• Sparse extended information filters
[Frese et al. 01, Thrun et al. 02]
• Thin junction tree filters
[Paskin 03]
• Rao-Blackwellisation (FastSLAM)
[Murphy 99, Montemerlo et al. 02, Eliazar et al. 03, Haehnel et al. 03]
EKF-SLAM Summary
•Quadratic in the number of landmarks: O(n2)
• Convergence results for the linear case.
• Can diverge if nonlinearities are large!
• Have been applied successfully in large-scale environments.
• Approximations reduce the computational complexity.
This paper appears in: Computer Vision and Pattern Recognition, 2006 IEEE Computer Society Conference on
Publication Date: 17-22 June 2006
Volume: 1, On page(s): 469- 476
ISSN: 1063-6919
ISBN: 0-7695-2597-0
Digital Object Identifier: 10.1109/CVPR.2006.263
Current Version Published: 2006-07-05
monocular SLAM
particle filter + top-down search => real-time, large number of landmarks
the first to apply this FastSLAM-type particle filter to single-camera SLAM
1. Introduction
SLAM = Simultaneous Localization and Mapping
: process of causally estimating both egomotion and structure in an online system
SLAM using visual data in computer vision
SFM (= structure from motion): reconstructing scene geometry
+ causal or recursive estimation techniques
perspective-projection cameras
filtering methods to allow indirect observation models
Kalman filtering framework
Extended Kalman filter = EKF (-> to linearize the observation and dynamics models of the system)
causal estimation with recursive algorithms (cp. estimation depending only on observations up to the current time)
=> online operation (cp. SFM on global nonlinear optimization)
Davision's SLAM with a single camera
> EKF estimation framework
> top-down Bayesian estimation approach searching for landmarks in image regions constrained by estimate > uncertainty (instead of performing extensive bottom-up image processing and feature matching)
> Bayesian partial-initialization scheme for incorporating new landmarks
- cannot scale to large environment
EKF = the Extended Kalman filter
- N*N covariace matrix for N landmarks
- updated with N*N computation cost
> SLAM system using a single camera as the only sensor
> frame-rate operation with many landmarks
> FastSLAM-style particle filter (the first use of such an approach in a monocular SLAM setting)
> top-down active search
> an efficient algorithm for discovering the depth of new landmarks that avoids linearization errors
> a novel method for using partially initialized landmarks to help constrain camera pose
FastSLAM
: based on the Rao-Blackwellized Particle Filter
2. Background
2.1 Scalable SLAM
> submap
bounded complexity -> bounded computation and space requirements
Montemerlo & Thrun
If the entire camera motion is known then the estimates of the positions of different landmarks become independent of each other.
PaulNewmanThesis.pdf
