Adding imuSimulator directory from old branch for new imu factors
Richard Roberts
parent
fe55148dd7
commit
29ae7226bc
|
|
@ -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];
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
|
|
@ -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;
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -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;
|
||||
|
||||
|
|
@ -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')
|
||||
|
||||
|
||||
|
|
@ -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')
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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);
|
||||
|
|
@ -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,:), '.-');
|
||||
|
|
@ -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))
|
||||
|
||||
|
|
@ -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')
|
||||
|
||||
|
||||
Loading…
Reference in New Issue