├── plots └── error.png ├── README.md ├── kalmanLSTM.py ├── trajectory.py ├── KalmanSensorStateProc.py ├── rawSensorStateProc.py ├── preProcessData.py ├── kalmaRNN.py └── initializeLatentStates.py /plots/error.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/noahjepstein/Kalman-RNN/HEAD/plots/error.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Emulating nonlinear Kalman filtering on 9-axis IMU data from an Android device with a recurrent neural network. 2 | 3 | Technical paper [can be found here.](https://goo.gl/VGBDrh "indoor-inertial-localization.pdf") 4 | -------------------------------------------------------------------------------- /kalmanLSTM.py: -------------------------------------------------------------------------------- 1 | import tensorflow as tf 2 | import numpy as np 3 | from tensorflow.models.rnn import rnn 4 | from tensorflow.models.rnn import rnn_cell 5 | # from tensorflow.models.rnn.rnn_cell import BasicLSTMCell 6 | 7 | pHolder = tf.placeholder(tf.float32, [None, 9]) 8 | Weights = tf.Variable(tf.zeros([6,])) -------------------------------------------------------------------------------- /trajectory.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | import os 4 | import pickle 5 | import thLib 6 | import pykalman 7 | 8 | class Trajectory: 9 | 10 | def __init__(self, length = 0, forTraining = True 11 | mag = None, accel = None, gyro = None, waypoints = None, 12 | rawPos = None, rawOrient = None, 13 | KalmanPos = None, KalmanOrient = None): 14 | 15 | self.length = length 16 | self.forTraining = forTraining 17 | self.mag = mag 18 | self.accel = accel 19 | self.gyro = gyro 20 | self.waypoints = waypoints 21 | self.rawPos = rawPos 22 | self.rawOrient = rawOrient 23 | self.KalmanPos = KalmanPos 24 | self.KalmanOrient = KalmanOrient 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /KalmanSensorStateProc.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import thLib.imus as IMU 3 | import rawSensorStateProc 4 | import math 5 | 6 | SAMPLE_RATE = 10 # hz 7 | 8 | # returns N x 3 ndarray 9 | # N is the number of samples in the trajectory 10 | # X Y and Z position in 3-space at each sample with respect to start position 11 | def getKalmanXYZ(traj): 12 | 13 | # stub -- doesn't do kalman filtering right now (thus is suceptible to 14 | # some severe integral drift) 15 | 16 | return rawSensorStateProc.getRawXYZ(traj) 17 | 18 | 19 | # returns [N, 4] ndarray 20 | # N is the number of samples in the trajectory 21 | # gives a quaternion for each sample that describes the orientation 22 | # of the sensor in 3-space with respect to the starting position 23 | def getKalmanOrientation(traj): 24 | 25 | # stub -- kalman filtration for orientation finding will be impelemented 26 | # but for now just uses raw orientation calculations 27 | 28 | quatArr = IMU.kalman_quat(SAMPLE_RATE, traj['accel'], traj['gyro'], traj['mag']) 29 | 30 | return quatArr 31 | -------------------------------------------------------------------------------- /rawSensorStateProc.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import thLib.imus as IMU 3 | import pykalman 4 | import math 5 | import thLib.rotmat as rotateMatrix 6 | 7 | SAMPLE_RATE = 10 # hz 8 | 9 | # takes trajectory dictionary object. to be replaced with trajectory 10 | # class object at a later date. 11 | # return Nx3 array of raw position estimates w.r.t starting null position 12 | def getRawXYZ(traj): 13 | 14 | quats, positions = IMU.calc_QPos(traj['initOrient'], traj['gyro'], np.array([0,0,0]), traj['accel'], SAMPLE_RATE) 15 | 16 | return positions 17 | 18 | 19 | 20 | # takes trajectory dictionary object. to be replaced w/ traj class obj 21 | # returns Nx4 array of raw orientation estimates (given as a quaternion w.r.t. origin) 22 | def getRawOrientation(traj): 23 | 24 | quats, positions = IMU.calc_QPos(traj['initOrient'], traj['gyro'], np.array([0,0,0]), traj['accel'], SAMPLE_RATE) 25 | 26 | return quats 27 | 28 | # takes orData, an Nx3 ndarray of orientation values about 29 | # X, Y, and Z axes in degrees, respective to short dimension of array. 30 | # converts values to angular velocity in deg/s. 31 | # outputs an Nx3 ndarray of angular velocity values about 32 | # X, Y, Z axes respective to the short dimension of the array. 33 | def orientationToGyro(orData): 34 | 35 | gyroData = [] 36 | 37 | for i, sample in enumerate(orData): 38 | if i == 0: 39 | gyroData.append([0,0,0]) 40 | else: 41 | gyroData.append([(i - j) / float(0.1) for i, j in zip(orData[i], orData[i - 1])]) 42 | 43 | return gyroData 44 | 45 | 46 | # PARAMS: initOrientation -- [1x3] vector that specifies orinetation 47 | # in degrees in X, Y, and Z directions, respectively. 48 | # RETURNS: 3x3 matrix that specifies the sensor's initial rotation 49 | # with respect to the earth's coordinate frame 50 | def calcInitialOrientation(initOrientation): 51 | 52 | Z = rotateMatrix.R3(initOrientation[2]) 53 | Y = rotateMatrix.R2(initOrientation[1]) 54 | X = rotateMatrix.R1(initOrientation[0]) 55 | 56 | return np.dot(np.dot(Z, Y), X) 57 | 58 | -------------------------------------------------------------------------------- /preProcessData.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | import os 4 | import pickle 5 | import glob 6 | import rawSensorStateProc 7 | # from trajectory import Trajectory 8 | 9 | WAYPOINTS_ELEMS_PER_LINE = 6 10 | UJILocDataFile = os.getcwd() + '/data/lines/c1/l1n_01.txt' 11 | 12 | def loadTrajectoryData(inFile = UJILocDataFile): 13 | 14 | with open(UJILocDataFile, 'r') as dataFile: 15 | data = dataFile.read() 16 | 17 | # 9-axis IMU data 18 | # trajectory: dictionary with three elements 19 | # N is number of samples in the trajectory (data taken at 10Hz) 20 | # mag: Nx3 numpy array where each line has XYZ mag data 21 | # gyro: Nx3 numpy array where each line has XYZ gyro vel data 22 | # accel: Nx3 numpy array where each line has XYZ lin accelerometer data 23 | segments = data.split("<", 2) 24 | IMUDataStr = segments[0].split('\n')[:-1] 25 | magArr = [] 26 | oriArr = [] 27 | accelArr = [] 28 | 29 | for i, lineStr in enumerate(IMUDataStr): 30 | 31 | lineStr = lineStr.split(' ', 10)[:-1] 32 | lineStr = [float(x) for x in lineStr] 33 | magArr.append(lineStr[1:4]) # xyz mag data for sample 34 | accelArr.append(lineStr[4:7]) # xyz accelerometer data for single samp 35 | oriArr.append(lineStr[7:10]) # xyz gyro data for sample 36 | 37 | # values initially are given as euler angles which are not good for imu-type calculations. 38 | # so we fix em! 39 | gyroArr = rawSensorStateProc.orientationToGyro(oriArr) 40 | initOrientationMatrix = rawSensorStateProc.calcInitialOrientation(oriArr[0]) 41 | 42 | # IMUData = [{'mag': magArr, 'gyro': gyroArr, 'accel': accelArr}] 43 | 44 | # process waypoint data 45 | # each waypoint consists of a latitude coordinate, longitude coordinate, 46 | # and index (what IMU dataopoint it represents) 47 | waypoints = [] 48 | waypointStr = segments[1].split(">", 2) 49 | numWaypoints = int(waypointStr[0]) 50 | waypointLns = waypointStr[1].lstrip().split('\n') 51 | 52 | for i, lineStr in enumerate(waypointLns): 53 | 54 | line = lineStr.split(' ', WAYPOINTS_ELEMS_PER_LINE) 55 | line = [float(x) for x in line] 56 | 57 | if i == 0: 58 | waypoints.append({'lat': line[0], 'long': line[1], 'index': line[4]}) 59 | 60 | waypoints.append({'lat': line[2], 'long': line[3], 'index': line[5]}) 61 | 62 | seqLen = line[5] 63 | 64 | 65 | traj = ({'waypoints': np.array(waypoints), 'mag': np.array(magArr), 'gyro': np.array(gyroArr), 66 | 'accel': np.array(accelArr), 'orientSensed': np.array(oriArr), 67 | 'initOrient': initOrientationMatrix, 'seqLen': seqLen}) 68 | 69 | return traj 70 | 71 | # loadTrajectoryData() -------------------------------------------------------------------------------- /kalmaRNN.py: -------------------------------------------------------------------------------- 1 | # kalmaRNN.py 2 | # Mimialistic RNN implementation with single hidden layer 3 | # Performs logistic regression on an IMU-data timeseries 4 | # Predicts kalman-filtered orientation (quaternion-representation!) 5 | # 6 | # This is a modified theano RNN implementation!! 7 | # Original version by T. Ramalho 8 | # https://github.com/tmramalho 9 | 10 | # Modified by Noah J Epstein 11 | # Modified Date: 5/3/2016 12 | # noahjepstein@gmail.com 13 | 14 | import theano 15 | import theano.tensor as T 16 | import theano.printing as tPrint 17 | import numpy as np 18 | import cPickle 19 | import random 20 | import matplotlib.pyplot as plt 21 | from sklearn.metrics import mean_squared_error 22 | 23 | class RNN(object): 24 | 25 | def __init__(self, nin, n_hidden, nout): 26 | 27 | rng = np.random.RandomState(1234) 28 | 29 | W_uh = np.asarray( 30 | rng.normal(size=(nin, n_hidden), scale= .01, loc = .0), dtype = theano.config.floatX) 31 | 32 | W_hh = np.asarray( 33 | rng.normal(size=(n_hidden, n_hidden), scale=.01, loc = .0), dtype = theano.config.floatX) 34 | 35 | W_hy = np.asarray( 36 | rng.normal(size=(n_hidden, nout), scale =.01, loc=0.0), dtype = theano.config.floatX) 37 | 38 | b_hh = np.zeros((n_hidden,), dtype=theano.config.floatX) 39 | b_hy = np.zeros((nout,), dtype=theano.config.floatX) 40 | 41 | self.activate = T.nnet.sigmoid 42 | 43 | lr = T.scalar() 44 | u = T.matrix() 45 | t = T.matrix() 46 | 47 | W_uh = theano.shared(W_uh, 'W_uh') 48 | W_hh = theano.shared(W_hh, 'W_hh') 49 | W_hy = theano.shared(W_hy, 'W_hy') 50 | b_hh = theano.shared(b_hh, 'b_hh') 51 | b_hy = theano.shared(b_hy, 'b_hy') 52 | 53 | h0_tm1 = theano.shared(np.zeros(n_hidden, dtype=theano.config.floatX)) 54 | 55 | h, _ = theano.scan(self.recurrentLoop, sequences = u, 56 | outputs_info = [h0_tm1], 57 | non_sequences = [W_hh, W_uh, W_hy, b_hh]) 58 | 59 | # calculates output based on 60 | y = T.dot(h[-1], W_hy) + b_hy 61 | 62 | # cost function is just mean squared error at the outputs_info 63 | cost = ((t - y)**2).mean(axis=0).sum() 64 | 65 | # calculate gradients across weights and biases wrt cost 66 | gW_hh, gW_uh, gW_hy, gb_hh, gb_hy = T.grad(cost, [W_hh, W_uh, W_hy, b_hh, b_hy]) 67 | 68 | 69 | self.trainModel = theano.function(inputs = [u, t, lr], outputs = cost, 70 | on_unused_input='warn', 71 | updates=[ (W_hh, W_hh - lr * gW_hh), 72 | (W_uh, W_uh - lr * gW_uh), 73 | (W_hy, W_hy - lr * gW_hy), 74 | (b_hh, b_hh - lr * gb_hh), 75 | (b_hy, b_hy - lr * gb_hy)], 76 | allow_input_downcast=True) 77 | 78 | self.test = theano.function(inputs = [u], outputs = cost, 79 | on_unused_input = 'warn', 80 | allow_input_downcast = True) 81 | 82 | 83 | # update 84 | def recurrentLoop(self, u_t, h_tm1, W_hh, W_uh, W_hy, b_hh): 85 | h_t = self.activate(T.dot(h_tm1, W_hh) + T.dot(u_t, W_uh) + b_hh) 86 | return h_t 87 | 88 | ############################################################################### 89 | ############################## JUST FOR RUNNING MODEL ######################### 90 | ############################################################################### 91 | 92 | # so we can run as a library too 93 | if __name__ == '__main__': 94 | 95 | RNNModel = RNN(9, 30, 4) 96 | lr = 0.001 97 | e = 1 98 | errorList = [] 99 | minSeqLen = 100 100 | 101 | # unpickle training data 102 | trajectories = cPickle.load(open('trainingBin.pickle', 'rb')) 103 | nTraj = len(trajectories) 104 | print nTraj 105 | 106 | print "training time!" 107 | 108 | testList = cPickle.load(open('testingBin.pickle', 'rb')) 109 | testCosts = [] 110 | 111 | for i in xrange(int(2)): 112 | 113 | for j, traj in enumerate(trajectories): 114 | 115 | # trains on each trajectory truncated to length 100 116 | # so we don't have to deal with time series of differing lengths 117 | u = traj['nnInput'][:100,:] 118 | t = traj['KalmanOrientEst'][:100,:] 119 | c = RNNModel.trainModel(u, t, lr) 120 | 121 | print "iteration {0}: {1}".format(i * nTraj + j, np.sqrt(c)) 122 | 123 | if j % 20 == 0: 124 | e = 0.1*np.sqrt(c) 125 | errorList.append(e) 126 | 127 | for traj in testList: 128 | 129 | cost = RNNModel.test(traj['nnInput']) 130 | err = 0.1 * np.sqrt(err) 131 | testCosts.append(err) 132 | 133 | 134 | plt.plot(errorList) 135 | plt.savefig('plots/error.png') 136 | 137 | plt.plot(testCosts) 138 | plt.savefig('plots/testCosts.png') 139 | 140 | # sys.setrecursionlimit(3000) # bc the nn is highly recurisve 141 | # cPickle.dump(rnn, open('trainedModel.pickle', 'wb'), protocol=cPickle.HIGHEST_PROTOCOL) 142 | 143 | 144 | 145 | 146 | -------------------------------------------------------------------------------- /initializeLatentStates.py: -------------------------------------------------------------------------------- 1 | # initializeLatentStates.py 2 | # Author: Noah J Epstein 3 | # noahjepstein@gmail.com 4 | # Modified: 5/5/2016 5 | # 6 | # > Processes 9-axis IMU trajectory data 7 | # to give inital estimates and kalman filtered estimates for 8 | # walking trajectories through hallways in the UJILoc-Mag dataset. 9 | # 10 | # > Adds new duplicate trajectories that are perturbed with noise 11 | # so that the dataset size is expanded and more robust due to 12 | # additional noise inclusion. 13 | # 14 | # > Saves data in pickled format so that it can easily be used 15 | # in batch format for training a recurrent neural network. 16 | # (See readme or kalmaRNN.py for details) 17 | # 18 | 19 | import numpy as np 20 | import cPickle 21 | import os 22 | import glob 23 | from pykalman import KalmanFilter 24 | from preProcessData import loadTrajectoryData 25 | import rawSensorStateProc 26 | import KalmanSensorStateProc 27 | 28 | TRAINING_DATAFILE = 'trainingBin.pickle' 29 | TESTING_DATAFILE = 'testingBin.pickle' 30 | IMU_NUM_AXES = 9 31 | 32 | trajFileDirTrain = os.getcwd() + '/data/*/*/*.txt' 33 | trajFileDirTest = os.getcwd() + '/data/*/*.txt' 34 | trajectoryFileList = np.array(glob.glob(trajFileDirTrain)) 35 | trajectoryFileList = np.append(trajectoryFileList, glob.glob(trajFileDirTest)) 36 | 37 | trajectoryData = [] # list of trajectories 38 | trajectoryDataTest = [] # just the ones for testing 39 | 40 | minSeqLen = 10000000 41 | 42 | 43 | # arguments: trajectory object with imu, position, orientation, metadata 44 | # RETURNS: same trajectory with the imu data perturbed by gaussian noise 45 | def addNoise(traj, noiseType): 46 | 47 | nMult = np.random.uniform(low = 0.0, high = 0.1) 48 | 49 | # for each measurement axis sequence, we perturb the values 50 | # based on the standard deviation of each measurement in the 51 | # range of samples 52 | 53 | for i in range(IMU_NUM_AXES): 54 | 55 | 56 | # add zero mean gaussian noise in proportion with the standard deviation 57 | # on that measurement axis 58 | 59 | if noiseType == "gaussian": 60 | noise = np.random.normal(loc = 0.0, 61 | scale = nMult * np.std(traj['nnInput'][:, i]), 62 | size = traj['nnInput'][:, i].shape ) 63 | elif noiseType == 'laplace': 64 | 65 | noise = np.random.laplace(loc = 0.0, 66 | scale = nMult * np.std(traj['nnInput'][:, i]), 67 | size = traj['nnInput'][:, i].shape ) 68 | 69 | elif noiseType == 'multi': 70 | 71 | noise = np.random.normal(loc = 0.0, 72 | scale = nMult * np.std(traj['nnInput'][:, i]), 73 | size = traj['nnInput'][:, i].shape ) 74 | 75 | noise = np.add( 76 | np.random.laplace(loc = 0.0, 77 | scale = nMult * np.std(traj['nnInput'][:, i]), 78 | size = traj['nnInput'][:, i].shape ), 79 | noise) 80 | else: 81 | noise = np.zeros_like(traj['nnInput'][:, i]) 82 | 83 | 84 | traj['nnInput'][:,i] = np.add(traj['nnInput'][:,i], noise) 85 | 86 | return traj 87 | 88 | 89 | ################################################################################## 90 | ############################## estimation stuff starts here ###################### 91 | ################################################################################## 92 | 93 | nTraj = 0 94 | 95 | for trajFilename in trajectoryFileList: 96 | 97 | traj = loadTrajectoryData(trajFilename) 98 | print trajFilename 99 | 100 | # For the LORD will pass through to smite the Egyptians; and when He sees 101 | # the blood on the lintel and on the two doorposts, the LORD will pass over 102 | # the door and will not allow the destroyer to come in to your houses to smite you. 103 | if '/tests/' in trajFilename: 104 | traj['testFile'] = True 105 | else: 106 | traj['testFile'] = False 107 | 108 | traj['rawXYZ'] = rawSensorStateProc.getRawXYZ(traj) 109 | traj['rawOrientEst'] = rawSensorStateProc.getRawOrientation(traj) 110 | traj['KalmanXYZ'] = KalmanSensorStateProc.getKalmanXYZ(traj) 111 | traj['KalmanOrientEst'] = KalmanSensorStateProc.getKalmanOrientation(traj) 112 | 113 | 114 | 115 | imuRaw = [] 116 | 117 | # setup plain NN input vectors for RNN training 118 | for i, magSamp in enumerate(traj['mag']): 119 | 120 | sample = np.concatenate((magSamp, traj['gyro'][i], traj['accel'][i])) 121 | imuRaw.append(sample) 122 | 123 | imuRaw = np.array(imuRaw) 124 | traj['nnInput'] = imuRaw 125 | 126 | 127 | if traj['testFile']: 128 | trajectoryDataTest.append(traj) 129 | else: 130 | trajectoryData.append(traj) 131 | 132 | nTraj += 1 133 | 134 | # add 20 noisy input sequences to increase the size of the training set 135 | # and add variability to the training set ot make it more robust 136 | # just gaussian noise for now, but could add numerous distributions 137 | # to create additional training data 138 | for i in range(60): 139 | if traj['testFile']: 140 | trajectoryDataTest.append(addNoise(traj, 'gaussian')) 141 | else: 142 | trajectoryData.append(addNoise(traj, 'gaussian')) 143 | nTraj += 1 144 | 145 | print nTraj 146 | 147 | # save to avoid doing processing again 148 | cPickle.dump(trajectoryData, open(TRAINING_DATAFILE, 'wb'), protocol=cPickle.HIGHEST_PROTOCOL) 149 | cPickle.dump(trajectoryDataTest, open(TESTING_DATAFILE, 'wb'), protocol=cPickle.HIGHEST_PROTOCOL) --------------------------------------------------------------------------------