mobile_robot_models.h

一个很全的matlab工具包

// Copyright (C) 2003 Klaas Gadeyne <klaas dot gadeyne at mech dot kuleuven dot ac dot be>
//  
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//  
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//  
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//  

/* File containing the system and observation models of the mobile
   robot localization with respect to wall example used for testing
   different Bayesian filters.
*/

#include "mobile_robot_wall_cts.h"

/* BUILDING THE SYSTEM MODEL and MEASUREMENT MODEL

SYSTEM MODEL

We suppose a robot is moving according to simple motion equations

x_k      = x_{k-1} + v_{k-1} * cos(theta_{k-1} * delta_t
y_k      = y_{k-1} + v_{k-1} * sin(theta_{k-1} * delta_t
theta_k  = theta_{k-1} + omega_{k-1} * delta_t


or

X_k = A * X_{k-1} + B * U_k

where
     
A = ones(STATE_SIZE) and

cos(theta_{k-1})   0
B = delta_t *  sin(theta_{k-1})   0
0                  1

MEASUREMENT MODEL

and measuring the (perpendicular) distance z to the wall y = ax + b

set WALL_CT = 1/sqrt(pow(a,2) + 1)

z = WALL_CT * a * x - WALL_CT * y + WALL_CT * b + GAUSSIAN_NOISE

or Z = H * X_k + J * U_k

where
     
H = [ WALL_CT * a       - WALL_CT      0 ]

and GAUSSIAN_NOISE = N((WALL_CT * b), SIGMA_MEAS_NOISE)

*/


using namespace BFL;

// Matrices
Matrix A(STATE_SIZE,STATE_SIZE);
Matrix B(STATE_SIZE,INPUT_SIZE);
Matrix H(MEAS_SIZE,STATE_SIZE);
vector<Matrix> v(2);

// We save all inputs/measurements for the test program
vector<ColumnVector> Inputs;
vector<ColumnVector> Measurements;

// Pink Floyd (aka noise :-)
ColumnVector SysNoise_Mu(STATE_SIZE);
SymmetricMatrix SysNoise_Cov(STATE_SIZE);
ColumnVector MeasNoise_Mu(MEAS_SIZE);
SymmetricMatrix MeasNoise_Cov(MEAS_SIZE);

// Prior
ColumnVector prior_mu(STATE_SIZE);  
SymmetricMatrix prior_sigma(STATE_SIZE);

// Measurement Noise
double wall_ct = 1/(sqrt(pow(DEFAULT_RICO_WALL,2.0) + 1));
double mu_meas_noise = DEFAULT_OFFSET_WALL * wall_ct;
unsigned int row, column, current_time; // indices

// Pdfs and models for the system model
Gaussian * System_Uncertainty = NULL;
LinearAnalyticConditionalGaussian * syspdf = NULL;
LinearAnalyticSystemModelGaussianUncertainty * sys_model = NULL;

// Pdfs and models for the measurement model
Gaussian * Measurement_Uncertainty = NULL;
LinearAnalyticConditionalGaussian * measpdf = NULL;
LinearAnalyticMeasurementModelGaussianUncertainty * meas_model = NULL;

// Prior
Gaussian * Prior_cont = NULL;

// Filter
Filter<ColumnVector,ColumnVector> * filter_p = NULL;

// Posterior
Pdf<ColumnVector> * post_p = NULL;

void init_system_model()
{
  SysNoise_Mu = 0.0;
  // Uncertainty or Noice (Additive) and Matrix A
  for (row=0; row < STATE_SIZE; row++)
    {
      for (column=0; column < STATE_SIZE; column++)
	{
	  // Noise and Matrix A
	  if (row == column) 
	    {
	      SysNoise_Cov[row][column] = SIGMA_SYSTEM_NOISE;
	      A[row][column]=1;
	    }
	  else 
	    {
	      SysNoise_Cov[row][column] = COR_SYSTEM_NOISE;
	      A[row][column]=0;
	    }
	}
    }
  // No noise on rotation
  SysNoise_Cov[STATE_SIZE-1][STATE_SIZE-1]=0.0001;
  
  if (System_Uncertainty == NULL)
    System_Uncertainty = new Gaussian(SysNoise_Mu, SysNoise_Cov);
  else
    {
      System_Uncertainty->ExpectedValueSet(SysNoise_Mu);
      System_Uncertainty->CovarianceSet(SysNoise_Cov);
    }

  // B consists of static and time-varying components
  // TIME-FIXED COMPONENTS OF B
  B[STATE_SIZE-1][0] = 0.0; B[0][INPUT_SIZE-1] = 0.0; B[1][INPUT_SIZE-1] = 0.0;
  B[STATE_SIZE-1][INPUT_SIZE-1] = 1.0;
  // TIME-VARYING INIT
  B[0][0] = cos(PRIOR_MU_THETA) * DELTA_T;
  B[1][0] = sin(PRIOR_MU_THETA) * DELTA_T;
  v[0] = A;
  v[1] = B;
  if (syspdf == NULL)
    syspdf = new LinearAnalyticConditionalGaussian(v,*System_Uncertainty);
  if (sys_model == NULL)
    sys_model = new LinearAnalyticSystemModelGaussianUncertainty(syspdf);
}

void init_measurement_model()
{
  // Fill up H
  H[0][0] = wall_ct * DEFAULT_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;
  
  if (Measurement_Uncertainty == NULL)
    Measurement_Uncertainty = new Gaussian(MeasNoise_Mu,MeasNoise_Cov);
  else 
    {
      Measurement_Uncertainty->ExpectedValueSet(MeasNoise_Mu);
      Measurement_Uncertainty->CovarianceSet(MeasNoise_Cov);
    }
  if (measpdf == NULL)
    measpdf = new LinearAnalyticConditionalGaussian(H,*Measurement_Uncertainty);
  if (meas_model == NULL)
    meas_model = new LinearAnalyticMeasurementModelGaussianUncertainty(measpdf);
}


void init_prior()
{
  /* The prior is a gaussian centered around [ PRIOR_MU_X
                                               PRIOR_MU_Y
                                               PRIOR_MU_THETA ]
  */
  // Creating a (continuous) Gaussian with prior_mu and prior_sigma
  prior_mu[0] = PRIOR_MU_X;
  prior_mu[1] = PRIOR_MU_Y;
  prior_mu[STATE_SIZE-1] = PRIOR_MU_THETA;
  for (row=0; row < STATE_SIZE; row++)
    {
      for (column=0; column < STATE_SIZE; column++)
	{
	  if (column == row) prior_sigma[row][column]=PRIOR_COV;
	  else prior_sigma[row][column]=PRIOR_COR;
	}
    }
  if (Prior_cont == NULL)
    Prior_cont = new Gaussian(prior_mu,prior_sigma);
  else
    {
      Prior_cont->CovarianceSet(prior_sigma);
      Prior_cont->ExpectedValueSet(prior_mu);
    }
}

// Initial State
/* This should not be a global variable, but we currently use these
   variables to update B in our dynamic system model instead of mean of
   the Posterior density
*/
vector<ColumnVector> States;

void simulate_measurements(unsigned int num_time_steps,
			   double lin_speed,
			   double rot_speed)
{
  /*****************************************
   * SIMULATION OF INPUTS AND MEASUREMENTS *
   *****************************************/

  // SIMULATION SYSTEM MODEL (no noise), to estimate the measurements
  SymmetricMatrix SysNoise_Cov_SIM(STATE_SIZE);
#define SIM_FACTOR 1000
  SysNoise_Cov_SIM = SysNoise_Cov/SIM_FACTOR;
  Gaussian System_Uncertainty_SIM(SysNoise_Mu,SysNoise_Cov_SIM);
  v[0] = A;
  v[1] = B;
  LinearAnalyticConditionalGaussian pdf(v,System_Uncertainty_SIM);
  LinearAnalyticSystemModelGaussianUncertainty sys_model_SIM(&pdf);

  Inputs.resize(num_time_steps);
  Measurements.resize(num_time_steps);
  States.resize(num_time_steps);
  
  States[0] = prior_mu;

  // Generating the input sequence (constant speed, no rotation)
  // And simulating the measurements (starting from noiseless system)
  Measurements[0]=meas_model->Simulate(States[0]);

  for (current_time=0; 
       current_time < num_time_steps - 1; 
       current_time++)
    {
      // Set inputs to constant values
      // resize to avoid segfaults, has something to do with the
      // declaration as ColumnVector tralala[]
      (Inputs[current_time]).resize(INPUT_SIZE);
      (Inputs[current_time])[0] = lin_speed;
      (Inputs[current_time])[INPUT_SIZE-1] = rot_speed;

      // Simulate the next state of the noiseless system
      // UPDATE B MATRIX TIME-VARYING COMPONENTS
      // Use the mean value of the current estimate to update this!
      // Currently we use the estimation values!
      B[0][0] = cos((States[current_time])[STATE_SIZE-1]) * DELTA_T;
      B[1][0] = sin((States[current_time])[STATE_SIZE-1]) * DELTA_T;
      sys_model_SIM.BSet(B);

      (States[current_time+1]).resize(STATE_SIZE);
      States[current_time+1] = sys_model_SIM.Simulate(States[current_time],
						      Inputs[current_time]);


#ifdef __DEBUG__
      cerr << "\nMAIN: states " << current_time+1 << " :\n"
	   << States[current_time+1];
#endif // __DEBUG__

      /* Simulate the measurement to be used by the filter using the
	 current state obtained by simulation.  NOTE THAT WE DON'T USE
	 Measurements[0] !!, NOR Inputs[num_time_steps-1]!!!
      */
      (Measurements[current_time+1]).resize(MEAS_SIZE);
      Measurements[current_time+1]=meas_model->Simulate(States[current_time+1]);

#ifdef __DEBUG__
      double probability = meas_model->ProbabilityGet(Measurements[current_time+1],States[current_time+1]);
      cerr << "\nMAIN: Meas " << current_time+1 << ": " << Measurements[current_time+1]
	   << "Prob : " << probability << "\n";
#endif // __DEBUG__

    }
}

// UPDATE B MATRIX TIME-VARYING COMPONENTS after first time
// Use the mean value of the current estimate to update this!
// Currently we use the estimation values!
void update_matrix_B(int current_time)
{
  if (current_time > 0)
    {
      B[0][0] = cos((States[current_time-1])[STATE_SIZE-1]) * DELTA_T;
      B[1][0] = sin((States[current_time-1])[STATE_SIZE-1]) * DELTA_T;
      sys_model->BSet(B);
    }
  else
    {
      B[STATE_SIZE-1][0] = 0.0; B[0][INPUT_SIZE-1] = 0.0; B[1][INPUT_SIZE-1] = 0.0;
      B[STATE_SIZE-1][INPUT_SIZE-1] = 1.0;
      // TIME-VARYING INIT
      B[0][0] = cos(PRIOR_MU_THETA) * DELTA_T;
      B[1][0] = sin(PRIOR_MU_THETA) * DELTA_T;
    }
}

void cleanup()
{
  // if (Prior_cont) delete Prior_cont;
  if (meas_model) delete meas_model;
  if (measpdf) delete measpdf;
  if (Measurement_Uncertainty) delete Measurement_Uncertainty;
  if (syspdf) delete syspdf;
  if (sys_model) delete sys_model;
  if (System_Uncertainty) delete System_Uncertainty;
}