diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index 7e3719143..35d27aa73 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -19,14 +19,14 @@ if ~exist('externalCoriolisConfiguration', 'var') clear all close all %% 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 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) - 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 estimationEnabled = 1; @@ -70,7 +70,6 @@ if ~exist('externalCoriolisConfiguration', 'var') 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 diff --git a/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m index 0a26bd760..4d02fd03e 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m @@ -2,7 +2,7 @@ clc clear all close all -% Flag to mark external configuration +% Flag to mark external configuration to the main test script externalCoriolisConfiguration = 1; %% 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) end +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Run tests with randomly generated initialPosition and accelFixed values @@ -60,12 +61,13 @@ end % - Navigation performed in rotating frame, ignoring coriolis effect % -%% Testing configuration -numPosTests = 10; -numAccelTests = 20; +% Testing configuration +numPosTests = 1; +numAccelTests = 10; totalNumTests = numPosTests * numAccelTests; % Storage variables + posFinErrorFixed = zeros(numPosTests, numAccelTests); rotFinErrorFixed = zeros(numPosTests, numAccelTests); velFinErrorFixed = zeros(numPosTests, numAccelTests); @@ -82,6 +84,7 @@ 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) @@ -95,6 +98,16 @@ while testIndPos <= numPosTests % generate a random acceleration vector 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 omegaCoriolisIMU = [0;0;0]; navFrameRotating = 0; @@ -102,6 +115,12 @@ while testIndPos <= numPosTests posFinErrorFixed(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100; rotFinErrorFixed(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,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; @@ -113,6 +132,10 @@ while testIndPos <= numPosTests posFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100; rotFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,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; @@ -124,6 +147,10 @@ while testIndPos <= numPosTests posFinErrorRot(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100; rotFinErrorRot(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,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; catch