├── .gitignore ├── README.md ├── kalman_filter.py ├── imm.py ├── plot.py ├── test.py └── data.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | __pycache__/ 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # interacting_multiple_model 2 | An implementation for interacting multiple model(imm) of kalman filter 3 | 4 | A detail description is [imm for prediction](https://note.yongcong.wang/Self-Driving/Prediction/imm-for-prediction/) 5 | -------------------------------------------------------------------------------- /kalman_filter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # By yongcong.wang @ 13/10/2020 4 | 5 | import math 6 | import numpy as np 7 | 8 | class KalmanFilter: 9 | def __init__(self, A, B, H, Q, R): 10 | self.A = A 11 | self.B = B 12 | self.H = H 13 | self.Q = Q 14 | self.R = R 15 | 16 | self.U = np.zeros((B.shape[1], 1)) 17 | self.X = np.zeros((A.shape[0], 1)) 18 | self.X_pre = self.X 19 | self.P = np.zeros(A.shape) 20 | self.P_pre = self.P 21 | 22 | def __init__(self, A, H): 23 | self.A = A 24 | self.B = np.eye(A.shape[0]) 25 | self.H = H 26 | self.Q = np.eye(A.shape[0]) 27 | self.R = np.eye(H.shape[0]) 28 | 29 | self.U = np.zeros((self.B.shape[1], 1)) 30 | self.X = np.zeros((A.shape[0], 1)) 31 | self.X_pre = self.X 32 | self.P = np.zeros(A.shape) 33 | self.P_pre = self.P 34 | 35 | def filt(self, Z): 36 | self.__predict() 37 | self.__update(Z) 38 | return self.X 39 | 40 | def __predict(self): 41 | self.X_pre = np.dot(self.A, self.X) + np.dot(self.B, self.U) 42 | self.P_pre = np.dot(np.dot(self.A, self.P), self.A.T) + self.Q 43 | 44 | def __update(self, Z): 45 | K = np.dot(np.dot(self.P_pre, self.H.T), 46 | np.linalg.inv(np.dot(np.dot(self.H, self.P_pre), self.H.T) +\ 47 | self.R)) 48 | self.X = self.X_pre + np.dot(K, Z - np.dot(self.H, self.X_pre)) 49 | self.P = self.P_pre - np.dot(np.dot(K, self.H), self.P_pre) 50 | -------------------------------------------------------------------------------- /imm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # By yongcong.wang @ 13/10/2020 4 | 5 | import math 6 | import numpy as np 7 | 8 | class Imm: 9 | def __init__(self, models, model_trans, P_trans, U_prob): 10 | self.models = models 11 | self.P_trans = P_trans 12 | self.U_prob = U_prob 13 | self.model_trans = model_trans 14 | 15 | self.mode_cnt = len(models) 16 | self.dim = models[0].A.shape[0] 17 | 18 | def filt(self, Z): 19 | # setp1: input mix 20 | u = np.dot(self.P_trans.T, self.U_prob) 21 | mu = np.zeros(self.P_trans.shape) 22 | for i in range(self.mode_cnt): 23 | for j in range(self.mode_cnt): 24 | mu[i, j] = self.P_trans[i, j] * self.U_prob[i, 0] / u[j, 0]; 25 | 26 | X_mix = [np.zeros(model.X.shape) for model in self.models] 27 | 28 | for j in range(self.mode_cnt): 29 | for i in range(self.mode_cnt): 30 | X_mix[j] += np.dot(self.model_trans[j][i], 31 | self.models[i].X) * mu[i, j] 32 | 33 | P_mix = [np.zeros(model.P.shape) for model in self.models] 34 | for j in range(self.mode_cnt): 35 | for i in range(self.mode_cnt): 36 | P = self.models[i].P + np.dot((self.models[i].X - X_mix[i]), 37 | (self.models[i].X - X_mix[i]).T) 38 | P_mix[j] += mu[i, j] * np.dot(np.dot(self.model_trans[j][i], P), 39 | self.model_trans[j][i].T) 40 | ## step2: filt 41 | for j in range(self.mode_cnt): 42 | self.models[j].X = X_mix[j] 43 | self.models[j].P = P_mix[j] 44 | self.models[j].filt(Z) 45 | 46 | ### step3: update probability 47 | for j in range(self.mode_cnt): 48 | mode = self.models[j] 49 | D = Z - np.dot(mode.H, mode.X_pre) 50 | S = np.dot(np.dot(mode.H, mode.P_pre), mode.H.T) + mode.R 51 | 52 | Lambda = (np.linalg.det(2 * math.pi * S)) ** (-0.5) * \ 53 | np.exp(-0.5 * np.dot(np.dot(D.T, np.linalg.inv(S)), D)) 54 | self.U_prob[j, 0] = Lambda * u[j, 0] 55 | self.U_prob = self.U_prob / np.sum(self.U_prob) 56 | 57 | return self.U_prob 58 | -------------------------------------------------------------------------------- /plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # By yongcong.wang @ 13/10/2020 4 | 5 | import matplotlib.pyplot as plt 6 | import numpy as np 7 | 8 | def plot_prediction(noise_x, noise_y, pred_x, pred_y): 9 | plt.figure(2) 10 | plt.plot(noise_x, noise_y, color='r', linewidth=4, label='pos_real') 11 | for i in range(len(pred_x)): 12 | plt.plot(pred_x[i], pred_y[i], color='g', alpha=0.6) 13 | plt.legend() 14 | 15 | def plot_position(std_x, std_y, noise_x, noise_y, filt_x, filt_y): 16 | plt.figure(1) 17 | 18 | plt.subplot(321) 19 | plt.plot(std_x, std_y, color='r', label='pos_std') 20 | plt.plot(noise_x, noise_y, color='b', label='pos_noise') 21 | plt.plot(filt_x, filt_y, color='g', label='pos_filt') 22 | plt.legend() 23 | 24 | plt.subplot(322) 25 | t = np.arange(0, len(std_x)) 26 | plt.plot(t, [noise_x[i] - std_x[i] for i in range(len(std_x))], 27 | color='r', label='pos_err_noise_x') 28 | plt.plot(t, [noise_y[i] - std_y[i] for i in range(len(std_y))], 29 | color='b', label='pos_err_noise_y') 30 | plt.plot(t, [filt_x[i] - std_x[i] for i in range(len(std_x))], 31 | color='g', label='pos_err_filt_x') 32 | plt.plot(t, [filt_y[i] - std_y[i] for i in range(len(std_y))], 33 | color='c', label='pos_err_filt_y') 34 | plt.legend() 35 | 36 | def plot_speed(std_dx, std_dy, noise_dx, noise_dy, filt_dx, filt_dy): 37 | plt.figure(1) 38 | 39 | plt.subplot(323) 40 | plt.plot(std_dx, std_dy, color='r', label='spd_std') 41 | plt.plot(noise_dx, noise_dy, color='b', label='spd_noise') 42 | plt.plot(filt_dx, filt_dy, color='g', label='spd_filt') 43 | plt.legend() 44 | 45 | plt.subplot(324) 46 | t = np.arange(0, len(std_dx)) 47 | plt.plot(t, [noise_dx[i] - std_dx[i] for i in range(len(std_dx))], 48 | color='r', label='spd_err_noise_dx') 49 | plt.plot(t, [noise_dy[i] - std_dy[i] for i in range(len(std_dy))], 50 | color='b', label='spd_err_noise_dy') 51 | plt.plot(t, [filt_dx[i] - std_dx[i] for i in range(len(std_dx))], 52 | color='g', label='spd_err_filt_dx') 53 | plt.plot(t, [filt_dy[i] - std_dy[i] for i in range(len(std_dy))], 54 | color='c', label='spd_err_filt_dy') 55 | plt.legend() 56 | 57 | def plot_prob(cv, ca, ct): 58 | plt.figure(1) 59 | 60 | plt.subplot(313) 61 | t = np.arange(0, len(cv)) 62 | plt.plot(t, cv, color='r', label='prob_cv') 63 | plt.plot(t, ca, color='b', label='prob_ca') 64 | plt.plot(t, ct, color='g', label='prob_ct') 65 | plt.legend() 66 | 67 | def plot_show(): 68 | plt.show() 69 | -------------------------------------------------------------------------------- /test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # By yongcong.wang @ 13/10/2020 4 | import math 5 | import numpy as np 6 | 7 | from kalman_filter import KalmanFilter 8 | from imm import Imm 9 | import data 10 | from plot import * 11 | 12 | dt = 0.1 13 | 14 | def kf_cv(): 15 | A = np.array([ 16 | [1., dt, 0., 0.], 17 | [0., 1., 0., 0.], 18 | [0., 0., 1., dt], 19 | [0., 0., 0., 1.] 20 | ]) 21 | H = np.array([ 22 | [1., 0., 0., 0.], 23 | [0., 1., 0., 0.], 24 | [0., 0., 1., 0.], 25 | [0., 0., 0., 1.] 26 | ]) 27 | return KalmanFilter(A, H) 28 | 29 | def kf_ca(): 30 | A = np.array([ 31 | [1., dt, 0.5 * dt**2, 0., 0., 0.], 32 | [0., 1., dt, 0., 0., 0.], 33 | [0., 0., 1., 0., 0., 0.], 34 | [0., 0., 0., 1., dt, 0.5 * dt**2], 35 | [0., 0., 0., 0., 1., dt], 36 | [0., 0., 1., 0., 0., 1.] 37 | ]) 38 | H = np.array([ 39 | [1., 0., 0., 0., 0., 0.], 40 | [0., 1., 0., 0., 0., 0.], 41 | [0., 0., 0., 1., 0., 0.], 42 | [0., 0., 0., 0., 1., 0.] 43 | ]) 44 | return KalmanFilter(A, H) 45 | 46 | def kf_ct(): 47 | dtheta = math.pi / 180 * 15 48 | theta = dtheta * dt 49 | A = np.array([ 50 | [1., math.sin(theta)/dtheta, 0., -(1 - math.cos(theta))/dtheta, 0.], 51 | [0., math.cos(theta), 0., -math.sin(theta), 0.], 52 | [0., (1 - math.cos(theta)) / dtheta, 1., math.sin(theta)/dtheta, 0.], 53 | [0., math.sin(theta), 0., math.cos(theta), 0.], 54 | [0., 0., 0., 0., 1.], 55 | ]) 56 | H = np.array([ 57 | [1., 0., 0., 0., 0.], 58 | [0., 1., 0., 0., 0.], 59 | [0., 0., 1., 0., 0.], 60 | [0., 0., 0., 1., 0.] 61 | ]) 62 | return KalmanFilter(A, H) 63 | 64 | def imm_cvat(): 65 | P_trans = np.array([ 66 | [0.98, 0.01, 0.01], 67 | [0.01, 0.98, 0.01], 68 | [0.01, 0.01, 0.98] 69 | ]) 70 | U_prob = np.array([0.8, 0.1, 0.1]).reshape((-1, 1)) 71 | 72 | models = [kf_cv(), kf_ca(), kf_ct()] 73 | r = np.array([ 74 | [5.], 75 | [2.], 76 | [5.], 77 | [2.5] 78 | ]) 79 | for model in models: 80 | model.R *= r 81 | 82 | T12 = np.array([ 83 | [1, 0, 0, 0], 84 | [0, 1, 0, 0], 85 | [0, 0, 0, 0], 86 | [0, 0, 1, 0], 87 | [0, 0, 0, 1], 88 | [0, 0, 0, 0] 89 | ]) 90 | T23 = np.array([ 91 | [1, 0, 0, 0, 0, 0], 92 | [0, 1, 0, 0, 0, 0], 93 | [0, 0, 0, 1, 0, 0], 94 | [0, 0, 0, 0, 1, 0], 95 | [0, 0, 0, 0, 0, 0] 96 | ]) 97 | model_trans = [ 98 | [np.eye(models[0].A.shape[0]), T12.T, np.dot(T12.T, T23.T)], 99 | [T12, np.eye(models[1].A.shape[0]), T23.T], 100 | [np.dot(T23, T12), T23, np.eye(models[2].A.shape[0])] 101 | ] 102 | 103 | return Imm(models, model_trans, P_trans, U_prob) 104 | 105 | def imm_cvt(): 106 | P_trans = np.array([ 107 | [0.98, 0.02], 108 | [0.02, 0.98] 109 | ]) 110 | U_prob = np.array([0.5, 0.5]).reshape((-1, 1)) 111 | 112 | models = [kf_cv(), kf_ct()] 113 | r = np.array([ 114 | [10.], 115 | [1.], 116 | [10.], 117 | [1.] 118 | ]) 119 | for model in models: 120 | model.R *= r 121 | 122 | T12 = np.array([ 123 | [1, 0, 0, 0], 124 | [0, 1, 0, 0], 125 | [0, 0, 0, 0], 126 | [0, 0, 1, 0], 127 | [0, 0, 0, 1], 128 | [0, 0, 0, 0] 129 | ]) 130 | T23 = np.array([ 131 | [1, 0, 0, 0, 0, 0], 132 | [0, 1, 0, 0, 0, 0], 133 | [0, 0, 0, 1, 0, 0], 134 | [0, 0, 0, 0, 1, 0], 135 | [0, 0, 0, 0, 0, 0] 136 | ]) 137 | T13 = np.dot(T23, T12) 138 | model_trans = [ 139 | [np.eye(models[0].A.shape[0]), T13.T], 140 | [T13, np.eye(models[1].A.shape[0])] 141 | ] 142 | 143 | return Imm(models, model_trans, P_trans, U_prob) 144 | 145 | def z_data(): 146 | cnt = 100 147 | z_std = data.cv_z(0., 10., 0., 10., dt, cnt) 148 | z_std += data.ct_z(z_std[-1][0,0], z_std[-1][1,0], 149 | z_std[-1][2,0], z_std[-1][3,0], math.pi/180*25, dt, cnt) 150 | z_std += data.ca_z(z_std[-1][0,0], z_std[-1][1,0], 6., 151 | z_std[-1][2,0], z_std[-1][3,0], 8., dt, cnt) 152 | 153 | return z_std 154 | 155 | def test_cvt(): 156 | z_std = z_data() 157 | z_noise = data.add_noise(z_std, np.array([ 158 | [5.], 159 | [.2], 160 | [5.], 161 | [.2] 162 | ])) 163 | 164 | imm = imm_cvt(); 165 | z0 = z_noise[0] 166 | imm.models[0].X = np.array([ 167 | [z0[0, 0]], 168 | [z0[1, 0]], 169 | [z0[2, 0]], 170 | [z0[3, 0]] 171 | ]) 172 | imm.models[1].X = np.array([ 173 | [z0[0, 0]], 174 | [z0[1, 0]], 175 | [z0[2, 0]], 176 | [z0[3, 0]], 177 | [0.] 178 | ]) 179 | 180 | prob = [] 181 | z_filt = [] 182 | for z in z_noise: 183 | prob.append(np.copy(imm.filt(z))) 184 | # merge 185 | x = np.zeros(imm.models[0].X.shape) 186 | for i in range(len(imm.models)): 187 | x += np.dot(imm.model_trans[0][i], imm.models[i].X) * prob[-1][i] 188 | z_filt.append(x) 189 | 190 | plot_position( 191 | [z[0,0] for z in z_std], 192 | [z[2,0] for z in z_std], 193 | [z[0,0] for z in z_noise], 194 | [z[2,0] for z in z_noise], 195 | [z[0,0] for z in z_filt], 196 | [z[2,0] for z in z_filt] 197 | ) 198 | plot_speed( 199 | [z[1,0] for z in z_std], 200 | [z[3,0] for z in z_std], 201 | [z[1,0] for z in z_noise], 202 | [z[3,0] for z in z_noise], 203 | [z[1,0] for z in z_filt], 204 | [z[3,0] for z in z_filt] 205 | ) 206 | plot_prob( 207 | [p[0,0] for p in prob], 208 | [p[1,0] for p in prob], 209 | [p[1,0] for p in prob], 210 | ) 211 | plot_show() 212 | #test_cvt() 213 | 214 | def test_cvat(): 215 | z_std = z_data() 216 | z_noise = data.add_noise(z_std, np.array([ 217 | [5.], 218 | [2], 219 | [5.], 220 | [2] 221 | ])) 222 | 223 | imm = imm_cvat(); 224 | z0 = z_noise[0] 225 | imm.models[0].X = np.array([ 226 | [z0[0, 0]], 227 | [z0[1, 0]], 228 | [z0[2, 0]], 229 | [z0[3, 0]] 230 | ]) 231 | imm.models[1].X = np.array([ 232 | [z0[0, 0]], 233 | [z0[1, 0]], 234 | [0.], 235 | [z0[2, 0]], 236 | [z0[3, 0]], 237 | [0.] 238 | ]) 239 | imm.models[2].X = np.array([ 240 | [z0[0, 0]], 241 | [z0[1, 0]], 242 | [z0[2, 0]], 243 | [z0[3, 0]], 244 | [0.] 245 | ]) 246 | 247 | prob = [] 248 | z_filt = [] 249 | for z in z_noise: 250 | prob.append(np.copy(imm.filt(z))) 251 | # merge 252 | x = np.zeros(imm.models[0].X.shape) 253 | for i in range(len(imm.models)): 254 | x += np.dot(imm.model_trans[0][i], imm.models[i].X) * prob[-1][i] 255 | z_filt.append(x) 256 | 257 | plot_position( 258 | [z[0,0] for z in z_std], 259 | [z[2,0] for z in z_std], 260 | [z[0,0] for z in z_noise], 261 | [z[2,0] for z in z_noise], 262 | [z[0,0] for z in z_filt], 263 | [z[2,0] for z in z_filt] 264 | ) 265 | plot_speed( 266 | [z[1,0] for z in z_std], 267 | [z[3,0] for z in z_std], 268 | [z[1,0] for z in z_noise], 269 | [z[3,0] for z in z_noise], 270 | [z[1,0] for z in z_filt], 271 | [z[3,0] for z in z_filt] 272 | ) 273 | plot_prob( 274 | [p[0,0] for p in prob], 275 | [p[1,0] for p in prob], 276 | [p[2,0] for p in prob], 277 | ) 278 | plot_show() 279 | 280 | #test_cvat() 281 | 282 | def test_imm_veh(): 283 | z_noise = data.veh_z_mia() 284 | 285 | imm = imm_cvat(); 286 | z0 = z_noise[0] 287 | imm.models[0].X = np.array([ 288 | [z0[0, 0]], 289 | [z0[1, 0]], 290 | [z0[2, 0]], 291 | [z0[3, 0]] 292 | ]) 293 | imm.models[1].X = np.array([ 294 | [z0[0, 0]], 295 | [z0[1, 0]], 296 | [0.], 297 | [z0[2, 0]], 298 | [z0[3, 0]], 299 | [0.] 300 | ]) 301 | imm.models[2].X = np.array([ 302 | [z0[0, 0]], 303 | [z0[1, 0]], 304 | [z0[2, 0]], 305 | [z0[3, 0]], 306 | [0.] 307 | ]) 308 | 309 | prob = [] 310 | z_filt = [] 311 | for z in z_noise: 312 | prob.append(np.copy(imm.filt(z))) 313 | # merge 314 | x = np.zeros(imm.models[0].X.shape) 315 | for i in range(len(imm.models)): 316 | x += np.dot(imm.model_trans[0][i], imm.models[i].X) * prob[-1][i] 317 | z_filt.append(x) 318 | #return 319 | 320 | plot_position( 321 | [z[0,0] for z in z_noise], 322 | [z[2,0] for z in z_noise], 323 | [z[0,0] for z in z_noise], 324 | [z[2,0] for z in z_noise], 325 | [z[0,0] for z in z_filt], 326 | [z[2,0] for z in z_filt] 327 | ) 328 | plot_speed( 329 | [z[1,0] for z in z_noise], 330 | [z[3,0] for z in z_noise], 331 | [z[1,0] for z in z_noise], 332 | [z[3,0] for z in z_noise], 333 | [z[1,0] for z in z_filt], 334 | [z[3,0] for z in z_filt] 335 | ) 336 | plot_prob( 337 | [p[0,0] for p in prob], 338 | [p[1,0] for p in prob], 339 | [p[2,0] for p in prob], 340 | ) 341 | plot_show() 342 | 343 | def test_imm_veh_pred(): 344 | z_noise = data.veh_z_mia() 345 | 346 | imm = imm_cvat(); 347 | z0 = z_noise[0] 348 | imm.models[0].X = np.array([ 349 | [z0[0, 0]], 350 | [z0[1, 0]], 351 | [z0[2, 0]], 352 | [z0[3, 0]] 353 | ]) 354 | imm.models[1].X = np.array([ 355 | [z0[0, 0]], 356 | [z0[1, 0]], 357 | [0.], 358 | [z0[2, 0]], 359 | [z0[3, 0]], 360 | [0.] 361 | ]) 362 | imm.models[2].X = np.array([ 363 | [z0[0, 0]], 364 | [z0[1, 0]], 365 | [z0[2, 0]], 366 | [z0[3, 0]], 367 | [0.] 368 | ]) 369 | 370 | prob = [] 371 | z_filt = [] 372 | pred_z = [] 373 | for z in z_noise: 374 | prob.append(np.copy(imm.filt(z))) 375 | # merge 376 | x = np.zeros(imm.models[0].X.shape) 377 | for i in range(len(imm.models)): 378 | x += np.dot(imm.model_trans[0][i], imm.models[i].X) * prob[-1][i] 379 | z_filt.append(x) 380 | 381 | # predict trajectory 382 | states = [imm.models[0].X.copy(), 383 | imm.models[1].X.copy(), 384 | imm.models[2].X.copy()] 385 | pred_step = [] 386 | for i in range(50): # predict 5s 387 | for i in range(len(states)): # each model predict 388 | states[i] = np.dot(imm.models[i].A, states[i]) 389 | x_step = np.zeros(x.shape) 390 | for i in range(len(imm.models)): # merge predict 391 | x_step += np.dot(imm.model_trans[0][i], states[i]) * prob[-1][i] 392 | pred_step.append(x_step.copy()) 393 | pred_z.append(pred_step) 394 | 395 | 396 | plot_position( 397 | [z[0,0] for z in z_noise], 398 | [z[2,0] for z in z_noise], 399 | [z[0,0] for z in z_noise], 400 | [z[2,0] for z in z_noise], 401 | [z[0,0] for z in z_filt], 402 | [z[2,0] for z in z_filt] 403 | ) 404 | plot_speed( 405 | [z[1,0] for z in z_noise], 406 | [z[3,0] for z in z_noise], 407 | [z[1,0] for z in z_noise], 408 | [z[3,0] for z in z_noise], 409 | [z[1,0] for z in z_filt], 410 | [z[3,0] for z in z_filt] 411 | ) 412 | plot_prob( 413 | [p[0,0] for p in prob], 414 | [p[1,0] for p in prob], 415 | [p[2,0] for p in prob], 416 | ) 417 | pred_x = [] 418 | pred_y = [] 419 | for step_z in pred_z: 420 | curr_x = [z[0,0] for z in step_z] 421 | pred_x.append(curr_x) 422 | curr_y = [z[2,0] for z in step_z] 423 | pred_y.append(curr_y) 424 | plot_prediction( 425 | [z[0,0] for z in z_noise], 426 | [z[2,0] for z in z_noise], 427 | pred_x, 428 | pred_y 429 | ) 430 | 431 | plot_show() 432 | 433 | test_imm_veh_pred() 434 | -------------------------------------------------------------------------------- /data.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # By yongcong.wang @ 13/10/2020 4 | 5 | import math 6 | import numpy as np 7 | 8 | def cv_z(x0, dx, y0, dy, dt, cnt): 9 | Z = [np.array([ 10 | [x0], 11 | [dx], 12 | [y0], 13 | [dy] 14 | ])] 15 | for i in np.arange(1, cnt): 16 | Z.append(np.array([ 17 | [Z[i-1][0, 0] + dx * dt], 18 | [dx], 19 | [Z[i-1][2, 0] + dy * dt], 20 | [dy] 21 | ])) 22 | 23 | return Z 24 | 25 | def ca_z(x0, dx, ddx, y0, dy, ddy, dt, cnt): 26 | Z = [np.array([ 27 | [x0], 28 | [dx], 29 | [y0], 30 | [dy] 31 | ])] 32 | for i in np.arange(1, cnt): 33 | Z.append(np.array([ 34 | [Z[i-1][0,0] + Z[i-1][1,0] * dt + 0.5 * ddx * dt**2], 35 | [Z[i-1][1,0]+ ddx * dt], 36 | [Z[i-1][2,0] + Z[i-1][3,0] * dt + 0.5 * ddy * dt**2], 37 | [Z[i-1][3,0]+ ddy * dt] 38 | ])) 39 | 40 | return Z 41 | 42 | def ct_z(x0, dx, y0, dy, dtheta, dt, cnt): 43 | Z = [np.array([ 44 | [x0], 45 | [dx], 46 | [y0], 47 | [dy] 48 | ])] 49 | theta = math.atan2(dy, dx) 50 | v = math.hypot(dx, dy) 51 | for i in np.arange(1, cnt): 52 | theta += dtheta * dt 53 | Z.append(np.array([ 54 | [Z[i-1][0, 0] + v * dt * math.cos(theta)], 55 | [v * math.cos(theta)], 56 | [Z[i-1][2, 0] + v * dt * math.sin(theta)], 57 | [v * math.sin(theta)] 58 | ])) 59 | 60 | return Z 61 | 62 | def add_noise(Z, gain): 63 | return [np.copy(z) + gain * (np.random.random(z.shape) - 0.5) for z in Z] 64 | 65 | def veh_z(): 66 | veh_x = [366663.97602476494, 366662.9946207391, 366662.6759009701, 366661.1622944487, 366659.58930389007, 366657.6565741472, 366656.4828280753, 366656.15361394116, 366654.88519447984, 366653.76224957936, 366653.1054994988, 366651.7114348413, 366651.16369055305, 366650.874347754, 366650.0616439397, 366649.3777791009, 366649.03453845903, 366648.4280717088, 366647.5943679608, 366647.2221279375, 366646.8465561758, 366646.5085824456, 366646.1340383063, 366644.3427743707, 366643.5881165787, 366642.846131407, 366642.45244532847, 366641.6908466484, 366641.3021880112, 366640.02856995794, 366639.4418653023, 366638.20045708644, 366637.2715907917, 366635.7736804261, 366634.0023193349, 366632.9302085623, 366632.56700302905, 366631.17368761724, 366630.0236935694, 366628.5333584803, 366627.37944804167, 366625.0030052651, 366623.1318177335, 366621.37524976314, 366621.06438229943, 366620.2211088776, 366619.4377327408, 366618.9085942329, 366618.006810715, 366616.7322307855, 366614.46118184115, 366612.7030734222, 366611.06044642493, 366610.19472178846, 366609.39105654863, 366606.6090175863, 366604.3080086172, 366598.9060458723, 366597.7777024018, 366596.7195034872, 366595.4905359966, 366593.75419501954, 366591.76287830947, 366590.9316969246, 366588.9129293184, 366586.7517641183, 366586.503739217, 366585.06951231934, 366582.5079019981, 366581.6616934893, 366580.8332130943, 366579.0708624139, 366579.0822817867, 366574.04615401925, 366572.23182608327, 366569.8904884309, 366569.20304190996, 366566.1490964378, 366564.1110583323, 366561.0055656462, 366557.7588090418] 67 | veh_y = [3452794.722778085, 3452794.7793918317, 3452794.7846858776, 3452794.8425639765, 3452794.859859606, 3452794.943143552, 3452795.01383029, 3452795.0187155725, 3452795.0543980645, 3452795.056762374, 3452795.105838403, 3452795.1812158637, 3452795.1416444327, 3452795.2034430234, 3452795.199478978, 3452795.2230842337, 3452795.228185663, 3452795.222976726, 3452795.4319262994, 3452795.392519834, 3452795.451516682, 3452795.2734702644, 3452795.403336747, 3452795.355112594, 3452795.3687331784, 3452795.271425586, 3452795.2614913397, 3452795.4582046536, 3452795.4890135247, 3452795.503599826, 3452795.5383209675, 3452795.412151686, 3452795.482762867, 3452795.4739192803, 3452795.3478328297, 3452795.4267890877, 3452795.392277105, 3452795.2418531007, 3452795.320684593, 3452795.118571157, 3452795.0164180165, 3452794.931638216, 3452794.7338928366, 3452794.719611816, 3452794.731344927, 3452794.7156484094, 3452794.6497643963, 3452794.66159596, 3452794.6561888997, 3452794.6781059774, 3452794.808928909, 3452794.754951435, 3452794.7838909705, 3452795.00084411, 3452795.062406074, 3452795.4179719803, 3452795.6870684233, 3452796.0814652485, 3452796.257860994, 3452796.1570433574, 3452796.2587265177, 3452796.341382583, 3452796.3802440464, 3452796.3916408345, 3452796.443479321, 3452796.5079668774, 3452796.485522179, 3452796.4657549523, 3452796.637628183, 3452796.514723928, 3452796.569685999, 3452796.716624091, 3452796.5667564655, 3452796.5611726535, 3452796.6226344635, 3452796.597070849, 3452796.6642564, 3452796.705468187, 3452796.6754435212, 3452796.777854291, 3452796.788157245] 68 | veh_vx = [-2.0457301139831543, -1.6275277137756348, -1.481507420539856, -1.5721306800842285, -1.5913820266723633, -1.711446762084961, -1.9512752294540405, -1.9528429508209229, -2.223374843597412, -2.3085055351257324, -2.4777731895446777, -2.50956654548645, -2.6480636596679688, -2.973581552505493, -2.904118061065674, -2.7575762271881104, -2.948342800140381, -2.877401828765869, -2.8440134525299072, -3.0466771125793457, -2.7784242630004883, -3.068000316619873, -2.8528850078582764, -3.2648470401763916, -3.2343382835388184, -3.480713129043579, -3.6524412631988525, -3.755077838897705, -3.7810585498809814, -3.990694046020508, -4.023892879486084, -4.242422580718994, -4.376273155212402, -4.49798059463501, -4.870119571685791, -5.108942031860352, -5.322845935821533, -5.2171478271484375, -5.0520734786987305, -5.521345615386963, -5.024937629699707, -4.704761981964111, -4.441832542419434, -4.153748989105225, -3.8849973678588867, -3.9184117317199707, -3.839837074279785, -3.8297390937805176, -3.7738473415374756, -3.8142247200012207, -3.9582109451293945, -3.847064971923828, -3.780780076980591, -3.576749801635742, -3.777549982070923, -4.601053714752197, -4.988664150238037, -5.534853458404541, -5.820122718811035, -5.744226932525635, -5.778326034545898, -5.627255916595459, -5.893857002258301, -5.749251365661621, -5.787508964538574, -5.773934364318848, -5.836709499359131, -5.809682846069336, -6.123110771179199, -6.851296901702881, -6.6507415771484375, -6.651390075683594, -6.995279312133789, -7.601310729980469, -7.775839328765869, -7.883184909820557, -7.9863176345825195, -7.968554496765137, -8.232288360595703, -8.069657325744629, -8.382157325744629] 69 | veh_vy = [0.03213828057050705, 0.059689901769161224, 0.035077374428510666, 0.04563223570585251, -0.0016576583730056882, 0.024200748652219772, 0.059037480503320694, 0.02201271429657936, 0.10545993596315384, 0.11560820788145065, 0.0005272969137877226, -0.028616689145565033, -0.0023022545501589775, -0.021318262442946434, -0.04088643565773964, 0.07851170003414154, 0.05645432323217392, 0.0007357442518696189, 0.045978743582963943, 0.1062195673584938, 0.13143151998519897, 0.1833164542913437, 0.0968942791223526, 0.0763341411948204, 0.048504866659641266, -0.13284415006637573, -0.11469078809022903, 0.15059320628643036, 0.24191589653491974, 0.20724499225616455, 0.2838839292526245, 0.16337160766124725, 0.19383645057678223, -0.034293510019779205, -0.23361560702323914, -0.21607182919979095, -0.22777225077152252, -0.32631608843803406, -0.45699799060821533, -0.31210339069366455, -0.23902200162410736, -0.08350783586502075, -0.10640449076890945, -0.052984755486249924, 0.03312419354915619, 0.19803743064403534, 0.3585452735424042, 0.3592667877674103, 0.38253459334373474, 0.24462901055812836, 0.23259013891220093, 0.24920426309108734, 0.3197888135910034, 0.33777230978012085, 0.3358265459537506, 0.5403535962104797, 0.4058835804462433, 0.27981337904930115, 0.4426557719707489, 0.39783811569213867, 0.49450671672821045, 0.4396500289440155, 0.273470014333725, 0.10902631282806396, 0.06052320823073387, 0.19513508677482605, 0.21698692440986633, 0.18330241739749908, 0.3825306296348572, 0.8777614235877991, 0.5815287828445435, 0.5906145572662354, 0.33125585317611694, 0.7801445126533508, -0.1767507791519165, 0.07661654055118561, 0.2465013712644577, 0.26706191897392273, 0.4562828540802002, 0.1739216446876526, 0.2454816997051239] 70 | 71 | z = [] 72 | for i in range(len(veh_x)): 73 | z.append(np.array([ 74 | [veh_x[i]], 75 | [veh_vx[i]], 76 | [veh_y[i]], 77 | [veh_vy[i]] 78 | ])) 79 | return z 80 | 81 | def ped_z(): 82 | ped_x = [366583.7586207068, 366584.0128932324, 366584.56668470363, 366584.8965274246, 366584.96823871817, 366585.10149136325, 366585.2266491947, 366585.8902639438, 366586.08900118154, 366586.21407185856, 366586.36237363383, 366586.99014688557, 366587.1842773995, 366587.2836528685, 366587.4213557554, 366587.61235941236, 366587.6200707757, 366587.7171604042, 366587.83296858775, 366588.446648015, 366588.4501254802, 366588.55877887533, 366588.7504491969, 366588.97016307304, 366589.2312950094, 366589.27575698216, 366589.32829185756, 366589.90464578057, 366590.144232042, 366590.24586949614, 366590.3177384324, 366590.40835125186, 366590.4665030377, 366590.49541966, 366590.64131664846, 366590.9943414579, 366591.1525334037, 366591.38564666064, 366591.53952350846, 366591.4116780433, 366591.63099214504, 366591.5618100828, 366591.7017052815, 366592.06871697406, 366592.1239869958, 366592.33838332817, 366592.6706793031, 366592.6894883165, 366592.9176207316, 366593.02116356516, 366593.43264232314, 366593.52811882726, 366593.50043680664, 366593.4953036112, 366593.7443676923, 366593.7523135081, 366593.9357344086, 366593.91548581555, 366594.1661268653, 366593.9963479345, 366594.12665532186, 366594.0540274834, 366594.1238355222, 366594.26799606916, 366594.2401390067, 366594.2774107874, 366594.3241178746, 366594.3921234738, 366594.39751701383, 366594.44049976225, 366594.42507136706, 366594.39357081545, 366594.5079210908] 83 | ped_y = [3452802.23274165, 3452802.2427489734, 3452802.157380052, 3452801.9229381387, 3452801.890366522, 3452801.943989553, 3452801.8828531355, 3452801.7501186607, 3452801.8306867997, 3452801.77883276, 3452801.752584325, 3452801.68809742, 3452801.586407931, 3452801.5152029754, 3452801.5195512967, 3452801.536490657, 3452801.4461812703, 3452801.506080153, 3452801.3508779416, 3452801.290994734, 3452801.290480953, 3452801.2281541023, 3452801.2339063124, 3452801.1602182873, 3452801.1165534845, 3452801.060248124, 3452801.028314167, 3452800.9844999015, 3452800.903987874, 3452800.8378190976, 3452800.9210015624, 3452800.899311129, 3452800.8318012473, 3452800.88191144, 3452800.874475844, 3452800.6752950405, 3452800.3824638696, 3452800.290975943, 3452800.278803653, 3452800.528730679, 3452800.1066831076, 3452799.981034644, 3452799.9040191146, 3452799.5212299614, 3452799.504520814, 3452799.1069297534, 3452798.6167423273, 3452798.6224304806, 3452797.942536528, 3452797.845152785, 3452797.171482465, 3452797.102554293, 3452796.9712729487, 3452796.870552968, 3452796.403340308, 3452796.3752547265, 3452796.2037282563, 3452796.0248268186, 3452795.8290828113, 3452795.802518943, 3452795.6734956712, 3452795.4888434033, 3452795.38760076, 3452795.3074917775, 3452795.1220016493, 3452795.059586271, 3452794.870385479, 3452794.772795678, 3452794.6688250834, 3452794.6000469583, 3452794.4997020387, 3452794.3381582284, 3452794.2620108044] 84 | ped_vx = [0.7238256335258484, 0.639672577381134, 0.7897564172744751, 0.6585690975189209, 0.6791093945503235, 0.7945690751075745, 0.6016510725021362, 0.460549920797348, 0.7457672953605652, 0.8732098340988159, 0.805212676525116, 1.0519752502441406, 1.1709372997283936, 1.1090607643127441, 1.060754418373108, 1.2517929077148438, 1.2421702146530151, 1.2008384466171265, 1.060544490814209, 1.0827932357788086, 0.9608871936798096, 0.9979074001312256, 1.2702217102050781, 1.262911319732666, 1.108064889907837, 1.178846001625061, 1.1997307538986206, 0.8758773803710938, 1.1190592050552368, 1.0902714729309082, 1.0724226236343384, 0.9496291875839233, 0.8632647395133972, 0.8452659845352173, 0.9867776036262512, 0.847038984298706, 0.7623805999755859, 0.8793231844902039, 0.9325444102287292, 0.7495713829994202, 0.5435964465141296, 0.28813278675079346, 0.46497365832328796, 0.5219458937644958, 0.444953590631485, 0.3514465093612671, 0.5726141333580017, 0.668704628944397, 0.4978678226470947, 0.37889906764030457, 0.5812103152275085, 0.6595303416252136, 0.39587685465812683, 0.41512081027030945, 0.473829448223114, 0.4206877052783966, 0.5919029116630554, 0.5470722317695618, 0.5153967142105103, 0.2822650671005249, 0.42101576924324036, 0.19177837669849396, 0.26111966371536255, 0.5238641500473022, 0.3501468300819397, 0.2952496111392975, 0.4474983811378479, 0.451397567987442, 0.5671170949935913, 0.5188154578208923, 0.4692383408546448, 0.2568801939487457, 0.41573265194892883] 85 | ped_vy = [-0.2130500227212906, -0.10842117667198181, -0.30901020765304565, -0.3418917655944824, -0.3550146222114563, -0.11495163291692734, -0.06626948714256287, -0.115512914955616, -0.1414618343114853, -0.13042031228542328, -0.05618571490049362, -0.233009934425354, -0.3721402883529663, -0.5309042930603027, -0.5551295280456543, -0.4548669755458832, -0.4668411910533905, -0.2531614303588867, -0.5055666565895081, -0.372606098651886, -0.3980276882648468, -0.5646659135818481, -0.49040424823760986, -0.3770105838775635, -0.30570921301841736, -0.38546639680862427, -0.38920164108276367, -0.17316950857639313, -0.17036254703998566, -0.1049417108297348, -0.17095723748207092, -0.16204555332660675, -0.3541968762874603, -0.23288148641586304, -0.17742377519607544, -0.19434435665607452, -0.49813875555992126, -0.5490744113922119, -0.6394816637039185, -0.554042637348175, -0.4390913248062134, -0.5146337151527405, -0.5495248436927795, -0.6552216410636902, -0.511761486530304, -0.7948772311210632, -1.0132256746292114, -0.9733282923698425, -1.231073021888733, -1.2844057083129883, -1.1654324531555176, -1.0570080280303955, -1.0976078510284424, -1.1070820093154907, -1.016290545463562, -0.9809934496879578, -1.1362088918685913, -1.1553255319595337, -1.0698350667953491, -1.0589735507965088, -1.049293041229248, -1.2025378942489624, -1.163025975227356, -1.1667100191116333, -1.1997871398925781, -1.1509995460510254, -1.3032857179641724, -1.417669415473938, -1.3370370864868164, -1.0651133060455322, -1.2295397520065308, -1.227583408355713, -1.1007834672927856] 86 | 87 | z = [] 88 | for i in range(len(ped_x)): 89 | z.append(np.array([ 90 | [ped_x[i]], 91 | [ped_vx[i]], 92 | [ped_y[i]], 93 | [ped_vy[i]] 94 | ])) 95 | return z 96 | 97 | def veh_z_mia(): 98 | veh_x = [ 99 | 739.6794456696546, 100 | 739.9457292620912, 101 | 739.9390335152374, 102 | 739.9262512400034, 103 | 739.9059787453987, 104 | 739.8854519051506, 105 | 739.8616394582762, 106 | 739.8323979021022, 107 | 739.7988022114209, 108 | 739.7681812487398, 109 | 739.7384492831201, 110 | 739.7134731237061, 111 | 739.6885867851354, 112 | 739.6831673289680, 113 | 739.6893052166449, 114 | 739.7117539878232, 115 | 739.7510500286336, 116 | 739.7945538472072, 117 | 739.8538032558058, 118 | 739.9137715889711, 119 | 739.9951309824976, 120 | 740.1034856187266, 121 | 740.2331660011013, 122 | 740.3899532264899, 123 | 740.6034654621083, 124 | 740.8611719909883, 125 | 741.1812109523418, 126 | 741.5705503125356, 127 | 742.0355360707657, 128 | 742.6471978477078, 129 | 743.3097248918915, 130 | 744.0453259763724, 131 | 745.1283810209459, 132 | 746.1910210044331, 133 | 747.3280391816162, 134 | 748.5089097884813, 135 | 749.5276712103077, 136 | 750.5613287020236, 137 | 751.5396167049110, 138 | 753.1778337785466, 139 | 754.8195191068477, 140 | 756.1960848881993, 141 | 757.2464030255368, 142 | 758.1561682102274, 143 | 759.1341189722203, 144 | 759.9798316151705, 145 | 761.0127317803251, 146 | 761.9193801610568, 147 | 762.9484504337926, 148 | 763.8495637255278 149 | ] 150 | veh_y = [ 151 | 1688.9775850803603, 152 | 1688.502671954344, 153 | 1688.3736984101072, 154 | 1688.0939365611425, 155 | 1687.6392271086904, 156 | 1687.215245523514, 157 | 1686.7332487565257, 158 | 1686.182475686139, 159 | 1685.5442299991919, 160 | 1684.9293234172674, 161 | 1684.2610276402552, 162 | 1683.5835787678448, 163 | 1682.5848716283524, 164 | 1681.8652548863206, 165 | 1681.0339771277613, 166 | 1680.2046813566672, 167 | 1679.442714060014, 168 | 1678.8681862990472, 169 | 1678.2507163979017, 170 | 1677.7550734619297, 171 | 1677.170110061223, 172 | 1676.5446207372104, 173 | 1675.9398548033364, 174 | 1675.3341920378248, 175 | 1674.648983451885, 176 | 1673.9753995307215, 177 | 1673.2867375188507, 178 | 1672.5847223863238, 179 | 1671.8815710840165, 180 | 1671.108791026441, 181 | 1670.4124533557556, 182 | 1669.762691094655, 183 | 1668.9921606743626, 184 | 1668.3871411347961, 185 | 1667.8708248677601, 186 | 1667.442543197128, 187 | 1667.1527626935724, 188 | 1666.9271758165385, 189 | 1666.7686159324471, 190 | 1666.5869599122964, 191 | 1666.5038680838675, 192 | 1666.4945779405343, 193 | 1666.50894768548, 194 | 1666.5346854523486, 195 | 1666.56946186355, 196 | 1666.610881860173, 197 | 1666.66938327427, 198 | 1666.7277046305953, 199 | 1666.7994767763266, 200 | 1666.8633091165948 201 | ] 202 | 203 | veh_vx = [0] 204 | for i in range(1, len(veh_x)): 205 | veh_vx.append((veh_x[i] - veh_x[i-1])/0.1) 206 | veh_vx[0] = veh_vx[1] 207 | 208 | veh_vy = [0] 209 | for i in range(1, len(veh_y)): 210 | veh_vy.append((veh_y[i] - veh_y[i-1])/0.1) 211 | veh_vy[0] = veh_vy[1] 212 | 213 | z = [] 214 | for i in range(len(veh_x)): 215 | z.append(np.array([ 216 | [veh_x[i]], 217 | [veh_vx[i]], 218 | [veh_y[i]], 219 | [veh_vy[i]] 220 | ])) 221 | return z 222 | --------------------------------------------------------------------------------