Re: [Bayes++] Bayes++ to predict robots location

[Michael S. <ma...@mi...>]

On Monday, 13. November 2006 07:09, Vinh wrote:
> Have been fiddeling arround for an hour. Could it be that I need to
> replace the linear prediction model with an "Unscented_predict_model"?
> I would derive from the mentioned model and then overwrite the
> function "f" to insert my own state transition function? Same with the
> noise i.e. covariance matrix Q?

That is correct. I would probably use 'Addative_predict_model' (in 
bayesFlt.hpp) which is the most general model that fits your problem. It can 
be used by the predict functions in the Unscented_scheme.

> >
> > Since the motion/prediction model is not linear due to the rotation,
> > what would I need to change to modify it so that the filter can deal
> > with non-linearities?

Yes. The most simple (Kinematic) model you can use for your 2d robot position 
estimator requires 3 states. Position in cartessian coordinates (x[0],x[1]) 
plus the orientation of robot (x[2]).

class Predict3 : public Addative_predict_model
{
public:
  Predict3 : ();
  const Vec& f(const Vec& x) const;
  void odometry (float speed, rotation) {
   cur_speed = speed; cur_rotation = rotation; 
  }

  mutable Vec xp;
  float cur_speed, cur_rotation;
};

The mutable 'xp' can be used as the return value for the 'f' function. In the 
class constructors it can be constructed with it fixed size of 3 elements.

Using the simplist kinematic model (bicycle model) the robot travels purely in 
the direction it points. In this case your predicted function 'f' is

xp[0] += cur_speed * cos(x[2]);
xp[1] += cur_speed * sin(x[2]);
xp[2] += cur_rotation;

All angles in radians. Before you ask the filter to predict, you simple call 
the 'odometry' function to tell it the input from your wheel encoders.

With the 'f' function the Unscented filter will automatically determine how 
the non-linearity effects future state and how they are correlated.


> >
> > In the state prediction (Linear_predict_model), there is something
> > looking like this:
> >
> >         q[0] = dt*sqr((1-Fvv)*V_NOISE);
> >         G(0,0) = 0.;
> >         G(1,0) = 1.;
> >
> > Can I leave it like that if I assume that the noise is always constant?

The noise model is the model is critical in such robot applications. If you 
want to get really good results you need to take a look at how you can model 
such physical effects such as the slip and slide of the wheels and wheel 
radius.

Alternatively you can use a simple assumed noise model. This can be fixed as 
above. But since you know the speed and rotation inputs it is probably better 
no make the noise proportional to these values.

The 'dt*sqr((1-Fvv)*V_NOISE)' noise from the PV example needs a bit of work to 
extend to the 2d robot case. It would probably be best to start with simple 
addative noise proportional to the inputs. If you assume addative white 
velocity noise that independant for each of the 3 states you have:

q[0] = dt* speed * SPEED_POSITION_NOISE^2;
q[1] = dt* speed * SPEED_POSITION_NOISE^2;
q[2] = dt* (speed * SPEED_ORIENTATION_NOISE^2 + rotation * 
ROTATION_ORIENTATION_NOISE^2;

This leaves you with three tuneable parameters.
	SPEED_POSITION_NOISE - how speed uncertainty effects position
	SPEED_ORIENTATION_NOISE - how speed uncertainty effects orientation
	ROTATION_ORIENTATION_NOISE - how rotation uncertainty effects orientation

You could probably have a ROTATION_POSITION_NOISE parameter, but once you go 
to this complexity you will probably want to go to a more physically 
realistic model!

All the best,
	Michael
-- 
___________________________________
Michael Stevens Systems Engineering

34128 Kassel, Germany
Phone/Fax: +49 561 5218038

Navigation Systems, Estimation  and
                 Bayesian Filtering
    http://bayesclasses.sf.net
___________________________________