setUP_simulation

Publication/src/AUVSimulator/AUVSimulator.py

@@ -21,14 +21,18 @@ class AUVSimulator:
     __loc = np.array([0, 0])
     __loc_prev = np.array([0, 0])
     __speed = 1.0
-    __ctd_data = np.empty([0, 4])
     __arrival = False
     __popup = False
 
     def __init__(self, random_seed: int = 0, sigma: float = 1.,
                  loc_start: np.ndarray = np.array([0, 0]), temporal_truth: bool = True) -> None:
         self.temporal_truth = temporal_truth
-        print("Temporal truth is on.") if self.temporal_truth else print("Temporal truth is off.")
+        if self.temporal_truth:
+            self.__ctd_data = np.empty([0, 4])
+            print("Temporal truth is ON.")
+        else:
+            self.__ctd_data = np.empty([0, 3])
+            print("Temporal truth is OFF.")
         self.ctd = CTDSimulator(random_seed=random_seed, sigma=sigma)
         self.messenger = Messenger()
         self.__loc = loc_start
@@ -112,9 +116,10 @@ def __collect_data(self) -> None:
             loc = np.stack((x_path, y_path), axis=1)
             if self.temporal_truth:
                 sal = self.ctd.get_salinity_at_dt_loc(dt, loc)
+                self.__ctd_data = np.stack((timestamp, x_path, y_path, sal), axis=1)
             else:
-                raise NotImplementedError
-            self.__ctd_data = np.stack((timestamp, x_path, y_path, sal), axis=1)
+                sal = self.ctd.get_salinity_at_loc(loc)
+                self.__ctd_data = np.stack((x_path, y_path, sal), axis=1)
 
     def arrive(self):
         self.__arrival = True

Publication/src/AUVSimulator/CTDSimulator.py

@@ -87,14 +87,31 @@ def get_salinity_at_dt_loc(self, dt: float, loc: np.ndarray) -> Union[np.ndarray
         self.timestamp += dt
         print("Current datetime: ", datetime.fromtimestamp(self.timestamp).strftime("%Y-%m-%d %H:%M:%S"))
         ts = np.array([self.timestamp])
-        dist, ind_time = self.timestamp_sinmod_tree.query(ts)
+        *_, ind_time = self.timestamp_sinmod_tree.query(ts)
         t1 = time()
         sorted_salinity = self.mu_truth[ind_time, :].flatten()
         # sorted_salinity = self.salinity_sinmod[ind_time, :, :].flatten()
         dist, ind_loc = self.grid_sinmod_tree.query(loc)
         print("Query salinity at timestamp and location takes: ", time() - t1)
         return sorted_salinity[ind_loc]
 
+    def get_salinity_at_loc(self, loc: np.ndarray) -> Union[np.ndarray, None]:
+        """
+        Get CTD measurement at given locations.
+
+        Args:
+            loc: np.array([x, y])
+
+        Returns:
+            salinity: np.array([salinity])
+        """
+        t0 = time()
+        print("Current Date: ", datetime.fromtimestamp(self.timestamp).strftime("%Y-%m-%d"))
+        sorted_salinity = np.mean(self.mu_truth, axis=0)
+        *_, ind_loc = self.grid_sinmod_tree.query(loc)
+        print("Query salinity at location takes: ", time() - t0)
+        return sorted_salinity[ind_loc]
+
 
 if __name__ == "__main__":
     c = CTDSimulator()

Publication/src/Agents/AgentRRTStar.py

@@ -21,7 +21,7 @@ class Agent:
     Agent
     """
     def __init__(self, neighbour_distance: float = 120, weight_eibv: float = 1., weight_ivr: float = 1.,
-                 sigma: float = .1, nugget: float = .01, random_seed: int = 1, debug=False, name: str = "Equal",
+                 sigma: float = .1, nugget: float = .01, random_seed: int = 1, debug: bool = False, name: str = "Equal",
                  budget_mode: bool = False, approximate_eibv: bool = False, fast_eibv: bool = True) -> None:
         """
         Set up the planning strategies and the AUV simulator for the operation.

Publication/src/Config.py

@@ -52,6 +52,9 @@ def __init__(self) -> None:
         x, y = WGS.latlon2xy(self.__wgs_loc_end[0], self.__wgs_loc_end[1])
         self.__loc_end = np.array([x, y])
 
+        """ Budget mode """
+        self.__budget_mode = False
+
         """ Default simulation parameter seteup. """
         self.__num_steps = 10  # number of steps.
         self.__num_replicates = 3  # number of replicates
@@ -113,6 +116,10 @@ def set_num_cores(self, value: int) -> None:
         """ Set the number of cores to use in the simulation study. """
         self.__num_cores = value
 
+    def set_budget_mode(self, value: bool) -> None:
+        """ Set the budget mode to be True or False. """
+        self.__budget_mode = value
+
     def get_polygon_border(self) -> np.ndarray:
         """ Return polygon for opa in x y coordinates. """
         return self.__polygon_border
@@ -157,6 +164,10 @@ def get_num_cores(self) -> int:
         """ Return the number of cores in the simulation study. """
         return self.__num_cores
 
+    def get_budget_mode(self) -> bool:
+        """ Return the budget mode. """
+        return self.__budget_mode
+
     def get_wgs_polygon_border(self) -> np.ndarray:
         """ Return polygon for the oprational area in wgs coordinates. """
         return self.__wgs_polygon_border

Publication/src/CostValley/CostValley.py

@@ -27,13 +27,14 @@ class CostValley:
     """ Cost fields construction. """
     def __init__(self, weight_eibv: float = 1., weight_ivr: float = 1., sigma: float = 1., nugget: float = .4,
                  budget_mode: bool = False, approximate_eibv: bool = False, fast_eibv: bool = True) -> None:
-        """ """
+
+        """ Budget mode """
+        self.__budget_mode = budget_mode
 
         """ GRF """
         self.__grf = GRF(sigma=sigma, nugget=nugget, approximate_eibv=approximate_eibv, fast_eibv=fast_eibv)
         self.__field = self.__grf.field
         self.__grid = self.__field.get_grid()
-        self.__budget_mode = budget_mode
 
         """ Weights """
         self.__weight_eibv = weight_eibv

Publication/src/Field.py

@@ -14,6 +14,7 @@
 from scipy.spatial.distance import cdist
 from math import cos, sin, radians
 from typing import Union
+from pykdtree.kdtree import KDTree
 
 
 class Field:
@@ -48,6 +49,7 @@ def __init__(self, neighbour_distance: float = 120):
         # grid element
         self.__grid = np.empty([0, 2])
         self.__construct_grid()
+        self.__grid_tree = KDTree(self.__grid)
 
         # neighbour element
         self.__neighbour_hash_table = dict()
@@ -158,17 +160,20 @@ def get_ind_from_location(self, location: np.ndarray) -> Union[np.ndarray, None]
         """
         Args:
             location: np.array([xp, yp])
+
         Returns: index of the closest waypoint.
+            ind: np.array([ind])
         """
-
         if len(location) > 0:
             dm = location.ndim
             if dm == 1:
-                d = cdist(self.__grid, location.reshape(1, -1))
-                return np.argmin(d, axis=0)
+                return self.__grid_tree.query(location.reshape(1, -1))[1]
+                # d = cdist(self.__grid, location.reshape(1, -1))
+                # return np.argmin(d, axis=0)
             elif dm == 2:
-                d = cdist(self.__grid, location)
-                return np.argmin(d, axis=0)
+                return self.__grid_tree.query(location)[1]
+                # d = cdist(self.__grid, location)
+                # return np.argmin(d, axis=0)
             else:
                 return None
         else:

Publication/src/GRF/GRF.py

@@ -20,6 +20,7 @@
 from numba import jit
 from joblib import Parallel, delayed
 from pykdtree.kdtree import KDTree
+from datetime import datetime
 import time
 import pandas as pd
 import os
@@ -31,7 +32,7 @@ class GRF:
     """
     def __init__(self, sigma: float = 1., nugget: float = .4,
                  approximate_eibv: bool = False, fast_eibv: bool = True,
-                 filepath_prior: str = os.getcwd() + "/../sinmod/samples_2022.05.10.nc") -> None:
+                 filepath_prior: str = os.getcwd() + "/../sinmod/samples_2022.05.11.nc") -> None:
         """ Initializes the parameters in GRF kernel. """
         self.__approximate_eibv = approximate_eibv
         self.__fast_eibv = fast_eibv
@@ -79,13 +80,21 @@ def __init__(self, sigma: float = 1., nugget: float = .4,
         self.__yg = vectorize(self.grid[:, 1])
         self.__distance_matrix = None
         self.__construct_grf_field()
+        self.__Sigma_prior = self.__Sigma
 
         # s1: update prior mean
+        datestring = filepath_prior.split("/")[-1].split("_")[-1][:-3].replace('.', '-') + " 10:00:00"
+        timestamp_prior = np.array([datetime.strptime(datestring, "%Y-%m-%d %H:%M:%S").timestamp()])
         self.__sinmod = SINMOD(filepath_prior)
         self.__salinity_sinmod = self.__sinmod.get_salinity()[:, 0, :, :]
-        self.__construct_prior_mean()
-        self.__mu_prior = self.__mu
-        self.__Sigma_prior = self.__Sigma
+        x, y, *_ = self.__sinmod.get_coordinates()
+        self.__grid_sinmod = np.stack((x.flatten(), y.flatten()), axis=1)
+        self.__grid_sinmod_tree = KDTree(self.__grid_sinmod)
+        *_, self.__ind_sinmod4grid = self.__grid_sinmod_tree.query(self.grid)
+        self.__timestamp_sinmod = self.__sinmod.get_timestamp()
+        self.__timestamp_sinmod_tree = KDTree(self.__timestamp_sinmod)
+        *_, ind_prior_time = self.__timestamp_sinmod_tree.query(timestamp_prior)
+        self.__mu = self.__salinity_sinmod[ind_prior_time, :, :].flatten()[self.__ind_sinmod4grid].reshape(-1, 1)
 
         # s2: load cdf table
         self.__load_cdf_table()
@@ -96,16 +105,6 @@ def __construct_grf_field(self) -> None:
         self.__Sigma = self.__sigma ** 2 * ((1 + self.__eta * self.__distance_matrix) *
                                             np.exp(-self.__eta * self.__distance_matrix))
 
-    def __construct_prior_mean(self) -> None:
-        # dataset_sinmod = pd.read_csv("./../prior/sinmod.csv").to_numpy()
-        # grid_sinmod = dataset_sinmod[:, :2]
-        # sal_sinmod = dataset_sinmod[:, -1]
-
-        # s1: interpolate onto grid.
-        # dm_grid_sinmod = cdist(self.grid, grid_sinmod)
-        # ind_close = np.argmin(dm_grid_sinmod, axis=1)
-        # self.__mu = vectorize(sal_sinmod[ind_close])
-
     def __load_cdf_table(self) -> None:
         """
         Load cdf table for the analytical solution.
@@ -163,19 +162,21 @@ def assimilate_temporal_data(self, dataset: np.ndarray) -> tuple:
         t_end = dataset[-1, 0]
         t_steps = int((t_end - t_start) // self.__ar1_corr_range)
 
-        distance_min, ind_min_distance = self.grid_kdtree.query(dataset[:, 1:3])
+        *_, ind_min_distance = self.grid_kdtree.query(dataset[:, 1:3])
         ind_assimilated = np.unique(ind_min_distance)
         salinity_assimilated = np.zeros([len(ind_assimilated), 1])
         for i in range(len(ind_assimilated)):
             ind_selected = np.where(ind_min_distance == ind_assimilated[i])[0]
             salinity_assimilated[i] = np.mean(dataset[ind_selected, -1])
-        self.__update_temporal(ind_measured=ind_assimilated, salinity_measured=salinity_assimilated, timestep=t_steps)
+        self.__update_temporal(ind_measured=ind_assimilated, salinity_measured=salinity_assimilated,
+                               timestep=t_steps, timestamp=np.array([t_end]))
         # t2 = time.time()
         # print("Data assimilation takes: ", t2 - t1, " seconds")
         """ Just for debugging. """
         return ind_assimilated, salinity_assimilated
 
-    def __update_temporal(self, ind_measured: np.ndarray, salinity_measured: np.ndarray, timestep=0):
+    def __update_temporal(self, ind_measured: np.ndarray, salinity_measured: np.ndarray,
+                          timestep=0, timestamp: np.ndarray = np.array([123424332])):
         """ Update GRF kernel with AR1 process.
         timestep here can only be 1, no larger than 1, if it is larger than 1, then the data assimilation needs to be
         properly adjusted to make sure that they correspond with each other.
@@ -187,14 +188,19 @@ def __update_temporal(self, ind_measured: np.ndarray, salinity_measured: np.ndar
             F[i, ind_measured[i]] = True
         R = np.eye(msamples) * self.__tau ** 2
 
+        # s1, get timestamped prior mean from SINMOD
+        *_, ind_time = self.__timestamp_sinmod_tree.query(timestamp)
+        salinity_sinmod = self.__salinity_sinmod[ind_time, :, :].flatten()
+        mu_prior = salinity_sinmod[self.__ind_sinmod4grid].reshape(-1, 1)
+
         t1 = time.time()
         # propagate
-        mt0 = self.__mu_prior + self.__ar1_coef * (self.__mu - self.__mu_prior)
+        mt0 = mu_prior + self.__ar1_coef * (self.__mu - mu_prior)
         St0 = self.__ar1_coef ** 2 * self.__Sigma + (1 - self.__ar1_coef ** 2) * self.__Sigma_prior
         mts = mt0
         Sts = St0
         for s in range(timestep):
-            mts = self.__mu_prior + self.__ar1_coef * (mts - self.__mu_prior)
+            mts = mu_prior + self.__ar1_coef * (mts - mu_prior)
             Sts = self.__ar1_coef**2 * Sts + (1 - self.__ar1_coef**2) * self.__Sigma_prior
 
         self.__mu = mts + Sts @ F.T @ np.linalg.solve(F @ Sts @ F.T + R, salinity_measured - F @ mts)

Publication/src/tests/test_agent_rrtstar.py

@@ -31,7 +31,7 @@ def setUp(self) -> None:
                             debug=debug, name="Equal", approximate_eibv=approximate_eibv, fast_eibv=fast_eibv)
 
     def test_run(self) -> None:
-        num_steps = 50
+        num_steps = 240
         self.agent1.run(num_steps)
         self.agent2.run(num_steps)
         self.agent3.run(num_steps)

Publication/src/tests/test_auv_simulator.py

@@ -23,7 +23,7 @@ def value(x, y):
 class TestAUVSimulator(TestCase):
 
     def setUp(self) -> None:
-        self.auv = AUVSimulator(random_seed=0, sigma=1., loc_start=np.array([0, 0]), temporal_truth=True)
+        self.auv = AUVSimulator(random_seed=0, sigma=1., loc_start=np.array([0, 0]), temporal_truth=False)
         self.polygon_border_wgs = pd.read_csv(os.getcwd() + "/csv/polygon_border.csv").to_numpy()
         x, y = WGS.latlon2xy(self.polygon_border_wgs[:, 0], self.polygon_border_wgs[:, 1])
         self.polygon_border = np.stack((x, y), axis=1)

Publication/src/tests/test_cost_valley.py

@@ -152,9 +152,9 @@ def test_update_cost_valley(self):
         self.cv.update_cost_valley(dataset[0, :2])
         self.plot_cost_valley()
 
-        # s4: move more and sample
-        dataset = np.array([[3000, -2000, 0, 22]])
-        self.grf.assimilate_data(dataset)
-        self.cv.update_cost_valley(dataset[0, :2])
-        self.plot_cost_valley()
+        # # s4: move more and sample
+        # dataset = np.array([[3000, -2000, 0, 22]])
+        # self.grf.assimilate_data(dataset)
+        # self.cv.update_cost_valley(dataset[0, :2])
+        # self.plot_cost_valley()
         print("End S4")

Publication/src/tests/test_grf.py

@@ -113,23 +113,23 @@ def test_prior_matern_covariance(self):
               vmin1=10, vmax1=30, vmin2=0, vmax2=self.sigma, cbar1="salinity", cbar2="std", stepsize1=1.5, threshold1=27)
         print("END S1")
 
-    def test_assimilate(self):
-        # c2: one
-        print("S2")
-        dataset = np.array([[3000, 1000, 10]])
-        self.g.assimilate_data(dataset)
-        plotf(self, v1=self.g.get_mu(), v2=np.sqrt(np.diag(self.g.get_covariance_matrix())),
-              vmin1=10, vmax1=30, vmin2=0, vmax2=self.sigma, cbar1="salinity", cbar2="std", stepsize1=1.5, threshold1=27)
-
-        # c3: multiple
-        dataset = np.array([[2000, -1000, 15],
-                            [1500, -1500, 10],
-                            [1400, -1800, 25],
-                            [2500, -1400, 20]])
-        self.g.assimilate_data(dataset)
-        plotf(self, v1=self.g.get_mu(), v2=np.sqrt(np.diag(self.g.get_covariance_matrix())),
-              vmin1=10, vmax1=30, vmin2=0, vmax2=self.sigma, cbar1="salinity", cbar2="std", stepsize1=1.5, threshold1=27)
-        print("End S2")
+    # def test_assimilate(self):
+    #     # c2: one
+    #     print("S2")
+    #     dataset = np.array([[3000, 1000, 10]])
+    #     self.g.assimilate_data(dataset)
+    #     plotf(self, v1=self.g.get_mu(), v2=np.sqrt(np.diag(self.g.get_covariance_matrix())),
+    #           vmin1=10, vmax1=30, vmin2=0, vmax2=self.sigma, cbar1="salinity", cbar2="std", stepsize1=1.5, threshold1=27)
+    #
+    #     # c3: multiple
+    #     dataset = np.array([[2000, -1000, 15],
+    #                         [1500, -1500, 10],
+    #                         [1400, -1800, 25],
+    #                         [2500, -1400, 20]])
+    #     self.g.assimilate_data(dataset)
+    #     plotf(self, v1=self.g.get_mu(), v2=np.sqrt(np.diag(self.g.get_covariance_matrix())),
+    #           vmin1=10, vmax1=30, vmin2=0, vmax2=self.sigma, cbar1="salinity", cbar2="std", stepsize1=1.5, threshold1=27)
+    #     print("End S2")
 
     def test_get_ei_field(self):
         # c1: no data assimilation