// $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.0using 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;}