diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index cf996c038..a159ed13e 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -9,91 +9,98 @@ 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 -includeCameraFactors = 0; -trajectoryLength = 50; +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 +% includeCameraFactors = 0; % not implemented yet +trajectoryLength = 2; % length of the ground truth trajectory -deltaT = 1.0; % amount of time between IMU measurements -vel = [0 0 0]; % initial velocity (used for generating IMU measurements -g = [0; 0; 0]; % gravity -omegaCoriolis = [0; 0; 0]; % Coriolis - -% Imu metadata +%% Imu metadata epsBias = 1e-20; -zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); % bias is not of interest and is set to zero +zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); IMU_metadata.AccelerometerSigma = 1e-5; IMU_metadata.GyroscopeSigma = 1e-7; IMU_metadata.IntegrationSigma = 1e-10; IMU_metadata.BiasAccelerometerSigma = epsBias; IMU_metadata.BiasGyroscopeSigma = epsBias; IMU_metadata.BiasAccOmegaInit = epsBias; - -noiseVel = noiseModel.Isotropic.Sigma(3, 1e-10); +noiseVel = noiseModel.Isotropic.Sigma(3, 0.1); noiseBias = noiseModel.Isotropic.Sigma(6, epsBias); -%% Create ground truth trajectory -unsmooth_DP = 0.5; % controls smoothness on translation norm -unsmooth_DR = 0.1; % controls smoothness on rotation norm - -gtValues = Values; -gtGraph = NonlinearFactorGraph; - +%% Between metadata if useRealData == 1 - sigma_ang = 1e-4; - sigma_cart = 40; + sigma_ang = 1e-4; sigma_cart = 40; else - sigma_ang = 1e-2; - sigma_cart = 0.1; + sigma_ang = 1e-2; sigma_cart = 0.1; end noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart]; noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); -if useRealData == 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, 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 -else -%% Create a random trajectory as ground truth - fprintf('\nCreating a random ground truth trajectory\n'); - % Add priors - currentPoseKey = symbol('x', 0); - currentPose = Pose3; % initial pose - gtValues.insert(currentPoseKey, currentPose); - gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); +%% Create ground truth trajectory +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 +else + %% Create a random trajectory as ground truth + 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 + g = [0; 0; 0]; % gravity + omegaCoriolis = [0; 0; 0]; % Coriolis + + unsmooth_DP = 0.5; % controls smoothness on translation norm + unsmooth_DR = 0.1; % controls smoothness on rotation norm + + fprintf('\nCreating a random ground truth trajectory\n'); + %% Add priors + currentPoseKey = symbol('x', 0); + gtValues.insert(currentPoseKey, currentPose); + gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); + if includeIMUFactors == 1 currentVelKey = symbol('v', 0); currentBiasKey = symbol('b', 0); @@ -105,28 +112,31 @@ else for i=1:trajectoryLength currentPoseKey = symbol('x', i); - currentVelKey = symbol('v', i); - currentBiasKey = symbol('b', 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,:)'); - + measurements.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 + + % Add the factors to the factor graph gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, 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 - gyro = gtDeltaMatrix(i, 1:3)./deltaT; - accel = (gtDeltaMatrix(i, 4:6) - vel.*deltaT).*(2/(deltaT*deltaT)); - vel = gtDeltaMatrix(i,4:6)./deltaT; + measurements.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, ... IMU_metadata.AccelerometerSigma.^2 * eye(3), ... @@ -147,9 +157,17 @@ else end end + +gtPoses = Values; +for i=0:trajectoryLength + currentPoseKey = symbol('x', i); + currentPose = gtValues.at(currentPoseKey); + gtPoses.insert(currentPoseKey, currentPose); +end + figure(1) hold on; -plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); +plot3DTrajectory(gtPoses, '-r', [], 1, Marginals(gtGraph, gtPoses)); axis equal numMonteCarloRuns = 100; @@ -186,7 +204,7 @@ for k=1:numMonteCarloRuns % optimize optimizer = GaussNewtonOptimizer(graph, gtValues); estimate = optimizer.optimize(); - + figure(1) plot3DTrajectory(estimate, '-b'); @@ -208,7 +226,7 @@ for k=1:numMonteCarloRuns % compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances NEES(k,i) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof end - + figure(2) hold on plot(NEES(k,:),'-b','LineWidth',1.5) @@ -216,7 +234,7 @@ end %% ANEES = mean(NEES); plot(ANEES,'-r','LineWidth',2) -plot(3*ones(size(ANEES,2),1),'k--'); % Expectation(ANEES) = number of dof +plot(3*ones(size(ANEES,2),1),'k--'); % Expectation(ANEES) = number of dof box on set(gca,'Fontsize',16) title('NEES and ANEES'); @@ -232,7 +250,7 @@ n = 3; % position vector dimension N = numMonteCarloRuns; % number of runs alpha = 0.01; % confidence level -% mean_value = n*N; % mean value of the Chi-square distribution +% mean_value = n*N; % mean value of the Chi-square distribution % (we divide by n * N and for this reason we expect ANEES around 1) r1 = chi2inv(alpha, n * N) / (n * N); r2 = chi2inv(1-alpha, n * N) / (n * N); @@ -252,19 +270,19 @@ set(gca,'Fontsize',16) title('NEES normalized by dof VS bounds'); %% NEES COMPUTATION (Bar-Shalom 2001, Section 5.4) -% the nees for a single experiment (i) is defined as -% NEES_i = xtilda' * inv(P) * xtilda, +% the nees for a single experiment (i) is defined as +% NEES_i = xtilda' * inv(P) * xtilda, % where xtilda in R^n is the estimation % error, and P is the covariance estimated by the approach we want to test -% +% % Average NEES. Given N Monte Carlo simulations, i=1,...,N, the average % NEES is: % ANEES = sum(NEES_i)/N % The quantity N*ANEES is distributed according to a Chi-square % distribution with N*n degrees of freedom. % -% For the single run case, N=1, therefore NEES = ANEES is distributed -% according to a chi-square distribution with n degrees of freedom (e.g. n=3 +% For the single run case, N=1, therefore NEES = ANEES is distributed +% according to a chi-square distribution with n degrees of freedom (e.g. n=3 % if we are testing a position estimate) % Therefore its mean should be n (difficult to see from a single run) % and, with probability alpha, it should hold: