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

Added flag to use scenario 2 ground truth data

release/4.3a0
djensen3 2014-04-07 23:12:31 -04:00
parent ed6788fff4
commit d677d1781d
2 changed files with 112 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m Normal file
View File

@ -0,0 +1,26 @@
function pos_ECEF = LatLonHRad_to_ECEF(LatLonH)
% convert latitude, longitude, height to XYZ in ECEF coordinates
% LatLonH(1) : latitude in radian
% LatLonH(2) : longitude in radian
% LatLonH(3) : height in meter
%
% Source: A. Chatfield, "Fundamentals of High Accuracy Inertial
% Navigation", 1997. pp. 10 Eq 1.18
%
% constants
a = 6378137.0; %m
e_sqr =0.006694379990141317;
% b = 6356752.3142; % m
%RAD_PER_DEG = pi/180;
phi = LatLonH(1);%*RAD_PER_DEG;
lambda = LatLonH(2);%*RAD_PER_DEG;
h = LatLonH(3);
RN = a/sqrt(1 - e_sqr*sin(phi)^2);
pos_ECEF = zeros(3,1);
pos_ECEF(1) = (RN + h )*cos(phi)*cos(lambda);
pos_ECEF(2) = (RN + h )*cos(phi)*sin(lambda);
pos_ECEF(3) = (RN*(1-e_sqr) + h)*sin(phi) ;

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

@ -8,20 +8,67 @@ clc
clear all
close all
%% Configuration
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;
unsmooth_DP = 0.5; % controls smoothness on translation norm
unsmooth_DR = 0.1; % controls smoothness on translation norm
unsmooth_DR = 0.1; % controls smoothness on rotation norm
% possibly create random trajectory as ground Truth
gtValues = Values;
gtGraph = NonlinearFactorGraph;
if useAspnData == 1
sigma_ang = 1e-4;
sigma_cart = 40;
else
sigma_ang = 1e-2;
sigma_cart = 0.1;
end
noiseVector = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart];
noise = noiseModel.Diagonal.Sigmas(noiseVector);
if useAspnData == 1
% Create a ground truth trajectory using scenario 2 data
fprintf('\nUsing Scenario 2 ground truth data\n');
% load scenario 2 ground truth data
gtScenario2 = load('truth_scen2.mat', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading');
% Add first pose
currentPoseKey = symbol('x', 0);
initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]);
initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)];
currentPose = Pose3.Expmap([initialRotation; initialPosition]); % initial pose
gtValues.insert(currentPoseKey, currentPose);
gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noise));
prevPose = currentPose;
% Limit the trajectory length
trajectoryLength = min([length(gtScenario2.Lat) trajectoryLength]);
for i=2:trajectoryLength
currentPoseKey = symbol('x', i-1);
gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(i); gtScenario2.Lon(i); gtScenario2.Alt(i)]);
gtRotation = [gtScenario2.Roll(i); gtScenario2.Pitch(i); gtScenario2.Heading(i)];
currentPose = Pose3.Expmap([gtRotation; gtECEF]);
% Generate measurements as the current pose measured in the frame of
% the previous pose
deltaPose = prevPose.between(currentPose);
gtDeltaMatrix(i-1,:) = Pose3.Logmap(deltaPose);
prevPose = currentPose;
% Add values
gtValues.insert(currentPoseKey, currentPose);
% Add the factor to the factor graph
gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noise));
end
else
% Create a random trajectory as ground truth
fprintf('\nCreating a random ground truth trajectory\n');
% Add first pose
currentPoseKey = symbol('x', 0);
currentPose = Pose3; % initial pose
gtValues.insert(currentPoseKey, currentPose);
@ -42,28 +89,37 @@ for i=1:trajectoryLength
% Add the factors to the factor graph
gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noise));
end
end
figure(1)
hold on;
plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues));
axis equal
numMonteCarloRuns = 100;
numMonteCarloRuns = 10;
for k=1:numMonteCarloRuns
fprintf('Monte Carlo Run %d.\n', k');
% create a new graph
graph = NonlinearFactorGraph;
% noisy prior
if useAspnData == 1
currentPoseKey = symbol('x', 0);
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));
else
currentPoseKey = symbol('x', 0);
noisyDelta = noiseVector .* randn(6,1);
initialPose = Pose3.Expmap(noisyDelta);
graph.add(PriorFactorPose3(currentPoseKey, initialPose, noise));
end
for i=1:trajectoryLength
for i=1:size(gtDeltaMatrix,1)
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
@ -80,7 +136,7 @@ for k=1:numMonteCarloRuns
marginals = Marginals(graph, estimate);
% for each pose in the trajectory
for i=1:trajectoryLength+1
for i=1:size(gtDeltaMatrix,1)+1
% compute estimation errors
currentPoseKey = symbol('x', i-1);
gtPosition = gtValues.at(currentPoseKey).translation.vector;