├── README.md ├── SLAM1D.py └── SLAM2D.py /README.md: -------------------------------------------------------------------------------- 1 | # SLAM 2 | 3 | Implementation of algorithms for SLAM: EKF, EIF and SEIF. 4 | Group Project with Dmitry ZHUKOV and Joseph LAM. 5 | 6 | Mainly based on this article: 7 | http://robots.stanford.edu/papers/thrun.tr-seif02.pdf 8 | 9 | For now, data association is supposed know as well as a previous noisy knowledge of the map. 10 | -------------------------------------------------------------------------------- /SLAM1D.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from numpy.linalg import inv 3 | from numpy import dot 4 | from matplotlib import pyplot as plt 5 | import seaborn 6 | 7 | class BasicMovement: 8 | def __init__(self, maxSpeed, covariance, robotFeaturesDim): 9 | self.maxSpeed = maxSpeed 10 | self.covariance = np.atleast_2d(covariance) 11 | self.robotFeaturesDim = robotFeaturesDim 12 | 13 | # Input the real state 14 | def move(self, state, covariance=None, command=None): 15 | move = self.__choose_command(state) if command is None else command 16 | noise = self.__get_noise(state, covariance) 17 | newState = state + move + noise 18 | return newState, move 19 | 20 | def __choose_command(self, state): # TO CHANGE FOR 2D 21 | dim = self.robotFeaturesDim 22 | way = 1 - 2 * np.random.randint(2) 23 | speed = self.maxSpeed * np.random.rand() 24 | newCommand = np.zeros_like(state) 25 | newCommand[:dim] = way * speed # TO CHANGE FOR 2D 26 | return newCommand 27 | 28 | def __get_noise(self, state, covariance): 29 | dim = self.robotFeaturesDim 30 | noise = np.zeros_like(state) 31 | covariance = self.covariance if covariance is None else covariance 32 | noise[:dim] = np.random.multivariate_normal(np.zeros(dim), covariance, 1).T 33 | return noise 34 | 35 | 36 | class BasicMeasurement: 37 | def __init__(self, covariance, robotFeaturesDim, envFeaturesDim, measureFunction, gradMeasureFunction, detectionSize=0): 38 | self.covariance = np.atleast_2d(covariance) 39 | self.robotFeaturesDim = robotFeaturesDim 40 | self.envFeaturesDim = envFeaturesDim 41 | self.measureFunction = measureFunction 42 | self.gradMeasureFunction = gradMeasureFunction 43 | self.detectionSize = detectionSize 44 | 45 | # Input the real state 46 | def measure(self, state): 47 | dim = state.shape[0] 48 | dimR = self.robotFeaturesDim 49 | dimE = self.envFeaturesDim 50 | rState = state[:dimR] 51 | envState = state[dimR:] 52 | nbLandmark = (dim - dimR) / dimE 53 | 54 | mes = np.zeros(nbLandmark) 55 | landmarkIds = np.zeros(nbLandmark).astype(int) 56 | j = 0 57 | for i, landmark in enumerate(envState.reshape((nbLandmark, dimE, 1))): 58 | if (np.linalg.norm(rState - landmark) < self.detectionSize) or (self.detectionSize is 0): 59 | mes[j] = self.measureFunction(rState, landmark) 60 | landmarkIds[j] = int(i) 61 | j += 1 62 | mes = mes[:j] 63 | landmarkIds = landmarkIds[:j] 64 | 65 | mes = np.array(mes) 66 | noise = self.__get_noise(mes) 67 | mes += noise # TO CHANGE for 2D --------------------------------------------- 68 | return mes.reshape((len(mes), 1)), landmarkIds 69 | 70 | def __get_noise(self, mes): 71 | noise = np.squeeze(np.random.multivariate_normal(np.zeros(self.envFeaturesDim), self.covariance, len(mes))) 72 | return noise 73 | 74 | class EKFModel: 75 | def __init__(self, dimension, robotFeaturesDim, envFeaturesDim, motionModel, mesModel, covMes, muInitial): 76 | self.robotFeaturesDim = robotFeaturesDim 77 | self.envFeaturesDim = envFeaturesDim 78 | self.dimension = dimension 79 | 80 | self.Sigma = np.eye(dimension) 81 | self.mu = muInitial.copy() # np.zeros((1, dimension)) 82 | self.S = np.zeros(dimension * robotFeaturesDim).reshape((dimension, robotFeaturesDim)) 83 | self.S[:robotFeaturesDim] = np.eye(robotFeaturesDim) 84 | self.Z = covMes 85 | self.motionModel = motionModel 86 | self.mesModel = mesModel 87 | 88 | def update(self, measures, landmarkIds, command, U): 89 | self.__motion_update(command, U) 90 | for ldmIndex, ldmMes in zip(landmarkIds, measures): 91 | self.__measurement_update(ldmMes, ldmIndex) 92 | return self.Sigma, self.mu 93 | 94 | def __motion_update(self, command, U): 95 | previousMeanState = self.mu 96 | _, meanStateChange = self.motionModel.move(previousMeanState, command=command) 97 | self.mu += meanStateChange 98 | self.Sigma += dot(dot(self.S, U), self.S.T) 99 | 100 | def __measurement_update(self, ldmMes, ldmIndex): 101 | mu = self.mu 102 | Sigma = self.Sigma 103 | meanMes, gradMeanMes = self.__get_mean_measurement_params(mu, ldmIndex) 104 | 105 | z = np.atleast_2d(ldmMes) 106 | zM = np.atleast_2d(meanMes) 107 | 108 | C = gradMeanMes 109 | toInvert = inv(dot(dot(C.T, Sigma), C) + self.Z) 110 | K = dot(dot(Sigma, C), toInvert) 111 | 112 | self.mu += dot(K, z - zM) 113 | self.Sigma = dot(np.eye(self.dimension) - dot(K, C.T), Sigma) 114 | 115 | def __get_mean_measurement_params(self, mu, ldmIndex): 116 | realIndex = self.robotFeaturesDim + ldmIndex * self.envFeaturesDim 117 | ldmMeanState = mu[realIndex: realIndex + self.envFeaturesDim] 118 | rMeanState = mu[:self.robotFeaturesDim] 119 | 120 | meanMes = self.mesModel.measureFunction(rMeanState, ldmMeanState) 121 | gradMeanMes = self.mesModel.gradMeasureFunction(mu, ldmIndex) 122 | return meanMes, gradMeanMes 123 | 124 | 125 | class EIFModel: 126 | def __init__(self, dimension, robotFeaturesDim, envFeaturesDim, motionModel, mesModel, covMes, muInitial): 127 | self.robotFeaturesDim = robotFeaturesDim 128 | self.envFeaturesDim = envFeaturesDim 129 | self.dimension = dimension 130 | 131 | self.H = np.eye(dimension) 132 | self.b = dot(muInitial.T, self.H) 133 | self.S = np.zeros(dimension * robotFeaturesDim).reshape((dimension, robotFeaturesDim)) 134 | self.S[:robotFeaturesDim] = np.eye(robotFeaturesDim) 135 | self.invZ = inv(covMes) 136 | self.motionModel = motionModel 137 | self.mesModel = mesModel 138 | 139 | def update(self, measures, landmarkIds, command, U): 140 | self.__motion_update(command, U) 141 | for ldmIndex, ldmMes in zip(landmarkIds, measures): 142 | self.__measurement_update(ldmMes, ldmIndex) 143 | return self.H, self.b 144 | 145 | def __motion_update(self, command, U): 146 | previousMeanState = self.estimate() 147 | _, meanStateChange = self.motionModel.move(previousMeanState, command=command) 148 | self.H = inv(inv(self.H) + dot(dot(self.S, U), self.S.T)) 149 | self.b = dot((previousMeanState + meanStateChange).T, self.H) 150 | 151 | def __measurement_update(self, ldmMes, ldmIndex): 152 | mu = self.estimate() 153 | meanMes, gradMeanMes = self.__get_mean_measurement_params(mu, ldmIndex) 154 | 155 | z = np.atleast_2d(ldmMes) 156 | zM = np.atleast_2d(meanMes) 157 | C = gradMeanMes 158 | 159 | self.H += dot(dot(C, self.invZ), C.T) 160 | self.b += dot(dot((z - zM + dot(C.T, mu)).T, self.invZ), C.T) 161 | 162 | def __get_mean_measurement_params(self, mu, ldmIndex): 163 | realIndex = self.robotFeaturesDim + ldmIndex * self.envFeaturesDim 164 | # import ipdb; ipdb.set_trace() 165 | ldmMeanState = mu[realIndex: realIndex + self.envFeaturesDim] 166 | rMeanState = mu[:self.robotFeaturesDim] 167 | 168 | meanMes = self.mesModel.measureFunction(rMeanState, ldmMeanState) 169 | gradMeanMes = self.mesModel.gradMeasureFunction(mu, ldmIndex) 170 | return meanMes, gradMeanMes 171 | 172 | def estimate(self, H=None, b=None): 173 | H = self.H if H is None else H 174 | b = self.b if b is None else b 175 | return dot(b, inv(H)).T 176 | 177 | 178 | measureFunction = lambda rState, landmark: np.sign(landmark[0, 0] - rState[0, 0]) * np.linalg.norm(rState - landmark) 179 | 180 | def gradMeasureFunction(state, ldmIndex): 181 | grad = np.zeros_like(state) 182 | grad[0] = -1 183 | grad[ldmIndex+1] = 1 184 | return grad 185 | 186 | T = 10000 # Number of timesteps 187 | nbLandmark = 10 188 | maxSpeed = 3 189 | robotFeaturesDim = 1 190 | envFeaturesDim = 1 191 | dimension = robotFeaturesDim + nbLandmark * envFeaturesDim 192 | 193 | # Detection parameters 194 | spaceBetween = 100 195 | detectionSize = 15 196 | 197 | covarianceMotion = np.eye(robotFeaturesDim) * 10 # motion noise variance 198 | covarianceMeasurements = np.eye(envFeaturesDim) * 10 # measurement noise variance 199 | 200 | motionModel = BasicMovement(maxSpeed, covarianceMotion, robotFeaturesDim) 201 | measurementModel = BasicMeasurement(covarianceMeasurements, robotFeaturesDim, envFeaturesDim, measureFunction, gradMeasureFunction, detectionSize) 202 | state = np.zeros((dimension, 1)) # Real robot state 203 | state[1:] = np.arange(0, nbLandmark * spaceBetween, spaceBetween).reshape(nbLandmark, 1) 204 | # state[1] = -100 205 | 206 | muSimple = np.zeros_like(state) # Estimated robot state basic 207 | # mu[1] = 50 208 | # mu[1:nbLandmark+1] = np.arange(0, nbLandmark * spaceBetween, spaceBetween).reshape(nbLandmark, 1) 209 | muSimple = state.copy() 210 | muSimple[1:] += np.random.multivariate_normal([0], covarianceMeasurements, nbLandmark).reshape(nbLandmark, 1) 211 | 212 | 213 | muEIF = np.zeros_like(state) # Estimated robot state using EIF Algorithm 214 | # muEIF[1] = 50 215 | # muEIF[1:nbLandmark+1] = np.arange(0, nbLandmark * spaceBetween, spaceBetween).reshape(nbLandmark, 1) 216 | muEIF = muSimple.copy() 217 | 218 | 219 | muEKF = np.zeros_like(state) # Estimated robot state using EIF Algorithm 220 | # muEIF[1] = 50 221 | # muEIF[1:nbLandmark+1] = np.arange(0, nbLandmark * spaceBetween, spaceBetween).reshape(nbLandmark, 1) 222 | muEKF = muSimple.copy() 223 | 224 | eif = EIFModel(dimension, robotFeaturesDim, envFeaturesDim, motionModel, measurementModel, covarianceMeasurements, muSimple) 225 | ekf = EKFModel(dimension, robotFeaturesDim, envFeaturesDim, motionModel, measurementModel, covarianceMeasurements, muSimple) 226 | 227 | mus_simple = np.zeros((T, dimension)) 228 | mus_eif = np.zeros((T, dimension)) 229 | mus_ekf = np.zeros((T, dimension)) 230 | states = np.zeros((T, dimension)) 231 | 232 | mus_simple[0] = np.squeeze(muSimple) 233 | mus_eif[0] = np.squeeze(muEIF) 234 | mus_ekf[0] = np.squeeze(muEKF) 235 | states[0] = np.squeeze(state) 236 | 237 | print("BEFORE") 238 | print("EIF estimate :") 239 | print(muEIF) 240 | print("EKF estimate :") 241 | print(muEKF) 242 | print("Real state :") 243 | print(state) 244 | print('\n') 245 | 246 | listStates = [] 247 | listCommands = [] 248 | listMeasures = [] 249 | listLandmarkIds = [] 250 | for t in range(1, T): 251 | state, motionCommand = motionModel.move(state) 252 | measures, landmarkIds = measurementModel.measure(state) 253 | 254 | listStates.append(state) 255 | listCommands.append(motionCommand) 256 | listMeasures.append(measures) 257 | listLandmarkIds.append(landmarkIds) 258 | 259 | 260 | for t in range(1, T): 261 | state = listStates[t-1] 262 | motionCommand = listCommands[t-1] 263 | measures = listMeasures[t-1] 264 | landmarkIds = listLandmarkIds[t-1] 265 | 266 | H, b = eif.update(measures, landmarkIds, motionCommand, covarianceMotion) 267 | # + measurement ! 268 | muEIF = eif.estimate() 269 | 270 | mus_eif[t] = np.squeeze(muEIF) 271 | 272 | for t in range(1, T): 273 | muSimple += listCommands[t-1] 274 | mus_simple[t] = np.squeeze(muSimple) 275 | states[t] = np.squeeze(listStates[t-1]) 276 | 277 | for t in range(1, T): 278 | state = listStates[t-1] 279 | motionCommand = listCommands[t-1] 280 | measures = listMeasures[t-1] 281 | landmarkIds = listLandmarkIds[t-1] 282 | 283 | Sigma, muEKF = ekf.update(measures, landmarkIds, motionCommand, covarianceMotion) 284 | mus_ekf[t] = np.squeeze(muEKF) 285 | 286 | print('\n') 287 | print('AFTER') 288 | print("EIF estimate :") 289 | print(muEIF) 290 | print("EKF estimate :") 291 | print(muEKF) 292 | print("Real state :") 293 | print(state) 294 | 295 | print(states[5, 0]) 296 | print(mus_simple[5, 0]) 297 | print(mus_eif[5, 0]) 298 | print(mus_ekf[5, 0]) 299 | 300 | 301 | plt.figure() 302 | plt.plot(states[:, 0]) 303 | plt.plot(mus_simple[:, 0]) 304 | plt.plot(mus_eif[:, 0]) 305 | plt.plot(mus_ekf[:, 0]) 306 | plt.legend(['Real position', 'Simple estimate', 'EIF estimate', 'EKF estimate']) 307 | plt.title("{0} landmarks".format(nbLandmark)) 308 | plt.show() 309 | -------------------------------------------------------------------------------- /SLAM2D.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from numpy.linalg import inv, multi_dot 3 | from numpy import dot 4 | from matplotlib import pyplot as plt 5 | from matplotlib.patches import Circle 6 | import math 7 | import seaborn 8 | from scipy import sparse 9 | from scipy.sparse import linalg 10 | 11 | 12 | def dots(*arg): 13 | return multi_dot(arg) 14 | 15 | 16 | class BasicMovement: 17 | def __init__(self, maxSpeed, maxRotation, covariance, measureFunction): 18 | self.maxSpeed = maxSpeed 19 | self.maxRotation = maxRotation 20 | self.measureFunction = measureFunction 21 | self.covariance = np.atleast_2d(covariance) 22 | 23 | # Input the real state 24 | def move(self, state, covariance=None, command=None): 25 | command = self.__choose_command(state) if command is None else command 26 | noise = self.__get_noise(covariance) 27 | idealMove = self.exact_move(state, command) 28 | realMove = self.__noisy_move(state, idealMove, noise) 29 | newState = state + realMove 30 | return clipState(newState), command 31 | 32 | def __choose_command(self, state): 33 | speed = self.maxSpeed * np.random.rand() 34 | if (np.linalg.norm(state[:2]) > 100): 35 | _, rotation = self.measureFunction(state[:3], [[0], [0]]) 36 | rotation = np.clip(rotation, -self.maxRotation, self.maxRotation) 37 | else: 38 | rotation = (np.random.rand() * 2 - 1) * self.maxRotation 39 | return [speed, rotation] 40 | 41 | def exact_move(self, state, command): 42 | speed, rotation = command 43 | angle = state[2] 44 | deltaX = speed * math.cos(angle) 45 | deltaY = speed * math.sin(angle) 46 | 47 | move = np.zeros_like(state) 48 | move[:3, 0] = [deltaX, deltaY, rotation] 49 | return move 50 | 51 | def __noisy_move(self, state, idealMove, noise): 52 | noisyMove = idealMove[:3] + noise 53 | noisySpeed, _ = self.measureFunction(noisyMove[:3], np.zeros_like(noise)[:2]) 54 | noisyRotation = noisyMove[2] 55 | 56 | maxs = [self.maxSpeed, self.maxRotation] 57 | mins = [0, -self.maxRotation] 58 | correctedCommand = np.clip([noisySpeed, noisyRotation], mins, maxs) 59 | return self.exact_move(state, correctedCommand) 60 | 61 | def __noisy_move2(self, state, idealMove, noise): 62 | noisyMove = np.zeros_like(state) 63 | noisyMove[:3] = idealMove[:3] + noise 64 | return noisyMove 65 | 66 | def __get_noise(self, covariance): 67 | covariance = self.covariance if covariance is None else covariance 68 | noise = np.random.multivariate_normal(np.zeros(covariance.shape[0]), covariance, 1).T 69 | return noise 70 | 71 | 72 | class BasicMeasurement: 73 | def __init__(self, covariance, robotFeaturesDim, envFeaturesDim, measureFunction, gradMeasureFunction, detectionSize=0, detectionCone=0): 74 | self.covariance = np.atleast_2d(covariance) 75 | self.robotFeaturesDim = robotFeaturesDim 76 | self.envFeaturesDim = envFeaturesDim 77 | self.measureFunction = measureFunction 78 | self.gradMeasureFunction = gradMeasureFunction 79 | self.detectionSize = detectionSize 80 | self.detectionCone = detectionCone 81 | 82 | # Input the real state 83 | def measure(self, state): 84 | dim = state.shape[0] 85 | dimR = self.robotFeaturesDim 86 | dimE = self.envFeaturesDim 87 | rState = state[:dimR] 88 | envState = state[dimR:] 89 | nbLandmark = (dim - dimR) / dimE 90 | 91 | mes = np.zeros(nbLandmark * dimE).reshape(nbLandmark, dimE) 92 | landmarkIds = np.zeros(nbLandmark) 93 | j = 0 94 | 95 | for i, landmark in enumerate(envState.reshape((nbLandmark, dimE, 1))): 96 | diffNorm, diffAngle = self.measureFunction(rState, landmark) 97 | angleOk = (abs(clipAngle(diffAngle, True)) < self.detectionCone / 2.) or (self.detectionCone is 0) 98 | distanceOk = (diffNorm < self.detectionSize) or (self.detectionSize is 0) 99 | 100 | if distanceOk and angleOk: 101 | mes[j] = [diffNorm, diffAngle] 102 | landmarkIds[j] = i 103 | j += 1 104 | 105 | mes = mes[:j] 106 | landmarkIds = landmarkIds[:j] 107 | mes = np.array(mes) + self.__get_noise(mes) 108 | return mes, landmarkIds 109 | 110 | def __get_noise(self, mes): 111 | noise = np.random.multivariate_normal(np.zeros(self.covariance.shape[0]), self.covariance, mes.shape[0]) 112 | return noise 113 | 114 | 115 | class SEIFModel: 116 | 117 | def __init__(self, dimension, robotFeaturesDim, envFeaturesDim, motionModel, mesModel, covMes, muInitial, maxLinks): 118 | self.robotFeaturesDim = robotFeaturesDim 119 | self.envFeaturesDim = envFeaturesDim 120 | self.dimension = dimension 121 | 122 | self.H = np.eye(dimension) 123 | self.b = dot(muInitial.T, self.H) 124 | self.mu = muInitial.copy() 125 | 126 | self.Sx = np.zeros(dimension * robotFeaturesDim).reshape((dimension, robotFeaturesDim)) 127 | self.Sx[:robotFeaturesDim] = np.eye(robotFeaturesDim) 128 | self.invZ = inv(covMes) 129 | self.motionModel = motionModel 130 | self.mesModel = mesModel 131 | self.maxLinks = maxLinks 132 | 133 | def update(self, measures, landmarkIds, command, U): 134 | self.__motion_update_sparse(command, U) 135 | self.__mean_update() 136 | for ldmIndex, ldmMes in zip(landmarkIds, measures): 137 | self.__measurement_update(ldmMes, int(ldmIndex)) 138 | self.__mean_update() 139 | self.__sparsification() 140 | return self.H, self.b, self.mu 141 | 142 | def __motion_update(self, command, U): 143 | r = self.robotFeaturesDim 144 | previousMeanState = self.estimate() 145 | meanStateChange = self.motionModel.exact_move(previousMeanState, command) 146 | newMeanState = clipState(previousMeanState + meanStateChange) 147 | 148 | # TO IMPROVE 149 | angle = previousMeanState[2, 0] # TO IMPROVE 150 | gradMeanMotion = np.zeros_like(self.H) # TO IMPROVE 151 | gradMeanMotion[2, 0:2] = command[0] * np.array([-math.sin(angle), math.cos(angle)]) # TO IMPROVE 152 | 153 | delta = dots(self.Sx.T, gradMeanMotion, self.Sx) 154 | G = dots(self.Sx, (inv(np.eye(r) + delta) - np.eye(r)), self.Sx.T) 155 | phi = np.eye(self.dimension) + G 156 | Hp = dots(phi.T, self.H, phi) 157 | deltaH = dots(Hp, self.Sx, inv(inv(U) + dots(self.Sx.T, Hp, self.Sx)), self.Sx.T, Hp) 158 | H = Hp - deltaH 159 | self.H = H 160 | self.b = dot(newMeanState.T, self.H) 161 | self.mu = newMeanState 162 | 163 | 164 | def __motion_update_sparse(self, command, U): 165 | r = self.robotFeaturesDim 166 | previousMeanState = self.estimate() 167 | meanStateChange = self.motionModel.exact_move(previousMeanState, command) 168 | newMeanState = clipState(previousMeanState + meanStateChange) 169 | 170 | # TO IMPROVE 171 | angle = previousMeanState[2, 0] # TO IMPROVE 172 | gradMeanMotion = np.zeros_like(self.H) # TO IMPROVE 173 | gradMeanMotion[2, 0:2] = command[0] * np.array([-math.sin(angle), math.cos(angle)]) # TO IMPROVE 174 | 175 | Sx = sparse.bsr_matrix(self.Sx) 176 | sH = sparse.bsr_matrix(self.H) 177 | invU = sparse.coo_matrix(inv(U)) 178 | sparseGradMeanMotion = sparse.bsr_matrix(gradMeanMotion) 179 | 180 | delta = Sx.T.dot(sparseGradMeanMotion).dot(Sx) 181 | G = Sx.dot(linalg.inv(sparse.eye(r) + delta) - sparse.eye(r)).dot(Sx.T) 182 | phi = sparse.eye(self.dimension) + G 183 | Hp = phi.T.dot(sH).dot(phi) 184 | deltaH = Hp.dot(Sx).dot(linalg.inv(invU + Sx.T.dot(Hp).dot(Sx))).dot(Sx.T).dot(Hp) 185 | # H = inv(Hp + dots(self.Sx, U, self.Sx.T)) 186 | H = Hp - deltaH 187 | # self.b = self.b - dot(previousMeanState.T, self.H - H) + dot(meanStateChange.T, H) 188 | self.H = H.todense() 189 | self.b = H.dot(newMeanState).T 190 | self.mu = newMeanState 191 | 192 | def __mean_update(self): 193 | ''' Coordinate ascent ''' 194 | mu = self.mu 195 | iterations = 30 196 | y0, yp = self.__partition_links() 197 | y = np.concatenate([np.arange(self.robotFeaturesDim), y0, yp]) 198 | 199 | # vMu = dot(self.b, inv(self.H)).T 200 | # muSave = [] 201 | # muSave2 = [] 202 | 203 | for t in xrange(iterations): 204 | for i in y: 205 | y2 = np.setdiff1d(y, i) 206 | mu[i] = (self.b[0, i] - dot(self.H[i, y2], mu[y2])) / self.H[i, i] 207 | # muSave.extend([np.linalg.norm(mu - vMu)]) 208 | self.mu = mu 209 | # plt.plot(muSave) 210 | 211 | def __measurement_update(self, ldmMes, ldmIndex): 212 | mu = self.mu 213 | meanMes, gradMeanMes = self.__get_mean_measurement_params(mu, ldmIndex) 214 | 215 | z = np.array(ldmMes).reshape(len(ldmMes), 1) 216 | zM = np.array(meanMes).reshape(len(ldmMes), 1) 217 | C = gradMeanMes 218 | 219 | mesError = (z - zM) 220 | mesError[1, 0] = clipAngle(mesError[1, 0], force=True) 221 | correction = mesError + dot(C.T, mu) 222 | correction[1, 0] = clipAngle(correction[1, 0]) 223 | self.H += dot(dot(C, self.invZ), C.T) 224 | self.b += dot(dot(correction.T, self.invZ), C.T) 225 | 226 | def __partition_links(self): 227 | r = self.robotFeaturesDim 228 | e = self.envFeaturesDim 229 | d = self.dimension 230 | l = (d - r) / e 231 | arrRF = np.arange(r) 232 | 233 | norms = np.array([np.linalg.norm(self.H[arrRF][:, np.arange(i * e + r, (i + 1) * e + r)]) for i in xrange(l)]) 234 | ids = np.argsort(norms) 235 | yp = ids[-self.maxLinks:] 236 | y0 = np.setdiff1d(np.where(norms > 0), yp) 237 | 238 | yp = np.concatenate([np.arange(y * e, (y + 1) * e) for y in yp]) + r 239 | if len(y0) > 0: 240 | y0 = np.concatenate([np.arange(y * e, (y + 1) * e) for y in y0]) + r 241 | 242 | return y0, yp 243 | 244 | def __build_projection_matrix(self, indices): 245 | d1 = self.H.shape[0] 246 | d2 = len(indices) 247 | 248 | S = np.zeros((d1, d2)) 249 | S[indices] = np.eye(d2) 250 | return S 251 | 252 | def __sparsification(self): 253 | x = np.arange(self.robotFeaturesDim) 254 | y0, yp = self.__partition_links() 255 | Sx = sparse.coo_matrix(self.__build_projection_matrix(x)) 256 | Sy0 = sparse.coo_matrix(self.__build_projection_matrix(y0)) 257 | Sxy0 = sparse.coo_matrix(self.__build_projection_matrix(np.concatenate((x, y0)))) 258 | Sxyp = sparse.coo_matrix(self.__build_projection_matrix(np.concatenate((x, yp)))) 259 | Sxy0yp = sparse.coo_matrix(self.__build_projection_matrix(np.concatenate((x, y0, yp)))) 260 | H = sparse.bsr_matrix(self.H) 261 | 262 | Hp = Sxy0yp.dot(Sxy0yp.T).dot(H).dot(Sxy0yp).dot(Sxy0yp.T) 263 | 264 | Ht = H - (0 if not y0.size else Hp.dot(Sy0).dot(linalg.inv(Sy0.T.dot(Hp).dot(Sy0))).dot(Sy0.T).dot(Hp)) \ 265 | + Hp.dot(Sxy0).dot(linalg.inv(Sxy0.T.dot(Hp).dot(Sxy0))).dot(Sxy0.T).dot(Hp) \ 266 | - H.dot(Sx).dot(linalg.inv(Sx.T.dot(H).dot(Sx))).dot(Sx.T).dot(H) 267 | eps = 1e-5 268 | Htt = Ht.todense() 269 | Htt[np.abs(Htt) < eps] = 0 270 | bt = self.b + (Ht - H).dot(self.mu) 271 | 272 | self.H = Htt 273 | self.b = bt 274 | 275 | def __get_mean_measurement_params(self, mu, ldmIndex): 276 | realIndex = self.robotFeaturesDim + ldmIndex * self.envFeaturesDim 277 | ldmMeanState = mu[realIndex: realIndex + self.envFeaturesDim] 278 | rMeanState = mu[:self.robotFeaturesDim] 279 | 280 | meanMes = self.mesModel.measureFunction(rMeanState, ldmMeanState) 281 | gradMeanMes = self.mesModel.gradMeasureFunction(rMeanState, ldmMeanState, realIndex) 282 | return meanMes, gradMeanMes 283 | 284 | def estimate(self): 285 | return self.mu 286 | 287 | 288 | class EIFModel: 289 | def __init__(self, dimension, robotFeaturesDim, envFeaturesDim, motionModel, mesModel, covMes, muInitial): 290 | self.robotFeaturesDim = robotFeaturesDim 291 | self.envFeaturesDim = envFeaturesDim 292 | self.dimension = dimension 293 | 294 | self.HH = np.eye(dimension) 295 | self.H = np.eye(dimension) 296 | self.b = dot(muInitial.T, self.H) 297 | self.bb = dot(muInitial.T, self.H) 298 | self.S = np.zeros(dimension * robotFeaturesDim).reshape((dimension, robotFeaturesDim)) 299 | self.S[:robotFeaturesDim] = np.eye(robotFeaturesDim) 300 | self.invZ = inv(covMes) 301 | self.motionModel = motionModel 302 | self.mesModel = mesModel 303 | 304 | def update(self, measures, landmarkIds, command, U): 305 | self.__motion_update(command, U) 306 | for ldmIndex, ldmMes in zip(landmarkIds, measures): 307 | self.__measurement_update(ldmMes, int(ldmIndex)) 308 | return self.H, self.b 309 | 310 | def __motion_update(self, command, U): 311 | previousMeanState = self.estimate() 312 | meanStateChange = self.motionModel.exact_move(previousMeanState, command) 313 | newMeanState = clipState(previousMeanState + meanStateChange) 314 | 315 | # TO IMPROVE 316 | angle = previousMeanState[2, 0] # TO IMPROVE 317 | gradMeanMotion = np.zeros_like(self.H) # TO IMPROVE 318 | gradMeanMotion[2, 0:2] = command[0] * np.array([-math.sin(angle), math.cos(angle)]) # TO IMPROVE 319 | 320 | IA = np.eye(self.H.shape[0]) + gradMeanMotion # TO IMPROVE 321 | sigma = dot(dot(IA, inv(self.H)), IA.T) + dot(dot(self.S, U), self.S.T) 322 | self.H = inv(sigma) 323 | self.b = dot((newMeanState).T, self.H) 324 | self.HH = self.H.copy() 325 | self.bb = self.b.copy() 326 | 327 | def __measurement_update(self, ldmMes, ldmIndex): 328 | mu = self.estimate() 329 | meanMes, gradMeanMes = self.__get_mean_measurement_params(mu, ldmIndex) 330 | 331 | z = np.array(ldmMes).reshape(len(ldmMes), 1) 332 | zM = np.array(meanMes).reshape(len(ldmMes), 1) 333 | C = gradMeanMes 334 | 335 | mesError = (z - zM) 336 | mesError[1, 0] = clipAngle(mesError[1, 0], force=True) 337 | mesError += dot(C.T, mu) 338 | mesError[1, 0] = clipAngle(mesError[1, 0]) 339 | self.H += dot(dot(C, self.invZ), C.T) 340 | self.b += dot(dot(mesError.T, self.invZ), C.T) 341 | 342 | def __get_mean_measurement_params(self, mu, ldmIndex): 343 | realIndex = self.robotFeaturesDim + ldmIndex * self.envFeaturesDim 344 | ldmMeanState = mu[realIndex: realIndex + self.envFeaturesDim] 345 | rMeanState = mu[:self.robotFeaturesDim] 346 | 347 | meanMes = self.mesModel.measureFunction(rMeanState, ldmMeanState) 348 | gradMeanMes = self.mesModel.gradMeasureFunction(rMeanState, ldmMeanState, realIndex) 349 | return meanMes, gradMeanMes 350 | 351 | def estimate(self, H=None, b=None): 352 | H = self.H if H is None else H 353 | b = self.b if b is None else b 354 | return clipState(dot(b, inv(H)).T) 355 | 356 | 357 | class EKFModel: 358 | def __init__(self, dimension, robotFeaturesDim, envFeaturesDim, motionModel, mesModel, covMes, muInitial): 359 | self.robotFeaturesDim = robotFeaturesDim 360 | self.envFeaturesDim = envFeaturesDim 361 | self.dimension = dimension 362 | 363 | self.Sigma = np.eye(dimension) 364 | self.mu = muInitial.copy() 365 | self.S = np.zeros(dimension * robotFeaturesDim).reshape((dimension, robotFeaturesDim)) 366 | self.S[:robotFeaturesDim] = np.eye(robotFeaturesDim) 367 | self.Z = covMes 368 | self.motionModel = motionModel 369 | self.mesModel = mesModel 370 | 371 | def update(self, measures, landmarkIds, command, U): 372 | self.__motion_update(command, U) 373 | for ldmIndex, ldmMes in zip(landmarkIds, measures): 374 | self.__measurement_update(ldmMes, int(ldmIndex)) 375 | return self.Sigma, self.mu 376 | 377 | def __motion_update(self, command, U): 378 | previousMeanState = self.mu 379 | meanStateChange = self.motionModel.exact_move(previousMeanState, command) 380 | newMeanState = clipState(previousMeanState + meanStateChange) 381 | 382 | # TO IMPROVE 383 | angle = previousMeanState[2, 0] # TO IMPROVE 384 | gradMeanMotion = np.zeros_like(self.Sigma) # TO IMPROVE 385 | gradMeanMotion[2, 0:2] = command[0] * np.array([-math.sin(angle), math.cos(angle)]) # TO IMPROVE 386 | 387 | IA = np.eye(self.Sigma.shape[0]) + gradMeanMotion # TO IMPROVE 388 | self.mu = newMeanState 389 | self.Sigma = dot(dot(IA, self.Sigma), IA.T) + dot(dot(self.S, U), self.S.T) 390 | 391 | def __measurement_update(self, ldmMes, ldmIndex): 392 | mu = self.mu 393 | Sigma = self.Sigma 394 | meanMes, gradMeanMes = self.__get_mean_measurement_params(mu, ldmIndex) 395 | 396 | z = np.array(ldmMes).reshape(len(ldmMes), 1) 397 | zM = np.array(meanMes).reshape(len(ldmMes), 1) 398 | C = gradMeanMes 399 | 400 | toInvert = inv(dot(dot(C.T, Sigma), C) + self.Z) 401 | K = dot(dot(Sigma, C), toInvert) 402 | 403 | mesError = (z - zM) 404 | mesError[1, 0] = clipAngle(mesError[1, 0], force=True) 405 | mesError = dot(K, mesError) 406 | mesError[1, 0] = clipAngle(mesError[1, 0]) 407 | 408 | self.mu += mesError 409 | self.Sigma = dot(np.eye(self.dimension) - dot(K, C.T), Sigma) 410 | 411 | def __get_mean_measurement_params(self, mu, ldmIndex): 412 | realIndex = self.robotFeaturesDim + ldmIndex * self.envFeaturesDim 413 | ldmMeanState = mu[realIndex: realIndex + self.envFeaturesDim] 414 | rMeanState = mu[:self.robotFeaturesDim] 415 | 416 | meanMes = self.mesModel.measureFunction(rMeanState, ldmMeanState) 417 | gradMeanMes = self.mesModel.gradMeasureFunction(rMeanState, ldmMeanState, realIndex) 418 | return meanMes, gradMeanMes 419 | 420 | def estimate(self): 421 | return self.mu 422 | 423 | 424 | def measureFunction(rState, landmark): 425 | rDim = 3 426 | diff = landmark - rState[:rDim-1] 427 | diffNorm = np.linalg.norm(diff) 428 | 429 | angle = rState[rDim-1, 0] 430 | diffAngle = math.atan2(diff[1], diff[0]) - angle 431 | diffAngle = clipAngle(diffAngle) 432 | 433 | return diffNorm, diffAngle 434 | 435 | 436 | def gradMeasureFunction(rState, landmark, ldmIndex): 437 | rDim = 3 438 | eDim = 2 439 | diff = (rState[:rDim-1] - landmark).flatten() 440 | diffNorm = np.linalg.norm(diff) 441 | 442 | grad = np.zeros(dimension * 2).reshape(dimension, 2) 443 | grad[:rDim-1, 0] = diff / diffNorm 444 | grad[ldmIndex:ldmIndex + eDim, 0] = -grad[:rDim-1, 0] 445 | grad[:rDim-1, 1] = np.array([-diff[1], diff[0]]) / (diffNorm**2) 446 | grad[ldmIndex:ldmIndex + eDim, 1] = -grad[:rDim-1, 1] 447 | grad[rDim-1, 1] = -1 448 | 449 | return grad 450 | 451 | 452 | def clipAngle(angle, force=False): 453 | if clip or force: 454 | angle = (angle + math.pi) % (2 * math.pi) - math.pi 455 | return angle 456 | 457 | 458 | def clipState(state): 459 | if clip: 460 | state[2, 0] = clipAngle(state[2, 0]) 461 | return state 462 | 463 | clip = False 464 | 465 | 466 | if __name__ == '__main__': 467 | 468 | dimension = None 469 | 470 | def simu(): 471 | global dimension 472 | T = 100 # Number of timesteps 473 | nbLandmark = 900 474 | maxSpeed = 5 475 | maxRotation = 45 * math.pi / 180 # 45 # en radians 476 | sizeMap = 50 477 | 478 | # Robot Detection Parameters 479 | detectionSize = 2 # 40 480 | detectionCone = 180 * math.pi / 180 # en radians 481 | 482 | # Dimension Constants 483 | robotFeaturesDim = 3 484 | envFeaturesDim = 2 485 | commandsDim = 2 486 | mesDim = 2 487 | dimension = robotFeaturesDim + nbLandmark * envFeaturesDim 488 | 489 | # Covariances for motions and measurements 490 | covarianceMotion = np.eye(robotFeaturesDim) 491 | covarianceMotion[0, 0] = 1 ** 2 # motion noise variance X 492 | covarianceMotion[1, 1] = 1 ** 2 # motion noise variance Y 493 | covarianceMotion[2, 2] = (5 * math.pi / 180) ** 2 # motion noise variance Angle 494 | 495 | covarianceMeasurements = np.eye(mesDim) 496 | covarianceMeasurements[0, 0] = 1 ** 2 # measurement noise variance distance 497 | covarianceMeasurements[1, 1] = (5 * math.pi / 180) ** 2 # motion noise variance Angle 498 | 499 | 500 | ## ---------------------------------------------------------------------- 501 | ## Simulation initialization 502 | 503 | ## ------------------- 504 | ## State Definition 505 | 506 | # Real robot state 507 | state = np.zeros((dimension, 1)) 508 | 509 | x = np.linspace(-sizeMap, sizeMap, np.sqrt(nbLandmark)) 510 | y = np.linspace(-sizeMap, sizeMap, np.sqrt(nbLandmark)) 511 | xv, yv = np.meshgrid(x, y) 512 | state[robotFeaturesDim:, 0] = np.vstack([xv.ravel(), yv.ravel()]).ravel(order="F") 513 | # state[robotFeaturesDim:] = np.random.rand(nbLandmark * envFeaturesDim).reshape(nbLandmark * envFeaturesDim, 1) * 300 - 150 514 | 515 | 516 | # Basic and EIF estimator for robot state 517 | mu = state.copy() 518 | mu[robotFeaturesDim:] += np.random.normal(0, covarianceMeasurements[0, 0], nbLandmark * envFeaturesDim).reshape(nbLandmark * envFeaturesDim, 1) 519 | muEKF = mu.copy() 520 | muEIF = mu.copy() 521 | muSEIF = mu.copy() 522 | 523 | ## -------------------- 524 | ## Models Definition 525 | 526 | motionModel = BasicMovement(maxSpeed, maxRotation, covarianceMotion, measureFunction) 527 | measurementModel = BasicMeasurement(covarianceMeasurements, robotFeaturesDim, envFeaturesDim, measureFunction, gradMeasureFunction, detectionSize, detectionCone) 528 | ekf = EKFModel(dimension, robotFeaturesDim, envFeaturesDim, motionModel, measurementModel, covarianceMeasurements, mu) 529 | eif = EIFModel(dimension, robotFeaturesDim, envFeaturesDim, motionModel, measurementModel, covarianceMeasurements, mu) 530 | seif = SEIFModel(dimension, robotFeaturesDim, envFeaturesDim, motionModel, measurementModel, covarianceMeasurements, mu, 4) 531 | 532 | mus_simple = np.zeros((T, dimension)) 533 | mus_ekf = np.zeros((T, dimension)) 534 | mus_eif = np.zeros((T, dimension)) 535 | mus_seif = np.zeros((T, dimension)) 536 | states = np.zeros((T, dimension)) 537 | 538 | mus_simple[0] = np.squeeze(mu) 539 | mus_ekf[0] = np.squeeze(muEKF) 540 | mus_eif[0] = np.squeeze(muEIF) 541 | mus_seif[0] = np.squeeze(muEIF) 542 | states[0] = np.squeeze(state) 543 | 544 | 545 | # LOG Initial state 546 | # print("BEFORE") 547 | # print("EIF estimate :") 548 | # print(muEIF) 549 | # print("EKF estimate :") 550 | # print(muEKF) 551 | # print("Real state :") 552 | # print(state) 553 | # print('\n') 554 | 555 | for t in range(1, T): 556 | print("\nIteration %d" % t) 557 | state, motionCommand = motionModel.move(state) 558 | measures, landmarkIds = measurementModel.measure(state) 559 | 560 | mu += motionModel.exact_move(mu, motionCommand) 561 | 562 | # H, _ = ekf.update(measures, landmarkIds, motionCommand, covarianceMotion) 563 | # print (H != 0).sum(), ' / ', H.size 564 | # H, _, _ = eif.update(measures, landmarkIds, motionCommand, covarianceMotion) 565 | # print (H != 0).sum(), ' / ', H.size 566 | H, _, _ = seif.update(measures, landmarkIds, motionCommand, covarianceMotion) 567 | print (H != 0).sum(), ' / ', H.size 568 | 569 | # muEKF = ekf.estimate() 570 | # muEIF = eif.estimate() 571 | muSEIF = seif.estimate() 572 | 573 | # print "np.linalg.norm(muEIF-muSEIF)" 574 | # print np.linalg.norm(muEIF-muSEIF) 575 | # print np.linalg.norm(eif.b - seif.b) 576 | # print np.linalg.norm(eif.H - seif.H) 577 | # print muEIF[:3] 578 | # print muSEIF[:3] 579 | 580 | 581 | mus_simple[t] = np.squeeze(mu) 582 | # mus_ekf[t] = np.squeeze(muEKF) 583 | # mus_eif[t] = np.squeeze(muEIF) 584 | mus_seif[t] = np.squeeze(muSEIF) 585 | states[t] = np.squeeze(state) 586 | 587 | 588 | # # LOG Final state 589 | # print('\n') 590 | # print('AFTER') 591 | # print("EIF estimate :") 592 | # print(muEIF) 593 | # # print("EKF estimate :") 594 | # # print(muEKF) 595 | # print("Real state :") 596 | # print(state) 597 | # print("Final Error EIF:") 598 | # print(state - muEIF) 599 | # # print("Final Error EKF:") 600 | # # print(state - muEKF) 601 | # print("Final Max Error EIF: %f" % max(state-muEIF)) 602 | # print("Final Norm Error EIF: %f" % np.linalg.norm(state-muEIF)) 603 | # # print("Final Max Error EKF: %f" % max(state-muEKF)) 604 | # # print("Final Norm Error EKF: %f" % np.linalg.norm(state-muEKF)) 605 | # print("Final Max Error SEIF: %f" % max(state-muSEIF)) 606 | # print("Final Norm Error SEIF: %f" % np.linalg.norm(state-muSEIF)) 607 | 608 | landmarks = state[robotFeaturesDim:].reshape(nbLandmark, 2) 609 | plt.figure() 610 | ax = plt.gca() 611 | for x, y in landmarks: 612 | ax.add_artist(Circle(xy=(x, y), 613 | radius=detectionSize, 614 | alpha=0.3)) 615 | plt.scatter(landmarks[:, 0], landmarks[:, 1]) 616 | 617 | plt.plot(states[:, 0], states[:, 1]) 618 | plt.plot(mus_simple[:, 0], mus_simple[:, 1]) 619 | # plt.plot(mus_ekf[:, 0], mus_ekf[:, 1]) 620 | # plt.plot(mus_eif[:, 0], mus_eif[:, 1]) 621 | plt.plot(mus_seif[:, 0], mus_seif[:, 1]) 622 | 623 | plt.legend(['Real position', 'Simple estimate', 'EKF estimate', 'EIF estimate', 'SEIF estimate']) 624 | plt.title("{0} landmarks".format(nbLandmark)) 625 | plt.show() 626 | 627 | import cProfile 628 | cProfile.run('simu()') 629 | --------------------------------------------------------------------------------