diff --git a/matlab/unstable_examples/+imuSimulator/testCoriolis.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m similarity index 70% rename from matlab/unstable_examples/+imuSimulator/testCoriolis.m rename to matlab/unstable_examples/+imuSimulator/coriolisExample.m index d79589ca5..9599c2508 100644 --- a/matlab/unstable_examples/+imuSimulator/testCoriolis.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -1,27 +1,38 @@ +%% coriolisExample.m +% Author(s): David Jensen (david.jensen@gtri.gatech.edu) +% This script demonstrates the relationship between object motion in inertial and rotating reference frames. +% For this example, we consider a fixed (inertial) reference frame located at the center of the earth and initially +% coincident with the rotating ECEF reference frame (X towards 0 longitude, Y towards 90 longitude, Z towards 90 latitude), +% which rotates with the earth. +% A body is moving in the positive Z-direction and positive Y-direction with respect to the fixed reference frame. +% 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. + clc clear all close all import gtsam.*; +%% General configuration deltaT = 0.1; timeElapsed = 10; times = 0:deltaT:timeElapsed; %omega = [0;0;7.292115e-5]; % Earth Rotation omega = [0;0;5*pi/10]; -velocity = [0;0;0]; % initially not moving -accelFixed = [0;0.1;0.1]; % accelerate in the positive z-direction +velocity = [0;0;0]; % initially not moving +accelFixed = [0;0.1;0.1]; % accelerate in the positive z-direction initialPosition = [0; 1.05; 0]; % start along the positive x-axis -% Initial state +%% Initial state of the body in the fixed in rotating frames should be the same currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition)); currentVelocityFixedGT = velocity; currentPoseRotatingGT = currentPoseFixedGT; currentPoseRotatingFrame = Pose3; -% Positions +%% Initialize storage variables positionsFixedGT = zeros(3, length(times)+1); positionsRotatingGT = zeros(3, length(times)+1); @@ -35,10 +46,13 @@ poses(1).R = currentPoseRotatingFrame.rotation.matrix; h = figure(1); +%% Main loop: iterate through i = 2; for t = times %% Create ground truth trajectory - % Update the pose and velocity + % Update the position and velocity + % x_t = x_0 + v_0*dt + 1/2*a_0*dt^2 + % v_t = v_0 + a_0*dt currentPositionFixedGT = Point3(currentPoseFixedGT.translation.vector ... + currentVelocityFixedGT * deltaT + 0.5 * accelFixed * deltaT * deltaT); currentVelocityFixedGT = currentVelocityFixedGT + accelFixed * deltaT; @@ -55,7 +69,7 @@ for t = times poses(i).p = currentPoseRotatingFrame.translation.vector; poses(i).R = currentPoseRotatingFrame.rotation.matrix; - % incremental graphing + % incremental plotting for animation figure(h) plot_trajectory(poses(i),1, '-k', 'Rotating Frame',0.1,0.75,1) hold on; @@ -76,10 +90,11 @@ for t = times i = i + 1; end - +%% Final plotting figure -sphere +sphere % sphere for reference hold on; +% trajectories in Fixed and Rotating frames plot3(positionsFixedGT(1,:), positionsFixedGT(2,:), positionsFixedGT(3,:)); plot3(positionsRotatingGT(1,:), positionsRotatingGT(2,:), positionsRotatingGT(3,:), '-r');