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

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

@ -12,8 +12,6 @@ import gtsam.*;
addpath(genpath('./Libraries'))
estimationEnabled = 1
% Check for an external configuarion. This is useful for setting up
% multiple tests
if ~exist('externalCoriolisConfiguration', 'var')
@ -21,13 +19,16 @@ if ~exist('externalCoriolisConfiguration', 'var')
clear all
close all
%% 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
IMU_type = 1; % IMU type 1 or type 2
useRealisticValues = 1; % use reaslist values for initial position and earth rotation
record_movie = 0; % 0 = do not record movie
% 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
deltaT = 0.01; % timestep
@ -43,21 +44,21 @@ if ~exist('externalCoriolisConfiguration', 'var')
omegaFixed = [0;0;0]; % constant rotation rate measurement
accelFixed = [-0.5;0.5;2]; % constant acceleration measurement
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
% surface (approximating Earth as a sphere)
initialPosition = [radiusEarth*sind(initialLongitude);
radiusEarth*cosd(initialLongitude);
radiusEarth*sind(initialLatitude)];
initialLongitude = 45; % longitude in degrees
initialLatitude = 30; % latitude in degrees
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,
% (ignoring the velocity due to the earth's rotation)
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
accelFixed = [1;0;0]; % constant acceleration measurement
accelFixed = [0.5;0.5;0.5]; % constant acceleration measurement
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)
end
@ -68,6 +69,8 @@ if ~exist('externalCoriolisConfiguration', 'var')
end
end
% From Wikipedia Angular Velocity page, dr/dt = W*r, where r is
% position vector and W is angular velocity tensor
% We add the initial velocity in the rotating frame because they
@ -139,6 +142,7 @@ if navFrameRotating == 0
else
fprintf('Performing navigation in the Rotating frame.\n');
end
fprintf('Estimation Enabled = %d\n', estimationEnabled);
fprintf('IMU_type = %d\n', IMU_type);
fprintf('Record Movie = %d\n', record_movie);
fprintf('Realistic Values = %d\n', useRealisticValues);
@ -223,6 +227,8 @@ for i = 1:length(times)
%currentVelocityRotatingGT = currentRot3RotatingGT.unrotate(Point3(currentVelocityFixedGT));
% TODO: check if initial velocity in rotating frame is correct
currentVelocityRotatingGT = (currentPositionRotatingGT-previousPositionRotatingGT)/deltaT;
%currentVelocityRotatingGT = (currentPositionRotatingGT - previousPositionRotatingGT ...
% - 0.5 * accelFixed * deltaT * deltaT) / deltaT + accelFixed * deltaT;
% Store GT (ground truth) poses
positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector;
@ -315,6 +321,7 @@ for i = 1:length(times)
end
%% incremental plotting for animation (ground truth)
if incrementalPlotting == 1
figure(h)
%plot_trajectory(currentRotatingFrameForPlot(i),1, '-k', 'Rotating Frame',0.1,0.75,1)
%hold on;
@ -341,10 +348,12 @@ for i = 1:length(times)
grid on;
%pause(0.1);
% record frames for movie
if record_movie == 1
frame = getframe(gcf);
writeVideo(writerObj,frame);
end
end
end
if record_movie == 1
@ -455,7 +464,11 @@ fprintf('Final rotation error = %f [rads]\n', norm(rotationsErrorVectors(:,end))
%% Plot final trajectories
figure
[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;
axis equal
% Ground truth trajectory in fixed reference frame

119
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
% 1 = record movie of the trajectories. One
% frame per time step (15 fps)
incrementalPlotting = 0;
estimationEnabled = 1;
%% Scenario Configuration
deltaT = 0.01; % timestep
@ -28,15 +30,16 @@ if useRealisticValues == 1
omegaFixed = [0;0;0]; % constant rotation rate measurement
accelFixed = [0.1;0;1]; % constant acceleration measurement
g = [0;0;0]; % Gravity
initialLongitude = -45; % longitude in degrees
initialLongitude = 45; % longitude in degrees
initialLatitude = 30; % latitude in degrees
% initial position at some [longitude, latitude] location on earth's
% surface (approximating Earth as a sphere)
initialPosition = [radiusEarth*sind(initialLongitude);
radiusEarth*cosd(initialLongitude);
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,
% (ignoring the velocity due to the earth's rotation)
else
omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame
omegaFixed = [0;0;0]; % constant rotation rate measurement
@ -47,36 +50,106 @@ else
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
numTests = 20
for testInd=1:numTests
% Run tests with randomly generated initialPosition and accelFixed values
% For each initial position, a number of acceleration vectors are
% generated. For each (initialPosition, accelFixed) pair, the coriolis test
% is run for the following 3 scenarios
% - Navigation performed in fixed frame
% - Navigation performed in rotating frame, including the coriolis effect
% - Navigation performed in rotating frame, ignoring coriolis effect
%
%% Testing configuration
numPosTests = 10;
numAccelTests = 20;
totalNumTests = numPosTests * numAccelTests;
% Storage variables
posFinErrorFixed = zeros(numPosTests, numAccelTests);
rotFinErrorFixed = zeros(numPosTests, numAccelTests);
velFinErrorFixed = zeros(numPosTests, numAccelTests);
posFinErrorRotCoriolis = zeros(numPosTests, numAccelTests);
rotFinErrorRotCoriolis = zeros(numPosTests, numAccelTests);
velFinErrorRotCoriolis = zeros(numPosTests, numAccelTests);
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;
accelFixed = 2*rand(3,1)-ones(3,1);
imuSimulator.coriolisExample
posFinErrorFixed(testInd) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100
rotFinErrorFixed(testInd) = norm(rotationsErrorVectors(:,end))
velFinErrorFixed(testInd) = norm(axisVelocityError(:,end))
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;
% 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))
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];
% disable coriolis effect by setting:
omegaCoriolisIMU = [0;0;0];
navFrameRotating = 1;
imuSimulator.coriolisExample
posFinErrorRot(testInd) = norm(axisPositionsError(:,end))/trajLen*100
rotFinErrorRot(testInd) = norm(rotationsErrorVectors(:,end))
velFinErrorRot(testInd) = norm(axisVelocityError(:,end))
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
fprintf('\nTotal: %d tests cancelled due to errors\n', numErrors);
mean(posFinErrorFixed)
max(posFinErrorFixed)
mean(posFinErrorFixed);
max(posFinErrorFixed);
% box(posFinErrorFixed);
box(posFinErrorFixed)
mean(posFinErrorRotCoriolis);
max(posFinErrorRotCoriolis);
% box(posFinErrorRotCoriolis);
mean(posFinErrorRot);
max(posFinErrorRot);
% box(posFinErrorRot);