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/Bayes Filter/README.md b/MyCode/Bayes Filter/README.md
new file mode 100644
index 0000000000..2c15ba502a
--- /dev/null
+++ b/MyCode/Bayes Filter/README.md	
@@ -0,0 +1,45 @@
+# Bayes Filter
+
+[Ref. 1](#reference)
+
+## Visualization project
+
+[Ref. 2](#reference)
+
+## Automatic sensor model estimate (ASME)
+
+**TODO**
+
+- [ ] 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)
+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
+* Motion model
+* Mesurement model
+* Main window
+* Key press plot
+* Main window handling class
+* B. F. calculations
+* draw boat
+
+### 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)
+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/bayes_filter_viz.py b/MyCode/Bayes Filter/bayes_filter_viz.py
new file mode 100644
index 0000000000..da9325b017
--- /dev/null
+++ b/MyCode/Bayes Filter/bayes_filter_viz.py	
@@ -0,0 +1,189 @@
+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):
+        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()
+
+    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, :])
+
+        # Initial p(x)
+        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.boat_pos)
+        self.setup_ax_sim()
+
+        # Mesurement model
+        self.ax_me = self.fig.add_subplot(gs[1, 0])
+
+        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)$', 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)$', fontsize=10)
+
+        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('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, 1.0), 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:orange'
+            label = 'Mesurement'
+        else:
+            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.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):
+        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
+
+        """
+        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):
+            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':
+            exit(0)
+        elif event.key == 'right':
+            ax = self.ax_sim
+            ax.clear()
+
+            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.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()
+
+            else:
+                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()
+
+        self.click_counter += 1
+
+
+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()
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/sensor_model.py b/MyCode/Bayes Filter/sensor_model.py
new file mode 100644
index 0000000000..a5327dafde
--- /dev/null
+++ b/MyCode/Bayes Filter/sensor_model.py	
@@ -0,0 +1,118 @@
+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 = 20, 1  # mean and standard deviation
+
+    sigma = sigma
+
+    s = np.random.normal(mu, sigma, 200)
+
+    bins = np.arange(-3, 3 + 2) - 0.5
+    bins += mu
+    print(bins)
+
+    x_pos = np.linspace(-3, 3, 100)
+    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))
+
+    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.legend()
+
+    fig.tight_layout()  # otherwise the right y-label is slightly clipped
+    plt.savefig('sm_ideal.pdf')
+    # plt.show()
+    plt.close()
+
+    # ** acquire synthetic samples from the sensor
+    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 0000000000..f0caea3765
Binary files /dev/null and b/MyCode/Bayes Filter/sm_ideal.pdf differ
diff --git a/MyCode/Bayes Filter/test.py b/MyCode/Bayes Filter/test.py
new file mode 100644
index 0000000000..f53d5ec388
--- /dev/null
+++ b/MyCode/Bayes Filter/test.py	
@@ -0,0 +1,149 @@
+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
+
+
+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
+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():
+    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()
+            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=(8, 4))
+    global simulation_scope
+    ax = axs
+
+    cid = fig.canvas.mpl_connect('key_release_event', onclick)
+
+    # 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()
+
+
+def task_2():
+    """Main window
+    """
+
+    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
+    """
+
+    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()
+    elif task == 2:
+        task_2()
+    elif task == 3:
+        task_3()
diff --git a/MyCode/Bayes Filter/utils.py b/MyCode/Bayes Filter/utils.py
new file mode 100644
index 0000000000..4de2c6a1e1
--- /dev/null
+++ b/MyCode/Bayes Filter/utils.py	
@@ -0,0 +1,66 @@
+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=''):
+    r"""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, 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)
+
+
+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
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
new file mode 100644
index 0000000000..b6cd090d28
--- /dev/null
+++ b/MyCode/Particle_Filter/1D.py
@@ -0,0 +1,89 @@
+import numpy as np
+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 = 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/Particle_Filter/poly.py b/MyCode/Particle_Filter/poly.py
new file mode 100644
index 0000000000..02c2f602a0
--- /dev/null
+++ b/MyCode/Particle_Filter/poly.py
@@ -0,0 +1,57 @@
+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__":
+
+    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_min = abs(np.min(ly))
+    ly = ly + (ly_min*2)
+    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=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)')
+
+    plt.show()
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