diff --git a/InterfaceLevel/makeAbsFactorFromMotionFactor.m b/InterfaceLevel/makeAbsFactorFromMotionFactor.m
index c1a5adc..3500080 100644
--- 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
index 6755037..d25dcf0 100644
--- 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