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

Added IMU type 2 to coriolis example.

release/4.3a0
djensen3 2014-02-11 11:13:13 -05:00
parent 1b13c14d79
commit 38e0e411fb
1 changed files with 160 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -17,40 +17,55 @@ import gtsam.*;
addpath(genpath('./Libraries')) addpath(genpath('./Libraries'))
%% General configuration %% General configuration
deltaT = 0.1; navFrameRotating = 0;
timeElapsed = 10; deltaT = 0.01;
timeElapsed = 5;
times = 0:deltaT:timeElapsed; times = 0:deltaT:timeElapsed;
IMU_type = 1;
record_movie = 0;
%omega = [0;0;7.292115e-5]; % Earth Rotation omegaEarthSeconds = 100*[0;0;7.292115e-5]; % Earth Rotation
omega = [0;0;pi/30]; % omegaRotatingFrame = omegaEarthSeconds/deltaT;%[0;0;pi/3000]; % rotation of the moving frame wrt fixed frame
omega = [0;0;0]; omegaRotatingFrame = [0;0;pi/300];
omegaFixed = [0;0;0]; currentRotatingFrame = Pose3; % initially coincide with fixed frame
velocity = [0;0;0]; % initially not moving omegaFixed = [0;0;0]; % constant rotation rate measurement
accelFixed = [0.1;0.1;0.1]; % accelerate in the positive z-direction accelFixed =10 * [0;0;0.1]; % constant acceleration measurement
initialPosition = [0; 1.05; 0]; % start along the positive x-axis
IMUinBody = Pose3;
g = [0;0;0]; % Gravity g = [0;0;0]; % Gravity
zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
if navFrameRotating == 0
omegaCoriolisIMU = [0;0;0];
else
omegaCoriolisIMU = omegaRotatingFrame;
end
% Initial conditions
velocity = [0;0;0]; % initially not moving
initialPosition = [1; 0; 0]; % start along the positive x-axis
%
currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition));
currentPoseRotatingGT = currentPoseFixedGT; % frames coincide for t=0
currentVelocityFixedGT = velocity;
%
epsBias = 1e-15;
sigma_init_x = noiseModel.Isotropic.Sigma(6, 1e-10);
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1e-10);
sigma_init_b = noiseModel.Isotropic.Sigma(6, epsBias);
% Imu metadata
zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); % bias is not of interest and is set to zero
IMU_metadata.AccelerometerSigma = 1e-5; IMU_metadata.AccelerometerSigma = 1e-5;
IMU_metadata.GyroscopeSigma = 1e-7; IMU_metadata.GyroscopeSigma = 1e-7;
IMU_metadata.IntegrationSigma = 1e-10; IMU_metadata.IntegrationSigma = 1e-10;
sigma_init_x = noiseModel.Isotropic.Sigma(6, 1e-10); IMU_metadata.BiasAccelerometerSigma = 1e-5;
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1e-10); IMU_metadata.BiasGyroscopeSigma = 1e-7;
sigma_init_b = noiseModel.Isotropic.Sigma(6, 1e-10); IMU_metadata.BiasAccOmegaInit = 1e-10;
%% Initial state of the body in the fixed in rotating frames should be the same
currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition));
currentVelocityFixedGT = velocity;
currentPoseRotatingGT = currentPoseFixedGT;
currentPoseRotatingFrame = Pose3;
%% Initialize storage variables %% Initialize storage variables
positionsFixedGT = zeros(3, length(times)); positionsInFixedGT = zeros(3, length(times));
positionsRotatingGT = zeros(3, length(times)); positionsInRotatingGT = zeros(3, length(times));
positionsEstimates = zeros(3,length(times)); positionsEstimates = zeros(3,length(times));
changePoseRotatingFrame = Pose3.Expmap([omega*deltaT; 0; 0; 0]); changePoseRotatingFrame = Pose3.Expmap([omegaRotatingFrame*deltaT; 0; 0; 0]); % rotation of the rotating frame at each time step
h = figure(1); h = figure(1);
% Solver object % Solver object
@ -61,6 +76,15 @@ isam = gtsam.ISAM2(isamParams);
newFactors = NonlinearFactorGraph; newFactors = NonlinearFactorGraph;
newValues = Values; newValues = Values;
% Video recording object
if record_movie == 1
writerObj = VideoWriter('trajectories.avi');
writerObj.Quality = 100;
writerObj.FrameRate = 15; %10;
open(writerObj);
set(gca,'nextplot','replacechildren');
set(gcf,'Renderer','zbuffer');
end
%% Main loop: iterate through the ground truth trajectory, add factors %% Main loop: iterate through the ground truth trajectory, add factors
% and values to the factor graph, and perform inference % and values to the factor graph, and perform inference
@ -74,25 +98,24 @@ for i = 1:length(times)
%% Set priors on the first iteration %% Set priors on the first iteration
if(i == 1) if(i == 1)
positionsFixedGT(:,1) = currentPoseFixedGT.translation.vector; currentPoseEstimate = currentPoseFixedGT; % known initial conditions
positionsRotatingGT(:,1) = currentPoseRotatingGT.translation.vector; currentVelocityEstimate = LieVector(currentVelocityFixedGT); % known initial conditions
poses(1).p = currentPoseRotatingFrame.translation.vector;
poses(1).R = currentPoseRotatingFrame.rotation.matrix;
currentPoseEstimate = currentPoseFixedGT;
currentVelocityEstimate = LieVector(currentVelocityFixedGT);
% Set Priors % Set Priors
newValues.insert(currentPoseKey, currentPoseEstimate); newValues.insert(currentPoseKey, currentPoseEstimate);
newValues.insert(currentVelKey, currentVelocityEstimate); newValues.insert(currentVelKey, currentVelocityEstimate);
newValues.insert(currentBiasKey, zeroBias); newValues.insert(currentBiasKey, zeroBias);
% Initial values, same for IMU types 1 and 2
newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseEstimate, sigma_init_x)); newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseEstimate, sigma_init_x));
newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityEstimate, 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 % Store data
positionsInFixedGT(:,1) = currentPoseFixedGT.translation.vector;
positionsInRotatingGT(:,1) = currentPoseRotatingGT.translation.vector;
positionsEstimates(:,i) = currentPoseEstimate.translation.vector; positionsEstimates(:,i) = currentPoseEstimate.translation.vector;
currentRotatingFrameForPlot(1).p = currentRotatingFrame.translation.vector;
currentRotatingFrameForPlot(1).R = currentRotatingFrame.rotation.matrix;
else else
%% Create ground truth trajectory %% Create ground truth trajectory
@ -103,35 +126,70 @@ for i = 1:length(times)
+ currentVelocityFixedGT * deltaT + 0.5 * accelFixed * deltaT * deltaT); + currentVelocityFixedGT * deltaT + 0.5 * accelFixed * deltaT * deltaT);
currentVelocityFixedGT = currentVelocityFixedGT + accelFixed * deltaT; currentVelocityFixedGT = currentVelocityFixedGT + accelFixed * deltaT;
currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); % constant orientation
% Rotate pose in fixed frame to get pose in rotating frame % Rotate pose in fixed frame to get pose in rotating frame
currentPoseRotatingFrame = currentPoseRotatingFrame.compose(changePoseRotatingFrame); currentRotatingFrame = currentRotatingFrame.compose(changePoseRotatingFrame);
currentPoseRotatingGT = currentPoseFixedGT.transform_to(currentPoseRotatingFrame); %currentPoseRotatingGT = currentPoseFixedGT.transform_to(currentRotatingFrame);
inverseCurrentRotatingFrame = (currentRotatingFrame.inverse);
currentPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentPoseFixedGT);
%inverseCurrentPoseRotatingGT = currentRotatingFrame.rotation.inverse;
%TODO: currentPoseRotatingGT.rotation = inverseCurrentPoseRotatingGT.compose(currentPoseFixedGT.rotation);
% Store GT (ground truth) poses % Store GT (ground truth) poses
positionsFixedGT(:,i) = currentPoseFixedGT.translation.vector; positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector;
positionsRotatingGT(:,i) = currentPoseRotatingGT.translation.vector; positionsInRotatingGT(:,i) = currentPoseRotatingGT.translation.vector;
poses(i).p = currentPoseRotatingFrame.translation.vector; currentRotatingFrameForPlot(i).p = currentRotatingFrame.translation.vector;
poses(i).R = currentPoseRotatingFrame.rotation.matrix; currentRotatingFrameForPlot(i).R = currentRotatingFrame.rotation.matrix;
%% Estimate trajectory in rotating frame using the ground truth measurements %% Estimate trajectory in rotating frame using GTSAM (ground truth measurements)
% Instantiate preintegrated measurements class % Instantiate preintegrated measurements class
currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... if IMU_type == 1
zeroBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ... currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3)); zeroBias, ...
IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
IMU_metadata.GyroscopeSigma.^2 * eye(3), ...
IMU_metadata.IntegrationSigma.^2 * eye(3));
elseif IMU_type == 2
currentSummarizedMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements( ...
zeroBias, ...
IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
IMU_metadata.GyroscopeSigma.^2 * eye(3), ...
IMU_metadata.IntegrationSigma.^2 * eye(3), ...
IMU_metadata.BiasAccelerometerSigma.^2 * eye(3), ...
IMU_metadata.BiasGyroscopeSigma.^2 * eye(3), ...
IMU_metadata.BiasAccOmegaInit.^2 * eye(6));
else
error('imuSimulator:coriolisExample:IMU_typeNotFound', ...
'IMU_type = %d does not exist.\nAvailable IMU types are 1 and 2\n', IMU_type);
end
% Add measurement % Add measurement
currentSummarizedMeasurement.integrateMeasurement(accelFixed, omegaFixed, deltaT); currentSummarizedMeasurement.integrateMeasurement(accelFixed, omegaFixed, deltaT);
% Add factors to graph % Add factors to graph
newFactors.add(ImuFactor( ... if IMU_type == 1
currentPoseKey-1, currentVelKey-1, ... newFactors.add(ImuFactor( ...
currentPoseKey, currentVelKey, ... currentPoseKey-1, currentVelKey-1, ...
currentBiasKey-1, currentSummarizedMeasurement, g, omega)); currentPoseKey, currentVelKey, ...
currentBiasKey-1, currentSummarizedMeasurement, g, omegaCoriolisIMU));
%newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b)); newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, zeroBias, ...
newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ... noiseModel.Isotropic.Sigma(6, epsBias)));
noiseModel.Isotropic.Sigma(6, 1e-10))); newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, ...
noiseModel.Isotropic.Sigma(6, epsBias)));
elseif IMU_type == 2
newFactors.add(CombinedImuFactor( ...
currentPoseKey-1, currentVelKey-1, ...
currentPoseKey, currentVelKey, ...
currentBiasKey-1, currentBiasKey, ...
currentSummarizedMeasurement, g, omegaCoriolisIMU, ...
noiseModel.Isotropic.Sigma(15, epsBias)));
else
error('imuSimulator:coriolisExample:IMU_typeNotFound', ...
'IMU_type = %d does not exist.\nAvailable IMU types are 1 and 2\n', IMU_type);
end
% Add values to the graph. Use the current pose and velocity % Add values to the graph. Use the current pose and velocity
% estimates as to values when interpreting the next pose and % estimates as to values when interpreting the next pose and
% velocity estimates % velocity estimates
@ -139,8 +197,7 @@ for i = 1:length(times)
newValues.insert(currentVelKey, currentVelocityEstimate); newValues.insert(currentVelKey, currentVelocityEstimate);
newValues.insert(currentBiasKey, zeroBias); newValues.insert(currentBiasKey, zeroBias);
%newFactors.print(''); %newFactors.print(''); newValues.print('');
%newValues.print('');
%% Solve factor graph %% Solve factor graph
if(i > 1) if(i > 1)
@ -154,25 +211,27 @@ for i = 1:length(times)
currentBias = isam.calculateEstimate(currentBiasKey); currentBias = isam.calculateEstimate(currentBiasKey);
positionsEstimates(:,i) = currentPoseEstimate.translation.vector; positionsEstimates(:,i) = currentPoseEstimate.translation.vector;
%velocitiesEstimates(:,i) = currentVelocityGlobal; velocitiesEstimates(:,i) = currentVelocityEstimate.vector;
biasEstimates(:,i) = currentBias.vector;
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(currentRotatingFrameForPlot(i),1, '-k', 'Rotating Frame',0.1,0.75,1)
%hold on;
plot3(positionsInFixedGT(1,1:i), positionsInFixedGT(2,1:i), positionsInFixedGT(3,1:i),'r');
hold on; hold on;
plot3(positionsFixedGT(1,1:i), positionsFixedGT(2,1:i), positionsFixedGT(3,1:i)); %plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'x');
plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'x'); %plot3(positionsInFixedGT(1,i), positionsInFixedGT(2,i), positionsInFixedGT(3,i), 'o');
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(positionsInRotatingGT(1,1:i), positionsInRotatingGT(2,1:i), positionsInRotatingGT(3,1:i), '-g');
plot3(positionsRotatingGT(1,1), positionsRotatingGT(2,1), positionsRotatingGT(3,1), 'xr'); %plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg');
plot3(positionsRotatingGT(1,i), positionsRotatingGT(2,i), positionsRotatingGT(3,i), 'or'); %plot3(positionsInRotatingGT(1,i), positionsInRotatingGT(2,i), positionsInRotatingGT(3,i), 'og');
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), '-b');
plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xg'); %plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb');
plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'og'); %plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob');
hold off; hold off;
xlabel('X axis') xlabel('X axis')
@ -182,42 +241,63 @@ for i = 1:length(times)
grid on; grid on;
%pause(0.1); %pause(0.1);
i = i + 1; if record_movie == 1
frame = getframe(gcf);
writeVideo(writerObj,frame);
end
end
if record_movie == 1
close(writerObj);
end end
figure figure
%% Print and plot trajectory error results %% Print and plot trajectory error results
positionsError = positionsRotatingGT - positionsEstimates; if navFrameRotating == 0
fprintf('Final position error = %f\n', positionsError(end)); axisPositionsError = positionsInFixedGT - positionsEstimates;
plot(times, positionsError); else
plotTitle = sprintf('Error in Estimated Position (omega = [%.2f; %.2f; %.2f])', omega(1), omega(2), omega(3)); axisPositionsError = positionsInRotatingGT - positionsEstimates;
end
plot(times, abs(axisPositionsError));
plotTitle = sprintf('Axis Error in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ...
IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3));
title(plotTitle); title(plotTitle);
xlabel('Time'); xlabel('Time');
ylabel('Error (ground_truth - estimate)'); ylabel('Error (ground_truth - estimate)');
legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST'); legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST');
figure
positionError3D = sqrt(axisPositionsError(1,:).^2+axisPositionsError(2,:).^2 + axisPositionsError(3,:).^2);
plot(times, positionError3D);
plotTitle = sprintf('3D Error in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ...
IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3));
title(plotTitle);
xlabel('Time');
ylabel('3D error [meters]');
fprintf('Final position error = %f\n', norm(axisPositionsError(:,end)));
%% Plot final trajectories %% Plot final trajectories
figure figure
sphere % sphere for reference sphere % sphere for reference
hold on; hold on;
% Ground truth trajectory in fixed reference frame % Ground truth trajectory in fixed reference frame
plot3(positionsFixedGT(1,:), positionsFixedGT(2,:), positionsFixedGT(3,:)); plot3(positionsInFixedGT(1,:), positionsInFixedGT(2,:), positionsInFixedGT(3,:),'r');
plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'x'); plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr');
plot3(positionsFixedGT(1,end), positionsFixedGT(2,end), positionsFixedGT(3,end), 'o'); plot3(positionsInFixedGT(1,end), positionsInFixedGT(2,end), positionsInFixedGT(3,end), 'or');
% Ground truth trajectory in rotating reference frame % Ground truth trajectory in rotating reference frame
plot3(positionsRotatingGT(1,:), positionsRotatingGT(2,:), positionsRotatingGT(3,:), '-r'); plot3(positionsInRotatingGT(1,:), positionsInRotatingGT(2,:), positionsInRotatingGT(3,:), '-g');
plot3(positionsRotatingGT(1,1), positionsRotatingGT(2,1), positionsRotatingGT(3,1), 'xr'); plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg');
plot3(positionsRotatingGT(1,end), positionsRotatingGT(2,end), positionsRotatingGT(3,end), 'or'); plot3(positionsInRotatingGT(1,end), positionsInRotatingGT(2,end), positionsInRotatingGT(3,end), 'og');
% Estimates % Estimates
plot3(positionsEstimates(1,:), positionsEstimates(2,:), positionsEstimates(3,:), '-g'); plot3(positionsEstimates(1,:), positionsEstimates(2,:), positionsEstimates(3,:), '-b');
plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xg'); plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb');
plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'og'); plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'ob');
xlabel('X axis') xlabel('X axis')
ylabel('Y axis') ylabel('Y axis')
zlabel('Z axis') zlabel('Z axis')
axis equal axis equal
grid on; grid on;
hold off; hold off;