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

Added error plots to coriolisExample

release/4.3a0
djensen3 2014-02-06 17:02:51 -05:00
parent 3e7e4d19f6
commit 1b13c14d79
1 changed files with 54 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
matlab/unstable_examples/+imuSimulator/coriolisExample.m
View File

@ -22,14 +22,14 @@ timeElapsed = 10;
times = 0:deltaT:timeElapsed; times = 0:deltaT:timeElapsed;
%omega = [0;0;7.292115e-5]; % Earth Rotation %omega = [0;0;7.292115e-5]; % Earth Rotation
%omega = [0;0;5*pi/10]; omega = [0;0;pi/30];
omega = [0;0;0]; omega = [0;0;0];
omegaFixed = [0;0;0]; omegaFixed = [0;0;0];
velocity = [0;0;0]; % initially not moving velocity = [0;0;0]; % initially not moving
accelFixed = [0;0.1;0.1]; % accelerate in the positive z-direction accelFixed = [0.1;0.1;0.1]; % accelerate in the positive z-direction
initialPosition = [0; 1.05; 0]; % start along the positive x-axis initialPosition = [0; 1.05; 0]; % start along the positive x-axis
IMUinBody = Pose3; IMUinBody = Pose3;
g = [0;0;0]; % Gravity, will need to fix this b/c of ECEF frame g = [0;0;0]; % Gravity
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;
@ -48,12 +48,11 @@ currentPoseRotatingFrame = Pose3;
%% Initialize storage variables %% Initialize storage variables
positionsFixedGT = zeros(3, length(times)); positionsFixedGT = zeros(3, length(times));
positionsRotatingGT = zeros(3, length(times)); positionsRotatingGT = zeros(3, length(times));
positionsEstimates = zeros(3,length(times));
changePoseRotatingFrame = Pose3.Expmap([omega*deltaT; 0; 0; 0]); changePoseRotatingFrame = Pose3.Expmap([omega*deltaT; 0; 0; 0]);
h = figure(1); h = figure(1);
positionsEstimates = zeros(3,length(times)+1);
% Solver object % Solver object
isamParams = ISAM2Params; isamParams = ISAM2Params;
isamParams.setFactorization('CHOLESKY'); isamParams.setFactorization('CHOLESKY');
@ -63,7 +62,8 @@ newFactors = NonlinearFactorGraph;
newValues = Values; newValues = Values;
%% Main loop: iterate through %% Main loop: iterate through the ground truth trajectory, add factors
% and values to the factor graph, and perform inference
for i = 1:length(times) for i = 1:length(times)
t = times(i); t = times(i);
@ -72,23 +72,27 @@ for i = 1:length(times)
currentVelKey = symbol('v', i); currentVelKey = symbol('v', i);
currentBiasKey = symbol('b', i); currentBiasKey = symbol('b', i);
%% Set priors on the first iteration
if(i == 1) if(i == 1)
positionsFixedGT(:,1) = currentPoseFixedGT.translation.vector; positionsFixedGT(:,1) = currentPoseFixedGT.translation.vector;
positionsRotatingGT(:,1) = currentPoseRotatingGT.translation.vector; positionsRotatingGT(:,1) = currentPoseRotatingGT.translation.vector;
poses(1).p = currentPoseRotatingFrame.translation.vector; poses(1).p = currentPoseRotatingFrame.translation.vector;
poses(1).R = currentPoseRotatingFrame.rotation.matrix; poses(1).R = currentPoseRotatingFrame.rotation.matrix;
currentPoseGlobal = currentPoseFixedGT; currentPoseEstimate = currentPoseFixedGT;
currentVelocityGlobal = LieVector(currentVelocityFixedGT); currentVelocityEstimate = LieVector(currentVelocityFixedGT);
% Set Priors % Set Priors
newValues.insert(currentPoseKey, currentPoseGlobal); newValues.insert(currentPoseKey, currentPoseEstimate);
newValues.insert(currentVelKey, currentVelocityGlobal); newValues.insert(currentVelKey, currentVelocityEstimate);
newValues.insert(currentBiasKey, zeroBias); newValues.insert(currentBiasKey, zeroBias);
newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x)); newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseEstimate, sigma_init_x));
newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityEstimate, sigma_init_v));
newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b)); newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b));
% Store data
positionsEstimates(:,i) = currentPoseEstimate.translation.vector;
else else
%% Create ground truth trajectory %% Create ground truth trajectory
@ -111,8 +115,7 @@ for i = 1:length(times)
poses(i).p = currentPoseRotatingFrame.translation.vector; poses(i).p = currentPoseRotatingFrame.translation.vector;
poses(i).R = currentPoseRotatingFrame.rotation.matrix; poses(i).R = currentPoseRotatingFrame.rotation.matrix;
%% Estimate trajectory in rotating frame using the ground truth %% Estimate trajectory in rotating frame using the ground truth measurements
% measurements
% Instantiate preintegrated measurements class % Instantiate preintegrated measurements class
currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
zeroBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ... zeroBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
@ -129,9 +132,11 @@ for i = 1:length(times)
newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ... newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ...
noiseModel.Isotropic.Sigma(6, 1e-10))); noiseModel.Isotropic.Sigma(6, 1e-10)));
% Add values to the graph % Add values to the graph. Use the current pose and velocity
newValues.insert(currentPoseKey, currentPoseGlobal); % estimates as to values when interpreting the next pose and
newValues.insert(currentVelKey, currentVelocityGlobal); % velocity estimates
newValues.insert(currentPoseKey, currentPoseEstimate);
newValues.insert(currentVelKey, currentVelocityEstimate);
newValues.insert(currentBiasKey, zeroBias); newValues.insert(currentBiasKey, zeroBias);
%newFactors.print(''); %newFactors.print('');
@ -143,29 +148,31 @@ for i = 1:length(times)
newFactors = NonlinearFactorGraph; newFactors = NonlinearFactorGraph;
newValues = Values; newValues = Values;
currentPoseGlobal = isam.calculateEstimate(currentPoseKey); % Get the new pose, velocity, and bias estimates
currentVelocityGlobal = isam.calculateEstimate(currentVelKey); currentPoseEstimate = isam.calculateEstimate(currentPoseKey);
currentVelocityEstimate = isam.calculateEstimate(currentVelKey);
currentBias = isam.calculateEstimate(currentBiasKey); currentBias = isam.calculateEstimate(currentBiasKey);
positionsEstimates(:,i) = currentPoseGlobal.translation.vector; positionsEstimates(:,i) = currentPoseEstimate.translation.vector;
%velocitiesEstimates(:,i) = currentVelocityGlobal; %velocitiesEstimates(:,i) = currentVelocityGlobal;
end end
end end
%% incremental plotting for animation (ground truth) %% incremental plotting for animation (ground truth)
figure(h) figure(h)
plot_trajectory(poses(i),1, '-k', 'Rotating Frame',0.1,0.75,1) plot_trajectory(poses(i),1, '-k', 'Rotating Frame',0.1,0.75,1)
hold on; hold on;
plot3(positionsFixedGT(1,1:i), positionsFixedGT(2,1:i), positionsFixedGT(3,1:i)); plot3(positionsFixedGT(1,1:i), positionsFixedGT(2,1:i), positionsFixedGT(3,1:i));
plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'o'); plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'x');
plot3(positionsFixedGT(1,end), positionsFixedGT(2,end), positionsFixedGT(3,end), 'x'); plot3(positionsFixedGT(1,i), positionsFixedGT(2,i), positionsFixedGT(3,i), 'o');
plot3(positionsRotatingGT(1,1:i), positionsRotatingGT(2,1:i), positionsRotatingGT(3,1:i), '-r'); plot3(positionsRotatingGT(1,1:i), positionsRotatingGT(2,1:i), positionsRotatingGT(3,1:i), '-r');
plot3(positionsRotatingGT(1,1), positionsRotatingGT(2,1), positionsRotatingGT(3,1), 'or'); plot3(positionsRotatingGT(1,1), positionsRotatingGT(2,1), positionsRotatingGT(3,1), 'xr');
plot3(positionsRotatingGT(1,end), positionsRotatingGT(2,end), positionsRotatingGT(3,end), 'xr'); plot3(positionsRotatingGT(1,i), positionsRotatingGT(2,i), positionsRotatingGT(3,i), 'or');
plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), '-g'); plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), '-g');
plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'og'); plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xg');
plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'xg'); plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'og');
hold off; hold off;
xlabel('X axis') xlabel('X axis')
@ -173,27 +180,41 @@ for i = 1:length(times)
zlabel('Z axis') zlabel('Z axis')
axis equal axis equal
grid on; grid on;
pause(0.1); %pause(0.1);
i = i + 1; i = i + 1;
end end
%% Final plotting figure
%% Print and plot trajectory error results
positionsError = positionsRotatingGT - positionsEstimates;
fprintf('Final position error = %f\n', positionsError(end));
plot(times, positionsError);
plotTitle = sprintf('Error in Estimated Position (omega = [%.2f; %.2f; %.2f])', omega(1), omega(2), omega(3));
title(plotTitle);
xlabel('Time');
ylabel('Error (ground_truth - estimate)');
legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST');
%% Plot final trajectories
figure figure
sphere % sphere for reference sphere % sphere for reference
hold on; hold on;
% trajectories in Fixed and Rotating frames % Ground truth trajectory in fixed reference frame
plot3(positionsFixedGT(1,:), positionsFixedGT(2,:), positionsFixedGT(3,:)); plot3(positionsFixedGT(1,:), positionsFixedGT(2,:), positionsFixedGT(3,:));
plot3(positionsRotatingGT(1,:), positionsRotatingGT(2,:), positionsRotatingGT(3,:), '-r');
% beginning and end points of Fixed
plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'x'); plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'x');
plot3(positionsFixedGT(1,end), positionsFixedGT(2,end), positionsFixedGT(3,end), 'o'); plot3(positionsFixedGT(1,end), positionsFixedGT(2,end), positionsFixedGT(3,end), 'o');
% beginning and end points of Rotating % Ground truth trajectory in rotating reference frame
plot3(positionsRotatingGT(1,:), positionsRotatingGT(2,:), positionsRotatingGT(3,:), '-r');
plot3(positionsRotatingGT(1,1), positionsRotatingGT(2,1), positionsRotatingGT(3,1), 'xr'); plot3(positionsRotatingGT(1,1), positionsRotatingGT(2,1), positionsRotatingGT(3,1), 'xr');
plot3(positionsRotatingGT(1,end), positionsRotatingGT(2,end), positionsRotatingGT(3,end), 'or'); plot3(positionsRotatingGT(1,end), positionsRotatingGT(2,end), positionsRotatingGT(3,end), 'or');
% Estimates
plot3(positionsEstimates(1,:), positionsEstimates(2,:), positionsEstimates(3,:), '-g');
plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xg');
plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'og');
xlabel('X axis') xlabel('X axis')
ylabel('Y axis') ylabel('Y axis')
zlabel('Z axis') zlabel('Z axis')