diff --git a/Gait Tracking With x-IMU/AHRS_Octave/AHRS_Octave.m b/Gait Tracking With x-IMU/AHRS_Octave/AHRS_Octave.m new file mode 100644 index 0000000..76a4f70 --- /dev/null +++ b/Gait Tracking With x-IMU/AHRS_Octave/AHRS_Octave.m @@ -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 \ No newline at end of file diff --git a/Gait Tracking With x-IMU/AHRS_Octave/UpdateIMU.m b/Gait Tracking With x-IMU/AHRS_Octave/UpdateIMU.m new file mode 100644 index 0000000..f14d4a6 --- /dev/null +++ b/Gait Tracking With x-IMU/AHRS_Octave/UpdateIMU.m @@ -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 \ No newline at end of file diff --git a/Gait Tracking With x-IMU/Script.m b/Gait Tracking With x-IMU/Script.m index 73b00e1..13b7cf5 100644 --- a/Gait Tracking With x-IMU/Script.m +++ b/Gait Tracking With x-IMU/Script.m @@ -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; @@ -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 @@ -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 % ------------------------------------------------------------------------- diff --git a/Gait Tracking With x-IMU/SixDofAnimation.m b/Gait Tracking With x-IMU/SixDofAnimation.m index f8fe864..cb0078c 100644 --- a/Gait Tracking With x-IMU/SixDofAnimation.m +++ b/Gait Tracking With x-IMU/SixDofAnimation.m @@ -1,4 +1,4 @@ -function fig = SixDOFanimation(varargin) +function fig = SixDoFanimation(varargin) %% Create local variables @@ -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; @@ -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)