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

minor

release/4.3a0
Luca 2014-02-17 10:41:42 -05:00
parent 8f39734580
commit 3a7a636f0f
2 changed files with 17 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -60,6 +60,12 @@ if ~exist('externalCoriolisConfiguration', 'var')
initialPosition = [0; 1; 0];% initial position in both frames initialPosition = [0; 1; 0];% initial position in both frames
initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation) initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation)
end end
if navFrameRotating == 0
omegaCoriolisIMU = [0;0;0];
else
omegaCoriolisIMU = omegaRotatingFrame;
end
end end
% From Wikipedia Angular Velocity page, dr/dt = W*r, where r is % From Wikipedia Angular Velocity page, dr/dt = W*r, where r is
@ -72,12 +78,6 @@ angularVelocityTensor = [ 0 -omegaRotatingFrame(3) omegaRot
initialVelocityFixedFrame = angularVelocityTensor * initialPosition + initialVelocity; initialVelocityFixedFrame = angularVelocityTensor * initialPosition + initialVelocity;
initialVelocityRotatingFrame = initialVelocity; initialVelocityRotatingFrame = initialVelocity;
if navFrameRotating == 0
omegaCoriolisIMU = [0;0;0];
else
omegaCoriolisIMU = omegaRotatingFrame;
end
% %
currentRotatingFrame = Pose3; % rotating frame initially coincides with fixed frame at t=0 currentRotatingFrame = Pose3; % rotating frame initially coincides with fixed frame at t=0
currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition)); currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition));

11
matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m
View File

@ -49,13 +49,24 @@ end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
numTests = 20 numTests = 20
for testInd=1:numTests for testInd=1:numTests
omegaCoriolisIMU = [0;0;0];
navFrameRotating = 0; navFrameRotating = 0;
accelFixed = 2*rand(3,1)-ones(3,1); accelFixed = 2*rand(3,1)-ones(3,1);
imuSimulator.coriolisExample imuSimulator.coriolisExample
posFinErrorFixed(testInd) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100 posFinErrorFixed(testInd) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100
rotFinErrorFixed(testInd) = norm(rotationsErrorVectors(:,end)) rotFinErrorFixed(testInd) = norm(rotationsErrorVectors(:,end))
velFinErrorFixed(testInd) = norm(axisVelocityError(:,end)) velFinErrorFixed(testInd) = norm(axisVelocityError(:,end))
% Run the same initial conditions but navigating in the rotating frame % Run the same initial conditions but navigating in the rotating frame
--> enable coriolis effect by setting:omegaCoriolisIMU = omegaRotatingFrame;
navFrameRotating = 1;
imuSimulator.coriolisExample
posFinErrorRot(testInd) = norm(axisPositionsError(:,end))/trajLen*100
rotFinErrorRot(testInd) = norm(rotationsErrorVectors(:,end))
velFinErrorRot(testInd) = norm(axisVelocityError(:,end))
% Run the same initial conditions but navigating in the rotating frame
--> disable coriolis effect by setting: omegaCoriolisIMU = [0;0;0];
navFrameRotating = 1; navFrameRotating = 1;
imuSimulator.coriolisExample imuSimulator.coriolisExample
posFinErrorRot(testInd) = norm(axisPositionsError(:,end))/trajLen*100 posFinErrorRot(testInd) = norm(axisPositionsError(:,end))/trajLen*100