Skip to content

Modified the code to work with Octave 3.6.4 in Windows #2

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
tjrantal wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Modified the code to work with Octave 3.6.4 in Windows #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 28 additions & 0 deletions Gait Tracking With x-IMU/AHRS_Octave/AHRS_Octave.m
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
function obj = AHRS_Octave(varargin)
%Assign constant values
obj = struct();
obj.SamplePeriod = 1/256;
obj.Quaternion = [1 0 0 0]; % output quaternion describing the sensor relative to the Earth
obj.q = [1 0 0 0];
obj.Kp = 2; % proportional gain
obj.Ki = 0; % integral gain
obj.KpInit = 200; % proportional gain used during initialisation
obj.initPeriod = 5; % initialisation period in seconds
obj.IntError = [0 0 0]'; % integral error
obj.KpRamped = 200; % internal proportional gain used to ramp during initialisation


for i = 1:2:nargin
if strcmp(varargin{i}, 'SamplePeriod'), obj.SamplePeriod = varargin{i+1};
elseif strcmp(varargin{i}, 'Quaternion')
obj.Quaternion = varargin{i+1};
obj.q = quaternConj(obj.Quaternion);
elseif strcmp(varargin{i}, 'Kp'), obj.Kp = varargin{i+1};
elseif strcmp(varargin{i}, 'Ki'), obj.Ki = varargin{i+1};
elseif strcmp(varargin{i}, 'KpInit'), obj.KpInit = varargin{i+1};
elseif strcmp(varargin{i}, 'InitPeriod'), obj.InitPeriod = varargin{i+1};
else error('Invalid argument');
end
obj.KpRamped = obj.KpInit;
end;
end
36 changes: 36 additions & 0 deletions Gait Tracking With x-IMU/AHRS_Octave/UpdateIMU.m
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
function obj = UpdateIMU(obj, Gyroscope, Accelerometer)

% Normalise accelerometer measurement
if(norm(Accelerometer) == 0) % handle NaN
warning(0, 'Accelerometer magnitude is zero. Algorithm update aborted.');
return;
else
Accelerometer = Accelerometer / norm(Accelerometer); % normalise measurement
end

% Compute error between estimated and measured direction of gravity
v = [2*(obj.q(2)*obj.q(4) - obj.q(1)*obj.q(3))
2*(obj.q(1)*obj.q(2) + obj.q(3)*obj.q(4))
obj.q(1)^2 - obj.q(2)^2 - obj.q(3)^2 + obj.q(4)^2]; % estimated direction of gravity
error = cross(v, Accelerometer');

% % Compute ramped Kp value used during init period
% if(obj.KpRamped > obj.Kp)
% obj.IntError = [0 0 0]';
% obj.KpRamped = obj.KpRamped - (obj.KpInit - obj.Kp) / (obj.InitPeriod / obj.SamplePeriod);
% else % init period complete
% obj.KpRamped = obj.Kp;
obj.IntError = obj.IntError + error; % compute integral feedback terms (only outside of init period)
% end

% Apply feedback terms
Ref = Gyroscope - (obj.Kp*error + obj.Ki*obj.IntError)';

% Compute rate of change of quaternion
pDot = 0.5 * quaternProd(obj.q, [0 Ref(1) Ref(2) Ref(3)]); % compute rate of change of quaternion
obj.q = obj.q + pDot * obj.SamplePeriod; % integrate rate of change of quaternion
obj.q = obj.q / norm(obj.q); % normalise quaternion

% Store conjugate
obj.Quaternion = quaternConj(obj.q);
end
98 changes: 71 additions & 27 deletions Gait Tracking With x-IMU/Script.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,21 @@
clc;
addpath('Quaternions');
addpath('ximu_matlab_library');
%OCTAVE 3.6.4 couldn't handle some of the Matlab features used in the script (e.g. class declarations -> ximu_matlab_library didn't work)
%Modified the code to enable running it with Octave on Windows platform
%OCTAVE
if exist ('OCTAVE_VERSION', 'builtin')
addpath('AHRS_Octave');
end

% -------------------------------------------------------------------------
% Select dataset (comment in/out)

filePath = 'Datasets/straightLine';
%OCTAVE
if exist ('OCTAVE_VERSION', 'builtin')
filePath = 'Datasets/straightLine_CalInertialAndMag.csv';
else
filePath = 'Datasets/straightLine';
end
startTime = 6;
stopTime = 26;

Expand All @@ -23,16 +33,26 @@
% Import data

samplePeriod = 1/256;
xIMUdata = xIMUdataClass(filePath, 'InertialMagneticSampleRate', 1/samplePeriod);
time = xIMUdata.CalInertialAndMagneticData.Time;
gyrX = xIMUdata.CalInertialAndMagneticData.Gyroscope.X;
gyrY = xIMUdata.CalInertialAndMagneticData.Gyroscope.Y;
gyrZ = xIMUdata.CalInertialAndMagneticData.Gyroscope.Z;
accX = xIMUdata.CalInertialAndMagneticData.Accelerometer.X;
accY = xIMUdata.CalInertialAndMagneticData.Accelerometer.Y;
accZ = xIMUdata.CalInertialAndMagneticData.Accelerometer.Z;
if exist ('OCTAVE_VERSION', 'builtin')
xIMUdata = dlmread(filePath,',',1,0);
time = xIMUdata(:,1)*samplePeriod;
gyrX = xIMUdata(:,2);
gyrY = xIMUdata(:,3);
gyrZ = xIMUdata(:,4);
accX = xIMUdata(:,5);
accY = xIMUdata(:,6);
accZ = xIMUdata(:,7);
else
xIMUdata = xIMUdataClass(filePath, 'InertialMagneticSampleRate', 1/samplePeriod);
time = xIMUdata.CalInertialAndMagneticData.Time;
gyrX = xIMUdata.CalInertialAndMagneticData.Gyroscope.X;
gyrY = xIMUdata.CalInertialAndMagneticData.Gyroscope.Y;
gyrZ = xIMUdata.CalInertialAndMagneticData.Gyroscope.Z;
accX = xIMUdata.CalInertialAndMagneticData.Accelerometer.X;
accY = xIMUdata.CalInertialAndMagneticData.Accelerometer.Y;
accZ = xIMUdata.CalInertialAndMagneticData.Accelerometer.Z;
end
clear('xIMUdata');

% -------------------------------------------------------------------------
% Manually frame data

Expand Down Expand Up @@ -90,36 +110,60 @@
plot(time, accY, 'g');
plot(time, accZ, 'b');
plot(time, acc_magFilt, ':k');
plot(time, stationary, 'k', 'LineWidth', 2);
plot(time, double(stationary), 'k', 'LineWidth', 2); %Octave couldn't plot booleans
title('Accelerometer');
xlabel('Time (s)');
ylabel('Acceleration (g)');
legend('X', 'Y', 'Z', 'Filtered', 'Stationary');
hold off;
linkaxes(ax,'x');

if ~exist ('OCTAVE_VERSION', 'builtin')
linkaxes(ax,'x'); %Octave 3.6.4 had not implemented linkaxes
end

% -------------------------------------------------------------------------
% Compute orientation

quat = zeros(length(time), 4);
AHRSalgorithm = AHRS('SamplePeriod', 1/256, 'Kp', 1, 'KpInit', 1);

% Initial convergence
initPeriod = 2;
indexSel = 1 : find(sign(time-(time(1)+initPeriod))+1, 1);
for i = 1:2000
AHRSalgorithm.UpdateIMU([0 0 0], [mean(accX(indexSel)) mean(accY(indexSel)) mean(accZ(indexSel))]);
end

% For all data
for t = 1:length(time)
if(stationary(t))
AHRSalgorithm.Kp = 0.5;
else
AHRSalgorithm.Kp = 0;
end
AHRSalgorithm.UpdateIMU(deg2rad([gyrX(t) gyrY(t) gyrZ(t)]), [accX(t) accY(t) accZ(t)]);
quat(t,:) = AHRSalgorithm.Quaternion;
if ~exist ('OCTAVE_VERSION', 'builtin')
AHRSalgorithm = AHRS('SamplePeriod', 1/256, 'Kp', 1, 'KpInit', 1);
for i = 1:2000
AHRSalgorithm.UpdateIMU([0 0 0], [mean(accX(indexSel)) mean(accY(indexSel)) mean(accZ(indexSel))]);
end

% For all data
for t = 1:length(time)
if(stationary(t))
AHRSalgorithm.Kp = 0.5;
else
AHRSalgorithm.Kp = 0;
end
AHRSalgorithm.UpdateIMU(deg2rad([gyrX(t) gyrY(t) gyrZ(t)]), [accX(t) accY(t) accZ(t)]);
quat(t,:) = AHRSalgorithm.Quaternion;
end
else %classdef wasn't implemented in Octave 3.6.4
AHRSStruct = AHRS_Octave('SamplePeriod', 1/256, 'Kp', 1, 'KpInit', 1);
% Initial convergence
initPeriod = 2;
indexSel = 1 : find(sign(time-(time(1)+initPeriod))+1, 1);
for i = 1:2000
AHRSStruct = UpdateIMU(AHRSStruct,[0 0 0], [mean(accX(indexSel)) mean(accY(indexSel)) mean(accZ(indexSel))]);
end

% For all data
for t = 1:length(time)
if(stationary(t))
AHRSStruct.Kp = 0.5;
else
AHRSStruct.Kp = 0;
end
AHRSStruct = UpdateIMU(AHRSStruct,deg2rad([gyrX(t) gyrY(t) gyrZ(t)]), [accX(t) accY(t) accZ(t)]);
quat(t,:) = AHRSStruct.Quaternion;
end
end

% -------------------------------------------------------------------------
Expand Down
12 changes: 7 additions & 5 deletions Gait Tracking With x-IMU/SixDofAnimation.m
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
function fig = SixDOFanimation(varargin)
function fig = SixDoFanimation(varargin)

%% Create local variables

Expand Down Expand Up @@ -95,7 +95,9 @@
set(fig, 'Position', Position);
end
set(gca, 'drawmode', 'fast');
lighting phong;
if ~exist ('OCTAVE_VERSION', 'builtin') %Octave 3.6.4 does not implement lighting
lighting phong;
end
set(gcf, 'Renderer', 'zbuffer');
hold on;
axis equal;
Expand Down Expand Up @@ -149,9 +151,9 @@
else
ShowArrowHeadStr = 'off';
end
quivXhandle = quiver3(ox, oy, oz, ux, vx, wx, 'r', 'ShowArrowHead', ShowArrowHeadStr, 'MaxHeadSize', 0.999999, 'AutoScale', 'off');
quivYhandle = quiver3(ox, oy, oz, uy, vy, wy, 'g', 'ShowArrowHead', ShowArrowHeadStr, 'MaxHeadSize', 0.999999, 'AutoScale', 'off');
quivZhandle = quiver3(ox, ox, oz, uz, vz, wz, 'b', 'ShowArrowHead', ShowArrowHeadStr, 'MaxHeadSize', 0.999999, 'AutoScale', 'off');
quivXhandle = quiver3(ox, oy, oz, ux, vx, wx, 'color',[1 0 0], 'ShowArrowHead', ShowArrowHeadStr, 'MaxHeadSize', 0.999999, 'AutoScale', 'off');
quivYhandle = quiver3(ox, oy, oz, uy, vy, wy, 'color',[0 1 0], 'ShowArrowHead', ShowArrowHeadStr, 'MaxHeadSize', 0.999999, 'AutoScale', 'off');
quivZhandle = quiver3(ox, ox, oz, uz, vz, wz, 'color',[0 0 1], 'ShowArrowHead', ShowArrowHeadStr, 'MaxHeadSize', 0.999999, 'AutoScale', 'off');

% Create legend
if(ShowLegend)
Expand Down