98 lines
3.8 KiB
Matlab
98 lines
3.8 KiB
Matlab
|
clc
|
||
|
|
clear all
|
||
|
|
close all
|
||
|
|
|
||
|
|
import gtsam.*;
|
||
|
|
|
||
|
|
deltaT = 0.001;
|
||
|
|
timeElapsed = 1;
|
||
|
|
times = 0:deltaT:timeElapsed;
|
||
|
|
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
disp('--------------------------------------------------------');
|
||
|
|
disp('Integration in body frame VS integration in navigation frame');
|
||
|
|
disp('TOY EXAMPLE:');
|
||
|
|
disp('- Body moves along a circular trajectory with constant rotation rate -omega- and constant -velocity- (in the body frame)');
|
||
|
|
disp('- We compare two integration schemes: integating in the navigation frame (similar to Lupton approach) VS integrating in the body frame')
|
||
|
|
disp('- Navigation frame is assumed inertial for simplicity')
|
||
|
|
omega = [0;0;2*pi];
|
||
|
|
velocity = [1;0;0];
|
||
|
|
|
||
|
|
%% Set initial conditions for the true trajectory and for the estimates
|
||
|
|
% (one estimate is obtained by integrating in the body frame, the other
|
||
|
|
% by integrating in the navigation frame)
|
||
|
|
% Initial state (body)
|
||
|
|
currentPoseGlobal = Pose3;
|
||
|
|
currentVelocityGlobal = velocity;
|
||
|
|
% Initial state estimate (integrating in navigation frame)
|
||
|
|
currentPoseGlobalIMUnav = currentPoseGlobal;
|
||
|
|
currentVelocityGlobalIMUnav = currentVelocityGlobal;
|
||
|
|
% Initial state estimate (integrating in the body frame)
|
||
|
|
currentPoseGlobalIMUbody = currentPoseGlobal;
|
||
|
|
currentVelocityGlobalIMUbody = currentVelocityGlobal;
|
||
|
|
|
||
|
|
%% Prepare data structures for actual trajectory and estimates
|
||
|
|
% Actual trajectory
|
||
|
|
positions = zeros(3, length(times)+1);
|
||
|
|
positions(:,1) = currentPoseGlobal.translation.vector;
|
||
|
|
poses(1).p = positions(:,1);
|
||
|
|
poses(1).R = currentPoseGlobal.rotation.matrix;
|
||
|
|
|
||
|
|
% Trajectory estimate (integrated in the navigation frame)
|
||
|
|
positionsIMUnav = zeros(3, length(times)+1);
|
||
|
|
positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector;
|
||
|
|
posesIMUnav(1).p = positionsIMUnav(:,1);
|
||
|
|
posesIMUnav(1).R = poses(1).R;
|
||
|
|
|
||
|
|
% Trajectory estimate (integrated in the body frame)
|
||
|
|
positionsIMUbody = zeros(3, length(times)+1);
|
||
|
|
positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector;
|
||
|
|
posesIMUbody(1).p = positionsIMUbody(:,1);
|
||
|
|
posesIMUbody(1).R = poses(1).R;
|
||
|
|
|
||
|
|
%% Main loop
|
||
|
|
i = 2;
|
||
|
|
for t = times
|
||
|
|
%% Create the actual trajectory, using the velocities and
|
||
|
|
% accelerations in the inertial frame to compute the positions
|
||
|
|
[ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
|
||
|
|
currentPoseGlobal, omega, velocity, velocity, deltaT);
|
||
|
|
|
||
|
|
%% Simulate IMU measurements, considering Coriolis effect
|
||
|
|
% (in this simple example we neglect gravity and there are no other forces acting on the body)
|
||
|
|
acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
|
||
|
|
omega, omega, velocity, velocity, deltaT);
|
||
|
|
|
||
|
|
%% Integrate in the body frame
|
||
|
|
[ currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody ] = imuSimulator.integrateIMUTrajectory_bodyFrame( ...
|
||
|
|
currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody, acc_omega, deltaT, velocity);
|
||
|
|
|
||
|
|
%% Integrate in the navigation frame
|
||
|
|
[ currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav ] = imuSimulator.integrateIMUTrajectory_navFrame( ...
|
||
|
|
currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT);
|
||
|
|
|
||
|
|
%% Store data in some structure for statistics and plots
|
||
|
|
positions(:,i) = currentPoseGlobal.translation.vector;
|
||
|
|
positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector;
|
||
|
|
positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector;
|
||
|
|
% -
|
||
|
|
poses(i).p = positions(:,i);
|
||
|
|
posesIMUbody(i).p = positionsIMUbody(:,i);
|
||
|
|
posesIMUnav(i).p = positionsIMUnav(:,i);
|
||
|
|
% -
|
||
|
|
poses(i).R = currentPoseGlobal.rotation.matrix;
|
||
|
|
posesIMUbody(i).R = currentPoseGlobalIMUbody.rotation.matrix;
|
||
|
|
posesIMUnav(i).R = currentPoseGlobalIMUnav.rotation.matrix;
|
||
|
|
i = i + 1;
|
||
|
|
end
|
||
|
|
|
||
|
|
figure(1)
|
||
|
|
hold on;
|
||
|
|
plot(positions(1,:), positions(2,:), '-b');
|
||
|
|
plot(positionsIMUbody(1,:), positionsIMUbody(2,:), '-r');
|
||
|
|
plot(positionsIMUnav(1,:), positionsIMUnav(2,:), ':k');
|
||
|
|
axis equal;
|
||
|
|
legend('true trajectory', 'traj integrated in body', 'traj integrated in nav')
|
||
|
|
|
||
|
|
|