The linear measurement model

We'll now build the measurement model, to correct the prediction of the position of the mobile robot, based on the distance measurement to the wall. Since we use a linear measurement model in this example, BFL already includes a ready-to-use implementation.

First we build the linear model that links the distance measurement to the position of the mobile robot.

$\displaystyle {z}_{k+1} = {H} {x}_{k+1}$ (2.2)

We build the matrix $ {H}$:
  Matrix H(1,2);
  double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
  H = 0.0;
  H(1,1) = wall_ct * RICO_WALL;
  H(1,2) = 0 - wall_ct;
where $ wall\_ct$ is a helper constant and $ RICO\_WALL$ is the slope of the wall. Then we create a Gaussian distribution, which is defined by a mean (Mu) and a covariance (Cov). This Gaussian distribution represents the uncertainty on the measured distance to the wall. The mean is $ MU\_MEAS\_NOISE$:
  ColumnVector measNoise_Mu(1);
  measNoise_Mu(1) = MU_MEAS_NOISE;
and the covariance is diagonal matrix, with $ SIGMA_MEAS_NOISE$ as the $ \sigma^2$ boundary:
  SymmetricMatrix measNoise_Cov(1);
  measNoise_Cov(1,1) = SIGMA_MEAS_NOISE;
The mean and covariance together define a two dimensional Gaussian distribution:
  Gaussian measurement_Uncertainty(measNoise_Mu, measNoise_Cov);

Now we create a linear probability density function (pdf) which represents the probability of the distance measurement. This pdf is defined by the matrix $ {H}$ which represents the linear model, together with the Gaussian distribution which represents the measurement uncertainty.

  LinearAnalyticConditionalGaussian meas_pdf(H, measurement_Uncertainty);
Finally we create the measurement model from the measurement pdf:
  LinearAnalyticMeasurementModelGaussianUncertainty meas_model(&meas_pdf);

fiep 2010-10-28