├── README.html
├── README.md
├── datasets
├── spiralStairs_CalInertialAndMag.csv
├── stairsAndCorridor_CalInertialAndMag.csv
└── straightLine_CalInertialAndMag.csv
├── image
├── 1_1_matlab.png
├── 1_1_python.png
├── 1_2_matlab.png
├── 1_2_python.png
├── 1_3_matlab.png
├── 1_3_python.png
├── 1_4_matlab.png
├── 1_4_python.png
├── 2_1_matlab.png
├── 2_1_python.png
├── 2_2_matlab.png
├── 2_2_python.png
├── 2_3_matlab.png
├── 2_3_python.png
├── 2_4_matlab.png
├── 2_4_python.png
├── 3_1_matlab.png
├── 3_1_python.png
├── 3_2_matlab.png
├── 3_2_python.png
├── 3_3_matlab.png
├── 3_3_python.png
├── 3_4_matlab.png
└── 3_4_python.png
├── requirements.txt
├── script.py
└── ximu_python_library
├── CalInertialAndMagneticDataClass.py
├── __pycache__
├── CalInertialAndMagneticDataClass.cpython-37.pyc
└── xIMUdataClass.cpython-37.pyc
└── xIMUdataClass.py
/README.html:
--------------------------------------------------------------------------------
1 | READMEGait_Tracking_with_x-imu_python
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Gait Tracking with x-imu Python
2 | This is the Python code of the [Gait-Tracking-With-x-IMU](https://github.com/xioTechnologies/Gait-Tracking-With-x-IMU) by [xioTechnologies](https://github.com/xioTechnologies), which originally run on MATLAB.
3 |
4 |
5 | The project Gait-Tracking-With-x-IMU is for the foot tracking algorithm demonstrated in Seb Madgwick's "[3D Tracking with IMU](https://www.youtube.com/watch?v=6ijArKE8vKU)" video. The foot tracking is enabled through [dead reckoning](https://en.wikipedia.org/wiki/Dead_reckoning) and integral drift corrected for each time the foot hit the ground.
6 | Please also see their [original post](http://www.x-io.co.uk/gait-tracking-with-x-imu/).
7 |
8 |
9 | ## Several Examples of Result
10 | I provide several results of this Python code, comparing with the results of the original code on MATLAB.
11 |
12 | ### spiralStairs_CalInertialAndMag.csv
13 |
14 | Python (This repository) | MATLAB ([xioTechnologies](https://github.com/xioTechnologies)'s)
15 | :-------------------------:|:-------------------------:
16 |  | 
17 |  | 
18 |  | 
19 |  | 
20 |
21 | ### straightLine_CalInertialAndMag.csv
22 |
23 | Python (This repository) | MATLAB ([xioTechnologies](https://github.com/xioTechnologies)'s)
24 | :-------------------------:|:-------------------------:
25 |  | 
26 |  | 
27 |  | 
28 |  | 
29 |
30 | ### stairsAndCorridor_CalInertialAndMag.csv
31 |
32 | Python (This repository) | MATLAB ([xioTechnologies](https://github.com/xioTechnologies)'s)
33 | :-------------------------:|:-------------------------:
34 |  | 
35 |  | 
36 |  | 
37 |  | 
--------------------------------------------------------------------------------
/image/1_1_matlab.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/1_1_matlab.png
--------------------------------------------------------------------------------
/image/1_1_python.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/1_1_python.png
--------------------------------------------------------------------------------
/image/1_2_matlab.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/1_2_matlab.png
--------------------------------------------------------------------------------
/image/1_2_python.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/1_2_python.png
--------------------------------------------------------------------------------
/image/1_3_matlab.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/1_3_matlab.png
--------------------------------------------------------------------------------
/image/1_3_python.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/1_3_python.png
--------------------------------------------------------------------------------
/image/1_4_matlab.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/1_4_matlab.png
--------------------------------------------------------------------------------
/image/1_4_python.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/1_4_python.png
--------------------------------------------------------------------------------
/image/2_1_matlab.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/2_1_matlab.png
--------------------------------------------------------------------------------
/image/2_1_python.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/2_1_python.png
--------------------------------------------------------------------------------
/image/2_2_matlab.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/2_2_matlab.png
--------------------------------------------------------------------------------
/image/2_2_python.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/2_2_python.png
--------------------------------------------------------------------------------
/image/2_3_matlab.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/2_3_matlab.png
--------------------------------------------------------------------------------
/image/2_3_python.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/2_3_python.png
--------------------------------------------------------------------------------
/image/2_4_matlab.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/2_4_matlab.png
--------------------------------------------------------------------------------
/image/2_4_python.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/2_4_python.png
--------------------------------------------------------------------------------
/image/3_1_matlab.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/3_1_matlab.png
--------------------------------------------------------------------------------
/image/3_1_python.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/3_1_python.png
--------------------------------------------------------------------------------
/image/3_2_matlab.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/3_2_matlab.png
--------------------------------------------------------------------------------
/image/3_2_python.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/3_2_python.png
--------------------------------------------------------------------------------
/image/3_3_matlab.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/3_3_matlab.png
--------------------------------------------------------------------------------
/image/3_3_python.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/3_3_python.png
--------------------------------------------------------------------------------
/image/3_4_matlab.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/3_4_matlab.png
--------------------------------------------------------------------------------
/image/3_4_python.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/image/3_4_python.png
--------------------------------------------------------------------------------
/requirements.txt:
--------------------------------------------------------------------------------
1 | pyquaternion
2 | ahrs
3 | scipy
4 | matplotlib
--------------------------------------------------------------------------------
/script.py:
--------------------------------------------------------------------------------
1 | import ahrs
2 | from ahrs.common.orientation import q_prod, q_conj, acc2q, am2q, q2R, q_rot
3 | import pyquaternion
4 | import ximu_python_library.xIMUdataClass as xIMU
5 | import numpy as np
6 | from scipy import signal
7 | from matplotlib import pyplot as plt
8 | from mpl_toolkits.mplot3d import Axes3D
9 |
10 | # filePath = 'datasets/straightLine'
11 | # startTime = 6
12 | # stopTime = 26
13 | # samplePeriod = 1/256
14 |
15 | # filePath = 'datasets/stairsAndCorridor'
16 | # startTime = 5
17 | # stopTime = 53
18 | # samplePeriod = 1/256
19 |
20 | filePath = 'datasets/spiralStairs'
21 | startTime = 4
22 | stopTime = 47
23 | samplePeriod = 1/256
24 |
25 |
26 | def main():
27 | xIMUdata = xIMU.xIMUdataClass(filePath, 'InertialMagneticSampleRate', 1/samplePeriod)
28 | time = xIMUdata.CalInertialAndMagneticData.Time
29 | gyrX = xIMUdata.CalInertialAndMagneticData.gyroscope[:,0]
30 | gyrY = xIMUdata.CalInertialAndMagneticData.gyroscope[:,1]
31 | gyrZ = xIMUdata.CalInertialAndMagneticData.gyroscope[:,2]
32 | accX = xIMUdata.CalInertialAndMagneticData.accelerometer[:,0]
33 | accY = xIMUdata.CalInertialAndMagneticData.accelerometer[:,1]
34 | accZ = xIMUdata.CalInertialAndMagneticData.accelerometer[:,2]
35 |
36 | indexSel = np.all([time>=startTime,time<=stopTime], axis=0)
37 | time = time[indexSel]
38 | gyrX = gyrX[indexSel]
39 | gyrY = gyrY[indexSel]
40 | gyrZ = gyrZ[indexSel]
41 | accX = accX[indexSel]
42 | accY = accY[indexSel]
43 | accZ = accZ[indexSel]
44 |
45 |
46 | # Compute accelerometer magnitude
47 | acc_mag = np.sqrt(accX*accX+accY*accY+accZ*accZ)
48 |
49 | # HP filter accelerometer data
50 | filtCutOff = 0.001
51 | b, a = signal.butter(1, (2*filtCutOff)/(1/samplePeriod), 'highpass')
52 | acc_magFilt = signal.filtfilt(b, a, acc_mag, padtype = 'odd', padlen=3*(max(len(b),len(a))-1))
53 |
54 | # Compute absolute value
55 | acc_magFilt = np.abs(acc_magFilt)
56 |
57 | # LP filter accelerometer data
58 | filtCutOff = 5
59 | b, a = signal.butter(1, (2*filtCutOff)/(1/samplePeriod), 'lowpass')
60 | acc_magFilt = signal.filtfilt(b, a, acc_magFilt, padtype = 'odd', padlen=3*(max(len(b),len(a))-1))
61 |
62 |
63 | # Threshold detection
64 | stationary = acc_magFilt < 0.05
65 |
66 | fig = plt.figure(figsize=(10, 5))
67 | ax1 = fig.add_subplot(2,1,1)
68 | ax2 = fig.add_subplot(2,1,2)
69 | ax1.plot(time,gyrX,c='r',linewidth=0.5)
70 | ax1.plot(time,gyrY,c='g',linewidth=0.5)
71 | ax1.plot(time,gyrZ,c='b',linewidth=0.5)
72 | ax1.set_title("gyroscope")
73 | ax1.set_xlabel("time (s)")
74 | ax1.set_ylabel("angular velocity (degrees/s)")
75 | ax1.legend(["x","y","z"])
76 | ax2.plot(time,accX,c='r',linewidth=0.5)
77 | ax2.plot(time,accY,c='g',linewidth=0.5)
78 | ax2.plot(time,accZ,c='b',linewidth=0.5)
79 | ax2.plot(time,acc_magFilt,c='k',linestyle=":",linewidth=1)
80 | ax2.plot(time,stationary,c='k')
81 | ax2.set_title("accelerometer")
82 | ax2.set_xlabel("time (s)")
83 | ax2.set_ylabel("acceleration (g)")
84 | ax2.legend(["x","y","z"])
85 | plt.show(block=False)
86 |
87 | # Compute orientation
88 | quat = np.zeros((time.size, 4), dtype=np.float64)
89 |
90 | # initial convergence
91 | initPeriod = 2
92 | indexSel = time<=time[0]+initPeriod
93 | gyr=np.zeros(3, dtype=np.float64)
94 | acc = np.array([np.mean(accX[indexSel]), np.mean(accY[indexSel]), np.mean(accZ[indexSel])])
95 | mahony = ahrs.filters.Mahony(Kp=1, Ki=0,KpInit=1, frequency=1/samplePeriod)
96 | q = np.array([1.0,0.0,0.0,0.0], dtype=np.float64)
97 | for i in range(0, 2000):
98 | q = mahony.updateIMU(q, gyr=gyr, acc=acc)
99 |
100 | # For all data
101 | for t in range(0,time.size):
102 | if(stationary[t]):
103 | mahony.Kp = 0.5
104 | else:
105 | mahony.Kp = 0
106 | gyr = np.array([gyrX[t],gyrY[t],gyrZ[t]])*np.pi/180
107 | acc = np.array([accX[t],accY[t],accZ[t]])
108 | quat[t,:]=mahony.updateIMU(q,gyr=gyr,acc=acc)
109 |
110 | # -------------------------------------------------------------------------
111 | # Compute translational accelerations
112 |
113 | # Rotate body accelerations to Earth frame
114 | acc = []
115 | for x,y,z,q in zip(accX,accY,accZ,quat):
116 | acc.append(q_rot(q_conj(q), np.array([x, y, z])))
117 | acc = np.array(acc)
118 | acc = acc - np.array([0,0,1])
119 | acc = acc * 9.81
120 |
121 | # Compute translational velocities
122 | # acc[:,2] = acc[:,2] - 9.81
123 |
124 | # acc_offset = np.zeros(3)
125 | vel = np.zeros(acc.shape)
126 | for t in range(1,vel.shape[0]):
127 | vel[t,:] = vel[t-1,:] + acc[t,:]*samplePeriod
128 | if stationary[t] == True:
129 | vel[t,:] = np.zeros(3)
130 |
131 | # Compute integral drift during non-stationary periods
132 | velDrift = np.zeros(vel.shape)
133 | stationaryStart = np.where(np.diff(stationary.astype(int)) == -1)[0]+1
134 | stationaryEnd = np.where(np.diff(stationary.astype(int)) == 1)[0]+1
135 | for i in range(0,stationaryEnd.shape[0]):
136 | driftRate = vel[stationaryEnd[i]-1,:] / (stationaryEnd[i] - stationaryStart[i])
137 | enum = np.arange(0,stationaryEnd[i]-stationaryStart[i])
138 | drift = np.array([enum*driftRate[0], enum*driftRate[1], enum*driftRate[2]]).T
139 | velDrift[stationaryStart[i]:stationaryEnd[i],:] = drift
140 |
141 | # Remove integral drift
142 | vel = vel - velDrift
143 | fig = plt.figure(figsize=(10, 5))
144 | plt.plot(time,vel[:,0],c='r',linewidth=0.5)
145 | plt.plot(time,vel[:,1],c='g',linewidth=0.5)
146 | plt.plot(time,vel[:,2],c='b',linewidth=0.5)
147 | plt.legend(["x","y","z"])
148 | plt.title("velocity")
149 | plt.xlabel("time (s)")
150 | plt.ylabel("velocity (m/s)")
151 | plt.show(block=False)
152 |
153 | # -------------------------------------------------------------------------
154 | # Compute translational position
155 | pos = np.zeros(vel.shape)
156 | for t in range(1,pos.shape[0]):
157 | pos[t,:] = pos[t-1,:] + vel[t,:]*samplePeriod
158 |
159 | fig = plt.figure(figsize=(10, 5))
160 | plt.plot(time,pos[:,0],c='r',linewidth=0.5)
161 | plt.plot(time,pos[:,1],c='g',linewidth=0.5)
162 | plt.plot(time,pos[:,2],c='b',linewidth=0.5)
163 | plt.legend(["x","y","z"])
164 | plt.title("position")
165 | plt.xlabel("time (s)")
166 | plt.ylabel("position (m)")
167 | plt.show(block=False)
168 |
169 | # -------------------------------------------------------------------------
170 | # Plot 3D foot trajectory
171 |
172 | posPlot = pos
173 | quatPlot = quat
174 |
175 | extraTime = 20
176 | onesVector = np.ones(int(extraTime*(1/samplePeriod)))
177 |
178 | # Create 6 DOF animation
179 | fig = plt.figure(figsize=(7, 7))
180 | ax = fig.add_subplot(111, projection='3d') # Axe3D object
181 | ax.plot(posPlot[:,0],posPlot[:,1],posPlot[:,2])
182 | min_, max_ = np.min(np.min(posPlot,axis=0)), np.max(np.max(posPlot,axis=0))
183 | ax.set_xlim(min_,max_)
184 | ax.set_ylim(min_,max_)
185 | ax.set_zlim(min_,max_)
186 | ax.set_title("trajectory")
187 | ax.set_xlabel("x position (m)")
188 | ax.set_ylabel("y position (m)")
189 | ax.set_zlabel("z position (m)")
190 | plt.show(block=False)
191 |
192 | plt.show()
193 |
194 | if __name__ == "__main__":
195 | main()
196 |
--------------------------------------------------------------------------------
/ximu_python_library/CalInertialAndMagneticDataClass.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 |
3 | # class DataBaseClass():
4 | # def __init__(self):
5 |
6 | # class TimeSeriesDataBaseClass():
7 | # def __init__(self):
8 | # self.DataBase = DataBaseClass()
9 |
10 | # class InertialAndMagneticDataBaseClass():
11 | # def __init__(self, data):
12 | # self.TimeSeriesDataBase = TimeSeriesDataBaseClass()
13 | # set_dataformat(data)
14 |
15 | # def set_dataformat(self, data):
16 | # self.gyroscope = data[:,1:4]
17 | # self.accelerometer = data[:,4:7]
18 | # self.magnetometer = data[:,7:10]
19 |
20 | class CalInertialAndMagneticDataClass():
21 | def __init__(self, filename, sr):
22 | self.FileNameAppendage = '_CalInertialAndMag.csv'
23 | self.SampleRate = sr
24 | self.GyroscopeUnits = 'degrees/s';
25 | self.AccelerometerUnits = 'g';
26 | self.MagnetometerUnits = 'G';
27 | data = np.loadtxt(filename+self.FileNameAppendage,
28 | delimiter=',',skiprows=1)
29 | # self.InertialAndMagneticDataBase = InertialAndMagneticDataBaseClass(data)
30 | self.set_dataformat(data)
31 | self.packetNum = data.shape[0]
32 | self.Time = np.arange(0,self.packetNum)*(1/self.SampleRate)
33 |
34 | def set_dataformat(self, data):
35 | self.gyroscope = data[:,1:4]
36 | self.accelerometer = data[:,4:7]
37 | self.magnetometer = data[:,7:10]
38 |
39 |
40 |
41 | def get_obj(filename, sr):
42 | obj = CalInertialAndMagneticDataClass(filename, sr)
43 | return obj
--------------------------------------------------------------------------------
/ximu_python_library/__pycache__/CalInertialAndMagneticDataClass.cpython-37.pyc:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/ximu_python_library/__pycache__/CalInertialAndMagneticDataClass.cpython-37.pyc
--------------------------------------------------------------------------------
/ximu_python_library/__pycache__/xIMUdataClass.cpython-37.pyc:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/daehwa/Gait-Tracking-With-x-IMU-Python/09ceab4879fdc06ee357a39c1d6beea07dd17a9e/ximu_python_library/__pycache__/xIMUdataClass.cpython-37.pyc
--------------------------------------------------------------------------------
/ximu_python_library/xIMUdataClass.py:
--------------------------------------------------------------------------------
1 | import os
2 | import sys
3 | this_dir = os.path.dirname(__file__)
4 | sys.path.append(this_dir)
5 | import CalInertialAndMagneticDataClass as calIM
6 |
7 | class xIMUdataClass():
8 | def __init__(self, filename='', sr_type='InertialMagneticSampleRate', sr = 20000/3):
9 | self.FileNamePrefix = filename
10 | self.ErrorData = []
11 | self.CommandData = []
12 | self.RegisterData = []
13 | self.DateTimeData = []
14 | self.RawBatteryAndThermometerData = []
15 | self.CalBatteryAndThermometerData = []
16 | self.RawInertialAndMagneticData = []
17 | self.CalInertialAndMagneticData = []
18 | self.QuaternionData = []
19 | self.RotationMatrixData = []
20 | self.EulerAnglesData = []
21 | self.DigitalIOdata = []
22 | self.RawAnalogueInputData = []
23 | self.CalAnalogueInputData = []
24 | self.PWMoutputData = []
25 | self.RawADXL345busData = []
26 | self.CalADXL345busData = []
27 | self.sr_type = sr_type
28 | self.sr = sr
29 | self.load_data()
30 | self.apply_samplerate()
31 | # self.plot()
32 |
33 | def load_data(self):
34 | dataImported = False
35 | try:
36 | self.CalInertialAndMagneticData = calIM.get_obj(self.FileNamePrefix, self.sr)
37 | dataImported = True
38 | except:
39 | print('')
40 | if not dataImported:
41 | print('No data was imported.')
42 | exit()
43 |
44 | def apply_samplerate(self):
45 | if self.sr_type == 'InertialMagneticSampleRate':
46 | try:
47 | h = self.CalInertialAndMagneticData
48 | h.SampleRate = self.sr
49 | except:
50 | print('')
51 | else:
52 | print('Invalid argument.')
53 | exit()
54 |
55 | # def plot(self):
56 | # try:
57 | # self.CalInertialAndMagneticData.plot() #########################
58 | # except:
59 | # continue
--------------------------------------------------------------------------------