├── .gitignore ├── Frames ├── 1_Arm.STL ├── 1_Body.STL └── 2_Tetraedron_Frame.STL ├── visualisation.blend ├── README.md ├── kalman.py ├── reception.py ├── processing.py └── kalman.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/* 2 | *.blend1 3 | serial/* 4 | serial/*/* 5 | *.pyc 6 | 7 | -------------------------------------------------------------------------------- /Frames/1_Arm.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HiveTracker/Kalman-Filter/HEAD/Frames/1_Arm.STL -------------------------------------------------------------------------------- /Frames/1_Body.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HiveTracker/Kalman-Filter/HEAD/Frames/1_Body.STL -------------------------------------------------------------------------------- /visualisation.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HiveTracker/Kalman-Filter/HEAD/visualisation.blend -------------------------------------------------------------------------------- /Frames/2_Tetraedron_Frame.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HiveTracker/Kalman-Filter/HEAD/Frames/2_Tetraedron_Frame.STL -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Kalman-Filter 2 | 3 | Here is the WIP repo for our Sensor fusion. It basically uses IMU data to improve Lighthouse positioning dynamism. 4 | This repo also contains the 3d models of the structure, helpful for the calibration algorithms for example... 5 | 6 | There are 3 kinds of files: 7 | - Data preprocessing: transforms accelerations from IMU and Lighthouses data into 3D points. 8 | - Sensor fusion: this Kalman Filter (and more variations in progress) estimates the tracker position as well as possible. 9 | - Visualization: this Blender file uses position calculated after Sensor fusion. 10 | 11 | Prerequisites: 12 | - Python libraries used are all in the Anaconda package: https://www.anaconda.com/download/. 13 | - Serial library can be found here: https://github.com/pyserial/pyserial/ 14 | - Find Blender on their website: https://www.blender.org/download/ 15 | 16 | Enclosures options: 17 | - Main enclosure: 4 arms and 1 body frame. 18 | - Tetraedron: it allows visibility for at least one diode in any orientation. 19 | 20 | Simulation: 21 | On the Dev branch, a mini-game simulation is in LH_Simu.blend, see the Python script inside. 22 | You can move the blue car with Z, Q, S, D key (Because of french keyboard), and the scanning/calculated position will appear with a green dot. 23 | 24 | Calibration: 25 | There is also an other way of calculating position from the Lighthouses. It use the assumption that Diodes are on a solid frame (our main enclosure). This Python code allows to know position and orientation of Lighthouses but isn't accurate enough for the moment. A visualization of the calculated points is also in the folder. 26 | -------------------------------------------------------------------------------- /kalman.py: -------------------------------------------------------------------------------- 1 | from sympy import * 2 | import numpy as np 3 | from math import* 4 | 5 | def linear_kalman(uvaAccelero, sUAccelero, uLightH, sULightHI, uTrue, sUTrueI): 6 | 7 | uEst = np.zeros(3) # u estimated (=u predicted) 8 | sUEst = np.zeros((3,3)) # and the matrix u of covariances associated 9 | K = np.zeros((3,3)) # Kalman Gain 10 | tempMat = np.zeros((3,3)) # Temporary matrix used to easly compute K 11 | I = np.eye(3) # Identity matrix 12 | dt = 1/120 # Time period 13 | 14 | # PREDICTION 15 | # Also there is the construction of covariances matrix of sUEst 16 | 17 | uAccelero = uvaAccelero[0] 18 | vAccelero = uvaAccelero[1] 19 | aAccelero = uvaAccelero[2] 20 | 21 | for i in range(3): 22 | uEst[i] = uAccelero[i] + dt * vAccelero[i] + 1/2 * aAccelero[i] * dt**2 23 | 24 | sUEst = [[np.abs(sUAccelero[0]),0 ,0 ], 25 | [0 ,np.abs(sUAccelero[1]),0 ], 26 | [0 ,0 ,np.abs(sUAccelero[2])]] 27 | 28 | # Construction of sUTrue an sULightH, the covariances matrix from sUTrueI 29 | # Matrix of covariances of sUTrueI (I stand for input) 30 | sUTrue = [[np.abs(sUTrueI[0]),0 ,0 ], 31 | [0 ,np.abs(sUTrueI[1]),0 ], 32 | [0 ,0 ,np.abs(sUTrueI[2])]] 33 | 34 | # Matrix sULightH covariances 35 | sULightH = [[np.abs(sULightHI[0]),0 ,0 ], 36 | [0 ,np.abs(sULightHI[1]),0 ], 37 | [0 ,0 ,np.abs(sULightHI[2])]] 38 | 39 | #UPDATE 40 | 41 | # Calculus of Kalman gain K 42 | for i in range(3): 43 | for j in range(3): 44 | K[i][j] = sUEst[i][j] + sULightH[i][j] 45 | K = sUEst * np.linalg.inv(K) 46 | 47 | for i in range(3): 48 | uTrue[i] = uEst[i] + (K[i][0]*(uLightH[0] - uEst[0]) + K[i][1]*(uLightH[1] - uEst[1]) + K[i][2]*(uLightH[2] - uEst[2]) ) 49 | # Calculus of sUTrue 50 | 51 | for i in range(3): 52 | for j in range(3): 53 | tempMat[i][j] = I[i][j] - K[i][j] 54 | sUTrue = tempMat.dot(sUTrue) 55 | 56 | return [uTrue, [sUTrue[0][0], sUTrue[1][1], sUTrue[2][2]]] 57 | 58 | # This function is a simple weighted fusion 59 | def madgwick(uvaAccelero, sUAccelero, uLightH, sULightH): 60 | 61 | uAccelero = uvaAccelero[0] 62 | denom = np.zeros(3) 63 | gamma = np.zeros(3) 64 | uEst = np.zeros(3) 65 | 66 | for i in range(3): 67 | denom[i] = np.abs(sUAccelero[i] + sULightH[i]) 68 | gamma[i] = sULightH[i] / denom[i] 69 | uEst[i] = gamma[i] * uLightH[i] + (1 - gamma[i]) * uAccelero[i] 70 | 71 | return uEst 72 | -------------------------------------------------------------------------------- /reception.py: -------------------------------------------------------------------------------- 1 | 2 | #!/usr/bin/python 3 | 4 | import glob 5 | import serial 6 | import sys 7 | import platform 8 | 9 | DEBUG_PRINT = 0 10 | 11 | class Reception: 12 | 13 | ############################################################################### 14 | def __init__(self): 15 | if DEBUG_PRINT: print("init") 16 | self.port = self.serial_init() 17 | 18 | 19 | ############################################################################### 20 | def serial_init(self): 21 | PLATFORM = platform.system() 22 | if "Linux" in PLATFORM: 23 | SERIAL_PATH = "/dev/ttyUSB*" 24 | elif "Darwin" in PLATFORM: 25 | SERIAL_PATH = "/dev/tty.usb*" # TODO: test it 26 | else: # Windows 27 | self.port = serial.Serial('COM5', 115200 * 2) 28 | SERIAL_PATH = 'WIN_WORKARAOUND' 29 | 30 | if SERIAL_PATH != 'WIN_WORKARAOUND': 31 | devices = glob.glob(SERIAL_PATH) 32 | self.port = serial.Serial(devices[0], 115200 * 2) 33 | 34 | success = self.port.isOpen() 35 | if success: 36 | if DEBUG_PRINT: print("Port open.") 37 | self.lookForHeader() 38 | else: 39 | print("\n!!! Error: serial device not found !!!") 40 | exit(-1) 41 | return self.port 42 | 43 | 44 | ############################################################################### 45 | def lookForHeader(self): 46 | if DEBUG_PRINT: print("seeking header\n") 47 | 48 | # packets structure: 49 | # 2 headers + 1 base_axis + (4 photodiodes * 2 bytes) + (3 accel * 2 bytes) 50 | 51 | while True: 52 | 53 | while self.readByte() != 255: 54 | pass # consume 55 | 56 | if self.readByte() != 255: 57 | continue 58 | 59 | break 60 | 61 | 62 | ############################################################################### 63 | def readByte(self): 64 | byte = ord(self.port.read(1)) 65 | if DEBUG_PRINT: 66 | print(byte) 67 | return byte 68 | 69 | 70 | ############################################################################### 71 | def parse_data(self): 72 | centroidNum = 4 73 | accelerationNum = 3 74 | 75 | base_axis = self.readByte() 76 | base = (base_axis >> 1) & 1 77 | axis = (base_axis >> 0) & 1 78 | 79 | if DEBUG_PRINT: print("\nbase, axis:", base, axis) 80 | 81 | centroids = [0 for i in range(centroidNum)] 82 | 83 | for i in range(centroidNum): 84 | centroids[i] = self.decodeTime() 85 | if DEBUG_PRINT: print("centroids[", i, "] =", centroids[i]) 86 | 87 | accelerations = [0 for i in range(accelerationNum)] 88 | for i in range(accelerationNum): 89 | accelerations[i] = self.decodeAccel() 90 | if DEBUG_PRINT: print("accelerations[", i, "] =", accelerations[i]) 91 | 92 | # consumes header 93 | for i in range(2): 94 | b = self.readByte() 95 | if (b != 255): 96 | if DEBUG_PRINT: print("header problem", i) 97 | self.lookForHeader() 98 | break 99 | 100 | return base, axis, centroids, accelerations 101 | 102 | 103 | ############################################################################### 104 | def decodeTime(self): 105 | rxl = self.readByte() # LSB first 106 | rxh = self.readByte() # MSB last 107 | time = (rxh << 8) + rxl # reconstruct packets 108 | time <<= 2 # (non-significant) lossy decompression 109 | time /= 16. # convert to us 110 | 111 | if (time >= 6777 or time < 1222): 112 | time = 0 113 | if DEBUG_PRINT: print("INVALID TIME") 114 | 115 | return time 116 | 117 | 118 | ############################################################################### 119 | # github.com/adafruit/Adafruit_BNO055/blob/master/Adafruit_BNO055.cpp#L359-L361 120 | def decodeAccel(self): 121 | rxl = self.readByte() # LSB first 122 | rxh = self.readByte() # MSB last 123 | accel = (rxh << 8) + rxl # reconstruct packets 124 | 125 | gravity = 9.81 126 | accel /= (1<<15) / (4.0 * gravity) # normalize with max amplitude 127 | accel -= gravity * 2.0 # remove max negative value (-2g)... 128 | # ...to stay positive during tx 129 | return accel 130 | 131 | 132 | ############################################################################### 133 | # This is just for debug, or to use as example 134 | if __name__ == "__main__": 135 | 136 | if DEBUG_PRINT: 137 | print("default main") 138 | 139 | # create an object (it also does the port initialization) 140 | rx = Reception() 141 | 142 | while True: 143 | # base = 0 or 1 (B or C) 144 | # axis = 0 or 1 (horizontal or vertical) 145 | # centroids = array of 4 floats in microseconds 146 | # accelerations = array of 3 floats in G (AKA m/s^2) 147 | base, axis, centroids, accelerations = rx.parse_data() 148 | 149 | if not DEBUG_PRINT: 150 | print( base, axis, centroids, accelerations ) 151 | -------------------------------------------------------------------------------- /processing.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # Compute the position of a Lighthouse given 3 | # sensor readings in a known configuration. 4 | 5 | #from math import * 6 | import serial 7 | from math import * 8 | import numpy as np 9 | from reception import * 10 | 11 | ######################################################### 12 | # PROCESSING LightHouse 13 | ######################################################### 14 | # Apply changement of coordinate M = [[0, 0, -1], [1, 0, 0], [0, 1, 0]] 15 | # RB_R = Mt . [Bonsai given rotation matrix] . M 16 | # t stand for transposed 17 | 18 | """ 19 | [[0.8633447, 0.02179115, -0.5041437], 20 | [-0.07533064, -0.9823064, -0.1714628], 21 | [-0.49896, 0.186009, -0.8464276]] 22 | 0.2055409 2.522384 -2.553286 1 23 | 24 | [[-0.8772146, 0.03632253, 0.4787225], 25 | [0.05409878, -0.9833049, 0.1737381], 26 | [0.4770408, 0.1783039, 0.8606042]] 27 | 2.326427 2.428492 1.591011 1 28 | 29 | ### TESTS 30 | 31 | [[0.7709669, 0.0003505305, 0.6368753], 32 | [-0.00685263, -0.9999374, 0.008845782], 33 | [0.6368385, -0.01118407, -0.7709163]] 34 | 35 | [[0.9063203, -0.003801087, -0.4225747], 36 | [0.001591456, -0.9999218, 0.01240765], 37 | [-0.4225887, -0.01191781, -0.9062433]] 38 | 39 | """ 40 | # INITIALSATION 41 | # Positionning LH 42 | p1 = [0.2055409, 2.522384, -2.553286] 43 | p2 = [2.326427, 2.428492, 1.591011] 44 | 45 | # Rotation Matrices LH1 and LH2 on world Basis 46 | 47 | m1 = [[0.8633447, 0.02179115, -0.5041437], 48 | [-0.07533064, -0.9823064, -0.1714628], 49 | [-0.49896, 0.186009, -0.8464276]] 50 | 51 | m2 = [[-0.8772146, 0.03632253, 0.4787225], 52 | [0.05409878, -0.9833049, 0.1737381], 53 | [0.4770408, 0.1783039, 0.8606042]] 54 | 55 | # Homogeneous coordinate matrices given by Vive sdk 56 | h1 = [[0.8633447, 0.02179115, -0.5041437, 0], 57 | [-0.07533064, -0.9823064, -0.1714628, 0], 58 | [-0.49896, 0.186009, -0.8464276, 0], 59 | [0.2055409, 2.522384, -2.553286, 1]] 60 | 61 | h2 = [[-0.8772146, 0.03632253, 0.4787225, 0], 62 | [0.05409878, -0.9833049, 0.1737381, 0], 63 | [0.4770408, 0.1783039, 0.8606042, 0], 64 | [2.326427, 2.428492, 1.591011, 1]] 65 | 66 | # Changement base matrix from OpenGL to Blender 67 | 68 | R_ob = [[ 1, 0, 0], 69 | [ 0, 0,-1], 70 | [ 0, 1, 0]] 71 | """ 72 | R_ob = [[ 0, 0,-1], 73 | [ 1, 0, 0], 74 | [ 0, 1, 0]] 75 | """ 76 | # Translation vectors after base Changement 77 | p1b = np.dot(R_ob,p1) 78 | #print(p1b) 79 | p2b = np.dot(R_ob,p2) 80 | #print(p2b) 81 | # Rotation matrix in the base changement 82 | 83 | R1 = np.matmul(np.linalg.inv(R_ob), np.matmul(m1, R_ob)) 84 | R2 = np.matmul(np.linalg.inv(R_ob), np.matmul(m2, R_ob)) 85 | """ 86 | R1 = np.matmul(R_ob, np.matmul(m1, np.linalg.inv(R_ob))) 87 | R2 = np.matmul(R_ob, np.matmul(m2, np.linalg.inv(R_ob))) 88 | """ 89 | def vect_uv(angle_scan): 90 | h1 = [[0.8633447, 0.02179115, -0.5041437, 0], 91 | [-0.07533064, -0.9823064, -0.1714628, 0], 92 | [-0.49896, 0.186009, -0.8464276, 0], 93 | [0.2055409, 2.522384, -2.553286, 1]] 94 | 95 | h2 = [[-0.8772146, 0.03632253, 0.4787225, 0], 96 | [0.05409878, -0.9833049, 0.1737381, 0], 97 | [0.4770408, 0.1783039, 0.8606042, 0], 98 | [2.326427, 2.428492, 1.591011, 1]] 99 | 100 | vecH1_loc = np.array([-cos(angle_scan[0]), 0, sin(angle_scan[0])]) 101 | vecV1_loc = np.array([0, -cos(angle_scan[1]), sin(angle_scan[1])]) 102 | 103 | vecH2_loc = np.array([-cos(angle_scan[2]), 0, sin(angle_scan[2])]) 104 | vecV2_loc = np.array([0, -cos(angle_scan[3]), sin(angle_scan[3])]) 105 | 106 | """ 107 | vecH1_loc = np.array([0, cos(angle_scan[0]), sin(angle_scan[0])]) 108 | vecV1_loc = np.array([cos(angle_scan[1]), 0, -sin(angle_scan[1])]) 109 | 110 | vecH2_loc = np.array([0, cos(angle_scan[2]), sin(angle_scan[2])]) 111 | vecV2_loc = np.array([cos(angle_scan[3]), 0, -sin(angle_scan[3])]) 112 | """ 113 | u = vecH1_loc + vecV1_loc 114 | v = vecH2_loc + vecV2_loc 115 | 116 | #print(angle_scan[2]) 117 | #print(angle_scan[3]) 118 | 119 | norm_u = np.linalg.norm(u) 120 | norm_v = np.linalg.norm(v) 121 | 122 | # u & v in homogeneous coordinates normalized 123 | u_loc = np.array([u[0]/norm_u, u[1]/norm_u, - u[2]/norm_u, 1]) 124 | v_loc = np.array([v[0]/norm_v, v[1]/norm_v, - v[2]/norm_v, 1]) 125 | # u test 126 | #u_loc = np.array([0,0,-1,1]) 127 | #v_loc = np.array([1,0,0,1]) 128 | 129 | #print("u_loc ", u_loc) 130 | 131 | # Transform line from relative coordinates to global lighthouse coordinate system (defined by matrix) (multiply vector by matrix) 132 | 133 | # For LH1 134 | # we need to transpose to convert from column-major to row-major 135 | h1 = np.array(h1).T # raw base transform 136 | p1 = np.array([0,0,0,1]) # (0,0,0) in homogeneous coordinates 137 | p1 = np.matmul(h1,p1) # p1 is position of base A 138 | u = np.matmul(h1,u_loc) # u vector after scanning of base A 139 | #print("u1 ", u) 140 | 141 | # now we fix all this to Blender space (swap Z with Y) 142 | swizzle = [0,2,1,3] 143 | p1 = p1[swizzle] 144 | u = u[swizzle] 145 | #print("u2 ", u) 146 | # For LH2 147 | h2 = np.array(h2).T # raw base transform 148 | p2 = np.array([0,0,0,1]) # (0,0,0) in homogeneous coordinates 149 | p2 = np.matmul(h2,p2) # p2 is position of base A 150 | v = np.matmul(h2,v_loc) # v vector after scanning of base A 151 | 152 | p2 = p2[swizzle] 153 | v = v[swizzle] 154 | 155 | #print("u atom ", u) 156 | return u[0:3], v[0:3] 157 | 158 | def diode_pos(angle_scan): 159 | h1 = [[0.8633447, 0.02179115, -0.5041437, 0], 160 | [-0.07533064, -0.9823064, -0.1714628, 0], 161 | [-0.49896, 0.186009, -0.8464276, 0], 162 | [0.2055409, 2.522384, -2.553286, 1]] 163 | 164 | h2 = [[-0.8772146, 0.03632253, 0.4787225, 0], 165 | [0.05409878, -0.9833049, 0.1737381, 0], 166 | [0.4770408, 0.1783039, 0.8606042, 0], 167 | [2.326427, 2.428492, 1.591011, 1]] 168 | 169 | vecH1_loc = np.array([-cos(angle_scan[0]), 0, sin(angle_scan[0])]) 170 | vecV1_loc = np.array([0, -cos(angle_scan[1]), sin(angle_scan[1])]) 171 | 172 | vecH2_loc = np.array([-cos(angle_scan[2]), 0, sin(angle_scan[2])]) 173 | vecV2_loc = np.array([0, -cos(angle_scan[3]), sin(angle_scan[3])]) 174 | 175 | """ 176 | vecH1_loc = np.array([0, cos(angle_scan[0]), sin(angle_scan[0])]) 177 | vecV1_loc = np.array([cos(angle_scan[1]), 0, -sin(angle_scan[1])]) 178 | 179 | vecH2_loc = np.array([0, cos(angle_scan[2]), sin(angle_scan[2])]) 180 | vecV2_loc = np.array([cos(angle_scan[3]), 0, -sin(angle_scan[3])]) 181 | """ 182 | u = vecH1_loc + vecV1_loc 183 | v = vecH2_loc + vecV2_loc 184 | 185 | #print(angle_scan[2]) 186 | #print(angle_scan[3]) 187 | 188 | norm_u = np.linalg.norm(u) 189 | norm_v = np.linalg.norm(v) 190 | 191 | # u & v in homogeneous coordinates normalized 192 | u_loc = np.array([u[0]/norm_u, u[1]/norm_u, - u[2]/norm_u, 1]) 193 | v_loc = np.array([v[0]/norm_v, v[1]/norm_v, - v[2]/norm_v, 1]) 194 | # u test 195 | #u_loc = np.array([0,0,-1,1]) 196 | #v_loc = np.array([1,0,0,1]) 197 | 198 | #print("u_loc ", u_loc) 199 | 200 | # Transform line from relative coordinates to global lighthouse coordinate system (defined by matrix) (multiply vector by matrix) 201 | 202 | # For LH1 203 | # we need to transpose to convert from column-major to row-major 204 | h1 = np.array(h1).T # raw base transform 205 | p1 = np.array([0,0,0,1]) # (0,0,0) in homogeneous coordinates 206 | p1 = np.matmul(h1,p1) # p1 is position of base A 207 | u = np.matmul(h1,u_loc) # u vector after scanning of base A 208 | #print("u1 ", u) 209 | 210 | # now we fix all this to Blender space (swap Z with Y) 211 | swizzle = [0,2,1,3] 212 | p1 = p1[swizzle] 213 | u = u[swizzle] 214 | #print("u2 ", u) 215 | # For LH2 216 | h2 = np.array(h2).T # raw base transform 217 | p2 = np.array([0,0,0,1]) # (0,0,0) in homogeneous coordinates 218 | p2 = np.matmul(h2,p2) # p2 is position of base A 219 | v = np.matmul(h2,v_loc) # v vector after scanning of base A 220 | 221 | p2 = p2[swizzle] 222 | v = v[swizzle] 223 | 224 | # reshape vectors 225 | u = u[0:3] 226 | v = v[0:3] 227 | p1 = p1[0:3] 228 | p2 = p2[0:3] 229 | """ 230 | for i in range(3): 231 | u[i] -= p1[i] 232 | v[i] -= p2[i] 233 | """ 234 | p0 = p1 235 | q0 = p2 236 | #print(p0," & ",q0) 237 | 238 | # STEP: resolve the system of imperfect intersection 239 | w0 = np.array([p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2]]) 240 | #w0 = p0 - q0 241 | #print(w0) 242 | a = np.dot(u, u) #scalar product of u and w0 243 | b = np.dot(u, v) 244 | c = np.dot(v, v) 245 | 246 | d = np.dot(u, w0) 247 | e = np.dot(v, w0) 248 | 249 | # Resolution of the linear system 250 | #k = np.array([[uu, -uv], [uv, -vv]]) 251 | #l = np.array([ABu, ABv]) 252 | #lambda_mu = np.linalg.solve(k, l) 253 | 254 | denom = a*c - b*b 255 | pS = np.zeros(3) 256 | qT = np.zeros(3) 257 | I = np.zeros(3) 258 | 259 | if denom >= 1e-6: 260 | s = (e * b - c * d) / denom 261 | t = (a * e - b * d) / denom 262 | 263 | for i in range(3): 264 | pS[i] = p0[i] + s*u[i] 265 | qT[i] = q0[i] + t*v[i] 266 | I[i] = -(pS[i] + qT[i]) / 2 267 | 268 | return I 269 | 270 | ########################################################## 271 | # PROCESSING IMU 272 | ########################################################## 273 | # Period of measurement of the IMU 274 | T = 1/120. 275 | # Standard deviation of IMU (m/s^2) considered the same on 3 axis 276 | S_ACC = 0.0423639918 277 | 278 | def IMU_pos(prevPos, prevVel, accel): 279 | # Initilisation of arrays. Two vectors the old "0" and the new "1" values 280 | velocity = np.zeros((2,3)) 281 | position = np.zeros((2,3)) 282 | 283 | for i in range(3): 284 | velocity[0][i] = prevVel[i] 285 | position[0][i] = prevPos[i] 286 | 287 | # Here begin the function 288 | # First integration 289 | for i in range(3): 290 | velocity[1][i] = velocity[0][i] + accel[i] * T 291 | 292 | # Second integration 293 | for i in range(3): 294 | position[1][i] = position[0][i] + velocity[0][i] * T 295 | 296 | return [position[1], velocity[1], accel] 297 | 298 | ######################################################### 299 | # MAIN 300 | ######################################################### 301 | def init_position(): 302 | # Initialize serial port and prepare data buffer 303 | return Reception() 304 | 305 | # Initialize Angle calculated from timings of LH 306 | scanAngle = [[0, 0, 0, 0], 307 | [0, 0, 0, 0], 308 | [0, 0, 0, 0], 309 | [0, 0, 0, 0]] 310 | # Initailize positions of diodes 0, 1, 2 and 3 311 | I_diode = [[0, 0, 0], 312 | [0, 0, 0], 313 | [0, 0, 0], 314 | [0, 0, 0]] 315 | 316 | raw_diode = np.zeros((4,4,3)) 317 | # Circular buffer index 318 | cbi = 0 319 | 320 | time = 4 321 | I_Accelero = [0, 0, 0] 322 | # Previous velocity 323 | velocity = [0,0,0] 324 | # Standard deviation of IMU 325 | s_I_Accelero = [0, 0, 0] 326 | s_velocity = [0, 0, 0] 327 | s_accelerations = [S_ACC, S_ACC, S_ACC] 328 | 329 | wasIMUInit = False 330 | 331 | def get_vect_uv(rx): 332 | global I_Accelero, velocity, s_I_Accelero, s_velocity, s_accelerations 333 | global FILTER, S_ACC, raw_diode, time, wasIMUInit, cbi, factor 334 | 335 | if not wasIMUInit : 336 | velocity = [0, 0, 0] 337 | wasIMUInit = True 338 | 339 | # Refresh data 340 | # base = 0 or 1 (B or C) 341 | # axis = 0 or 1 (horizontal or vertical) 342 | # centroids = array of 4 floats in microseconds 343 | # accelerations = array of 3 floats in G (AKA m/s^2) 344 | base, axis, centroids, accelerations = rx.parse_data() 345 | 346 | # Periode of one scan in micro seconds 347 | T_scan = 8333 348 | 349 | # Convert time of scanning into angle in radians 350 | for i in range(4): 351 | scanAngle[i][base*2 + axis] = centroids[i] * pi / T_scan 352 | 353 | # For Lighthouses 354 | #for i in range(4): 355 | # Scan for diode number 0 356 | return vect_uv(scanAngle[0]) 357 | 358 | def get_position(rx): 359 | global I_Accelero, velocity, s_I_Accelero, s_velocity, s_accelerations 360 | global FILTER, S_ACC, raw_diode, time, wasIMUInit, cbi, factor 361 | 362 | if not wasIMUInit : 363 | velocity = [0, 0, 0] 364 | wasIMUInit = True 365 | 366 | # Refresh data 367 | # base = 0 or 1 (B or C) 368 | # axis = 0 or 1 (horizontal or vertical) 369 | # centroids = array of 4 floats in microseconds 370 | # accelerations = array of 3 floats in G (AKA m/s^2) 371 | base, axis, centroids, accelerations = rx.parse_data() 372 | 373 | # Periode of one scan in micro seconds 374 | T_scan = 8333 375 | time += 1 376 | 377 | 378 | # Convert time of scanning into angle in radians 379 | for i in range(4): 380 | scanAngle[i][base*2 + axis] = centroids[i] * pi / T_scan 381 | 382 | # For Lighthouses 383 | for i in range(4): 384 | I_diode[i] = diode_pos(scanAngle[i]) 385 | 386 | FILTER = 1 387 | # Factor of smoothness for low pass filter 388 | factor = 0.1 389 | 390 | # Low pass filter 391 | if FILTER == 1 : 392 | for d in range(4): 393 | for xyz in range(3): 394 | I_diode[d][xyz] = (1 - factor) * raw_diode[0][d][xyz] + factor * I_diode[d][xyz] 395 | raw_diode[0][d][xyz] = I_diode[d][xyz] 396 | 397 | 398 | # Low pass filter using 4 last optical data 399 | elif FILTER == 2 : 400 | for d in range(4): 401 | for xyz in range(3): 402 | raw_diode[cbi][d][xyz] = I_diode[d][xyz] 403 | average = 0 404 | for t in range(4): 405 | average += raw_diode[t][d][xyz] 406 | I_diode[d][xyz] = average / 4 407 | 408 | # Circular buffer index 409 | cbi = (cbi+1) % 4 410 | 411 | I_LH = [(I_diode[0][0] + I_diode[3][0]) / 2, (I_diode[0][1] + I_diode[3][1]) / 2, (I_diode[0][2] + I_diode[3][2]) / 2] 412 | # Position where the IMU will be at calibration 413 | averagePos = I_LH 414 | 415 | # For IMU 416 | 417 | # Reset position of IMU at (1/120 * 4)s 418 | # We consider variance on measurement, the same on 3 axis 419 | if time >= 4 : 420 | # off_set allows to calibrate position of the IMU 421 | off_set = averagePos 422 | I_Accelero = [0, 0, 0] 423 | time = 0 424 | for i in range(3): 425 | I_Accelero[i] = off_set[i] 426 | velocity[i] = 0 427 | s_I_Accelero[i] = 0 428 | s_velocity[i] = 0 429 | s_accelerations[i] = S_ACC 430 | 431 | # Update data of the accelerometer 432 | I_Accelero, velocity, accel = IMU_pos(I_Accelero, velocity, accelerations) 433 | # Update standard deviation of accelerometer 434 | s_I_Accelero, s_velocity, s_accelerations = IMU_pos(s_I_Accelero, s_velocity, s_accelerations) 435 | 436 | return I_diode, [I_Accelero, velocity, accel], [s_I_Accelero, s_velocity, s_accelerations] 437 | -------------------------------------------------------------------------------- /kalman.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : Linear_Kalman_Filter.cpp 3 | // Author : Julien 4 | // Version : 5 | // Copyright : Your copyright notice 6 | // Description : KF linear 1D & 2D 7 | //============================================================================ 8 | 9 | //#include 10 | 11 | #include 12 | //#include 13 | #include 14 | #include 15 | using namespace std; 16 | 17 | //=========================================================== 18 | //FUNCTION OF 1D LINEAR KALMAN FILTER 19 | //=========================================================== 20 | float *linearKFD(float xAccelero, float sXAccelero, float xLightH, float sXLightH, float xTrue, float sXTrue) 21 | { 22 | float xEst, sXEst; //x estimated and covariances associated 23 | float K; //Kalman Gain 24 | 25 | //Predict (which normally follows a mathematical law, but here the prediction is given by IMU) 26 | xEst=xAccelero; 27 | sXEst=sXAccelero; 28 | 29 | //Update 30 | //K=xEst/(xEst+sXTrue); 31 | //K=sXEst/(sXEst+sXTrue); 32 | 33 | K=sXEst/(sXEst+sXLightH); 34 | //cout << "K = " <