zhengxuelong/matlab/temp/imu_plot.m

function imu_plot(data_name)
    load(data_name);
    clc
    acc(:,1) = m(1,(178:258));
    acc(:,2) = m(2,(178:258));
    acc(:,3) = m(3,(178:258));
    acc = acc * 9.81;
    figure('Position', [9 39 900 300], 'NumberTitle', 'off', 'Name', 'Velocity');
    hold on;
    plot(acc(:,1), 'r');
    plot(acc(:,2), 'g');
    plot(acc(:,3), 'b');
    title('ACC');
    xlabel('Time (s)');
    ylabel('ACC (m/s)');
    legend('X', 'Y', 'Z');
    hold off;

   % Integrate acceleration to yield velocity
    vel = zeros(size(acc));
    for t = 2:length(vel)-1
        vel(t,:) = vel(t-1,:) + acc(t,:) * 0.01;
    end


% Compute integral drift during non-stationary periods
velDrift = zeros(size(vel));
stationaryStart = 1;
stationaryEnd = length(vel);
for i = 1:numel(stationaryEnd)
    driftRate = vel(stationaryEnd(i)-1, :) / (stationaryEnd(i) - stationaryStart(i));
    enum = 1:(stationaryEnd(i) - stationaryStart(i));
    drift = [enum'*driftRate(1) enum'*driftRate(2) enum'*driftRate(3)];
    velDrift(stationaryStart(i):stationaryEnd(i)-1, :) = drift;
end

% Remove integral drift
vel = vel - velDrift;

% Plot translational velocity
figure('Position', [9 39 900 300], 'NumberTitle', 'off', 'Name', 'Velocity');
hold on;
plot(vel(:,1), 'r');
plot(vel(:,2), 'g');
plot(vel(:,3), 'b');
title('Velocity');
xlabel('Time (s)');
ylabel('Velocity (m/s)');
legend('X', 'Y', 'Z');
hold off;

% -------------------------------------------------------------------------
% Compute translational position

% Integrate velocity to yield position
pos = zeros(size(vel));
for t = 2:length(pos)
    pos(t,:) = pos(t-1,:) + vel(t,:) * 0.01;    % integrate velocity to yield position
end

% Plot translational position
figure('Position', [9 39 900 600], 'NumberTitle', 'off', 'Name', 'Position');
hold on;
plot(pos(:,1), 'r');
plot(pos(:,2), 'g');
plot(pos(:,3), 'b');
title('Position');
xlabel('Time (s)');
ylabel('Position (m)');
legend('X', 'Y', 'Z');
hold off;
  
% plot(vel(:,2), 'g');
% plot(vel(:,3), 'b');
%     plot(acc(:,1));
%     
%     plot(acc(:,2));
%     hold on; 
%     plot(acc(:,3));
%     hold on; 
%        plot3(pos(:,1),pos(:,2),pos(:,3))
%      plot(acc(:,1));
%      hold on; 
%      plot(vel(:,1));
%      hold on;
%      plot(pos(:,1), 'r');
%      hold on;
%      plot(pos(:,2), 'g');
%      hold on;
%      plot(pos(:,3), 'b');
%      hold on;
%      plot(acc(:,2));
%      hold on; 
%      plot(acc(:,3));
%      hold on; 
%      plot(vel);
%      hold on; 
end