NumericalErrorPropagator.cc

Go to the documentation of this file.

#include <utl/NumericalErrorPropagator.h>
#include <utl/CovarianceMatrix.h>
#include <vector>

using namespace std;
using utl::CovarianceMatrix;
using utl::NumericalErrorPropagator;


void
NumericalErrorPropagator::operator()(vector<double>& oVec,
                                     CovarianceMatrix& oCov,
                                     const vector<double>& iVec,
                                     const CovarianceMatrix& iCov,
                                     const Quality quality)
  const
{
  const unsigned int nInput = iVec.size();

  Transform(oVec,iVec);

  const unsigned int nOutput = oVec.size();

  double jacobi[nOutput][nInput];

  vector<double> xp(iVec), xm(iVec);
  vector<double> fp(nOutput), fm(nOutput);

  if (quality == eFaster)
  {
    for (unsigned inp = 0; inp < nInput; ++inp) {

      if (inp > 0)
      {
        xp [inp-1] = iVec[inp-1];
        xm [inp-1] = iVec[inp-1];
      }

      if (iCov(inp,inp)>0)
      {
        const double dx = 0.1*iCov.Std(inp);

        xp [inp] += dx;
        xm [inp] -= dx;

        Transform(fp , xp);
        Transform(fm , xm);

        for (unsigned out = 0; out < nOutput; ++out)
          jacobi[out][inp] = ( fp[out] - fm[out] ) / (2*dx);
      }
      else
      {
        for (unsigned out = 0; out < nOutput; ++out)
          jacobi[out][inp] = 0.0;
      }
    }
  }
  else // quality == eBetter -> use Five-Point-Stencil derivative
  {
    vector<double> xpp(iVec), xmm(iVec);
    vector<double> fpp(nOutput), fmm(nOutput);

    for (unsigned inp = 0; inp < nInput; ++inp) {

      if (inp > 0)
      {
        xpp[inp-1] = iVec[inp-1];
        xp [inp-1] = iVec[inp-1];
        xm [inp-1] = iVec[inp-1];
        xmm[inp-1] = iVec[inp-1];
      }

      if (iCov(inp,inp)>0)
      {
        const double dx = 0.1*iCov.Std(inp);

        xpp[inp] += 2*dx;
        xp [inp] += dx;
        xm [inp] -= dx;
        xmm[inp] -= 2*dx;

        Transform(fpp,xpp);
        Transform(fp ,xp );
        Transform(fm ,xm );
        Transform(fmm,xmm);

        for (unsigned out = 0; out < nOutput; ++out)
          jacobi[out][inp] = ( -fpp[out] + 8*fp[out] - 8*fm[out] + fmm[out] ) / (12*dx);
      }
      else
      {
        for (unsigned out = 0; out < nOutput; ++out)
          jacobi[out][inp] = 0.0;
      }
    }
  }

  oCov.SetExtent(nOutput);

  for (unsigned int i=0;i<nOutput;++i)
    for (unsigned int j=0;j<nOutput;++j)
  {
    oCov(i,j) = 0.0;
    for (unsigned int k=0;k<nInput;++k)
      for (unsigned int l=0;l<nInput;++l)
        oCov(i,j) += jacobi[i][k] * jacobi[j][l] * iCov(k,l);
  }
}