From d677d1781d054249d0a97790eca6a2dac1d1b99d Mon Sep 17 00:00:00 2001 From: djensen3 Date: Mon, 7 Apr 2014 23:12:31 -0400 Subject: [PATCH] Added flag to use scenario 2 ground truth data --- .../+imuSimulator/LatLonHRad_to_ECEF.m | 26 ++++ .../+imuSimulator/covarianceAnalysisBetween.m | 116 +++++++++++++----- 2 files changed, 112 insertions(+), 30 deletions(-) create mode 100644 matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m diff --git a/matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m b/matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m new file mode 100644 index 000000000..bcc163eba --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m @@ -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) ; \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index bbd5dfff3..cb2db2965 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -8,62 +8,118 @@ 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; -sigma_ang = 1e-2; -sigma_cart = 0.1; +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); -currentPoseKey = symbol('x', 0); -currentPose = Pose3; % initial pose -gtValues.insert(currentPoseKey, currentPose); -gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noise)); - -for i=1:trajectoryLength - currentPoseKey = symbol('x', i); - gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5 - gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad) - gtDeltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition]; - deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)'); - - % "Deduce" ground truth measurements - % deltaPose are the gt measurements - save them in some structure - currentPose = currentPose.compose(deltaPose); +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; - % Add the factors to the factor graph - gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noise)); -end + % 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); + gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noise)); + for i=1:trajectoryLength + currentPoseKey = symbol('x', i); + gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5 + gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad) + gtDeltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition]; + deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)'); + + % "Deduce" ground truth measurements + % deltaPose are the gt measurements - save them in some structure + currentPose = currentPose.compose(deltaPose); + gtValues.insert(currentPoseKey, currentPose); + + % 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 - currentPoseKey = symbol('x', 0); - noisyDelta = noiseVector .* randn(6,1); - initialPose = Pose3.Expmap(noisyDelta); - graph.add(PriorFactorPose3(currentPoseKey, initialPose, noise)); + 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;