test_conditionalgaussian.cpp

一个很全的matlab工具包

// $Id: test_conditionalgaussian.cpp,v 2.22 2004/11/12 09:05:52 kgadeyne Exp $
// Copyright (C) 2002 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.
//  

// TEST program to check the Gaussian class
#include <pdf/linearanalyticconditionalgaussian.h>
#include <sample/sample.h>
#include <matrix_wrapper.h>
#include <vector_wrapper.h>

#include <iostream>
#include <fstream>

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

#define NUM_SAMPLES 1000
#define DIMENSION 2 // Size of the state space dimension (before the | )

#define NUM_CONDITIONAL_ARGS 2
#define NEW_NUM_CONDITIONAL_ARGS 3

#define STATE_SIZE 2 // Sizes of the resp. 1st and 2nd argument after |
#define INPUT_SIZE 2 

#define PLOT_LIMIT 4
#define PLOT_INTERVAL 0.1

ofstream functionfile("data_visualisation/function_conditionalgaussian.dat", ios::out);
ofstream samplesfile("data_visualisation/samples_conditionalgaussian.dat", ios::out);

int main()
{
  cerr << "==================================================\n"
       << "Test of the Conditional Gaussian Class for linear\n"
       << "analytic systems\n"
       << "=================================================="
       << endl;


  // Creating a Gaussian with mu and sigma
  ColumnVector mu(DIMENSION);  
  SymmetricMatrix sigma(DIMENSION);
  mu = 1.5; sigma = 0.0;

  for (int i=0; i < DIMENSION; i++)
    {
      for (int j=0; j < DIMENSION; j++)
	{
	  if (i == j) sigma[i][j]=1.0;
	}
    }

  cout << "mu_noise = \n" << mu 
       << "\nsigma_noise = \n" << sigma << endl;

  // Conditional gaussian with 2 conditional args and state vector of
  // dimension 2

  Matrix a(2,2); a = 1.0;
  Matrix b(2,2); b = 1.0;

  cout << "A = \n" << a << "\nB = \n" << b << endl;
  
  cerr << "Creating Additive Noise Gaussian" << endl;
  Gaussian My_Noise(mu,sigma);
  vector<Matrix> v(2); v[0] = a; v[1] = b;

  cerr << "Creating Linear Conditional Gaussian with 2 conditional arguments" << endl;
  LinearAnalyticConditionalGaussian My_CondGaussian(v, My_Noise);

  // Setting the conditional args;
  ColumnVector x(STATE_SIZE); x[0] = 1.0; x[STATE_SIZE-1] = 0.5;
  ColumnVector u(INPUT_SIZE); u[0] = 1.0; u[INPUT_SIZE-1] = 0.5;
  
  std::vector<ColumnVector> cond_args(NUM_CONDITIONAL_ARGS);
  cond_args[0] = x; 
  cond_args[NUM_CONDITIONAL_ARGS-1] = u;

  My_CondGaussian.ConditionalArgumentsSet(cond_args);

  cout << "Value of conditional args = \n" << endl;
  ColumnVector temp_result;
  for (int i=0; i < NUM_CONDITIONAL_ARGS; i++)
    {
      temp_result = My_CondGaussian.ConditionalArgumentGet(i);
      cout << temp_result << endl;
    }

  // Test some member functions
  cout << "Expected Value = \n" << My_CondGaussian.ExpectedValueGet() 
       << endl;
  cout << "Covariance Matrix = \n" << My_CondGaussian.CovarianceGet() 
       << endl;

  // Plotting the function
  ColumnVector state(DIMENSION); double pdf_value;
  if (functionfile)
    {
      for (double state_x = (0-PLOT_LIMIT) ; state_x <= PLOT_LIMIT; state_x += PLOT_INTERVAL)
	{
	  for (double state_y = (0-PLOT_LIMIT) ; state_y <= PLOT_LIMIT; state_y += PLOT_INTERVAL)
	    {
	      state[0] = state_x; state[1] = state_y;
	      pdf_value = My_CondGaussian.ProbabilityGet(state);
	      functionfile << state[0] << " " << state[1] << " " << pdf_value << "\n";
	    }
	}
      functionfile.close();
    }
  else  
    { 
      cerr << "Error writing to file\n" << endl;
    }


  // Generating Samples with cholesky sampling
  cerr << "Generating samples with Cholesky sampling" << endl;
  
  list<Sample<ColumnVector> > samples(NUM_SAMPLES);
  list<Sample<ColumnVector> >::iterator it;
  samples = My_CondGaussian.SampleFrom(NUM_SAMPLES,CHOLESKY,NULL);

  // Plot the list of samples
  if (samplesfile)
    {
      for (it = samples.begin(); it != samples.end(); it++)
	{
	  samplesfile << it->ValueGet();
	}
    }
  else    {cerr << "Error writing to file\n" << endl; exit(-1);    }
  samplesfile.close();


  // Calculating the mean
  ColumnVector mean(DIMENSION); ColumnVector sum(DIMENSION); 
  for (int k = 0; k < DIMENSION; k++){ mean[k] = sum[k] = 0; }
  
 for (it = samples.begin(); it != samples.end(); it++)
    { 
      sum += it->ValueGet();
    }
  mean = sum * (1.0 / NUM_SAMPLES);
  
  // Calculating the covariance matrix
  ColumnVector diff(DIMENSION);
  Matrix tmp(DIMENSION,DIMENSION);
  SymmetricMatrix diffsum(DIMENSION), tmpSym(DIMENSION), Covariance(DIMENSION);

  for (it = samples.begin(); it != samples.end(); it++)
    {
      diff = (it->ValueGet() - mean);
      tmp = diff * diff.transpose();
      tmp.convertToSymmetricMatrix(tmpSym);
      diffsum += tmpSym;
    }

  Covariance = diffsum / (NUM_SAMPLES-1);

  cout << "number of experiments = " << NUM_SAMPLES << endl;
  cout << "mean  = \n" << mean << endl;
  cout << "Covariance = \n" << Covariance  << endl;
  
  cout << "Now see if we can change the number of ConditionalArguments to " << NEW_NUM_CONDITIONAL_ARGS << endl;
  Matrix c(2,3); c = 1.0;
  cout << "\nC = \n" << c << endl;
  
  My_CondGaussian.NumConditionalArgumentsSet(NEW_NUM_CONDITIONAL_ARGS);
  ColumnVector yet_another_input(3);
  yet_another_input = 1;
  My_CondGaussian.MatrixSet(2,c);
  My_CondGaussian.ConditionalArgumentSet(2,yet_another_input);
  My_CondGaussian.ExpectedValueGet();

  cout << "Value of conditional args = \n" << endl;
  for (int i=0; i < NEW_NUM_CONDITIONAL_ARGS; i++)
    {
      temp_result = My_CondGaussian.ConditionalArgumentGet(i);
      cout << temp_result << endl;
    }

  // Test some member functions
  cout << "Expected Value = \n" << My_CondGaussian.ExpectedValueGet() 
       << endl;
  cout << "Covariance Matrix = \n" << My_CondGaussian.CovarianceGet() 
       << endl;  

  cerr << "==================================================\n"
       << "Test Done\n"
       << "==================================================\n"
       << endl;

  return 0;
}