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

added random initial positions to monte carlo tests

release/4.3a0
djensen 2014-02-19 16:57:15 -05:00
parent 3a7a636f0f
commit dfcd2cb3ba
2 changed files with 162 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -12,8 +12,6 @@ import gtsam.*;
addpath(genpath('./Libraries')) addpath(genpath('./Libraries'))
estimationEnabled = 1
% Check for an external configuarion. This is useful for setting up % Check for an external configuarion. This is useful for setting up
% multiple tests % multiple tests
if ~exist('externalCoriolisConfiguration', 'var') if ~exist('externalCoriolisConfiguration', 'var')
@ -21,13 +19,16 @@ if ~exist('externalCoriolisConfiguration', 'var')
clear all clear all
close all close all
%% General configuration %% General configuration
navFrameRotating = 1; % 0 = perform navigation in the fixed frame navFrameRotating = 0; % 0 = perform navigation in the fixed frame
% 1 = perform navigation in the rotating frame % 1 = perform navigation in the rotating frame
IMU_type = 1; % IMU type 1 or type 2 IMU_type = 1; % IMU type 1 or type 2
useRealisticValues = 1; % use reaslist values for initial position and earth rotation useRealisticValues = 1; % use reaslist values for initial position and earth rotation
record_movie = 0; % 0 = do not record movie record_movie = 0; % 0 = do not record movie
% 1 = record movie of the trajectories. One % 1 = record movie of the trajectories. One
% frame per time step (15 fps % frame per time step (15 fps)
incrementalPlotting = 0; % turn incremental plotting on and off. Turning plotting off increases
% speed for batch testing
estimationEnabled = 1;
%% Scenario Configuration %% Scenario Configuration
deltaT = 0.01; % timestep deltaT = 0.01; % timestep
@ -43,21 +44,21 @@ if ~exist('externalCoriolisConfiguration', 'var')
omegaFixed = [0;0;0]; % constant rotation rate measurement omegaFixed = [0;0;0]; % constant rotation rate measurement
accelFixed = [-0.5;0.5;2]; % constant acceleration measurement accelFixed = [-0.5;0.5;2]; % constant acceleration measurement
g = [0;0;0]; % Gravity g = [0;0;0]; % Gravity
initialLongitude = 45; % longitude in degrees
initialLatitude = 30; % latitude in degrees
% initial position at some [longitude, latitude] location on earth's % initial position at some [longitude, latitude] location on earth's
% surface (approximating Earth as a sphere) % surface (approximating Earth as a sphere)
initialPosition = [radiusEarth*sind(initialLongitude); initialLongitude = 45; % longitude in degrees
radiusEarth*cosd(initialLongitude); initialLatitude = 30; % latitude in degrees
radiusEarth*sind(initialLatitude)]; initialAltitude = 0; % Altitude above Earth's surface in meters
[x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude);
initialPosition = [x; y; z];
initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame, initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame,
% (ignoring the velocity due to the earth's rotation) % (ignoring the velocity due to the earth's rotation)
else else
omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame omegaRotatingFrame = [0;0;pi/5]; % rotation of the moving frame wrt fixed frame
omegaFixed = [0;0;0]; % constant rotation rate measurement omegaFixed = [0;0;0]; % constant rotation rate measurement
accelFixed = [1;0;0]; % constant acceleration measurement accelFixed = [0.5;0.5;0.5]; % constant acceleration measurement
g = [0;0;0]; % Gravity g = [0;0;0]; % Gravity
initialPosition = [0; 1; 0];% initial position in both frames initialPosition = [1; 0; 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
@ -68,6 +69,8 @@ if ~exist('externalCoriolisConfiguration', 'var')
end 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
% position vector and W is angular velocity tensor % position vector and W is angular velocity tensor
% We add the initial velocity in the rotating frame because they % We add the initial velocity in the rotating frame because they
@ -139,6 +142,7 @@ if navFrameRotating == 0
else else
fprintf('Performing navigation in the Rotating frame.\n'); fprintf('Performing navigation in the Rotating frame.\n');
end end
fprintf('Estimation Enabled = %d\n', estimationEnabled);
fprintf('IMU_type = %d\n', IMU_type); fprintf('IMU_type = %d\n', IMU_type);
fprintf('Record Movie = %d\n', record_movie); fprintf('Record Movie = %d\n', record_movie);
fprintf('Realistic Values = %d\n', useRealisticValues); fprintf('Realistic Values = %d\n', useRealisticValues);
@ -223,6 +227,8 @@ for i = 1:length(times)
%currentVelocityRotatingGT = currentRot3RotatingGT.unrotate(Point3(currentVelocityFixedGT)); %currentVelocityRotatingGT = currentRot3RotatingGT.unrotate(Point3(currentVelocityFixedGT));
% TODO: check if initial velocity in rotating frame is correct % TODO: check if initial velocity in rotating frame is correct
currentVelocityRotatingGT = (currentPositionRotatingGT-previousPositionRotatingGT)/deltaT; currentVelocityRotatingGT = (currentPositionRotatingGT-previousPositionRotatingGT)/deltaT;
%currentVelocityRotatingGT = (currentPositionRotatingGT - previousPositionRotatingGT ...
% - 0.5 * accelFixed * deltaT * deltaT) / deltaT + accelFixed * deltaT;
% Store GT (ground truth) poses % Store GT (ground truth) poses
positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector; positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector;
@ -315,35 +321,38 @@ for i = 1:length(times)
end end
%% incremental plotting for animation (ground truth) %% incremental plotting for animation (ground truth)
figure(h) if incrementalPlotting == 1
%plot_trajectory(currentRotatingFrameForPlot(i),1, '-k', 'Rotating Frame',0.1,0.75,1) figure(h)
%hold on; %plot_trajectory(currentRotatingFrameForPlot(i),1, '-k', 'Rotating Frame',0.1,0.75,1)
plot3(positionsInFixedGT(1,1:i), positionsInFixedGT(2,1:i), positionsInFixedGT(3,1:i),'r'); %hold on;
hold on; plot3(positionsInFixedGT(1,1:i), positionsInFixedGT(2,1:i), positionsInFixedGT(3,1:i),'r');
plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr'); hold on;
plot3(positionsInFixedGT(1,i), positionsInFixedGT(2,i), positionsInFixedGT(3,i), 'or'); plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr');
plot3(positionsInFixedGT(1,i), positionsInFixedGT(2,i), positionsInFixedGT(3,i), 'or');
plot3(positionsInRotatingGT(1,1:i), positionsInRotatingGT(2,1:i), positionsInRotatingGT(3,1:i), 'g');
plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg'); plot3(positionsInRotatingGT(1,1:i), positionsInRotatingGT(2,1:i), positionsInRotatingGT(3,1:i), 'g');
plot3(positionsInRotatingGT(1,i), positionsInRotatingGT(2,i), positionsInRotatingGT(3,i), 'og'); plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg');
plot3(positionsInRotatingGT(1,i), positionsInRotatingGT(2,i), positionsInRotatingGT(3,i), 'og');
if(estimationEnabled)
plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), 'b'); if(estimationEnabled)
plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb'); plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), 'b');
plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob'); plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb');
end plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob');
end
hold off;
xlabel('X axis') hold off;
ylabel('Y axis') xlabel('X axis')
zlabel('Z axis') ylabel('Y axis')
axis equal zlabel('Z axis')
grid on; axis equal
%pause(0.1); grid on;
%pause(0.1);
if record_movie == 1
frame = getframe(gcf); % record frames for movie
writeVideo(writerObj,frame); if record_movie == 1
frame = getframe(gcf);
writeVideo(writerObj,frame);
end
end end
end end
@ -455,7 +464,11 @@ fprintf('Final rotation error = %f [rads]\n', norm(rotationsErrorVectors(:,end))
%% Plot final trajectories %% Plot final trajectories
figure figure
[x,y,z] = sphere(30); [x,y,z] = sphere(30);
mesh(radiusEarth*x,radiusEarth*y, radiusEarth*z) % where (a,b,c) is center of the sphere % sphere for reference if useRealisticValues
mesh(radiusEarth*x,radiusEarth*y, radiusEarth*z) % where (a,b,c) is center of the sphere % sphere for reference
else
mesh(x,y,z);
end
hold on; hold on;
axis equal axis equal
% Ground truth trajectory in fixed reference frame % Ground truth trajectory in fixed reference frame

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

@ -13,6 +13,8 @@ useRealisticValues = 1; % use reaslist values for initial position and earth
record_movie = 0; % 0 = do not record movie record_movie = 0; % 0 = do not record movie
% 1 = record movie of the trajectories. One % 1 = record movie of the trajectories. One
% frame per time step (15 fps) % frame per time step (15 fps)
incrementalPlotting = 0;
estimationEnabled = 1;
%% Scenario Configuration %% Scenario Configuration
deltaT = 0.01; % timestep deltaT = 0.01; % timestep
@ -28,15 +30,16 @@ if useRealisticValues == 1
omegaFixed = [0;0;0]; % constant rotation rate measurement omegaFixed = [0;0;0]; % constant rotation rate measurement
accelFixed = [0.1;0;1]; % constant acceleration measurement accelFixed = [0.1;0;1]; % constant acceleration measurement
g = [0;0;0]; % Gravity g = [0;0;0]; % Gravity
initialLongitude = -45; % longitude in degrees
initialLongitude = 45; % longitude in degrees
initialLatitude = 30; % latitude in degrees initialLatitude = 30; % latitude in degrees
% initial position at some [longitude, latitude] location on earth's initialAltitude = 0; % Altitude above Earth's surface in meters
% surface (approximating Earth as a sphere) [x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude);
initialPosition = [radiusEarth*sind(initialLongitude); initialPosition = [x; y; z];
radiusEarth*cosd(initialLongitude);
radiusEarth*sind(initialLatitude)];
initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame, initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame,
% (ignoring the velocity due to the earth's rotation) % (ignoring the velocity due to the earth's rotation)
else else
omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame
omegaFixed = [0;0;0]; % constant rotation rate measurement omegaFixed = [0;0;0]; % constant rotation rate measurement
@ -47,36 +50,106 @@ else
end end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
numTests = 20
for testInd=1:numTests % Run tests with randomly generated initialPosition and accelFixed values
omegaCoriolisIMU = [0;0;0]; % For each initial position, a number of acceleration vectors are
navFrameRotating = 0; % generated. For each (initialPosition, accelFixed) pair, the coriolis test
accelFixed = 2*rand(3,1)-ones(3,1); % is run for the following 3 scenarios
imuSimulator.coriolisExample % - Navigation performed in fixed frame
posFinErrorFixed(testInd) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100 % - Navigation performed in rotating frame, including the coriolis effect
rotFinErrorFixed(testInd) = norm(rotationsErrorVectors(:,end)) % - Navigation performed in rotating frame, ignoring coriolis effect
velFinErrorFixed(testInd) = norm(axisVelocityError(:,end)) %
% Run the same initial conditions but navigating in the rotating frame %% Testing configuration
--> enable coriolis effect by setting:omegaCoriolisIMU = omegaRotatingFrame; numPosTests = 10;
navFrameRotating = 1; numAccelTests = 20;
imuSimulator.coriolisExample totalNumTests = numPosTests * numAccelTests;
posFinErrorRot(testInd) = norm(axisPositionsError(:,end))/trajLen*100
rotFinErrorRot(testInd) = norm(rotationsErrorVectors(:,end)) % Storage variables
velFinErrorRot(testInd) = norm(axisVelocityError(:,end)) posFinErrorFixed = zeros(numPosTests, numAccelTests);
rotFinErrorFixed = zeros(numPosTests, numAccelTests);
% Run the same initial conditions but navigating in the rotating frame velFinErrorFixed = zeros(numPosTests, numAccelTests);
--> disable coriolis effect by setting: omegaCoriolisIMU = [0;0;0];
navFrameRotating = 1; posFinErrorRotCoriolis = zeros(numPosTests, numAccelTests);
imuSimulator.coriolisExample rotFinErrorRotCoriolis = zeros(numPosTests, numAccelTests);
posFinErrorRot(testInd) = norm(axisPositionsError(:,end))/trajLen*100 velFinErrorRotCoriolis = zeros(numPosTests, numAccelTests);
rotFinErrorRot(testInd) = norm(rotationsErrorVectors(:,end))
velFinErrorRot(testInd) = norm(axisVelocityError(:,end)) posFinErrorRot = zeros(numPosTests, numAccelTests);
rotFinErrorRot = zeros(numPosTests, numAccelTests);
velFinErrorRot = zeros(numPosTests, numAccelTests);
numErrors = 0;
testIndPos = 1;
testIndAccel = 1;
while testIndPos <= numPosTests
%generate a random initial position vector
initialLongitude = rand()*360 - 180; % longitude in degrees (-90 to 90)
initialLatitude = rand()*180 - 90; % latitude in degrees (-180 to 180)
initialAltitude = rand()*150; % Altitude above Earth's surface in meters (0-150)
[x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude);
initialPosition = [x; y; z];
while testIndAccel <= numAccelTests
[testIndPos testIndAccel]
% generate a random acceleration vector
accelFixed = 2*rand(3,1)-ones(3,1);
try
omegaCoriolisIMU = [0;0;0];
navFrameRotating = 0;
imuSimulator.coriolisExample
posFinErrorFixed(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100;
rotFinErrorFixed(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end));
velFinErrorFixed(testIndPos, testIndAccel) = norm(axisVelocityError(:,end));
close all;
% Run the same initial conditions but navigating in the rotating frame
% enable coriolis effect by setting:
omegaCoriolisIMU = omegaRotatingFrame;
navFrameRotating = 1;
imuSimulator.coriolisExample
posFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100;
rotFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end));
velFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisVelocityError(:,end));
close all;
% Run the same initial conditions but navigating in the rotating frame
% disable coriolis effect by setting:
omegaCoriolisIMU = [0;0;0];
navFrameRotating = 1;
imuSimulator.coriolisExample
posFinErrorRot(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100;
rotFinErrorRot(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end));
velFinErrorRot(testIndPos, testIndAccel) = norm(axisVelocityError(:,end));
close all;
catch
numErrors = numErrors + 1;
fprintf('*ERROR*');
fprintf('%d tests cancelled due to errors\n', numErrors);
fprintf('restarting test with new accelFixed');
testIndAccel = testIndAccel - 1;
end
testIndAccel = testIndAccel + 1;
end
testIndAccel = 1;
testIndPos = testIndPos + 1;
end end
fprintf('\nTotal: %d tests cancelled due to errors\n', numErrors);
mean(posFinErrorFixed) mean(posFinErrorFixed);
max(posFinErrorFixed) max(posFinErrorFixed);
% box(posFinErrorFixed);
box(posFinErrorFixed) mean(posFinErrorRotCoriolis);
max(posFinErrorRotCoriolis);
% box(posFinErrorRotCoriolis);
mean(posFinErrorRot);
max(posFinErrorRot);
% box(posFinErrorRot);