particle sim ready with noise induced height

Author: basameera

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,45 +19,62 @@
     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()
             # 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])
 
-            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()

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,15 +32,24 @@
     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
     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.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)')

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