Code Appendix
main_func.py (the main function that runs the whole program)
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 | import pygame import os import numpy # import main # from main import position from readIMU import GetPos import RPi.GPIO as GPIO ############################################## ## Inititlize parameter ############################################# os.putenv('SDL_VIDEODRIVER','fbcon') os.putenv('SDL_FBDEV', '/dev/fb1') os.putenv('SDL_MOUSEDRV', 'TSLIB') os.putenv('SDL_MOUSEDEV', '/dev/input/touchscreen') GPIO.setup(17, GPIO.IN, pull_up_down=GPIO.PUD_UP) # exit button def GPIO17_callback(channel): exit() GPIO.add_event_detect(17, GPIO.FALLING, callback=GPIO17_callback,bouncetime=300) pygame.init() pygame.mouse.set_visible(False) black = 0, 0, 0 size = width, height = 320, 240 screen = pygame.display.set_mode(size) screen.fill((0,0,0)) try: while True: points = GetPos() points = numpy.array(points) points = points[:,[0,1]] ranges = numpy.ptp(points, axis=0) x_scale = 230/ranges[0] y_scale = 180/ranges[1] scale = min(x_scale,y_scale) points[:,0] = points[:,0]*scale points[:,1] = points[:,1]*scale means = numpy.mean(points,axis=0) points[:,0] = points[:,0] + (160-means[0]) points[:,1] = points[:,1] + (120-means[1]) ranges = numpy.ptp(points, axis=0) screen.fill(black) pygame.draw.lines(screen, (255,255,255), False, points, 2) #pygame.draw.circle(screen,(255,255,255),[60,250],40) pygame.display.flip() except KeyboardInterrupt: print "stop showing the record." |
calibration.py
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 | import numpy as np # axises # | X | Y | Z | # pos1 | east | sky | south | (Y) # pos2 | east | north | sky | (-X) # pos3 | ground| east | south | (Y) # pos4 | west | ground| south | (-Y) # pos5 | sky | west | south | (Z) # pos6 | south | west | ground| (-Z) g = 9.80532 # gravity force in Ithaca, NY # reading calibration data Cali_Pos1_all = np.loadtxt('calibration_data/calibrate_pos1.txt',delimiter = ',') Cali_Pos2_all = np.loadtxt('calibration_data/calibrate_pos2.txt',delimiter = ',') Cali_Pos3_all = np.loadtxt('calibration_data/calibrate_pos3.txt',delimiter = ',') Cali_Pos4_all = np.loadtxt('calibration_data/calibrate_pos4.txt',delimiter = ',') Cali_Pos5_all = np.loadtxt('calibration_data/calibrate_pos5.txt',delimiter = ',') Cali_Pos6_all = np.loadtxt('calibration_data/calibrate_pos6.txt',delimiter = ',') X_1 = np.mean(Cali_Pos1_all[:,[0]]) X_2 = np.mean(Cali_Pos2_all[:,[0]]) X_3 = np.mean(Cali_Pos3_all[:,[0]]) X_4 = np.mean(Cali_Pos4_all[:,[0]]) X_5 = np.mean(Cali_Pos5_all[:,[0]]) X_6 = np.mean(Cali_Pos6_all[:,[0]]) Y_1 = np.mean(Cali_Pos1_all[:,[1]]) Y_2 = np.mean(Cali_Pos2_all[:,[1]]) Y_3 = np.mean(Cali_Pos3_all[:,[1]]) Y_4 = np.mean(Cali_Pos4_all[:,[1]]) Y_5 = np.mean(Cali_Pos5_all[:,[1]]) Y_6 = np.mean(Cali_Pos6_all[:,[1]]) Z_1 = np.mean(Cali_Pos1_all[:,[2]]) Z_2 = np.mean(Cali_Pos2_all[:,[2]]) Z_3 = np.mean(Cali_Pos3_all[:,[2]]) Z_4 = np.mean(Cali_Pos4_all[:,[2]]) Z_5 = np.mean(Cali_Pos5_all[:,[2]]) Z_6 = np.mean(Cali_Pos6_all[:,[2]]) # calculate calibration matrix a_x0 = (X_1 + X_2 + X_4 + X_6)/4 S_ax = (X_3 - X_5)/2 K_ax1 = (X_4 - X_1)/2 K_ax2 = (X_6 - X_2)/2 K_ax3 = (-X_1 - X_2 + 2*X_3 - X_4 + 2 * X_5 - X_6)/4 a_y0 = (Y_2 + Y_3 + Y_5 + Y_6)/4 S_ay = (Y_4 - Y_1)/2 K_ay1 = (Y_3 - Y_5)/2 K_ay2 = (Y_6 - Y_2)/2 K_ay3 = (2*Y_1 - Y_2 - Y_3 + 2*Y_4 - Y_5 - Y_6)/4 a_z0 = (Z_1 + Z_3 + Z_4 + Z_5)/4 S_az = (Z_6 - Z_2)/2 K_az1 = (Z_3 - Z_5)/2 K_az2 = (Z_4 - Z_1)/2 K_az3 = (-Z_1 + 2 * Z_2 - Z_3 - Z_4 - Z_5 + 2 * Z_6)/4 Offset = np.array([[a_x0],[a_y0],[a_z0]])/g Scale = np.array([[S_ax, K_ax1, K_ax2], [K_ay1, S_ay, K_ay2], [K_az1, K_az2, S_az]])/g Scale_2 = np.array([[K_ax3, 0 , 0], [0 ,K_ay3, 0], [0, 0, K_ax3]])/g ###################################################### # simpler way of calibration (code commented out) ###################################################### # Bx = (X_pos_g + X_neg_g)/2 # Sx = g * 2/(X_pos_g - X_neg_g) # By = (Y_pos_g + Y_neg_g)/2 # Sy = g * 2/(Y_pos_g - Y_neg_g) # Bz = (Z_pos_g + Z_neg_g)/2 # Sz = g * 2/(Z_pos_g - Z_neg_g) # S = np.array([[Sx,0,0],[0,Sy,0],[0,0,Sz]]) # B = np.array([[Bx],[By],[Bz]]) print "Offset: ", Offset, '\n' print "Scale: ",Scale, '\n' print "scale mcm", Scale_2, '\n' def return_calibration_matrix(): # return calibration matrix return Offset, Scale, Scale_2 |
calibration_data.py
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 | import logging import sys import time import numpy as np import RPi.GPIO from Adafruit_BNO055 import BNO055 # Create and configure the BNO sensor connection. Make sure only ONE of the # below 'bno = ...' lines is uncommented: # Raspberry Pi configuration with serial UART and RST connected to GPIO 18: bno = BNO055.BNO055(serial_port='/dev/ttyAMA0', rst=18) # BeagleBone Black configuration with default I2C connection (SCL=P9_19, SDA=P9_20), # and RST connected to pin P9_12: #bno = BNO055.BNO055(rst='P9_12') # Enable verbose debug logging if -v is passed as a parameter. if len(sys.argv) == 2 and sys.argv[1].lower() == '-v': logging.basicConfig(level=logging.DEBUG) # Initialize the BNO055 and stop if something went wrong. if not bno.begin(): raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?') # Print system status and self test result. status, self_test, error = bno.get_system_status() print('System status: {0}'.format(status)) print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test)) # Print out an error if system status is in error mode. if status == 0x01: print('System error: {0}'.format(error)) print('See datasheet section 4.3.59 for the meaning.') # Print BNO055 software revision and other diagnostic data. sw, bl, accel, mag, gyro = bno.get_revision() print('Software version: {0}'.format(sw)) print('Bootloader version: {0}'.format(bl)) print('Accelerometer ID: 0x{0:02X}'.format(accel)) print('Magnetometer ID: 0x{0:02X}'.format(mag)) print('Gyroscope ID: 0x{0:02X}\n'.format(gyro)) print('Reading BNO055 data, press Ctrl-C to quit...') print ("# | X | Y | Z |" + '\n' "pos1 | east | sky | south | (Y)" + '\n' "pos2 | east | north | sky | (Z)" + '\n' "pos3 | ground| east | south | (-X)" + '\n' "pos4 | west | ground| south | (-Y)" + '\n' "pos5 | sky | west | south | (X)" + '\n' "pos6 | south | west | ground| (-Z)" + '\n') raw_input("first test z axis, press enter to continue") t = 35998 ###########!!!!!!!!!!!!!!!!remember to change back first_data = True while True: # Read the Euler angles for heading, roll, pitch (all in degrees). # heading, roll, pitch = bno.read_euler() # Read the calibration status, 0=uncalibrated and 3=fully calibrated. # sys, gyro, accel, mag = bno.get_calibration_status() # Accelerometer data (in meters per second squared): # AccX,AccY,AccZ = bno.read_linear_acceleration() RawX,RawY,RawZ = bno.read_accelerometer() Acc_t = np.array([[RawX, RawY, RawZ]]) # Orientation as a quaternion: # Qx,Qy,Qz,Qw = bno.read_quaternion() # Quat = [Qx, Qy, Qz, Qw] # Get all data # print('Heading={0:0.2F} Roll={1:0.2F} Pitch={2:0.2F}\tSys_cal={3} Gyro_cal={4} Accel_cal={5} Mag_cal={6}'.format( # heading, roll, pitch, sys, gyro, accel, mag)) # # Convert to strings for display strACC = ', '.join(str(num).rjust(5) for num in Acc_t) # strQuat = ', '.join(str(num).rjust(5) for num in Quat) # Display in a pretty way print("t:" + str(t) + "|" + "Accel: " + strACC +'\n') # + "Quat:" + strQuat if not first_data: Acc = np.concatenate((Acc,Acc_t), axis = 0) else: Acc = Acc_t first_data = False t = t + 1 time.sleep(1./100) if t == 11999: file_name = 'calibrate_pos1.txt' np.savetxt(file_name ,Acc,delimiter=',') first_data = True print ("# | X | Y | Z |" + '\n' "pos1 | east | sky | south | (Y)" + '\n' "pos2 | east | north | sky | (Z)" + '\n' "pos3 | ground| east | south | (-X)" + '\n' "pos4 | west | ground| south | (-Y)" + '\n' "pos5 | sky | west | south | (X)" + '\n' "pos6 | south | west | ground| (-Z)" + '\n') raw_input("pos1 finished, press enter to continue for pos2") elif t == 23999: file_name = 'calibrate_pos2.txt' np.savetxt(file_name ,Acc,delimiter=',') first_data = True print ("# | X | Y | Z |" + '\n' "pos1 | east | sky | south | (Y)" + '\n' "pos2 | east | north | sky | (Z)" + '\n' "pos3 | ground| east | south | (-X)" + '\n' "pos4 | west | ground| south | (-Y)" + '\n' "pos5 | sky | west | south | (X)" + '\n' "pos6 | south | west | ground| (-Z)" + '\n') raw_input("pos2 finished, press enter to continue for pos3") elif t == 35999: file_name = 'calibrate_pos3.txt' np.savetxt(file_name ,Acc,delimiter=',') first_data = True print ("# | X | Y | Z |" + '\n' "pos1 | east | sky | south | (Y)" + '\n' "pos2 | east | north | sky | (Z)" + '\n' "pos3 | ground| east | south | (-X)" + '\n' "pos4 | west | ground| south | (-Y)" + '\n' "pos5 | sky | west | south | (X)" + '\n' "pos6 | south | west | ground| (-Z)" + '\n') raw_input("pos3 finished, press enter to continue for pos4") elif t == 47999: file_name = 'calibrate_pos4.txt' np.savetxt(file_name ,Acc,delimiter=',') first_data = True print ("# | X | Y | Z |" + '\n' "pos1 | east | sky | south | (Y)" + '\n' "pos2 | east | north | sky | (Z)" + '\n' "pos3 | ground| east | south | (-X)" + '\n' "pos4 | west | ground| south | (-Y)" + '\n' "pos5 | sky | west | south | (X)" + '\n' "pos6 | south | west | ground| (-Z)" + '\n') raw_input("pos4 finished, press enter to continue for pos5") elif t == 59999: file_name = 'calibrate_pos5.txt' np.savetxt(file_name ,Acc,delimiter=',') first_data = True print ("# | X | Y | Z |" + '\n' "pos1 | east | sky | south | (Y)" + '\n' "pos2 | east | north | sky | (Z)" + '\n' "pos3 | ground| east | south | (-X)" + '\n' "pos4 | west | ground| south | (-Y)" + '\n' "pos5 | sky | west | south | (X)" + '\n' "pos6 | south | west | ground| (-Z)" + '\n') raw_input("pos5 finished, press enter to continue for pos6 ") elif t == 71999: file_name = 'calibrate_pos6.txt' np.savetxt(file_name ,Acc,delimiter=',') first_data = True print ("# | X | Y | Z |" + '\n' "pos1 | east | sky | south | (Y)" + '\n' "pos2 | east | north | sky | (Z)" + '\n' "pos3 | ground| east | south | (-X)" + '\n' "pos4 | west | ground| south | (-Y)" + '\n' "pos5 | sky | west | south | (X)" + '\n' "pos6 | south | west | ground| (-Z)" + '\n') raw_input("pos finished, press enter to continue ") exit() |
quatern_func.py
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 | import numpy as np def quaternProd(a, b): # lm=q(1); p1=q(2); p2=q(3); p3=q(4); # q1=[lm -p1 -p2 -p3 # p1 lm -p3 p2 # p2 p3 lm -p1 # p3 -p2 p1 lm]*m; lm = a[0] p1 = a[1] p2 = a[2] p3 = a[3] temp = np.array([[lm, -p1, -p2, -p3], [p1, lm, -p3, p2], [p2, p3, lm, -p1], [p3, -p2, p1, lm]]) # print temp, b q1 = np.dot(temp,np.transpose([b])) q1 = np.transpose(q1) q1 = q1[0] return q1 def quaternInv(a): b = [a[0], -a[1], -a[2], -a[3]] return b def quaternRot(Vec,Quat): V = [0., Vec[0],Vec[1],Vec[2]] # print V Ve = quaternProd(quaternProd(quaternInv(Quat),V),Quat) Ve = np.array([[Ve[1],Ve[2],Ve[3]]]) return Ve |
filter.py
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 | from __future__ import division import numpy as np import warnings class AHRS(object): def __init__(self, SamplePeriod = 1./200, Kp = 2., KpInit = 0., Quaternion = np.array([1, 0, 0, 0]), Ki = 0., InitPeriod = 5. ): """docstring for AHRS""" self.SamplePeriod = SamplePeriod self.Quaternion = Quaternion # output quaternion describing the sensor relative to the Earth self.Kp = Kp # proportional gain self.Ki = Ki # integral gain self.KpInit = KpInit # proportional gain used during initialisation self.InitPeriod = InitPeriod # initialisation period in seconds self.q = np.array([1., 0, 0, 0]) #% internal quaternion describing the Earth relative to the sensor self.IntError = np.array([[0.], [0.], [0.]]) #% integral error self.KpRamped = None #% internal proportional gain used to ramp during initialisation def UpdateIMU(self, Gyr, Acc): if np.linalg.norm(Acc) == 0: warnings.warn("Accelerometer magnitude is zero. Algorithm update aborted.") return else: Acc = np.array(Acc / np.linalg.norm(Acc)) # print Acc v = np.array([[2*(self.q[1]*self.q[3] - self.q[0]*self.q[2])], [2*(self.q[0]*self.q[1] + self.q[2]*self.q[3])], [self.q[0]**2 - self.q[1]**2 - self.q[2]**2 + self.q[3]**2]]) # print v error = np.cross(v,np.transpose([Acc]),axis = 0) # print error self.IntError = self.IntError + error Ref = Gyr - np.transpose(self.Kp*error + self.Ki*self.IntError) pDot = np.multiply(0.5 , self.quaternProd_single(self.q, [0, Ref[0,0], Ref[0,1], Ref[0,2]])) self.q = self.q + pDot * self.SamplePeriod; # % integrate rate of change of quaternion self.q = self.q / np.linalg.norm(self.q); self.Quaternion = self.quaternConj(self.q); def quaternProd_single(self, a, b): ab = [None] * 4 ab[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3] ab[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2] ab[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1] ab[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0] return ab def quaternProd(self, a, b): ab = [] # print a # print b ab = np.concatenate((a[:,[0]] * b[:,[0]] - a[:,[1]] * b[:,[1]] - a[:,[2]] * b[:,[2]] - a[:,[3]] * b[:,[3]], a[:,[0]] * b[:,[1]] + a[:,[1]] * b[:,[0]] + a[:,[2]] * b[:,[3]] - a[:,[3]] * b[:,[2]], a[:,[0]] * b[:,[2]] - a[:,[1]] * b[:,[3]] + a[:,[2]] * b[:,[0]] + a[:,[3]] * b[:,[1]], a[:,[0]] * b[:,[3]] + a[:,[1]] * b[:,[2]] - a[:,[2]] * b[:,[1]] + a[:,[3]] * b[:,[0]]),axis = 1) return ab def quaternConj(self, q): if q.ndim == 1: qConj = [q[0], -q[1], -q[2], -q[3]] else: qConj = np.concatenate((q[:,[0]], -q[:,[1]], -q[:,[2]], -q[:,[3]]),axis = 1) # print qConj.shape # raw_input("wait") return qConj def quaternRotate(self, v,q): row, col = v.shape # print row,col # print 'shape:', np.zeros((row,1)) # print 'q: ', q temp = self.quaternProd(q,np.concatenate((np.zeros((row,1)), v),axis = 1)) v0XYZ = self.quaternProd(temp, self.quaternConj(q)) v = v0XYZ[:, 1:4] # print v.shape return v |
readIMU.py
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 | from __future__ import division from scipy import signal from filter import AHRS import numpy as np import matplotlib.pyplot as plt import matplotlib import math # import LSM9DS0 as IMU import csv import logging import sys import time import numpy as np import RPi.GPIO as GPIO from quatern_func import * # import draw from Adafruit_BNO055 import BNO055 ########################### # get data ############################# GPIO.setmode(GPIO.BCM) GPIO.setup(5, GPIO.IN, pull_up_down=GPIO.PUD_UP) # Create and configure the BNO sensor connection. Make sure only ONE of the # below 'bno = ...' lines is uncommented: # Raspberry Pi configuration with serial UART and RST connected to GPIO 18: bno = BNO055.BNO055(serial_port='/dev/ttyAMA0', rst=18) # BeagleBone Black configuration with default I2C connection (SCL=P9_19, SDA=P9_20), # and RST connected to pin P9_12: #bno = BNO055.BNO055(rst='P9_12') # Enable verbose debug logging if -v is passed as a parameter. if len(sys.argv) == 2 and sys.argv[1].lower() == '-v': logging.basicConfig(level=logging.DEBUG) # Initialize the BNO055 and stop if something went wrong. if not bno.begin(): raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?') # Print system status and self test result. status, self_test, error = bno.get_system_status() print('System status: {0}'.format(status)) print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test)) # Print out an error if system status is in error mode. if status == 0x01: print('System error: {0}'.format(error)) print('See datasheet section 4.3.59 for the meaning.') # Print BNO055 software revision and other diagnostic data. sw, bl, accel, mag, gyro = bno.get_revision() print('Software version: {0}'.format(sw)) print('Bootloader version: {0}'.format(bl)) print('Accelerometer ID: 0x{0:02X}'.format(accel)) print('Magnetometer ID: 0x{0:02X}'.format(mag)) print('Gyroscope ID: 0x{0:02X}\n'.format(gyro)) print('Reading BNO055 data, press Ctrl-C to quit...') def GetPos(): while True: sys, gyro, accel, mag = bno.get_calibration_status() print "accelerometer calibration status: ", accel if accel == 3: # wait until the accelerometer calibration finish print "accelerometer calibration finished, press buttom 5 to start and press again to end" break while True: if not GPIO.input(5): # wait until the button is pressed print "start getting data" break t = 0 first_data = True while True: # Accelerometer data (in meters per second squared): AccX,AccY,AccZ = bno.read_linear_acceleration() # RawX,RawY,RawZ = bno.read_accelerometer() Acc_t = np.array([[AccX, AccY, AccZ]]) # Orientation as a quaternion: Qx,Qy,Qz,Qw = bno.read_quaternion() Quat_t = np.array([[Qw, Qx, Qy, Qz]]) if not first_data: Acc = np.concatenate((Acc,Acc_t), axis = 0) Quat = np.concatenate((Quat,Quat_t), axis = 0) else: Acc = Acc_t Quat = Quat_t first_data = False t = t + 1 time.sleep(1./100) if t > 50 and (not GPIO.input(5)): # Button is pressed print "end getting data" break plt.rcParams['lines.linewidth'] = 0.5 samplePeriod = 1./100 # ################################################ # # this commented out part is for the LSM9DS) # ################################################ # # ACC = imu.rawAccel() # # GYR = imu.rawGyro() # # MAG = imu.rawMag() # # # # accX = ACC[0] * imu.accelscale # # accY = ACC[1] * imu.accelscale # # accZ = ACC[2] * imu.accelscale # # gryX = GYR[0] * imu.gyroscale # # gryY = GYR[1] * imu.gyroscale # # gryZ = GRY[2] * imu.gyroscale # ################################################ accX = Acc[:,0] accY = Acc[:,1] accZ = Acc[:,2] t = len(accX) runtime = np.array([i*samplePeriod for i in xrange(t)]) acc_mag = np.sqrt(accX**2 + accY**2 + accZ**2) ########################## ## butterworth filters ############################ filtCutOff = 0.001 b, a = signal.butter(1,(2*filtCutOff)/(1/samplePeriod), 'high') acc_magFilt = signal.filtfilt(b, a, acc_mag) # % Compute absolute value acc_magFilt = np.absolute(acc_magFilt) # % LP filter accelerometer data filtCutOff = 5. b, a = signal.butter(1, (2*filtCutOff)/(1/samplePeriod), 'low') acc_magFilt = signal.filtfilt(b, a, acc_magFilt) accX_filt = signal.filtfilt(b, a, accX) accY_filt = signal.filtfilt(b, a, accY) accZ_filt = signal.filtfilt(b, a, accZ) accX_drift = np.mean(accX) accY_drift = np.mean(accY) accZ_drift = np.mean(accZ) ############################## # extract acceleration mean , minimize drift ############################## accX_fix_drift = accX_filt - accX_drift accY_fix_drift = accY_filt - accY_drift accZ_fix_drift = accZ_filt - accZ_drift # Threshold detection stationary = [a < 0.07 for a in acc_magFilt] ############################################ # plots used to debug ############################################ # print t # plt.figure('before rotate') # plt.subplot(211) # plt.plot(runtime, accX_filt,'r') # plt.plot(runtime, accY_filt,'g') # plt.plot(runtime, accZ_filt,'b') # plt.title('Filted_acc') # plt.xlabel('runTime (s)') # plt.ylabel('Acceleration (g)') # plt.subplot(212) # plt.plot(runtime, accX, 'r') # plt.plot(runtime, accY, 'g') # plt.plot(runtime, accZ, 'b') # plt.plot(runtime, acc_magFilt, ':k') # plt.title('acc') # plt.plot(runtime, stationary, 'k',) # plt.title('Accelerometer') # plt.xlabel('runTime (s)') # plt.ylabel('Acceleration (g)') # plt.figure('accx') # plt.plot(runtime, accX_filt,'g') # plt.plot(runtime, accX_fix_drift,'r') # plt.figure('accy') # plt.plot(runtime, accY_filt,'g') # plt.plot(runtime, accY_fix_drift,'r') # plt.figure('accz') # plt.plot(runtime, accZ_filt,'g') # plt.plot(runtime, accZ_fix_drift,'r') # quat = np.zeros((t,4)) ########################################################################## # #This commented out part is for LSM9DS0 # ########################################################################## #Filter = AHRS(Kp = 1, KpInit = 1) #initPeriod = 4 #index = int(initPeriod/samplePeriod) # for i in xrange(2000): # Filter.UpdateIMU([0,0,0], # [np.mean(accX[0:index],dtype = np.float64), # np.mean(accY[0:index],dtype = np.float64), # np.mean(accZ[0:index],dtype = np.float64)]) # for i in xrange(t): # if stationary[i]: # Filter.Kp = 0.5 # else: # Filter.Kp = 0 # Filter.UpdateIMU([math.radians(gyrX[i]) ,math.radians(gyrY[i]) ,math.radians(gyrZ[i])],[accX[i], accY[i], accZ[i]]) # quat[i,:] = Filter.Quaternion # print [np.transpose(accX), np.transpose(accY), np.transpose(accZ)] # acc = Filter.quaternRotate(np.concatenate((np.transpose([accX_filt]), np.transpose([accY_filt]), np.transpose([accZ_filt])),axis = 1), Filter.quaternConj(Quat)) # acc = acc * 9.81 # acc[:,[2]] = acc[:,[2]] - 9.81 # acc_d = Filter.quaternRotate(np.concatenate((np.transpose([accX_fix_drift]), np.transpose([accY_fix_drift]), np.transpose([accZ_fix_drift])),axis = 1), Filter.quaternConj(Quat)) # acc[:,[1]] = acc[:,[1]]- np.mean(acc[:,[1]]) # acc[:,[2]] = acc[:,[2]]- np.mean(acc[:,[2]]) # acc[:,[0]] = acc[:,[0]]- np.mean(acc[:,[0]]) ######################################################################## #################################################################### # plt.figure('after rotate with drift') # plt.plot(runtime, acc[:,[0]], 'r') # plt.plot(runtime, acc[:,[1]], 'g') # plt.plot(runtime, acc[:,[2]], 'b') # plt.title('Acceleration') # plt.xlabel('runTime (s)') # plt.ylabel('Acceleration (m/s/s)') ###################################################### ## Using quaternion to do the frame transformation (only for BNO055) ###################################################### l = len(accX) acc_fix_drift = np.concatenate((np.transpose([accX_fix_drift]), np.transpose([accY_fix_drift]), np.transpose([accZ_fix_drift])),axis = 1) for i in xrange(l): # acc = quaternRot([Gx,Gy,Gz], quaternInv([qw,qx,qy,qz])) # print acc_fix_drift[i,:],quaternInv(Quat[i,:]) acc_d_t = quaternRot(acc_fix_drift[i,:],quaternInv(Quat[i,:])) if i == 0: acc_d = acc_d_t else: acc_d = np.concatenate((acc_d,acc_d_t),axis = 0) # print acc_d.shape, acc.shape # plt.figure('after rotate without drift') # plt.plot(runtime, acc_d[:,[0]], 'r') # plt.plot(runtime, acc_d[:,[1]], 'g') # plt.plot(runtime, acc_d[:,[2]], 'b') # plt.title('Acceleration') # plt.xlabel('runTime (s)') # plt.ylabel('Acceleration (m/s/s)') # # plt.show() vel = np.zeros(acc_d.shape) t_total = len(vel) - 2 for a in xrange(t_total): i = a + 1 vel[[i],:] = vel[[i-1],:] + acc_d[[i],:] * samplePeriod if stationary[i]: vel[[i],:] = [[0,0,0]] ##################################################################### ## calculate velocity drift ##################################################################### velDrift = np.zeros(vel.shape) stationary_temp = [0] + np.diff(1 * np.array(stationary)) stationaryStart = [i for i,x in enumerate(stationary_temp) if x == -1] stationaryEnd = [i for i,x in enumerate(stationary_temp) if x == 1] # print stationaryEnd, '\n', stationaryStart if len(stationaryEnd) > len(stationaryStart): if len(stationaryEnd) > 1: stationaryEnd = stationaryEnd[1:] else: stationaryEnd = [] if len(stationaryStart) > len(stationaryEnd): stationaryEnd = stationaryEnd.append(t_total) if stationaryStart == [] : stationaryStart = [10] if stationaryEnd == [] or stationaryEnd == None: stationaryEnd = [t_total] for i in xrange(len(stationaryEnd)): # print stationaryStart, '\n', stationaryEnd # print stationaryEnd[i] driftRate = vel[stationaryEnd[i]-1,:] / (stationaryEnd[i]- stationaryStart[i]) enum = range(0,(stationaryEnd[i] - stationaryStart[i])) drift = np.concatenate((np.transpose([enum])*driftRate[0], np.transpose([enum])*driftRate[1], np.transpose([enum])*driftRate[2]),axis = 1) velDrift[stationaryStart[i]:stationaryEnd[i],:] = drift vel = vel-velDrift # plt.figure('velocity') # plt.plot(runtime, vel[:,0], 'r') # plt.plot(runtime, vel[:,1], 'g') # plt.plot(runtime, vel[:,2], 'b') # plt.title('Velocity') # plt.xlabel('runTime (s)') # plt.ylabel('Velocity (m/s)') # plt.show() pos = np.zeros(vel.shape) for a in xrange(t_total): i = a + 1 pos[[i],:] = pos[[i-1],:] + vel[[i],:] * samplePeriod pos_x_min = np.min(pos[:,0]) pos_x_max = np.max(pos[:,0]) pos_y_min = np.min(pos[:,1]) pos_y_max = np.max(pos[:,1]) pos_z_min = np.min(pos[:,2]) pos_z_max = np.max(pos[:,2]) max_range = np.max([pos_x_max,pos_y_max,pos_z_max]) min_range = np.min([pos_x_min,pos_y_min,pos_z_min]) axis_range = max_range - min_range max_range = max_range + 0.1 * axis_range min_range = min_range - 0.1 * axis_range l = len(pos[:,0]) pos = pos[0:l-1,:] runtime = runtime[:l-1] # plt.figure('position') # plt.plot(runtime, pos[:,0], 'r') # plt.plot(runtime, pos[:,1], 'g') # plt.plot(runtime, pos[:,2], 'b') # plt.title('Position') # plt.xlabel('runTime (s)') # plt.ylabel('Position (m)') # plt.figure('trace') np.savetxt('position_data' ,pos, delimiter=',') # # print pos[:,0] # plt.plot(pos[:,0],pos[:,1],'r') # # plt.plot([pos[0,0],pos[l-2,0]],[pos[0,1],pos[l-2,1]],'w') # plt.axis([min_range, max_range, min_range, max_range]) # plt.title('movement') # plt.show() return pos #draw_shape(pos) |
draw.py
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 | import pygame import os import numpy import main from main import position os.putenv('SDL_VIDEODRIVER','fbcon') os.putenv('SDL_FBDEV', '/dev/fb1') os.putenv('SDL_MOUSEDRV', 'TSLIB') os.putenv('SDL_MOUSEDEV', '/dev/input/touchscreen') pygame.init() pygame.mouse.set_visible(False) size = width, height = 320, 240 screen = pygame.display.set_mode(size) screen.fill((0,0,0)) def draw_shape(points): points = numpy.array(points)[:,[0,1]] ranges = numpy.ptp(points, axis=0) x_scale = 260/ranges[0] y_scale = 200/ranges[1] print ranges[0] print ranges[1] scale = min(x_scale,y_scale) points[:,0] = points[:,0]*scale points[:,1] = points[:,1]*scale means = numpy.mean(points,axis=0) points[:,0] = points[:,0] + (160-means[0]) points[:,1] = points[:,1] + (120-means[1]) ranges = numpy.ptp(points, axis=0) print ranges[0] print ranges[1] try: pygame.draw.lines(screen, (255,255,255), False, points, 2) #pygame.draw.circle(screen,(255,255,255),[60,250],40) pygame.display.flip() #print "successful" except KeyboardInterrupt: print "stop showing the record." if __name__ == "__main__": points = [] for i in xrange(3): points.append([]) for j in xrange(3): points[i].append(i+j) print points draw_shape(points) |
References
[1] Tedaldi, David, Alberto Pretto, and Emanuele Menegatti. "A robust and easy to implement method for IMU calibration without external equipments." 2014 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2014.
[2] Syed, Z. F., et al. "A new multi-position calibration method for MEMS inertial navigation systems." Measurement Science and Technology 18.7 (2007): 1897.
[3] Brooks, Richard R., and Sundararaja S. Iyengar. Multi-sensor fusion: fundamentals and applications with software. Prentice-Hall, Inc., 1998.
[4] Milosevic, Bojan, et al. "A SmartPen for 3D interaction and sketch-based surface modeling." The International Journal of Advanced Manufacturing Technology 84.5-8 (2016): 1625-1645.
[5] Zhi, Ruoyu. "A Drift Eliminated Attitude & Position Estimation Algorithm In 3D." (2016).
[6] BNO055 library https://github.com/adafruit/Adafruit_Python_BNO055
[7] LSM9DS0 library https://github.com/jackweath/Adafruit_LSM9DS0
Budget & Contributions
LSM9DS0: $26.49(1), BNO055: $35.95(1), Total Cost:$62.44
Ziqi Yang: Circuit Design; Sensor Fusion; Sensor Calibration; Movement Restoration; Website Design
Haowen Tao: Pygame Display; Website Design
Contact
Ziqi Yang (zy259@cornell.edu), Haowen Tao (ht398@cornell.edu)
We'd like to thank for all the support from ECE5725 staff, including Professor Skovira and all the TAs! Thank you!