/**
* Parametrized constructor.
* \param aBelief The belief-state of the gaussian distribution.
*/
gaussian_pdf(const BeliefState& aBelief) : mean_state(aBelief.get_mean_state()), L(aBelief.get_covariance().size()), factor(-1) {
const matrix_type& E = aBelief.get_covariance().get_matrix();
try {
decompose_Cholesky(E,L);
} catch(singularity_error&) { return; };
factor = scalar_type(1);
for(size_type i = 0; i < L.get_row_count(); ++i)
factor *= scalar_type(6.28318530718) * L(i,i);
};
/**
* Parametrized constructor.
* \param aBelief The belief-state of the gaussian distribution.
*/
gaussian_pdf(const BeliefState& aBelief) : mean_state(aBelief.get_mean_state()),
factor(-1) {
decompose_QR(aBelief.get_covariance().get_covarying_block(),QX,RX);
decompose_QR(aBelief.get_covariance().get_informing_inv_block(),QY,RY);
factor = scalar_type(1);
for(size_type i = 0; i < RX.get_row_count(); ++i)
factor *= scalar_type(6.28318530718) * RX(i,i) / RY(i,i);
};
virtual void compute_cost_hessian(mat<double,mat_structure::symmetric>& H, const vect_n<double>& x, double f, const vect_n<double>& x_grad) const {
first_eval->compute_cost_hessian(H, x, f, x_grad);
mat<double,mat_structure::symmetric> H_tmp(H.get_row_count());
second_eval->compute_cost_hessian(H_tmp, x, f, x_grad);
H += H_tmp;
};