From ab551c5f6dd3590b91d8f0ffc5a696c038d81c1b Mon Sep 17 00:00:00 2001 From: basameera Date: Fri, 8 May 2020 21:22:56 +0200 Subject: [PATCH 01/19] particle filter 1D sim started --- .gitignore | 2 ++ MyCode/Particle_Filter/1D.py | 39 ++++++++++++++++++++++++++++++++++++ 2 files changed, 41 insertions(+) create mode 100644 MyCode/Particle_Filter/1D.py diff --git a/.gitignore b/.gitignore index c971b8f9c5..794caf3b6a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,5 @@ +.vscode + *.csv *.gif *.g2o diff --git a/MyCode/Particle_Filter/1D.py b/MyCode/Particle_Filter/1D.py new file mode 100644 index 0000000000..658e4367dc --- /dev/null +++ b/MyCode/Particle_Filter/1D.py @@ -0,0 +1,39 @@ +import numpy as np +import matplotlib.pyplot as plt + +if __name__ == "__main__": + velocity = 1 # m/s + DT = 0.1 # sec + start_pos = 0 # meters + time = 0.0 + + SIM_TIME = 10 + + positions = [] + pos = start_pos + positions.append(pos) + + # for t in range(sim_time): + # pos = pos + (velocity * DT) + # positions.append(pos) + + show_animation = True + + while SIM_TIME >= time: + time += DT + + pos += velocity * DT + # print(pos) + + if show_animation: + plt.cla() + # for stopping simulation with the esc key. + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + plt.scatter(pos, 0) + plt.grid(True) + plt.xlim(-1, 11) + plt.xlabel('X Position (m)') + plt.title('Simulation time: {:.2f} seconds'.format(time)) + + plt.pause(0.001) From 7d4faf5d50402ea5bea0edb3289daf8eb37066ca Mon Sep 17 00:00:00 2001 From: basameera Date: Sat, 9 May 2020 01:17:25 +0200 Subject: [PATCH 02/19] mountain sim ready --- MyCode/Particle_Filter/1D.py | 51 ++++++++++++++++++++++++---------- MyCode/Particle_Filter/poly.py | 46 ++++++++++++++++++++++++++++++ 2 files changed, 83 insertions(+), 14 deletions(-) create mode 100644 MyCode/Particle_Filter/poly.py diff --git a/MyCode/Particle_Filter/1D.py b/MyCode/Particle_Filter/1D.py index 658e4367dc..1bd49fcb88 100644 --- a/MyCode/Particle_Filter/1D.py +++ b/MyCode/Particle_Filter/1D.py @@ -1,39 +1,62 @@ import numpy as np import matplotlib.pyplot as plt +from numpy.polynomial import Chebyshev as T if __name__ == "__main__": velocity = 1 # m/s - DT = 0.1 # sec + DT = 1 # sec start_pos = 0 # meters time = 0.0 - SIM_TIME = 10 + plane_height = 5 - positions = [] pos = start_pos - positions.append(pos) - # for t in range(sim_time): - # pos = pos + (velocity * DT) - # positions.append(pos) + x = np.linspace(-1, 0.9, 100) + y1 = T.basis(2)(x) + T.basis(5)(x) + y2 = T.basis(3)(x) + T.basis(5)(x) + y3 = T.basis(4)(x) + T.basis(5)(x) + + ly = np.concatenate((y2, y1, y3)) + ly = ly + abs(np.min(ly)) + lx = np.arange(len(ly)) + + SIM_TIME = len(ly) show_animation = True + fig, axs = plt.subplots(1, 1, figsize=(14, 5)) + axs.set_axisbelow(True) + while SIM_TIME >= time: time += DT - + print(time, ly[int(time)], end='\r') pos += velocity * DT - # print(pos) if show_animation: plt.cla() # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) - plt.scatter(pos, 0) - plt.grid(True) - plt.xlim(-1, 11) - plt.xlabel('X Position (m)') - plt.title('Simulation time: {:.2f} seconds'.format(time)) + + axs.grid(ls='--') + axs.plot(lx, ly, c='g') + axs.fill_between(lx, ly, color="green", alpha=0.6) + + # Plot plane route + axs.axhline(y=plane_height, xmin=0, xmax=1, + c='r', ls='--', lw=1) + # plane current pos + axs.scatter(pos, plane_height, c='r', lw=3) + + # distance to current height + axs.scatter(pos, ly[int(time)], c='k', lw=3) + + # settings + axs.grid(True) + # plt.xlim(-1, SIM_TIME) + axs.set_xlabel('X Position (m)') + axs.set_ylabel('Height (m)') + axs.set_title('Simulation time: {:.2f} seconds'.format(time)) plt.pause(0.001) diff --git a/MyCode/Particle_Filter/poly.py b/MyCode/Particle_Filter/poly.py new file mode 100644 index 0000000000..8081818ffd --- /dev/null +++ b/MyCode/Particle_Filter/poly.py @@ -0,0 +1,46 @@ +import matplotlib.pyplot as plt +import numpy as np +from numpy.polynomial import Chebyshev as T + +if __name__ == "__main__": + + x = np.linspace(-1, 0.9, 100) + + y1 = T.basis(2)(x) + T.basis(5)(x) + y2 = T.basis(3)(x) + T.basis(5)(x) + y3 = T.basis(4)(x) + T.basis(5)(x) + + # Poly curves + # fig, axs = plt.subplots(1, 1, figsize=(6, 4)) + + # ax = axs + # ax.set_axisbelow(True) + # ax.grid(ls='--') + + # ax.plot(x, y1, label='y1') + # ax.plot(x, y2, label='y2') + # ax.plot(x, y3, label='y3') + + # ax.legend(loc="upper left") + + # Mountains + fig, axs = plt.subplots(1, 1, figsize=(14, 5)) + ax = axs + ax.set_axisbelow(True) + ax.grid(ls='--') + + ly = np.concatenate((y2, y1, y3)) + ly = ly + abs(np.min(ly)) + lx = np.arange(len(ly)) + + # plot mountain + ax.plot(lx, ly, c='g') + ax.fill_between(lx, ly, color="green", alpha=0.6) + + # Plot plane route + ax.axhline(y=5, xmin=0, xmax=1, c='r', ls='--', lw=0.75) + + ax.set_xlabel('X position (m)') + ax.set_ylabel('Height (m)') + + plt.show() From 3e0fd16f003796cd3d9b4327da468e04332a5b0c Mon Sep 17 00:00:00 2001 From: basameera Date: Sat, 9 May 2020 01:52:37 +0200 Subject: [PATCH 03/19] particle sim ready with noise induced height --- MyCode/Particle_Filter/1D.py | 56 ++++++++++++++++++++++----------- MyCode/Particle_Filter/poly.py | 15 +++++++-- MyCode/Particle_Filter/utils.py | 11 +++++++ 3 files changed, 61 insertions(+), 21 deletions(-) create mode 100644 MyCode/Particle_Filter/utils.py diff --git a/MyCode/Particle_Filter/1D.py b/MyCode/Particle_Filter/1D.py index 1bd49fcb88..302a5c0f13 100644 --- a/MyCode/Particle_Filter/1D.py +++ b/MyCode/Particle_Filter/1D.py @@ -1,14 +1,15 @@ import numpy as np import matplotlib.pyplot as plt from numpy.polynomial import Chebyshev as T +from utils import create_toy_data if __name__ == "__main__": velocity = 1 # m/s - DT = 1 # sec + DT = 5 # sec start_pos = 0 # meters time = 0.0 - plane_height = 5 + plane_height = 10 pos = start_pos @@ -18,20 +19,25 @@ y3 = T.basis(4)(x) + T.basis(5)(x) ly = np.concatenate((y2, y1, y3)) - ly = ly + abs(np.min(ly)) + ly_min = abs(np.min(ly)) + ly = ly + (ly_min*2) + lx = np.arange(len(ly)) + # std + std = 1 + y_sine_noise = create_toy_data(ly, std) + SIM_TIME = len(ly) show_animation = True fig, axs = plt.subplots(1, 1, figsize=(14, 5)) - axs.set_axisbelow(True) + ax = axs + ax.set_axisbelow(True) - while SIM_TIME >= time: - time += DT - print(time, ly[int(time)], end='\r') - pos += velocity * DT + # while SIM_TIME > time: + for pos in range(0, len(lx), DT): if show_animation: plt.cla() @@ -39,24 +45,36 @@ plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) - axs.grid(ls='--') - axs.plot(lx, ly, c='g') - axs.fill_between(lx, ly, color="green", alpha=0.6) + ax.grid(ls='--') + ax.plot(lx, ly, c='g') + ax.fill_between(lx, ly, color="green", alpha=0.6) + + # std range + ax.fill_between(lx, ly - std, ly + std, + color="gray", label="std.", alpha=0.5) + + # noise within std + + ax.scatter(lx, y_sine_noise, c='b', marker='*', lw=0.5) # Plot plane route - axs.axhline(y=plane_height, xmin=0, xmax=1, - c='r', ls='--', lw=1) + ax.axhline(y=plane_height, xmin=0, xmax=1, + c='r', ls='--', lw=1) # plane current pos - axs.scatter(pos, plane_height, c='r', lw=3) + ax.scatter(pos, plane_height, c='r', lw=3, marker='>') # distance to current height - axs.scatter(pos, ly[int(time)], c='k', lw=3) + + ax.scatter(pos, ly[pos], c='k', lw=3) # settings - axs.grid(True) + ax.grid(True) # plt.xlim(-1, SIM_TIME) - axs.set_xlabel('X Position (m)') - axs.set_ylabel('Height (m)') - axs.set_title('Simulation time: {:.2f} seconds'.format(time)) + ax.set_xlabel('X Position (m)') + ax.set_ylabel('Height (m)') + ax.set_title('Simulation time: {:.1f} seconds'.format(pos+DT)) plt.pause(0.001) + + if show_animation: + plt.show() diff --git a/MyCode/Particle_Filter/poly.py b/MyCode/Particle_Filter/poly.py index 8081818ffd..02c2f602a0 100644 --- a/MyCode/Particle_Filter/poly.py +++ b/MyCode/Particle_Filter/poly.py @@ -1,6 +1,8 @@ import matplotlib.pyplot as plt import numpy as np from numpy.polynomial import Chebyshev as T +from utils import create_toy_data + if __name__ == "__main__": @@ -30,7 +32,8 @@ ax.grid(ls='--') ly = np.concatenate((y2, y1, y3)) - ly = ly + abs(np.min(ly)) + ly_min = abs(np.min(ly)) + ly = ly + (ly_min*2) lx = np.arange(len(ly)) # plot mountain @@ -38,7 +41,15 @@ ax.fill_between(lx, ly, color="green", alpha=0.6) # Plot plane route - ax.axhline(y=5, xmin=0, xmax=1, c='r', ls='--', lw=0.75) + ax.axhline(y=10, xmin=0, xmax=1, c='r', ls='--', lw=0.75) + + # std + std = 1 + ax.fill_between(lx, ly - std, ly + std, + color="r", label="std.", alpha=0.5) + + y_sine_noise = create_toy_data(ly, std) + ax.scatter(lx, y_sine_noise, c='b', marker='*', lw=0.5) ax.set_xlabel('X position (m)') ax.set_ylabel('Height (m)') diff --git a/MyCode/Particle_Filter/utils.py b/MyCode/Particle_Filter/utils.py new file mode 100644 index 0000000000..b218250b56 --- /dev/null +++ b/MyCode/Particle_Filter/utils.py @@ -0,0 +1,11 @@ +import numpy as np + +np.random.seed(123) + +def create_toy_data(ly, std): + t = ly + np.random.normal(scale=std, size=ly.shape) + return t + + +if __name__ == "__main__": + pass From 7ab65216f853210cb346a9bc38c17eb027c8e626 Mon Sep 17 00:00:00 2001 From: basameera Date: Sat, 9 May 2020 18:55:24 +0200 Subject: [PATCH 04/19] bayes filter --- MyCode/Bayes Filter/README.md | 8 ++++++++ MyCode/Kalman Filter/1_test.py | 0 MyCode/Particle_Filter/1D.py | 21 +++++++++++++++------ 3 files changed, 23 insertions(+), 6 deletions(-) create mode 100644 MyCode/Bayes Filter/README.md create mode 100644 MyCode/Kalman Filter/1_test.py diff --git a/MyCode/Bayes Filter/README.md b/MyCode/Bayes Filter/README.md new file mode 100644 index 0000000000..1693e56859 --- /dev/null +++ b/MyCode/Bayes Filter/README.md @@ -0,0 +1,8 @@ +# Bayes Filter + +[Ref. 1](#reference) + +### Reference + +1. Bayes Filter Youtube (https://www.youtube.com/watch?v=6uEgLv1Mr2s) +2. The Two Stages of Bayes Filtering (https://www.youtube.com/watch?v=Qa8YMP9dQYo) \ No newline at end of file diff --git a/MyCode/Kalman Filter/1_test.py b/MyCode/Kalman Filter/1_test.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/MyCode/Particle_Filter/1D.py b/MyCode/Particle_Filter/1D.py index 302a5c0f13..b6cd090d28 100644 --- a/MyCode/Particle_Filter/1D.py +++ b/MyCode/Particle_Filter/1D.py @@ -2,10 +2,18 @@ import matplotlib.pyplot as plt from numpy.polynomial import Chebyshev as T from utils import create_toy_data +from skylynx.utils import cli_args if __name__ == "__main__": + # argparse + cli_params = dict( + DT=5, + ) + + args = cli_args(cli_params) + velocity = 1 # m/s - DT = 5 # sec + DT = int(args['DT']) # sec start_pos = 0 # meters time = 0.0 @@ -53,10 +61,6 @@ ax.fill_between(lx, ly - std, ly + std, color="gray", label="std.", alpha=0.5) - # noise within std - - ax.scatter(lx, y_sine_noise, c='b', marker='*', lw=0.5) - # Plot plane route ax.axhline(y=plane_height, xmin=0, xmax=1, c='r', ls='--', lw=1) @@ -64,8 +68,13 @@ ax.scatter(pos, plane_height, c='r', lw=3, marker='>') # distance to current height + ax.scatter(pos, ly[pos], c='k', lw=2) + + # noise within std + ax.scatter(lx, y_sine_noise, c='b', marker='*', lw=0.5, alpha=0.5) - ax.scatter(pos, ly[pos], c='k', lw=3) + # current noisy distance sample + ax.scatter(pos, y_sine_noise[pos], c='k', marker='*', lw=0.5) # settings ax.grid(True) From 38add1fb93b0c3ed5b0388f8ac428566632ac8c6 Mon Sep 17 00:00:00 2001 From: basameera Date: Sat, 9 May 2020 20:50:02 +0200 Subject: [PATCH 05/19] bayes filter visualization --- MyCode/Bayes Filter/README.md | 17 ++++++- MyCode/Bayes Filter/main.py | 89 +++++++++++++++++++++++++++++++++++ MyCode/Bayes Filter/test.py | 46 ++++++++++++++++++ MyCode/Bayes Filter/utils.py | 46 ++++++++++++++++++ 4 files changed, 197 insertions(+), 1 deletion(-) create mode 100644 MyCode/Bayes Filter/main.py create mode 100644 MyCode/Bayes Filter/test.py create mode 100644 MyCode/Bayes Filter/utils.py diff --git a/MyCode/Bayes Filter/README.md b/MyCode/Bayes Filter/README.md index 1693e56859..ef4cb4381e 100644 --- a/MyCode/Bayes Filter/README.md +++ b/MyCode/Bayes Filter/README.md @@ -2,7 +2,22 @@ [Ref. 1](#reference) +## Visualization project + +[Ref. 2](#reference) + +### To Do + +* PDF as bar plot +* Motion model +* Mesurement model + ### Reference 1. Bayes Filter Youtube (https://www.youtube.com/watch?v=6uEgLv1Mr2s) -2. The Two Stages of Bayes Filtering (https://www.youtube.com/watch?v=Qa8YMP9dQYo) \ No newline at end of file +2. The Two Stages of Bayes Filtering (https://www.youtube.com/watch?v=Qa8YMP9dQYo) +3. Bayesian Filtering Review (https://www.youtube.com/watch?v=rlp4uaVNHVM) + +To watch + +1. Introduction to Bayesian statistics, part 1: The basic concepts (https://www.youtube.com/watch?v=0F0QoMCSKJ4) diff --git a/MyCode/Bayes Filter/main.py b/MyCode/Bayes Filter/main.py new file mode 100644 index 0000000000..f6ace37bc4 --- /dev/null +++ b/MyCode/Bayes Filter/main.py @@ -0,0 +1,89 @@ +import numpy as np +import matplotlib.pyplot as plt +from numpy.polynomial import Chebyshev as T +from utils import * +from skylynx.utils import cli_args + +if __name__ == "__main__": + # argparse + cli_params = dict( + DT=5, + ) + + args = cli_args(cli_params) + + velocity = 1 # m/s + DT = int(args['DT']) # sec + start_pos = 0 # meters + time = 0.0 + + plane_height = 10 + + pos = start_pos + + x = np.linspace(-1, 0.9, 100) + y1 = T.basis(2)(x) + T.basis(5)(x) + y2 = T.basis(3)(x) + T.basis(5)(x) + y3 = T.basis(4)(x) + T.basis(5)(x) + + ly = np.concatenate((y2, y1, y3)) + ly_min = abs(np.min(ly)) + ly = ly + (ly_min*2) + + lx = np.arange(len(ly)) + + # std + std = 1 + # y_sine_noise = create_toy_data(ly, std) + + SIM_TIME = len(ly) + + show_animation = True + + fig, axs = plt.subplots(1, 1, figsize=(14, 5)) + ax = axs + ax.set_axisbelow(True) + + # while SIM_TIME > time: + for pos in range(0, len(lx), DT): + + if show_animation: + plt.cla() + # for stopping simulation with the esc key. + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + + ax.grid(ls='--') + ax.plot(lx, ly, c='g') + ax.fill_between(lx, ly, color="green", alpha=0.6) + + # std range + ax.fill_between(lx, ly - std, ly + std, + color="gray", label="std.", alpha=0.5) + + # Plot plane route + ax.axhline(y=plane_height, xmin=0, xmax=1, + c='r', ls='--', lw=1) + # plane current pos + ax.scatter(pos, plane_height, c='r', lw=3, marker='>') + + # distance to current height + ax.scatter(pos, ly[pos], c='k', lw=2) + + # noise within std + # ax.scatter(lx, y_sine_noise, c='b', marker='*', lw=0.5, alpha=0.5) + + # current noisy distance sample + # ax.scatter(pos, y_sine_noise[pos], c='k', marker='*', lw=0.5) + + # settings + ax.grid(True) + # plt.xlim(-1, SIM_TIME) + ax.set_xlabel('X Position (m)') + ax.set_ylabel('Height (m)') + ax.set_title('Simulation time: {:.1f} seconds'.format(pos+DT)) + + plt.pause(0.001) + + if show_animation: + plt.show() diff --git a/MyCode/Bayes Filter/test.py b/MyCode/Bayes Filter/test.py new file mode 100644 index 0000000000..09f570bbc8 --- /dev/null +++ b/MyCode/Bayes Filter/test.py @@ -0,0 +1,46 @@ +from skylynx.utils import cli_args +from utils import * +import matplotlib.pyplot as plt +import numpy as np + + +def task_1(): + """PDF as bar plot + """ + + fig, axs = plt.subplots(1, 2, figsize=(8, 4)) + + # Mesurement model + y = np.ones(shape=(3,))/3 + x = np.arange(len(y)) - 1 + + ax = axs[0] + ax.set_title(r'Mesurement model $p(Z_t|X_t)$') + bar_plot(ax, x, y, color='r') + + # Motion model + p = [0, 0.75, 0.25] + y = np.array(p) + x = np.arange(len(y)) + + ax = axs[1] + ax.set_title(r'Motion model $p(X_{t+1}|X_t)$') + bar_plot(ax, x, y, color='b') + ax.axvline(0.25, c='k', ls='--') + ax.axvline(-0.25, c='k', ls='--') + + plt.show() + + +if __name__ == "__main__": + # argparse + cli_params = dict( + task=0, + ) + + args = cli_args(cli_params) + task = int(args['task']) + + # PDF as bar plot + if task == 1: + task_1() diff --git a/MyCode/Bayes Filter/utils.py b/MyCode/Bayes Filter/utils.py new file mode 100644 index 0000000000..98bb01bebe --- /dev/null +++ b/MyCode/Bayes Filter/utils.py @@ -0,0 +1,46 @@ +import numpy as np + + +def bar_plot(ax, x, y, color=None): + """Bar Plot + + Parameters + ---------- + x : 1D array + + y : 1D array + + """ + + ax.set_axisbelow(True) + ax.grid(ls='--') + ax.bar(x, y, color=color, width=0.5) + # plt.xlabel("File names") + # plt.ylabel("Execution time (seconds)") + # plt.title("Effects of Numba (loop of 1e6)") + # plt.xticks(x_pos, x) + # plt.savefig('stats.pdf') + ax.set_ylim(0, 1) + + +def norm(x): + """Normalize array + + Parameters + ---------- + x : [type] + [description] + + Returns + ------- + [type] + [description] + """ + x = np.array(x) + if int(np.sum(x)) != 1: + x = x/np.sum(x) + return x + + +if __name__ == "__main__": + pass From 60142bbee44bdd1441e355fc4f6be27f8d02a630 Mon Sep 17 00:00:00 2001 From: basameera Date: Sun, 10 May 2020 14:50:01 +0200 Subject: [PATCH 06/19] bayes filter main plot ready --- MyCode/Bayes Filter/test.py | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/MyCode/Bayes Filter/test.py b/MyCode/Bayes Filter/test.py index 09f570bbc8..b91e90df15 100644 --- a/MyCode/Bayes Filter/test.py +++ b/MyCode/Bayes Filter/test.py @@ -4,6 +4,31 @@ import numpy as np +def task_2(): + """start + """ + + fig, axs = plt.subplots(1, 1, figsize=(8, 4)) + + simulation_scope = 10 + + # Initial p(x) + p = [0.5, 0.5] + y = np.array(p) + + y = np.concatenate((y, np.zeros(simulation_scope-len(p),))) + + x = np.arange(simulation_scope) + + ax = axs + ax.set_title(r'Initial $p(x)$') + bar_plot(ax, x, y, color='k') + + ax.set_xlim(-1, simulation_scope+1) + + plt.show() + + def task_1(): """PDF as bar plot """ @@ -44,3 +69,5 @@ def task_1(): # PDF as bar plot if task == 1: task_1() + elif task == 2: + task_2() From 8bd64b80e46e5dc02888ea98b6ee95a2280e1ffa Mon Sep 17 00:00:00 2001 From: basameera Date: Sun, 10 May 2020 15:38:17 +0200 Subject: [PATCH 07/19] main window ready --- MyCode/Bayes Filter/README.md | 2 ++ MyCode/Bayes Filter/test.py | 54 ++++++++++++++++++++++++++++++++++- 2 files changed, 55 insertions(+), 1 deletion(-) diff --git a/MyCode/Bayes Filter/README.md b/MyCode/Bayes Filter/README.md index ef4cb4381e..8f3587ca58 100644 --- a/MyCode/Bayes Filter/README.md +++ b/MyCode/Bayes Filter/README.md @@ -11,6 +11,8 @@ * PDF as bar plot * Motion model * Mesurement model +* Main window +* Key press plot ### Reference diff --git a/MyCode/Bayes Filter/test.py b/MyCode/Bayes Filter/test.py index b91e90df15..de6baaf48b 100644 --- a/MyCode/Bayes Filter/test.py +++ b/MyCode/Bayes Filter/test.py @@ -4,8 +4,58 @@ import numpy as np +def ax_set_settings(ax): + ax.set_axisbelow(True) + ax.grid(ls='--') + ax.set_xlabel('') + ax.set_xlim(-1, 11) + ax.set_ylim(0, 1) + ax.set_title('Bayes Filter') + ax.legend() + + +click_counter = 0 + + +def get_color_label(): + global click_counter + if click_counter % 2 == 0: + color = 'tab:blue' + label = 'Mesurement' + else: + color = 'tab:orange' + label = 'Motion' + return color, label + + +def task_3(): + + def onclick(event): + # print('you pressed', event.key, event.xdata, event.ydata) + global click_counter + if event.key == 'escape': + exit(0) + elif event.key == 'right': + ax.clear() + data = np.random.random((10,)) + c, l = get_color_label() + ax.scatter(data, data, c=c, label=l) + ax_set_settings(ax) + + fig.canvas.draw() + click_counter += 1 + + fig, axs = plt.subplots(1, 1, figsize=(14, 5)) + ax = axs + + cid = fig.canvas.mpl_connect('key_release_event', onclick) + ax.plot(np.arange(0, 1, 0.1), c='tab:blue', label='Init') + ax_set_settings(ax) + plt.show() + + def task_2(): - """start + """Main window """ fig, axs = plt.subplots(1, 1, figsize=(8, 4)) @@ -71,3 +121,5 @@ def task_1(): task_1() elif task == 2: task_2() + elif task == 3: + task_3() From e963b047217a354c273ee7cb6e48adac75fd50a1 Mon Sep 17 00:00:00 2001 From: basameera Date: Sun, 10 May 2020 16:02:57 +0200 Subject: [PATCH 08/19] boat image --- MyCode/Bayes Filter/README.md | 3 +++ MyCode/Bayes Filter/bayes_filter_viz.py | 21 +++++++++++++++ MyCode/Bayes Filter/test.py | 36 ++++++++++++++++++++----- MyCode/Bayes Filter/utils.py | 10 +++---- 4 files changed, 59 insertions(+), 11 deletions(-) create mode 100644 MyCode/Bayes Filter/bayes_filter_viz.py diff --git a/MyCode/Bayes Filter/README.md b/MyCode/Bayes Filter/README.md index 8f3587ca58..f8fd221f8a 100644 --- a/MyCode/Bayes Filter/README.md +++ b/MyCode/Bayes Filter/README.md @@ -13,6 +13,9 @@ * Mesurement model * Main window * Key press plot +* Main window handling class +* B. F. calculations +* draw boat ### Reference diff --git a/MyCode/Bayes Filter/bayes_filter_viz.py b/MyCode/Bayes Filter/bayes_filter_viz.py new file mode 100644 index 0000000000..58bec7b157 --- /dev/null +++ b/MyCode/Bayes Filter/bayes_filter_viz.py @@ -0,0 +1,21 @@ +from skylynx.utils import cli_args +from utils import * +import matplotlib.pyplot as plt +import numpy as np +import matplotlib.image as mpimg +from matplotlib.offsetbox import TextArea, DrawingArea, OffsetImage, AnnotationBbox + + +class BayesFilterBoat(object): + + def __init__(self): + pass + + def hello(self, msg): + print(msg) + + +if __name__ == "__main__": + + boat_bf = BayesFilterBoat() + boat_bf.hello('sameera') diff --git a/MyCode/Bayes Filter/test.py b/MyCode/Bayes Filter/test.py index de6baaf48b..f53d5ec388 100644 --- a/MyCode/Bayes Filter/test.py +++ b/MyCode/Bayes Filter/test.py @@ -2,6 +2,8 @@ from utils import * import matplotlib.pyplot as plt import numpy as np +import matplotlib.image as mpimg +from matplotlib.offsetbox import TextArea, DrawingArea, OffsetImage, AnnotationBbox def ax_set_settings(ax): @@ -15,6 +17,15 @@ def ax_set_settings(ax): click_counter = 0 +simulation_scope = 10 +boat = mpimg.imread('/home/sameera/Downloads/sailboat.png') + + +def draw_boat(ax, x=0): + global boat + imagebox = OffsetImage(boat, zoom=0.2) + ab = AnnotationBbox(imagebox, (x, 0.8), frameon=False) + ax.add_artist(ab) def get_color_label(): @@ -39,17 +50,33 @@ def onclick(event): ax.clear() data = np.random.random((10,)) c, l = get_color_label() - ax.scatter(data, data, c=c, label=l) + p = [0.5, 0.5, 1] + y = norm(p) + y = np.concatenate((y, np.zeros(simulation_scope-len(p),))) + x = np.arange(simulation_scope) + + bar_plot(ax, x, y, color='tab:blue', label='Init') + draw_boat(ax, 2) ax_set_settings(ax) fig.canvas.draw() click_counter += 1 - fig, axs = plt.subplots(1, 1, figsize=(14, 5)) + fig, axs = plt.subplots(1, 1, figsize=(8, 4)) + global simulation_scope ax = axs cid = fig.canvas.mpl_connect('key_release_event', onclick) - ax.plot(np.arange(0, 1, 0.1), c='tab:blue', label='Init') + + # Initial p(x) + p = [0.6, 0.5] + y = np.array(p) + y = np.concatenate((y, np.zeros(simulation_scope-len(p),))) + x = np.arange(simulation_scope) + + # ax.plot(np.arange(0, 1, 0.1), c='tab:blue', label='Init') + bar_plot(ax, x, y, color='tab:blue', label='Init') + draw_boat(ax) ax_set_settings(ax) plt.show() @@ -59,15 +86,12 @@ def task_2(): """ fig, axs = plt.subplots(1, 1, figsize=(8, 4)) - simulation_scope = 10 # Initial p(x) p = [0.5, 0.5] y = np.array(p) - y = np.concatenate((y, np.zeros(simulation_scope-len(p),))) - x = np.arange(simulation_scope) ax = axs diff --git a/MyCode/Bayes Filter/utils.py b/MyCode/Bayes Filter/utils.py index 98bb01bebe..e5c558f2e1 100644 --- a/MyCode/Bayes Filter/utils.py +++ b/MyCode/Bayes Filter/utils.py @@ -1,7 +1,7 @@ import numpy as np -def bar_plot(ax, x, y, color=None): +def bar_plot(ax, x, y, color=None, label=''): """Bar Plot Parameters @@ -12,15 +12,15 @@ def bar_plot(ax, x, y, color=None): """ - ax.set_axisbelow(True) - ax.grid(ls='--') - ax.bar(x, y, color=color, width=0.5) + # ax.set_axisbelow(True) + # ax.grid(ls='--') + ax.bar(x, y, color=color, width=0.5, label=label) # plt.xlabel("File names") # plt.ylabel("Execution time (seconds)") # plt.title("Effects of Numba (loop of 1e6)") # plt.xticks(x_pos, x) # plt.savefig('stats.pdf') - ax.set_ylim(0, 1) + # ax.set_ylim(0, 1) def norm(x): From b4ced008cc2bc96abfc3df81fb26ac9a8f07d5f4 Mon Sep 17 00:00:00 2001 From: basameera Date: Sun, 10 May 2020 16:03:34 +0200 Subject: [PATCH 09/19] BF boat class --- MyCode/Bayes Filter/bayes_filter_viz.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/MyCode/Bayes Filter/bayes_filter_viz.py b/MyCode/Bayes Filter/bayes_filter_viz.py index 58bec7b157..25c740ab33 100644 --- a/MyCode/Bayes Filter/bayes_filter_viz.py +++ b/MyCode/Bayes Filter/bayes_filter_viz.py @@ -11,11 +11,7 @@ class BayesFilterBoat(object): def __init__(self): pass - def hello(self, msg): - print(msg) - if __name__ == "__main__": - + boat_bf = BayesFilterBoat() - boat_bf.hello('sameera') From 3f1af3a3c694b2f1a0381b71d2e0218277a41aa8 Mon Sep 17 00:00:00 2001 From: basameera Date: Sun, 10 May 2020 16:03:56 +0200 Subject: [PATCH 10/19] bfb --- MyCode/Bayes Filter/bayes_filter_viz.py | 1 + 1 file changed, 1 insertion(+) diff --git a/MyCode/Bayes Filter/bayes_filter_viz.py b/MyCode/Bayes Filter/bayes_filter_viz.py index 25c740ab33..7ff2e536b0 100644 --- a/MyCode/Bayes Filter/bayes_filter_viz.py +++ b/MyCode/Bayes Filter/bayes_filter_viz.py @@ -14,4 +14,5 @@ def __init__(self): if __name__ == "__main__": + boat_bf = BayesFilterBoat() From 7e5dcd9f9543b7f6c4ba8da8d100106be58198f0 Mon Sep 17 00:00:00 2001 From: basameera Date: Sun, 10 May 2020 16:37:22 +0200 Subject: [PATCH 11/19] BFB window ready --- MyCode/Bayes Filter/bayes_filter_viz.py | 44 +++++++++++++++++++++++-- MyCode/Bayes Filter/line.py | 18 ++++++++++ 2 files changed, 60 insertions(+), 2 deletions(-) create mode 100644 MyCode/Bayes Filter/line.py diff --git a/MyCode/Bayes Filter/bayes_filter_viz.py b/MyCode/Bayes Filter/bayes_filter_viz.py index 7ff2e536b0..e1366294bf 100644 --- a/MyCode/Bayes Filter/bayes_filter_viz.py +++ b/MyCode/Bayes Filter/bayes_filter_viz.py @@ -6,13 +6,53 @@ from matplotlib.offsetbox import TextArea, DrawingArea, OffsetImage, AnnotationBbox + class BayesFilterBoat(object): def __init__(self): - pass + self.boat_img = mpimg.imread('/home/sameera/Downloads/sailboat.png') + self.click_counter = 0 + self.simulation_scope = 10 + def main(self): + self._draw_main_window() -if __name__ == "__main__": + def _draw_main_window(self): + # self.fig, self.axs = plt.subplots(1, 3, figsize=(12, 4)) + + self.fig = plt.figure(figsize=(10, 8)) + gs = self.fig.add_gridspec(2, 2) + + # sim axis + self.ax_sim = self.fig.add_subplot(gs[0, :]) + self.ax_sim.set_title('Simulation') + + # Mesurement model + self.ax_me = self.fig.add_subplot(gs[1, 0]) + self.ax_me.set_title('Mesurement Model') + # Motion model + self.ax_mo = self.fig.add_subplot(gs[1, 1]) + self.ax_mo.set_title('Motion Model') + + line, = self.ax_sim.plot([0], [0]) + self.cid = line.figure.canvas.mpl_connect('button_press_event', self) + self.line = line + self.xs = list(line.get_xdata()) + self.ys = list(line.get_ydata()) + plt.show() + + def __call__(self, event): + # print('click', event) + if event.inaxes != self.line.axes: + return + self.xs.append(event.xdata) + self.ys.append(event.ydata) + self.line.set_data(self.xs, self.ys) + self.line.figure.canvas.draw() + + +if __name__ == "__main__": boat_bf = BayesFilterBoat() + boat_bf.main() diff --git a/MyCode/Bayes Filter/line.py b/MyCode/Bayes Filter/line.py new file mode 100644 index 0000000000..e706dfc045 --- /dev/null +++ b/MyCode/Bayes Filter/line.py @@ -0,0 +1,18 @@ +from matplotlib import pyplot as plt + +fig = plt.figure(figsize=(10, 8)) +gs = fig.add_gridspec(2, 2) + + +ax_sim = fig.add_subplot(gs[0, :]) +ax_sim.set_title('Simulation') + + +ax_me = fig.add_subplot(gs[1, 0]) +ax_me.set_title('Mesurement Model') + + +ax_mo = fig.add_subplot(gs[1, 1]) +ax_mo.set_title('Motion Model') + +plt.show() From 4384a5cb6ed5fed1fc602827aa4f25568d5d80cb Mon Sep 17 00:00:00 2001 From: basameera Date: Sun, 10 May 2020 16:53:26 +0200 Subject: [PATCH 12/19] BFB mesure, motion models are ready --- MyCode/Bayes Filter/bayes_filter_viz.py | 31 ++++++++++++++++++++----- 1 file changed, 25 insertions(+), 6 deletions(-) diff --git a/MyCode/Bayes Filter/bayes_filter_viz.py b/MyCode/Bayes Filter/bayes_filter_viz.py index e1366294bf..7efd916f96 100644 --- a/MyCode/Bayes Filter/bayes_filter_viz.py +++ b/MyCode/Bayes Filter/bayes_filter_viz.py @@ -6,7 +6,6 @@ from matplotlib.offsetbox import TextArea, DrawingArea, OffsetImage, AnnotationBbox - class BayesFilterBoat(object): def __init__(self): @@ -18,22 +17,37 @@ def main(self): self._draw_main_window() def _draw_main_window(self): - # self.fig, self.axs = plt.subplots(1, 3, figsize=(12, 4)) - - self.fig = plt.figure(figsize=(10, 8)) + self.fig = plt.figure(figsize=(10, 8), constrained_layout=False) gs = self.fig.add_gridspec(2, 2) # sim axis self.ax_sim = self.fig.add_subplot(gs[0, :]) self.ax_sim.set_title('Simulation') + self.set_ylim(self.ax_sim) + self.ax_sim.set_xlim(-1, 11) # Mesurement model self.ax_me = self.fig.add_subplot(gs[1, 0]) - self.ax_me.set_title('Mesurement Model') + + self.me_y = np.ones(shape=(3,))/3 + self.me_x = np.arange(len(self.me_y)) - 1 + + self.ax_me.set_title(r'Mesurement model $p(Z_t|X_t)$') + bar_plot(self.ax_me, self.me_x, self.me_y, color='r') + self.set_ylim(self.ax_me) # Motion model self.ax_mo = self.fig.add_subplot(gs[1, 1]) - self.ax_mo.set_title('Motion Model') + self.ax_mo.set_title(r'Motion model $p(X_{t+1}|X_t)$') + + self.mo_p = [0, 0.75, 0.25] + self.mo_y = np.array(self.mo_p) + self.mo_x = np.arange(len(self.mo_y)) + + bar_plot(self.ax_mo, self.mo_x, self.mo_y, color='b') + self.ax_mo.axvline(0.25, c='k', ls='--') + self.ax_mo.axvline(-0.25, c='k', ls='--') + self.set_ylim(self.ax_mo) line, = self.ax_sim.plot([0], [0]) self.cid = line.figure.canvas.mpl_connect('button_press_event', self) @@ -42,6 +56,11 @@ def _draw_main_window(self): self.ys = list(line.get_ydata()) plt.show() + def set_ylim(self, ax): + ax.set_ylim(0, 1) + ax.set_axisbelow(True) + ax.grid(ls='--') + def __call__(self, event): # print('click', event) if event.inaxes != self.line.axes: From 8533b007bde3464990ce8168531884c302951a5c Mon Sep 17 00:00:00 2001 From: basameera Date: Sun, 10 May 2020 17:20:18 +0200 Subject: [PATCH 13/19] sim ready for logic --- MyCode/Bayes Filter/bayes_filter_viz.py | 70 ++++++++++++++++++++----- 1 file changed, 57 insertions(+), 13 deletions(-) diff --git a/MyCode/Bayes Filter/bayes_filter_viz.py b/MyCode/Bayes Filter/bayes_filter_viz.py index 7efd916f96..80e891677e 100644 --- a/MyCode/Bayes Filter/bayes_filter_viz.py +++ b/MyCode/Bayes Filter/bayes_filter_viz.py @@ -18,13 +18,25 @@ def main(self): def _draw_main_window(self): self.fig = plt.figure(figsize=(10, 8), constrained_layout=False) + self.fig.suptitle('Bayes Filter - Boat simulation', fontsize=13) gs = self.fig.add_gridspec(2, 2) # sim axis self.ax_sim = self.fig.add_subplot(gs[0, :]) - self.ax_sim.set_title('Simulation') - self.set_ylim(self.ax_sim) - self.ax_sim.set_xlim(-1, 11) + self.ax_sim.set_title('Simulation', fontsize=10) + + # Initial p(x) + self.sim_p = [0.6, 0.5] + self.sim_y = np.array(self.sim_p) + self.sim_y = np.concatenate( + (self.sim_y, np.zeros(self.simulation_scope-len(self.sim_p),))) + self.sim_x = np.arange(self.simulation_scope) + + bar_plot(self.ax_sim, self.sim_x, self.sim_y, + color='tab:blue', label='Init') + self.draw_boat(self.ax_sim) + + self.setup_ax_sim() # Mesurement model self.ax_me = self.fig.add_subplot(gs[1, 0]) @@ -32,13 +44,13 @@ def _draw_main_window(self): self.me_y = np.ones(shape=(3,))/3 self.me_x = np.arange(len(self.me_y)) - 1 - self.ax_me.set_title(r'Mesurement model $p(Z_t|X_t)$') + self.ax_me.set_title(r'Mesurement model $p(Z_t|X_t)$', fontsize=10) bar_plot(self.ax_me, self.me_x, self.me_y, color='r') self.set_ylim(self.ax_me) # Motion model self.ax_mo = self.fig.add_subplot(gs[1, 1]) - self.ax_mo.set_title(r'Motion model $p(X_{t+1}|X_t)$') + self.ax_mo.set_title(r'Motion model $p(X_{t+1}|X_t)$', fontsize=10) self.mo_p = [0, 0.75, 0.25] self.mo_y = np.array(self.mo_p) @@ -50,25 +62,57 @@ def _draw_main_window(self): self.set_ylim(self.ax_mo) line, = self.ax_sim.plot([0], [0]) - self.cid = line.figure.canvas.mpl_connect('button_press_event', self) + self.cid = line.figure.canvas.mpl_connect('key_release_event', self) self.line = line self.xs = list(line.get_xdata()) self.ys = list(line.get_ydata()) plt.show() + def draw_boat(self, ax, x=0): + imagebox = OffsetImage(self.boat_img, zoom=0.2) + ab = AnnotationBbox(imagebox, (x, 0.8), frameon=False) + ax.add_artist(ab) + def set_ylim(self, ax): ax.set_ylim(0, 1) ax.set_axisbelow(True) ax.grid(ls='--') + def get_color_label(self): + if self.click_counter % 2 == 0: + color = 'tab:blue' + label = 'Mesurement' + else: + color = 'tab:orange' + label = 'Motion' + return color, label + + def setup_ax_sim(self): + self.ax_sim.set_xlim(-1, 11) + self.ax_sim.legend() + self.set_ylim(self.ax_sim) + def __call__(self, event): - # print('click', event) - if event.inaxes != self.line.axes: - return - self.xs.append(event.xdata) - self.ys.append(event.ydata) - self.line.set_data(self.xs, self.ys) - self.line.figure.canvas.draw() + + self.click_counter += 1 + if event.key == 'escape': + exit(0) + elif event.key == 'right': + ax = self.ax_sim + ax.clear() + data = np.random.random((10,)) + c, l = self.get_color_label() + p = [0.5, 0.5, 1] + y = norm(p) + y = np.concatenate((y, np.zeros(self.simulation_scope-len(p),))) + x = np.arange(self.simulation_scope) + + bar_plot(ax, x, y, color=c, label=l) + self.draw_boat(ax, 2) + + self.setup_ax_sim() + + self.fig.canvas.draw() if __name__ == "__main__": From 4d0b0eaa3247ce222312b6f2919a13d8e67feb44 Mon Sep 17 00:00:00 2001 From: basameera Date: Sun, 10 May 2020 18:24:30 +0200 Subject: [PATCH 14/19] mesurement part ready --- MyCode/Bayes Filter/bayes_filter_viz.py | 81 +++++++++++++++++++------ 1 file changed, 61 insertions(+), 20 deletions(-) diff --git a/MyCode/Bayes Filter/bayes_filter_viz.py b/MyCode/Bayes Filter/bayes_filter_viz.py index 80e891677e..167095d52d 100644 --- a/MyCode/Bayes Filter/bayes_filter_viz.py +++ b/MyCode/Bayes Filter/bayes_filter_viz.py @@ -9,9 +9,19 @@ class BayesFilterBoat(object): def __init__(self): + np.random.seed(123) self.boat_img = mpimg.imread('/home/sameera/Downloads/sailboat.png') self.click_counter = 0 self.simulation_scope = 10 + self.boat_pos = 0 + + self.SS_INIT = 0 # INIT + self.SS_READ_ME = 1 # read mesurement + self.SS_FINISH_ME = 2 # finish mesurement + self.SS_MOTION = 3 # motion + + self.sim_state = self.SS_READ_ME + self.run = True def main(self): self._draw_main_window() @@ -23,19 +33,20 @@ def _draw_main_window(self): # sim axis self.ax_sim = self.fig.add_subplot(gs[0, :]) - self.ax_sim.set_title('Simulation', fontsize=10) # Initial p(x) - self.sim_p = [0.6, 0.5] - self.sim_y = np.array(self.sim_p) + self.sim_p = [1, 2, 1] + self.sim_y = norm(self.sim_p) self.sim_y = np.concatenate( (self.sim_y, np.zeros(self.simulation_scope-len(self.sim_p),))) self.sim_x = np.arange(self.simulation_scope) + self.boat_pos = np.argmax(self.sim_y) + print('boat pos:', self.boat_pos) + bar_plot(self.ax_sim, self.sim_x, self.sim_y, color='tab:blue', label='Init') - self.draw_boat(self.ax_sim) - + self.draw_boat(self.ax_sim, self.boat_pos) self.setup_ax_sim() # Mesurement model @@ -70,7 +81,7 @@ def _draw_main_window(self): def draw_boat(self, ax, x=0): imagebox = OffsetImage(self.boat_img, zoom=0.2) - ab = AnnotationBbox(imagebox, (x, 0.8), frameon=False) + ab = AnnotationBbox(imagebox, (x, 1.0), frameon=False) ax.add_artist(ab) def set_ylim(self, ax): @@ -80,40 +91,70 @@ def set_ylim(self, ax): def get_color_label(self): if self.click_counter % 2 == 0: - color = 'tab:blue' + color = 'tab:orange' label = 'Mesurement' else: - color = 'tab:orange' + color = 'tab:blue' label = 'Motion' return color, label def setup_ax_sim(self): self.ax_sim.set_xlim(-1, 11) self.ax_sim.legend() - self.set_ylim(self.ax_sim) + self.ax_sim.set_ylim(0, 1.2) + self.ax_sim.set_axisbelow(True) + self.ax_sim.grid(ls='--') + self.ax_sim.set_title('Simulation', fontsize=10) + + def read_mesurement(self): + """NOTE: This need to change for better sim. + At the moment, same mesurement model was used for reading mesurement. + + This give how much of a position change does happen from the current boat position + + """ + return np.random.choice(self.me_x, p=self.me_y) + + def get_me_prob(self, pred_boat_pos): + new_me_y = np.zeros_like(self.sim_y) + for n, me_x in enumerate(self.me_x): + idx = pred_boat_pos + me_x + if idx >= 0 and idx < self.simulation_scope: + new_me_y[idx] = self.me_y[n] + return new_me_y def __call__(self, event): - self.click_counter += 1 if event.key == 'escape': exit(0) elif event.key == 'right': ax = self.ax_sim ax.clear() - data = np.random.random((10,)) - c, l = self.get_color_label() - p = [0.5, 0.5, 1] - y = norm(p) - y = np.concatenate((y, np.zeros(self.simulation_scope-len(p),))) - x = np.arange(self.simulation_scope) - - bar_plot(ax, x, y, color=c, label=l) - self.draw_boat(ax, 2) - self.setup_ax_sim() + if self.click_counter % 2 == 0: + # read ME + # add the positoin change to boat position to get the new boat position + pred_boat_pos = self.read_mesurement() + self.boat_pos + print('predicted boat pos:', pred_boat_pos) + Z = self.get_me_prob(pred_boat_pos) + # multiply position prob. with sensor pos prob. to get the predicted boat prob. + pred_sim_y = self.sim_y * Z + # normamlize + self.sim_y = norm(pred_sim_y) + self.boat_pos = np.argmax(self.sim_y) + c, l = self.get_color_label() + bar_plot(self.ax_sim, self.sim_x, self.sim_y, + color=c, label=l) + self.draw_boat(self.ax_sim, self.boat_pos) + self.setup_ax_sim() + + else: + qwe = 'Motion' self.fig.canvas.draw() + self.click_counter += 1 + if __name__ == "__main__": From cacbc29565589db905b3c202016bc27a5abf5299 Mon Sep 17 00:00:00 2001 From: basameera Date: Sun, 10 May 2020 18:47:08 +0200 Subject: [PATCH 15/19] BFB sim first version done --- MyCode/Bayes Filter/bayes_filter_viz.py | 35 ++++++++++++++++++++++--- 1 file changed, 31 insertions(+), 4 deletions(-) diff --git a/MyCode/Bayes Filter/bayes_filter_viz.py b/MyCode/Bayes Filter/bayes_filter_viz.py index 167095d52d..740478ed34 100644 --- a/MyCode/Bayes Filter/bayes_filter_viz.py +++ b/MyCode/Bayes Filter/bayes_filter_viz.py @@ -118,11 +118,19 @@ def read_mesurement(self): def get_me_prob(self, pred_boat_pos): new_me_y = np.zeros_like(self.sim_y) for n, me_x in enumerate(self.me_x): - idx = pred_boat_pos + me_x - if idx >= 0 and idx < self.simulation_scope: - new_me_y[idx] = self.me_y[n] + k = pred_boat_pos + me_x + if k >= 0 and k < self.simulation_scope: + new_me_y[k] = self.me_y[n] return new_me_y + def get_mo_prob(self, pos): + new_mo_y = np.zeros_like(self.sim_y) + for n, mo_x in enumerate(self.mo_x): + k = pos + mo_x + if k >= 0 and k < self.simulation_scope: + new_mo_y[k] = self.mo_y[n] + return new_mo_y + def __call__(self, event): if event.key == 'escape': @@ -141,6 +149,7 @@ def __call__(self, event): pred_sim_y = self.sim_y * Z # normamlize self.sim_y = norm(pred_sim_y) + # self.pre_boat_pos = self.boat_pos self.boat_pos = np.argmax(self.sim_y) c, l = self.get_color_label() bar_plot(self.ax_sim, self.sim_x, self.sim_y, @@ -149,7 +158,25 @@ def __call__(self, event): self.setup_ax_sim() else: - qwe = 'Motion' + idx = np.where(self.sim_y != 0)[0] + + array = np.zeros(shape=(len(idx), len(self.sim_y))) + + for n, i in enumerate(idx): + array[n] = self.get_mo_prob(i) * self.sim_y[i] + + pred_sim_y = np.sum(array, axis=0) + print(pred_sim_y) + + self.sim_y = norm(pred_sim_y) + print(self.sim_y) + # self.pre_boat_pos = self.boat_pos + self.boat_pos = np.argmax(self.sim_y) + c, l = self.get_color_label() + bar_plot(self.ax_sim, self.sim_x, self.sim_y, + color=c, label=l) + self.draw_boat(self.ax_sim, self.boat_pos) + self.setup_ax_sim() self.fig.canvas.draw() From a87e3e592e94b43a95aae6dfc67ce4a45584f02f Mon Sep 17 00:00:00 2001 From: basameera Date: Mon, 25 May 2020 13:13:51 +0200 Subject: [PATCH 16/19] ideal sensor model --- MyCode/Bayes Filter/sensor_model.py | 59 +++++++++++++++++++++++++++++ 1 file changed, 59 insertions(+) create mode 100644 MyCode/Bayes Filter/sensor_model.py diff --git a/MyCode/Bayes Filter/sensor_model.py b/MyCode/Bayes Filter/sensor_model.py new file mode 100644 index 0000000000..9ba8aad78a --- /dev/null +++ b/MyCode/Bayes Filter/sensor_model.py @@ -0,0 +1,59 @@ +import numpy as np +import matplotlib.pyplot as plt + +if __name__ == "__main__": + np.random.seed(123) + + # ** Model the sensor ** + + mu, sigma = 100, 1 # mean and standard deviation + + factor = 1 + sigma = sigma / factor + + s = np.random.normal(mu, sigma, 200) + + bins = np.arange(-3, 3 + 2) - 0.5 + bins = bins/factor + bins += mu + print(bins) + + x_pos = np.linspace(-3, 3, 100) + x_pos = x_pos/factor + x_pos += mu + + # dual + + fig, ax1 = plt.subplots() + plt.title('Ideal model') + + color = 'blue' + ax1.set_axisbelow(True) + ax1.grid(axis='x') + ax1.set_xlabel('Sample (cm)') + ax1.set_ylabel('Frequency', color=color) + count, bins, ignored = ax1.hist(s, bins, density=False, color=color) + ax1.tick_params(axis='y', labelcolor=color) + + ax2 = ax1.twinx() # instantiate a second axes that shares the same x-axis + # ax2.set_axisbelow(True) + ax2.grid(axis='y') + + pdf = 1/(sigma * np.sqrt(2 * np.pi)) * \ + np.exp(- (x_pos - mu)**2 / (2 * sigma**2)) + pdf /= factor + + color = 'red' + # we already handled the x-label with ax1 + ax2.set_ylabel('Probability', color=color) + ax2.plot(x_pos, pdf, color=color, + label='Ideal model P() - $N(\mu={},\sigma={})$'.format(mu, sigma)) + ax2.tick_params(axis='y', labelcolor=color) + # ax2.set_ylim(0, 41.85) + ax2.legend() + + fig.tight_layout() # otherwise the right y-label is slightly clipped + plt.show() + + # ** acquire synthetic samples from the sensor + s = np.random.normal(mu, sigma, 100) From 64169695337176d1f1ad01bb786b22f2b3297a9f Mon Sep 17 00:00:00 2001 From: basameera Date: Mon, 25 May 2020 14:06:21 +0200 Subject: [PATCH 17/19] sensor model estimate init flow ready --- MyCode/Bayes Filter/bayes_filter_viz.py | 2 +- MyCode/Bayes Filter/sensor_model.py | 77 +++++++++++++++++++++--- MyCode/Bayes Filter/sm_ideal.pdf | Bin 0 -> 19118 bytes MyCode/Bayes Filter/utils.py | 22 ++++++- 4 files changed, 90 insertions(+), 11 deletions(-) create mode 100644 MyCode/Bayes Filter/sm_ideal.pdf diff --git a/MyCode/Bayes Filter/bayes_filter_viz.py b/MyCode/Bayes Filter/bayes_filter_viz.py index 740478ed34..da9325b017 100644 --- a/MyCode/Bayes Filter/bayes_filter_viz.py +++ b/MyCode/Bayes Filter/bayes_filter_viz.py @@ -107,7 +107,7 @@ def setup_ax_sim(self): self.ax_sim.set_title('Simulation', fontsize=10) def read_mesurement(self): - """NOTE: This need to change for better sim. + r"""NOTE: This need to change for better sim. At the moment, same mesurement model was used for reading mesurement. This give how much of a position change does happen from the current boat position diff --git a/MyCode/Bayes Filter/sensor_model.py b/MyCode/Bayes Filter/sensor_model.py index 9ba8aad78a..a5327dafde 100644 --- a/MyCode/Bayes Filter/sensor_model.py +++ b/MyCode/Bayes Filter/sensor_model.py @@ -1,25 +1,37 @@ import numpy as np import matplotlib.pyplot as plt +from scipy.stats import norm +from utils import gen_pdf + +""" +# TODO: Need to learn how `norm.fit()` work -> maximum likelihood + +Automatic sensor modeling flow: +1. Move to a random position in space +2. Take 200 samples (dataset) +3. Using `norm.fit()` to estimate mu, std +4. Do steps 1. to 3. for two more different random positions in space. (Now there's three mu values and std values) +5. Get the avg. of all std values as the model parameter +6. using this `std`, do `norm.fit()` again to refine the mu estimates. + +""" if __name__ == "__main__": np.random.seed(123) # ** Model the sensor ** - mu, sigma = 100, 1 # mean and standard deviation + mu, sigma = 20, 1 # mean and standard deviation - factor = 1 - sigma = sigma / factor + sigma = sigma s = np.random.normal(mu, sigma, 200) bins = np.arange(-3, 3 + 2) - 0.5 - bins = bins/factor bins += mu print(bins) x_pos = np.linspace(-3, 3, 100) - x_pos = x_pos/factor x_pos += mu # dual @@ -41,7 +53,6 @@ pdf = 1/(sigma * np.sqrt(2 * np.pi)) * \ np.exp(- (x_pos - mu)**2 / (2 * sigma**2)) - pdf /= factor color = 'red' # we already handled the x-label with ax1 @@ -49,11 +60,59 @@ ax2.plot(x_pos, pdf, color=color, label='Ideal model P() - $N(\mu={},\sigma={})$'.format(mu, sigma)) ax2.tick_params(axis='y', labelcolor=color) - # ax2.set_ylim(0, 41.85) ax2.legend() fig.tight_layout() # otherwise the right y-label is slightly clipped - plt.show() + plt.savefig('sm_ideal.pdf') + # plt.show() + plt.close() # ** acquire synthetic samples from the sensor - s = np.random.normal(mu, sigma, 100) + n_samples = 200 + mesurement = 50 + std = sigma + sensor_data = np.random.normal(mesurement, std, n_samples) + + # set sensor resolution to 1 cm + sensor_data = np.round(sensor_data) + + bins = np.arange(-3, 3 + 2) - 0.5 + bins = bins + bins += mesurement + + bin_pos = np.arange(-3, 3+1) + mesurement + print(bin_pos) + + fig, axs = plt.subplots(1, 2) + fig.suptitle('Sensor mesurements') + ax = axs[0] + + count, bins, ignored = ax.hist(sensor_data, bins) + + print(np.min(sensor_data)) + + ax.set_title('Frequency') + + ax.set_axisbelow(True) + ax.grid() + + ax = axs[1] + prob = count/n_samples + ax.bar(bin_pos, prob) + + mu_est, sigma_est = norm.fit(sensor_data) + mu_est = np.round(mu_est) + sigma_est = np.round(sigma_est) + + x_pos = np.linspace(-3, 3, 100) + mesurement + + ax.plot(x_pos, gen_pdf(x_pos, mu_est, sigma_est), c='r', + label='Ideal model P() - $N(\mu={},\sigma={})$'.format(mu_est, sigma_est)) + + ax.legend() + ax.set_axisbelow(True) + ax.grid() + ax.set_title('Probability') + ax.set_xlabel('Z (cm)') + + plt.show() diff --git a/MyCode/Bayes Filter/sm_ideal.pdf b/MyCode/Bayes Filter/sm_ideal.pdf new file mode 100644 index 0000000000000000000000000000000000000000..f0caea3765d2ccfa79dea131d311861a37324391 GIT binary patch literal 19118 zcmeHvc|4U*^uH|O+V{1(cCy_4a&6gV-=i$qBm1=_OLi&BE@jCYTI@obl1L$2rCnKy zkSG=9H_w$mlJDo+cX_>j|9thDp65Bw%$zf4&NFl7%zLhgv4)m3N(KuPx$q2rz6FMY zBjG-d2VsheaD=hL0T+Kb8ctC34mJzJ{nq}ewRZ5aDV=qL@kkjtI0lQ8!9#qEp<)4RS~GR=_X#9A0X0H;`lUulv+HySiL@rOmb)iV z6C9!C3G}Gp;^gBD)UNO1eIURMj)Ho}6e{Q+Ky-2Nf*s23-lyl!f0^a_Y*nktrDG~* z`I0*Xs3lIA_XyO8SP+jA&x-FWJSY)o%5M4N+KsK_Tcp}caTh$>WwHrMm}I6M-;}1h zm-1w1lQ6S$FA?Lj<2$~rSikA^UtaW{@D031*EgJW^(H|RfbaXE~Ss&;G%7Pjehph*R;EP_g77|-E$cVbUMtzfA=stqgG`8UH@`-81jJ8 zZcCwgF*w!XSNlH9@^kuL*T^Qe?&-OHW4U*zeeC#zQFr@gD@H@iQhNu}nsecl|p@DKaV4J|@ycoqjv{jqaO8 zeR9g1ohMn+I!)nXQpi*awJvuW0e;<4s;>dfiANR8w+Tds)nw$(ggjPkX7k!k%}fjgPj&V!M!#mR>2DN3xWVzz=LJl2Le^x5{-P&y7uBv0PD&CbUssqTRfikG7DF zC0bqQddoy~1c#T`6qf0H98)L;><_<-$&K_%@fBliXl+CMyeBT905j4k% z1k|h{GRo_OfaboN7mBDOX?x`5_QvmswYmA>yS-^`&hXbEefd6$))M<|+V)))d8SQV zbRj-<^BJ7t=baT;iNo?-O|kr8F@lTgs_nUsEc?7Jvni1!eX}p(CI*%A=wIETzZ37L z>)!CfaDfatdEzFAwnb{R&SG?*+X?g8Lx5brjlONiaY;KzwO8g+!{Ipg%B&8%OqVmJ z$y#>N2C6Zy18r?yMtpwvas1v`!ts>|z39Cu2>W0bYa6qCOQ*?u(cuVIH+E{-_@--* z^bU5^v$S6f+@g8AMOk1rl;wSlj?GS?TXxHE#f0O8y$MTpC3f64t-FMJRDUpQ zT+X5{GQKdYa6KI1czfn`V%?5Z#*fqj@;2L#>egTPY59Du^FS_}pEy=<+bEM-K$D^_ zr}Os7EBWDwor5Oz*Pk8V5k2wnb*ou!=}^&xUSBkATpLXl7ecN@&@Xwv6Yh{Pe{u}N zU}T%{=(mT$8U=EA!rmDppD>FUvoN!$cL^Vf$ti=nyAH%u38WQWztlIKa5;M-)c^b3 z^w_Sqdy=B3eQ=Kx%U z>r{QCMYqBoWv$>^FHZ(}vBs@=Pao^Dgj{mQJ}GY(Yc#|upSgm-`OGO-c?Q-RcqYs9 zSr_*hc)Ir;jns^4M)rE<4K|jamK7ofd0t#s1+0g745dqK*7uxq_tNWDc*QgGc%RGD z$GicCfX(Ha%+7c?BZIvBkDQC?PdLwqgm&z0>G#X@a`YG+e#{%_?Rb0}b10`9t+!0W zEv4j8m7rAPfMXSTp=U?uA75Sh^6t%D1|DNKq%EOy1~IJ_X?NT_gDqCBgtoRikZOrVs89EVh?|?FybX)AipX$^ z+T6YeS)OJrCx&-7zOFuUX7CQC3xB7VpP>&_j@?-;ZQwD^$HZgBwkEXt>?6)U;4hmP~tL+2QL^8#< zDqZr`E$yM^{-E|bhrX#@w>oQQPjz!rHiMf6A*0fh==fsdB8` zzHOQo8FG)gb?0n6;EBo)CGt90VBB%sFf;fgxkqiRO-$dy6r=3(h)P8?89lUWIYwmT z$Lnq1#hulCP*m6|-c&EZ^m>nwO?Q59OlSCZF-Q$0?E1Mpa;mF<%E(FBp5X!z`Z`$tW z-s5b~i=@=Zl-G^A(TXlXjy_N$+go|n{*C;dvqjwML<#a=tSFdh3Kq$=K> zK+H?JRm}LQH6+b#KB!veDz%@Kj&f_o%M!{H{EVNeR-zmY3Md%%g|rd^o+wstUG!s| z>7kZNh%Qbh@be$zy`qO4oue616qNJX&X}vw5k)(i^jUS1H;uLEj=hoZ9)4-h6Ix3H zC#z|4-F9h(meQm!&IMv;X9i|oF|@E{kkih7?kU!x9v`*g4o`lih}_A){PqmxtY6W~ zX|4TQ{e3yINe36gyQsvD_qfw@P);Ns?bz%!^d&pgQULOu0W zzB%55gqw9Mo2jwzXt}_(x6V?brf;UwS)87J>Q(kbo$ssd$X^of_`utJqVr_9 zuCp9NNJF+vcx}Yu(S2r26???eH5Ke;=Fx&z3|KVXdD7(cnG$%&qpb=)?!BlSf7I&} z6*<`}DT?=@<)T&#$1gO=Wo^Ge7saR1c-?TzVSZt^UUid!@CG}{vu%R&E8)3^KgU{2 zUnqUKFh=Y9boPyLW~3NiE)nk>&mon%A`g&ysz@-O(3<5kUhkqhG%V zNgS=P&^yO`t1{)B*yIvhZcqh|jnTgHPUUW@m|J^W=a>BA>h{ZUHRauEEOc1ml9xQv zvSiTy@#4!u!cej3V`bIAY-+RVPgh0^yri~!e64Coz5d{{U-AzXy$FrvcN0t=bsyeR zOD_jEd@Ophy}sv_7l+5Fp~=EU(d)Uld6_4Bg-(A7<7DesOFV!2dx?_3E5XlSG*c^N zefLR-X4KCIC4GiNQ`!TPZdgLuiEE!F zQ>&gg`)76xB)_fyFtew5D!J|ewqT@SN~oHVokQ~La{;5>*IWfo(mbzOTng%VXOw{b zaFK4{zNBu7ozIeL*CX%cOYa=5C!Y0B&&*gX+jfaE_NBex?j9R5o7_>(To!XiikFjd zCFIDDM+ijkuCR#ZY}uvrXAs#ZZ-g}wmlv4Emrh;gDr{-LYxtHs7prb#iSPcA?pBi+z6@u#sC2<}T-=v}yln5f!;VpVla3Sa zV5Po^5Q56M6cIORxL$7}EVM*flS^%N-5`xZ0&%4=I20U($0Ok=Bwh|$Jp7yUk4DMC z5N3gn0i=bPmJiWuZE3Q$7Fxe9z!@u?pc0^;BE%u8S-+A`@VK83U-WrI>jPtF%YE#6 z3h_lZEI$7@mV7&3C2S%j{(Y=hLdi`jxWEIKPmZJARDLe9X9Q|P?^KIrWShHP+S!Yo zBiBlnt`;iU@!7VH>(G6>w>AMHVFXQg7lYFeTy3nq`?aP$mXp;o*(rSrQvG@}Sl!9<#v5MF2hFs6 zdcME#TNU>mtAUZ_kppi_93tW`xxMkpII#SKqHLE@-6r~7zXJUq=of>;{1^1AWU6=R zHj?J)1p9Rr5qMad(^ZW5>7o0tpC6gme=1vPZpKD0y6uKBO~F!l#GS(UwvrD#^4mt@ z7LiLQv7GWxP1F|SwU{&CKEU~*F@>6qsCli2N<7xi3_HW^YUCXR#bfpgY|E+};WulD zdB)-%uCRKNR?dQo4NnKvnG))QN~xkh;UBlvrnh8(q-SkQ#!Mh2abC*40$EJ*w=L^ z^m0?5sdh_Ud&4s8qgS6kWLZ~aBVKLGFFw$+k}!L6etXp%<_Fn8rcdA|odyL6eti@U z{w}0Q0a%6~bl4MU23FtFMvk8Det}TP!OejP1q=M)sGp$&O>eL&hGGHG>RrR#)zt-B zse?7PHC$E>jxdD2q}6c6ubj%t(8ope4}fZeBVh=L-+yK-Ad7~JzZ22jH^7GoN0H(f zt0fH_0*LOU0Due<2`$ROzh8HNyS~FuPG}qs2;|@e+VN-G)|ne5E@n;YRb5!RI|sOd zMz6+S)hA=&SmNTCz@s2&-QLu4@m}K&U;|a6A(E zzmZ`7-Fh%6IXD*dCkW}lcToy_=G#pL9!9arJad_+q z0}WrT1LOsKN%t5mP=p*Q;baM3+Q+h8Z>Z~FBb5{p@CuuP&q6d2d;8JS@+NeR#u;!o-{NP$2Gpja5h69>wWl(HrqbR}6D#0d*MAR!e&Vna=Xt{{>61!A|F zU^vi3Qb|a^|J(opSF1p;a>qeZ0m;|QV6D5>uAs|cYl4uZ!pfpRfz^J5?BF+1p*E3> z2^bVq>Q^n0P)H<<^zLT`1UvzJovR5lgjE}W?BMrA(lxLP$R^e$l085+v6e`-^3x_D zdsv?!xk%5T4#+mv64Y`$QV#SXkQK6lO%eg{AlU^Nit+2xnoX=-fh_>}urSgUx?h(_ zeZBe~%JVBhb^-M&bhZ8E)Imf3+F)l0oF(u`3V{Cf1kvjU!+-OhfRV0uk1#k{3#4&% zJsE3R{5Wh}D+?wOZ=<4oH1i%6+r2Gy_x0~bN~!BPXC>AMq_{Xb#NS5aYa+V@t+uN0 zdKxg^U*NxlC8O};PXBXeL$9CN{!Jn*;dhB@u|Fk>&)P&H`beymJEOiPqa`m{sRsS% zCXCms>facn@VMVNYo1hKr4XQ34>c?liagR9f=@qo?FipJ1sz_gB#~W3lJcLs+MJxa z?s(MKh~46xF_mXHSCLe8?A!s{v-cd(Wh1mFDRUv$y1z4*uv4 z$vr(Z`r|3h`wM5II9XFTP3mR(=sT7=2FgduxL`-)zTe zoJlp-QR2Sx=kMYI9cFJ{GQVx!&F1QiCc=4+E;T40d3Z*ju)MXz-T#}o&S7Th$Ip3# z!uF=9_NVh^y&p{ol&)@}F*wENA;?2$K;wEl@<-75+(N2DQgEGuCL@ld-I(nP2c$z% zMh;bSAtJ6NKSH0Z$hM$DH`0#VE9B6g6BlH@aaOoF@rv7|_zBlLr=P~frwJ}TSIW&@ zXt|NAL1jlfBK45<1kd3vw3q^e?%9K5l&2i7eU9@xjn{258<@7Osd{-p)Zy89b)6}i zp~ue`appdYaOiWC1Kak5Q;Rb!Nw=?lwe@7NX};eqI&kv4_>12}ne-)nblzDkbMeQt zGWzbGNS#D7aYSHFZJ8q6U6gjEZ)C(#VJ!G@e)s*>1`Pz;J&E2z$=lB~zt-j?x75nF zSGFPZtQX32kH;INpPtyM$8(u3zpWv0A}jnhlPtEJIxt!97Q^!)x4p1#-@p}xYudxX zI7O~A=97b5C%H^G#=i;}6(zjWeI2GKE?^Ui)Z%0vHS1G3QD<}Uqn+Ak#>>35w^R@6 zOw@@fJ`Ox=B;(E1&CKN7Z2ra2;${Bv{ygx>;bZZpR9bb+c z3I=t&`Eda`Wg?u`e}MYApJ=f`mMsOLIi#|B&rZToWxKvRv6BInQBGGMH7>BZziR%x ze}q~&eB~tfx8@hS?#$Q-Zlbln-F-uYiE+Kwa3(t5z+A6Ram+`Ny_UJrp=va}Qnjt0 z*52MtFSnb8kB{Y0;>}*w(L2@;lTLp*bJuq6iXz8$+RM>mCX{yZp~JK?KHS;g`mDvG!}Z0IO|L zZj{(o>S7gz>WRjJM9a9dfq?~$$%VWU+l*H5Se%aFD4R&`aDS8ammm3|-b85th`o3=3cdSj$9Qr7YT4hv>FG};)6MVstKjiRwwYK(ugeKf&7D{)JT z1C8)aUSgB|KL1vEc&+h8f{irSN(62pwYjZJI+X<7EE{`HX@V4ljLRJ zTbE;wSoA4yu4Q2TvHRnKhPJ5Pcqh$;;kiamo})ZpIL+p&l&0HaCzYBKXvSWvopKP) z#Z5JxIrpahhV2;U;ms=y*B!ICC>xRKpEBEX?gibn;uZ5VtmSihq~YmgJN1tJA&b5^ zg+SgkQlyigLn6GkIEf%{IGQ-&SlKRfW7Z-3W*MsJ7QZEx(F14WETvh|v)i~9UJfO{ z)WIINBS)v&JLCzE_v|(NGUTD%VRBp$C04>Sie~B0z2pchq-~d1fbHa<%AWM^dak_U zW@~M*iLTZbSDbj!5lNPjkF~Hp65Q*>GaPuQpXX@WI{TT!M7Y@XWbs{V*mg z(HxU8iM$E4vRK8ujbZpq-L}5bqEd^c-0v?^vpxzoJS`l0XDMQmlRR`TrAcumsbKC0 z#fh_fNyvuZ)U_&74CUn ztUebf&c;M0{VIxT>u*$a>BoSN_z8p(gE zN{3qTtI9I+Ts$SQC-noBLf00GYu{g_CHIEDl1j39`d(k`L!bLOezuF(w>y51+2yHs z#->9uL6g1wsyI!=BULuPFst@O=VFcE`YjKI*oNFhnCV$>^-Lo_9=a1U{DPx?d_lvl zSY!L(2^J;s+V-(rj_PyE7rJZs^O;0Ledu*PBq&gZ}KDWahFR`OC7gY zbyk%UGBYb@WTq=>W4^?AGrF^E3)$2JH+mCs{?>IzV}BpX3{0+gX@IZuCA-ZlvMG;{ zd^%Ix&pzsRX;N;%3lS3ML^wa+(W4)swr09MnrfkbWcx1EdyJIQ-O<5a!#L4h>x$zTMmWOPw7?PdRrPQ6?RE&$NdF;gW!!N6>6u3x!S-RUh|DBFQ+-Bz{`up3(0Eh#u?@ohcRn}HG4aMwLK_g?0 z2P*-G`&ag#A7s4NYDk{QsTsY1JE(`8X+m>&7)zYW$ZoYj+R#0jDeC%x*3r+49|*ln z5mP(o7t^n^^x5w)+4=8>w?1d;=NGivM8d!Ii9yg~y@VJ%#vB-2jX4sHH$mdDVZj$P{E|h?#@_a~O#QTml zIwY|bckc;KSkg#2mtIl)Iw-~SOHBHm>s><}O@{cTNfB=9=!imE{f`D`1LIZRj7uCQ z(te)vyE3m*efcVFi6_lyCvy^`>J`IFNBpLiU#!Tt%L%r=ag~7EVDevi|L|KV%bd3S8d-<`#)*d z%;a|8Zgn1GjCK7aF%x!Xn~Cjn=Og`2-VZ&FaD1+;uD?(^7W=fv7IT(zrh1Fe{Xs=H6|^(i2t&S)ol-Q3!g2#OAp9MVhyg zTZ@rwQ7!E+cd66~Bi(&Jn5B4J+~=6cs3^s0RVR7>BCmvbr$v|ozL}be=vVjP9RF$d z%g>@exEyuXK3G$A&qdLgH` zQucd#vm&vYwUaS?k1acJyNtAlZ{AhCaqQ+7yj%`KCuBcWA@fPDAB?@joi%Cd!t5Wz z8(NPFc}B7n8cho^1K_9T;!KXIvZ%$eSu!mF{_sjm^i{6Z;e+wrD?GNtJ7HTb_UPf`rjFWj)7)s|jP?&UlfXnuzpjk;MnFx$wks_QmHz zS#|2zY&Sv#9cE`G-?jlg{Au5vY%OP>wPC(7cm+`GN!VUv4S!H#|ELj8>|o-iznQ~! zT##PZVC%|7;-cxZStWj-qN5oBA1-|zTe#ONebrFughtD?ug5(6%H&}hzFG%A9t&`2 zukBPX8KT4~QFt_vU3zi&tE`)L08iw5w!}BsQhxQ7hfN(*6bZ*n7dElezx87_?0IWK zd)`PI4QS6hLe#ZDJhScyA6)mDuTe{AlRPWh?1e*o0%bnERJ_^T)|_H{T$7e*a^Cym zYVZAlQR9cr&VO2@-f0vR0wHDi_%p3y~-+wGM$nHXiaFmZ3JzX)OC-+`?(5YH~IVsO^ zob_S*En&qmtPr2rBdHxH6^!^?D|V_(3z1!tYv9YyexmsM#=;k{g0_rlqj zUT{*an>t-4_gr80Q9^#Xy>75*Lkgdb@=zg#-@a?GW~bxnKDYVz7PdM((D!?|f6K%2 zyR~~e;X3gGJ>kWn8L;H?t`JF%TTJPNET7_s&Th?Yjw`AdOIzOdgd9oLm=ySqUN@?e z?B>gz=fZDn5l*%>+oUu9){8-H@B)>Lni(io*JyIBho!9NZBiy{kMMPAnbHWV%%!?$ zcff2M$FT@^muzKa4M$Qx?rE8flZAYK_M9p^hmxs(RF6uiM zDORy4_=<5AQ!AVE?cIJ@ML~mO?w+3!QAyAuv}M%TgM->CYk9*IA54vdOP@t#GFSFIw+Ea~FH7G7(X3A~e#wO%-RlhYv6+Ls+ z;*D5&y6}TM6peJ+q;-GmVQpyK&zJ}nNu8z@uacFTsCG<%Yd6{L`(x3x^gVH zB+Srbwavmp!X91OjLFiCHrJWd%=9#G4qnp?yz6nQ?1HtF?SuIWsj3m~SE0eOgS!@v z4wI+$McX<=F{UUq*?&KGKL z9x}R`m1vu?v@xBR$j7Owi#m145Ou!za$!!w;8at2WmBMQ=79UXuJ$AIZkCvI zrCYd3u@44szZ59eSG=`8TSb=H;NoQGd z>{Cf5HXuS&(*J8Hv4Z_z@m(4YxK;CG>Jmwb?ZRwIiMwlw*JSq`eUW0B)^R{Yl!y7b zXpKRA>zikp^|3jp)TqX-B8LL%TE6k?TfM1{o!jZaITT}%kE@7vtv})9+;(X3mVRPf zd>gCrnea^7%IuQ^&uE3pv<-r$Z`E4kX9YBhzha~^Zgz+ZZ%Ht85{C6xN3ui_u2mnq zHcm(2Y(46dcUp?x%T46U;S+JK4|3~D9qv5Je>DGtTGKkTc#~fETW4xRCy<7z$TqO< zB;(B>f4@n^f9r;zz~1)y?$tbbj{$5vsLz^@uzuuGu1FZlfBd6r3VB*p?Tf0Q!W@$3 z!u&`4gZ2-7MQyS_@I&XFGNpF395&s<^e!dg>PHS^ccjVfxV`$@)E#``c1;}&yHfNn24Azdenia-nY~F{{?>K* zb7PTk>A4F1R5;8Q9vRe~J3&!eVy(qVgx zq*&H1_t@ErJ@(39X*->~tXQ^X*J~EyC9BK)P8_=#TKt}H%&O0abn?PoPh}m0+3rc? zj8L6Bt9>zi(aPKk-Z#y!V)pM-I5;qPlAhe-0usfU&_|>k@X2zYEqe(={jQ94OcaLDDEEi zpfb-%{4|eCTrmG>dTa-O!hJTy1Pp7&Lb!Ouem&6^@zh<6424JbZ!b>D-RiT&zptS2 z7zNkdutOEUeO!P0>CteNoot8gzpXs_67pl2g1<4+e3P#ETL%dZpbzWKOxlEyMxjb` zZ-V2nn*YFT&Arscm3OeNEiv)xaCvDz<2y#oS&}b4c3RYr%;s?448B?TvMZ2}lfcZe zyPw8s?8l)I7stGX1 zEanz*Aq(dzPR_g8y!(DR=)>37(F=M4>gU=hwrRiC(vI(Wd#L&@it#EPTGTFiz94Bz z_yp}Fhu6)PdWD&8DU_HLn~=Rn#|aJs(Wyqi|JcUGtx@7}$_G~vm>v;vWx#f@ zG;nc12$tJ;!&a(414=lng_0#`h|9w<1ltf*OaX3&IG-0hKg zr>88h&(tV$qehVKVxY+TuG8>*$=Sl3+M2${ukC$#MeQOvW$oX(C-dS0Mxx7PGS#Bm zMLy(&1%|%8Zp3rwAP+H!l12TlJ?+63P6MlY#ex*WEw5K1+GeCj3)AMi^7+2<H5dDQ`-brx*ku8hP)DR6!jNMR8bMO0Z$6>Q3!An z)CmMO0H~34B-GQz6&zG0-4aP?ODqF-H3XX+>|J{SYy+44GBK%!~ppvTqPK38RC;*~p2p7LV2TwS_ zxFNIvQVSf2g@L6!!Wbatpp&WuSvUf)bp!&y5zZhtfS>`usXMp;)}C+#z=(rN0b5@< z!ViuBC{_TWLIC*#0k9MWLxcdR)t^{d02BQOD+@UPhLr^>mc|0iAxf5j1(krkF$lQ+ zJJ1tE#Ymck$p15}?B8Kzp%3CWSlD_G|G$Kh#h^E0WdFnVZp6rf&RLI*1qX%y!Ny{N z9sPH(v1?!Y|6yZE*vkLI#{Q?+SYT=YyVzKO1cgwu|2=Fhfc=7L^?!hkMgPLZLipJK zzt~vZzhh(N0A3lwOs~VmLicN!*tP5b8a9?ReXU_*wcsQutSK0T;3NntxHE=>PJ;$U zFjJr*-x=IF0C`sL9N_@=Orkjf+%R+p`#Ft~AU+_kGeAPG=5T?7iI9|cb$%iNB0!GS z*$IGnN$*Ir89?xosv=ErAi7I>^K)JVurt!5I~*7V>CO{C#8>aUpc$4#^M+$dY+=drvK|j1enw+sDuCU+c=%1YbnV3ms{NNxWN4g^d zO!n&C>O>4qW&t{kG&h4iSyG-=M*s({qJb0vaNJKBR;P5ZceqLivpj(2lkNinav!>b zCR*IjCPNMZ06CK~0O#x{komWzISNklV*nE^G#qSfKz03-6?$Ilc2(5sPgtk|+S)w+ z>qj^cP7WX_0}3Kk(QDa&OY!q)%x2jzYuTWmwAeHw^a32F`9~V4)jy^AhX^(Q4oAoM`e+$I?Uz1@)*HFY?^&;PCvsC(8bR`&@6=V3rA_1vAI13{$U z12O&QM9}JQnyiy_y{DiKepDmC-N#!4`Uw+p4S6&YjYNWx51cr_p$U>mv=|a8CILf` z@_{2k;$98`zMeh-p6-rtj0{Q!BL#O02=J9hApUtF<3l_EihvVL&Vf$ghdtJp@pX2E zJ32Ud02^2>FR@Oshlnn&Flab|fl~`V|G=RS9QeRo;lF4QA|CpKd;g@NWbpu2yOD;L z14`SF4+DMO8)#VIWBx${e%?kJ4h(F+)4&tJm2aeBz@d|kG&Jx{H_$L>=&OSMezgMt zY@k760}Tg_O&e(FKV<=DQ#Q5@i9v%i02|6;u+X^pdwm#LaKPnv8i)h`Q5FTx-fXN7 zg+oH4z;E@T@N&?I`x_03lY>U<4K!$i+d#u30b*q%4Fiqazn8_!Lc{3qG+D^i-$>h} zy|S{<=)a+?9R3fPSRg?I$Zvd6NCG(gvVn#|frB<1Y0x3JjWj$om_vWRY!`)+ z1BatF(9qz_?;kWYG>QLS7JLeS(q!>}(g?^uXc#mAAa1A+#3}#ai^2WTZzv3b@Q41e z7-&5HT|O)htV=i2fDvt^$>Qx=i{|}Iis1N`E literal 0 HcmV?d00001 diff --git a/MyCode/Bayes Filter/utils.py b/MyCode/Bayes Filter/utils.py index e5c558f2e1..4de2c6a1e1 100644 --- a/MyCode/Bayes Filter/utils.py +++ b/MyCode/Bayes Filter/utils.py @@ -1,8 +1,28 @@ import numpy as np +def gen_pdf(x, mu=0.0, sigma=1.0): + r"""[summary] + + Parameters + ---------- + x : [type] + [description] + mu : float, optional + [description], by default 0.0 + sigma : float, optional + [description], by default 1.0 + + Returns + ------- + [type] + [description] + """ + return 1/(sigma * np.sqrt(2 * np.pi)) * np.exp(- (x - mu)**2 / (2 * sigma**2)) + + def bar_plot(ax, x, y, color=None, label=''): - """Bar Plot + r"""Bar Plot Parameters ---------- From 23e47e8523142324f72130dca3d5eb287e5be912 Mon Sep 17 00:00:00 2001 From: basameera Date: Mon, 25 May 2020 14:08:08 +0200 Subject: [PATCH 18/19] ASME readme --- MyCode/Bayes Filter/README.md | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/MyCode/Bayes Filter/README.md b/MyCode/Bayes Filter/README.md index f8fd221f8a..adccb7ca74 100644 --- a/MyCode/Bayes Filter/README.md +++ b/MyCode/Bayes Filter/README.md @@ -6,6 +6,20 @@ [Ref. 2](#reference) +## Automatic sensor model estimate (ASME) + +**TODO** + +- [ ] Need to learn how `norm.fit()` work -> maximum likelihood + +Automatic sensor modeling flow: +1. Move to a random position in space +2. Take 200 samples (dataset) +3. Using `norm.fit()` to estimate mu, std +4. Do steps 1. to 3. for two more different random positions in space. (Now there's three mu values and std values) +5. Get the avg. of all std values as the model parameter +6. using this `std`, do `norm.fit()` again to refine the mu estimates. + ### To Do * PDF as bar plot From bfd1b7cc96266e2a467b76fbfd712c2a4822e762 Mon Sep 17 00:00:00 2001 From: basameera Date: Fri, 29 May 2020 12:33:14 +0200 Subject: [PATCH 19/19] bayes filter readme - maximum likelihood --- MyCode/Bayes Filter/README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/MyCode/Bayes Filter/README.md b/MyCode/Bayes Filter/README.md index adccb7ca74..2c15ba502a 100644 --- a/MyCode/Bayes Filter/README.md +++ b/MyCode/Bayes Filter/README.md @@ -12,6 +12,9 @@ - [ ] Need to learn how `norm.fit()` work -> maximum likelihood +https://towardsdatascience.com/probability-concepts-explained-introduction-a7c0316de465 +https://towardsdatascience.com/probability-concepts-explained-maximum-likelihood-estimation-c7b4342fdbb1 + Automatic sensor modeling flow: 1. Move to a random position in space 2. Take 200 samples (dataset)