├── .idea ├── .name ├── vcs.xml ├── misc.xml ├── inspectionProfiles │ └── profiles_settings.xml ├── modules.xml ├── estimation_algorithms_.iml └── workspace.xml ├── results ├── kf_mean.txt ├── kf_variance.txt ├── ekf_mean.txt ├── pf_mean.txt ├── ukf_mean.txt ├── ekf_variance.txt ├── pf_variance.txt └── ukf_variance.txt ├── .gitattributes ├── figuresEKF.png ├── figuresPF.png ├── figuresUKF.png ├── figuresKalman.png ├── __pycache__ ├── models.cpython-311.pyc ├── kalman_filter.cpython-311.pyc ├── particle_filter.cpython-311.pyc ├── filter_comparison.cpython-311.pyc ├── trajectory_generator.cpython-311.pyc ├── extended_kalman_filter.cpython-311.pyc └── unscented_kalman_filter.cpython-311.pyc ├── README.md ├── LICENSE ├── models.py ├── test.py ├── main.py ├── particle_filter.py ├── trajectory_generator.py ├── kalman_filter.py ├── extended_kalman_filter.py ├── filter_comparison.py └── unscented_kalman_filter.py /.idea/.name: -------------------------------------------------------------------------------- 1 | estimation_algorithms_ -------------------------------------------------------------------------------- /results/kf_mean.txt: -------------------------------------------------------------------------------- 1 | 0.102587 2 | 0.754145 3 | 0.102369 4 | 0.753820 5 | -------------------------------------------------------------------------------- /results/kf_variance.txt: -------------------------------------------------------------------------------- 1 | 0.000101 2 | 0.000605 3 | 0.000103 4 | 0.000532 5 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /figuresEKF.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/n-jokic/estimation_algorithms/HEAD/figuresEKF.png -------------------------------------------------------------------------------- /figuresPF.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/n-jokic/estimation_algorithms/HEAD/figuresPF.png -------------------------------------------------------------------------------- /figuresUKF.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/n-jokic/estimation_algorithms/HEAD/figuresUKF.png -------------------------------------------------------------------------------- /figuresKalman.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/n-jokic/estimation_algorithms/HEAD/figuresKalman.png -------------------------------------------------------------------------------- /results/ekf_mean.txt: -------------------------------------------------------------------------------- 1 | 0.055210 2 | 0.162448 3 | 0.173735 4 | 0.063156 5 | 0.175611 6 | 0.174269 7 | 0.077123 8 | -------------------------------------------------------------------------------- /results/pf_mean.txt: -------------------------------------------------------------------------------- 1 | 0.033678 2 | 0.043826 3 | 0.100569 4 | 0.033337 5 | 0.045800 6 | 0.100827 7 | 0.083330 8 | -------------------------------------------------------------------------------- /results/ukf_mean.txt: -------------------------------------------------------------------------------- 1 | 0.052993 2 | 0.074632 3 | 0.370974 4 | 0.067419 5 | 0.068218 6 | 0.170731 7 | 0.076463 8 | -------------------------------------------------------------------------------- /results/ekf_variance.txt: -------------------------------------------------------------------------------- 1 | 0.000132 2 | 0.000991 3 | 0.000081 4 | 0.000110 5 | 0.000878 6 | 0.000083 7 | 0.000048 8 | -------------------------------------------------------------------------------- /results/pf_variance.txt: -------------------------------------------------------------------------------- 1 | 0.000084 2 | 0.000252 3 | 0.000058 4 | 0.000077 5 | 0.000330 6 | 0.000051 7 | 0.000049 8 | -------------------------------------------------------------------------------- /results/ukf_variance.txt: -------------------------------------------------------------------------------- 1 | 0.000037 2 | 0.000313 3 | 0.000087 4 | 0.000035 5 | 0.000335 6 | 0.000080 7 | 0.000046 8 | -------------------------------------------------------------------------------- /__pycache__/models.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/n-jokic/estimation_algorithms/HEAD/__pycache__/models.cpython-311.pyc -------------------------------------------------------------------------------- /__pycache__/kalman_filter.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/n-jokic/estimation_algorithms/HEAD/__pycache__/kalman_filter.cpython-311.pyc -------------------------------------------------------------------------------- /__pycache__/particle_filter.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/n-jokic/estimation_algorithms/HEAD/__pycache__/particle_filter.cpython-311.pyc -------------------------------------------------------------------------------- /__pycache__/filter_comparison.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/n-jokic/estimation_algorithms/HEAD/__pycache__/filter_comparison.cpython-311.pyc -------------------------------------------------------------------------------- /__pycache__/trajectory_generator.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/n-jokic/estimation_algorithms/HEAD/__pycache__/trajectory_generator.cpython-311.pyc -------------------------------------------------------------------------------- /__pycache__/extended_kalman_filter.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/n-jokic/estimation_algorithms/HEAD/__pycache__/extended_kalman_filter.cpython-311.pyc -------------------------------------------------------------------------------- /__pycache__/unscented_kalman_filter.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/n-jokic/estimation_algorithms/HEAD/__pycache__/unscented_kalman_filter.cpython-311.pyc -------------------------------------------------------------------------------- /.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /.idea/inspectionProfiles/profiles_settings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | -------------------------------------------------------------------------------- /.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # estimation_algorithms 2 | Implementation of followinf estimation algorithms in python: Kalman Filter, Extended Kalman Filter, Unscented Kalman Filter, Cubature Kalman Filter, Rao-Blackwellized Particle Filter 3 | 4 | Currently Kalman Filter, Extended Kalman Filter, Unscented Kalman Filter and Particle filter are fully implemented. 5 | -------------------------------------------------------------------------------- /.idea/estimation_algorithms_.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 15 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 n-jokic 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /models.py: -------------------------------------------------------------------------------- 1 | import jax 2 | import jax.numpy as jnp 3 | import numpy as np 4 | def proces_model(x_state): 5 | dt = 0.05 6 | 7 | x_new = jnp.array([x_state[0] + (jnp.cos(x_state[6])*x_state[1] - jnp.sin(x_state[6])*x_state[4])*dt, 8 | x_state[1] + x_state[2]*dt, 9 | x_state[2], 10 | x_state[3] + (jnp.cos(x_state[6]) * x_state[4] + jnp.sin(x_state[6]) * x_state[1]) * dt, 11 | x_state[4] + x_state[5] * dt, 12 | x_state[5], 13 | x_state[6] 14 | ]) 15 | return x_new 16 | 17 | 18 | def measurement_model(x_state): 19 | z = jnp.array([x_state[0], 20 | x_state[2], 21 | x_state[3], 22 | x_state[5], 23 | x_state[6] 24 | ]) 25 | return z 26 | 27 | def process_model_kalman(x_state): 28 | x_new = x_state 29 | return x_state 30 | def measurement_model_kalman(x_state): 31 | return x_state 32 | 33 | def proces_model_ukf(x_state): 34 | num_sigma_states = x_state.shape[0] 35 | x_new = np.zeros(x_state.shape) 36 | for idx in range(num_sigma_states): 37 | x_new[idx, :, :] = proces_model(x_state[idx, :, 0]).reshape((x_state.shape[1], 1)) 38 | return x_new 39 | 40 | 41 | def measurement_model_ukf(x_state): 42 | num_sigma_states = x_state.shape[0] 43 | z_new = np.zeros((x_state.shape[0], x_state.shape[1]-2, x_state.shape[2])) 44 | for idx in range(num_sigma_states): 45 | z_new[idx, :, :] = measurement_model(x_state[idx, :, 0]).reshape((x_state.shape[1]-2, 1)) 46 | 47 | return z_new 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /test.py: -------------------------------------------------------------------------------- 1 | import jax 2 | import jax.numpy as jnp 3 | import numpy as np 4 | from jax import random 5 | import extended_kalman_filter as ekf 6 | import unscented_kalman_filter as ukf 7 | 8 | key = random.PRNGKey(0) 9 | import matplotlib.pyplot as plt 10 | def process_model(x): 11 | 12 | A = jnp.array([[0.5, 0.1], [0, 1]]) 13 | x = A@x 14 | return x 15 | 16 | def state_update(x, sigma_p): 17 | r = np.random.randn()*sigma_p 18 | x = process_model(x) + np.array([0, 1])*r 19 | return x 20 | 21 | def measurement_model(x): 22 | C = jnp.array([[1, 0]]) 23 | a = jnp.power(C@x, 1) 24 | return a 25 | 26 | t = np.linspace(0, 1, num = 100) 27 | n = len(t) 28 | x0 = np.array([3, 0.1]) 29 | print(x0.shape) 30 | x = np.zeros((2, n)) 31 | y = np.zeros((1, n)) 32 | xk = x0 33 | for idx, i in enumerate(t): 34 | x[:, idx] = xk 35 | y[:, idx] = measurement_model(xk) 36 | xk = state_update(xk, sigma_p=0.001) 37 | 38 | print(np.array(x)) 39 | print(np.array(y)) 40 | 41 | #plt.plot(t, x.T) 42 | #plt.show() 43 | 44 | f = lambda m: process_model(m) 45 | h = lambda z: measurement_model(z) 46 | Jf = jax.jacobian(f) 47 | Jh = jax.jacobian(h) 48 | R = 0.01 49 | 50 | ekf_1 = ekf.ExtendedKalmanFilter(dim_x=2, dim_z=1) 51 | ekf_1.Q = np.eye(2)*10 52 | ekf_1.dxh = Jh 53 | ekf_1.R = np.eye(1)*R 54 | ekf_1.dxf = Jf 55 | ekf_1.f = process_model 56 | ekf_1.h = measurement_model 57 | ekf_1.P = np.eye(2)*10 58 | ekf_1.x = np.array([12., -1.]) 59 | 60 | ekf_est = [] 61 | for idx, i in enumerate(t): 62 | y[:, idx] += np.random.randn()*R 63 | 64 | ekf_est = np.zeros((2, n)) 65 | for idx, i in enumerate(t): 66 | ekf_1.predict() 67 | ekf_1.update(z=y[:, idx]) 68 | ekf_est[:, idx] = ekf_1.x 69 | 70 | print(ekf_est) 71 | 72 | def measurement_model_2(x): 73 | C = jnp.array([[0, 1]]) 74 | a = jnp.power(C@x, 1) 75 | return a 76 | h = lambda z: measurement_model_2(z) 77 | Jh_2 = jax.jacobian(h) 78 | xk = np.array([3, 0.1]) 79 | y_2 = np.zeros((1, n)) 80 | for idx, i in enumerate(t): 81 | xk = x[:, idx] 82 | y_2[:, idx] = measurement_model_2(xk) + np.random.randn()*R 83 | 84 | #xk = state_update(xk, sigma_p=1) 85 | 86 | 87 | ekf_1 = ekf.ExtendedKalmanInformationFilter(dim_x=2, dim_z=1) 88 | ekf_1.Q = np.eye(2)*1 89 | ekf_1.dxh = [Jh, Jh_2] 90 | ekf_1.R_inv = np.zeros((2, 1, 1)) 91 | ekf_1.R_inv[0, :, :] = np.eye(1)*1/R 92 | ekf_1.R_inv[1, :, :] = np.eye(1)*1/R 93 | ekf_1.dxf = Jf 94 | ekf_1.f = process_model 95 | ekf_1.h = [measurement_model, measurement_model_2] 96 | ekf_1.P_inv = np.eye(2)*10 97 | ekf_1.x = np.array([3., 0.1]) 98 | 99 | print(Jh(xk).shape) 100 | ekf_est = np.zeros((2, n)) 101 | for idx, i in enumerate(t): 102 | ekf_1.update(z=np.array([y[:, idx], y_2[:, idx]]), multiple_sensors=True) 103 | ekf_est[:, idx] = ekf_1.x 104 | ekf_1.predict() 105 | 106 | 107 | ukf_1 = ukf.UnscentedKalmanFilter(dim_x=2, dim_z=1) 108 | ukf_1.h=measurement_model 109 | ukf_1.f=process_model 110 | ukf_1.P=np.eye(2)*1 111 | ukf_1.x=np.array([3., 0.1]) 112 | ukf_1.R=np.eye(1)*R 113 | ukf_1.Q=np.eye(2)*1 114 | ekf_est = np.zeros((2, n)) 115 | ukf_1.max_iter = 1000 116 | for idx, i in enumerate(t): 117 | ukf_1.update(z=y[:, idx]) 118 | ekf_est[:, idx] = ukf_1.x 119 | ukf_1.prediction() 120 | 121 | 122 | 123 | plt.plot(t, x.T) 124 | plt.plot(t, ekf_est.T, 'k--') 125 | plt.plot(t, y.T, color='black', linestyle='dotted') 126 | #plt.plot(t, y_2.T, color = 'red', linestyle = 'dotted') 127 | plt.show() 128 | 129 | ukf_1 = ukf.UnscentedKalmanInformationFilter(dim_x=2, dim_z=1) 130 | ukf_1.h = [measurement_model, measurement_model_2] 131 | ukf_1.f = process_model 132 | ukf_1.P_inv = np.eye(2)*1 133 | ukf_1.x = np.array([3., 0.1]) 134 | ukf_1.y = np.array([3., 0.1]) 135 | ekf_1.R_inv = np.zeros((2, 1, 1)) 136 | ekf_1.R_inv[0, :, :] = np.eye(1)*1/R 137 | ekf_1.R_inv[1, :, :] = np.eye(1)*1/R 138 | ukf_1.Q=np.eye(2)*1 139 | ekf_est = np.zeros((2, n)) 140 | ukf_1.max_iter = 1000 141 | for idx, i in enumerate(t): 142 | ukf_1.update(z=np.array([y[:, idx], y_2[:, idx]]), multiple_sensors=True) 143 | ekf_est[:, idx] = ukf_1.x 144 | ukf_1.prediction() 145 | 146 | plt.plot(t, x.T) 147 | plt.plot(t, ekf_est.T, 'k--') 148 | plt.plot(t, y.T, color='black', linestyle='dotted') 149 | #plt.plot(t, y_2.T, color = 'red', linestyle = 'dotted') 150 | plt.show() 151 | 152 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | # This is a sample Python script. 2 | 3 | # Press Shift+F10 to execute it or replace it with your code. 4 | # Press Double Shift to search everywhere for classes, files, tool windows, actions, and settings. 5 | 6 | 7 | import numpy as np 8 | import filter_comparison 9 | import matplotlib.pyplot as plt 10 | import os 11 | import trajectory_generator 12 | 13 | 14 | trajectory_generator.plot_example(101, 0.05) 15 | print('done_example') 16 | 17 | #error = np.array([10, 1, -10, -1]) 18 | #filter_comparison.only_kalman(np.zeros(4, ) + error, plot=True, save_plot=True) 19 | kf_res = filter_comparison.only_kalman(np.zeros(4, ), plot=True, save_plot=True) 20 | print(kf_res[1]) 21 | print(kf_res[0]) 22 | 23 | #error = np.array([10, 1, 0.4, -10, -1, -.4, 1]) 24 | #filter_comparison.run_comparison(np.zeros(7, ) + error, plot=True, save_plot=True) 25 | filter_comparison.run_comparison(np.zeros(7, ), plot=True, save_plot=True) 26 | 27 | plt.close('all') 28 | 29 | print('done, first part') 30 | 31 | N_mc = 100 32 | 33 | t_kf = [] 34 | kf_rms = np.zeros((N_mc, 4)) 35 | t_ekf = [] 36 | ekf_rms = np.zeros((N_mc, 7)) 37 | t_ukf = [] 38 | ukf_rms = np.zeros((N_mc, 7)) 39 | t_pf = [] 40 | pf_rms = np.zeros((N_mc, 7)) 41 | no_filt_rms = np.zeros((N_mc, 4)) 42 | 43 | for i in range(N_mc): 44 | kf_res = filter_comparison.only_kalman(np.zeros(4, )) 45 | t_kf.append(kf_res[2]) 46 | kf_rms[i, :] = kf_res[0] 47 | other_res = filter_comparison.run_comparison(np.zeros(7, )) 48 | ekf_rms[i, :] = other_res[0] 49 | ukf_rms[i, :] = other_res[1] 50 | pf_rms[i, :] = other_res[2] 51 | t_ekf.append(other_res[3]) 52 | t_ukf.append(other_res[4]) 53 | t_pf.append(other_res[5]) 54 | no_filt_rms[i, :] = other_res[6] 55 | print(i) 56 | 57 | output_folder = "results" 58 | # Create the output folder if it doesn't exist 59 | os.makedirs(output_folder, exist_ok=True) 60 | 61 | 62 | m_kf = np.mean(kf_rms, axis=0) 63 | v_kf = np.var(kf_rms, axis=0) 64 | 65 | variable_name = 'kf' 66 | mean_filename = os.path.join(output_folder, f"{variable_name}_mean.txt") 67 | variance_filename = os.path.join(output_folder, f"{variable_name}_variance.txt") 68 | np.savetxt(mean_filename, m_kf, delimiter=',', fmt='%f') 69 | np.savetxt(variance_filename, v_kf, delimiter=',', fmt='%f') 70 | 71 | m_ekf = np.mean(ekf_rms, axis=0) 72 | v_ekf = np.var(ekf_rms, axis=0) 73 | 74 | variable_name = 'ekf' 75 | mean_filename = os.path.join(output_folder, f"{variable_name}_mean.txt") 76 | variance_filename = os.path.join(output_folder, f"{variable_name}_variance.txt") 77 | np.savetxt(mean_filename, m_ekf, delimiter=',', fmt='%f') 78 | np.savetxt(variance_filename, v_ekf, delimiter=',', fmt='%f') 79 | 80 | m_ukf = np.mean(ukf_rms, axis=0) 81 | v_ukf = np.var(ukf_rms, axis=0) 82 | 83 | variable_name = 'ukf' 84 | mean_filename = os.path.join(output_folder, f"{variable_name}_mean.txt") 85 | variance_filename = os.path.join(output_folder, f"{variable_name}_variance.txt") 86 | np.savetxt(mean_filename, m_ukf, delimiter=',', fmt='%f') 87 | np.savetxt(variance_filename, v_ukf, delimiter=',', fmt='%f') 88 | 89 | m_pf = np.mean(pf_rms, axis=0) 90 | v_pf = np.var(pf_rms, axis=0) 91 | 92 | variable_name = 'pf' 93 | mean_filename = os.path.join(output_folder, f"{variable_name}_mean.txt") 94 | variance_filename = os.path.join(output_folder, f"{variable_name}_variance.txt") 95 | np.savetxt(mean_filename, m_pf, delimiter=',', fmt='%f') 96 | np.savetxt(variance_filename, v_pf, delimiter=',', fmt='%f') 97 | 98 | 99 | print('done!') 100 | t = {'Kalman': t_kf, 'EKF': t_ekf, 'UKF': t_ukf, 'PF': t_pf} 101 | for key in t: 102 | data = t[key] 103 | 104 | # Compute mean and standard deviation 105 | mean = np.mean(data) 106 | std = np.std(data) 107 | 108 | # Create subplots 109 | fig, ax = plt.subplots() 110 | 111 | # Plot histogram 112 | ax.hist(data, bins=30, alpha=0.5, color='blue', edgecolor='black') 113 | ax.axvline(mean, color='red', linestyle='dashed', linewidth=1.5, label='srednja vrednost') 114 | 115 | # Plot bars for 3 standard deviations 116 | ax.axvspan(mean - 3 * std, mean + 3 * std, color='orange', alpha=0.2, label='+- 3 std') 117 | # Set labels and title 118 | ax.set_xlabel('t [s]') 119 | #ax.set_title('Histogram of : ' + algorithm + ' estmation') 120 | #ax.set_xlim([0.58, 0.71]) 121 | # Add legend 122 | ax.legend() 123 | plt.rcParams['text.usetex'] = False 124 | # Display the plot 125 | ax.set_rasterized(True) 126 | FULL_PATH = r'C:\Users\milos\OneDrive\VIII_semestar\diplomski\code\estimation_algorithms\figures' + key+ '.png' 127 | plt.savefig(FULL_PATH, format='png', dpi=600) 128 | plt.show() 129 | 130 | 131 | 132 | 133 | -------------------------------------------------------------------------------- /particle_filter.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | import numpy as np 4 | 5 | 6 | class particle(): 7 | def __init__(self, x_state): 8 | self.x_current = x_state 9 | self.x_new = np.zeros(x_state.shape) 10 | 11 | def prediction(self): 12 | dt = 0.05 13 | x_state = self.x_current 14 | sigma_pos = 0 15 | sigma = 0.2 16 | self.x_new = np.array([x_state[0] + (np.cos(x_state[6]) * x_state[1] - np.sin(x_state[6]) * x_state[4]) * dt, 17 | x_state[1] + x_state[2] * dt, 18 | x_state[2] + np.random.randn()*sigma, 19 | x_state[3] + (np.cos(x_state[6]) * x_state[4] + np.sin(x_state[6]) * x_state[1]) * dt, 20 | x_state[4] + x_state[5] * dt, 21 | x_state[5] + np.random.randn()*sigma, 22 | x_state[6] + np.random.randn()*sigma 23 | ]) 24 | 25 | self.x_current = self.x_new 26 | 27 | def likelihood(self, z): 28 | sigma = 0.1**2 29 | x_obs = z[0] 30 | acc_x_obs = z[1] 31 | y_obs = z[2] 32 | acc_y_obs = z[3] 33 | theta_obs = z[4] 34 | 35 | x_pred = self.x_current[0] 36 | acc_x_pred = self.x_current[2] 37 | y_pred = self.x_current[3] 38 | acc_y_pred = self.x_current[5] 39 | theta_pred = self.x_current[6] 40 | 41 | sigma_pos = .1**2 42 | sigma_theta = 0.1**2 43 | 44 | 45 | return np.exp(-(x_pred-x_obs)**2/2/sigma_pos)*np.exp(-(y_pred-y_obs)**2/2/sigma_pos)*\ 46 | np.exp(-(acc_y_pred-acc_y_obs)**2/2/sigma)*np.exp(-(acc_x_pred-acc_x_obs)**2/2/sigma)*\ 47 | np.exp(-(theta_pred-theta_obs)**2/2/sigma_theta) 48 | 49 | 50 | class particle_filter(): 51 | 52 | def __init__(self, **kwargs): 53 | 54 | if('sensor_data' in kwargs): 55 | self.sensor_data = kwargs['sensor_data'] 56 | else: 57 | self.sensor_data = np.array([]) 58 | 59 | if('N_particles' in kwargs): 60 | self.Np = kwargs['N_particles'] 61 | else: 62 | self.Np = 10 63 | 64 | self.particles = [particle(x_state=np.zeros((7, ))) for _ in range(self.Np)] 65 | 66 | if('algorithm' in kwargs): 67 | self.algorithm = kwargs['algorithm'] 68 | else: 69 | self.algorithm = "no_resampling" 70 | 71 | self.weights = np.ones((self.Np, 1))/self.Np 72 | 73 | self.t = 0 74 | 75 | self.old_particles = self.particles 76 | self.old_weights = self.weights 77 | 78 | def setSensorData(self, data): 79 | self.sensor_data = np.array(data) 80 | 81 | def setNumberParticles(self, Np): 82 | self.Np = Np 83 | self.particles = [particle() for _ in range(self.Np)] 84 | self.weights = np.ones((self.Np, 1))/Np 85 | 86 | def setCurrentStep(self, t): 87 | self.t = t 88 | def setAlgorithm(self, alg): 89 | self.algorithm = alg 90 | 91 | def resetParticles(self, **kwargs): 92 | self.particles = [particle(x_state=np.zeros((7, ))) for _ in range(self.Np)] 93 | 94 | self.old_particles = self.particles 95 | 96 | self.weights = np.ones((self.Np, 1))/self.Np 97 | self.old_weights = self.weights 98 | self.t = 0 99 | 100 | def particle_filtering(self): 101 | 102 | if self.t > len(self.sensor_data): 103 | print('No more sensor data') 104 | return None 105 | 106 | 107 | z = self.sensor_data[self.t] 108 | self.t += 1 109 | 110 | for i in range(self.Np): 111 | self.particles[i].prediction() 112 | 113 | self.weights[i] *= self.particles[i].likelihood(z) 114 | 115 | w_sum = np.sum(self.weights) 116 | if w_sum!=0: 117 | self.weights = self.weights/w_sum 118 | else: 119 | self.weights = np.ones((self.Np, 1))/self.Np 120 | 121 | estimation = np.zeros((7, )) 122 | 123 | for idx, part in enumerate(self.particles): 124 | estimation += part.x_current*self.weights[idx] 125 | 126 | self.old_particles = self.particles 127 | self.old_weights = self.weights 128 | 129 | self.particles, self.weights = self.__update_particles(self.algorithm) 130 | 131 | return estimation 132 | 133 | def __update_particles(self, algorithm): 134 | if algorithm == "no_resampling": 135 | return self.__no_resampling() 136 | if algorithm == "iterative_resampling": 137 | return self.__iterative_resampling() 138 | if algorithm == "dynamic_resampling": 139 | return self.__dynamic_resampling() 140 | 141 | print('Wrong algorithm specified') 142 | return self.particles, self.weights 143 | 144 | def __no_resampling(self): 145 | 146 | return self.particles, self.weights 147 | 148 | def __iterative_resampling(self): 149 | new_particles = [0]*self.Np 150 | new_weights = np.ones((self.Np, 1))/self.Np 151 | 152 | indices = np.random.choice(self.Np, self.Np, p=self.weights.flatten()) 153 | 154 | for idx in range(self.Np): 155 | idy = indices[idx] 156 | x_state = self.particles[idy].x_current 157 | 158 | new_particles[idx] = particle(x_state=x_state + np.random.randn(x_state.shape[0])*0.01) 159 | 160 | return new_particles, new_weights 161 | 162 | def __dynamic_resampling(self): 163 | 164 | w_s = np.sum(self.weights**2) 165 | if w_s != 0: 166 | effective_size =1/np.sum(self.weights**2) 167 | else: 168 | effective_size = self.Np/2 169 | 170 | if(effective_size < self.Np/2): 171 | return self.__iterative_resampling() 172 | else: 173 | return self.__no_resampling() -------------------------------------------------------------------------------- /trajectory_generator.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import os 4 | 5 | def trajectory_param_init(t): 6 | _T = t[-1] 7 | 8 | theta = np.sin(2*np.pi/_T*t)*np.pi/2 9 | acc_x_bf = -np.sin(4*np.pi/_T*t)*3 10 | acc_y_bf = np.sin(4*np.pi / _T * t)*3 11 | 12 | return acc_x_bf, acc_y_bf, theta 13 | 14 | 15 | def generate_trajectory(t): 16 | acc_x_bf, acc_y_bf, theta = trajectory_param_init(t) 17 | NUM_STATES = 7 # 2 positions, speeds and accelerations and 1 tilting angle 18 | trajectory = np.zeros(((len(t), NUM_STATES))) 19 | 20 | dt = t[1]-t[0] 21 | 22 | for idx, val in enumerate(t): 23 | trajectory[idx, 2] = acc_x_bf[idx] 24 | trajectory[idx, 5] = acc_y_bf[idx] 25 | trajectory[idx, 6] = theta[idx] 26 | 27 | if idx != 0: 28 | trajectory[idx, 1] = trajectory[idx-1, 1] + trajectory[idx-1, 2]*dt # update the speed_x 29 | trajectory[idx, 4] = trajectory[idx - 1, 4] + trajectory[idx - 1, 5] * dt # update the speed_y 30 | trajectory[idx, 6] = theta[idx] # update the tilting angle 31 | 32 | old_theta = theta[idx-1] 33 | 34 | trajectory[idx, 0] = trajectory[idx-1, 0] + (np.cos(old_theta)*trajectory[idx - 1, 1] 35 | - np.sin(old_theta)*trajectory[idx - 1, 4])*dt # update pos_x 36 | trajectory[idx, 3] = trajectory[idx-1, 3] + (np.sin(old_theta) * trajectory[idx - 1, 1] 37 | + np.cos(old_theta) * trajectory[idx - 1, 4])*dt # update pos_y 38 | 39 | return trajectory 40 | 41 | 42 | def measure(x_state): 43 | sigma_pos = 0.1 44 | sigma = 0.1 45 | sigma_theta = 0.1 46 | 47 | z = np.array([x_state[0] + np.random.randn()*sigma_pos, 48 | x_state[2] + np.random.randn()*sigma, 49 | x_state[3] + np.random.randn()*sigma_pos, 50 | x_state[5] + np.random.randn()*sigma, 51 | x_state[6] + np.random.randn()*sigma_theta 52 | ]) 53 | return z 54 | 55 | 56 | def measure_full_trajectory(trajectory): 57 | duration = len(trajectory[:, 0]) 58 | z = np.zeros((duration, 5)) 59 | 60 | for idx in range(duration): 61 | z[idx, :] = measure(trajectory[idx, :]) 62 | 63 | return z 64 | 65 | 66 | def measure_full_trajectory_kalman(trajectory): 67 | duration = len(trajectory[:, 0]) 68 | z = np.zeros((duration, 2)) 69 | 70 | for idx in range(duration): 71 | tmp = measure(trajectory[idx, :]) 72 | z[idx, :] = np.array([tmp[0], tmp[2]]) 73 | 74 | return z 75 | 76 | def plot_example(N, dt): 77 | dt = dt 78 | N = N 79 | t_end = (N - 1) * dt 80 | t = np.linspace(0, t_end, N) 81 | trajectory = generate_trajectory(t) 82 | z = measure_full_trajectory(trajectory) 83 | symb = ['sx [m]', 'ax [ms^-2] ', 'sy [m]', 'ay [ms^-2]', r'theta [rad]'] 84 | traj = [0, 2, 3, 5, 6] 85 | file_symb = ['sx', 'ax', 'sy', 'ay', r'theta'] 86 | 87 | 88 | # Generate a timestamp for unique filenames 89 | num_meas = 5 90 | 91 | for state_idx in range(num_meas): 92 | f = plt.figure(figsize=(8, 4)) 93 | plt.plot(t, z[:, state_idx], 'b--', marker='o', markevery=50, label='zašumljena trajektorija') 94 | plt.plot(t, trajectory[:, traj[state_idx]], 'k-', label='trajektorija') 95 | plt.grid(True) 96 | plt.legend() 97 | plt.xlabel('t [s]') 98 | plt.ylabel(symb[state_idx]) 99 | plt.tight_layout() 100 | nm = os.path.join('figures', f'example_{file_symb[state_idx]}.png') 101 | plt.savefig(nm, dpi=600) 102 | plt.close(f) 103 | num_state = 7 104 | state_symb = ['sx [m]', 'vx [ms^-1]', 'ax [ms^-2] ', 'sy [m]', 'vy [ms^-1]', 'ay [ms^-2]', r'theta [rad]'] 105 | file_symb = ['sx', 'vx', 'ax ', 'sy', 'vy', 'ay', r'theta'] 106 | for state_idx in range(num_state): 107 | if state_idx in set([0, 2, 3, 5, 6]): 108 | 109 | continue 110 | f = plt.figure(figsize=(8, 4)) 111 | plt.plot(t, trajectory[:, state_idx], 'k-', label='trajektorija') 112 | plt.grid(True) 113 | plt.legend() 114 | plt.xlabel('t [s]') 115 | plt.ylabel(state_symb[state_idx]) 116 | plt.tight_layout() 117 | nm = os.path.join('figures', f'example_{file_symb[state_idx]}.png') 118 | plt.savefig(nm, dpi=600) 119 | plt.close(f) 120 | 121 | v_trans = np.zeros((trajectory.shape[0], 2)) 122 | v_trans[:, 0] = np.cos(trajectory[:, 6]) * trajectory[:, 1] - np.sin(trajectory[:, 6]) * trajectory[:, 4] 123 | v_trans[:, 1] = np.cos(trajectory[:, 6]) * trajectory[:, 4] + np.sin(trajectory[:, 6]) * trajectory[:, 1] 124 | state_symb = ['vx [ms^-1]', 'vy [ms^-1]'] 125 | file_symb = ['vx', 'vy'] 126 | for idx in range(2): 127 | f = plt.figure(figsize=(8, 4)) 128 | plt.plot(t, v_trans[:, idx], 'k-', label='trajektorija') 129 | plt.grid(True) 130 | plt.legend() 131 | plt.xlabel('t [s]') 132 | plt.ylabel(state_symb[idx]) 133 | plt.tight_layout() 134 | nm = os.path.join('figures', f'example_{file_symb[idx]}_trans.png') 135 | plt.savefig(nm, dpi=600) 136 | plt.close(f) 137 | 138 | a_trans = np.zeros((trajectory.shape[0], 2)) 139 | a_trans[:, 0] = np.cos(trajectory[:, 6]) * trajectory[:, 2] - np.sin(trajectory[:, 6]) * trajectory[:, 5] 140 | a_trans[:, 1] = np.cos(trajectory[:, 6]) * trajectory[:, 5] + np.sin(trajectory[:, 6]) * trajectory[:, 2] 141 | z_trans = np.zeros((trajectory.shape[0], 2)) 142 | z_trans[:, 0] = np.cos(trajectory[:, 6]) * z[:, 1] - np.sin(trajectory[:, 6]) * z[:, 3] 143 | z_trans[:, 1] = np.cos(trajectory[:, 6]) * z[:, 3] + np.sin(trajectory[:, 6]) * z[:, 1] 144 | state_symb = ['ax [ms^-2]', 'ay [ms^-2]'] 145 | file_symb = ['ax', 'ay'] 146 | for idx in range(2): 147 | f = plt.figure(figsize=(8, 4)) 148 | plt.plot(t, a_trans[:, idx], 'k-', label='trajektorija') 149 | plt.plot(t, z_trans[:, idx], 'b--', marker='o', markevery=50, label='zašumljena trajektorija') 150 | plt.grid(True) 151 | plt.legend() 152 | plt.xlabel('t [s]') 153 | plt.ylabel(state_symb[idx]) 154 | plt.tight_layout() 155 | nm = os.path.join('figures', f'example_{file_symb[idx]}_trans.png') 156 | plt.savefig(nm, dpi=600) 157 | plt.close(f) 158 | 159 | 160 | -------------------------------------------------------------------------------- /.idea/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 54 | 55 | 60 | 61 | 62 | 64 | 65 | 67 | 68 | 69 | 72 | { 73 | "keyToString": { 74 | "RunOnceActivity.ShowReadmeOnStart": "true", 75 | "ignore.virus.scanning.warn.message": "true", 76 | "last_opened_file_path": "C:/Users/milos/OneDrive/VIII_semestar/diplomski/code/estimation_algorithms", 77 | "run.code.analysis.last.selected.profile": "aDefault", 78 | "settings.editor.selected.configurable": "com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" 79 | } 80 | } 81 | 82 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 1690043218142 99 | 103 | 104 | 105 | -------------------------------------------------------------------------------- /kalman_filter.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math as math 3 | 4 | 5 | class KalmanFilter(object): 6 | # x is the state vector, z measurement and u control vector 7 | # Model being used is in form : 8 | # x = Fx + Gu + w, z = Hx + v 9 | # w and v have covariance matrices Q and R 10 | def __init__(self, dim_x, dim_z, dim_u=0): 11 | if dim_x < 1: 12 | raise ValueError('dim_x must be 1 or greater') 13 | if dim_z < 1: 14 | raise ValueError('dim_z must be 1 or greater') 15 | if dim_u < 0: 16 | raise ValueError('dim_u must be 0 or greater') 17 | 18 | self.dim_x = dim_x 19 | self.dim_z = dim_z 20 | self.dim_u = dim_u 21 | 22 | self.x = np.zeros((dim_x, )) # state 23 | self.P = np.eye(dim_x) # uncertainty covariance 24 | self.Q = np.eye(dim_x) # process uncertainty 25 | self.B = None # control transition matrix 26 | self.F = np.eye(dim_x) # state transition matrix 27 | self.H = np.zeros((dim_z, dim_x)) # measurement function 28 | self.R = np.eye(dim_z) # measurement uncertainty 29 | self.z = np.array((dim_z, )) 30 | 31 | self.K = np.zeros((dim_x, dim_z)) # Kalman gain 32 | self.v = np.zeros((dim_z, )) # innovation 33 | self.S = np.zeros((dim_z, dim_z)) # system uncertainty 34 | self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty 35 | 36 | self._I = np.eye(dim_x) # identity matrix 37 | 38 | self.x_prior = self.x.copy() 39 | self.P_prior = self.P.copy() 40 | 41 | self.x_post = self.x.copy() 42 | self.P_post = self.P.copy() 43 | 44 | self.inv = np.linalg.inv 45 | 46 | def predict(self, u=None, B=None, F=None, Q=None): 47 | # Prediction step of KF algorithm 48 | # Prediction is calculated as expected value of the model, conditioned by the measurements 49 | if B is None: 50 | B = self.B 51 | if F is None: 52 | F = self.F 53 | if Q is None: 54 | Q = self.Q 55 | elif np.isscalar(Q): 56 | Q = np.eye(self.dim_x)*Q 57 | 58 | # x_hat = Fx + Bu, it is assumed that noise is 0 mean 59 | 60 | if B is not None and u is not None: 61 | self.x = F @ self.x + B @ self.x 62 | else: 63 | self.x = F @ self.x 64 | 65 | # Need to update the uncertainty, P = FPF' + Q 66 | 67 | self.P = F @ self.P @ F.T + Q 68 | 69 | # save prior 70 | self.x_prior = self.x.copy() 71 | self.P_prior = self.P.copy() 72 | 73 | def update(self, z, R = None, H = None): 74 | # update stage of the filtering process 75 | # final estimate is calculated as : x_estimate = x_estimate_old K*innovation where the innovation is given by: 76 | # innovation = (z_measurement - Hx_prediction), K is kalman gain, it is derived by minimizing the mse 77 | # K = PH'*S^-1, where the S is the innovation uncertainty, S = HPH'+R 78 | 79 | if z is None: 80 | self.z = np.array([None]*self.dim_z) 81 | self.x_post = self.x.copy() 82 | self.P_post = self.P.copy() 83 | self.v = np.zeros((self.dim_z, )) 84 | return 85 | 86 | if R is None: 87 | R = self.R 88 | elif np.isscalar(R): 89 | R = np.eye(self.dim_z)*R 90 | if H is None: 91 | H = self.H 92 | 93 | # innovation calculation: 94 | self.v = z - H @ self.x 95 | PHT = self.P @ H.T 96 | 97 | # now the innovation uncertainty: S = HPH' + R 98 | self.S = H @ PHT + R 99 | self.SI = self.inv(self.S) 100 | 101 | # Now to calculate the Kalman gain 102 | self.K = PHT @ self.SI 103 | 104 | # final prediction can be made as x = x + K*innovation 105 | self.x = self.x + self.K @ self.v 106 | 107 | # P = (I-KH)P(I-KH)' + KRK' a more numerically stable version of P = (I-KH)P 108 | I_KH = self._I - self.K @ H 109 | self.P = I_KH @ self.P @ I_KH.T + self.K @ R @ self.K.T 110 | # save measurement and posterior state 111 | self.z = np.copy(z) 112 | self.x_post = self.x.copy() 113 | self.P_post = self.P.copy() 114 | 115 | 116 | class KalmanInformationFilter(object): 117 | 118 | # actually inverse of the Kalman filter, allowing you to easily denote having 119 | # no information at initialization. 120 | 121 | def __init__(self, dim_x, dim_z, dim_u=0): 122 | 123 | if dim_x < 1: 124 | raise ValueError('dim_x must be 1 or greater') 125 | if dim_z < 1: 126 | raise ValueError('dim_z must be 1 or greater') 127 | if dim_u < 0: 128 | raise ValueError('dim_u must be 0 or greater') 129 | 130 | self.dim_x = dim_x 131 | self.dim_z = dim_z 132 | self.dim_u = dim_u 133 | 134 | self.x = np.zeros((dim_x, )) # state 135 | self.x_info = np.zeros((dim_x, )) # state in information space 136 | self.P_inv = np.eye(dim_x) # uncertainty covariance 137 | self.Q = np.eye(dim_x) # process uncertainty 138 | self.B = 0. # control transition matrix 139 | self.F = 0. # state transition matrix 140 | self._F_inv = 0. # state transition matrix 141 | self.H = np.zeros((dim_z, dim_x)) # Measurement function 142 | self.R_inv = np.eye(dim_z) # state uncertainty 143 | self.z = np.array((dim_z, )) 144 | 145 | self.K = 0. # kalman gain 146 | self.v = np.zeros((dim_z, )) # innovation 147 | self.S = 0. # system uncertainty in measurement space 148 | 149 | # identity matrix. 150 | self._I = np.eye(dim_x) 151 | self.inv = np.linalg.inv 152 | 153 | # save priors and posteriors 154 | self.x_prior = np.copy(self.x) 155 | self.P_inv_prior = np.copy(self.P_inv) 156 | self.x_post = np.copy(self.x) 157 | self.P_inv_post = np.copy(self.P_inv) 158 | 159 | 160 | def update(self, z, R_inv=None, H = None, multiple_sensors = False): 161 | # update stage of the filtering process 162 | # estimation is preformed in information space as: y = y + i, where i is the information contribution 163 | # if there are multiple sensors information is additive. i = H'R_inv z 164 | # Total information content can be updated as Pinv = Pinv + I, I = H'R_invH 165 | 166 | if z is None: 167 | self.z = np.array([None] * self.dim_z) 168 | self.x_post = self.x.copy() 169 | self.P_post = self.P.copy() 170 | self.v = np.zeros((self.dim_z, )) 171 | return 172 | 173 | if R_inv is None: 174 | R_inv = self.R_inv 175 | elif np.isscalar(R_inv): 176 | R_inv = np.eye(self.dim_z) * R_inv 177 | 178 | if H is None: 179 | H = self.H 180 | 181 | H_T = H.T 182 | if multiple_sensors: 183 | # It is assumed that data has been processed externally for None values, reason is that it's much easier 184 | # to do it apriori. 185 | z = z.reshape((*z.shape, 1)) # reshaping into a 3D array of form N x dim_z x 1 for broadcasting purposes 186 | # R_inv will have the shape N x dim_z x dim_z in this case 187 | else: 188 | 189 | R_inv = R_inv.reshape((1, *R_inv.shape)) # reshaping to make the following code work for both cases, now E_inv has 190 | # shape (1, dim_z, dim_z) 191 | z = z.reshape((1, self.dim_z, 1)) 192 | 193 | P_inv = self.P_inv 194 | 195 | ik = H_T @ (R_inv @ z).sum(axis=0) # sensor information contribution 196 | ik = ik.squeeze() 197 | Ik = (H_T @ R_inv @ H).sum(axis=0) # sensor uncertainty contribution 198 | 199 | self.x_info += ik 200 | self.P_inv += Ik 201 | self.x = np.linalg.solve(P_inv, self.x_info) 202 | 203 | # save measurement and posterior state 204 | self.z = np.copy(z) 205 | self.x_post = self.x.copy() 206 | self.P_inv_post = self.P_inv.copy() 207 | 208 | def predict(self, u=None, B=None, F=None, Q=None): 209 | # Prediction step of KF algorithm 210 | # Prediction is calculated as expected value of the model, conditioned by the measurements 211 | if B is None: 212 | B = self.B 213 | if F is None: 214 | F = self.F 215 | if Q is None: 216 | Q = self.Q 217 | elif np.isscalar(Q): 218 | Q = np.eye(self.dim_x) * Q 219 | 220 | # x_hat = Fx + Bu, it is assumed that noise is 0 mean 221 | 222 | if B is not None and u is not None: 223 | self.x = np.dot(F, self.x) + np.dot(B, u) 224 | else: 225 | self.x = np.dot(F, self.x) 226 | 227 | # Need to update the uncertainty, P_inv = (FPF' + Q)^-1 228 | 229 | self.P_inv = self.inv(F @ self.inv(self.P_inv) @ F.T + Q) 230 | 231 | # In information space x_info = Pinv*x 232 | self.x_info = np.dot(self.P_inv, self.x) 233 | 234 | # save prior 235 | self.x_prior = self.x.copy() 236 | self.P_inv_prior = self.P_inv.copy() 237 | -------------------------------------------------------------------------------- /extended_kalman_filter.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class ExtendedKalmanFilter(object): 5 | def __init__(self, dim_x, dim_z, dim_u=0): 6 | if dim_x < 1: 7 | raise ValueError('dim_x must be 1 or greater') 8 | if dim_z < 1: 9 | raise ValueError('dim_z must be 1 or greater') 10 | if dim_u < 0: 11 | raise ValueError('dim_u must be 0 or greater') 12 | 13 | self.dim_u = dim_u 14 | self.dim_z = dim_z 15 | self.dim_x = dim_x 16 | 17 | self.x = np.zeros((dim_x, )) # state 18 | self.P = np.eye(dim_x) # estimation uncertainty 19 | self.f = None # non-linear process model 20 | self.h = None # non-linear measurement model 21 | self.dxf = None # process model Jacobian 22 | self.dxh = None # measurement model Jacobian 23 | self.R = np.eye(dim_z) # measurement noise 24 | self.Q = np.eye(dim_x) # process noise 25 | 26 | self.v = np.zeros((dim_z, )) # innovation 27 | self.z = np.zeros((dim_z, )) # measurement 28 | 29 | self.K = np.zeros((dim_x, dim_z)) # Kalman gain matrix 30 | self.S = np.zeros((dim_z, dim_z)) # innovation covariance 31 | self.SI = np.zeros((dim_z, dim_z)) # innovation covariance inversion 32 | 33 | self._I = np.eye(dim_x) # identity matrix 34 | self.inv = np.linalg.inv 35 | 36 | # these will always be a copy of x,P after predict() is called 37 | self.x_prior = self.x.copy() 38 | self.P_prior = self.P.copy() 39 | 40 | # these will always be a copy of x,P after update() is called 41 | self.x_post = self.x.copy() 42 | self.P_post = self.P.copy() 43 | 44 | def predict(self, u=None, f=None, dxf=None, Q=None): 45 | # Prediction step of KF algorithm 46 | # Prediction is calculated as expected value of the model, conditioned by the measurements 47 | if f is None: 48 | f = self.f 49 | if dxf is None: 50 | dxf = self.dxf 51 | if Q is None: 52 | Q = self.Q 53 | elif np.isscalar(Q): 54 | Q = np.eye(self.dim_x) * Q 55 | 56 | # x_hat = Fx + Bu, it is assumed that noise is 0 mean 57 | 58 | if u is not None: 59 | x = f(self.x, u) 60 | else: 61 | x = f(self.x) 62 | 63 | # Need to update the uncertainty, P = dxf P dxf' + Q, dxf is the jacobian of f, 64 | # jacobian is evaluated at previous value of x 65 | 66 | self.P = dxf(self.x) @ self.P @ dxf(self.x).T + Q 67 | self.x = x 68 | 69 | # save prior 70 | self.x_prior = self.x.copy() 71 | self.P_prior = self.P.copy() 72 | 73 | def update(self, z, R=None, h=None, dxh=None): 74 | # update stage of the filtering process 75 | # final estimate is calculated as : x_estimate = x_estimate_old K*innovation where the innovation is given by: 76 | # innovation = (z_measurement - Hx_prediction), K is kalman gain, it is derived by minimizing the mse 77 | # K = PH'*S^-1, where the S is the innovation uncertainty, S = HPH'+R 78 | 79 | if z is None: 80 | self.z = np.array([None] * self.dim_z) 81 | self.x_post = self.x.copy() 82 | self.P_post = self.P.copy() 83 | self.v = np.zeros((self.dim_z, )) 84 | return 85 | 86 | if R is None: 87 | R = self.R 88 | elif np.isscalar(R): 89 | R = np.eye(self.dim_z) * R 90 | 91 | if h is None: 92 | h = self.h 93 | if dxh is None: 94 | dxh = self.dxh 95 | 96 | # innovation calculation: 97 | self.v = z - h(self.x) 98 | PHT = self.P @ dxh(self.x).T 99 | 100 | # now the innovation uncertainty: S = dxh P dhx' + R dhx is jacobian of h evaluated at predicted value of x 101 | self.S = dxh(self.x) @ PHT + R 102 | self.SI = self.inv(self.S) 103 | 104 | # Now to calculate the Kalman gain 105 | self.K = PHT @ self.SI 106 | 107 | # final prediction can be made as x = x + K*innovation 108 | self.x = self.x + self.K @ self.v 109 | 110 | # P = P - KSK' 111 | 112 | self.P = self.P - self.K @ self.S @ self.K.T 113 | # save measurement and posterior state 114 | self.z = np.copy(z) 115 | self.x_post = self.x.copy() 116 | self.P_post = self.P.copy() 117 | 118 | class ExtendedKalmanInformationFilter(object): 119 | def __init__(self, dim_x, dim_z, dim_u=0): 120 | if dim_x < 1: 121 | raise ValueError('dim_x must be 1 or greater') 122 | if dim_z < 1: 123 | raise ValueError('dim_z must be 1 or greater') 124 | if dim_u < 0: 125 | raise ValueError('dim_u must be 0 or greater') 126 | 127 | self.dim_u = dim_u 128 | self.dim_z = dim_z 129 | self.dim_x = dim_x 130 | 131 | self.x = np.zeros((dim_x, )) # state 132 | self.x_info = np.zeros((dim_x, )) # state in information space 133 | self.P_inv = np.eye(dim_x) # estimation information 134 | self.f = None # non-linear process model 135 | self.h = None # non-linear measurement model 136 | self.dxf = None # process model Jacobian 137 | self.dxh = None # measurement model Jacobian 138 | self.R_inv = np.eye(dim_z) # measurement noise 139 | self.Q = np.eye(dim_x) # process noise 140 | 141 | self.v = np.zeros((dim_z, )) # innovation 142 | self.z = np.zeros((dim_z, )) # measurement 143 | 144 | self.K = np.zeros((dim_x, dim_z)) # Kalman gain matrix 145 | self.S = np.zeros((dim_z, dim_z)) # innovation covariance in information space 146 | 147 | self._I = np.eye(dim_x) # identity matrix 148 | self.inv = np.linalg.inv 149 | 150 | # these will always be a copy of x,P after predict() is called 151 | self.x_prior = self.x.copy() 152 | self.P_inv_prior = self.P_inv.copy() 153 | 154 | # these will always be a copy of x,P after update() is called 155 | self.x_post = self.x.copy() 156 | self.P_inv_post = self.P_inv.copy() 157 | 158 | def predict(self, u=None, f=None, dxf=None, Q=None): 159 | # Prediction step of KF algorithm 160 | # Prediction is calculated as expected value of the model, conditioned by the measurements 161 | if f is None: 162 | f = self.f 163 | if dxf is None: 164 | dxf = self.dxf 165 | if Q is None: 166 | Q = self.Q 167 | elif np.isscalar(Q): 168 | Q = np.eye(self.dim_x) * Q 169 | 170 | # x_hat = Fx + Bu, it is assumed that noise is 0 mean 171 | 172 | if u is not None: 173 | x_info = f(self.x, u) 174 | else: 175 | x_info = f(self.x) 176 | 177 | # Need to update the uncertainty, P = dxf P dxf' + Q, dxf is the jacobian of f, 178 | # jacobian is evaluated at previous value of x 179 | 180 | self.P_inv = self.inv(dxf(self.x) @ self.inv(self.P_inv) @ dxf(self.x).T + Q) 181 | self.x_info = x_info 182 | self.x = np.linalg.solve(self.P_inv, self.x_info) 183 | 184 | # save prior 185 | self.x_prior = self.x.copy() 186 | self.P_inv_prior = self.P_inv.copy() 187 | 188 | def update(self, z, R_inv=None, h=None, dxh=None, multiple_sensors=False): 189 | # update stage of the filtering process 190 | # final estimate is calculated as : x_estimate = x_estimate_old K*innovation where the innovation is given by: 191 | # innovation = (z_measurement - Hx_prediction), K is kalman gain, it is derived by minimizing the mse 192 | # K = PH'*S^-1, where the S is the innovation uncertainty, S = HPH'+R 193 | 194 | if z is None: 195 | self.z = np.array([[None] * self.dim_z]).T 196 | self.x_post = self.x.copy() 197 | self.P_inv_post = self.P_inv.copy() 198 | self.v = np.zeros((self.dim_z, )) 199 | return 200 | 201 | if R_inv is None: 202 | R_inv = self.R_inv 203 | elif np.isscalar(R_inv): 204 | R_inv = np.eye(self.dim_z) * R_inv 205 | 206 | if h is None: 207 | h = self.h 208 | if dxh is None: 209 | dxh = self.dxh 210 | 211 | if multiple_sensors: 212 | number_of_sensors = z.shape[0] # It is assumed that measurements are stacked in rows 213 | self.v = np.zeros((number_of_sensors, self.dim_z)) 214 | jacobian_h = np.zeros((number_of_sensors, self.dim_z, self.dim_x)) 215 | bias = np.zeros((number_of_sensors, self.dim_z)) 216 | for idx in range(number_of_sensors): 217 | h_cur = h[idx] 218 | dxh_cur = dxh[idx] 219 | self.v[idx, :] = z[idx, :] - h_cur(self.x) 220 | jacobian_h[idx, :, :] = dxh_cur(self.x) 221 | bias[idx, :] = jacobian_h[idx, :, :] @ self.x 222 | self.v = self.v.reshape((*self.v.shape, 1)) 223 | bias = bias.reshape((*bias.shape, 1)) 224 | else: 225 | self.v = np.zeros((1, self.dim_z)) 226 | jacobian_h = np.zeros((1, self.dim_x, self.dim_x)) 227 | bias = np.zeros((1, self.dim_z)) 228 | self.v[0, :] = z - h(self.x) 229 | jacobian_h[0, :, :] = dxh(self.x) 230 | bias[0, :] = jacobian_h[0, :, :] @ self.x 231 | self.v = self.v.reshape((*self.v.shape, 1)) 232 | bias = bias.reshape((*bias.shape, 1)) 233 | R_inv = R_inv.reshape((1, *R_inv.shape)) 234 | 235 | jacobian_h_T = jacobian_h.transpose(0, 2, 1) # Transposing only the matrices 236 | ik = (jacobian_h_T @ R_inv @ (self.v + bias)).sum(axis=0) # sensor information contribution 237 | ik = ik.squeeze() 238 | Ik = (jacobian_h_T @ R_inv @jacobian_h).sum(axis=0) # sensor uncertainty contribution 239 | 240 | # final prediction can be made as x = x + sum(ik) 241 | self.x_info += ik 242 | 243 | # P = P + sum(Ik) 244 | self.P_inv += Ik 245 | 246 | # save measurement and posterior state 247 | self.z = np.copy(z) 248 | self.x = np.linalg.solve(self.P_inv, self.x_info) 249 | 250 | self.x_post = self.x.copy() 251 | self.P_inv_post = self.P_inv.copy() -------------------------------------------------------------------------------- /filter_comparison.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | 3 | import extended_kalman_filter as ekf 4 | import trajectory_generator 5 | import unscented_kalman_filter as ukf 6 | import particle_filter as pf 7 | import kalman_filter as kf 8 | import models 9 | import jax 10 | import datetime 11 | jax.config.update('jax_platform_name', 'cpu') 12 | import numpy as np 13 | import os 14 | import time 15 | 16 | 17 | def plot_results(t, trajectory, ekf_est, ukf_est, pf_est, save_plot=False): 18 | # Turn interactive plotting off 19 | plt.ioff() 20 | num_states = trajectory.shape[1] 21 | symb = ['sx [m]', 'vx [ms^-1]', 'ax [ms^-2] ', 'sy [m]', 'vy [ms^-1]', 'ay [ms^-2]', r'theta [rad]'] 22 | file_symb = ['sx', 'vx', 'ax ', 'sy', 'vy', 'ay', r'theta'] 23 | # Generate a timestamp for unique filenames 24 | timestamp = datetime.datetime.now().strftime("%Y%m%d%H%M%S") 25 | 26 | for state_idx in range(num_states): 27 | if state_idx == 0 or state_idx == 3 or state_idx == 6: 28 | plt.figure(figsize=(8, 4)) 29 | plt.plot(t, trajectory[:, state_idx], 'k-', label='trajektorija') 30 | plt.plot(t, ekf_est[:, state_idx], 'b--', marker='o', markevery=50, label='prošireni KF') 31 | plt.grid(True) 32 | plt.legend() 33 | plt.xlabel('t [s]') 34 | plt.ylabel(symb[state_idx]) 35 | plt.tight_layout() 36 | 37 | plt.plot(t, ukf_est[:, state_idx], 'g-.', marker='s', markevery=50, label='neosetljiv KF') 38 | plt.grid(True) 39 | plt.legend() 40 | plt.xlabel('t [s]') 41 | plt.ylabel(symb[state_idx]) 42 | plt.tight_layout() 43 | 44 | plt.plot(t, pf_est[:, state_idx], 'r:', marker='^', markevery=50, label='čestični filtar') 45 | plt.grid(True) 46 | plt.legend() 47 | plt.xlabel('t [s]') 48 | plt.ylabel(symb[state_idx]) 49 | plt.tight_layout() 50 | if save_plot: 51 | nm = os.path.join('figures', f'all_{file_symb[state_idx]}.png') 52 | plt.savefig(nm, dpi=600) 53 | 54 | if state_idx == 1 or state_idx == 2: 55 | th = trajectory[:, 6] 56 | plt.figure(figsize=(8, 4)) 57 | plt.plot(t, np.cos(th)*trajectory[:, state_idx] - np.sin(th)*trajectory[:, state_idx+3], 'k-', label='trajektorija') 58 | plt.plot(t, np.cos(th)*ekf_est[:, state_idx] - np.sin(th)*ekf_est[:, state_idx+3], 'b--', marker='o', markevery=50, label='prošireni KF') 59 | plt.grid(True) 60 | plt.legend() 61 | plt.xlabel('t [s]') 62 | plt.ylabel(symb[state_idx]) 63 | plt.tight_layout() 64 | 65 | plt.plot(t, np.cos(th)*ukf_est[:, state_idx] - np.sin(th)*ukf_est[:, state_idx+3], 'g-.', marker='s', markevery=50, label='neosetljiv KF') 66 | plt.grid(True) 67 | plt.legend() 68 | plt.xlabel('t [s]') 69 | plt.ylabel(symb[state_idx]) 70 | plt.tight_layout() 71 | 72 | plt.plot(t, np.cos(th)*pf_est[:, state_idx] - np.sin(th)*pf_est[:, state_idx+3], 'r:', marker='^', markevery=50, label='čestični filtar') 73 | plt.grid(True) 74 | plt.legend() 75 | plt.xlabel('t [s]') 76 | plt.ylabel(symb[state_idx]) 77 | plt.tight_layout() 78 | if save_plot: 79 | nm = os.path.join('figures', f'all_{file_symb[state_idx]}.png') 80 | plt.savefig(nm, dpi=600) 81 | 82 | if state_idx == 4 or state_idx == 5: 83 | th = trajectory[:, 6] 84 | plt.figure(figsize=(8, 4)) 85 | plt.plot(t, np.cos(th) * trajectory[:, state_idx] + np.sin(th) * trajectory[:, state_idx - 3], 'k-', 86 | label='trajektorija') 87 | plt.plot(t, np.cos(th) * ekf_est[:, state_idx] + np.sin(th) * ekf_est[:, state_idx - 3], 'b--', marker='o', 88 | markevery=50, label='prošireni KF') 89 | plt.grid(True) 90 | plt.legend() 91 | plt.xlabel('t [s]') 92 | plt.ylabel(symb[state_idx]) 93 | plt.tight_layout() 94 | 95 | plt.plot(t, np.cos(th) * ukf_est[:, state_idx] + np.sin(th) * ukf_est[:, state_idx - 3], 96 | 'g-.', marker = 's', markevery = 50, label = 'neosetljiv KF') 97 | plt.grid(True) 98 | plt.legend() 99 | plt.xlabel('t [s]') 100 | plt.ylabel(symb[state_idx]) 101 | plt.tight_layout() 102 | 103 | 104 | plt.plot(t, np.cos(th) * pf_est[:, state_idx] + np.sin(th) * pf_est[:, state_idx - 3], 'r:', marker='^', 105 | markevery=50, label='čestični filtar') 106 | plt.grid(True) 107 | plt.legend() 108 | plt.xlabel('t [s]') 109 | plt.ylabel(symb[state_idx]) 110 | plt.tight_layout() 111 | if save_plot: 112 | nm = os.path.join('figures', f'all_{file_symb[state_idx]}.png') 113 | plt.savefig(nm, dpi=600) 114 | 115 | 116 | 117 | plt.close('all') 118 | 119 | def param_init(x0=np.zeros((7, ))): 120 | dt = 0.05 121 | N = 101 122 | t_end = (N-1)*dt 123 | t = np.linspace(0, t_end, N) 124 | 125 | trajectory = trajectory_generator.generate_trajectory(t) 126 | z = trajectory_generator.measure_full_trajectory(trajectory) 127 | x0 = x0 128 | 129 | R = (np.diag([.1, 0.1, .1, 0.1, 0.1])**2) 130 | Q = np.array([[0, 0, 0, 0, 0, 0, 0], 131 | [0, 0, 0.1 ** 2 * dt, 0, 0, 0, 0], 132 | [0, 0.1 ** 2 * dt, 0.1 ** 2, 0, 0, 0, 0], 133 | [0, 0, 0, 0, 0, 0, 0], 134 | [0, 0, 0, 0, 0, 0.1 ** 2 * dt, 0], 135 | [0, 0, 0, 0, 0.1 ** 2 * dt, 0.1 ** 2, 0], 136 | [0, 0, 0, 0, 0, 0, 0.1 ** 2]] 137 | ) 138 | 139 | P = np.eye(7) 140 | 141 | return t, trajectory, z, x0, R, Q, P 142 | 143 | def ekf_init(x0, R, Q, P): 144 | f = lambda x: models.proces_model(x) 145 | h = lambda z: models.measurement_model(z) 146 | Jf = jax.jacobian(f) 147 | Jh = jax.jacobian(h) 148 | 149 | ekf_1 = ekf.ExtendedKalmanFilter(dim_x=7, dim_z=5) 150 | ekf_1.Q = Q 151 | ekf_1.dxh = Jh 152 | ekf_1.R = R 153 | ekf_1.dxf = Jf 154 | ekf_1.f = models.proces_model 155 | ekf_1.h = models.measurement_model 156 | ekf_1.P = P 157 | ekf_1.x = x0 158 | 159 | return ekf_1 160 | 161 | def ukf_init(x0, R, Q, P): 162 | ukf_1 = ukf.UnscentedKalmanFilter(dim_x=7, dim_z=5) 163 | ukf_1.h = models.measurement_model_ukf 164 | ukf_1.f = models.proces_model_ukf 165 | ukf_1.P = P 166 | ukf_1.x = x0 167 | ukf_1.R = R 168 | ukf_1.Q = Q 169 | 170 | return ukf_1 171 | 172 | def pf_init(z, t): 173 | sensor_data = [z[idx, :] for idx in range(len(t))] 174 | pf_1 = pf.particle_filter(sensor_data=sensor_data, N_particles=20000, algorithm="dynamic_resampling") 175 | 176 | return pf_1 177 | 178 | 179 | def run_comparison(x0, plot=False, save_plot=False): 180 | t, trajectory, z, x0, R, Q, P = param_init(x0) 181 | dt = t[1] - t[0] 182 | 183 | ekf = ekf_init(x0, R, Q, P) 184 | ekf_est = np.zeros(trajectory.shape) 185 | 186 | start = time.time() 187 | for idx, i in enumerate(t): 188 | ekf.update(z=z[idx, :]) 189 | ekf_est[idx, :] = np.array(ekf.x) 190 | ekf.predict() 191 | t_ekf = time.time()-start 192 | ukf = ukf_init(x0, R, Q, P) 193 | ukf_est = np.zeros(trajectory.shape) 194 | 195 | start = time.time() 196 | for idx, i in enumerate(t): 197 | ukf.update(z=z[idx, :]) 198 | ukf_est[idx, :] = np.array(ukf.x) 199 | ukf.prediction() 200 | t_ukf = time.time() - start 201 | 202 | pf = pf_init(z, t) 203 | pf_est = np.zeros(trajectory.shape) 204 | start = time.time() 205 | for idx, i in enumerate(t): 206 | pf_est[idx, :] = pf.particle_filtering() 207 | t_pf = time.time()-start 208 | 209 | if plot: 210 | plot_results(t, trajectory, ekf_est, ukf_est, pf_est, save_plot) 211 | 212 | rms_ekf = np.sqrt(np.mean((ekf_est - trajectory) ** 2, axis=0)) 213 | rms_ukf = np.sqrt(np.mean((ukf_est - trajectory) ** 2, axis=0)) 214 | rms_pf = np.sqrt(np.mean((pf_est - trajectory) ** 2, axis=0)) 215 | 216 | kf_est = np.zeros((trajectory.shape[0], 4)) 217 | rms_calc = np.zeros((trajectory.shape[0], 4)) 218 | kf_est[:, 0] = z[:, 0] 219 | kf_est[:, 2] = z[:, 1] 220 | kf_est[:, 1] = np.cos(trajectory[:, 6]) * z[:, 1] - np.sin(trajectory[:, 6]) * z[:, 3] 221 | kf_est[:, 3] = np.cos(trajectory[:, 6]) * z[:, 3] + np.sin(trajectory[:, 6]) * z[:, 1] 222 | 223 | rms_calc[:, 0] = trajectory[:, 0] 224 | rms_calc[:, 1] = np.cos(trajectory[:, 6]) * trajectory[:, 1] - np.sin(trajectory[:, 6]) * trajectory[:, 4] 225 | rms_calc[:, 2] = trajectory[:, 3] 226 | rms_calc[:, 3] = np.cos(trajectory[:, 6]) * trajectory[:, 4] + np.sin(trajectory[:, 6]) * trajectory[:, 1] 227 | 228 | rms = np.sqrt(np.mean((kf_est - rms_calc) ** 2, axis=0)) 229 | 230 | return rms_ekf, rms_ukf, rms_pf, t_ekf, t_ukf, t_pf, rms 231 | 232 | 233 | def param_init_kalman(x0): 234 | dt = 0.05 235 | N = 101 236 | t_end = (N - 1) * dt 237 | t = np.linspace(0, t_end, N) 238 | 239 | trajectory = trajectory_generator.generate_trajectory(t) 240 | z = trajectory_generator.measure_full_trajectory_kalman(trajectory) 241 | 242 | x0 = x0 243 | 244 | R = np.diag([.1, .1])**2 245 | Q = np.array([[0, 0.1**2*dt, 0, 0], 246 | [0.1**2*dt, 0.1**2, 0, 0], 247 | [0, 0, 0, 0.1**2*dt], 248 | [0, 0, 0.1**2*dt, 0.1**2]]) 249 | P = np.eye(4) 250 | 251 | return t, trajectory, z, x0, R, Q, P 252 | 253 | def kf_init(x0, R, Q, P): 254 | dt = 0.05 255 | kf_1 = kf.KalmanFilter(4, 2) 256 | kf_1.P = P 257 | kf_1.R = R 258 | kf_1.Q = Q 259 | kf_1.x = x0 260 | kf_1.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]]) 261 | kf_1.F = np.array([[1, dt, 0, 0], 262 | [0, 1, 0, 0], 263 | [0, 0, 1, dt], 264 | [0, 0, 0, 1]]) 265 | return kf_1 266 | 267 | 268 | def plot_kalman(t, trajectory, kf_est, save_plot=False): 269 | # Turn interactive plotting off 270 | plt.ioff() 271 | num_states = kf_est.shape[1] 272 | symb = ['sx [m]', 'vx [ms^-1]', 'sy [m]', 'vy [ms^-1]'] 273 | file_symb = ['sx', 'vx', 'sy', 'vy'] 274 | # Generate a timestamp for unique filenames 275 | timestamp = datetime.datetime.now().strftime("%Y%m%d%H%M%S") 276 | 277 | for state_idx in range(num_states): 278 | if state_idx == 0: 279 | plt.figure(figsize=(8, 4)) 280 | plt.plot(t, trajectory[:, state_idx], 'k-', label='trajektorija') 281 | plt.plot(t, kf_est[:, state_idx], 'b--', marker='o', markevery=50, label='KF') 282 | plt.grid(True) 283 | plt.legend() 284 | plt.xlabel('t [s]') 285 | plt.ylabel(symb[state_idx]) 286 | plt.tight_layout() 287 | if state_idx==1: 288 | plt.figure(figsize=(8, 4)) 289 | v = np.cos(trajectory[:, 6])*trajectory[:, 1] - np.sin(trajectory[:, 6])*trajectory[:, 4] 290 | plt.plot(t, v, 'k-', label='trajektorija') 291 | plt.plot(t, kf_est[:, state_idx], 'b--', marker='o', markevery=50, label='KF') 292 | plt.grid(True) 293 | plt.legend() 294 | plt.xlabel('t [s]') 295 | plt.ylabel(symb[state_idx]) 296 | plt.tight_layout() 297 | if state_idx==3: 298 | plt.figure(figsize=(8, 4)) 299 | v = np.cos(trajectory[:, 6])*trajectory[:, 4] + np.sin(trajectory[:, 6])*trajectory[:, 1] 300 | plt.plot(t, v, 'k-', label='trajektorija') 301 | plt.plot(t, kf_est[:, state_idx], 'b--', marker='o', markevery=50, label='KF') 302 | plt.grid(True) 303 | plt.legend() 304 | plt.xlabel('t [s]') 305 | plt.ylabel(symb[state_idx]) 306 | plt.tight_layout() 307 | if state_idx == 2: 308 | plt.figure(figsize=(8, 4)) 309 | plt.plot(t, trajectory[:, state_idx+1], 'k-', label='trajektorija') 310 | plt.plot(t, kf_est[:, state_idx], 'b--', marker='o', markevery=50, label='KF') 311 | plt.grid(True) 312 | plt.legend() 313 | plt.xlabel('t [s]') 314 | plt.ylabel(symb[state_idx]) 315 | plt.tight_layout() 316 | 317 | 318 | if save_plot: 319 | nm = os.path.join('figures', f'kf_{file_symb[state_idx]}.png') 320 | plt.savefig(nm, dpi=600) 321 | 322 | 323 | 324 | def only_kalman(x0, plot=False, save_plot=False): 325 | t, trajectory, z, x0, R, Q, P = param_init_kalman(x0) 326 | 327 | kf = kf_init(x0, R, Q, P) 328 | kf_est = np.zeros((trajectory.shape[0], 4)) 329 | 330 | start_time = time.time() 331 | for idx, i in enumerate(t): 332 | kf.update(z=z[idx, :]) 333 | kf_est[idx, :] = np.array(kf.x) 334 | kf.predict() 335 | runtime = time.time()-start_time 336 | if plot: 337 | plot_kalman(t, trajectory, kf_est, save_plot) 338 | 339 | rms_calc = np.zeros((trajectory.shape[0], 4)) 340 | rms_calc[:, 0] = trajectory[:, 0] 341 | rms_calc[:, 1] = np.cos(trajectory[:, 6])*trajectory[:, 1] - np.sin(trajectory[:, 6])*trajectory[:, 4] 342 | rms_calc[:, 2] = trajectory[:, 3] 343 | rms_calc[:, 3] = np.cos(trajectory[:, 6])*trajectory[:, 4] + np.sin(trajectory[:, 6])*trajectory[:, 1] 344 | 345 | rms = np.sqrt(np.mean((kf_est - rms_calc)**2, axis=0)) 346 | 347 | kf_est[:, 0] = z[:, 0] 348 | kf_est[:, 2] = z[:, 1] 349 | 350 | rms_true = np.sqrt(np.mean((kf_est - rms_calc)**2, axis=0)) 351 | 352 | return rms, rms_true, runtime 353 | -------------------------------------------------------------------------------- /unscented_kalman_filter.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | import numpy as np 4 | import scipy 5 | 6 | 7 | def nearPD(A, epsilon=1e-8, zero=0): 8 | r"""This function clips eigen values of an uncertainty matrix `A`, clipping threshold is specified by `epsilon`. 9 | 10 | Since eigen value represent variances it makes no physical sense for them to be negative, but that can happen 11 | after the update step in UKF algorthm, to fix this issue we are clipping the eigen values. 12 | 13 | Parameters 14 | ---------- 15 | A: :class:`~numpy:numpy.ndarray` 16 | square ``(n, n) ndarray`` representing uncertainty matrix 17 | 18 | epsilon: :py:class:`float`, optional 19 | lowest possible value for eigen values (default is 0, it is recommended to set it to small positive value) 20 | 21 | Returns 22 | ------- 23 | A: class:`~numpy:numpy.ndarray` 24 | reconstructed uncertainty matrix with clipped eigen values. 25 | 26 | Notes 27 | ----- 28 | 29 | An arbitrary square matrix :math:`M` can be decomposed into a form :math:`MV = SV', where :math:'S' is a diagonal 30 | matrix containing eigen values :math:`\lambda` and :math:`V` is a matrix of eigenvectors. One of the properties 31 | that the eigenvector matrix satisfies is :math:`VV^T = I`, hence we can reconstruct the original matrix in the 32 | following way 33 | 34 | .. math:: 35 | M = SVV^T 36 | 37 | TODO : explore a more efficient approach, A v = l v -> (A + cI)v = l v + c v = (l + c) v, i.e, easier to just add 38 | TODO : 'unit' values to appropriate places, more efficient than direct reconstruction! Issue: this approach doesn't 39 | TODO : guarantee real values of vector v, but theory implies that if A is symmetric then v and l are real 40 | 41 | 42 | """ 43 | C = (A + A.T)/2 44 | 45 | eigval, eigvec = scipy.linalg.eigh(C) 46 | 47 | eigval = np.real(eigval) 48 | eigvec = np.real(eigvec) 49 | 50 | eigval[eigval < 0] = epsilon 51 | eigval[eigval < zero] = epsilon 52 | eigval[eigval > 4] = 4 53 | 54 | diag_matrix = np.diag(eigval) 55 | 56 | R = diag_matrix@eigvec@eigvec.T 57 | R = (R+R.T)/2 58 | 59 | return R 60 | 61 | class UnscentedKalmanFilter(object): 62 | r"""Class used for unscented kalman filter, multiple instances are possible. 63 | 64 | Unscented kalman filter is a filtering algorithm that provides optimal estimates of the states in 65 | MSE (mean square error) sense, details about the theory can be found in Notes section of this page. 66 | 67 | Attributes 68 | ---------- 69 | dim_x : :py:class:`int` 70 | dimensionality of state space, must be 1 or greater 71 | 72 | dim_z : :py:class:`int` 73 | dimensionality of measurement space, must be 1 or greater 74 | 75 | dim_u : :py:class:`int` 76 | dimensionality of control space, must be 0 or greater 77 | 78 | x : :class:`~numpy:numpy.ndarray` 79 | state space array with following shape ``(dim_x, ) ndarray`` 80 | 81 | P : :class:`~numpy:numpy.ndarray` 82 | state space uncertainty matrix with following shape ``(dim_x, dim_x) ndarray`` 83 | 84 | P_chol : :class:`~numpy:numpy.ndarray` 85 | Cholesky decomposition of uncertainty matrix 'P' with following shape ``(dim_x, dim_x) ndarray`` 86 | 87 | x_prior : :class:`~numpy:numpy.ndarray` 88 | hard copy of the state 'x' after the predict step, shape ``(dim_x, ) ndarray`` 89 | 90 | P_prior : :class:`~numpy:numpy.ndarray` 91 | hard copy of the matrix 'P' after the predict step, shape ``(dim_x, dim_x) ndarray`` 92 | 93 | x_post : :class:`~numpy:numpy.ndarray` 94 | hard copy of the state 'x' after the update step, shape ``(dim_x, ) ndarray`` 95 | 96 | P_post : :class:`~numpy:numpy.ndarray` 97 | hard copy of the matrix 'P' after the update step, shape ``(dim_x, dim_x) ndarray`` 98 | 99 | X_norm : :class:`~numpy:numpy.ndarray`, optional 100 | matrix used for normalisation of X sigma points, shape ``(dim_x, dim_x) ndarray``. 101 | 102 | X_renorm : :class:`~numpy:numpy.ndarray`, optional 103 | matrix used for renormalization of X sigma points, shape ``(dim_x, dim_x) ndarray``. 104 | 105 | Z_norm : :class:`~numpy:numpy.ndarray`, optional 106 | matrix used for normalisation of Z sigma points, shape ``(dim_x, dim_x) ndarray``. 107 | 108 | Z_renorm : :class:`~numpy:numpy.ndarray`, optional 109 | matrix used for renormalization of Z sigma points, shape ``(dim_x, dim_x) ndarray``. 110 | 111 | divergence_uncertainty : :class:`~numpy:numpy.ndarray` 112 | matrix that is used as the value of 'P_chol' in the case that 113 | :func:`~unscented_kalman_filter.UnscentedKalmanFilter._safe_cholesky` fails to find the Cholesky decomposition 114 | 115 | Q : :class:`~numpy:numpy.ndarray` 116 | matrix representing uncertainty in estimator model with following shape ``(dim_x, dim_x) ndarray`` 117 | 118 | f : function 119 | User specified function that is used to predict the future state of the model 120 | 121 | z : :class:`~numpy:numpy.ndarray` 122 | measurement space array with following shape ``(dim_z, ) ndarray`` 123 | 124 | R : :class:`~numpy:numpy.ndarray` 125 | matrix representing uncertainty in measurements ``(dim_z, dim_z) ndarray`` 126 | 127 | K : :class:`~numpy:numpy.ndarray` 128 | Kalman gain matrix with following shape ``(dim_x, dim_z) ndarray`` 129 | 130 | v : :class:`~numpy:numpy.ndarray` 131 | innovation array with following shape ``(dim_z, ) ndarray``, basically error between predicted measurement and 132 | true measurement, it can be used to verify the filter - if the filter is properly tuned `v` should have 133 | statistical properties of white noise. 134 | 135 | S : :class:`~numpy:numpy.ndarray` 136 | matrix representing uncertainty in the innovation array, shape ``(dim_z, dim_z) ndarray`` 137 | 138 | SI : :class:`~numpy:numpy.ndarray` 139 | inverse of `S`, calculated once to reduce the number of inversions 140 | 141 | C : :class:`~numpy:numpy.ndarray` 142 | matrix representing cross-correlation between state and measurement space, shape ``(dim_x, dim_z) ndarray`` 143 | 144 | Wp : :class:`~numpy:numpy.ndarray` 145 | weights for sigma points mean estimation, shape ``(2 * dim_x + 1, 1) ndarray``. This should not be specified 146 | by the user. 147 | 148 | Wc : :class:`~numpy:numpy.ndarray` 149 | weights for sigma points covariance estimation, shape ``(2 * dim_x + 1, 1) ndarray``. This should not be 150 | specified by the user. 151 | 152 | eps_Q: :class:`~numpy:numpy.ndarray` 153 | Matrix filled with small values used to guarantee that all eigen values of P are > 0, 154 | this can be specified by the user, shape ``(dim_x, dim_x) ndarray`` 155 | 156 | eps_R: :class:`~numpy:numpy.ndarray` 157 | Matrix filled with small values used to guarantee that all eigen values of S are > 0, 158 | this can be specified by the user, shape ``(dim_z, dim_z) ndarray`` 159 | 160 | Methods 161 | ------- 162 | prediction(u=None, f=None, Q=None, **kwargsf): 163 | Starts the prediction step of filtering algorithm, after the prediction step is done results are stored in 164 | `x_prior` and `P_prior` 165 | update(self, z, h=None, R=None, **kwargsh): 166 | Starts the update step of filtering algorithm, after the prediction step is done results are stored in 167 | `x_post` and `P_post` 168 | 169 | Notes 170 | ----- 171 | 172 | Private attributes `_alpha`, `_beta` and `_k` are used to determine the spread of sigma points. Spread can be 173 | decreased by decreasing `_alpha` or `_k`. Spread is very sensitive to values of `_alpha`, fine-tuning can be 174 | achieved by changing the value of `_k`. After the values `_alpha`, `_beta` and `_k` have been changed 175 | (that can be achieved by using setter functions, i.e., `ukf.alpha = value`) weights `Wc` and `Wp` get updated 176 | automatically. It is recommended to not decrease `_alpha` more than 1e-4 because numerical issued arise when 177 | small values of `_alpha` are used. 178 | 179 | In usual use case it is recommended to use only `x_post`, `x_prior`, `P_post` and `P_prior` as the outputs of 180 | the filter, because only these variables are hard copies, other variables might get change via the reference if 181 | a hard copy is not made when they are extracted from unscented kalman filter. 182 | 183 | `dt_estimation`, `timing_estimation` are optional attributes that can be used externally for the purpose of 184 | synchronisation, internally they have no use. `dt_kalman_gain` and `timing_kalman_gain` can be used to 185 | change the rate at which is kalman gain updated. 186 | 187 | If `X_norm` and `Z_norm` normalisation matrices are used it is assumed that internally the states and uncertainty 188 | matrices are normalised - only when we are using the estimation/measurement model we are re-normalising the sigma 189 | points. 190 | 191 | """ 192 | 193 | def __init__(self, dim_x=1, dim_z=1, dim_u=0): 194 | r""" Constructor for the :class:`UnscentedKalmanFilter` 195 | 196 | Parameters 197 | ---------- 198 | dim_x: :py:class:`int` 199 | dimension of the state space, must be 1 or greater 200 | dim_z: :py:class:`int` 201 | dimension of the measurement space, must be 1 or greater 202 | dim_u: :py:class:`int` 203 | dimension of the control space, must be 0 or greater 204 | 205 | Notes 206 | ----- 207 | Other attributes of the :class:`UnscentedKalmanFilter` are set to default values, before any use 208 | user must specify those attributes by accessing the object fields. 209 | 210 | """ 211 | 212 | if dim_x < 1: 213 | raise ValueError('dim_x must be 1 or greater') 214 | if dim_z < 1: 215 | raise ValueError('dim_z must be 1 or greater') 216 | if dim_u < 0: 217 | raise ValueError('dim_u must be 0 or greater') 218 | 219 | 220 | 221 | # values used for the purposes of synchronisation, estimation gets performed only if t - timing > dt, otherwise 222 | # appropriate prediction value gets returned, timing is incremented by dt each time the estimation is pref. 223 | self.dt_estimation = 1 224 | self.timing_estimation = 0 225 | 226 | self.dt_kalman_gain = 1 227 | self.timing_kalman_gain = 0 228 | 229 | # Names of the variables that can be specified by the user, it makes it easier to plot out the data later on 230 | # names should be formatted as a dictionary where the key is position in the state array, and value is the name 231 | # of the variable, currently unused, might use them in the future for plotting purposes 232 | self.state_names = None 233 | self.measurement_names = None 234 | self.control_names = None 235 | 236 | # dimensionality of state space, measurement space and control space 237 | self.dim_x = dim_x 238 | self.dim_z = dim_z 239 | self.dim_u = dim_u 240 | 241 | # Parameters for sigma point generation : 242 | self._alpha = 1e-2 243 | self._beta = 2 244 | self._k = 0 245 | 246 | self.x = np.zeros((dim_x,)) # state 247 | self.P = np.eye(dim_x) # estimation uncertainty covariance 248 | self.P_chol = np.eye(dim_x) # Cholesky decomposition of P 249 | self.Q = np.eye(dim_x) # process uncertainty 250 | self.f = None # non-linear process function 251 | 252 | self.z = np.zeros((dim_z,)) # measurement 253 | self.h = None # non-linear measurement function 254 | self.R = np.eye(dim_z) # measurement uncertainty 255 | 256 | self.K = np.zeros((dim_x, dim_z)) # Kalman gain matrix 257 | self.v = np.zeros((dim_z,)) # innovation vector 258 | self.S = np.zeros((dim_z, dim_z)) # innovation covariance 259 | self.SI = np.zeros((dim_z, dim_z)) # innovation covariance inverse 260 | self.C = np.zeros((dim_x, dim_z)) # measurement and state cross-covariance 261 | 262 | self._I = np.eye(dim_x) # identity matrix 263 | 264 | # values of x after prediction step 265 | self.x_prior = self.x.copy() 266 | self.P_prior = self.P.copy() 267 | 268 | self.X_trans = np.zeros((2 * dim_x + 1, dim_x)) # Transformed sigma points 269 | self.Wp = np.ones((2 * dim_x + 1, 1)) # weights for sigma points for state estimation 270 | self.Wc = np.ones((2 * dim_x + 1, 1)) # weights for sigma points for covariance estimation 271 | self._update_weights() 272 | 273 | # values of x after update step 274 | self.x_post = self.x.copy() 275 | self.P_post = self.P.copy() 276 | 277 | self.inv = np.linalg.inv # storing the linalg.inv function for easier access 278 | self.cholesky = np.linalg.cholesky # storing the linalg.cholesky function for easier access 279 | self.eps_Q = np.eye(dim_x) * 0 # Can be changed, used to guarantee that all eigen values of P are > 0 280 | self.eps_R = np.eye(dim_z) * 0 # Can be changed, used to guarantee that all eigen values of P are > 0 281 | self.divergence_uncertainty = 1 # Can be changed, represents our uncertainty in estimation after algorithm 282 | # diverges 283 | 284 | @property # getter property for alpha 285 | def alpha(self): 286 | """Getter and setter methods for `_alpha` are defined using the 287 | property tag, value of `_alpha` can be got/set using the regular syntax for 288 | attribute access 289 | """ 290 | return self._alpha 291 | 292 | @property # getter property for alpha 293 | def beta(self): 294 | """Getter and setter method for _beta are defined using the 295 | property tag, value of `_beta` can be got/set using the regular syntax for 296 | attribute access 297 | """ 298 | return self._beta 299 | 300 | @property # getter property for alpha 301 | def k(self): 302 | """Getter and setter method for _k are defined using the 303 | property tag, value of `_k` can be got/set using the regular syntax for 304 | attribute access 305 | """ 306 | return self._k 307 | 308 | @alpha.setter 309 | def alpha(self, value): 310 | # alpha parameter determines the spread of sigma points 311 | # small values of alpha increase the accuracy of estimation 312 | # but that leads to larger value of weights and more numerical errors 313 | # alpha should never be larger than 1, that is a theoretical limit. 314 | if not np.isscalar(value): 315 | raise ValueError('alpha must be a scalar') 316 | if value < 1e-4: 317 | raise ValueError('alpha must be greater than 1e-4') 318 | if value > 1: 319 | raise ValueError('alpha must be less than 1') 320 | self._alpha = value 321 | 322 | # now we update the value of weights, they depend on alpha, beta and k 323 | self._update_weights() 324 | 325 | @k.setter 326 | def k(self, value): 327 | # parameter that also impacts the spread of sigma points, it's recommended tuning k over alpha 328 | # since weight values are less sensitive to k 329 | # increasing k increases the spread and decreasing reduces it 330 | # only real limit is that k must be greater than -dim_x, because at some point we calculate the value 331 | # of expression sqrt(dim_x + k) 332 | 333 | if not np.isscalar(value): 334 | raise ValueError('k must be a scalar') 335 | if value <= -self.dim_x: 336 | raise ValueError('k must greater than -x') 337 | self._k = value 338 | 339 | # now we update the value of weights, they depend on alpha, beta and k 340 | self._update_weights() 341 | 342 | @beta.setter 343 | def beta(self, value): 344 | # this parameter impacts the estimation of distribution skewness, for Gaussian distributions 345 | # beta = 2 is an optimal choice, deviating from this choice can increase the accuracy if we 346 | # know in which direction is the transformed distribution skewed 347 | 348 | if not np.isscalar(value): 349 | raise ValueError('beta must be a scalar') 350 | self._beta = value 351 | 352 | # now we update the value of weights, they depend on alpha, beta and k 353 | self._update_weights() 354 | 355 | def _update_weights(self): 356 | r""" This function updates the value of weights Wp and Wc. 357 | 358 | Notes 359 | ----- 360 | Calculating the weight values using the theoretical expressions from Sigma_Point_Kalman_filters(UKF+PF) 361 | paper in NoNLin Filtering section of the GDrive, it is not recommended to change these expressions 362 | unless there is some theoretical justification 363 | """ 364 | 365 | # Reading the parameter values for the purpose of having more readable mathematical expressions 366 | alpha = self.alpha 367 | k = self.k 368 | n = self.dim_x 369 | beta = self.beta 370 | lamb = (n + k) * alpha ** 2 - n 371 | 372 | # Calculating the weights 373 | Wp = self.Wp * 0 + 1 374 | Wp /= 2 * (n + lamb) 375 | Wc = self.Wc * 0 + 1 376 | Wc /= 2 * (n + lamb) 377 | Wp[0, 0] *= 2 * lamb 378 | Wc[0, 0] = Wp[0, 0] + 1 - alpha ** 2 + beta 379 | Wc[0, 0] = 0 # setting this to 0 guarantees that predicted matrix P > 0 380 | 381 | # Storing the calculated values 382 | self.Wp = Wp 383 | self.Wc = Wc 384 | 385 | def _safe_cholesky(self): 386 | r""" This is used to find the Cholesky decomposition of matrix `P` in a safe way. 387 | 388 | This is a private method, it is not part of the public API. 389 | 390 | Notes 391 | ----- 392 | Cholesky decomposition tries to find a 'square-root' of a matrix, i.e., a matrix that satisfies the following 393 | expression: :math:`P = AA^T` , for the procedure to work matrix P must satisfy following conditions: 394 | * `P` is a symmetric matrix, :math:`P = P^T` 395 | * `P` > 0, i.e. `P` must not have any eigen value that is < 0 396 | 397 | Theoretically we don't expect `P` to have any eig values < 0, since that implies complex value 398 | of standard deviation, so if it happens that `P` < 0 that is a result of either alg diverging or numerical err, 399 | we are fixing that by utilising :func:`near_PD` function, essentially it clips all the eigen values from some 400 | epsilon. 401 | 402 | In case that the procedure fails we set the value of 'P_chol' to 'divergence_uncertainty'. 403 | """ 404 | 405 | # Reading the values 406 | 407 | P = self.P 408 | 409 | self.P = nearPD(P) 410 | 411 | try: 412 | self.P_chol = self.cholesky(self.P) 413 | except: 414 | self.P_chol = np.sqrt(np.abs(self.P))*self.divergence_uncertainty 415 | print('broke!') 416 | 417 | def _generate_sigma_points(self): 418 | r"""This function generates sigma points for purposes of inference of statistical 419 | properties of non-linear process model. 420 | 421 | This is a private method, it is not part of the public API. 422 | 423 | Notes 424 | ----- 425 | For the purpose of Sigma point generation theoretical expressions from Sigma_Point_Kalman_filters(UKF+PF) 426 | paper in NoNLin Filtering section of the GDrive are being used, it is not recommended to change these 427 | expressions unless there is some theoretical justification. 428 | 429 | Central point of generated Sigma points is around current estimate of `x`, 2n points are spread 430 | around it, the spread is determined by uncertainty matrix `P` and choice of parameters `_alpha` and `_k`, 431 | if the spread of points is smaller we will get more accurate image of nonlinear trans. 432 | 433 | """ 434 | 435 | # Reading the parameter values for the purpose of having more readable mathematical expressions 436 | alpha = self.alpha 437 | k = self.k 438 | 439 | n = self.dim_x 440 | lamb = (alpha ** 2) * (n + k) - n 441 | a = np.sqrt(n + lamb) 442 | 443 | X = np.zeros((2 * n + 1, n)) 444 | self._safe_cholesky() # updating self.P_chol 445 | P_chol = a * self.P_chol 446 | 447 | # calculating the sigma points 448 | X[0, :] = self.x 449 | X[1:n + 1, :] = (self.x + P_chol) 450 | X[n + 1:2 * n + 1, :] = (self.x - P_chol) 451 | 452 | return X 453 | 454 | def prediction(self, u=None, f=None, Q=None, **kwargsf): 455 | r"""This is the prediction step of filtering algorithm. 456 | 457 | Based on the current estimation of state space `x`, current values of control `u` and estimator model `f` 458 | we predict the state of the system 1 step in the future. 459 | 460 | Parameters 461 | ---------- 462 | u: :class:`~numpy:numpy.ndarray`, optional 463 | Control signals at the current time step (default value is None, if `u` is None then it's not passed to 464 | estimator model `f`). 465 | 466 | f : function, optional 467 | Estimator model function (default is None, if `f` is None then function stored in attribute `self.f` is 468 | used as estimator model) 469 | 470 | Q : :class:`~numpy:numpy.ndarray`, optional 471 | Process uncertainty matrix (default is None, if `Q` is None then matrix stored in attribute `self.Q` is 472 | used as process uncertainty) 473 | 474 | **kwargsf : :py:class:`dict`, optional 475 | dictionary of keyword arguments that get passed along to estimator model `f` (default is None) 476 | 477 | Notes 478 | ----- 479 | Results of the update step are stored in `x_prior` and `P_prior` these variables are hard copies, so they can 480 | be used safely outside the :class:`UnscentedKalmanFilter` 481 | 482 | """ 483 | # This is the prediction step of Kalman algorithm, based on the current estimation of state space, current 484 | # values of control u and process model f we predict the state of the system 1 step in the future 485 | 486 | # Just making the function more flexible, in usual use case only u will get passed in as an argument, 487 | # but there might exist some use cases where f and Q could be adaptive based on some external logic 488 | if f is None: 489 | f = self.f 490 | if Q is None: 491 | Q = self.Q 492 | elif np.isscalar(Q): 493 | Q = np.eye(self.dim_x) * Q 494 | 495 | # First we need to generate 2n + 1 sigma points, n is number of states in the model 496 | # Wc are weights used for covariance estimation 497 | # Wp are weights used for mean estimation 498 | X = self._generate_sigma_points() 499 | 500 | # Now we need to propagate the points through the process model 501 | if u is None: 502 | X = f(X.reshape((*X.shape, 1)), 503 | **kwargsf) # need to add 1 extra dimension so the data gets treated as N 1-d inputs 504 | if X.ndim == 3: 505 | X = X.squeeze(axis=2) 506 | else: 507 | if np.isscalar(u): 508 | u = np.array([u]) # need to add 1 extra dimension so the data gets treated as N 1-d inputs 509 | # 1 extra dimension is added to u as well, because by the default numpy expands from the left 510 | # and that would lead to errors, i.e., (2,) -> (1, 2) 511 | X = f(X.reshape((*X.shape, 1)), 512 | u.reshape((*u.shape, 1)), **kwargsf) 513 | if X.ndim == 3: 514 | X = X.squeeze(axis=2) 515 | 516 | # Now we can use the propagated sigma points for mean and covariance estimation: 517 | # x = sum(Wp_iX_i) equation for mean calculation 518 | # P = sum(Wc_i(X_i - x)(X_i - x)') equation for covariance calculation 519 | # Wp and Wc weights add up to 1, i.e., sum(Wp) = sum(Wc) = 1 520 | self.X_trans = X 521 | x = (X * self.Wp).sum(axis=0) # Python magic: Wp has the shape (2n+1, 1) and X has shape (2n+1, n) 522 | # as a first step Wp*X will result in matrix with shape (2n+1, n), each row from X will get multiplied 523 | # element wise with values from row vector Wp, next we want to sum all the rows, that can be achieved 524 | # by using the np.sum(Wp*X, axis=0), if axis=1 then columns are summed 525 | 526 | # More info about broadcasting can be found here: https://numpy.org/doc/stable/user/basics.broadcasting.html 527 | 528 | # This expression estimates the uncertainty in the predicted position of the x 529 | # using the standard expression for covariance estimation 530 | self.P = ((X - x) * self.Wc).T @ (X - x) + Q # Similarly like before (X-x)*Wp 531 | # multiples each row of (X-x) with values from row vector Wc. 532 | 533 | self.P = self.P / 2 + self.P.T / 2 534 | 535 | self.x = x 536 | 537 | self.x_prior = x.copy() 538 | self.P_prior = self.P.copy() 539 | 540 | def update(self, z, h=None, R=None, update_gain=True, **kwargsh): 541 | r"""This is the update step of filtering algorithm 542 | 543 | Based on our confidence in the model and measurements we make optimal decision - if we trust the model more 544 | than the measurement then the estimation is more biased towards the predicted value `x`, 545 | in other case we are biased towards the measurement `z`. 546 | 547 | Parameters 548 | ---------- 549 | z: :class:`~numpy:numpy.ndarray` 550 | Measurement at the current time step (if `z` is None it is assumed that no measurement is available, 551 | in that case we use predicted value as the estimate). 552 | 553 | h : function, optional 554 | Measurement model function (default is None, if `h` is None then function stored in attribute `self.h` 555 | is used as measurement model) 556 | 557 | R : :class:`~numpy:numpy.ndarray`, optional 558 | Measurement uncertainty matrix (default is None, if `Q` is None then matrix stored in attribute `self.R` 559 | is used as measurement uncertainty) 560 | 561 | update_gain : :py:class`bool`, optional 562 | used to specify if the Kalman gain should be updated (default value is ``True``) 563 | 564 | **kwargsh : :py:class:`dict`, optional 565 | dictionary of keyword arguments that get passed along to measurement model `h` (default is None) 566 | 567 | Notes 568 | ----- 569 | Results of the update step are stored in `x_post` and `P_post` these variables are hard copies, so they can 570 | be used safely outside the :class:`UnscentedKalmanFilter` 571 | 572 | """ 573 | # This is the update step of Kalman algorithm, based on our confidence in the model and measurements 574 | # we make optimal decision - if we trust the model more than the measurement then the estimation is more 575 | # biased with state space values more consistent with the model, in other case we are biased towards the 576 | # measurement 577 | 578 | # z is the measurement at the present time 579 | 580 | if z is None: # No measurement is available, value of P could be restarted to reflect our lack 581 | # of confidence in predicted values, since no measurement is available 582 | self.x_post = self.x.copy() 583 | self.P_post = self.P.copy() 584 | self.z = np.array([None] * self.dim_z) 585 | self.v = np.array(0 * self.dim_z) 586 | return 587 | else: 588 | z = z 589 | # Just making the function more flexible, in usual use case only u will get passed in as an argument, 590 | # but there might exist some use cases where h and R could be adaptive based on some external logic 591 | if h is None: 592 | h = self.h 593 | if R is None: 594 | R = self.R 595 | elif np.isscalar(R): 596 | R = np.eye(self.dim_z) * R 597 | 598 | # First we need to generate 2n + 1 sigma points, n is number of states in the model 599 | # Wc are weights used for covariance estimation 600 | # Wp are weights used for mean estimation 601 | X = self.X_trans 602 | x = self.x 603 | P = self.P 604 | 605 | # Now we need to propagate the points through the measurement model 606 | # X here contains n columns and 2n+1 rows. Each row is a state prediction is the state space for a given sigma 607 | # point. The following instruction unpacks the rows (each associated to a sigma point) so that each sigma point 608 | # prediction becomes an element of an array instead of a row of a matrix. 609 | # it also applies the measurement model, i.e. takes predicted state X to produce predicted measurement Z 610 | Z = h(X.reshape((*X.shape, 1)), 611 | **kwargsh) # need to add 1 extra dimension so the data gets treated as N 1-d inputs 612 | if Z.ndim == 3: # bring Z back to be a 2D array of array, for easier handling 613 | Z = Z 614 | Z = Z.squeeze(axis=2) 615 | 616 | # Now we can use the propagated sigma points for mean and covariance estimation: 617 | # z_hat = sum(Wp_iX_i) equation for mean calculation 618 | # S = sum(Wc_i(Z_i - z_hat)(Z_i - z_hat)') equation for covariance calculation 619 | # C = sum(Wc_i(X_i - x)(Z_i - z_hat)') equation for cross-covariance 620 | # Wp and Wc weights add up to 1, i.e., sum(Wp) = sum(Wc) = 1 621 | 622 | # z_hat is the center/the best prediction among the scattered Z points 623 | z_hat = np.sum(Z * self.Wp, axis=0) # Python magic: Wp has the shape (2n+1, 1) and X has shape (2n+1, n) 624 | # as a first step Wp*X will result in matrix with shape (2n+1, n), each row from X will get multiplied 625 | # element wise with values from row vector Wp, next we want to sum all the rows, that can be achieved 626 | # by using the np.sum(Wp*X, axis=0), if axis=1 then columns are summed 627 | 628 | # More info about broadcasting can be found here: https://numpy.org/doc/stable/user/basics.broadcasting.html 629 | 630 | # This expression estimates the uncertainty in the measurement z using 631 | # the standard expression for covariance estimation 632 | S = ((Z - z_hat) * self.Wc).T @ (Z - z_hat) + R # Similarly like before (X-x)*Wp 633 | # multiples each row of (X-x) with values from row vector Wc. 634 | 635 | S = S / 2 + S.T / 2 # Making sure that S is a symmetric matrix 636 | 637 | # This expression estimates the similarity between current state and measurement 638 | C = ((X - x) * self.Wc).T @ (Z - z_hat) # each row of (X-x) is multiplied by a scalar, element of array Wc 639 | # (X - x) * self.Wc).T has dimension [n 2n+1] , (Z - z_hat) has dim [2n+1,size of meas array] 640 | # C has dim [n, size of meas array 641 | 642 | if update_gain: 643 | SI = self.inv(S) # size of S matrix depends on the size of measurements array 644 | K = C @ SI # kalman gain, decides what to trust more, measurements or model 645 | else: 646 | K = self.K 647 | SI = self.SI 648 | 649 | # Now we can calculate the Kalman gain: 650 | # K = C*SI and use it for final update: 651 | # x = x + K(z - z_hat), v = z - z_hat -> innovation 652 | # P = P - KSK' - covariance update, to ensure symmetry and numerical stability we will preform 653 | # correction : P = P/2 + P'/2 654 | 655 | # high uncertainty: big S, small SI, small P (P~C) 656 | 657 | v = z - z_hat # innovation info element, difference between actual observation and predicted observation 658 | x = x + K @ v # update the final estimate of state array with the innovation information 659 | P = P - K @ S @ K.T # update the covariance matrix (uncertainty in the state space) based on kalman gain 660 | P = P / 2 + P.T / 2 # Making sure that P is a symmetric matrix 661 | 662 | self.x = x 663 | self.v = v 664 | self.z = z 665 | self.K = K 666 | self.S = S 667 | self.SI = SI 668 | self.C = C 669 | self.P = P 670 | self.x_post = x.copy() 671 | self.P_post = P.copy() 672 | 673 | 674 | 675 | 676 | class UnscentedKalmanInformationFilter(object): 677 | 678 | def __init__(self, dim_x=1, dim_z=1, dim_u=0): 679 | 680 | if dim_x < 1: 681 | raise ValueError('dim_x must be 1 or greater') 682 | if dim_z < 1: 683 | raise ValueError('dim_z must be 1 or greater') 684 | if dim_u < 0: 685 | raise ValueError('dim_u must be 0 or greater') 686 | 687 | self.dim_x = dim_x 688 | self.dim_z = dim_z 689 | self.dim_u = dim_u 690 | 691 | # Parameters for sigma point generation : 692 | self._alpha = 0.001 693 | self._beta = 2 694 | self._k = 3 - dim_x 695 | 696 | self.x = np.zeros((dim_x, )) # state 697 | self.x_info = np.zeros((dim_x, )) # state in information space 698 | self.P = np.eye(dim_x) # estimation uncertainty covariance 699 | self.P_chol = np.eye(dim_x) # Cholesky decomposition of P 700 | self.P_inv = np.eye(dim_x) # estimation uncertainty covariance inversion 701 | self.Q = np.eye(dim_x) # process uncertainty 702 | self.f = None # non-linear process function 703 | 704 | self.z = np.zeros((dim_z, )) # measurement 705 | self.h = None # non-linear measurement function 706 | self.R_inv = np.eye(dim_z) # measurement uncertainty 707 | 708 | self.K = np.zeros((dim_x, dim_z)) # Kalman gain matrix 709 | self.v = np.zeros((dim_z, 1)) # innovation vector 710 | self.S = np.zeros((dim_z, dim_z)) # innovation covariance 711 | self.C = np.zeros((dim_x, dim_z)) # measurement and state cross-covariance 712 | 713 | self._I = np.eye(dim_x) # identity matrix 714 | 715 | # values of x after prediction step 716 | self.x_prior = self.x.copy() 717 | self.P_inv_prior = self.P.copy() 718 | 719 | self.Wp = np.ones((2 * dim_x + 1, 1)) # weights for sigma points for state estimation 720 | self.Wc = np.ones((2 * dim_x + 1, 1)) # weights for sigma points for covariance estimation 721 | self._update_weights() 722 | 723 | # values of x after update step 724 | self.x_post = self.x.copy() 725 | self.P_inv_post = self.P.copy() 726 | 727 | self.inv = np.linalg.inv 728 | self.cholesky = np.linalg.cholesky 729 | self.eps = np.eye(dim_x) * 0.0001 730 | self.max_iter = 10 731 | self.divergence_uncertainty = 5 732 | 733 | self._recalculate_weights = False 734 | 735 | @property # getter property for alpha 736 | def alpha(self): 737 | return self._alpha 738 | 739 | @property # getter property for alpha 740 | def beta(self): 741 | return self._beta 742 | 743 | @property # getter property for alpha 744 | def k(self): 745 | return self._k 746 | 747 | @alpha.setter 748 | def alpha(self, value): 749 | if not np.isscalar(value): 750 | raise ValueError('alpha must be a scalar') 751 | if value < 1e-4: 752 | raise ValueError('alpha must be greater than 1e-4') 753 | if value > 1: 754 | raise ValueError('alpha must be less than 1') 755 | self._alpha = value 756 | self._update_weights() 757 | 758 | @k.setter 759 | def k(self, value): 760 | if not np.isscalar(value): 761 | raise ValueError('k must be a scalar') 762 | self._k = value 763 | self._update_weights() 764 | 765 | @beta.setter 766 | def beta(self, value): 767 | if not np.isscalar(value): 768 | raise ValueError('beta must be a scalar') 769 | self._beta = value 770 | self._update_weights() 771 | 772 | def _update_weights(self): 773 | alpha = self.alpha 774 | beta = self.beta 775 | k = self.k 776 | n = self.dim_x 777 | 778 | lamb = (n + k) * alpha ** 2 - n 779 | Wp = self.Wp * 0 + 1 780 | Wp /= 2 * (n + lamb) 781 | Wc = self.Wc * 0 + 1 782 | Wc /= 2 * (n + lamb) 783 | Wp[0, 0] *= 2 * lamb 784 | Wc[0, 0] = Wp[0, 0] + 1 - alpha ** 2 + beta 785 | 786 | self.Wp = Wp 787 | self.Wc = Wc 788 | 789 | def _safe_cholesky(self): 790 | r""" This is used to find the Cholesky decomposition of matrix `P` in a safe way. 791 | 792 | This is a private method, it is not part of the public API. 793 | 794 | Notes 795 | ----- 796 | Cholesky decomposition tries to find a 'square-root' of a matrix, i.e., a matrix that satisfies the following 797 | expression: :math:`P = AA^T` , for the procedure to work matrix P must satisfy following conditions: 798 | * `P` is a symmetric matrix, :math:`P = P^T` 799 | * `P` > 0, i.e. `P` must not have any eigen value that is < 0 800 | 801 | Theoretically we don't expect `P` to have any eig values < 0, since that implies complex value 802 | of standard deviation, so if it happens that `P` < 0 that is a result of either alg diverging or numerical err, 803 | we are fixing that by utilising :func:`near_PD` function, essentially it clips all the eigen values from some 804 | epsilon. 805 | 806 | In case that the procedure fails we set the value of 'P_chol' to 'divergence_uncertainty'. 807 | """ 808 | 809 | # Reading the values 810 | 811 | P = self.P 812 | 813 | self.P = nearPD(P) 814 | 815 | try: 816 | self.P_chol = self.cholesky(self.P) 817 | except: 818 | self.P_chol = self.divergence_uncertainty*self.Q 819 | 820 | def _generate_sigma_points(self): 821 | 822 | alpha = self.alpha 823 | k = self.k 824 | 825 | n = self.dim_x 826 | lamb = (alpha ** 2) * (n + k) - n 827 | a = np.sqrt(n + lamb) 828 | X = np.zeros((2 * n + 1, n)) 829 | self._safe_cholesky() 830 | P_chol = a * self.P_chol 831 | X[0, :] = self.x 832 | X[1:n + 1, :] = (self.x + P_chol) 833 | X[n + 1:2 * n + 1, :] = (self.x - P_chol) 834 | 835 | return X 836 | 837 | def prediction(self, u=None, f=None, Q=None): 838 | if f is None: 839 | f = self.f 840 | if Q is None: 841 | Q = self.Q 842 | elif np.isscalar(Q): 843 | Q = np.eye(self.dim_x) * Q 844 | 845 | # First we need to generate 2n + 1 sigma points, n is number of states in the model 846 | # Wc are weights used for covariance estimation 847 | # Wp are weights used for mean estimation 848 | X = self._generate_sigma_points() 849 | # Now we need to propagate the points through the process model 850 | if u is None: 851 | X = f(X.reshape((*X.shape, 1))) # need to add 1 extra dimension so the data gets treated as N 1-d inputs 852 | if X.ndim == 3: 853 | X = X.squeeze(axis=2) 854 | else: 855 | if np.isscalar(u): 856 | u = np.array([u]) 857 | 858 | X = f(X.reshape((*X.shape, 1)), u.reshape((*u.shape, 1))) # need to add 1 extra dimension so the data gets 859 | # treated as N 1-d inputs 860 | if X.ndim == 3: 861 | X = X.squeeze(axis=2) 862 | 863 | # Now we can use the propagated sigma points for mean and covariance estimation: 864 | # x = sum(Wp_iX_i) equation for mean calculation 865 | # P = sum(Wc_i(X_i - x)(X_i - x)') equation for covariance calculation 866 | 867 | x = (X * self.Wp).sum(axis=0) # Python magic: Wp has the shape (2n+1, 1) and X has shape (2n+1, n) 868 | # as a first step Wp*X will result in matrix with shape (2n+1, n), each row from X will get multiplied 869 | # element wise with values from row vector Wp, next we want to sum all the rows, that can be achieved 870 | # by using the np.sum(Wp*X, axis=0), if axis=1 then columns are summed 871 | 872 | # More info about broadcasting can be found here: https://numpy.org/doc/stable/user/basics.broadcasting.html 873 | self.P = ((X - x) * self.Wc).T @ (X - x) + Q 874 | self.P_inv = self.inv(self.P) 875 | 876 | self.x = x 877 | self.x_info = self.P_inv @ self.x 878 | 879 | self.x_prior = x.copy() 880 | self.P_inv_prior = self.P_inv.copy() 881 | 882 | def update(self, z, h=None, R_inv=None, multiple_sensors=False): 883 | 884 | if z is None: # No measurement is available 885 | self.x_post = self.x.copy() 886 | self.P_post = self.P.copy() 887 | self.z = np.array([None] * self.dim_z) 888 | self.v = np.array(0 * self.dim_z) 889 | return 890 | 891 | if h is None: 892 | h = self.h 893 | if R_inv is None: 894 | R_inv = self.R_inv 895 | elif np.isscalar(R_inv): 896 | R_inv = np.eye(self.dim_z) * R_inv 897 | 898 | # First we need to generate 2n + 1 sigma points, n is number of states in the model 899 | # Wc are weights used for covariance estimation 900 | # Wp are weights used for mean estimation 901 | X = self._generate_sigma_points() 902 | x = self.x 903 | P = self.P 904 | 905 | # Now we need to propagate the points through the measurement model 906 | 907 | if multiple_sensors: 908 | number_of_sensors = z.shape[0] # It is assumed that measurements are stacked in rows 909 | self.v = np.zeros((number_of_sensors, self.dim_z)) 910 | C = np.zeros((number_of_sensors, self.dim_x, self.dim_z)) 911 | bias = np.zeros((number_of_sensors, self.dim_z)) 912 | for idx in range(number_of_sensors): 913 | Z = h[idx](X.reshape((*X.shape, 1))) 914 | if Z.ndim == 3: 915 | Z = Z.squeeze(axis=2) 916 | 917 | z_hat = np.sum(Z * self.Wp, axis=0) 918 | self.v[idx, :] = z[idx, :] - z_hat 919 | 920 | C[idx, :, :] = ((X - x) * self.Wc).T @ (Z - z_hat) 921 | bias[idx, :] = C[idx, :, :].T @ self.x_info 922 | 923 | self.v = self.v.reshape((*self.v.shape, 1)) 924 | bias = bias.reshape((*bias.shape, 1)) 925 | else: 926 | number_of_sensors = 1 # It is assumed that measurements are stacked in rows 927 | self.v = np.zeros((number_of_sensors, self.dim_z)) 928 | C = np.zeros((number_of_sensors, self.dim_x, self.dim_z)) 929 | bias = np.zeros((number_of_sensors, self.dim_z)) 930 | for idx in range(number_of_sensors): 931 | Z = h(X.reshape((*X.shape, 1))) 932 | if Z.ndim == 3: 933 | Z = Z.squeeze(axis=2) 934 | 935 | z_hat = np.sum(Z * self.Wp, axis=0) 936 | self.v[idx, :] = z - z_hat 937 | 938 | C[idx, :, :] = ((X - x) * self.Wc).T @ (Z - z_hat) 939 | bias[idx, :] = C[idx, :, :].T @ self.x_info 940 | 941 | self.v = self.v.reshape((*self.v.shape, 1)) 942 | bias = bias.reshape((*bias.shape, 1)) 943 | R_inv = R_inv.reshape((1, *R_inv.shape)) 944 | 945 | C_T = C.transpose(0, 2, 1) # Transposing only the matrices 946 | ik = (self.P_inv @ C @ R_inv @ (self.v + bias)).sum(axis=0) # sensor information contribution 947 | ik = ik.squeeze() 948 | Ik = (self.P_inv @ C @ R_inv @ C_T @ self.P_inv.T).sum(axis=0) # sensor uncertainty contribution 949 | 950 | self.x_info += ik 951 | self.P_inv += Ik 952 | self.P = self.inv(self.P_inv) 953 | self.P = self.P / 2 + self.P.T / 2 + self.eps 954 | 955 | self.x = self.P @ self.x_info 956 | #self.v = v 957 | self.z = z 958 | self.C = C 959 | self.P = P 960 | 961 | self.x_post = x.copy() 962 | self.P_inv_post = P.copy() 963 | --------------------------------------------------------------------------------