void SumFactor::_internal_update(
Variable* var,
std::vector<Gaussian*> y,
std::vector<Gaussian*> fy,
std::vector<double>* a) {
double sum_pi = 0.0, sum_tau = 0.0, new_pi, new_tau, da;
unsigned int i = 0, size = a->size();
Gaussian gy, gfy;
for (i = 0; i < size; ++i) {
da = (*a)[i];
gy = *y[i];
gfy = *fy[i];
sum_pi = sum_pi + ((da * da) / (gy.pi - gfy.pi));
sum_tau = sum_tau + (da * (gy.tau - gfy.tau) / (gy.pi - gfy.pi));
}
new_pi = 1.0 / sum_pi;
new_tau = new_pi * sum_tau;
Gaussian* gaussian = new Gaussian();
gaussian->init_pi_tau(new_pi, new_tau);
var->update_message(this, gaussian);
}
void LikelihoodFactor::update_mean() {
Gaussian x = *this->value->value;
Gaussian fx = *this->value->get_message(this);
double a = 1.0 / (1.0 + this->variance * (x.pi - fx.pi));
Gaussian* gaussian = new Gaussian();
gaussian->init_pi_tau(
a * (x.pi - fx.pi),
a * (x.tau - fx.tau)
);
this->mean->update_message(this, gaussian);
}
void TruncateFactorDraw::update() {
Gaussian* x = this->variable->value;
Gaussian* fx = this->variable->get_message(this);
double c, d, sqrt_c, V, W, t, e, mW;
c = x->pi - fx->pi;
d = x->tau = fx->tau;
sqrt_c = sqrt(c);
t = d / sqrt_c;
e = epsilon * sqrt_c;
V = Vdraw(t, e);
W = Wdraw(t, e);
mW = 1.0 - W;
Gaussian* gaussian = new Gaussian();
gaussian->init_pi_tau(c / mW, (d + sqrt_c * V) / mW);
this->variable->update_value(this, gaussian);
}