From 29ae7226bc364aaa8902f6fde6ba5789fb59c055 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 28 Jan 2014 12:19:25 -0500 Subject: [PATCH] Adding imuSimulator directory from old branch for new imu factors --- .../+imuSimulator/+lib/antisim.m | 7 + .../+imuSimulator/+lib/arrow3d.m | 140 +++++++++++++++++ .../+imuSimulator/+lib/getxyz.m | 14 ++ .../+imuSimulator/+lib/plot_trajectory.m | 19 +++ .../+imuSimulator/+lib/ref_frame_plot.m | 54 +++++++ .../+imuSimulator/+lib/rotatePoints.m | 82 ++++++++++ .../+imuSimulator/+lib/uth2rot.m | 14 ++ .../+imuSimulator/IMUComparison.m | 146 ++++++++++++++++++ .../+imuSimulator/IMUComparison_with_cov.m | 128 +++++++++++++++ .../+imuSimulator/calculateIMUMeas_coriolis.m | 14 ++ .../+imuSimulator/calculateIMUMeasurement.m | 26 ++++ .../+imuSimulator/integrateIMUTrajectory.m | 17 ++ .../integrateIMUTrajectory_bodyFrame.m | 26 ++++ .../integrateIMUTrajectory_navFrame.m | 21 +++ .../+imuSimulator/integrateTrajectory.m | 24 +++ .../+imuSimulator/test1onestep.m | 48 ++++++ .../+imuSimulator/test2constglobal.m | 34 ++++ .../+imuSimulator/test3constbody.m | 96 ++++++++++++ .../+imuSimulator/test4circle.m | 97 ++++++++++++ 19 files changed, 1007 insertions(+) create mode 100644 matlab/unstable_examples/+imuSimulator/+lib/antisim.m create mode 100644 matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m create mode 100644 matlab/unstable_examples/+imuSimulator/+lib/getxyz.m create mode 100644 matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m create mode 100644 matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m create mode 100644 matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m create mode 100644 matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m create mode 100644 matlab/unstable_examples/+imuSimulator/IMUComparison.m create mode 100644 matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m create mode 100644 matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m create mode 100644 matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m create mode 100644 matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m create mode 100644 matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m create mode 100644 matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m create mode 100644 matlab/unstable_examples/+imuSimulator/integrateTrajectory.m create mode 100644 matlab/unstable_examples/+imuSimulator/test1onestep.m create mode 100644 matlab/unstable_examples/+imuSimulator/test2constglobal.m create mode 100644 matlab/unstable_examples/+imuSimulator/test3constbody.m create mode 100644 matlab/unstable_examples/+imuSimulator/test4circle.m diff --git a/matlab/unstable_examples/+imuSimulator/+lib/antisim.m b/matlab/unstable_examples/+imuSimulator/+lib/antisim.m new file mode 100644 index 000000000..6e9194567 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/antisim.m @@ -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]; + diff --git a/matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m b/matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m new file mode 100644 index 000000000..aef3a5f10 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m @@ -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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/matlab/unstable_examples/+imuSimulator/+lib/getxyz.m b/matlab/unstable_examples/+imuSimulator/+lib/getxyz.m new file mode 100644 index 000000000..d2c70db3a --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/getxyz.m @@ -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 \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m b/matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m new file mode 100644 index 000000000..1bababd33 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m @@ -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) \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m b/matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m new file mode 100644 index 000000000..9c73d1c69 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m @@ -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; diff --git a/matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m b/matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m new file mode 100644 index 000000000..4380d9d5d --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m @@ -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 + + + + + + + + + + + + + + diff --git a/matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m b/matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m new file mode 100644 index 000000000..6735ad6e5 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m @@ -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; + diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison.m b/matlab/unstable_examples/+imuSimulator/IMUComparison.m new file mode 100644 index 000000000..68e20bb25 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison.m @@ -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') + + diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m new file mode 100644 index 000000000..c589bea32 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m @@ -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') + + diff --git a/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m b/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m new file mode 100644 index 000000000..c86e40a21 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m @@ -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 + diff --git a/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m b/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m new file mode 100644 index 000000000..534b9365e --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m @@ -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 + diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m new file mode 100644 index 000000000..3f72f1215 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m @@ -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 + diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m new file mode 100644 index 000000000..50b223060 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m @@ -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 + diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m new file mode 100644 index 000000000..b919520ac --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m @@ -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 + diff --git a/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m b/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m new file mode 100644 index 000000000..e51b622b0 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m @@ -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 + diff --git a/matlab/unstable_examples/+imuSimulator/test1onestep.m b/matlab/unstable_examples/+imuSimulator/test1onestep.m new file mode 100644 index 000000000..883569849 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/test1onestep.m @@ -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); \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/test2constglobal.m b/matlab/unstable_examples/+imuSimulator/test2constglobal.m new file mode 100644 index 000000000..19956d08a --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/test2constglobal.m @@ -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,:), '.-'); diff --git a/matlab/unstable_examples/+imuSimulator/test3constbody.m b/matlab/unstable_examples/+imuSimulator/test3constbody.m new file mode 100644 index 000000000..b3ee2edfc --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/test3constbody.m @@ -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)) + diff --git a/matlab/unstable_examples/+imuSimulator/test4circle.m b/matlab/unstable_examples/+imuSimulator/test4circle.m new file mode 100644 index 000000000..22ee175dd --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/test4circle.m @@ -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') + +