bayesclasses-general

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

[Vinh <arb...@go...>]

Hi,
I'm just started playing around with the bayes++ library to accomplish
following task:

A vision system provides me with the position of our robot on the
ground (2d). In addition, I want to merge this information with the
internal wheelencoders (giving me the speed and rotation of the robot)
to get an estimate of the position of the robot.
Since the robot is rotating as well the system model is not linear.
First I thought of using a particle filter to get the estimate, but
that probably would be overkill, making the system slower than needed
since the underlying probability distribution could simply be one
gaussian.
I had a look at the PV example, but got stucked and would like to ask
for advice.

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?

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?

Thanks very much for your help!!

Vinh

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

[Vinh <arb...@go...>]

This is what I have so far. I've tried changing the observation to be
a 2D position and the state itself the same. It compiles and runs,
however the results a far from what I expected.
Somehow the values of the filter stay very close to the one of the
initial estimate which would mean that the initial estimate's
covariance should be very small or the observations covariance very
big - none of this intended. Anyone can help?

here's some sample output. The file, derived from the PV example, is attached.

----
True     [2](7.180000e+01,1.436000e+02)
Direct   [2](1.435908e+02,2.871707e+02),[2,2]((8.571428e-05,0.000000e+00),(0.000000e+00,8.571429e-05))
True     [2](7.190000e+01,1.438000e+02)
Direct   [2](1.437973e+02,2.875657e+02),[2,2]((8.571428e-05,0.000000e+00),(0.000000e+00,8.571429e-05))
True     [2](7.200000e+01,1.440000e+02)
Direct   [2](1.439857e+02,2.879593e+02),[2,2]((8.571428e-05,0.000000e+00),(0.000000e+00,8.571429e-05))
True     [2](7.210000e+01,1.442000e+02)
Direct   [2](1.441731e+02,2.883677e+02),[2,2]((8.571428e-05,0.000000e+00),(0.000000e+00,8.571429e-05))
------

On 11/13/06, Vinh <arb...@go...> 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?
>
> Vinh
>
> On 11/13/06, Vinh <arb...@go...> wrote:
> > Hi,
> > I'm just started playing around with the bayes++ library to accomplish
> > following task:
> >
> > A vision system provides me with the position of our robot on the
> > ground (2d). In addition, I want to merge this information with the
> > internal wheelencoders (giving me the speed and rotation of the robot)
> > to get an estimate of the position of the robot.
> > Since the robot is rotating as well the system model is not linear.
> > First I thought of using a particle filter to get the estimate, but
> > that probably would be overkill, making the system slower than needed
> > since the underlying probability distribution could simply be one
> > gaussian.
> > I had a look at the PV example, but got stucked and would like to ask
> > for advice.
> >
> > 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?
> >
> > 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?
> >
> > Thanks very much for your help!!
> >
> > Vinh
> >
>

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

[Vinh <arb...@go...>]

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?

Vinh

On 11/13/06, Vinh <arb...@go...> wrote:
> Hi,
> I'm just started playing around with the bayes++ library to accomplish
> following task:
>
> A vision system provides me with the position of our robot on the
> ground (2d). In addition, I want to merge this information with the
> internal wheelencoders (giving me the speed and rotation of the robot)
> to get an estimate of the position of the robot.
> Since the robot is rotating as well the system model is not linear.
> First I thought of using a particle filter to get the estimate, but
> that probably would be overkill, making the system slower than needed
> since the underlying probability distribution could simply be one
> gaussian.
> I had a look at the PV example, but got stucked and would like to ask
> for advice.
>
> 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?
>
> 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?
>
> Thanks very much for your help!!
>
> Vinh
>

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
___________________________________

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

[Nicola B. <nb...@es...>]

Vinh,
I didn't have time to look at the entire code, but for sure the following 
doesn't look very correct:

const Vec& PVpredict::f(const Vec& x) const
{
  // Functional part of addative model
  // Note: Reference return value as a speed optimisation, MUST be copied by 
caller.
  Vec* v = new Vec(2); 
  (*v)[0] = x[0] + 0.1;
  (*v)[1] = x[1] + 0.2;
  return *v;
}

Everytime this member returns a reference to a _new_ location, allocated with 
a _local_ pointer. Well, I guess that's not what you want...
Regards
Nicola


On Monday 13 Nov 2006 14:04, Vinh wrote:
> This is what I have so far. I've tried changing the observation to be
> a 2D position and the state itself the same. It compiles and runs,
> however the results a far from what I expected.
> Somehow the values of the filter stay very close to the one of the
> initial estimate which would mean that the initial estimate's
> covariance should be very small or the observations covariance very
> big - none of this intended. Anyone can help?
>
> here's some sample output. The file, derived from the PV example, is
> attached.
>
> ----
> True     [2](7.180000e+01,1.436000e+02)
> Direct  
> [2](1.435908e+02,2.871707e+02),[2,2]((8.571428e-05,0.000000e+00),(0.000000e
>+00,8.571429e-05)) True     [2](7.190000e+01,1.438000e+02)
> Direct  
> [2](1.437973e+02,2.875657e+02),[2,2]((8.571428e-05,0.000000e+00),(0.000000e
>+00,8.571429e-05)) True     [2](7.200000e+01,1.440000e+02)
> Direct  
> [2](1.439857e+02,2.879593e+02),[2,2]((8.571428e-05,0.000000e+00),(0.000000e
>+00,8.571429e-05)) True     [2](7.210000e+01,1.442000e+02)
> Direct  
> [2](1.441731e+02,2.883677e+02),[2,2]((8.571428e-05,0.000000e+00),(0.000000e
>+00,8.571429e-05)) ------
>
> On 11/13/06, Vinh <arb...@go...> 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?
> >
> > Vinh
> >
> > On 11/13/06, Vinh <arb...@go...> wrote:
> > > Hi,
> > > I'm just started playing around with the bayes++ library to accomplish
> > > following task:
> > >
> > > A vision system provides me with the position of our robot on the
> > > ground (2d). In addition, I want to merge this information with the
> > > internal wheelencoders (giving me the speed and rotation of the robot)
> > > to get an estimate of the position of the robot.
> > > Since the robot is rotating as well the system model is not linear.
> > > First I thought of using a particle filter to get the estimate, but
> > > that probably would be overkill, making the system slower than needed
> > > since the underlying probability distribution could simply be one
> > > gaussian.
> > > I had a look at the PV example, but got stucked and would like to ask
> > > for advice.
> > >
> > > 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?
> > >
> > > 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?
> > >
> > > Thanks very much for your help!!
> > >
> > > Vinh

-- 
------------------------------------------
Nicola Bellotto
University of Essex
Department of Computer Science
Wivenhoe Park
Colchester CO4 3SQ
United Kingdom

Room: 1N1.2.8
Tel. +44 (0)1206 874094
URL: http://privatewww.essex.ac.uk/~nbello
------------------------------------------

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

[Vinh <arb...@go...>]

Hi Nicola,
if there's any chance you can have a look at the code I would greatly
appreciate it. The project is due in two days and I'm basically
stucked with this. I was wondering whether I get the concept right,
not too much of memory leaks. Should I overwrite the functions "f" and
"Q" and are those corresponding to the system motion model (f) and
covariance of the motion model (Q)? But if I measure a simple 2D
position while my state is a 2d position as well, should I use a
Linrz_uncorrelated_observe_model like in the PV example?
I found out that I used the random functions wrong (was too late at
night). But that didn't fix the problem that the estimate constantly
jumps towards the measurement, even though their covariance is quite
high.

Regards,
Vinh

On 11/14/06, Nicola Bellotto <nb...@es...> wrote:
> Vinh,
> I didn't have time to look at the entire code, but for sure the following
> doesn't look very correct:
>
> const Vec& PVpredict::f(const Vec& x) const
> {
>   // Functional part of addative model
>   // Note: Reference return value as a speed optimisation, MUST be copied by
> caller.
>   Vec* v = new Vec(2);
>   (*v)[0] = x[0] + 0.1;
>   (*v)[1] = x[1] + 0.2;
>   return *v;
> }
>
> Everytime this member returns a reference to a _new_ location, allocated with
> a _local_ pointer. Well, I guess that's not what you want...
> Regards
> Nicola
>
>
> On Monday 13 Nov 2006 14:04, Vinh wrote:
> > This is what I have so far. I've tried changing the observation to be
> > a 2D position and the state itself the same. It compiles and runs,
> > however the results a far from what I expected.
> > Somehow the values of the filter stay very close to the one of the
> > initial estimate which would mean that the initial estimate's
> > covariance should be very small or the observations covariance very
> > big - none of this intended. Anyone can help?
> >
> > here's some sample output. The file, derived from the PV example, is
> > attached.
> >
> > ----
> > True     [2](7.180000e+01,1.436000e+02)
> > Direct
> > [2](1.435908e+02,2.871707e+02),[2,2]((8.571428e-05,0.000000e+00),(0.000000e
> >+00,8.571429e-05)) True     [2](7.190000e+01,1.438000e+02)
> > Direct
> > [2](1.437973e+02,2.875657e+02),[2,2]((8.571428e-05,0.000000e+00),(0.000000e
> >+00,8.571429e-05)) True     [2](7.200000e+01,1.440000e+02)
> > Direct
> > [2](1.439857e+02,2.879593e+02),[2,2]((8.571428e-05,0.000000e+00),(0.000000e
> >+00,8.571429e-05)) True     [2](7.210000e+01,1.442000e+02)
> > Direct
> > [2](1.441731e+02,2.883677e+02),[2,2]((8.571428e-05,0.000000e+00),(0.000000e
> >+00,8.571429e-05)) ------
> >
> > On 11/13/06, Vinh <arb...@go...> 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?
> > >
> > > Vinh
> > >
> > > On 11/13/06, Vinh <arb...@go...> wrote:
> > > > Hi,
> > > > I'm just started playing around with the bayes++ library to accomplish
> > > > following task:
> > > >
> > > > A vision system provides me with the position of our robot on the
> > > > ground (2d). In addition, I want to merge this information with the
> > > > internal wheelencoders (giving me the speed and rotation of the robot)
> > > > to get an estimate of the position of the robot.
> > > > Since the robot is rotating as well the system model is not linear.
> > > > First I thought of using a particle filter to get the estimate, but
> > > > that probably would be overkill, making the system slower than needed
> > > > since the underlying probability distribution could simply be one
> > > > gaussian.
> > > > I had a look at the PV example, but got stucked and would like to ask
> > > > for advice.
> > > >
> > > > 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?
> > > >
> > > > 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?
> > > >
> > > > Thanks very much for your help!!
> > > >
> > > > Vinh
>
> --
> ------------------------------------------
> Nicola Bellotto
> University of Essex
> Department of Computer Science
> Wivenhoe Park
> Colchester CO4 3SQ
> United Kingdom
>
> Room: 1N1.2.8
> Tel. +44 (0)1206 874094
> URL: http://privatewww.essex.ac.uk/~nbello
> ------------------------------------------
>
> -------------------------------------------------------------------------
> Using Tomcat but need to do more? Need to support web services, security?
> Get stuff done quickly with pre-integrated technology to make your job easier
> Download IBM WebSphere Application Server v.1.0.1 based on Apache Geronimo
> http://sel.as-us.falkag.net/sel?cmd=lnk&kid=120709&bid=263057&dat=121642
> _______________________________________________
> Bayesclasses-general mailing list
> Bay...@li...
> https://lists.sourceforge.net/lists/listinfo/bayesclasses-general
>

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

[Nicola B. <nb...@es...>]

Vinh,
In general, if you use a Linrz_* model, you implement the prediction "f" (or 
observation "h") and the relative Jacobian "Fx" (or "Hx"), for example for 
the EKF. Other filters, like UKF, don't need linearized models instead.
Have a look inside "bayesFlt.hpp" to get more information about models.
Regards,
Nicola

On Monday 13 Nov 2006 22:56, you wrote:
> Hi Nicola,
> if there's any chance you can have a look at the code I would greatly
> appreciate it. The project is due in two days and I'm basically
> stucked with this. I was wondering whether I get the concept right,
> not too much of memory leaks. Should I overwrite the functions "f" and
> "Q" and are those corresponding to the system motion model (f) and
> covariance of the motion model (Q)? But if I measure a simple 2D
> position while my state is a 2d position as well, should I use a
> Linrz_uncorrelated_observe_model like in the PV example?
> I found out that I used the random functions wrong (was too late at
> night). But that didn't fix the problem that the estimate constantly
> jumps towards the measurement, even though their covariance is quite
> high.
>
> Regards,
> Vinh
>
> On 11/14/06, Nicola Bellotto <nb...@es...> wrote:
> > Vinh,
> > I didn't have time to look at the entire code, but for sure the following
> > doesn't look very correct:
> >
> > const Vec& PVpredict::f(const Vec& x) const
> > {
> >   // Functional part of addative model
> >   // Note: Reference return value as a speed optimisation, MUST be copied
> > by caller.
> >   Vec* v = new Vec(2);
> >   (*v)[0] = x[0] + 0.1;
> >   (*v)[1] = x[1] + 0.2;
> >   return *v;
> > }
> >
> > Everytime this member returns a reference to a _new_ location, allocated
> > with a _local_ pointer. Well, I guess that's not what you want...
> > Regards
> > Nicola
> >
> > On Monday 13 Nov 2006 14:04, Vinh wrote:
> > > This is what I have so far. I've tried changing the observation to be
> > > a 2D position and the state itself the same. It compiles and runs,
> > > however the results a far from what I expected.
> > > Somehow the values of the filter stay very close to the one of the
> > > initial estimate which would mean that the initial estimate's
> > > covariance should be very small or the observations covariance very
> > > big - none of this intended. Anyone can help?
> > >
> > > here's some sample output. The file, derived from the PV example, is
> > > attached.
> > >
> > > ----
> > > True     [2](7.180000e+01,1.436000e+02)
> > > Direct
> > > [2](1.435908e+02,2.871707e+02),[2,2]((8.571428e-05,0.000000e+00),(0.000
> > >000e +00,8.571429e-05)) True     [2](7.190000e+01,1.438000e+02)
> > > Direct
> > > [2](1.437973e+02,2.875657e+02),[2,2]((8.571428e-05,0.000000e+00),(0.000
> > >000e +00,8.571429e-05)) True     [2](7.200000e+01,1.440000e+02)
> > > Direct
> > > [2](1.439857e+02,2.879593e+02),[2,2]((8.571428e-05,0.000000e+00),(0.000
> > >000e +00,8.571429e-05)) True     [2](7.210000e+01,1.442000e+02)
> > > Direct
> > > [2](1.441731e+02,2.883677e+02),[2,2]((8.571428e-05,0.000000e+00),(0.000
> > >000e +00,8.571429e-05)) ------
> > >
> > > On 11/13/06, Vinh <arb...@go...> 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?
> > > >
> > > > Vinh
> > > >
> > > > On 11/13/06, Vinh <arb...@go...> wrote:
> > > > > Hi,
> > > > > I'm just started playing around with the bayes++ library to
> > > > > accomplish following task:
> > > > >
> > > > > A vision system provides me with the position of our robot on the
> > > > > ground (2d). In addition, I want to merge this information with the
> > > > > internal wheelencoders (giving me the speed and rotation of the
> > > > > robot) to get an estimate of the position of the robot.
> > > > > Since the robot is rotating as well the system model is not linear.
> > > > > First I thought of using a particle filter to get the estimate, but
> > > > > that probably would be overkill, making the system slower than
> > > > > needed since the underlying probability distribution could simply
> > > > > be one gaussian.
> > > > > I had a look at the PV example, but got stucked and would like to
> > > > > ask for advice.
> > > > >
> > > > > 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?
> > > > >
> > > > > 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?
> > > > >
> > > > > Thanks very much for your help!!
> > > > >
> > > > > Vinh
> >
> > --
> > ------------------------------------------
> > Nicola Bellotto
> > University of Essex
> > Department of Computer Science
> > Wivenhoe Park
> > Colchester CO4 3SQ
> > United Kingdom
> >
> > Room: 1N1.2.8
> > Tel. +44 (0)1206 874094
> > URL: http://privatewww.essex.ac.uk/~nbello
> > ------------------------------------------
> >
> > -------------------------------------------------------------------------
> > Using Tomcat but need to do more? Need to support web services, security?
> > Get stuff done quickly with pre-integrated technology to make your job
> > easier Download IBM WebSphere Application Server v.1.0.1 based on Apache
> > Geronimo
> > http://sel.as-us.falkag.net/sel?cmd=lnk&kid=120709&bid=263057&dat=121642
> > _______________________________________________
> > Bayesclasses-general mailing list
> > Bay...@li...
> > https://lists.sourceforge.net/lists/listinfo/bayesclasses-general

-- 
------------------------------------------
Nicola Bellotto
University of Essex
Department of Computer Science
Wivenhoe Park
Colchester CO4 3SQ
United Kingdom

Room: 1N1.2.8
Tel. +44 (0)1206 874094
URL: http://privatewww.essex.ac.uk/~nbello
------------------------------------------