Fixed bug
							parent
							
								
									d677d1781d
								
							
						
					
					
						commit
						a58d00c0f9
					
				|  | @ -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 | ||||
| 
 | ||||
| %% Create ground truth trajectory | ||||
| trajectoryLength = 49; | ||||
| trajectoryLength = 100; | ||||
| unsmooth_DP = 0.5; % controls smoothness on translation 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)]); | ||||
|     initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)]; | ||||
|     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 | ||||
|     currentPoseKey = symbol('x', 0); | ||||
|     noisyDelta = noiseVector .* randn(6,1); | ||||
|  | @ -119,7 +119,7 @@ for k=1:numMonteCarloRuns | |||
|     currentPoseKey = symbol('x', i); | ||||
|      | ||||
|     % 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); | ||||
|      | ||||
|     % Add the factors to the factor graph | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue