It works!

src/factor_graph.rs

@@ -54,6 +54,8 @@ impl Variable {
 
         self.factors.insert(factor, value * old / self.value);
         self.value = value;
+
+        // debug!("Variable::value value={:?}", self.value);
     }
 
     pub fn get_value(&self) -> Gaussian {
@@ -65,6 +67,8 @@ impl Variable {
 
         self.factors.insert(factor, message);
         self.value = self.value / old * message;
+
+        // debug!("Variable::message value={:?}", self.value);
     }
 
     pub fn get_message(&self, factor: usize) -> Gaussian {
@@ -95,6 +99,8 @@ impl PriorFactor {
     }
 
     pub fn start(&self, variable_arena: &mut VariableArena) {
+        debug!("Prior::down var={:?}, value={:?}", self.variable.index, self.gaussian);
+
         variable_arena
             .get_mut(self.variable)
             .unwrap()
@@ -137,6 +143,8 @@ impl LikelihoodFactor {
         let a = 1.0 / (1.0 + self.variance * (x.pi() - fx.pi()));
         let gaussian = Gaussian::from_pi_tau(a * (x.pi() - fx.pi()), a * (x.tau() - fx.tau()));
 
+        debug!("Likelihood::up var={:?}, value={:?}", self.mean.index, gaussian);
+
         variable_arena
             .get_mut(self.mean)
             .unwrap()
@@ -152,6 +160,8 @@ impl LikelihoodFactor {
         let a = 1.0 / (1.0 + self.variance * (y.pi() - fy.pi()));
         let gaussian = Gaussian::from_pi_tau(a * (y.pi() - fy.pi()), a * (y.tau() - fy.tau()));
 
+        debug!("Likelihood::down var={:?}, value={:?}", self.value.index, gaussian);
+
         variable_arena
             .get_mut(self.value)
             .unwrap()
@@ -210,6 +220,12 @@ impl SumFactor {
 
         let gaussian = Gaussian::from_pi_tau(new_pi, new_tau);
 
+        if variable == self.sum {
+            debug!("Sum::down var={:?}, value={:?}", variable.index, gaussian);
+        } else {
+            debug!("Sum::up var={:?}, value={:?}", variable.index, gaussian);
+        }
+
         variable_arena
             .get_mut(variable)
             .unwrap()
@@ -264,6 +280,12 @@ impl SumFactor {
 
         self.internal_update(variable_arena, idx_term, y, fy, &a);
     }
+
+    pub fn update_all_terms(&self, variable_arena: &mut VariableArena) {
+        for i in 0..self.terms.len() {
+            self.update_term(variable_arena, i);
+        }
+    }
 }
 
 fn v_win(t: f64, e: f64) -> f64 {
@@ -337,6 +359,8 @@ impl TruncateFactor {
 
         let gaussian = Gaussian::from_pi_tau(c / m_w, (d + sqrt_c * v) / m_w);
 
+        debug!("Trunc::up var={:?}, value={:?}", self.variable.index, gaussian);
+
         variable_arena
             .get_mut(self.variable)
             .unwrap()