Fix absolute factor covariance when deleting oldest keyframe

joansola · Oct 19, 2018 · d7e5740 · d7e5740
1 parent 73c9db1
commit d7e5740
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 6 deletions.
diff --git a/InterfaceLevel/makeAbsFactorFromMotionFactor.m b/InterfaceLevel/makeAbsFactorFromMotionFactor.m
@@ -9,13 +9,13 @@
 
 % Measurement is the straight data
 Fac.meas.y = Frm.state.x;
+% TODO use real covariance of the robot state
+sigma = 1e-6; % 1mm error 
+Fac.meas.R = sigma*eye(numel(Fac.meas.y));
+Fac.meas.W = Fac.meas.R ^ (-1);
 
 % Expectation has zero covariance -- and info is not defined
-Fac.exp.e = Fac.meas.y; % expectation
-
-sigma = 1e4;
-Fac.err.Wsqrt = sigma*eye(numel(Fac.err.z));
-Fac.err.W     = sigma*Fac.err.Wsqrt;
+Fac.exp.e = Frm.state.x; % expectation is the state
 
 Fac.err.size  = numel(Fac.err.z);
 

diff --git a/Slam/addFrmToTrj.m b/Slam/addFrmToTrj.m
@@ -72,7 +72,7 @@
     else
         % Delete factor after cleaning up graph
 
-        for frm = [Fac(fac).frames];
+        for frm = [Fac(fac).frames]
             % Remove this factor from frame's factors list
             Frm(frm).factors([Frm(frm).factors] == fac) = [];
         end