test_linear_analytic_system_model_gaussian_uncertainty.cpp

一个很全的matlab工具包

// $Id: test_linear_analytic_system_model_gaussian_uncertainty.cpp,v 2.17 2004/11/12 08:57:34 kgadeyne Exp $
// Tests the Linear System Model with Gaussian uncertainty (additive)
// Mobile Robot Dead Reckoning
// Actually the linear system isn't linear at all :-)

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

#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

#define SIGMA_NOISE 0.0001 // Noise variance (constant for every input here)
#define CORRELATION_NOISE 0.0 // Correlation between different noise (idem)

#define LINEAR_SPEED 1.0
#define ROT_SPEED 0.0

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


int main()
{
  cerr << "==================================================\n"
       << "Test program for linear analytic system model with\n" 
       << "additive gaussian noise: deadreckoning of mobile robot\n"
       << "==================================================" << endl;

  Matrix A(STATE_SIZE,STATE_SIZE);
  Matrix B(STATE_SIZE,INPUT_SIZE);

  // We save all states/inputs for the test program
  ColumnVector States[NUM_TIME_STEPS];
  ColumnVector Inputs[NUM_TIME_STEPS];



  // Uncertainty or Noice (Additive)
  ColumnVector Noise_Mu(STATE_SIZE);
  for (int row=0; row < STATE_SIZE; row++){Noise_Mu[row] = 0;}    
  SymmetricMatrix Noise_Cov(STATE_SIZE);
  for (int row=0; row < STATE_SIZE; row++)
    {
      for (int column=0; column < STATE_SIZE; column++)
	{
	  if (row == column) {Noise_Cov[row][column] = SIGMA_NOISE;}
	  else {Noise_Cov[row][column] = CORRELATION_NOISE;}
	}
    }
  Gaussian System_Uncertainty(Noise_Mu,Noise_Cov);
 
  int current_time; // Time index

  // MATRIX A: unit matrix 
  // TODO: make function ones() to fill this up!
  for (int row=0; row < STATE_SIZE; row++)
    {
      for (int column=0; column < STATE_SIZE; column++)
	{
	  if (row == column) A[row][column]=1;
	  else A[row][column]=0;
	}
    }

  // Initial position Position at time 0
  ColumnVector start_state(STATE_SIZE); 
  for (int row=0; row < STATE_SIZE; row++){start_state[row] = 0;}    
  States[0] = start_state;
  
  // Initialisation of input sequence (TODO) (constant speed, no rotation)
  for (int time=0; time < NUM_TIME_STEPS; time++)
    {
      // First resize the matrices to avoid segfaults
            (States[time]).resize(STATE_SIZE);
            (Inputs[time]).resize(INPUT_SIZE);
      // Actual speed initialisation
      (Inputs[time])[0] = LINEAR_SPEED;
      (Inputs[time])[INPUT_SIZE-1] = ROT_SPEED;

      cout << "Input at time " << time << ":\n" << Inputs[time] << "\n";
    }

  vector<Matrix> v(2);
  v[0] = A;
  v[1] = B;
  LinearAnalyticConditionalGaussian pdf(v,System_Uncertainty);
  LinearAnalyticSystemModelGaussianUncertainty my_model(&pdf);
  //  TIME-FIXED COMPONENTS
  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;

  // Initialize state
  for (current_time = 1; current_time < NUM_TIME_STEPS; current_time++)
    {
      // UPDATE B MATRIX TIME-VARYING COMPONENTS
      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;
      
      // UPDATE SYSTEM MODEL
      my_model.BSet(B);

      // Simulate system
      States[current_time] = my_model.Simulate(States[current_time-1],
					       Inputs[current_time-1]);
      cout << "At time " << current_time << " we're at (x,y,and theta)\n" << States[current_time] << endl; 
    }

  return 0;
}