Merge pull request #14 from tmcg0/fix-opalimudata

remove dependency on OpalIMUData

matlab/examples/imuPoseEstimationExample.m

@@ -5,10 +5,11 @@
 % -------------------------------------------------------------------- %
 
 % example usage of the MATLAB implementation of the imuPoseEstimator class
+
 clc; clear; close all;
 
-addpath('/home/tmcgrath/bioslam/matlab/src');
-addpath(genpath('/home/tmcgrath/bioslam/matlab/utils'));
+addpath(fullfile(fileparts(matlab.desktop.editor.getActiveFilename),'..','..','matlab','src')) % add src/ directory
+addpath(fullfile(fileparts(matlab.desktop.editor.getActiveFilename),'..','..','matlab','utils')) % add utils/ directory
 
 VarStrToCharMap.clear();
 
@@ -17,18 +18,18 @@
 testDataDir=fullfile(filepath,'..','..','test','data');
 
 % construct a data file
-imus=OpalIMUData(fullfile(testDataDir,'20170412-162455-Y2_TUG_5.h5'));
-% imus=OpalIMUData(fullfile(dropboxpath,'Research','treadmill_study','data','imu','20190923-124627-S330MinWalk.h5'));
-% imus=cutOpalIMU(imus,400,451);
-% rightThighImu=imus(strcmp('Right Thigh',{imus.label}));
-% rightShankImu=imus(strcmp('Right Tibia',{imus.label}));
+imus=ImuData(fullfile(testDataDir,'20170411-154746-Y1_TUG_6.h5'));
+% imus=ImuData(fullfile(dropboxpath,'Research','treadmill_study','data','imu','20190923-124627-S330MinWalk.h5'));
 myImu=imus(strcmp('Sternum',{imus.label}));
 figure('units','normalized','position',[0.1300 0.5500 0.250 0.250]);
-quatplot(myImu.qAPDM); title('APDM Quaternion'); drawnow;
+% plot quaternion from manufacturer's onboard filter:
+figure;
+lh=plot(repmat(myImu.time,[1 4]),myImu.qAPDM);
+set(lh,{'color'},{[0 0 0]; [1 0 0]; [0 1 0]; [0 0 1]});
+legend('q_s','q_x','q_y','q_z');
+grid on; xlabel('time (sec)'); ylabel('quaternion component');
 
 % construct imuposeestimator
-doOnlinePlot=1;
-
 ipe=imuPoseEstimator(myImu,1,'test');
 % --- add settings ---
 ipe.accG=[0 0 -9.81];
@@ -44,7 +45,7 @@
 ipe.m_magnetometerNoise=[1e3 1e3 1e3];
 % --------------------
 ipe.setup();
-% ipe.robustOptimize(doOnlinePlot);
+% ipe.robustOptimize(1);
 ipe.fastOptimize();
 % ipe.defaultOptimize();
 % ipe.plotImuFactorErrors();

matlab/examples/lowerBodyPoseEstimationExample.m

@@ -20,16 +20,14 @@
 testDataDir=fullfile(filepath,'..','..','test','data');
 
 % construct a data file
-imus=OpalIMUData(fullfile(testDataDir,'20170411-154746-Y1_TUG_6.h5'));
+imus=ImuData(fullfile(testDataDir,'20170411-154746-Y1_TUG_6.h5'));
 rightThighImu=imus(strcmp('Right Thigh',{imus.label}));
 rightShankImu=imus(strcmp('Right Tibia',{imus.label}));
 sacrumImu=imus(strcmp('Sacrum',{imus.label}));
 rightFootImu=imus(strcmp('Right Foot',{imus.label}));
 leftThighImu=imus(strcmp('Left Thigh',{imus.label}));
 leftShankImu=imus(strcmp('Left Tibia',{imus.label}));
 leftFootImu=imus(strcmp('Left Foot',{imus.label}));
-% figure('units','normalized','position',[0.1300 0.5500 0.250 0.250]);
-% quatplot(myImu.qAPDM); drawnow;
 
 % construct imuposeestimators
 sacrumIpe=imuPoseEstimator(sacrumImu,1,'sacrum');

matlab/src/imuPoseEstimator.m

@@ -62,6 +62,7 @@
         accG=[0 0 -9.81];
         magG=[30.5, 14.91, -56.3];
         % opt params
+        m_optType=1
         m_maxIterations=5000
         m_relErrDecreaseLimit=1e-5;
         m_absErrDecreaseLimit=1e-8;
@@ -84,10 +85,10 @@
             %IMUPOSEESTIMATOR Construct an instance of this class
             %   imuPoseEstimator(imu,id)
             if nargin==0
-                % if no arguments were passed in, simply run the associated unit test for this class.
-                run('/home/tmcgrath/bioslam/matlab/test/imuPoseEstimatorUnitTest.m')
+                % constructs empty copy of class
+                return
             elseif nargin==3 % normal usage
-                if isa(varargin{1},'OpalIMUData') && isa(varargin{2},'numeric') && isa(varargin{3},'char')
+                if isa(varargin{1},'ImuData') && isa(varargin{2},'numeric') && isa(varargin{3},'char')
                     obj.m_imu=varargin{1}; obj.m_imuBiasModelMode=varargin{2}; obj.id = varargin{3};
                     %m_magnetometerNoise=gtsam::noiseModel::Isotropic::Sigmas(magNoiseVec);
                     % now, from m_strId, set the variable names and add them to the VarStrToCharMap
@@ -149,6 +150,9 @@ function setAnyRequestedPriors(obj)
             end
         end
         function setupCombinedImuFactorDynamicBias(obj)
+            warning('bug: using dynamic bias estimation is currently broken functionality, using static bias for now. See issue on GitHub at https://github.com/tmcg0/bioslam/issues/4');
+            setupImuFactorStaticBias(obj);
+            return
             %setupCombinedImuFactorDynamicBias Setup factor graph with dynamic bias (gtsam's CombinedImuFactor)
             %   Detailed explanation goes here
             dt=obj.m_imu.time(2)-obj.m_imu.time(1);

matlab/test/imuPoseEstimatorUnitTest.m

@@ -7,20 +7,25 @@
 % unit test the MATLAB implementation of the imuPoseEstimator class
 clc; clear; close all;
 
-addpath('/home/tmcgrath/bioslam/matlab/src');
-addpath(genpath('/home/tmcgrath/bioslam/matlab/utils'));
+addpath(fullfile(fileparts(matlab.desktop.editor.getActiveFilename),'..','..','matlab','src')) % add src/ directory
+addpath(fullfile(fileparts(matlab.desktop.editor.getActiveFilename),'..','..','matlab','utils')) % add utils/ directory
 
 VarStrToCharMap.clear();
 
-testDataDir=fullfile(strcat(filesep,'home'),'tmcgrath','bioslam','test','data');
+% get test data directory
+[filepath,name,ext] = fileparts(matlab.desktop.editor.getActiveFilename);
+testDataDir=fullfile(filepath,'..','..','test','data');
 
 % construct a data file
-imus=OpalIMUData(fullfile(testDataDir,'20170411-154746-Y1_TUG_6.h5'));
+imus=ImuData(fullfile(testDataDir,'20170411-154746-Y1_TUG_6.h5'));
 myImu=imus(strcmp('Right Thigh',{imus.label}));
 
-% 
+% plot quaternion from manufacturer's onboard filter:
 figure('units','normalized','position',[0.1300 0.5500 0.250 0.250]);
-quatplot(myImu.qAPDM); drawnow;
+lh=plot(repmat(myImu.time,[1 4]),myImu.qAPDM);
+set(lh,{'color'},{[0 0 0]; [1 0 0]; [0 1 0]; [0 0 1]});
+legend('q_s','q_x','q_y','q_z');
+grid on; xlabel('time (sec)'); ylabel('quaternion component');
 
 % construct imuposeestimator
 doOnlinePlot=1;

matlab/test/testIpeInitialConditionBehavior.m

@@ -7,16 +7,23 @@
 % unit test the MATLAB implementation of the imuPoseEstimator class
 clc; clear; close all;
 
-addpath('/home/tmcgrath/bioslam/matlab/src');
-addpath('/home/tmcgrath/bioslam/matlab/utils');
-testDataDir=fullfile(strcat(filesep,'home'),'tmcgrath','bioslam','test','data');
+addpath(fullfile(fileparts(matlab.desktop.editor.getActiveFilename),'..','..','matlab','src')) % add src/ directory
+addpath(fullfile(fileparts(matlab.desktop.editor.getActiveFilename),'..','..','matlab','utils')) % add utils/ directory
+
+% get test data directory
+[filepath,name,ext] = fileparts(matlab.desktop.editor.getActiveFilename);
+testDataDir=fullfile(filepath,'..','..','test','data');
 
 % construct a data file
-imus=OpalIMUData(fullfile(testDataDir,'20170411-154746-Y1_TUG_6.h5'));
+imus=ImuData(fullfile(testDataDir,'20170411-154746-Y1_TUG_6.h5'));
 myImu=imus(strcmp('Right Thigh',{imus.label}));
 
+% plot quaternion from manufacturer's onboard filter:
 figure('units','normalized','position',[0.1300 0.5500 0.250 0.250]);
-quatplot(myImu.qAPDM); drawnow;
+lh=plot(repmat(myImu.time,[1 4]),myImu.qAPDM);
+set(lh,{'color'},{[0 0 0]; [1 0 0]; [0 1 0]; [0 0 1]});
+legend('q_s','q_x','q_y','q_z');
+grid on; xlabel('time (sec)'); ylabel('quaternion component');
 
 % first clear varstrtocharmap
 VarStrToCharMap.clear();
@@ -28,14 +35,14 @@
 
 
 % construct imuposeestimator
-ipe=imuPoseEstimator(myImu,'test');
+ipe=imuPoseEstimator(myImu,1,'test');
 
 ipe.setupImuFactorStaticBias();
 
 % () now that it's setup, you can inspect the error of the imufactor at the initial condition
 getFirstImuFactor(ipe.m_graph,ipe.m_initialValues);
 
-ipe.robustOptimize(0);
+ipe.fastOptimize();
 % ipe.plotEstimatedValues();
 % ipe.plotAccelDebug();
 

matlab/test/testSimpleEkf.m

@@ -7,24 +7,27 @@
 % unit test the MATLAB implementation the simple EKF
 clc; clear; close all;
 
-addpath('/home/tmcgrath/bioslam/matlab/src');
-addpath(genpath('/home/tmcgrath/bioslam/matlab/utils'));
+addpath(fullfile(fileparts(matlab.desktop.editor.getActiveFilename),'..','..','matlab','src')) % add src/ directory
+addpath(fullfile(fileparts(matlab.desktop.editor.getActiveFilename),'..','..','matlab','utils')) % add utils/ directory
 
-testDataDir=fullfile(strcat(filesep,'home'),'tmcgrath','bioslam','test','data');
+% get test data directory
+[filepath,name,ext] = fileparts(matlab.desktop.editor.getActiveFilename);
+testDataDir=fullfile(filepath,'..','..','test','data');
 
 % construct a data file
-imus=OpalIMUData(fullfile(testDataDir,'20170411-154746-Y1_TUG_6.h5'));
+imus=ImuData(fullfile(testDataDir,'20170411-154746-Y1_TUG_6.h5'));
 myImu=imus(strcmp('Right Thigh',{imus.label}));
 
-% 
+% plot quaternion from manufacturer's onboard filter:
 figure('units','normalized','position',[0.1300 0.5500 0.250 0.250]);
-quatplot(myImu.qAPDM); drawnow;
+lh=plot(repmat(myImu.time,[1 4]),myImu.qAPDM);
+set(lh,{'color'},{[0 0 0]; [1 0 0]; [0 1 0]; [0 0 1]});
+legend('q_s','q_x','q_y','q_z');
+grid on; xlabel('time (sec)'); ylabel('quaternion component');
+title('manufacturer filter');
 
 % now run the EKF
 imu=myImu;
-% [x,P]=simpleImuOrientationForwardEkf([imu.gx imu.gy imu.gz],[imu.ax imu.ay imu.az],1/imu.sampleRate,[],[]);
-% figure; quatplot(x);
-
 [x,P]=simpleForwardBackwardEkf([imu.gx imu.gy imu.gz],[imu.ax imu.ay imu.az],1/imu.sampleRate,4);
 
 % test: is x similar to APDM's x?
@@ -33,7 +36,14 @@
 r1e=rmse(r1); r2e=rmse(r2); r3e=rmse(r3);
 fprintf('error relative to APDM (euler angles, rad): [%.4f %.4f %.4f]\n',[r1e r2e r3e]);
 
+% plot bioslam's EKF implementation:
+figure('units','normalized','position',[0.1300 0.5500 0.250 0.250]);
+lh=plot(repmat(myImu.time,[1 4]),x);
+set(lh,{'color'},{[0 0 0]; [1 0 0]; [0 1 0]; [0 0 1]});
+legend('q_s','q_x','q_y','q_z');
+grid on; xlabel('time (sec)'); ylabel('quaternion component');
+title('EKF results');
 
 function armse=rmse(a)
-armse=sqrt(mean(a.^2));
+    armse=sqrt(mean(a.^2));
 end