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

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

@ -28,10 +28,13 @@ noiseBias = noiseModel.Isotropic.Sigma(6, epsBias);
%% Between metadata
if useRealData == 1
sigma_ang = 1e-4; sigma_cart = 0.01;
sigma_ang = 1e-1; sigma_cart = 1;
else
sigma_ang = 1e-2; sigma_cart = 0.1;
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];
noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose);
@ -182,7 +185,7 @@ hold on;
plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues));
axis equal
dis('Plotted ground truth')
disp('Plotted ground truth')
numMonteCarloRuns = 100;
for k=1:numMonteCarloRuns
@ -191,24 +194,17 @@ for k=1:numMonteCarloRuns
graph = NonlinearFactorGraph;
% noisy prior
if useRealData == 1
currentPoseKey = symbol('x', 0);
initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]);
initialRotation = [gtScenario.Roll(1); gtScenario.Pitch(1); gtScenario.Heading(1)];
initialPose = Pose3.Expmap([initialRotation; initialPosition] + (noiseVector .* randn(6,1))); % initial noisy pose
graph.add(PriorFactorPose3(currentPoseKey, initialPose, noisePose));
else
currentPoseKey = symbol('x', 0);
measurements.posePrior = currentPose;
noisyDelta = noiseVectorPose .* randn(6,1);
initialPose = Pose3.Expmap(noisyDelta);
graph.add(PriorFactorPose3(currentPoseKey, initialPose, noisePose));
end
noisyInitialPose = Pose3.Expmap(noisyDelta);
graph.add(PriorFactorPose3(currentPoseKey, noisyInitialPose, noisePose));
for i=1:size(gtDeltaMatrix,1)
for i=1:size(measurements.gtDeltaMatrix,1)
currentPoseKey = symbol('x', i);
% 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);
% Add the factors to the factor graph
@ -225,7 +221,7 @@ for k=1:numMonteCarloRuns
marginals = Marginals(graph, estimate);
% for each pose in the trajectory
for i=1:size(gtDeltaMatrix,1)+1
for i=1:size(measurements.gtDeltaMatrix,1)+1
% compute estimation errors
currentPoseKey = symbol('x', i-1);
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
set(gca,'Fontsize',16)
title('NEES and ANEES');
%print('-djpeg', horzcat('runs-',testName));
saveas(gcf,horzcat(folderName,'runs-',testName,'.fig'),'fig');
%%
figure(1)
box on
set(gca,'Fontsize',16)
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
n = 3; % position vector dimension
@ -282,6 +282,11 @@ plot(r2*ones(size(ANEES,2),1),'k-.');
box on
set(gca,'Fontsize',16)
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)
% the nees for a single experiment (i) is defined as