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

Fixed bug

release/4.3a0
djensen3 2014-04-09 11:34:59 -04:00
parent d677d1781d
commit a58d00c0f9
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m
View File

@ -12,7 +12,7 @@ close all
useAspnData = 1; % controls whether or not to use the ASPN data for scenario 2 as the ground truth traj useAspnData = 1; % controls whether or not to use the ASPN data for scenario 2 as the ground truth traj
%% Create ground truth trajectory %% Create ground truth trajectory
trajectoryLength = 49; trajectoryLength = 100;
unsmooth_DP = 0.5; % controls smoothness on translation norm unsmooth_DP = 0.5; % controls smoothness on translation norm
unsmooth_DR = 0.1; % controls smoothness on rotation norm unsmooth_DR = 0.1; % controls smoothness on rotation norm
@ -107,7 +107,7 @@ for k=1:numMonteCarloRuns
initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]); initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]);
initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)]; initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)];
initialPose = Pose3.Expmap([initialRotation; initialPosition] + (noiseVector .* randn(6,1))); % initial noisy pose initialPose = Pose3.Expmap([initialRotation; initialPosition] + (noiseVector .* randn(6,1))); % initial noisy pose
graph.add(PriorFactorPose3(currentPoseKey, currentPose, noise)); graph.add(PriorFactorPose3(currentPoseKey, initialPose, noise));
else else
currentPoseKey = symbol('x', 0); currentPoseKey = symbol('x', 0);
noisyDelta = noiseVector .* randn(6,1); noisyDelta = noiseVector .* randn(6,1);
@ -119,7 +119,7 @@ for k=1:numMonteCarloRuns
currentPoseKey = symbol('x', i); currentPoseKey = symbol('x', i);
% for each measurement: add noise and add to graph % for each measurement: add noise and add to graph
noisyDelta = gtDeltaMatrix(i,:)';% + (noiseVector .* randn(6,1)); noisyDelta = gtDeltaMatrix(i,:)' + (noiseVector .* randn(6,1));
noisyDeltaPose = Pose3.Expmap(noisyDelta); noisyDeltaPose = Pose3.Expmap(noisyDelta);
% Add the factors to the factor graph % Add the factors to the factor graph