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

Adding imuSimulator directory from old branch for new imu factors

release/4.3a0
Richard Roberts 2014-01-28 12:19:25 -05:00
parent fe55148dd7
commit 29ae7226bc
19 changed files with 1007 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
matlab/unstable_examples/+imuSimulator/+lib/antisim.m Normal file
View File

@ -0,0 +1,7 @@
function S = antisim(v)
% costruisce la matrice antisimmetrica S (3x3) a partire dal vettore v
% Uso: S = antisim(v)
S=[0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0];

140
matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m Normal file
View File

@ -0,0 +1,140 @@
function arrowHandle = arrow3D(pos, deltaValues, colorCode, stemRatio, rhoRatio)
% arrowHandle = arrow3D(pos, deltaValues, colorCode, stemRatio) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Used to plot a single 3D arrow with a cylindrical stem and cone arrowhead
% pos = [X,Y,Z] - spatial location of the starting point of the arrow (end of stem)
% deltaValues = [QX,QY,QZ] - delta parameters denoting the magnitude of the arrow along the x,y,z-axes (relative to 'pos')
% colorCode - Color parameters as per the 'surf' command. For example, 'r', 'red', [1 0 0] are all examples of a red-colored arrow
% stemRatio - The ratio of the length of the stem in proportion to the arrowhead. For example, a call of:
% arrow3D([0,0,0], [100,0,0] , 'r', 0.82) will produce a red arrow of magnitude 100, with the arrowstem spanning a distance
% of 82 (note 0.82 ratio of length 100) while the arrowhead (cone) spans 18.
% rhoRatio - The ratio of the cylinder radius (0.05 is the default)
% value)
%
% Example:
% arrow3D([0,0,0], [4,3,7]); %---- arrow with default parameters
% axis equal;
%
% Author: Shawn Arseneau
% Created: September 14, 2006
% Updated: September 18, 2006
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin<2 || nargin>5
error('Incorrect number of inputs to arrow3D');
end
if numel(pos)~=3 || numel(deltaValues)~=3
error('pos and/or deltaValues is incorrect dimensions (should be three)');
end
if nargin<3
colorCode = 'interp';
end
if nargin<4
stemRatio = 0.75;
end
if nargin<5
rhoRatio = 0.05;
end
X = pos(1); %---- with this notation, there is no need to transpose if the user has chosen a row vs col vector
Y = pos(2);
Z = pos(3);
[sphi, stheta, srho] = cart2sph(deltaValues(1), deltaValues(2), deltaValues(3));
%******************************************* CYLINDER == STEM *********************************************
cylinderRadius = srho*rhoRatio;
cylinderLength = srho*stemRatio;
[CX,CY,CZ] = cylinder(cylinderRadius);
CZ = CZ.*cylinderLength; %---- lengthen
%----- ROTATE CYLINDER
[row, col] = size(CX); %---- initial rotation to coincide with X-axis
newEll = rotatePoints([0 0 -1], [CX(:), CY(:), CZ(:)]);
CX = reshape(newEll(:,1), row, col);
CY = reshape(newEll(:,2), row, col);
CZ = reshape(newEll(:,3), row, col);
[row, col] = size(CX);
newEll = rotatePoints(deltaValues, [CX(:), CY(:), CZ(:)]);
stemX = reshape(newEll(:,1), row, col);
stemY = reshape(newEll(:,2), row, col);
stemZ = reshape(newEll(:,3), row, col);
%----- TRANSLATE CYLINDER
stemX = stemX + X;
stemY = stemY + Y;
stemZ = stemZ + Z;
%******************************************* CONE == ARROWHEAD *********************************************
coneLength = srho*(1-stemRatio);
coneRadius = cylinderRadius*1.5;
incr = 4; %---- Steps of cone increments
coneincr = coneRadius/incr;
[coneX, coneY, coneZ] = cylinder(cylinderRadius*2:-coneincr:0); %---------- CONE
coneZ = coneZ.*coneLength;
%----- ROTATE CONE
[row, col] = size(coneX);
newEll = rotatePoints([0 0 -1], [coneX(:), coneY(:), coneZ(:)]);
coneX = reshape(newEll(:,1), row, col);
coneY = reshape(newEll(:,2), row, col);
coneZ = reshape(newEll(:,3), row, col);
newEll = rotatePoints(deltaValues, [coneX(:), coneY(:), coneZ(:)]);
headX = reshape(newEll(:,1), row, col);
headY = reshape(newEll(:,2), row, col);
headZ = reshape(newEll(:,3), row, col);
%---- TRANSLATE CONE
V = [0, 0, srho*stemRatio]; %---- centerline for cylinder: the multiplier is to set the cone 'on the rim' of the cylinder
Vp = rotatePoints([0 0 -1], V);
Vp = rotatePoints(deltaValues, Vp);
headX = headX + Vp(1) + X;
headY = headY + Vp(2) + Y;
headZ = headZ + Vp(3) + Z;
%************************************************************************************************************
hStem = surf(stemX, stemY, stemZ, 'FaceColor', colorCode, 'EdgeColor', 'none');
hold on;
hHead = surf(headX, headY, headZ, 'FaceColor', colorCode, 'EdgeColor', 'none');
if nargout==1
arrowHandle = [hStem, hHead];
end

14
matlab/unstable_examples/+imuSimulator/+lib/getxyz.m Normal file
View File

@ -0,0 +1,14 @@
function [c] = getxyz(poses, j)
% The function extract the Cartesian variables from pose (pose.p = positions,
% pose.R = rotations). In particular, if there are T poses,
% - getxyz(pose, 1) estracts the vector x \in R^T,
% - getxyz(pose, 2) estracts the vector y \in R^T,
% - getxyz(pose, 3) estracts the vector z \in R^T.
L = length(poses);
c = [];
for i=1:L % for each pose
c = [c poses(i).p(j)];
end
c = c(:); % column vector

19
matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m Normal file
View File

@ -0,0 +1,19 @@
function [] = plot_trajectory(pose, step, color, plot_name,a,b,c)
% Plot a collection of poses: positions are connected by a solid line
% of color "color" and it is shown a ref frame for each pose.
% Plot_name defines the name of the created figure.
x = getxyz(pose,1);
y = getxyz(pose,2);
z = getxyz(pose,3);
plot3(x,y,z,color)
n = length(x)-1;
hold on
for i=1:step:n+1
ref_frame_plot(pose(i).R,pose(i).p,a,b,c)
end
title(plot_name);
xlabel('x')
ylabel('y')
zlabel('z')
view(3)

54
matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m Normal file
View File

@ -0,0 +1,54 @@
function ref_frame_plot(Ref,OriginRef,rhoRatio,stemRatio,axsize)
% ref_frame plot a 3D representation of a reference frame
% given by three unit vectors
%
% ref_frame(Ref,DimSpace,OriginRef)
% Ref is a 3 x 3 orthogonal matrix representing the unit vectors
% of the reference frame to be drawn
% DimSpace is a 3 x 2 matrix with min an max dimensions of the space
% [xmin xmax; ymin ymax; zmin zmax]
% default value = [-1,5 +1.5] for all dimensions
% OriginRef is the reference frame origin point
% default value = [0 0 0]'
% Copright (C) Basilio Bona 2007
n=nargin;
if n == 1
OriginRef=[0 0 0]';
colorcode=['r','g','b'];
rhoRatio=0.05;
stemRatio=0.75;
axsize=1;
end
if n == 2
colorcode=['r','g','b'];
rhoRatio=0.05;
stemRatio=0.75;
axsize=1;
end
if n == 3
colorcode=['r','g','b'];
stemRatio=0.75;
axsize=1;
end
if n == 4
colorcode=['r','g','b'];
axsize=1;
end
if n == 5
colorcode=['r','g','b'];
end
% xproj=DimSpace(1,2); yproj=DimSpace(2,2); zproj=DimSpace(3,1);
%hold off;
arrow3d(OriginRef, axsize*Ref(:,1), colorcode(1), stemRatio, rhoRatio);
arrow3d(OriginRef, axsize*Ref(:,2), colorcode(2), stemRatio, rhoRatio);
arrow3d(OriginRef, axsize*Ref(:,3), colorcode(3), stemRatio, rhoRatio);
axis equal; xlabel('X'); ylabel('Y'); zlabel('Z');
% lighting phong;
% camlight left;

82
matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m Normal file
View File

@ -0,0 +1,82 @@
function rotatedData = rotatePoints(alignmentVector, originalData)
% rotatedData = rotatePoints(alignmentVector, originalData) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Rotate the 'originalData' in the form of Nx2 or Nx3 about the origin by aligning the x-axis with the alignment vector
%
% Rdata = rotatePoints([1,2,-1], [Xpts(:), Ypts(:), Zpts(:)]) - rotate the (X,Y,Z)pts in 3D with respect to the vector [1,2,-1]
%
% Rotating using spherical components can be done by first converting using [dX,dY,dZ] = cart2sph(theta, phi, rho); alignmentVector = [dX,dY,dZ];
%
% Example:
% %% Rotate the point [3,4,-7] with respect to the following:
% %%%% Original associated vector is always [1,0,0]
% %%%% Calculate the appropriate rotation requested with respect to the x-axis. For example, if only a rotation about the z-axis is
% %%%% sought, alignmentVector = [2,1,0] %% Note that the z-component is zero
% rotData = rotatePoints(alignmentVector, [3,4,-7]);
%
% Author: Shawn Arseneau
% Created: Feb.2, 2006
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
alignmentDim = numel(alignmentVector);
DOF = size(originalData,2); %---- DOF = Degrees of Freedom (i.e. 2 for two dimensional and 3 for three dimensional data)
if alignmentDim~=DOF
error('Alignment vector does not agree with originalData dimensions');
end
if DOF<2 || DOF>3
error('rotatePoints only does rotation in two or three dimensions');
end
if DOF==2 % 2D rotation...
[rad_theta, rho] = cart2pol(alignmentVector(1), alignmentVector(2));
deg_theta = -1 * rad_theta * (180/pi);
ctheta = cosd(deg_theta); stheta = sind(deg_theta);
Rmatrix = [ctheta, -1.*stheta;...
stheta, ctheta];
rotatedData = originalData*Rmatrix;
else % 3D rotation...
[rad_theta, rad_phi, rho] = cart2sph(alignmentVector(1), alignmentVector(2), alignmentVector(3));
rad_theta = rad_theta * -1;
deg_theta = rad_theta * (180/pi);
deg_phi = rad_phi * (180/pi);
ctheta = cosd(deg_theta); stheta = sind(deg_theta);
Rz = [ctheta, -1.*stheta, 0;...
stheta, ctheta, 0;...
0, 0, 1]; %% First rotate as per theta around the Z axis
rotatedData = originalData*Rz;
[rotX, rotY, rotZ] = sph2cart(-1* (rad_theta+(pi/2)), 0, 1); %% Second rotation corresponding to phi
rotationAxis = [rotX, rotY, rotZ];
u = rotationAxis(:)/norm(rotationAxis); %% Code extract from rotate.m from MATLAB
cosPhi = cosd(deg_phi);
sinPhi = sind(deg_phi);
invCosPhi = 1 - cosPhi;
x = u(1);
y = u(2);
z = u(3);
Rmatrix = [cosPhi+x^2*invCosPhi x*y*invCosPhi-z*sinPhi x*z*invCosPhi+y*sinPhi; ...
x*y*invCosPhi+z*sinPhi cosPhi+y^2*invCosPhi y*z*invCosPhi-x*sinPhi; ...
x*z*invCosPhi-y*sinPhi y*z*invCosPhi+x*sinPhi cosPhi+z^2*invCosPhi]';
rotatedData = rotatedData*Rmatrix;
end

14
matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m Normal file
View File

@ -0,0 +1,14 @@
function R = uth2rot(u,teta)
% Uso: R = uth2rot(u,teta)
%
% calcola la matrice R
% a partire da asse u ed angolo di rotazione teta (in rad)
S=antisim(u);
t=teta;
n=norm(u);
R = eye(3) + sin(t)/n*S + (1-cos(t))/n^2*S^2;

146
matlab/unstable_examples/+imuSimulator/IMUComparison.m Normal file
View File

@ -0,0 +1,146 @@
import gtsam.*;
deltaT = 0.001;
summarizedDeltaT = 0.1;
timeElapsed = 1;
times = 0:deltaT:timeElapsed;
omega = [0;0;2*pi];
velocity = [1;0;0];
summaryTemplate = gtsam.ImuFactorPreintegratedMeasurements( ...
gtsam.imuBias.ConstantBias([0;0;0], [0;0;0]), ...
1e-3 * eye(3), 1e-3 * eye(3), 1e-3 * eye(3));
%% Set initial conditions for the true trajectory and for the estimates
% (one estimate is obtained by integrating in the body frame, the other
% by integrating in the navigation frame)
% Initial state (body)
currentPoseGlobal = Pose3;
currentVelocityGlobal = velocity;
% Initial state estimate (integrating in navigation frame)
currentPoseGlobalIMUnav = currentPoseGlobal;
currentVelocityGlobalIMUnav = currentVelocityGlobal;
% Initial state estimate (integrating in the body frame)
currentPoseGlobalIMUbody = currentPoseGlobal;
currentVelocityGlobalIMUbody = currentVelocityGlobal;
%% Prepare data structures for actual trajectory and estimates
% Actual trajectory
positions = zeros(3, length(times)+1);
positions(:,1) = currentPoseGlobal.translation.vector;
poses(1).p = positions(:,1);
poses(1).R = currentPoseGlobal.rotation.matrix;
% Trajectory estimate (integrated in the navigation frame)
positionsIMUnav = zeros(3, length(times)+1);
positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector;
posesIMUnav(1).p = positionsIMUnav(:,1);
posesIMUnav(1).R = poses(1).R;
% Trajectory estimate (integrated in the body frame)
positionsIMUbody = zeros(3, length(times)+1);
positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector;
posesIMUbody(1).p = positionsIMUbody(:,1);
posesIMUbody(1).R = poses(1).R;
%% Solver object
isamParams = ISAM2Params;
isamParams.setRelinearizeSkip(1);
isam = gtsam.ISAM2(isamParams);
initialValues = Values;
initialValues.insert(symbol('x',0), currentPoseGlobal);
initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal));
initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0]));
initialFactors = NonlinearFactorGraph;
initialFactors.add(PriorFactorPose3(symbol('x',0), ...
currentPoseGlobal, noiseModel.Isotropic.Sigma(6, 1.0)));
initialFactors.add(PriorFactorLieVector(symbol('v',0), ...
LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, 1.0)));
initialFactors.add(PriorFactorConstantBias(symbol('b',0), ...
imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, 1.0)));
%% Main loop
i = 2;
lastSummaryTime = 0;
lastSummaryIndex = 0;
currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate);
for t = times
%% Create the actual trajectory, using the velocities and
% accelerations in the inertial frame to compute the positions
[ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
currentPoseGlobal, omega, velocity, velocity, deltaT);
%% Simulate IMU measurements, considering Coriolis effect
% (in this simple example we neglect gravity and there are no other forces acting on the body)
acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
omega, omega, velocity, velocity, deltaT);
%% Accumulate preintegrated measurement
currentSummarizedMeasurement.integrateMeasurement(acc_omega(1:3), acc_omega(4:6), deltaT);
%% Update solver
if t - lastSummaryTime >= summarizedDeltaT
% Create IMU factor
initialFactors.add(ImuFactor( ...
symbol('x',lastSummaryIndex), symbol('v',lastSummaryIndex), ...
symbol('x',lastSummaryIndex+1), symbol('v',lastSummaryIndex+1), ...
symbol('b',0), currentSummarizedMeasurement, [0;0;1], [0;0;0], ...
noiseModel.Isotropic.Sigma(9, 1e-6)));
% Predict movement in a straight line (bad initialization)
if lastSummaryIndex > 0
initialPose = isam.calculateEstimate(symbol('x',lastSummaryIndex)) ...
.compose(Pose3(Rot3, Point3( velocity * t - lastSummaryTime) ));
initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex));
else
initialPose = Pose3;
initialVel = LieVector(velocity);
end
initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose);
initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel);
% Update solver
isam.update(initialFactors, initialValues);
initialFactors = NonlinearFactorGraph;
initialValues = Values;
lastSummaryIndex = lastSummaryIndex + 1;
lastSummaryTime = t;
currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate);
end
%% Integrate in the body frame
[ currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody ] = imuSimulator.integrateIMUTrajectory_bodyFrame( ...
currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody, acc_omega, deltaT, velocity);
%% Integrate in the navigation frame
[ currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav ] = imuSimulator.integrateIMUTrajectory_navFrame( ...
currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT);
%% Store data in some structure for statistics and plots
positions(:,i) = currentPoseGlobal.translation.vector;
positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector;
positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector;
% -
poses(i).p = positions(:,i);
posesIMUbody(i).p = positionsIMUbody(:,i);
posesIMUnav(i).p = positionsIMUnav(:,i);
% -
poses(i).R = currentPoseGlobal.rotation.matrix;
posesIMUbody(i).R = currentPoseGlobalIMUbody.rotation.matrix;
posesIMUnav(i).R = currentPoseGlobalIMUnav.rotation.matrix;
i = i + 1;
end
figure(1)
hold on;
plot(positions(1,:), positions(2,:), '-b');
plot(positionsIMUbody(1,:), positionsIMUbody(2,:), '-r');
plot(positionsIMUnav(1,:), positionsIMUnav(2,:), ':k');
plot3DTrajectory(isam.calculateEstimate, 'g-');
axis equal;
legend('true trajectory', 'traj integrated in body', 'traj integrated in nav')

128
matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m Normal file
View File

@ -0,0 +1,128 @@
close all
clc
import gtsam.*;
deltaT = 0.001;
summarizedDeltaT = 0.1;
timeElapsed = 1;
times = 0:deltaT:timeElapsed;
omega = [0;0;2*pi];
velocity = [1;0;0];
g = [0;0;0];
cor_v = [0;0;0];
summaryTemplate = gtsam.ImuFactorPreintegratedMeasurements( ...
gtsam.imuBias.ConstantBias([0;0;0], [0;0;0]), ...
1e-3 * eye(3), 1e-3 * eye(3), 1e-3 * eye(3));
%% Set initial conditions for the true trajectory and for the estimates
% (one estimate is obtained by integrating in the body frame, the other
% by integrating in the navigation frame)
% Initial state (body)
currentPoseGlobal = Pose3;
currentVelocityGlobal = velocity;
%% Prepare data structures for actual trajectory and estimates
% Actual trajectory
positions = zeros(3, length(times)+1);
positions(:,1) = currentPoseGlobal.translation.vector;
poses(1).p = positions(:,1);
poses(1).R = currentPoseGlobal.rotation.matrix;
%% Solver object
isamParams = ISAM2Params;
isamParams.setRelinearizeSkip(1);
isam = gtsam.ISAM2(isamParams);
sigma_init_x = 1.0;
sigma_init_v = 1.0;
sigma_init_b = 1.0;
initialValues = Values;
initialValues.insert(symbol('x',0), currentPoseGlobal);
initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal));
initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0]));
initialFactors = NonlinearFactorGraph;
% Prior on initial pose
initialFactors.add(PriorFactorPose3(symbol('x',0), ...
currentPoseGlobal, noiseModel.Isotropic.Sigma(6, sigma_init_x)));
% Prior on initial velocity
initialFactors.add(PriorFactorLieVector(symbol('v',0), ...
LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, sigma_init_v)));
% Prior on initial bias
initialFactors.add(PriorFactorConstantBias(symbol('b',0), ...
imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, sigma_init_b)));
%% Main loop
i = 2;
lastSummaryTime = 0;
lastSummaryIndex = 0;
currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate);
for t = times
%% Create the ground truth trajectory, using the velocities and accelerations in the inertial frame to compute the positions
[ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
currentPoseGlobal, omega, velocity, velocity, deltaT);
%% Simulate IMU measurements, considering Coriolis effect
% (in this simple example we neglect gravity and there are no other forces acting on the body)
acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
omega, omega, velocity, velocity, deltaT);
%% Accumulate preintegrated measurement
currentSummarizedMeasurement.integrateMeasurement(acc_omega(1:3), acc_omega(4:6), deltaT);
%% Update solver
if t - lastSummaryTime >= summarizedDeltaT
% Create IMU factor
initialFactors.add(ImuFactor( ...
symbol('x',lastSummaryIndex), symbol('v',lastSummaryIndex), ...
symbol('x',lastSummaryIndex+1), symbol('v',lastSummaryIndex+1), ...
symbol('b',0), currentSummarizedMeasurement, g, cor_v, ...
noiseModel.Isotropic.Sigma(9, 1e-6)));
% Predict movement in a straight line (bad initialization)
if lastSummaryIndex > 0
initialPose = isam.calculateEstimate(symbol('x',lastSummaryIndex)) ...
.compose(Pose3(Rot3, Point3( velocity * t - lastSummaryTime) ));
initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex));
else
initialPose = Pose3;
initialVel = LieVector(velocity);
end
initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose);
initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel);
key_pose = symbol('x',lastSummaryIndex+1);
% Update solver
isam.update(initialFactors, initialValues);
initialFactors = NonlinearFactorGraph;
initialValues = Values;
isam.calculateEstimate(key_pose);
M = isam.marginalCovariance(key_pose);
lastSummaryIndex = lastSummaryIndex + 1;
lastSummaryTime = t;
currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate);
end
%% Store data in some structure for statistics and plots
positions(:,i) = currentPoseGlobal.translation.vector;
i = i + 1;
end
figure(1)
hold on;
plot(positions(1,:), positions(2,:), '-b');
plot3DTrajectory(isam.calculateEstimate, 'g-');
axis equal;
legend('true trajectory', 'traj integrated in body', 'traj integrated in nav')

14
matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m Normal file
View File

@ -0,0 +1,14 @@
function [ acc_omega ] = calculateIMUMeas_coriolis(omega1Body, omega2Body, velocity1Body, velocity2Body, deltaT)
import gtsam.*;
% gyro measured rotation rate
measuredOmega = omega1Body;
% Acceleration measurement (in this simple toy example no other forces
% act on the body and the only acceleration is the centripetal Coriolis acceleration)
measuredAcc = Point3(cross(omega1Body, velocity1Body)).vector;
acc_omega = [ measuredAcc; measuredOmega ];
end

26
matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m Normal file
View File

@ -0,0 +1,26 @@
function [ acc_omega ] = calculateIMUMeasurement(omega1Body, omega2Body, velocity1Body, velocity2Body, deltaT, imuFrame)
%CALCULATEIMUMEASUREMENT Calculate measured body frame acceleration in
%frame 1 and measured angular rates in frame 1.
import gtsam.*;
% Calculate gyro measured rotation rate by transforming body rotation rate
% into the IMU frame.
measuredOmega = imuFrame.rotation.unrotate(Point3(omega1Body)).vector;
% Transform both velocities into IMU frame, accounting for the velocity
% induced by rigid body rotation on a lever arm (Coriolis effect).
velocity1inertial = imuFrame.rotation.unrotate( ...
Point3(velocity1Body + cross(omega1Body, imuFrame.translation.vector))).vector;
imu2in1 = Rot3.Expmap(measuredOmega * deltaT);
velocity2inertial = imu2in1.rotate(imuFrame.rotation.unrotate( ...
Point3(velocity2Body + cross(omega2Body, imuFrame.translation.vector)))).vector;
% Acceleration in IMU frame
measuredAcc = (velocity2inertial - velocity1inertial) / deltaT;
acc_omega = [ measuredAcc; measuredOmega ];
end

17
matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m Normal file
View File

@ -0,0 +1,17 @@
function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory( ...
initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT)
%INTEGRATETRAJECTORY Integrate one trajectory step from IMU measurement
import gtsam.*;
imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT);
accelGlobal = initialPoseGlobal.rotation().rotate(Point3(acc_omegaIMU(1:3))).vector;
finalPosition = Point3(initialPoseGlobal.translation.vector ...
+ initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT;
finalRotation = initialPoseGlobal.rotation.compose(imu2in1);
finalPose = Pose3(finalRotation, finalPosition);
end

26
matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m Normal file
View File

@ -0,0 +1,26 @@
function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory_bodyFrame( ...
initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT, velocity1Body)
% Before integrating in the body frame we need to compensate for the Coriolis
% effect
acc_body = acc_omegaIMU(1:3) - Point3(cross(acc_omegaIMU(4:6), velocity1Body)).vector;
% after compensating for coriolis this will be essentially zero
% since we are moving at constant body velocity
import gtsam.*;
%% Integrate in the body frame
% Integrate rotations
imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT);
% Integrate positions
finalPositionBody = velocity1Body * deltaT + 0.5 * acc_body * deltaT * deltaT;
finalVelocityBody = velocity1Body + acc_body * deltaT;
%% Express the integrated quantities in the global frame
finalVelocityGlobal = initialVelocityGlobal + (initialPoseGlobal.rotation().rotate(Point3(finalVelocityBody)).vector() );
finalPosition = initialPoseGlobal.translation().vector() + initialPoseGlobal.rotation().rotate( Point3(finalPositionBody)).vector() ;
finalRotation = initialPoseGlobal.rotation.compose(imu2in1);
% Include position and rotation in a pose
finalPose = Pose3(finalRotation, Point3(finalPosition) );
end

21
matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m Normal file
View File

@ -0,0 +1,21 @@
function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory_navFrame( ...
initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT)
%INTEGRATETRAJECTORY Integrate one trajectory step from IMU measurement
import gtsam.*;
% Integrate rotations
imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT);
finalRotation = initialPoseGlobal.rotation.compose(imu2in1);
intermediateRotation = initialPoseGlobal.rotation.compose( Rot3.Expmap(acc_omegaIMU(4:6) * deltaT/2 ));
% Integrate positions (equation (1) in Lupton)
accelGlobal = intermediateRotation.rotate(Point3(acc_omegaIMU(1:3))).vector;
finalPosition = Point3(initialPoseGlobal.translation.vector ...
+ initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT;
% Include position and rotation in a pose
finalPose = Pose3(finalRotation, finalPosition);
end

24
matlab/unstable_examples/+imuSimulator/integrateTrajectory.m Normal file
View File

@ -0,0 +1,24 @@
function [ finalPose, finalVelocityGlobal ] = integrateTrajectory( ...
initialPose, omega1Body, velocity1Body, velocity2Body, deltaT)
%INTEGRATETRAJECTORY Integrate one trajectory step
import gtsam.*;
% Rotation: R^1_2
body2in1 = Rot3.Expmap(omega1Body * deltaT);
% Velocity 2 in frame 1: v^1_2 = R^1_2 v^2_2
velocity2inertial = body2in1.rotate(Point3(velocity2Body)).vector;
% Acceleration: a^1 = (v^1_2 - v^1_1)/dt
accelBody1 = (velocity2inertial - velocity1Body) / deltaT;
% Velocity 1 in frame W: v^W_1 = R^W_1 v^1_1
initialVelocityGlobal = initialPose.rotation().rotate(Point3(velocity1Body)).vector;
% Acceleration in frame W: a^W = R^W_1 a^1
accelGlobal = initialPose.rotation().rotate(Point3(accelBody1)).vector;
finalPosition = Point3(initialPose.translation.vector + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT;
finalRotation = initialPose.rotation.compose(body2in1);
finalPose = Pose3(finalRotation, finalPosition);
end

48
matlab/unstable_examples/+imuSimulator/test1onestep.m Normal file
View File

@ -0,0 +1,48 @@
import gtsam.*;
deltaT = 0.01;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Constant global velocity w/ lever arm
disp('--------------------------------------------------------');
disp('Constant global velocity w/ lever arm');
omega = [0;0;0.1];
velocity = [1;0;0];
R = Rot3.Expmap(omega * deltaT);
velocity2body = R.unrotate(Point3(velocity)).vector;
acc_omegaExpected = [-0.01; 0; 0; 0; 0; 0.1];
acc_omegaActual = imuSimulator.calculateIMUMeasurement(omega, omega, velocity, velocity2body, deltaT, Pose3(Rot3, Point3([1;0;0])));
disp('IMU measurement discrepancy:');
disp(acc_omegaActual - acc_omegaExpected);
initialPose = Pose3;
finalPoseExpected = Pose3(Rot3.Expmap(omega * deltaT), Point3(velocity * deltaT));
finalVelocityExpected = velocity;
[ finalPoseActual, finalVelocityActual ] = imuSimulator.integrateTrajectory(initialPose, omega, velocity, velocity2body, deltaT);
disp('Final pose discrepancy:');
disp(finalPoseExpected.between(finalPoseActual).matrix);
disp('Final velocity discrepancy:');
disp(finalVelocityActual - finalVelocityExpected);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Constant body velocity w/ lever arm
disp('--------------------------------------------------------');
disp('Constant body velocity w/ lever arm');
omega = [0;0;0.1];
velocity = [1;0;0];
acc_omegaExpected = [-0.01; 0.1; 0; 0; 0; 0.1];
acc_omegaActual = imuSimulator.calculateIMUMeasurement(omega, omega, velocity, velocity, deltaT, Pose3(Rot3, Point3([1;0;0])));
disp('IMU measurement discrepancy:');
disp(acc_omegaActual - acc_omegaExpected);
initialPose = Pose3;
initialVelocity = velocity;
finalPoseExpected = Pose3.Expmap([ omega; velocity ] * deltaT);
finalVelocityExpected = finalPoseExpected.rotation.rotate(Point3(velocity)).vector;
[ finalPoseActual, finalVelocityActual ] = imuSimulator.integrateTrajectory(initialPose, omega, velocity, velocity, deltaT);
disp('Final pose discrepancy:');
disp(finalPoseExpected.between(finalPoseActual).matrix);
disp('Final velocity discrepancy:');
disp(finalVelocityActual - finalVelocityExpected);

34
matlab/unstable_examples/+imuSimulator/test2constglobal.m Normal file
View File

@ -0,0 +1,34 @@
import gtsam.*;
deltaT = 0.01;
timeElapsed = 1000;
times = 0:deltaT:timeElapsed;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Constant global velocity w/ lever arm
disp('--------------------------------------------------------');
disp('Constant global velocity w/ lever arm');
omega = [0;0;0.1];
velocity = [1;0;0];
% Initial state
currentPoseGlobal = Pose3;
currentVelocityGlobal = velocity;
% Positions
positions = zeros(3, length(times)+1);
i = 2;
for t = times
velocity1body = currentPoseGlobal.rotation.unrotate(Point3(currentVelocityGlobal)).vector;
R = Rot3.Expmap(omega * deltaT);
velocity2body = currentPoseGlobal.rotation.compose(R).unrotate(Point3(currentVelocityGlobal)).vector;
[ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory(currentPoseGlobal, omega, velocity1body, velocity2body, deltaT);
positions(:,i) = currentPoseGlobal.translation.vector;
i = i + 1;
end
figure;
plot(positions(1,:), positions(2,:), '.-');

96
matlab/unstable_examples/+imuSimulator/test3constbody.m Normal file
View File

@ -0,0 +1,96 @@
clc
clear all
close all
import gtsam.*;
addpath(genpath('./Libraries'))
deltaT = 0.01;
timeElapsed = 25;
times = 0:deltaT:timeElapsed;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Constant body velocity w/ lever arm
disp('--------------------------------------------------------');
disp('Constant body velocity w/ lever arm');
omega = [0;0;0.1];
velocity = [1;0;0];
RIMUinBody = Rot3.Rz(-pi/2);
% RIMUinBody = eye(3)
IMUinBody = Pose3(RIMUinBody, Point3([1;0;0]));
% Initial state (body)
currentPoseGlobal = Pose3;
currentVelocityGlobal = velocity;
% Initial state (IMU)
currentPoseGlobalIMU = Pose3; %currentPoseGlobal.compose(IMUinBody);
%currentVelocityGlobalIMU = IMUinBody.rotation.unrotate(Point3(velocity)).vector; % no Coriolis here?
currentVelocityGlobalIMU = IMUinBody.rotation.unrotate( ...
Point3(velocity + cross(omega, IMUinBody.translation.vector))).vector;
% Positions
% body trajectory
positions = zeros(3, length(times)+1);
positions(:,1) = currentPoseGlobal.translation.vector;
poses(1).p = positions(:,1);
poses(1).R = currentPoseGlobal.rotation.matrix;
% Expected IMU trajectory (from body trajectory and lever arm)
positionsIMUe = zeros(3, length(times)+1);
positionsIMUe(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector;
posesIMUe(1).p = positionsIMUe(:,1);
posesIMUe(1).R = poses(1).R * IMUinBody.rotation.matrix;
% Integrated IMU trajectory (from IMU measurements)
positionsIMU = zeros(3, length(times)+1);
positionsIMU(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector;
posesIMU(1).p = positionsIMU(:,1);
posesIMU(1).R = IMUinBody.compose(currentPoseGlobalIMU).rotation.matrix;
i = 2;
for t = times
[ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
currentPoseGlobal, omega, velocity, velocity, deltaT);
acc_omega = imuSimulator.calculateIMUMeasurement( ...
omega, omega, velocity, velocity, deltaT, IMUinBody);
[ currentPoseGlobalIMU, currentVelocityGlobalIMU ] = imuSimulator.integrateIMUTrajectory( ...
currentPoseGlobalIMU, currentVelocityGlobalIMU, acc_omega, deltaT);
% Store data in some structure for statistics and plots
positions(:,i) = currentPoseGlobal.translation.vector;
positionsIMUe(:,i) = currentPoseGlobal.translation.vector + currentPoseGlobal.rotation.matrix * IMUinBody.translation.vector;
positionsIMU(:,i) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector;
poses(i).p = positions(:,i);
posesIMUe(i).p = positionsIMUe(:,i);
posesIMU(i).p = positionsIMU(:,i);
poses(i).R = currentPoseGlobal.rotation.matrix;
posesIMUe(i).R = poses(i).R * IMUinBody.rotation.matrix;
posesIMU(i).R = IMUinBody.compose(currentPoseGlobalIMU).rotation.matrix;
i = i + 1;
end
figure(1)
plot_trajectory(poses, 50, '-k', 'body trajectory',0.1,0.75,1)
hold on
plot_trajectory(posesIMU, 50, '-r', 'imu trajectory',0.1,0.75,1)
figure(2)
hold on;
plot(positions(1,:), positions(2,:), '-b');
plot(positionsIMU(1,:), positionsIMU(2,:), '-r');
plot(positionsIMUe(1,:), positionsIMUe(2,:), ':k');
axis equal;
disp('Mismatch between final integrated IMU position and expected IMU position')
disp(norm(posesIMUe(end).p - posesIMU(end).p))
disp('Mismatch between final integrated IMU orientation and expected IMU orientation')
disp(norm(posesIMUe(end).R - posesIMU(end).R))

97
matlab/unstable_examples/+imuSimulator/test4circle.m Normal file
View File

@ -0,0 +1,97 @@
clc
clear all
close all
import gtsam.*;
deltaT = 0.001;
timeElapsed = 1;
times = 0:deltaT:timeElapsed;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('--------------------------------------------------------');
disp('Integration in body frame VS integration in navigation frame');
disp('TOY EXAMPLE:');
disp('- Body moves along a circular trajectory with constant rotation rate -omega- and constant -velocity- (in the body frame)');
disp('- We compare two integration schemes: integating in the navigation frame (similar to Lupton approach) VS integrating in the body frame')
disp('- Navigation frame is assumed inertial for simplicity')
omega = [0;0;2*pi];
velocity = [1;0;0];
%% Set initial conditions for the true trajectory and for the estimates
% (one estimate is obtained by integrating in the body frame, the other
% by integrating in the navigation frame)
% Initial state (body)
currentPoseGlobal = Pose3;
currentVelocityGlobal = velocity;
% Initial state estimate (integrating in navigation frame)
currentPoseGlobalIMUnav = currentPoseGlobal;
currentVelocityGlobalIMUnav = currentVelocityGlobal;
% Initial state estimate (integrating in the body frame)
currentPoseGlobalIMUbody = currentPoseGlobal;
currentVelocityGlobalIMUbody = currentVelocityGlobal;
%% Prepare data structures for actual trajectory and estimates
% Actual trajectory
positions = zeros(3, length(times)+1);
positions(:,1) = currentPoseGlobal.translation.vector;
poses(1).p = positions(:,1);
poses(1).R = currentPoseGlobal.rotation.matrix;
% Trajectory estimate (integrated in the navigation frame)
positionsIMUnav = zeros(3, length(times)+1);
positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector;
posesIMUnav(1).p = positionsIMUnav(:,1);
posesIMUnav(1).R = poses(1).R;
% Trajectory estimate (integrated in the body frame)
positionsIMUbody = zeros(3, length(times)+1);
positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector;
posesIMUbody(1).p = positionsIMUbody(:,1);
posesIMUbody(1).R = poses(1).R;
%% Main loop
i = 2;
for t = times
%% Create the actual trajectory, using the velocities and
% accelerations in the inertial frame to compute the positions
[ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
currentPoseGlobal, omega, velocity, velocity, deltaT);
%% Simulate IMU measurements, considering Coriolis effect
% (in this simple example we neglect gravity and there are no other forces acting on the body)
acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
omega, omega, velocity, velocity, deltaT);
%% Integrate in the body frame
[ currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody ] = imuSimulator.integrateIMUTrajectory_bodyFrame( ...
currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody, acc_omega, deltaT, velocity);
%% Integrate in the navigation frame
[ currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav ] = imuSimulator.integrateIMUTrajectory_navFrame( ...
currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT);
%% Store data in some structure for statistics and plots
positions(:,i) = currentPoseGlobal.translation.vector;
positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector;
positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector;
% -
poses(i).p = positions(:,i);
posesIMUbody(i).p = positionsIMUbody(:,i);
posesIMUnav(i).p = positionsIMUnav(:,i);
% -
poses(i).R = currentPoseGlobal.rotation.matrix;
posesIMUbody(i).R = currentPoseGlobalIMUbody.rotation.matrix;
posesIMUnav(i).R = currentPoseGlobalIMUnav.rotation.matrix;
i = i + 1;
end
figure(1)
hold on;
plot(positions(1,:), positions(2,:), '-b');
plot(positionsIMUbody(1,:), positionsIMUbody(2,:), '-r');
plot(positionsIMUnav(1,:), positionsIMUnav(2,:), ':k');
axis equal;
legend('true trajectory', 'traj integrated in body', 'traj integrated in nav')