├── requirements.txt ├── slides └── filtering_tutorial.pdf ├── ema_1d_signal.py ├── kf_1d_signal.py ├── simple_kf_1d_signal.py ├── kf_2d_position.py ├── ukf_2d_pose_simple_noise.py ├── ekf_2d_pose_simple_noise.py ├── ekf_2d_pose_off_centered.py ├── ekf_2d_pose_odometry.py ├── ukf_2d_pose.py ├── ekf_2d_pose_vel_constraints.py ├── pf_2d_pose.py ├── ekf_2d_pose.py └── README.md /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | matplotlib 3 | filterpy -------------------------------------------------------------------------------- /slides/filtering_tutorial.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/filtering_tutorial/HEAD/slides/filtering_tutorial.pdf -------------------------------------------------------------------------------- /ema_1d_signal.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | if __name__ == '__main__': 5 | # Prepare a noisy signal 6 | true_signal = lambda t: 10 * np.sin(2*np.pi/2*t) * np.cos(2*np.pi/10*t) 7 | times = np.arange(1, 10, 0.01) 8 | truth = true_signal(times) 9 | obs_signal = truth + np.random.normal(scale=1, size=truth.size) 10 | 11 | # Perform exponential moving average 12 | alpha = 0.125 13 | xs = [] 14 | for z in obs_signal: 15 | if len(xs) == 0: 16 | xs.append(z) 17 | else: 18 | xs.append(xs[-1] + alpha * (z - xs[-1])) 19 | 20 | # Visualize the results 21 | plt.figure() 22 | plt.plot(times, obs_signal, 'b-', label='Observation', alpha=0.3) 23 | plt.plot(times, xs, 'g-', label='Exp. Moving Avg.') 24 | plt.plot(times, truth, 'r-', label='Truth', alpha=0.5) 25 | plt.xlabel('Time [sec]') 26 | plt.ylabel('Amplitude') 27 | plt.grid() 28 | plt.legend() 29 | plt.show() -------------------------------------------------------------------------------- /kf_1d_signal.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from filterpy.kalman import KalmanFilter 4 | 5 | if __name__ == '__main__': 6 | # Prepare a noisy signal 7 | true_signal = lambda t: 10 * np.sin(2*np.pi/2*t) * np.cos(2*np.pi/10*t) 8 | times = np.arange(1, 10, 0.01) 9 | truth = true_signal(times) 10 | obs_signal = truth + np.random.normal(scale=1, size=truth.size) 11 | 12 | # Instantiate Kalman filter for noise filtering 13 | kf = KalmanFilter(dim_x=1, dim_z=1) 14 | kf.F = np.eye(1) 15 | kf.H = np.eye(1) 16 | kf.P = 10 * np.eye(1) 17 | kf.Q = 0.02 * np.eye(1) 18 | kf.R = 1 * np.eye(1) 19 | 20 | xs = [] 21 | for z in obs_signal: 22 | # Predict and update the Kalman filter 23 | kf.predict() 24 | kf.update(z) 25 | xs.append(kf.x.flatten()) 26 | 27 | # Visualize the results 28 | plt.figure() 29 | plt.plot(times, obs_signal, 'b-', label='Observation', alpha=0.3) 30 | plt.plot(times, xs, 'g-', label='Kalman Filter') 31 | plt.plot(times, truth, 'r-', label='Truth', alpha=0.5) 32 | plt.xlabel('Time [sec]') 33 | plt.ylabel('Amplitude') 34 | plt.grid() 35 | plt.legend() 36 | plt.show() -------------------------------------------------------------------------------- /simple_kf_1d_signal.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | if __name__ == '__main__': 5 | # Prepare a noisy signal 6 | true_signal = lambda t: 10 * np.sin(2*np.pi/2*t) * np.cos(2*np.pi/10*t) 7 | times = np.arange(1, 10, 0.01) 8 | truth = true_signal(times) 9 | obs_signal = truth + np.random.normal(scale=1, size=truth.size) 10 | 11 | # Perform the simple 1-D Kalman filter 12 | var_init, var_q, var_r = 10, 0.02, 1 13 | xs, var = [], [] 14 | for z in obs_signal: 15 | if len(xs) == 0: 16 | xs.append(z) 17 | var.append(var_init) 18 | else: 19 | # Predict signal change 20 | pred_x = xs[-1] 21 | pred_var = var[-1] + var_q 22 | 23 | # Correct signal change 24 | alpha = pred_var / (pred_var + var_r) 25 | print(alpha) 26 | xs.append(pred_x + alpha * (z - pred_x)) 27 | var.append((1 - alpha) * pred_var) 28 | 29 | # Visualize the results 30 | plt.figure() 31 | plt.plot(times, obs_signal, 'b-', label='Observation', alpha=0.3) 32 | plt.plot(times, xs, 'g-', label='Simple KF (x)') 33 | plt.plot(times, var, 'g--', label='Simple KF (Var)') 34 | plt.plot(times, truth, 'r-', label='Truth', alpha=0.5) 35 | plt.xlabel('Time [sec]') 36 | plt.ylabel('Amplitude') 37 | plt.grid() 38 | plt.legend() 39 | plt.show() -------------------------------------------------------------------------------- /kf_2d_position.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from filterpy.kalman import KalmanFilter 4 | 5 | if __name__ == '__main__': 6 | # Define experimental configuration 7 | dt, t_end = 0.1, 8 8 | r, w = 10., np.pi / 4 9 | get_true_position = lambda t: r * np.array([[np.cos(w * t)], [np.sin(w * t)]]) # Circular motion 10 | gps_noise_std = 1 11 | 12 | # Instantiate Kalman filter for position tracking 13 | localizer_name = 'Kalman Filter' 14 | localizer = KalmanFilter(dim_x=2, dim_z=2) 15 | localizer.F = np.eye(2) 16 | localizer.H = np.eye(2) 17 | localizer.Q = 0.1 * np.eye(2) 18 | localizer.R = gps_noise_std * gps_noise_std * np.eye(2) 19 | 20 | times, truth, zs, xs, = [], [], [], [] 21 | for t in np.arange(0, t_end, dt): 22 | # Simulate position observation with additive Gaussian noise 23 | true = get_true_position(t) 24 | z = true + np.random.normal(size=true.shape, scale=gps_noise_std) 25 | 26 | # Predict and update the Kalman filter 27 | localizer.predict() 28 | localizer.update(z) 29 | 30 | times.append(t) 31 | truth.append(true.flatten()) 32 | zs.append(z.flatten()) 33 | xs.append(localizer.x.flatten()) 34 | times, truth, zs, xs = np.array(times), np.array(truth), np.array(zs), np.array(xs) 35 | 36 | # Visualize the results 37 | plt.figure() 38 | plt.plot(truth[:,0], truth[:,1], 'r-', label='Truth') 39 | plt.plot(zs[:,0], zs[:,1], 'b+', label='Observation') 40 | plt.plot(xs[:,0], xs[:,1], 'g-', label=localizer_name) 41 | plt.axis('equal') 42 | plt.xlabel('X') 43 | plt.ylabel('Y') 44 | plt.grid() 45 | plt.legend() 46 | 47 | plt.figure() 48 | plt.plot(times, truth[:,0], 'r-', label='Truth') 49 | plt.plot(times, zs[:,0], 'b+', label='Observation') 50 | plt.plot(times, xs[:,0], 'g-', label=localizer_name) 51 | plt.xlabel('Time') 52 | plt.ylabel('X') 53 | plt.grid() 54 | plt.legend() 55 | 56 | plt.figure() 57 | plt.plot(times, truth[:,1], 'r-', label='Truth') 58 | plt.plot(times, zs[:,1], 'b+', label='Observation') 59 | plt.plot(times, xs[:,1], 'g-', label=localizer_name) 60 | plt.xlabel('Time') 61 | plt.ylabel('Y') 62 | plt.grid() 63 | plt.legend() 64 | 65 | plt.show() -------------------------------------------------------------------------------- /ukf_2d_pose_simple_noise.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from filterpy.kalman import UnscentedKalmanFilter, MerweScaledSigmaPoints 4 | from ekf_2d_pose import plot_results 5 | 6 | def fx(state, dt): 7 | x, y, theta, v, w = state.flatten() 8 | vt, wt = v * dt, w * dt 9 | s, c = np.sin(theta + wt / 2), np.cos(theta + wt / 2) 10 | return np.array([ 11 | x + vt * c, 12 | y + vt * s, 13 | theta + wt, 14 | v, 15 | w]) # Note) UKF prefers to use horizontal vectors. 16 | 17 | def hx(state): 18 | x, y, *_ = state.flatten() 19 | return np.array([x, y]) # Note) UKF prefers to use horizontal vectors. 20 | 21 | 22 | 23 | if __name__ == '__main__': 24 | # Define experimental configuration 25 | dt, t_end = 0.1, 8 26 | r, w = 10., np.pi / 4 27 | get_true_position = lambda t: r * np.array([[np.cos(w * t)], [np.sin(w * t)]]) # Circular motion 28 | get_true_heading = lambda t: np.arctan2(np.cos(w * t), -np.sin(w * t)) 29 | gps_noise_std = 1 30 | 31 | # Instantiate UKF for pose (and velocity) tracking 32 | localizer_name = 'UKF+SimpleNoise' 33 | points = MerweScaledSigmaPoints(5, alpha=.1, beta=2., kappa=-1) 34 | localizer = UnscentedKalmanFilter(dim_x=5, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) 35 | localizer.Q = 0.1 * np.eye(5) 36 | localizer.R = gps_noise_std * gps_noise_std * np.eye(2) 37 | 38 | truth, state, obser, covar = [], [], [], [] 39 | for t in np.arange(0, t_end, dt): 40 | # Simulate position observation with additive Gaussian noise 41 | true_pos = get_true_position(t) 42 | true_ori = get_true_heading(t) 43 | gps_data = true_pos + np.random.normal(size=true_pos.shape, scale=gps_noise_std) 44 | 45 | # Predict and update the UKF 46 | localizer.predict() 47 | localizer.update(gps_data.flatten()) # Note) UKF prefers to use horizontal vectors. 48 | 49 | if localizer.x[2] >= np.pi: 50 | localizer.x[2] -= 2 * np.pi 51 | elif localizer.x[2] < -np.pi: 52 | localizer.x[2] += 2 * np.pi 53 | 54 | # Record true state, observation, estimated state, and its covariance 55 | truth.append([t] + true_pos.flatten().tolist() + [true_ori, r * w, w]) 56 | state.append([t] + localizer.x.flatten().tolist()) 57 | obser.append([t] + gps_data.flatten().tolist()) 58 | covar.append([t] + localizer.P.flatten().tolist()) 59 | 60 | # Visualize the results 61 | plot_results(localizer_name, truth, state, obser, covar) 62 | plt.show() -------------------------------------------------------------------------------- /ekf_2d_pose_simple_noise.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from filterpy.kalman import ExtendedKalmanFilter 4 | from ekf_2d_pose import plot_results 5 | 6 | def fx(state, dt): 7 | x, y, theta, v, w = state.flatten() 8 | vt, wt = v * dt, w * dt 9 | s, c = np.sin(theta + wt / 2), np.cos(theta + wt / 2) 10 | return np.array([ 11 | [x + vt * c], 12 | [y + vt * s], 13 | [theta + wt], 14 | [v], 15 | [w]]) 16 | 17 | def Fx(state, dt): 18 | x, y, theta, v, w = state.flatten() 19 | vt, wt = v * dt, w * dt 20 | s, c = np.sin(theta + wt / 2), np.cos(theta + wt / 2) 21 | return np.array([ 22 | [1, 0, -vt * s, dt * c, -vt * dt * s / 2], 23 | [0, 1, vt * c, dt * s, vt * dt * c / 2], 24 | [0, 0, 1, 0, dt], 25 | [0, 0, 0, 1, 0], 26 | [0, 0, 0, 0, 1]]) 27 | 28 | def hx(state): 29 | x, y, *_ = state.flatten() 30 | return np.array([[x], [y]]) 31 | 32 | def Hx(state): 33 | return np.eye(2, 5) 34 | 35 | 36 | 37 | if __name__ == '__main__': 38 | # Define experimental configuration 39 | dt, t_end = 0.1, 8 40 | r, w = 10., np.pi / 4 41 | get_true_position = lambda t: r * np.array([[np.cos(w * t)], [np.sin(w * t)]]) # Circular motion 42 | get_true_heading = lambda t: np.arctan2(np.cos(w * t), -np.sin(w * t)) 43 | gps_noise_std = 1 44 | 45 | # Instantiate EKF for pose (and velocity) tracking 46 | localizer_name = 'EKF+SimpleNoise' 47 | localizer = ExtendedKalmanFilter(dim_x=5, dim_z=2) 48 | localizer.Q = 0.1 * np.eye(5) 49 | localizer.R = gps_noise_std * gps_noise_std * np.eye(2) 50 | 51 | truth, state, obser, covar = [], [], [], [] 52 | for t in np.arange(0, t_end, dt): 53 | # Simulate position observation with additive Gaussian noise 54 | true_pos = get_true_position(t) 55 | true_ori = get_true_heading(t) 56 | gps_data = true_pos + np.random.normal(size=true_pos.shape, scale=gps_noise_std) 57 | 58 | # Predict and update the EKF 59 | localizer.F = Fx(localizer.x, dt) 60 | localizer.x = fx(localizer.x, dt) 61 | localizer.predict() 62 | localizer.update(gps_data, Hx, hx) 63 | 64 | if localizer.x[2] >= np.pi: 65 | localizer.x[2] -= 2 * np.pi 66 | elif localizer.x[2] < -np.pi: 67 | localizer.x[2] += 2 * np.pi 68 | 69 | # Record true state, observation, estimated state, and its covariance 70 | truth.append([t] + true_pos.flatten().tolist() + [true_ori, r * w, w]) 71 | state.append([t] + localizer.x.flatten().tolist()) 72 | obser.append([t] + gps_data.flatten().tolist()) 73 | covar.append([t] + localizer.P.flatten().tolist()) 74 | 75 | # Visualize the results 76 | plot_results(localizer_name, truth, state, obser, covar) 77 | plt.show() -------------------------------------------------------------------------------- /ekf_2d_pose_off_centered.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from ekf_2d_pose import EKFLocalizer, plot_results 4 | 5 | class EKFLocalizerOC(EKFLocalizer): 6 | def __init__(self, v_noise_std=1, w_noise_std=1, gps_noise_std=1, gps_offset=(0,0), dt=1): 7 | super().__init__(v_noise_std, w_noise_std, gps_noise_std, dt) 8 | self.h = self.hx 9 | self.H = self.Hx 10 | self.gps_offset_x, self.gps_offset_y = gps_offset.flatten() 11 | 12 | def hx(self, state): 13 | x, y, theta, *_ = state.flatten() 14 | s, c = np.sin(theta), np.cos(theta) 15 | return np.array([ 16 | [x + self.gps_offset_x * c - self.gps_offset_y * s], 17 | [y + self.gps_offset_x * s + self.gps_offset_y * c]]) 18 | 19 | def Hx(self, state): 20 | _, _, theta, *_ = state.flatten() 21 | s, c = np.sin(theta), np.cos(theta) 22 | return np.array([ 23 | [1, 0, -self.gps_offset_x * s - self.gps_offset_y * c, 0, 0], 24 | [0, 1, self.gps_offset_x * c - self.gps_offset_y * s, 0, 0]]) 25 | 26 | 27 | 28 | if __name__ == '__main__': 29 | # Define experimental configuration 30 | dt, t_end = 0.1, 8 31 | r, w = 10., np.pi / 4 32 | get_true_position = lambda t: r * np.array([[np.cos(w * t)], [np.sin(w * t)]]) # Circular motion 33 | get_true_heading = lambda t: np.arctan2(np.cos(w * t), -np.sin(w * t)) 34 | gps_noise_std = 1 35 | gps_offset = np.array([[1], [0]]) 36 | 37 | # Instantiate EKF for pose (and velocity) tracking 38 | localizer_name = 'EKF+OffCentered' 39 | localizer = EKFLocalizerOC(v_noise_std=0.1, w_noise_std=0.1, gps_noise_std=gps_noise_std, gps_offset=gps_offset, dt=dt) 40 | 41 | truth, state, obser, covar = [], [], [], [] 42 | for t in np.arange(0, t_end, dt): 43 | # Simulate position observation with off-centered GPS and additive Gaussian noise 44 | true_pos = get_true_position(t) 45 | true_ori = get_true_heading(t) 46 | R = np.array([[np.cos(true_ori), -np.sin(true_ori)], [np.sin(true_ori), np.cos(true_ori)]]) 47 | gps_data = true_pos + R @ gps_offset + np.random.normal(size=true_pos.shape, scale=gps_noise_std) 48 | 49 | # Predict and update the EKF 50 | localizer.predict() 51 | localizer.update(gps_data) 52 | 53 | if localizer.x[2] >= np.pi: 54 | localizer.x[2] -= 2 * np.pi 55 | elif localizer.x[2] < -np.pi: 56 | localizer.x[2] += 2 * np.pi 57 | 58 | # Record true state, observation, estimated state, and its covariance 59 | truth.append([t] + true_pos.flatten().tolist() + [true_ori, r * w, w]) 60 | state.append([t] + localizer.x.flatten().tolist()) 61 | obser.append([t] + gps_data.flatten().tolist()) 62 | covar.append([t] + localizer.P.flatten().tolist()) 63 | 64 | # Visualize the results 65 | plot_results(localizer_name, truth, state, obser, covar) 66 | plt.show() -------------------------------------------------------------------------------- /ekf_2d_pose_odometry.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from filterpy.kalman import ExtendedKalmanFilter 4 | from ekf_2d_pose import plot_results 5 | 6 | class EKFLocalizerOD(ExtendedKalmanFilter): 7 | def __init__(self, v_noise_std=1, w_noise_std=1, gps_noise_std=1, dt=1): 8 | super().__init__(dim_x=3, dim_z=2) 9 | self.motion_noise = np.array([[v_noise_std * v_noise_std, 0], [0, w_noise_std * w_noise_std]]) 10 | self.h = lambda x: x[0:2] 11 | self.H = lambda x: np.eye(2, 3) 12 | self.R = gps_noise_std * gps_noise_std * np.eye(2) 13 | self.dt = dt 14 | 15 | def predict(self, u): 16 | x, y, theta = self.x.flatten() 17 | v, w = u.flatten() 18 | vt, wt = v * self.dt, w * self.dt 19 | s, c = np.sin(theta + wt / 2), np.cos(theta + wt / 2) 20 | 21 | # Predict the state 22 | self.x[0] = x + vt * c 23 | self.x[1] = y + vt * s 24 | self.x[2] = theta + wt 25 | 26 | # Predict the covariance 27 | self.F = np.array([ 28 | [1, 0, -vt * s], 29 | [0, 1, vt * c], 30 | [0, 0, 1]]) 31 | W = np.array([ 32 | [self.dt * c, -vt * self.dt * s / 2], 33 | [self.dt * s, vt * self.dt * c / 2], 34 | [0, self.dt]]) 35 | self.Q = W @ self.motion_noise @ W.T 36 | self.P = self.F @ self.P @ self.F.T + self.Q 37 | 38 | # Save prior 39 | self.x_prior = np.copy(self.x) 40 | self.P_prior = np.copy(self.P) 41 | 42 | def update(self, z): 43 | super().update(z, HJacobian=self.H, Hx=self.h, R=self.R) 44 | 45 | 46 | 47 | if __name__ == '__main__': 48 | # Define experimental configuration 49 | dt, t_end = 0.1, 8 50 | r, w = 10., np.pi / 4 51 | get_true_position = lambda t: r * np.array([[np.cos(w * t)], [np.sin(w * t)]]) # Circular motion 52 | get_true_heading = lambda t: np.arctan2(np.cos(w * t), -np.sin(w * t)) 53 | gps_noise_std = 1 54 | 55 | # Instantiate EKF for pose tracking 56 | localizer_name = 'EKF+Odometry' 57 | localizer = EKFLocalizerOD(v_noise_std=0.2, w_noise_std=0.2, gps_noise_std=gps_noise_std, dt=dt) 58 | 59 | truth, state, obser, covar = [], [], [], [] 60 | for t in np.arange(0, t_end, dt): 61 | # Simulate position observation (with additive Gaussian noise) and velocity observation (with slippage loss) 62 | true_pos = get_true_position(t) 63 | true_ori = get_true_heading(t) 64 | gps_data = true_pos + np.random.normal(size=true_pos.shape, scale=gps_noise_std) 65 | odo_data = np.array([r * w * 0.95, w * 0.90]) 66 | 67 | # Predict and update the EKF 68 | localizer.predict(odo_data) 69 | localizer.update(gps_data) 70 | 71 | if localizer.x[2] >= np.pi: 72 | localizer.x[2] -= 2 * np.pi 73 | elif localizer.x[2] < -np.pi: 74 | localizer.x[2] += 2 * np.pi 75 | 76 | # Record true state, observation, estimated state, and its covariance 77 | truth.append([t] + true_pos.flatten().tolist() + [true_ori, r * w, w]) 78 | state.append([t] + localizer.x.flatten().tolist()) 79 | obser.append([t] + gps_data.flatten().tolist()) 80 | covar.append([t] + localizer.P.flatten().tolist()) 81 | 82 | # Visualize the results 83 | plot_results(localizer_name, truth, state, obser, covar) 84 | plt.show() -------------------------------------------------------------------------------- /ukf_2d_pose.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from filterpy.kalman import UnscentedKalmanFilter, MerweScaledSigmaPoints 4 | from ekf_2d_pose import plot_results 5 | 6 | class UKFLocalizer(UnscentedKalmanFilter): 7 | def __init__(self, v_noise_std=1, w_noise_std=1, gps_noise_std=1, dt=1): 8 | self.sigma_points = MerweScaledSigmaPoints(5, alpha=.1, beta=2., kappa=-1) 9 | super().__init__(dim_x=5, dim_z=2, dt=dt, fx=self.fx, hx=self.hx, points=self.sigma_points) 10 | self.motion_noise = np.array([[v_noise_std * v_noise_std, 0], [0, w_noise_std * w_noise_std]]) 11 | self.R = gps_noise_std * gps_noise_std * np.eye(2) 12 | self.dt = dt 13 | 14 | def fx(self, state, dt): 15 | x, y, theta, v, w = state.flatten() 16 | vt, wt = v * dt, w * dt 17 | s, c = np.sin(theta + wt / 2), np.cos(theta + wt / 2) 18 | return np.array([ 19 | x + vt * c, 20 | y + vt * s, 21 | theta + wt, 22 | v, 23 | w]) 24 | 25 | def hx(self, state): 26 | x, y, _, _, _ = state.flatten() 27 | return np.array([x, y]) 28 | 29 | def predict(self): 30 | x, y, theta, v, w = self.x.flatten() 31 | vt, wt = v * self.dt, w * self.dt 32 | s, c = np.sin(theta + wt / 2), np.cos(theta + wt / 2) 33 | 34 | # Set the covariance of transition noise 35 | W = np.array([ 36 | [self.dt * c, -vt * self.dt * s / 2], 37 | [self.dt * s, vt * self.dt * c / 2], 38 | [0, self.dt], 39 | [1, 0], 40 | [0, 1]]) 41 | self.Q = W @ self.motion_noise @ W.T 42 | 43 | super().predict() 44 | 45 | def update(self, z): 46 | super().update(z.flatten()) 47 | 48 | 49 | if __name__ == '__main__': 50 | # Define experimental configuration 51 | dt, t_end = 0.1, 8 52 | r, w = 10., np.pi / 4 53 | get_true_position = lambda t: r * np.array([[np.cos(w * t)], [np.sin(w * t)]]) # Circular motion 54 | get_true_heading = lambda t: np.arctan2(np.cos(w * t), -np.sin(w * t)) 55 | gps_noise_std = 1 56 | 57 | # Instantiate UKF for pose (and velocity) tracking 58 | localizer_name = 'UKF' 59 | localizer = UKFLocalizer(v_noise_std=0.1, w_noise_std=0.1, gps_noise_std=gps_noise_std, dt=dt) 60 | 61 | truth, state, obser, covar = [], [], [], [] 62 | for t in np.arange(0, t_end, dt): 63 | # Simulate position observation with additive Gaussian noise 64 | true_pos = get_true_position(t) 65 | true_ori = get_true_heading(t) 66 | gps_data = true_pos + np.random.normal(size=true_pos.shape, scale=gps_noise_std) 67 | 68 | # Predict and update the UKF 69 | localizer.predict() 70 | localizer.update(gps_data) 71 | 72 | if localizer.x[2] >= np.pi: 73 | localizer.x[2] -= 2 * np.pi 74 | elif localizer.x[2] < -np.pi: 75 | localizer.x[2] += 2 * np.pi 76 | 77 | # Record true state, observation, estimated state, and its covariance 78 | truth.append([t] + true_pos.flatten().tolist() + [true_ori, r * w, w]) 79 | state.append([t] + localizer.x.flatten().tolist()) 80 | obser.append([t] + gps_data.flatten().tolist()) 81 | covar.append([t] + localizer.P.flatten().tolist()) 82 | 83 | # Visualize the results 84 | plot_results(localizer_name, truth, state, obser, covar) 85 | plt.show() -------------------------------------------------------------------------------- /ekf_2d_pose_vel_constraints.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from filterpy.kalman import ExtendedKalmanFilter 4 | from ekf_2d_pose import EKFLocalizer, plot_results 5 | 6 | class EKFLocalizerVC(ExtendedKalmanFilter): 7 | def __init__(self, v_noise_std=1, w_noise_std=1, gps_noise_std=1, dt=1, v_epsilon=0.001, w_max=1): 8 | super().__init__(dim_x=5, dim_z=2) 9 | self.motion_noise = np.array([[v_noise_std * v_noise_std, 0], [0, w_noise_std * w_noise_std]]) 10 | self.h = lambda x: x[0:2] 11 | self.H = lambda x: np.eye(2, 5) 12 | self.R = gps_noise_std * gps_noise_std * np.eye(2) 13 | self.dt = dt 14 | self.v_epsilon = v_epsilon 15 | self.w_max = w_max 16 | 17 | def predict(self): 18 | x, y, theta, v, w = self.x.flatten() 19 | vt, wt = v * self.dt, w * self.dt 20 | s, c = np.sin(theta + wt / 2), np.cos(theta + wt / 2) 21 | w_sat = self.w_max * np.tanh(w / self.w_max) 22 | 23 | # Predict the state 24 | self.x[0] = x + vt * c 25 | self.x[1] = y + vt * s 26 | self.x[2] = theta + wt 27 | #self.x[3] = v # Not necessary 28 | self.x[4] = w_sat # Constraint #2) Angular rate saturation 29 | 30 | # Predict the covariance 31 | self.F = np.array([ 32 | [1, 0, -vt * s, self.dt * c, -vt * self.dt * s / 2], 33 | [0, 1, vt * c, self.dt * s, vt * self.dt * c / 2], 34 | [0, 0, 1, 0, self.dt], 35 | [0, 0, 0, 1, 0], 36 | [0, 0, 0, 0, 1 - w_sat * w_sat]]) # Constraint #2) Angular rate saturation 37 | W = np.array([ 38 | [self.dt * c, -vt * self.dt * s / 2], 39 | [self.dt * s, vt * self.dt * c / 2], 40 | [0, self.dt], 41 | [1, 0], 42 | [0, 1]]) 43 | self.Q = W @ self.motion_noise @ W.T 44 | self.P = self.F @ self.P @ self.F.T + self.Q 45 | 46 | # Save prior 47 | self.x_prior = np.copy(self.x) 48 | self.P_prior = np.copy(self.P) 49 | 50 | def update(self, z): 51 | super().update(z, HJacobian=self.H, Hx=self.h, R=self.R) 52 | if self.v_epsilon > 0 and self.x[3] < self.v_epsilon: 53 | # Constraint #1) Heading angle correction 54 | self.x[2] += np.pi 55 | self.x[3] *= -1 56 | 57 | 58 | 59 | if __name__ == '__main__': 60 | # Define experimental configuration 61 | dt, t_end = 0.1, 8 62 | r, w = 10., np.pi / 4 63 | get_true_position = lambda t: r * np.array([[np.cos(w * t)], [np.sin(w * t)]]) # Circular motion 64 | get_true_heading = lambda t: np.arctan2(np.cos(w * t), -np.sin(w * t)) 65 | gps_noise_std = 1 66 | gps_outlier_deadzone = (-12, -8, -5, 5) # (x_min, x_max, y_min, y_max) 67 | gps_outlier_noise_std = 10 68 | is_inside_deadzone = lambda x, y, x_min, x_max, y_min, y_max: (x_min <= x <= x_max) and (y_min <= y <= y_max) 69 | 70 | # Note) Please compare the results by `EKFLocalizer` and `EKFLocalizerVC` 71 | np.random.seed(0) # For reproducibility 72 | 73 | # Instantiate EKF for pose (and velocity) tracking 74 | localizer_name = 'EKF+VelocityConstraints' 75 | localizer = EKFLocalizerVC(v_noise_std=0.1, w_noise_std=0.1, gps_noise_std=gps_noise_std, dt=dt) 76 | 77 | truth, state, obser, covar = [], [], [], [] 78 | for t in np.arange(0, t_end, dt): 79 | # Simulate position observation with additive Gaussian noise 80 | true_pos = get_true_position(t) 81 | true_ori = get_true_heading(t) 82 | if is_inside_deadzone(*true_pos, *gps_outlier_deadzone): 83 | gps_data = true_pos + np.random.normal(size=true_pos.shape, scale=gps_outlier_noise_std) 84 | else: 85 | gps_data = true_pos + np.random.normal(size=true_pos.shape, scale=gps_noise_std) 86 | 87 | # Predict and update the EKF 88 | localizer.predict() 89 | localizer.update(gps_data) 90 | 91 | if localizer.x[2] >= np.pi: 92 | localizer.x[2] -= 2 * np.pi 93 | elif localizer.x[2] < -np.pi: 94 | localizer.x[2] += 2 * np.pi 95 | 96 | # Record true state, observation, estimated state, and its covariance 97 | truth.append([t] + true_pos.flatten().tolist() + [true_ori, r * w, w]) 98 | state.append([t] + localizer.x.flatten().tolist()) 99 | obser.append([t] + gps_data.flatten().tolist()) 100 | covar.append([t] + localizer.P.flatten().tolist()) 101 | 102 | # Visualize the results 103 | plot_results(localizer_name, truth, state, obser, covar) 104 | plt.show() -------------------------------------------------------------------------------- /pf_2d_pose.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import scipy.stats 4 | from filterpy.monte_carlo import systematic_resample 5 | from ekf_2d_pose import plot_results 6 | 7 | class PFLocalizer: 8 | def __init__(self, v_noise_std=1, w_noise_std=1, gps_noise_std=1, dt=1, 9 | N=1000, x_range=(-15, +15), y_range=(-15, +15), theta_range=(-np.pi, np.pi), v_range=(1, 10), w_range=(-np.pi/2, np.pi/2)): 10 | self.v_noise_std = v_noise_std 11 | self.w_noise_std = w_noise_std 12 | self.gps_noise_std = gps_noise_std 13 | self.dt = dt 14 | 15 | # Spread the initial particles uniformly 16 | self.pts = np.zeros((N, 5)) 17 | self.pts[:,0] = np.random.uniform(*x_range, size=N) 18 | self.pts[:,1] = np.random.uniform(*y_range, size=N) 19 | self.pts[:,2] = np.random.uniform(*theta_range, size=N) 20 | self.pts[:,3] = np.random.uniform(*v_range, size=N) 21 | self.pts[:,4] = np.random.uniform(*w_range, size=N) 22 | self.weight = np.ones(N) / N 23 | 24 | def predict(self): 25 | # Move the particles 26 | v_noise = self.v_noise_std * np.random.randn(len(self.pts)) 27 | w_noise = self.w_noise_std * np.random.randn(len(self.pts)) 28 | v_delta = (self.pts[:,3] + v_noise) * self.dt 29 | w_delta = (self.pts[:,4] + w_noise) * self.dt 30 | self.pts[:,0] += v_delta * np.cos(self.pts[:,2] + w_delta / 2) 31 | self.pts[:,1] += v_delta * np.sin(self.pts[:,2] + w_delta / 2) 32 | self.pts[:,2] += w_delta 33 | self.pts[:,3] += v_noise 34 | self.pts[:,4] += w_noise 35 | 36 | def update(self, z): 37 | # Update weights of the particles 38 | d = np.linalg.norm(self.pts[:,0:2] - z.flatten(), axis=1) 39 | self.weight *= scipy.stats.norm(scale=gps_noise_std).pdf(d) 40 | self.weight += 1e-10 41 | self.weight /= sum(self.weight) 42 | 43 | # Resample the particles 44 | N = len(self.pts) 45 | if neff(self.weight) < N / 2: 46 | indices = systematic_resample(self.weight) 47 | self.pts[:] = self.pts[indices] 48 | self.weight = np.ones(N) / N 49 | 50 | def get_state(self): 51 | xy = np.average(self.pts[:,0:2], weights=self.weight, axis=0) 52 | c = np.average(np.cos(self.pts[:,2]), weights=self.weight) 53 | s = np.average(np.sin(self.pts[:,2]), weights=self.weight) 54 | theta = np.arctan2(s, c) 55 | vw = np.average(self.pts[:,3:5], weights=self.weight, axis=0) 56 | return np.hstack((xy, theta, vw)) 57 | 58 | def neff(weight): 59 | return 1. / np.sum(np.square(weight)) 60 | 61 | 62 | 63 | if __name__ == '__main__': 64 | # Define experimental configuration 65 | dt, t_end = 0.1, 8 66 | r, w = 10., np.pi / 4 67 | get_true_position = lambda t: r * np.array([[np.cos(w * t)], [np.sin(w * t)]]) # Circular motion 68 | get_true_heading = lambda t: np.arctan2(np.cos(w * t), -np.sin(w * t)) 69 | gps_noise_std = 1 70 | show_animation = True # Type `%matplotlib qt5` in IPython console in Spyder to watch the animation 71 | show_animation_sec = 0.1 # Reduce the pause time if you want faster animation 72 | 73 | # Instantiate particle filter for pose (and velocity) tracking 74 | localizer_name = 'Particle Filter' 75 | localizer = PFLocalizer(v_noise_std=0.5, w_noise_std=0.5, gps_noise_std=gps_noise_std, dt=dt) 76 | 77 | # Prepare animation (online visualization) of the particle filter 78 | if show_animation: 79 | plt.ion() 80 | fig, ax = plt.subplots() 81 | ax.set_aspect('equal') 82 | ax.set_xlim(-15, 15) 83 | ax.set_ylim(-15, 15) 84 | ax.set_xlabel('X [m]') 85 | ax.set_ylabel('Y [m]') 86 | ax.grid(True) 87 | l_pts, = ax.plot([], [], 'k.', label='Particles', alpha=0.1, markeredgewidth=0) 88 | l_tru, = ax.plot([], [], 'r-', label='Truth') 89 | l_obs, = ax.plot([], [], 'b+', label='Observation') 90 | l_loc, = ax.plot([], [], 'g-', label=localizer_name) 91 | ax.legend() 92 | 93 | truth, state, obser = np.empty((0, 6)), np.empty((0, 6)), np.empty((0, 3)) 94 | for t in np.arange(0, t_end, dt): 95 | # Simulate position observation with additive Gaussian noise 96 | true_pos = get_true_position(t) 97 | true_ori = get_true_heading(t) 98 | gps_data = true_pos + np.random.normal(size=true_pos.shape, scale=gps_noise_std) 99 | 100 | # Predict and update the particle filter 101 | localizer.predict() 102 | localizer.update(gps_data) 103 | 104 | # Record true state, observation, estimated state, and its covariance 105 | truth = np.vstack((truth, [t] + true_pos.flatten().tolist() + [true_ori, r * w, w])) 106 | state = np.vstack((state, [t] + localizer.get_state().flatten().tolist())) 107 | obser = np.vstack((obser, [t] + gps_data.flatten().tolist())) 108 | 109 | # Draw animation of the particle filter 110 | if show_animation: 111 | l_pts.set_data(localizer.pts[:,0], localizer.pts[:,1]) 112 | l_tru.set_data(truth[:,1], truth[:,2]) 113 | l_obs.set_data(obser[:,1], obser[:,2]) 114 | l_loc.set_data(state[:,1], state[:,2]) 115 | plt.draw() 116 | plt.pause(show_animation_sec) 117 | 118 | # Visualize the results 119 | plt.ioff() 120 | plot_results(localizer_name, truth, state, obser) 121 | plt.show() -------------------------------------------------------------------------------- /ekf_2d_pose.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from filterpy.kalman import ExtendedKalmanFilter 4 | from filterpy.stats import plot_covariance 5 | 6 | class EKFLocalizer(ExtendedKalmanFilter): 7 | def __init__(self, v_noise_std=1, w_noise_std=1, gps_noise_std=1, dt=1): 8 | super().__init__(dim_x=5, dim_z=2) 9 | self.motion_noise = np.array([[v_noise_std * v_noise_std, 0], [0, w_noise_std * w_noise_std]]) 10 | self.h = lambda x: x[0:2] 11 | self.H = lambda x: np.eye(2, 5) 12 | self.R = gps_noise_std * gps_noise_std * np.eye(2) 13 | self.dt = dt 14 | 15 | def predict(self): 16 | x, y, theta, v, w = self.x.flatten() 17 | vt, wt = v * self.dt, w * self.dt 18 | s, c = np.sin(theta + wt / 2), np.cos(theta + wt / 2) 19 | 20 | # Predict the state 21 | self.x[0] = x + vt * c 22 | self.x[1] = y + vt * s 23 | self.x[2] = theta + wt 24 | #self.x[3] = v # Not necessary 25 | #self.x[4] = w # Not necessary 26 | 27 | # Predict the covariance 28 | self.F = np.array([ 29 | [1, 0, -vt * s, self.dt * c, -vt * self.dt * s / 2], 30 | [0, 1, vt * c, self.dt * s, vt * self.dt * c / 2], 31 | [0, 0, 1, 0, self.dt], 32 | [0, 0, 0, 1, 0], 33 | [0, 0, 0, 0, 1]]) 34 | W = np.array([ 35 | [self.dt * c, -vt * self.dt * s / 2], 36 | [self.dt * s, vt * self.dt * c / 2], 37 | [0, self.dt], 38 | [1, 0], 39 | [0, 1]]) 40 | self.Q = W @ self.motion_noise @ W.T 41 | self.P = self.F @ self.P @ self.F.T + self.Q 42 | 43 | # Save prior 44 | self.x_prior = np.copy(self.x) 45 | self.P_prior = np.copy(self.P) 46 | 47 | def update(self, z): 48 | super().update(z, HJacobian=self.H, Hx=self.h, R=self.R) 49 | 50 | def plot_results(localizer_name, truth, state, obser=None, covar=None, covar_step=5, covar_sigma=0.98): # 0.98 means 2 x sigma. 51 | truth = np.array(truth) 52 | state = np.array(state) 53 | if obser is not None: 54 | obser = np.array(obser) 55 | if covar is not None: 56 | covar = np.array(covar) 57 | state_dim = len(state[0]) - 1 58 | 59 | plt.figure('XY') 60 | plt.plot(truth[:,1], truth[:,2], 'r-', label='Truth') 61 | if obser is not None: 62 | plt.plot(obser[:,1], obser[:,2], 'b+', label='Observation') 63 | plt.plot(state[:,1], state[:,2], 'g-', label=localizer_name) 64 | if covar is not None: 65 | for i, cov in enumerate(covar): 66 | if i % covar_step == 0: 67 | plot_covariance(state[i][1:3], cov[1:].reshape(state_dim, state_dim)[0:2,0:2], interval=covar_sigma, edgecolor='g', alpha=0.5) 68 | plt.axis('equal') 69 | plt.xlabel('X [m]') 70 | plt.ylabel('Y [m]') 71 | plt.grid() 72 | plt.legend() 73 | 74 | plt.figure('Time-X') 75 | plt.plot(truth[:,0], truth[:,1], 'r-', label='Truth') 76 | if obser is not None: 77 | plt.plot(obser[:,0], obser[:,1], 'b+', label='Observation') 78 | plt.plot(state[:,0], state[:,1], 'g-', label=localizer_name) 79 | plt.xlabel('Time [sec]') 80 | plt.ylabel('X [m]') 81 | plt.grid() 82 | plt.legend() 83 | 84 | plt.figure('Time-Y') 85 | plt.plot(truth[:,0], truth[:,2], 'r-', label='Truth') 86 | if obser is not None: 87 | plt.plot(obser[:,0], obser[:,2], 'b+', label='Observation') 88 | plt.plot(state[:,0], state[:,2], 'g-', label=localizer_name) 89 | plt.xlabel('Time [sec]') 90 | plt.ylabel('Y [m]') 91 | plt.grid() 92 | plt.legend() 93 | 94 | if state_dim >= 3: 95 | plt.figure('Time-Theta') 96 | plt.plot(truth[:,0], np.rad2deg(truth[:,3]), 'r-', label='Truth') 97 | plt.plot(state[:,0], np.rad2deg(state[:,3]), 'g-', label=localizer_name) 98 | plt.xlabel('Time [sec]') 99 | plt.ylabel(r'Orientaiton $\theta$ [deg]') 100 | plt.grid() 101 | plt.legend() 102 | 103 | if state_dim >= 4: 104 | plt.figure('Time-V') 105 | plt.plot(truth[:,0], truth[:,4], 'r-', label='Truth') 106 | plt.plot(state[:,0], state[:,4], 'g-', label=localizer_name) 107 | plt.xlabel('Time [sec]') 108 | plt.ylabel('Linear Velocity $v$ [m/s]') 109 | plt.grid() 110 | plt.legend() 111 | 112 | if state_dim >= 5: 113 | plt.figure('Time-W') 114 | plt.plot(truth[:,0], np.rad2deg(truth[:,5]), 'r-', label='Truth') 115 | plt.plot(state[:,0], np.rad2deg(state[:,5]), 'g-', label=localizer_name) 116 | plt.xlabel('Time [sec]') 117 | plt.ylabel('Angular Velocity $w$ [deg/s]') 118 | plt.grid() 119 | plt.legend() 120 | 121 | 122 | 123 | if __name__ == '__main__': 124 | # Define experimental configuration 125 | dt, t_end = 0.1, 8 126 | r, w = 10., np.pi / 4 127 | get_true_position = lambda t: r * np.array([[np.cos(w * t)], [np.sin(w * t)]]) # Circular motion 128 | get_true_heading = lambda t: np.arctan2(np.cos(w * t), -np.sin(w * t)) 129 | gps_noise_std = 1 130 | 131 | # Instantiate EKF for pose (and velocity) tracking 132 | localizer_name = 'EKF' 133 | localizer = EKFLocalizer(v_noise_std=0.1, w_noise_std=0.1, gps_noise_std=gps_noise_std, dt=dt) 134 | 135 | truth, state, obser, covar = [], [], [], [] 136 | for t in np.arange(0, t_end, dt): 137 | # Simulate position observation with additive Gaussian noise 138 | true_pos = get_true_position(t) 139 | true_ori = get_true_heading(t) 140 | gps_data = true_pos + np.random.normal(size=true_pos.shape, scale=gps_noise_std) 141 | 142 | # Predict and update the EKF 143 | localizer.predict() 144 | localizer.update(gps_data) 145 | 146 | if localizer.x[2] >= np.pi: 147 | localizer.x[2] -= 2 * np.pi 148 | elif localizer.x[2] < -np.pi: 149 | localizer.x[2] += 2 * np.pi 150 | 151 | # Record true state, observation, estimated state, and its covariance 152 | truth.append([t] + true_pos.flatten().tolist() + [true_ori, r * w, w]) 153 | state.append([t] + localizer.x.flatten().tolist()) 154 | obser.append([t] + gps_data.flatten().tolist()) 155 | covar.append([t] + localizer.P.flatten().tolist()) 156 | 157 | # Visualize the results 158 | plot_results(localizer_name, truth, state, obser, covar) 159 | plt.show() -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # An Intuitive Tutorial on Bayesian Filtering 2 | ## Introduction 3 | This short tutorial aims to make readers understand Bayesian filtering intuitively. Instead of derivation of Kalman filter, I introduce Kalman filter from weighted average and moving average. I expect that readers will have intuition on Kalman filter such as meaning of equations. 4 | * To clone this repository (codes and slides): `git clone https://github.com/mint-lab/filtering_tutorial.git` 5 | * To install required Python packages: `pip install -r requirements.txt` 6 | * To fork this repository to your Github: [Click here](https://github.com/mint-lab/filtering_tutorial/fork) 7 | * To download codes and slides as a ZIP file: [Click here](https://github.com/mint-lab/filtering_tutorial/archive/master.zip) 8 | * To see the lecture slides: [Click here](https://github.com/mint-lab/filtering_tutorial/blob/master/slides/filtering_tutorial.pdf) 9 | * :memo: Please don't miss Roger Labbe's great book, [Kalman and Bayesian Filters in Python](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python) 10 | 11 | This tutorial contains example applications to **2-D localization (a.k.a. target tracking) with various conditions and situations**. It is important for users to know how to define the following five items in their applications. I hope that readers can build their intuition from my series of examples. My codes are based on [FilterPy](https://github.com/rlabbe/filterpy/), but their contents and your understanding may not be limited to the library. 12 | 1. State variable 13 | 1. State transition function 14 | 1. State transition noise 15 | 1. Observation function 16 | 1. Observation noise 17 | 18 | 19 | 20 | ## Code Examples 21 | ### 1) From Weighted and Moving Average to Simple 1-D Kalman Filter 22 | * **Merging two weight measurements** 23 | * **Merging two weight measurements with their variance** ([inverse-variance weighted average](https://en.wikipedia.org/wiki/Inverse-variance_weighting)) 24 | * **1-D noisy signal filtering with [exponential moving average](https://en.wikipedia.org/wiki/Moving_average#Exponential_moving_average)**: [`ema_1d_signal.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/ema_1d_signal.py) 25 | * **1-D noisy signal filtering with simple 1-D Kalman filter**: [`simple_kf_1d_signal.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/simple_kf_1d_signal.py) 26 | 27 | ### 2) [Kalman Filter](https://en.wikipedia.org/wiki/Kalman_filter) 28 | * **1-D noisy signal filtering**: [`kf_1d_signal.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/kf_1d_signal.py) 29 | * State variable: $\mathbf{x} = x$ 30 | * State transition function: $\mathbf{x}\_{k+1} = f(\mathbf{x}\_k; \mathbf{u}\_{k+1}) = \mathbf{x}\_k$ 31 | * Control input: $\mathbf{u}_k = [ ]$ 32 | * State transition noise: $\mathrm{Q} = \sigma^2_Q$ 33 | * Observation function: $\mathbf{z} = h(\mathbf{x}) = \mathbf{x}$ 34 | * Observation: 1-D signal value 35 | * Observation noise: $\mathrm{R} = \sigma^2_{R}$ 36 | 37 | * **2-D position tracking** (_without_ class inheritance): [`kf_2d_position.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/kf_2d_position.py) 38 | * State variable: $\mathbf{x} = [x, y]^\top$ 39 | * State transition function: $\mathbf{x}\_{k+1} = f(\mathbf{x}\_k; \mathbf{u}\_{k+1}) = \mathbf{x}\_k$ 40 | * Control input: $\mathbf{u}_k = [ ]$ 41 | * State transition noise: $\mathrm{Q} = \mathrm{diag}(\sigma^2_x, \sigma^2_y)$ 42 | * Observation function: $\mathbf{z} = h(\mathbf{x}) = [x, y]^\top$ 43 | * Observation: $\mathbf{z} = [x_{GPS}, y_{GPS}]^\top$ 44 | * Observation noise: $\mathrm{R} = \mathrm{diag}(\sigma^2_{GPS}, \sigma^2_{GPS})$ 45 | 46 | ### 3) [Extended Kalman Filter](https://en.wikipedia.org/wiki/Extended_Kalman_filter) (EKF) 47 | * **2-D pose tracking with simple transition noise** (_without_ class inheritance): [`ekf_2d_pose_simple_noise.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/ekf_2d_pose_simple_noise.py) 48 | * State variable: $\mathbf{x} = [x, y, \theta, v, w]^\top$ 49 | * State transition function: Constant velocity model (time interval: $t$) 50 | * Control input: $\mathbf{u}_k = [ ]$ 51 | ```math 52 | \mathbf{x}_{k+1} = f(\mathbf{x}_k; \mathbf{u}_{k+1}) = \begin{bmatrix} x_k + v_k t \cos(\theta_k + w_k t / 2) \\ y_k + v_k t \sin(\theta_k + w_k t / 2) \\ \theta_k + w_k t \\ v_k \\ w_k \end{bmatrix} 53 | ``` 54 | * * State transition noise: $\mathrm{Q} = \mathrm{diag}(\sigma^2_x, \sigma^2_y, \sigma^2_\theta, \sigma^2_v, \sigma^2_w)$ 55 | * Observation function: $\mathbf{z} = h(\mathbf{x}) = [x, y]^\top$ 56 | * Observation: $\mathbf{z} = [x_{GPS}, y_{GPS}]^\top$ 57 | * Observation noise: $\mathrm{R} = \mathrm{diag}(\sigma^2_{GPS}, \sigma^2_{GPS})$ 58 | 59 | * **2-D pose tracking** (_using_ class inheritance): [`ekf_2d_pose.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/ekf_2d_pose.py) 60 | * Its _state variable_, _state transition function_, _observation function_, and _observation noise_ are same with [`ekf_2d_pose_simple_noise.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/ekf_2d_pose_simple_noise.py). 61 | * State transition noise derived from **translational and rotational motion noises** ($\sigma_v^2$ and $\sigma_w^2$) [[Thrun05]](http://www.probabilistic-robotics.org/) 62 | ```math 63 | \mathrm{Q} = \mathrm{W}^\top \mathrm{M} \mathrm{W} \quad \text{where} \quad \mathrm{W} = \begin{bmatrix} \frac{\partial f}{\partial v} & \frac{\partial f}{\partial w} \end{bmatrix} \quad \text{and} \quad \mathrm{M} = \begin{bmatrix} \sigma^2_v & 0 \\ 0 & \sigma^2_w \end{bmatrix} 64 | ``` 65 | 66 | * **2-D pose tracking with [two velocity constraints]()**: [`ekf_2d_pose_vel_constraints.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/ekf_2d_pose_vel_constraints.py) 67 | * Its _state variable_, _state transition noise_, _observation function_, and _observation noise_ are same with [`ekf_2d_pose.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/ekf_2d_pose.py). 68 | * State transition function with **angular rate saturation** [[Cho24]](http://doi.org/10.1109/ACCESS.2024.3432335) 69 | ```math 70 | \mathbf{x}_{k+1} = f(\mathbf{x}_k; \mathbf{u}_{k+1}) = \begin{bmatrix} x_k + v_k t \cos(\theta_k + w_k t / 2) \\ y_k + v_k t \sin(\theta_k + w_k t / 2) \\ \theta_k + w_k t \\ v_k \\ w_{max} \tanh{(w_k / w_{max})} \end{bmatrix} 71 | ``` 72 | * * Post-processing with **heading angle correction** [[Cho24]](http://doi.org/10.1109/ACCESS.2024.3432335) 73 | ```math 74 | \mathbf{x}_k = \left\{ \begin{array}{ll} 75 | [x_k, y_k, \theta_k+\pi, -v_k, w_k]^\top & \text{if} \;\; v_k < \epsilon_- \\ 76 | \mathbf{x}_k & \text{otherwise} 77 | \end{array} \right. 78 | ``` 79 | 80 | * **2-D pose tracking with odometry**: [`ekf_2d_pose_odometry.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/ekf_2d_pose_odometry.py) 81 | * Its _state transition noise_, _observation function_, and _observation noise_ are same with [`ekf_2d_pose.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/ekf_2d_pose.py). 82 | * State variable: $\mathbf{x} = [x, y, \theta]^\top$ 83 | * State transition function with **translational and rotational control inputs**: Constant velocity model (time interval: $t$) 84 | * Control input: $\mathbf{u}_k = [v_k, w_k]^\top$ 85 | ```math 86 | \mathbf{x}_{k+1} = f(\mathbf{x}_k; \mathbf{u}_{k+1}) = \begin{bmatrix} x_k + v_{k+1} t \cos(\theta_k + w_{k+1} t / 2) \\ y_k + v_{k+1} t \sin(\theta_k + w_{k+1} t / 2) \\ \theta_k + w_{k+1} t \end{bmatrix} 87 | ``` 88 | 89 | * **2-D pose tracking with [off-centered GPS](http://doi.org/10.1109/TITS.2019.2915108)**: [`ekf_2d_pose_off_centered.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/ekf_2d_pose_off_centered.py) 90 | * Its _state variable_, _state transition function_, _state transition noise_, and _observation noise_ are same with [`ekf_2d_pose.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/ekf_2d_pose.py). 91 | * Observation function with off-centered GPS [[Choi20]](http://doi.org/10.1109/TITS.2019.2915108): Off-centered GPS ( $o_x$ and $o_y$ are frontal and lateral offset of the GPS.)

92 | ```math 93 | \mathbf{z} = \begin{bmatrix} x_{GPS} \\ y_{GPS} \end{bmatrix} = h(\mathbf{x}) = \begin{bmatrix} x + o_x \cos \theta - o_y \sin \theta \\ y + o_x \sin \theta + o_y \cos \theta \end{bmatrix} 94 | ``` 95 | 96 | ### 4) [Unscented Kalman Filter](https://en.wikipedia.org/wiki/Kalman_filter#Unscented_Kalman_filter) (UKF) 97 | * **2-D pose tracking with simple transition noise**: [`ukf_2d_pose_simple_noise.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/ukf_2d_pose_simple_noise.py) 98 | * Its five definitions (and also implementation style) are same with [`ekf_2d_pose_simple_noise.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/ekf_2d_pose_simple_noise.py). 99 | * **2D pose tracking**: [`ukf_2d_pose.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/ukf_2d_pose.py) 100 | * Its five definitions (and also implementation style) are same with [`ekf_2d_pose.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/ekf_2d_pose.py). 101 | 102 | ### 5) [Particle Filter](https://en.wikipedia.org/wiki/Particle_filter) 103 | * **2-D pose tracking**: [`pf_2d_pose.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/pf_2d_pose.py) 104 | * Its five definitions (and also implementation style) are same with [`ekf_2d_pose.py`](https://github.com/mint-lab/filtering_tutorial/blob/master/ekf_2d_pose.py). 105 | 106 | 107 | 108 | ## References 109 | * [FilterPy Documentation](https://filterpy.readthedocs.io/en/latest/): FilterPy API and examples 110 | * Bookmarks: [Kalman filter](https://filterpy.readthedocs.io/en/latest/kalman/KalmanFilter.html), [EKF](https://filterpy.readthedocs.io/en/latest/kalman/ExtendedKalmanFilter.html), [UKF](https://filterpy.readthedocs.io/en/latest/kalman/UnscentedKalmanFilter.html), [resampling](https://filterpy.readthedocs.io/en/latest/monte_carlo/resampling.html) (for particle filter) 111 | * [Kalman and Bayesian Filters in Python](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python): A great introduction on Bayesian filtering with FilterPy 112 | * Bookmarks: [Table of Contents](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/table_of_contents.ipynb), [Kalman filter](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/08-Designing-Kalman-Filters.ipynb), [EKF](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/11-Extended-Kalman-Filters.ipynb), [UKF](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/10-Unscented-Kalman-Filter.ipynb), [particle filter](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/12-Particle-Filters.ipynb) 113 | * Particle filter 114 | * [pfilter](https://github.com/johnhw/pfilter), John Williamson 115 | * [Monte Carlo Particle Filter for Localization](https://github.com/p16i/particle-filter), Pattarawat Chormai 116 | * [particle_filter_demo](https://github.com/mjl/particle_filter_demo), Martin J. Laubach 117 | * [Particle Filter for _Turtle_ Localization](https://github.com/leimao/Particle-Filter), Lei Mao 118 | 119 | 120 | 121 | ## How to Cite 122 | If you want to cite this tutorial and codes, please cite one of the following papers. 123 | 124 | ```bibtex 125 | @article{Cho24, 126 | author = {Se-Hyoung Cho and Sunglok Choi}, 127 | title = {Accurate and Resilient {GPS}-only Localization with Velocity Constraints}, 128 | journal = {IEEE Access}, 129 | volume = {12}, 130 | year = {2024}, 131 | doi = {10.1109/ACCESS.2024.3432335} 132 | } 133 | ``` 134 | 135 | ```bibtex 136 | @article{Choi20, 137 | author = {Sunglok Choi and Jong-Hwan Kim}, 138 | title = {Leveraging Localization Accuracy with Off-centered {GPS}}, 139 | journal = {IEEE Transactions on Intelligent Transportation Systems}, 140 | volume = {21}, 141 | number = {6}, 142 | year = {2020}, 143 | doi = {10.1109/TITS.2019.2915108} 144 | } 145 | ``` 146 | 147 | 148 | 149 | ## Authors 150 | * [Sunglok Choi](https://mint-lab.github.io/sunglok/) 151 | * [Hyunkil Hwang](https://github.com/Hyunkil76) 152 | 153 | 154 | 155 | ## Acknowledgement 156 | This tutorial was supported by the following R&D projects in Korea. 157 | * AI-based Localization and Path Planning on 3D Building Surfaces (granted by [MSIT](https://www.msit.go.kr/)/[NRF](https://www.nrf.re.kr/), grant number: 2021M3C1C3096810) 158 | --------------------------------------------------------------------------------