[Bayes++] EKF bayes++ questions

[Jack C. <jac...@dr...>]

Our research department is interested in using Bayes++ to do SLAM for
unmanned ground vehicles.  As a primer, I have recently started playing
around with Bayes++ to do a simple EKF localization filter to predict
the stat (x,y,theta) of a robot given a control U (v,w) of velocity and
steering.  The lack of documentation has made this difficult so I
thought I would send an email to ensure I am on the right track.  Here
are my questions.  Thanks in advance for your help.

1.  It appears as though I should be using the Covariance_scheme.  Is
this correct?

2.  My state prediction model is as follows:

	x' = x + (-(v/w)*sin(b) + (v/w)*sin(b+w*dt))
	y' = y + ((v/w)*cos(b) - (v/w)*cos(b+w*dt))
	theta' = theta + (w*dt)

I am wondering how I put this in the prediction model.  First of all,
I'm not sure whether I should be using a linrz_predict_model,
linear_predict_model or Gaussian_predict_model.  Second the matrix Fx
seems to implement x' = Fx(x) but I don't see how to incorporate the
control component into the prediction.  As of now I make Fx a 3x3
identity matrix, do the update and simply add the second term directly
on to the state.  Is this the correct way to handle this?  I've included
this code snippet below:

void Jpredict::stateUpdate( float dt, Vec& U,
Bayesian_filter::Covariance_scheme& kf  ) 
{
	float a = U[0] / U[1];

	Vec A(3);
	A[0] = -a*sin(kf.x[2]) + a*sin(kf.x[2]+U[1]*dt);
	A[1] = a*cos(kf.x[2]) - a*cos(kf.x[2]+U[1]*dt);;
	A[2] = U[1]*dt;
	
	//State transition matrix Fx is 3x3 identity
	Fx(0,0) = 1;
	Fx(0,1) = 0;
	Fx(0,2) = 0;
	Fx(1,0) = 0;
	Fx(1,1) = 1;
	Fx(0,2) = 0;
	Fx(2,0) = 0;
	Fx(2,1) = 0;
	Fx(2,2) = 1;
		
	kf.update();
	
	kf.x[0] = kf.x[0] + A[0];
	kf.x[1] = kf.x[1] + A[1];
	kf.x[2] = kf.x[2] + A[2];
	
	q[0] = 1;
        G(0,0) = 0.;
        G(1,0) = 1.;
}