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

fixed test on real data (gt)

release/4.3a0
Luca 2014-04-10 22:00:07 -04:00
parent 24157ca124
commit 46c6d41cd6
2 changed files with 148 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -9,13 +9,13 @@ clear all
close all
%% Configuration
useRealData = 0; % controls whether or not to use the Real data (is available) as the ground truth traj
includeIMUFactors = 1; % if true, IMU type 1 Factors will be generated for the random trajectory
useRealData = 1; % controls whether or not to use the Real data (is available) as the ground truth traj
includeIMUFactors = 0; % if true, IMU type 1 Factors will be generated for the random trajectory
% includeCameraFactors = 0; % not implemented yet
trajectoryLength = 2; % length of the ground truth trajectory
trajectoryLength = 210; % length of the ground truth trajectory
%% Imu metadata
epsBias = 1e-20;
epsBias = 1e-7;
zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
IMU_metadata.AccelerometerSigma = 1e-5;
IMU_metadata.GyroscopeSigma = 1e-7;
@ -28,7 +28,7 @@ noiseBias = noiseModel.Isotropic.Sigma(6, epsBias);
%% Between metadata
if useRealData == 1
sigma_ang = 1e-4; sigma_cart = 40;
sigma_ang = 1e-4; sigma_cart = 0.01;
else
sigma_ang = 1e-2; sigma_cart = 0.1;
end
@ -40,55 +40,58 @@ gtValues = Values;
gtGraph = NonlinearFactorGraph;
if useRealData == 1
% % % %% Create a ground truth trajectory from Real data (if available)
% % % fprintf('\nUsing real data as ground truth\n');
% % % gtScenario2 = load('truth_scen2.mat', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading');
% Time: [4201x1 double]
% Lat: [4201x1 double]
% Lon: [4201x1 double]
% Alt: [4201x1 double]
% VEast: [4201x1 double]
% VNorth: [4201x1 double]
% VUp: [4201x1 double]
% Roll: [4201x1 double]
% Pitch: [4201x1 double]
% 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, noisePose));
% % % 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, noisePose));
% % % end
subsampleStep = 20;
%% Create a ground truth trajectory from Real data (if available)
fprintf('\nUsing real data as ground truth\n');
gtScenario = load('truth_scen2.mat', 'Time', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading',...
'VEast', 'VNorth', 'VUp');
Org_lat = gtScenario.Lat(1);
Org_lon = gtScenario.Lon(1);
initialPositionECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]);
% Limit the trajectory length
trajectoryLength = min([length(gtScenario.Lat) trajectoryLength]);
for i=1:trajectoryLength
currentPoseKey = symbol('x', i-1);
scenarioInd = subsampleStep * (i-1) + 1
gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]);
% truth in ENU
dX = gtECEF(1) - initialPositionECEF(1);
dY = gtECEF(2) - initialPositionECEF(2);
dZ = gtECEF(3) - initialPositionECEF(3);
[xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon);
gtPosition = [xlt, ylt, zlt]';
gtRotation = Rot3; % Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
currentPose = Pose3(gtRotation, Point3(gtPosition));
% Add values
gtValues.insert(currentPoseKey, currentPose);
if i==1 % first time step, add priors
warning('roll-pitch-yaw is different from Rodriguez')
warning('using identity rotation')
gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose));
measurements.posePrior = currentPose;
else
% Generate measurements as the current pose measured in the frame of
% the previous pose
deltaPose = prevPose.between(currentPose);
measurements.gtDeltaMatrix(i-1,:) = Pose3.Logmap(deltaPose);
% Add the factor to the factor graph
gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose));
end
prevPose = currentPose;
end
else
%% Create a random trajectory as ground truth
currentVel = [0 0 0]; % initial velocity (used to generate IMU measurements)
currentVel = [0; 0; 0]; % initial velocity (used to generate IMU measurements)
currentPose = Pose3; % initial pose % initial pose
deltaT = 1.0; % amount of time between IMU measurements
deltaT = 0.1; % amount of time between IMU measurements
g = [0; 0; 0]; % gravity
omegaCoriolis = [0; 0; 0]; % Coriolis
@ -104,9 +107,9 @@ else
if includeIMUFactors == 1
currentVelKey = symbol('v', 0);
currentBiasKey = symbol('b', 0);
gtValues.insert(currentVelKey, LieVector(vel'));
gtValues.insert(currentVelKey, LieVector(currentVel));
gtValues.insert(currentBiasKey, zeroBias);
gtGraph.add(PriorFactorLieVector(currentVelKey, LieVector(vel'), noiseVel));
gtGraph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseVel));
gtGraph.add(PriorFactorConstantBias(currentBiasKey, zeroBias, noiseBias));
end
@ -116,60 +119,71 @@ else
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];
measurements.deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)');
gtMeasurements.deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)');
% "Deduce" ground truth measurements
% deltaPose are the gt measurements - save them in some structure
currentPose = currentPose.compose(deltaPose);
currentPose = currentPose.compose(gtMeasurements.deltaPose);
gtValues.insert(currentPoseKey, currentPose);
% Add the factors to the factor graph
gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose));
gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, gtMeasurements.deltaPose, noisePose));
% Add IMU factors
if includeIMUFactors == 1
currentVelKey = symbol('v', i); % not used if includeIMUFactors is false
currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false
% create accel and gyro measurements based on
measurements.imu.gyro = gtDeltaMatrix(i, 1:3)./deltaT;
gtMeasurements.imu.gyro = gtDeltaMatrix(i, 1:3)'./deltaT;
% acc = (deltaPosition - initialVel * dT) * (2/dt^2)
measurements.imu.accel = (gtDeltaMatrix(i, 4:6) - currentVel.*deltaT).*(2/(deltaT*deltaT));
% update current velocity
currentVel = gtDeltaMatrix(i,4:6)./deltaT;
imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
zeroBias, ...
gtMeasurements.imu.accel = (gtDeltaMatrix(i, 4:6)' - currentVel.*deltaT).*(2/(deltaT*deltaT));
% Initialize preintegration
imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(zeroBias, ...
IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
IMU_metadata.GyroscopeSigma.^2 * eye(3), ...
IMU_metadata.IntegrationSigma.^2 * eye(3));
imuMeasurement.integrateMeasurement(accel', gyro', deltaT);
gtGraph.add(ImuFactor( ...
currentPoseKey-1, currentVelKey-1, ...
currentPoseKey, currentVelKey, ...
% Preintegrate
imuMeasurement.integrateMeasurement(gtMeasurements.imu.accel, gtMeasurements.imu.gyro, deltaT);
% Add Imu factor
gtGraph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ...
currentBiasKey-1, imuMeasurement, g, omegaCoriolis));
% Add between on biases
gtGraph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, zeroBias, ...
noiseModel.Isotropic.Sigma(6, epsBias)));
% Additional prior on zerobias
gtGraph.add(PriorFactorConstantBias(currentBiasKey, zeroBias, ...
noiseModel.Isotropic.Sigma(6, epsBias)));
gtValues.insert(currentVelKey, LieVector(vel'));
% update current velocity
currentVel = gtDeltaMatrix(i,4:6)'./deltaT;
gtValues.insert(currentVelKey, LieVector(currentVel));
gtGraph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseVel));
gtValues.insert(currentBiasKey, zeroBias);
end
end
end % end of trajectory length
end
end % end of ground truth creation
gtPoses = Values;
for i=0:trajectoryLength
currentPoseKey = symbol('x', i);
currentPose = gtValues.at(currentPoseKey);
gtPoses.insert(currentPoseKey, currentPose);
end
warning('Additional prior on zerobias')
warning('Additional PriorFactorLieVector on velocities')
% gtPoses = Values;
% for i=0:trajectoryLength
% currentPoseKey = symbol('x', i);
% currentPose = gtValues.at(currentPoseKey);
% gtPoses.insert(currentPoseKey, currentPose);
% end
figure(1)
hold on;
plot3DTrajectory(gtPoses, '-r', [], 1, Marginals(gtGraph, gtPoses));
plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues));
axis equal
dis('Plotted ground truth')
numMonteCarloRuns = 100;
for k=1:numMonteCarloRuns
fprintf('Monte Carlo Run %d.\n', k');
@ -179,8 +193,8 @@ for k=1:numMonteCarloRuns
% noisy prior
if useRealData == 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)];
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

55
matlab/unstable_examples/+imuSimulator/ct2ENU.m Normal file
View File

@ -0,0 +1,55 @@
function [dx,dy,dz]=ct2ENU(dX,dY,dZ,lat,lon)
% CT2LG Converts CT coordinate differences to local geodetic.
% Local origin at lat,lon,h. If lat,lon are vectors, dx,dy,dz
% are referenced to orgin at lat,lon of same index. If
% astronomic lat,lon input, output is in local astronomic
% system. Vectorized in both dx,dy,dz and lat,lon. See also
% LG2CT.
% Version: 2011-02-19
% Useage: [dx,dy,dz]=ct2lg(dX,dY,dZ,lat,lon)
% Input: dX - vector of X coordinate differences in CT
% dY - vector of Y coordinate differences in CT
% dZ - vector of Z coordinate differences in CT
% lat - lat(s) of local system origin (rad); may be vector
% lon - lon(s) of local system origin (rad); may be vector
% Output: dx - vector of x coordinates in local system (east)
% dy - vector of y coordinates in local system (north)
% dz - vector of z coordinates in local system (up)
% Copyright (c) 2011, Michael R. Craymer
% All rights reserved.
% Email: mike@craymer.com
if nargin ~= 5
warning('Incorrect number of input arguements');
return
end
n=length(dX);
if length(lat)==1
lat=ones(n,1)*lat;
lon=ones(n,1)*lon;
end
R=zeros(3,3,n);
R(1,1,:)=-sin(lat').*cos(lon');
R(1,2,:)=-sin(lat').*sin(lon');
R(1,3,:)=cos(lat');
R(2,1,:)=sin(lon');
R(2,2,:)=-cos(lon');
R(2,3,:)=zeros(1,n);
R(3,1,:)=cos(lat').*cos(lon');
R(3,2,:)=cos(lat').*sin(lon');
R(3,3,:)=sin(lat');
RR=reshape(R(1,:,:),3,n);
dx_temp=sum(RR'.*[dX dY dZ],2);
RR=reshape(R(2,:,:),3,n);
dy_temp=sum(RR'.*[dX dY dZ],2);
RR=reshape(R(3,:,:),3,n);
dz=sum(RR'.*[dX dY dZ],2);
dx = -dy_temp;
dy = dx_temp;