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

consistent IMU factor1

release/4.3a0
Luca 2014-04-17 16:08:38 -04:00
parent 3cae615991
commit 322e3e08c8
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
View File

@ -48,12 +48,18 @@ if options.useRealData == 1
if i == 0 if i == 0
currentVel = [0 0 0]'; currentVel = [0 0 0]';
else else
% integrate & store intermediate measurements % integrate & store intermediate measurements
for j=1:options.subsampleStep % we integrate all the intermediate measurements for j=1:options.subsampleStep % we integrate all the intermediate measurements
scenarioIndIMU1 = scenarioInd+j-1; previousScenarioInd = options.subsampleStep * (i-1) + 1;
scenarioIndIMU2 = scenarioInd+j; scenarioIndIMU1 = previousScenarioInd+j-1;
scenarioIndIMU2 = previousScenarioInd+j;
IMUPose1 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU1); IMUPose1 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU1);
IMUPose2 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU2); IMUPose2 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU2);
% scenarioIndIMU1
% IMUPose1.print('IMUPose1');
% IMUPose2.print('IMUPose2');
IMUdeltaPose = IMUPose1.between(IMUPose2); IMUdeltaPose = IMUPose1.between(IMUPose2);
IMUdeltaPoseVector = Pose3.Logmap(IMUdeltaPose); IMUdeltaPoseVector = Pose3.Logmap(IMUdeltaPose);
IMUdeltaRotVector = IMUdeltaPoseVector(1:3); IMUdeltaRotVector = IMUdeltaPoseVector(1:3);