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

completed test infrastructure for simulated and real consistency tests

release/4.3a0
Luca 2014-04-10 22:56:46 -04:00
parent 46c6d41cd6
commit f38d8d7c83
1 changed files with 28 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -28,10 +28,13 @@ noiseBias = noiseModel.Isotropic.Sigma(6, epsBias);
%% Between metadata %% Between metadata
if useRealData == 1 if useRealData == 1
sigma_ang = 1e-4; sigma_cart = 0.01; sigma_ang = 1e-1; sigma_cart = 1;
else else
sigma_ang = 1e-2; sigma_cart = 0.1; sigma_ang = 1e-2; sigma_cart = 0.1;
end end
testName = sprintf('sa-%1.2g-sc-%1.2g',sigma_ang,sigma_cart)
folderName = 'results/'
noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart]; noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart];
noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose);
@ -55,13 +58,13 @@ if useRealData == 1
trajectoryLength = min([length(gtScenario.Lat) trajectoryLength]); trajectoryLength = min([length(gtScenario.Lat) trajectoryLength]);
for i=1:trajectoryLength for i=1:trajectoryLength
currentPoseKey = symbol('x', i-1); currentPoseKey = symbol('x', i-1);
scenarioInd = subsampleStep * (i-1) + 1 scenarioInd = subsampleStep * (i-1) + 1
gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]); gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]);
% truth in ENU % truth in ENU
dX = gtECEF(1) - initialPositionECEF(1); dX = gtECEF(1) - initialPositionECEF(1);
dY = gtECEF(2) - initialPositionECEF(2); dY = gtECEF(2) - initialPositionECEF(2);
dZ = gtECEF(3) - initialPositionECEF(3); dZ = gtECEF(3) - initialPositionECEF(3);
[xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon); [xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon);
gtPosition = [xlt, ylt, zlt]'; gtPosition = [xlt, ylt, zlt]';
@ -76,15 +79,15 @@ if useRealData == 1
warning('using identity rotation') warning('using identity rotation')
gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose));
measurements.posePrior = currentPose; measurements.posePrior = currentPose;
else else
% Generate measurements as the current pose measured in the frame of % Generate measurements as the current pose measured in the frame of
% the previous pose % the previous pose
deltaPose = prevPose.between(currentPose); deltaPose = prevPose.between(currentPose);
measurements.gtDeltaMatrix(i-1,:) = Pose3.Logmap(deltaPose); measurements.gtDeltaMatrix(i-1,:) = Pose3.Logmap(deltaPose);
% Add the factor to the factor graph % Add the factor to the factor graph
gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose)); gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose));
end end
prevPose = currentPose; prevPose = currentPose;
end end
else else
@ -182,7 +185,7 @@ hold on;
plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues));
axis equal axis equal
dis('Plotted ground truth') disp('Plotted ground truth')
numMonteCarloRuns = 100; numMonteCarloRuns = 100;
for k=1:numMonteCarloRuns for k=1:numMonteCarloRuns
@ -191,24 +194,17 @@ for k=1:numMonteCarloRuns
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
% noisy prior % noisy prior
if useRealData == 1 currentPoseKey = symbol('x', 0);
currentPoseKey = symbol('x', 0); measurements.posePrior = currentPose;
initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]); noisyDelta = noiseVectorPose .* randn(6,1);
initialRotation = [gtScenario.Roll(1); gtScenario.Pitch(1); gtScenario.Heading(1)]; noisyInitialPose = Pose3.Expmap(noisyDelta);
initialPose = Pose3.Expmap([initialRotation; initialPosition] + (noiseVector .* randn(6,1))); % initial noisy pose graph.add(PriorFactorPose3(currentPoseKey, noisyInitialPose, noisePose));
graph.add(PriorFactorPose3(currentPoseKey, initialPose, noisePose));
else
currentPoseKey = symbol('x', 0);
noisyDelta = noiseVectorPose .* randn(6,1);
initialPose = Pose3.Expmap(noisyDelta);
graph.add(PriorFactorPose3(currentPoseKey, initialPose, noisePose));
end
for i=1:size(gtDeltaMatrix,1) for i=1:size(measurements.gtDeltaMatrix,1)
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,:)' + (noiseVectorPose .* randn(6,1)); noisyDelta = measurements.gtDeltaMatrix(i,:)' + (noiseVectorPose .* 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
@ -225,7 +221,7 @@ for k=1:numMonteCarloRuns
marginals = Marginals(graph, estimate); marginals = Marginals(graph, estimate);
% for each pose in the trajectory % for each pose in the trajectory
for i=1:size(gtDeltaMatrix,1)+1 for i=1:size(measurements.gtDeltaMatrix,1)+1
% compute estimation errors % compute estimation errors
currentPoseKey = symbol('x', i-1); currentPoseKey = symbol('x', i-1);
gtPosition = gtValues.at(currentPoseKey).translation.vector; gtPosition = gtValues.at(currentPoseKey).translation.vector;
@ -252,12 +248,16 @@ plot(3*ones(size(ANEES,2),1),'k--'); % Expectation(ANEES) = number of dof
box on box on
set(gca,'Fontsize',16) set(gca,'Fontsize',16)
title('NEES and ANEES'); title('NEES and ANEES');
%print('-djpeg', horzcat('runs-',testName));
saveas(gcf,horzcat(folderName,'runs-',testName,'.fig'),'fig');
%% %%
figure(1) figure(1)
box on box on
set(gca,'Fontsize',16) set(gca,'Fontsize',16)
title('Ground truth and estimates for each MC runs'); title('Ground truth and estimates for each MC runs');
%print('-djpeg', horzcat('gt-',testName));
saveas(gcf,horzcat(folderName,'gt-',testName,'.fig'),'fig');
%% Let us compute statistics on the overall NEES %% Let us compute statistics on the overall NEES
n = 3; % position vector dimension n = 3; % position vector dimension
@ -282,6 +282,11 @@ plot(r2*ones(size(ANEES,2),1),'k-.');
box on box on
set(gca,'Fontsize',16) set(gca,'Fontsize',16)
title('NEES normalized by dof VS bounds'); title('NEES normalized by dof VS bounds');
%print('-djpeg', horzcat('ANEES-',testName));
saveas(gcf,horzcat(folderName,'ANEES-',testName,'.fig'),'fig');
logFile = horzcat(folderName,'log-',testName);
save(logFile)
%% NEES COMPUTATION (Bar-Shalom 2001, Section 5.4) %% NEES COMPUTATION (Bar-Shalom 2001, Section 5.4)
% the nees for a single experiment (i) is defined as % the nees for a single experiment (i) is defined as