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

coriolisExample: added basic IMU inference and fixed a plotting bug

release/4.3a0
djensen3 2014-02-06 15:10:36 -05:00
parent f327a4ab46
commit 3e7e4d19f6
1 changed files with 128 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -14,16 +14,29 @@ close all
import gtsam.*; import gtsam.*;
addpath(genpath('./Libraries'))
%% General configuration %% General configuration
deltaT = 0.1; deltaT = 0.1;
timeElapsed = 10; 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;5*pi/10];
omega = [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;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;
g = [0;0;0]; % Gravity, will need to fix this b/c of ECEF frame
zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
IMU_metadata.AccelerometerSigma = 1e-5;
IMU_metadata.GyroscopeSigma = 1e-7;
IMU_metadata.IntegrationSigma = 1e-10;
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, 1e-10);
%% Initial state of the body in the fixed in rotating frames should be the same %% Initial state of the body in the fixed in rotating frames should be the same
currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition)); currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition));
@ -33,22 +46,51 @@ currentPoseRotatingGT = currentPoseFixedGT;
currentPoseRotatingFrame = Pose3; currentPoseRotatingFrame = Pose3;
%% Initialize storage variables %% Initialize storage variables
positionsFixedGT = zeros(3, length(times)+1); positionsFixedGT = zeros(3, length(times));
positionsRotatingGT = zeros(3, length(times)+1); positionsRotatingGT = zeros(3, length(times));
positionsFixedGT(:,1) = currentPoseFixedGT.translation.vector;
positionsRotatingGT(:,1) = currentPoseRotatingGT.translation.vector;
changePoseRotatingFrame = Pose3.Expmap([omega*deltaT; 0; 0; 0]); changePoseRotatingFrame = Pose3.Expmap([omega*deltaT; 0; 0; 0]);
poses(1).p = currentPoseRotatingFrame.translation.vector;
poses(1).R = currentPoseRotatingFrame.rotation.matrix;
h = figure(1); h = figure(1);
positionsEstimates = zeros(3,length(times)+1);
% Solver object
isamParams = ISAM2Params;
isamParams.setFactorization('CHOLESKY');
isamParams.setRelinearizeSkip(10);
isam = gtsam.ISAM2(isamParams);
newFactors = NonlinearFactorGraph;
newValues = Values;
%% Main loop: iterate through %% Main loop: iterate through
i = 2; for i = 1:length(times)
for t = times t = times(i);
% Create keys for current state
currentPoseKey = symbol('x', i);
currentVelKey = symbol('v', i);
currentBiasKey = symbol('b', i);
if(i == 1)
positionsFixedGT(:,1) = currentPoseFixedGT.translation.vector;
positionsRotatingGT(:,1) = currentPoseRotatingGT.translation.vector;
poses(1).p = currentPoseRotatingFrame.translation.vector;
poses(1).R = currentPoseRotatingFrame.rotation.matrix;
currentPoseGlobal = currentPoseFixedGT;
currentVelocityGlobal = LieVector(currentVelocityFixedGT);
% Set Priors
newValues.insert(currentPoseKey, currentPoseGlobal);
newValues.insert(currentVelKey, currentVelocityGlobal);
newValues.insert(currentBiasKey, zeroBias);
newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x));
newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b));
else
%% Create ground truth trajectory %% Create ground truth trajectory
% Update the position and velocity % Update the position and velocity
% x_t = x_0 + v_0*dt + 1/2*a_0*dt^2 % x_t = x_0 + v_0*dt + 1/2*a_0*dt^2
@ -69,16 +111,62 @@ for t = 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;
% incremental plotting for animation %% Estimate trajectory in rotating frame using the ground truth
% measurements
% Instantiate preintegrated measurements class
currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
zeroBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3));
% Add measurement
currentSummarizedMeasurement.integrateMeasurement(accelFixed, omegaFixed, deltaT);
% Add factors to graph
newFactors.add(ImuFactor( ...
currentPoseKey-1, currentVelKey-1, ...
currentPoseKey, currentVelKey, ...
currentBiasKey-1, currentSummarizedMeasurement, g, omega));
%newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b));
newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ...
noiseModel.Isotropic.Sigma(6, 1e-10)));
% Add values to the graph
newValues.insert(currentPoseKey, currentPoseGlobal);
newValues.insert(currentVelKey, currentVelocityGlobal);
newValues.insert(currentBiasKey, zeroBias);
%newFactors.print('');
%newValues.print('');
%% Solve factor graph
if(i > 1)
isam.update(newFactors, newValues);
newFactors = NonlinearFactorGraph;
newValues = Values;
currentPoseGlobal = isam.calculateEstimate(currentPoseKey);
currentVelocityGlobal = isam.calculateEstimate(currentVelKey);
currentBias = isam.calculateEstimate(currentBiasKey);
positionsEstimates(:,i) = currentPoseGlobal.translation.vector;
%velocitiesEstimates(:,i) = currentVelocityGlobal;
end
end
%% 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,:), positionsFixedGT(2,:), positionsFixedGT(3,:)); plot3(positionsFixedGT(1,1:i), positionsFixedGT(2,1:i), positionsFixedGT(3,1:i));
plot3(positionsRotatingGT(1,:), positionsRotatingGT(2,:), positionsRotatingGT(3,:), '-r');
plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'o'); plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'o');
plot3(positionsFixedGT(1,end), positionsFixedGT(2,end), positionsFixedGT(3,end), 'x'); plot3(positionsFixedGT(1,end), positionsFixedGT(2,end), positionsFixedGT(3,end), 'x');
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), 'or');
plot3(positionsRotatingGT(1,end), positionsRotatingGT(2,end), positionsRotatingGT(3,end), 'xr'); plot3(positionsRotatingGT(1,end), positionsRotatingGT(2,end), positionsRotatingGT(3,end), 'xr');
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,end), positionsEstimates(2,end), positionsEstimates(3,end), 'xg');
hold off; hold off;
xlabel('X axis') xlabel('X axis')
ylabel('Y axis') ylabel('Y axis')
@ -99,12 +187,12 @@ plot3(positionsFixedGT(1,:), positionsFixedGT(2,:), positionsFixedGT(3,:));
plot3(positionsRotatingGT(1,:), positionsRotatingGT(2,:), positionsRotatingGT(3,:), '-r'); plot3(positionsRotatingGT(1,:), positionsRotatingGT(2,:), positionsRotatingGT(3,:), '-r');
% beginning and end points of Fixed % beginning and end points of Fixed
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,end), positionsFixedGT(2,end), positionsFixedGT(3,end), 'o');
% beginning and end points of Rotating % beginning and end points of Rotating
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,end), positionsRotatingGT(2,end), positionsRotatingGT(3,end), 'or');
xlabel('X axis') xlabel('X axis')
ylabel('Y axis') ylabel('Y axis')