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

added option for external initial conditions

release/4.3a0
djensen3 2014-02-14 10:22:21 -05:00
parent a01fe12ee6
commit e65d075c20
1 changed files with 51 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -8,14 +8,16 @@
% Its position is plotted in both the fixed and rotating reference frames to simulate how an observer in each frame would % Its position is plotted in both the fixed and rotating reference frames to simulate how an observer in each frame would
% experience the body's motion. % experience the body's motion.
clc
clear all
close all
import gtsam.*; import gtsam.*;
addpath(genpath('./Libraries')) addpath(genpath('./Libraries'))
% Check for an external configuarion. This is useful for setting up
% multiple tests
if ~exist('externalCoriolisConfiguration', 'var')
clc
clear all
close all
%% General configuration %% General configuration
navFrameRotating = 1; % 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
@ -23,7 +25,7 @@ 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
%% Scenario Configuration %% Scenario Configuration
deltaT = 0.01; % timestep deltaT = 0.01; % timestep
@ -33,16 +35,14 @@ times = 0:deltaT:timeElapsed;
% Initial Conditions % Initial Conditions
omegaEarthSeconds = [0;0;7.292115e-5]; % Earth Rotation rate (rad/s) omegaEarthSeconds = [0;0;7.292115e-5]; % Earth Rotation rate (rad/s)
radiusEarth = 6378.1*1000; % radius of Earth is 6,378.1 km radiusEarth = 6378.1*1000; % radius of Earth is 6,378.1 km
angularVelocityTensorEarth = [ 0 -omegaEarthSeconds(3) omegaEarthSeconds(2);
omegaEarthSeconds(3) 0 -omegaEarthSeconds(1);
-omegaEarthSeconds(2) omegaEarthSeconds(1) 0 ];
if useRealisticValues == 1 if useRealisticValues == 1
omegaRotatingFrame = omegaEarthSeconds; % rotation of the moving frame wrt fixed frame omegaRotatingFrame = omegaEarthSeconds; % 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 = [0.5;-0.5;0]; % constant acceleration measurement accelFixed = [-0.5;0.5;2]; % constant acceleration measurement
g = [0;0;0]; % Gravity g = [0;0;0]; % Gravity
initialLongitude = 45; initialLongitude = 45; % longitude in degrees
initialLatitude = 30; 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); initialPosition = [radiusEarth*sind(initialLongitude);
@ -58,6 +58,7 @@ else
initialPosition = [0; 1; 0];% initial position in both frames 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) initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation)
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
@ -109,7 +110,7 @@ velocitiesEstimates = zeros(3,length(times));
rotationsErrorVectors = zeros(3,length(times)); % Rotating/Fixed frame selected later rotationsErrorVectors = zeros(3,length(times)); % Rotating/Fixed frame selected later
changePoseRotatingFrame = Pose3.Expmap([omegaRotatingFrame*deltaT; 0; 0; 0]); % rotation of the rotating frame at each time step changePoseRotatingFrame = Pose3.Expmap([omegaRotatingFrame*deltaT; 0; 0; 0]); % rotation of the rotating frame at each time step
h = figure(1); h = figure;
% Solver object % Solver object
isamParams = ISAM2Params; isamParams = ISAM2Params;
@ -146,7 +147,12 @@ fprintf('omegaCoriolisIMU = [%f %f %f]\n', omegaCoriolisIMU(1), omegaCoriolisIMU
fprintf('omegaFixed = [%f %f %f]\n', omegaFixed(1), omegaFixed(2), omegaFixed(3)); fprintf('omegaFixed = [%f %f %f]\n', omegaFixed(1), omegaFixed(2), omegaFixed(3));
fprintf('accelFixed = [%f %f %f]\n', accelFixed(1), accelFixed(2), accelFixed(3)); fprintf('accelFixed = [%f %f %f]\n', accelFixed(1), accelFixed(2), accelFixed(3));
fprintf('Initial Velocity = [%f %f %f]\n', initialVelocity(1), initialVelocity(2), initialVelocity(3)); fprintf('Initial Velocity = [%f %f %f]\n', initialVelocity(1), initialVelocity(2), initialVelocity(3));
if(exist('initialLatitude', 'var') && exist('initialLongitude', 'var'))
fprintf('Initial Position\n\t[Long, Lat] = [%f %f] degrees\n\tEFEC = [%f %f %f]\n', ...
initialLongitude, initialLatitude, initialPosition(1), initialPosition(2), initialPosition(3));
else
fprintf('Initial Position = [%f %f %f]\n', initialPosition(1), initialPosition(2), initialPosition(3)); fprintf('Initial Position = [%f %f %f]\n', initialPosition(1), initialPosition(2), initialPosition(3));
end
fprintf('\n'); fprintf('\n');
%% Main loop: iterate through the ground truth trajectory, add factors %% Main loop: iterate through the ground truth trajectory, add factors