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