MatrixWrapper::SymmetricMatrix
NonLinearAnalyticConditionalGaussian_Ginac::CovarianceGet() const
{
if (cond_size!=0)
{
MatrixWrapper::ColumnVector u_num (u_size);
MatrixWrapper::ColumnVector x_num (x_size);
GiNaC::ex substitute (func_size);
MatrixWrapper::Matrix D (func_size,cond_size);
u_num = ConditionalArgumentGet(1);
x_num = ConditionalArgumentGet(0);
for (unsigned int i=0; i<cond_size; i++)
{
// temp variable to substitute in
substitute = dfunc_dcond[i];
// substitute all u_sym with u_num
for (unsigned int j=0; j<u_size; j++)
substitute = substitute.subs( u_sym[j]==u_num(j+1) );
// substitute all x_sym with x_num
for (unsigned int j=0; j<x_size; j++)
substitute = substitute.subs( x_sym[j]==x_num(j+1) );
// convert substitute back to matrix
GiNaC::matrix substitute_matrix = GiNaC::ex_to<GiNaC::matrix>(substitute);
// build matrix D
for (unsigned int j=0; j<func_size; j++)
D(j+1,i+1) = GiNaC::ex_to<GiNaC::numeric>( substitute_matrix(j,0).evalf() ).to_double();
}
//cout << "D: " << D << endl;
//cout << "CondCov:\n" << (Matrix)cond_covariance << endl;
MatrixWrapper::Matrix temp = D * (MatrixWrapper::Matrix)AdditiveNoiseSigmaGet() * D.transpose();
// convert func_covariance_matrix to symmetric matrix
MatrixWrapper::SymmetricMatrix additiveNoise(temp.rows());
temp.convertToSymmetricMatrix(additiveNoise);
return additiveNoise;
}
else
{
return AdditiveNoiseSigmaGet();
}
}
MatrixWrapper::ColumnVector
NonLinearAnalyticConditionalGaussian_Ginac::ExpectedValueGet() const
{
MatrixWrapper::ColumnVector u_num (u_size);
MatrixWrapper::ColumnVector x_num (x_size);
MatrixWrapper::ColumnVector func_num(func_size);
GiNaC::ex substitute (func_size);
MatrixWrapper::ColumnVector expected(func_size);
u_num = ConditionalArgumentGet(1);
x_num = ConditionalArgumentGet(0);
// use Mu of additive noise
if (cond_size!=0)
for (unsigned int i=0; i<u_size; i++)
for (unsigned int j=0; j<cond_size; j++)
if (u_sym[i] == cond_sym[j])
u_num(i+1) += (this->AdditiveNoiseMuGet())(j+1);
// evaluate func
for (unsigned int i=0; i<func_size; i++)
{
// temp variable to substitute in
substitute = func_sym(i,0);
// substitute all u_sym with u_num
for (unsigned int j=0; j<u_size; j++)
substitute = substitute.subs( u_sym[j]==u_num(j+1) );
// substitute all x_sym with x_num
for (unsigned int j=0; j<x_size; j++)
substitute = substitute.subs( x_sym[j]==x_num(j+1) );
// build matrix func_num
func_num(i+1) = GiNaC::ex_to<GiNaC::numeric>( substitute.evalf() ).to_double();
}
expected = func_num;
if (cond_size==0)
expected += AdditiveNoiseMuGet();
return expected;
}
MatrixWrapper::Matrix
NonLinearAnalyticConditionalGaussian_Ginac::dfGet(unsigned int i) const
{
// Check if i = 0, since this is the old df_dxGet method!
assert(i == 0);
// evaluate function
MatrixWrapper::ColumnVector u_num (u_size);
MatrixWrapper::ColumnVector x_num (x_size);
GiNaC::ex substitute (func_size);
MatrixWrapper::Matrix F (func_size, x_size);
u_num = ConditionalArgumentGet(1);
x_num = ConditionalArgumentGet(0);
// numeric evaluation of derivative: dfunc_dx = F
for (unsigned int i=0; i<x_size; i++)
{
// temp variable to substitute in
substitute = dfunc_dx[i];
// substitute all u_sym with u_num
for (unsigned int j=0; j<u_size; j++)
substitute = substitute.subs( u_sym[j]==u_num(j+1) );
// substitute all x_sym with x_num
for (unsigned int j=0; j<x_size; j++)
substitute = substitute.subs( x_sym[j]==x_num(j+1) );
// convert substitute to matrix. Now all elements in matrix are accessible
GiNaC::matrix substitute_matrix = GiNaC::ex_to<GiNaC::matrix>(substitute);
// build matrix F
for (unsigned int j=0; j<func_size; j++)
F(j+1,i+1) = GiNaC::ex_to<GiNaC::numeric>( substitute_matrix(j,0).evalf() ).to_double();
}
return F;
}
void defiSite::print(FILE* f) const {
fprintf(f, "Site '%p' %s\n", name(),
orientStr());
fprintf(f, " DO X %g %g BY %g\n",
x_orig(),
x_num(),
x_step());
fprintf(f, " DO Y %g %g BY %g\n",
y_orig(),
y_num(),
y_step());
}
