From 46c6d41cd6437364163330ef87d61eb01bb95793 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 10 Apr 2014 22:00:07 -0400 Subject: [PATCH] fixed test on real data (gt) --- .../+imuSimulator/covarianceAnalysisBetween.m | 172 ++++++++++-------- .../unstable_examples/+imuSimulator/ct2ENU.m | 55 ++++++ 2 files changed, 148 insertions(+), 79 deletions(-) create mode 100644 matlab/unstable_examples/+imuSimulator/ct2ENU.m diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index a159ed13e..bcf4fc124 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -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 diff --git a/matlab/unstable_examples/+imuSimulator/ct2ENU.m b/matlab/unstable_examples/+imuSimulator/ct2ENU.m new file mode 100644 index 000000000..b7257b664 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/ct2ENU.m @@ -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; + +