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

additional info saved for monte carlo tests

release/4.3a0
djensen 2014-02-25 14:12:24 -05:00
parent c623dafecf
commit 98712b21c9
2 changed files with 33 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -19,14 +19,14 @@ if ~exist('externalCoriolisConfiguration', 'var')
clear all clear all
close all close all
%% General configuration %% General configuration
navFrameRotating = 0; % 0 = perform navigation in the fixed frame navFrameRotating = 1; % 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 incrementalPlotting = 1; % turn incremental plotting on and off. Turning plotting off increases
% speed for batch testing % speed for batch testing
estimationEnabled = 1; estimationEnabled = 1;
@ -70,7 +70,6 @@ if ~exist('externalCoriolisConfiguration', 'var')
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

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

@ -2,7 +2,7 @@ clc
clear all clear all
close all close all
% Flag to mark external configuration % Flag to mark external configuration to the main test script
externalCoriolisConfiguration = 1; externalCoriolisConfiguration = 1;
%% General configuration %% General configuration
@ -49,6 +49,7 @@ else
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
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Run tests with randomly generated initialPosition and accelFixed values % Run tests with randomly generated initialPosition and accelFixed values
@ -60,12 +61,13 @@ end
% - Navigation performed in rotating frame, ignoring coriolis effect % - Navigation performed in rotating frame, ignoring coriolis effect
% %
%% Testing configuration % Testing configuration
numPosTests = 10; numPosTests = 1;
numAccelTests = 20; numAccelTests = 10;
totalNumTests = numPosTests * numAccelTests; totalNumTests = numPosTests * numAccelTests;
% Storage variables % Storage variables
posFinErrorFixed = zeros(numPosTests, numAccelTests); posFinErrorFixed = zeros(numPosTests, numAccelTests);
rotFinErrorFixed = zeros(numPosTests, numAccelTests); rotFinErrorFixed = zeros(numPosTests, numAccelTests);
velFinErrorFixed = zeros(numPosTests, numAccelTests); velFinErrorFixed = zeros(numPosTests, numAccelTests);
@ -82,6 +84,7 @@ velFinErrorRot = zeros(numPosTests, numAccelTests);
numErrors = 0; numErrors = 0;
testIndPos = 1; testIndPos = 1;
testIndAccel = 1; testIndAccel = 1;
while testIndPos <= numPosTests while testIndPos <= numPosTests
%generate a random initial position vector %generate a random initial position vector
initialLongitude = rand()*360 - 180; % longitude in degrees (-90 to 90) initialLongitude = rand()*360 - 180; % longitude in degrees (-90 to 90)
@ -95,6 +98,16 @@ while testIndPos <= numPosTests
% generate a random acceleration vector % generate a random acceleration vector
accelFixed = 2*rand(3,1)-ones(3,1); accelFixed = 2*rand(3,1)-ones(3,1);
%lla = oldTestInfo(testIndPos,testIndAccel).initialPositionLLA;
%initialLongitude = lla(1);
%initialLatitude = lla(2);
%initialAltitude = lla(3);
%initialPosition = oldTestInfo(testIndPos, testIndAccel).initialPositionECEF;
testInfo(testIndPos, testIndAccel).initialPositionLLA = [initialLongitude, initialLatitude, initialAltitude];
testInfo(testIndPos, testIndAccel).initialPositionECEF = initialPosition;
testInfo(testIndPos, testIndAccel).accelFixed = accelFixed;
try try
omegaCoriolisIMU = [0;0;0]; omegaCoriolisIMU = [0;0;0];
navFrameRotating = 0; navFrameRotating = 0;
@ -102,6 +115,12 @@ while testIndPos <= numPosTests
posFinErrorFixed(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100; posFinErrorFixed(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100;
rotFinErrorFixed(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end)); rotFinErrorFixed(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end));
velFinErrorFixed(testIndPos, testIndAccel) = norm(axisVelocityError(:,end)); velFinErrorFixed(testIndPos, testIndAccel) = norm(axisVelocityError(:,end));
testInfo(testIndPos, testIndAccel).fixedPositionError = axisPositionsError(:,end);
testInfo(testIndPos, testIndAccel).fixedVelocityError = axisVelocityError(:,end);
testInfo(testIndPos, testIndAccel).fixedRotationError = rotationsErrorVectors(:,end);
testInfo(testIndPos, testIndAccel).fixedEstTrajLength = trajectoryLengthEstimated;
testInfo(testIndPos, testIndAccel).trajLengthFixedFrameGT = trajectoryLengthFixedFrameGT;
testInfo(testIndPos, testIndAccel).trajLengthRotFrameGT = trajectoryLengthRotatingFrameGT;
close all; close all;
@ -113,6 +132,10 @@ while testIndPos <= numPosTests
posFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100; posFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100;
rotFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end)); rotFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end));
velFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisVelocityError(:,end)); velFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisVelocityError(:,end));
testInfo(testIndPos, testIndAccel).rotCoriolisPositionError = axisPositionsError(:,end);
testInfo(testIndPos, testIndAccel).rotCoriolisVelocityError = axisVelocityError(:,end);
testInfo(testIndPos, testIndAccel).rotCoriolisRotationError = rotationsErrorVectors(:,end);
testInfo(testIndPos, testIndAccel).rotCoriolisEstTrajLength = trajectoryLengthEstimated;
close all; close all;
@ -124,6 +147,10 @@ while testIndPos <= numPosTests
posFinErrorRot(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100; posFinErrorRot(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100;
rotFinErrorRot(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end)); rotFinErrorRot(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end));
velFinErrorRot(testIndPos, testIndAccel) = norm(axisVelocityError(:,end)); velFinErrorRot(testIndPos, testIndAccel) = norm(axisVelocityError(:,end));
testInfo(testIndPos, testIndAccel).rotPositionError = axisPositionsError(:,end);
testInfo(testIndPos, testIndAccel).rotVelocityError = axisVelocityError(:,end);
testInfo(testIndPos, testIndAccel).rotRotationError = rotationsErrorVectors(:,end);
testInfo(testIndPos, testIndAccel).rotEstTrajLength = trajectoryLengthEstimated;
close all; close all;
catch catch