test_linear_analytic_measurement_model_gaussian_uncertainty.cpp

一个很全的matlab工具包

// Test Program
// Tests the Linear Measurement Model with Gaussian uncertainty (additive)
// Measuring the (vertical) distance to a wall

#include <model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
#include <cmath> // For sinus
#include <pdf/gaussian.h>

#define MEASUREMENT_SIZE 1
#define STATE_SIZE 3
#define INPUT_SIZE 2
#define DELTA_T 1 // Delta t (for discretisation)
#define NUM_TIME_STEPS 15 // Number of steps that simulation is running

// Coordinates of wall
#define RICO_WALL 0.5
#define OFFSET_WALL 30

// Measurement noise
#define WALL_CT 1/(sqrt(pow(RICO_WALL,2) + 1))
#define MU_MEAS_NOISE OFFSET_WALL*WALL_CT
#define SIGMA_MEAS_NOISE 0.5

using namespace BFL;
using namespace std;
using namespace MatrixWrapper;


Matrix H(MEASUREMENT_SIZE,STATE_SIZE);

// We save all measurements/inputs for the test program
ColumnVector States[NUM_TIME_STEPS];
ColumnVector Measurements[NUM_TIME_STEPS];
ColumnVector MeasNoise_Mu(MEASUREMENT_SIZE);
SymmetricMatrix MeasNoise_Cov(MEASUREMENT_SIZE);

int main()
{
  cerr << "==================================================\n"
       << "Test program for linear analytic measurement model with\n" 
       << "additive gaussian noise: measuring the perpendicular\n"
       << "distance to a wall\n"
       << "==================================================" << endl;

  // Fill up H
  H[0][0] = WALL_CT * RICO_WALL;
  H[0][1] = 0 - WALL_CT;
  H[0][STATE_SIZE-1] = 0;
  
  // Construct the measurement noise (a scalar in this case)
  MeasNoise_Mu[0] = MU_MEAS_NOISE;
  MeasNoise_Cov[0][0] = SIGMA_MEAS_NOISE;
  
  // Initialisation of input sequence (TODO) (constant speed, no
  // rotation)
  (States[0]).resize(STATE_SIZE);
  States[0] = 0.0;
  ColumnVector Deplacement(STATE_SIZE);
#define DELTA_X 1.0
#define DELTA_Y 1.0
  Deplacement[0] = DELTA_X;   
  Deplacement[1] = DELTA_Y;
  Deplacement[2] = 0.0;

  Gaussian Measurement_Uncertainty(MeasNoise_Mu,MeasNoise_Cov);
  LinearAnalyticConditionalGaussian pdf(H,Measurement_Uncertainty);
  LinearAnalyticMeasurementModelGaussianUncertainty my_model(&pdf);

  double probability = 0.0;
  for (int current_time = 0; current_time < NUM_TIME_STEPS-1; current_time++)
    {
      (Measurements[current_time]).resize(MEASUREMENT_SIZE);
      Measurements[current_time] = my_model.Simulate(States[current_time]);
      probability = my_model.ProbabilityGet(Measurements[current_time],States[current_time]);

      cout << "At time " << current_time
	   << " distance = " << Measurements[current_time] << "\n" 
	   << " Probability of this simulated Measurement = " 
	   << probability << endl; 
      
      (States[current_time+1]).resize(STATE_SIZE);
      States[current_time+1] = States[current_time] + Deplacement;

    }

  Measurements[NUM_TIME_STEPS-1] = my_model.Simulate(States[NUM_TIME_STEPS-1]);

  return 0;
}