├── .gitignore ├── helperMethods.py ├── kalmanFilter.py ├── pos_final.json ├── readme.md └── sensorFusion.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | -------------------------------------------------------------------------------- /helperMethods.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | 4 | EARTH_RADIUS = 6371 * 1000.0 # meters 5 | 6 | class helperMethods(object): 7 | ''' 8 | HelperMethods class has all fucntions that help in calculations 9 | with handling GPS data and doing operations like conversion from 10 | Radians to Degrees (and vice-versa) or computing distance between 11 | GPS points. 12 | ''' 13 | def __init__(self): 14 | pass 15 | 16 | def latToMtrs(self, latitude): 17 | ''' 18 | Converts Latitude to Meters 19 | ''' 20 | distance = self.getDistMtrs(latitude, 0.0, 0.0, 0.0) 21 | if(distance < 0): 22 | distance *= -1 23 | 24 | return distance 25 | 26 | def lonToMtrs(self, longitude): 27 | ''' 28 | Converts Longitiude to Meters 29 | ''' 30 | distance = self.getDistMtrs(0.0, longitude, 0.0, 0.0) 31 | if(longitude < 0): 32 | distance *= -1 33 | 34 | return distance 35 | 36 | def degToRad(self, latOrLon): 37 | ''' 38 | Converts Degrees to Radians 39 | ''' 40 | 41 | return (latOrLon * np.pi) / 180.0 42 | 43 | def radToDeg(self, latOrLon): 44 | ''' 45 | Converts radians to degrees 46 | ''' 47 | 48 | return (latOrLon * 180.0) / np.pi 49 | 50 | def getDistMtrs(self, lat_from, lon_from, lat_to, lon_to): 51 | ''' 52 | Get distance between two GPS points 53 | ''' 54 | deltaLon = self.degToRad(lon_to - lon_from) 55 | deltaLat = self.degToRad(lat_to - lat_from) 56 | 57 | a = np.power(np.sin(deltaLat/2.0), 2) + \ 58 | np.cos(self.degToRad(lat_from)) * np.cos(self.degToRad(lat_to)) * \ 59 | np.power(np.sin(deltaLon/2.0), 2) 60 | c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1.0 - a)) 61 | 62 | return EARTH_RADIUS * c 63 | 64 | def getPointAhead(self, lat_from, lon_from, distMtrs, azimuth): 65 | ''' 66 | Helper function for mtrsToGeopoint 67 | ''' 68 | radiusFraction = distMtrs / EARTH_RADIUS 69 | bearing = self.degToRad(azimuth) 70 | lat1 = self.degToRad(lat_from) 71 | lon1 = self.degToRad(lon_from) 72 | 73 | lat2_part1 = np.sin(lat1) * np.cos(radiusFraction) 74 | lat2_part2 = np.cos(lat1) * np.sin(radiusFraction) * np.cos(bearing) 75 | 76 | lat2 = np.arcsin(lat2_part1 + lat2_part2) 77 | 78 | lon2_part1 = np.sin(bearing) * np.sin(radiusFraction) * np.cos(lat1) 79 | lon2_part2 = np.cos(radiusFraction) - (np.sin(lat1) * np.sin(lat2)) 80 | 81 | lon2 = lon1 + np.arctan2(lon2_part1, lon2_part2) 82 | lon2 = np.mod((lon2 + 3 * np.pi), (2 * np.pi)) - np.pi 83 | 84 | return self.radToDeg(lat2), self.radToDeg(lon2) 85 | 86 | 87 | def mtrsToGeopoint(self, latAsMtrs, lonAsMtrs): 88 | ''' 89 | Conversion between GPS points to meters 90 | ''' 91 | lat_tmp, lon_tmp = self.getPointAhead(0.0, 0.0, lonAsMtrs, 90.0) 92 | lat_ret, lon_ret = self.getPointAhead(lat_tmp, lon_tmp, latAsMtrs, 0.0) 93 | 94 | return lat_ret, lon_ret 95 | 96 | -------------------------------------------------------------------------------- /kalmanFilter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | from helperMethods import helperMethods 4 | 5 | class kalmanFilter(helperMethods): 6 | ''' 7 | Kalman Filter class fuses the data from GPS and IMU. 8 | The predict and update functions play the most vital role. 9 | ''' 10 | def __init__(self, initPos, initVel, posStdDev, accStdDev, currTime): 11 | helperMethods.__init__(self) 12 | # set these values from the arguments received 13 | # current state 14 | self.X = np.array([[np.float64(initPos)], [np.float64(initVel)]]) 15 | # Identity matrix 16 | self.I = np.identity(2) 17 | # Initial guess for covariance 18 | self.P = np.identity(2) 19 | # transformation matrix for input data 20 | self.H = np.identity(2) 21 | # process (accelerometer) error variance 22 | self.Q = np.array([[accStdDev * accStdDev, 0], [0, accStdDev * accStdDev]]) 23 | # measurement (GPS) error variance 24 | self.R = np.array([[posStdDev * posStdDev, 0], [0, posStdDev * posStdDev]]) 25 | # current time 26 | self.currStateTime = currTime 27 | # self.A = defined in predict 28 | # self.B = defined in predict 29 | # self.u = defined in predict 30 | # self.z = defined in update 31 | 32 | # main functions 33 | def predict(self, accThisAxis, timeNow): 34 | ''' 35 | Predict function perform the initial matrix multiplications. 36 | Objective is to predict current state and compute P matrix. 37 | ''' 38 | deltaT = timeNow - self.currStateTime 39 | self.B = np.array([[0.5 * deltaT * deltaT], [deltaT]]) 40 | self.A = np.array([[1.0, deltaT], [0.0, 1.0]]) 41 | self.u = np.array([[accThisAxis]]) 42 | 43 | self.X = np.add(np.matmul(self.A, self.X), np.matmul(self.B, self.u)) 44 | self.P = np.add(np.matmul(np.matmul(self.A, self.P), np.transpose(self.A)), self.Q) 45 | self.currStateTime = timeNow 46 | 47 | def update(self, pos, velThisAxis, posError, velError): 48 | ''' 49 | Update function performs the update when the GPS data has been 50 | received. 51 | ''' 52 | self.z = np.array([[pos], [velThisAxis]]) 53 | if(not posError): 54 | self.R[0, 0] = posError * posError 55 | else: 56 | self.R[1, 1] = velError * velError 57 | y = np.subtract(self.z, self.X) 58 | s = np.add(self.P, self.R) 59 | try: 60 | sInverse = np.linalg.inv(s) 61 | except np.linalg.LinAlgError: 62 | print("Matrix is not invertible") 63 | pass 64 | else: 65 | K = np.matmul(self.P, sInverse) 66 | self.X = np.add(self.X, np.matmul(K, y)) 67 | self.P = np.matmul(np.subtract(self.I, K), self.P) 68 | 69 | def getPredictedPos(self): 70 | ''' 71 | Returns predicted position in that axis. 72 | ''' 73 | 74 | return self.X[0, 0] 75 | 76 | def getPredictedVel(self): 77 | ''' 78 | Returns predicted velocity in that axis. 79 | ''' 80 | 81 | return self.X[1, 0] 82 | 83 | 84 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # Sensor Fusion 2 | 3 | ## Overview 4 | 5 | This is a python implementation of sensor fusion of GPS and IMU data. 6 | 7 | Major Credits: 8 | [Scott Lobdell](https://github.com/slobdell) 9 | I watched Scott's videos ([video1](https://www.youtube.com/watch?v=T9jXoG0QYIA&t=573s) and [video2](https://www.youtube.com/watch?v=6M6wSLD-8M8&t=687s)) over and over again and learnt a lot. 10 | His original implementation is in Golang, found [here](https://github.com/slobdell/kalman-filter-example) and a [blog post](http://scottlobdell.me/2017/01/gps-accelerometer-sensor-fusion-kalman-filter-practical-walkthrough/) covering the details. 11 | 12 | Do I owe him a beer, now? Yes! 13 | 14 | ## Run instructions 15 | ``` 16 | git clone https://github.com/smahajan07/sensor-fusion.git 17 | cd sensor-fusion 18 | python sensorFusion.py 19 | ``` 20 | ## TODO: 21 | Add dependencies 22 | -------------------------------------------------------------------------------- /sensorFusion.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import json 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | from kalmanFilter import kalmanFilter 6 | from helperMethods import helperMethods 7 | 8 | # setting some constants 9 | ACTUAL_GRAVITY = 9.80665 10 | 11 | # open json file with collected data 12 | file_name = 'pos_final.json' 13 | with open(file_name) as data_file: 14 | data = json.load(data_file) 15 | # read initial data 16 | initialData = data[0] 17 | 18 | # set standard deviations 19 | latLonStdDev = 2.0 20 | altStdDev = 3.518522417151836 21 | accEastStdDev = ACTUAL_GRAVITY * 0.033436506994600976 22 | accNorthStdDev = ACTUAL_GRAVITY * 0.05355371135598354 23 | accUpStdDev = ACTUAL_GRAVITY * 0.2088683796078286 24 | 25 | # create object of helper class 26 | helperObj = helperMethods() 27 | 28 | # create objects of kalman filter 29 | objEast = kalmanFilter(helperObj.lonToMtrs(initialData["gps_lon"]), \ 30 | initialData["vel_east"], latLonStdDev, \ 31 | accEastStdDev, initialData["timestamp"]) 32 | 33 | objNorth = kalmanFilter(helperObj.latToMtrs(initialData["gps_lat"]), \ 34 | initialData["vel_north"], latLonStdDev, \ 35 | accNorthStdDev, initialData["timestamp"]) 36 | 37 | objUp = kalmanFilter(initialData["gps_alt"], \ 38 | initialData["vel_down"] * -1.0, latLonStdDev, \ 39 | accUpStdDev, initialData["timestamp"]) 40 | 41 | # lists for collecting final points to plot 42 | pointsToPlotLat = [] 43 | pointsToPlotLon = [] 44 | 45 | # lists for plotting original data points 46 | orgLat = [] 47 | orgLon = [] 48 | 49 | # run loop over new readings 50 | for i in range(1,len(data)): #len(data) 51 | currData = data[i] 52 | 53 | # call the predict function for all objects 54 | # (since we already have the first reading, we call call predict) 55 | objEast.predict(currData["abs_east_acc"] * ACTUAL_GRAVITY, 56 | currData["timestamp"]) 57 | objNorth.predict(currData["abs_north_acc"] * ACTUAL_GRAVITY, 58 | currData["timestamp"]) 59 | objUp.predict(currData["abs_up_acc"] * ACTUAL_GRAVITY, 60 | currData["timestamp"]) 61 | 62 | # if GPS data is not zero, proceed 63 | if(currData["gps_lat"] != 0.0): 64 | 65 | defPosErr = 0.0 66 | 67 | # call the update function for all objects 68 | vEast = currData["vel_east"] 69 | longitude = objEast.lonToMtrs(currData["gps_lon"]) 70 | objEast.update(longitude, vEast, defPosErr, currData["vel_error"]) 71 | 72 | vNorth = currData["vel_north"] 73 | latitude = objNorth.latToMtrs(currData["gps_lat"]) 74 | objNorth.update(latitude, vNorth, defPosErr, currData["vel_error"]) 75 | 76 | vUp = currData["vel_down"] * -1.0 77 | objUp.update(currData["gps_alt"], vUp, currData["altitude_error"], 78 | currData["vel_error"]) 79 | # append original points to plot 80 | orgLat.append(currData["gps_lat"]) 81 | orgLon.append(currData["gps_lon"]) 82 | 83 | # get predicted values 84 | predictedLonMtrs = objEast.getPredictedPos() 85 | predictedLatMtrs = objNorth.getPredictedPos() 86 | predictedAlt = objUp.getPredictedPos() 87 | 88 | predictedLat, predictedLon = helperObj.mtrsToGeopoint(predictedLatMtrs, 89 | predictedLonMtrs) 90 | 91 | predictedVE = objEast.getPredictedVel() 92 | predictedVN = objNorth.getPredictedVel() 93 | 94 | resultantV = np.sqrt(np.power(predictedVE, 2) + np.power(predictedVN, 2)) 95 | deltaT = currData["timestamp"] - initialData["timestamp"] 96 | 97 | # print("{} seconds in, Lat: {}, Lon: {}, Alt: {}, Vel(mph): {}".format( 98 | # deltaT, predictedLat, predictedLon, predictedAlt, resultantV)) 99 | 100 | # append predicted points to list 101 | pointsToPlotLat.append(predictedLat) 102 | pointsToPlotLon.append(predictedLon) 103 | 104 | plt.subplot(2,1,1) 105 | plt.title('Original') 106 | plt.plot(orgLat, orgLon) 107 | 108 | plt.subplot(2,1,2) 109 | plt.title('Fused') 110 | plt.plot(pointsToPlotLat, pointsToPlotLon) 111 | 112 | plt.show() 113 | --------------------------------------------------------------------------------