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