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

added monte carlo runs

release/4.3a0
Luca 2014-02-14 17:41:58 -05:00
parent e65d075c20
commit 4ef8cec118
2 changed files with 106 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -12,6 +12,8 @@ import gtsam.*;
addpath(genpath('./Libraries'))
estimationEnabled = 1
% Check for an external configuarion. This is useful for setting up
% multiple tests
if ~exist('externalCoriolisConfiguration', 'var')
@ -53,7 +55,7 @@ if ~exist('externalCoriolisConfiguration', 'var')
else
omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame
omegaFixed = [0;0;0]; % constant rotation rate measurement
accelFixed = [0.1;0;0]; % constant acceleration measurement
accelFixed = [1;0;0]; % constant acceleration measurement
g = [0;0;0]; % Gravity
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)
@ -286,7 +288,7 @@ for i = 1:length(times)
%newFactors.print(''); newValues.print('');
%% Solve factor graph
if(i > 1)
if(i > 1 && estimationEnabled)
isam.update(newFactors, newValues);
newFactors = NonlinearFactorGraph;
newValues = Values;
@ -325,9 +327,11 @@ for i = 1:length(times)
plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg');
plot3(positionsInRotatingGT(1,i), positionsInRotatingGT(2,i), positionsInRotatingGT(3,i), 'og');
plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), 'b');
plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb');
plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob');
if(estimationEnabled)
plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), 'b');
plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb');
plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob');
end
hold off;
xlabel('X axis')
@ -387,6 +391,21 @@ title(plotTitle);
xlabel('Time [s]');
ylabel('3D error [meters]');
% Plot 3D position error
if navFrameRotating == 0
trajLen = trajectoryLengthFixedFrameGT;
else
trajLen = trajectoryLengthRotatingFrameGT;
end
figure
plot(times, (positionError3D/trajLen)*100);
plotTitle = sprintf('3D Error normalized by distance in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ...
IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3));
title(plotTitle);
xlabel('Time [s]');
ylabel('normalized 3D error [% of distance traveled]');
% Plot individual axis velocity errors
figure
plot(times, axisVelocityError);
@ -435,22 +454,26 @@ fprintf('Final rotation error = %f [rads]\n', norm(rotationsErrorVectors(:,end))
%% Plot final trajectories
figure
sphere % sphere for reference
[x,y,z] = sphere(30);
mesh(radiusEarth*x,radiusEarth*y, radiusEarth*z) % where (a,b,c) is center of the sphere % sphere for reference
hold on;
axis equal
% Ground truth trajectory in fixed reference frame
plot3(positionsInFixedGT(1,:), positionsInFixedGT(2,:), positionsInFixedGT(3,:),'r');
plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr');
plot3(positionsInFixedGT(1,end), positionsInFixedGT(2,end), positionsInFixedGT(3,end), 'or');
plot3(positionsInFixedGT(1,:), positionsInFixedGT(2,:), positionsInFixedGT(3,:),'r','lineWidth',4);
plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr','lineWidth',4);
plot3(positionsInFixedGT(1,end), positionsInFixedGT(2,end), positionsInFixedGT(3,end), 'or','lineWidth',4);
% Ground truth trajectory in rotating reference frame
plot3(positionsInRotatingGT(1,:), positionsInRotatingGT(2,:), positionsInRotatingGT(3,:), '-g');
plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg');
plot3(positionsInRotatingGT(1,end), positionsInRotatingGT(2,end), positionsInRotatingGT(3,end), 'og');
plot3(positionsInRotatingGT(1,:), positionsInRotatingGT(2,:), positionsInRotatingGT(3,:), '-g','lineWidth',4);
plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg','lineWidth',4);
plot3(positionsInRotatingGT(1,end), positionsInRotatingGT(2,end), positionsInRotatingGT(3,end), 'og','lineWidth',4);
% Estimates
plot3(positionsEstimates(1,:), positionsEstimates(2,:), positionsEstimates(3,:), '-b');
plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb');
plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'ob');
if(estimationEnabled)
plot3(positionsEstimates(1,:), positionsEstimates(2,:), positionsEstimates(3,:), '-b','lineWidth',4);
plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb','lineWidth',4);
plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'ob','lineWidth',4);
end
xlabel('X axis')
ylabel('Y axis')
@ -471,5 +494,3 @@ hold off;
% TODO: logging velocity errors
%velocities..

68
matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m Normal file
View File

@ -0,0 +1,68 @@
clc
clear all
close all
% Flag to mark external configuration
externalCoriolisConfiguration = 1;
%% General configuration
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)
%% Scenario Configuration
deltaT = 0.01; % timestep
timeElapsed = 5; % Total elapsed time
times = 0:deltaT:timeElapsed;
% Initial Conditions
omegaEarthSeconds = [0;0;7.292115e-5]; % Earth Rotation rate (rad/s)
radiusEarth = 6378.1*1000; % radius of Earth is 6,378.1 km
if useRealisticValues == 1
omegaRotatingFrame = omegaEarthSeconds; % rotation of the moving frame wrt fixed frame
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
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)];
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
accelFixed = [1;0;0]; % constant acceleration measurement
g = [0;0;0]; % Gravity
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)
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
numTests = 10
% for testInd=1:numTests
% accelFixed = 2*rand(3,1)-ones(3,1);
% imuSimulator.coriolisExample
% posFinError(testInd) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100
% rotFinError(testInd) = norm(rotationsErrorVectors(:,end))
% velFinError(testInd) = norm(axisVelocityError(:,end))
% end
% Run the same initial conditions but navigating in the rotating frame
navFrameRotating = 1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for testInd=1:numTests
accelFixed = 2*rand(3,1)-ones(3,1);
imuSimulator.coriolisExample
posFinErrorRot(testInd) = norm(axisPositionsError(:,end))/trajLen*100
rotFinErrorRot(testInd) = norm(rotationsErrorVectors(:,end))
velFinErrorRot(testInd) = norm(axisVelocityError(:,end))
end