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()); }