The nonlinear system model

For this particle filter, we could directly use the non linear system model that was presented in the previous example with the extended Kalman filter. However, we wanted to make this system model more general, and show how to build a general non linear system model that is not limited to Gaussian additional noise.

The noise on the system model is still considered Gaussian (just because this is easy, but you're not limited by Gaussian noise like in the previous tutorial examples with Kalman filters). 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 is chosen to be $MU\_SYSTEM\_NOISE\_X$, $MU\_SYSTEM\_NOISE\_Y$ and $MU\_SYSTEM\_NOISE\_THETA$ for the $x$, $y$-position and angle respectively :

  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 $0.01^2$ as the $\sigma^2$ boundary:

  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 general 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, and therefore we have to implement it ourselves. We give the class we'll create a specific name for this example: NonlinearSystemPdf. BFL provides a general class of a conditional pdf from which we can inherit the functionality: ConditionalPdf. This is the most general conditional pdf.

To generate our own class we start with the implementation of the header file: nonlinearSystemPdf.h.

We start the class implementation with the inclusion of the header of the class we inherit from:

  #include <pdf/conditionalpdf.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 NonLinearSystemPdf: 
     public ConditionalPdf<MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector>
   {
    public:
     NonlinearSystemPdf( const Gaussian& additiveNoise);
     virtual ~NonlinearSystemPdf();
   };
  }

When we look at the documentation of ConditionalPdf, we notice that there are many unimplemented methods. However, for a system model of a particle filter, we only need to implement one of these functions:

• bool SampleFrom (...);

This function allows the particle filter to sample from the system distribution. To implement this function, we add to following declaration to the public part of the header file:

  virtual bool SampleFrom (Sample<MatrixWrapper::ColumnVector>& one_sample, ...)

The header file is ready, and we can now switch to the cpp file for the implementation: nonlinearSystemPdf.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 SampleFrom(...). 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);

Then we take one sample from the additive Gaussian noise:

  Sample<ColumnVector> noise;
  _additiveNoise.SampleFrom(noise, method, args);

And finally store the result in the output variable of this function:

  one_sample.ValueSet(state + noise.ValueGet());

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

  NonlinearSystemPdf sys_pdf(system_Uncertainty);

Finally we create the system model from the system pdf:

  SystemModel<ColumnVector> sys_model(&sys_pdf);