12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Small fix, but now have the same priorFactorVector issue as IMUKittiExampleGPS

release/4.3a0
lvzhaoyang 2014-12-07 00:50:10 -05:00
parent 9dfd6a10e6
commit bd2b92b75d
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
View File

@ -89,7 +89,7 @@ isam = ISAM2(isamParams);
currentIMUPoseGlobal = Pose3();
%% Get initial conditions for the estimated trajectory
currentVelocityGlobal = LieVector([1;0;0]); % the vehicle is stationary at the beginning
currentVelocityGlobal = [1;0;0]; % the vehicle is stationary at the beginning
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0);
@ -127,7 +127,7 @@ for i=1:steps
fg.add(PriorFactorCal3_S2(calibrationKey,K_corrupt,K_cov));
% velocity and bias evolution
fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
fg.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
result = initial;