debugging reaction to small error

bsbretly · Sep 13, 2015 · 8fe4c97 · 8fe4c97
1 parent b323152
commit 8fe4c97
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 3 deletions.
diff --git a/include/fl/filter/gaussian/robust_multi_sensor_gaussian_filter.hpp b/include/fl/filter/gaussian/robust_multi_sensor_gaussian_filter.hpp
@@ -260,11 +260,19 @@ class RobustMultiSensorGaussianFilter
             // set the current sensor's parameter
             local_feature_model.body_moments(y_mean, y_cov, i);
 
+            auto feature = local_feature_model.feature_obsrv(
+                        y.middleRows(i * local_obsrv_dim, local_obsrv_dim));
+
             joint_feature_y.middleRows(i * local_feature_dim, local_feature_dim) =
-                local_feature_model.feature_obsrv(
-                    y.middleRows(i * local_obsrv_dim, local_obsrv_dim));
+                    feature;
 
             low_level_obsrv_fg(i) = 0.75;
+
+//            std::cout << "mean: " << y_mean
+//                      << "    std: " << std::sqrt(y_cov(0,0))
+//                      << "   y - mean: " << y(i) - y_mean(0)
+//                      << "   phi: " << feature.transpose() << std::endl;
+
         }
         MEASURE("local feature computation");
 

diff --git a/include/fl/filter/gaussian/update_policy/multi_sensor_sigma_point_update_polizzle.hpp b/include/fl/filter/gaussian/update_policy/multi_sensor_sigma_point_update_polizzle.hpp
@@ -261,7 +261,18 @@ class MultiSensorSigmaPointUpdatePolizzle<
                 c_yy_given_x.colPivHouseholderQr().solve(innovation).eval();
 
             C += A_i.transpose() * c_yy_given_x_inv_A_i;
-            D += A_i.transpose() * c_yy_given_x_inv_innovation;
+            auto delta = A_i.transpose() * c_yy_given_x_inv_innovation;
+            std::cout << "delta: " << delta.transpose() << std::endl;
+            std::cout << "A " << std::endl << A_i << std::endl
+                         << "c_yy_given_x_inv_innovation" << std::endl
+                            << c_yy_given_x_inv_innovation.transpose()
+                            << std::endl << std::endl;
+            PF(y.middleRows(i * dim_y, dim_y));
+            PF(c_xy);
+            PF(c_yy);
+
+
+            D += delta;
         }
 
 

diff --git a/include/fl/model/observation/robust_feature_obsrv_model.hpp b/include/fl/model/observation/robust_feature_obsrv_model.hpp
@@ -188,6 +188,18 @@ class RobustFeatureObsrvModel
 
         }
 
+//        if(y(0) < 0.9)
+//        {
+//            std::cout << "feature: " << y.transpose()
+//                      << "    weight: " << weight
+//                      << "    input_obsrv: " << input_obsrv.transpose()
+//                      << "    normalizer: " << normalizer
+//                      << std::endl;
+
+//            std::cout << "prob_y " << prob_y << "  prob_tail: " << prob_tail <<
+//                         std::endl;
+//        }
+
         return y;
     }