First we build the linear model that links the distance measurement to the position of the mobile robot.
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 is a helper constant and 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 :
ColumnVector measNoise_Mu(1); measNoise_Mu(1) = MU_MEAS_NOISE;and the covariance is diagonal matrix, with as the 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 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: