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

Fixed initial velocity

release/4.3a0
djensen3 2014-02-13 14:34:38 -05:00
parent 0fabfc39c2
commit ed1bcb2761
1 changed files with 55 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -20,7 +20,7 @@ addpath(genpath('./Libraries'))
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
IMU_type = 1; % IMU type 1 or type 2 IMU_type = 1; % IMU type 1 or type 2
useRealisticValues = 0; % 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)
@ -32,22 +32,43 @@ 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
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;0]; % constant acceleration measurement
g = [0;0;0]; % Gravity g = [0;0;0]; % Gravity
initialVelocity = radiusEarth * omegaEarthSeconds [vector.. is a cross product, wee wiki link] ; %[0;0;0]; % TODO % initial velocity initialLongitude = 45;
initialPosition = [4509997.76107; 4509997.76107; 3189050]; % initial position (Earth radius 6371 km) initialLatitude = 30;
% 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 else
omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame omegaRotatingFrame = [0;0;pi/300]; % 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.1;0;0]; % constant acceleration measurement accelFixed = [0.1;0;0]; % constant acceleration measurement
g = [0;0;0]; % Gravity g = [0;0;0]; % Gravity
initialVelocity = [0;0;0]; % initial velocity initialPosition = [0; 1; 0];% initial position in both frames
initialPosition = [0; 1; 0]; % initial position, at 45 degrees longitude and 30 degrees latitude on earth surface initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation)
end 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
% are the same frame at t=0, so no transformation is needed
angularVelocityTensor = [ 0 -omegaRotatingFrame(3) omegaRotatingFrame(2);
omegaRotatingFrame(3) 0 -omegaRotatingFrame(1);
-omegaRotatingFrame(2) omegaRotatingFrame(1) 0 ];
initialVelocityFixedFrame = angularVelocityTensor * initialPosition + initialVelocity;
initialVelocityRotatingFrame = initialVelocity;
if navFrameRotating == 0 if navFrameRotating == 0
omegaCoriolisIMU = [0;0;0]; omegaCoriolisIMU = [0;0;0];
else else
@ -58,7 +79,8 @@ end
currentRotatingFrame = Pose3; % rotating frame initially coincides with fixed frame at t=0 currentRotatingFrame = Pose3; % rotating frame initially coincides with fixed frame at t=0
currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition)); currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition));
currentPoseRotatingGT = currentPoseFixedGT; % frames coincide for t=0 currentPoseRotatingGT = currentPoseFixedGT; % frames coincide for t=0
currentVelocityFixedGT = initialVelocity; currentVelocityFixedGT = initialVelocityFixedFrame;
currentVelocityRotatingGT = initialVelocityRotatingFrame;
% %
epsBias = 1e-20; epsBias = 1e-20;
sigma_init_x = noiseModel.Isotropic.Sigma(6, 1e-10); sigma_init_x = noiseModel.Isotropic.Sigma(6, 1e-10);
@ -119,11 +141,13 @@ fprintf('Realistic Values = %d\n', useRealisticValues);
fprintf('deltaT = %f\n', deltaT); fprintf('deltaT = %f\n', deltaT);
fprintf('timeElapsed = %f\n', timeElapsed); fprintf('timeElapsed = %f\n', timeElapsed);
fprintf('omegaRotatingFrame = [%f %f %f]\n', omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); fprintf('omegaRotatingFrame = [%f %f %f]\n', omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3));
fprintf('omegaCoriolisIMU = [%f %f %f]\n', omegaCoriolisIMU(1), omegaCoriolisIMU(2), omegaCoriolisIMU(3));
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));
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));
fprintf('\n'); fprintf('\n');
%% Main loop: iterate through the ground truth trajectory, add factors %% Main loop: iterate through the ground truth trajectory, add factors
% and values to the factor graph, and perform inference % and values to the factor graph, and perform inference
for i = 1:length(times) for i = 1:length(times)
@ -136,8 +160,13 @@ for i = 1:length(times)
%% Set priors on the first iteration %% Set priors on the first iteration
if(i == 1) if(i == 1)
currentPoseEstimate = currentPoseFixedGT; % known initial conditions % known initial conditions
currentVelocityEstimate = LieVector(currentVelocityFixedGT); % known initial conditions currentPoseEstimate = currentPoseFixedGT;
if navFrameRotating == 1
currentVelocityEstimate = LieVector(currentVelocityRotatingGT);
else
currentVelocityEstimate = LieVector(currentVelocityFixedGT);
end
% Set Priors % Set Priors
newValues.insert(currentPoseKey, currentPoseEstimate); newValues.insert(currentPoseKey, currentPoseEstimate);
@ -264,16 +293,16 @@ for i = 1:length(times)
%hold on; %hold on;
plot3(positionsInFixedGT(1,1:i), positionsInFixedGT(2,1:i), positionsInFixedGT(3,1:i),'r'); plot3(positionsInFixedGT(1,1:i), positionsInFixedGT(2,1:i), positionsInFixedGT(3,1:i),'r');
hold on; hold on;
%plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'x'); plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr');
%plot3(positionsInFixedGT(1,i), positionsInFixedGT(2,i), positionsInFixedGT(3,i), 'o'); plot3(positionsInFixedGT(1,i), positionsInFixedGT(2,i), positionsInFixedGT(3,i), 'or');
plot3(positionsInRotatingGT(1,1:i), positionsInRotatingGT(2,1:i), positionsInRotatingGT(3,1:i), '-g'); plot3(positionsInRotatingGT(1,1:i), positionsInRotatingGT(2,1:i), positionsInRotatingGT(3,1:i), 'g');
%plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg'); plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg');
%plot3(positionsInRotatingGT(1,i), positionsInRotatingGT(2,i), positionsInRotatingGT(3,i), 'og'); 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:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), 'b');
%plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb'); plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb');
%plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob'); plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob');
hold off; hold off;
xlabel('X axis') xlabel('X axis')
@ -378,16 +407,16 @@ grid on;
hold off; hold off;
% TODO: logging rotation errors % TODO: logging rotation errors
for all time steps %for all time steps
Rerror = Rgt' * Restimated; % Rerror = Rgt' * Restimated;
% transforming rotation matrix to axis-angle representation % % transforming rotation matrix to axis-angle representation
vector_error = Rot3.Logmap(Rerror); % vector_error = Rot3.Logmap(Rerror);
norm(vector_error) % norm(vector_error)
%
axis angle: [u,theta], with norm(u)=1 % axis angle: [u,theta], with norm(u)=1
vector_error = u * theta; % vector_error = u * theta;
% TODO: logging velocity errors % TODO: logging velocity errors
velocities.. %velocities..