The nonlinear system model

In contrast with the system model of the previous model, the system model which includes the orientation of the robot is no longer linear.

The noise on the system model is still considered Gaussian and is therefore implemented as explained in the first tutorial. We create a Gaussian distribution, which is defined by a mean (Mu) and a covariance (Cov). This Gaussian distribution represents the uncertainty on the predicted position of the mobile robot. The mean values are defined in the file $mobile\_robot\_wall\_cts.h$:

  ColumnVector sys_noise_Mu(STATE_SIZE);
  sys_noise_Mu(1) = MU_SYSTEM_NOISE_X;
  sys_noise_Mu(2) = MU_SYSTEM_NOISE_Y;
  sys_noise_Mu(3) = MU_SYSTEM_NOISE_THETA;

and the covariance is chosen as a diagonal matrix, with the digaonal values defined in the file $mobile\_robot\_wall\_cts.h$:

  SymmetricMatrix sys_noise_Cov(STATE_SIZE);
  sys_noise_Cov = 0.0;
  sys_noise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
  sys_noise_Cov(1,2) = 0.0;
  sys_noise_Cov(1,3) = 0.0;
  sys_noise_Cov(2,1) = 0.0;
  sys_noise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;
  sys_noise_Cov(2,3) = 0.0;
  sys_noise_Cov(3,1) = 0.0;
  sys_noise_Cov(3,2) = 0.0;
  sys_noise_Cov(3,3) = SIGMA_SYSTEM_NOISE_THETA;

The mean and covariance together define a three dimensional Gaussian distribution:

  Gaussian system_Uncertainty(sys_noise_Mu, sys_noise_Cov);

Now we have to create a non linear conditional probability density function (pdf) which represents the probability of the predicted position given the current position of the mobile robot. BFL does not provide a class for such a non linear conditional pdf^2.2. Therefore we have to implement it ourselves. We give the class we'll create a specific name for this example: NonLinearAnalyticConditionalGaussianMobile. BFL provides a general class of an analytic conditional pdf with Gaussian uncertainty from which we can inherit the functionality: AnalyticConditionalGaussianAdditiveNoise. Indeed, our pdf is analytic and has some additive Gaussian noise.
To generate our own class we start with the implementation of the header file: nonlinearanalyticconditionalgaussianmobile.h.
We start the class implementation with the inclusion of the header of the class we inherit from:

  #include <pdf/analyticconditionalgaussian_additivenoise.h>

The class will be created in the BFL namespace and obviously needs a constructor and destructor. The constructor expects a Gaussian representing the system noise.

  namespace BFL
  {
   class NonLinearAnalyticConditionalGaussianMobile : 
    public AnalyticConditionalGaussianAdditiveNoise
   {
    public:
     NonLinearAnalyticConditionalGaussianMobile(const Gaussian& additiveNoise);
     virtual ~NonLinearAnalyticConditionalGaussianMobile();
   };
  }

When we look at the documentation of AnalyticConditionalGaussianAdditiveNoise, we notice that there are at least two methods we have to implement when inheriting:

• ColumnVector ExpectedValueGet(),
  which will return the expected value of the conditional pdf and depends on our problem specific process equations.
• Matrix dfGet(unsigned int i),
  which will return the derivative to the i'th conditional argument. In this case the derivative to the state (first conditional argument) is only relevant.

Moreover this are the only two functions which needs re-implementation. Therefore we add to following declarations to the public part of the header file:

  virtual MatrixWrapper::ColumnVector    ExpectedValueGet() 	const;
  virtual MatrixWrapper::Matrix          dfGet(unsigned int i)  const;

The header file is ready, and we can now switch to the cpp file for the implementation: nonlinearanalyticconditionalgaussianmobile.cpp.
The conditional arguments of the conditional pdf, the state and the input (velocity), are private variables of the conditional pdf.
Now, we switch to the implementation of ExpectedValueGet(). To calculate the expected value of the conditional pdf, the current state and input are necessary:

  ColumnVector state = ConditionalArgumentGet(0);
  ColumnVector vel  = ConditionalArgumentGet(1);

The expected value is then calculated and returned by:

  state(1) += cos(state(3)) * vel(1);
  state(2) += sin(state(3)) * vel(1);
  state(3) += vel(2);
  return state + AdditiveNoiseMuGet();

Now, only the implementation of dfGet(unsigned int i) is left. We will only need the derivative according to the first conditional variable which is the state. To calculate this derivative again the current conditional arguments are necessary:

  ColumnVector state = ConditionalArgumentGet(0);
  ColumnVector vel = ConditionalArgumentGet(1);

The derivative according to the state is a 3x3 matrix calculated by:

  Matrix df(3,3);
  df(1,1)=1;
  df(1,2)=0;
  df(1,3)=-vel(1)*sin(state(3));
  df(2,1)=0;
  df(2,2)=1;
  df(2,3)=vel(1)*cos(state(3));
  df(3,1)=0;
  df(3,2)=0;
  df(3,3)=1;

Finally the function still has to return the calculated derivative:

  return df;

The implementation of our own system model is terminated. We can now create an instance of the non linear conditional probability density function.

  NonLinearAnalyticConditionalGaussianMobile sys_pdf(system_Uncertainty);

Finally we create the system model from the system pdf:

  AnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);