From d7e57405ee1f33eeff3ca255f788299656743f0c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= Date: Fri, 19 Oct 2018 10:29:35 +0200 Subject: [PATCH] Fix absolute factor covariance when deleting oldest keyframe --- InterfaceLevel/makeAbsFactorFromMotionFactor.m | 10 +++++----- Slam/addFrmToTrj.m | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) 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