Kalman filter position

array_temp/data_preprocessing.py

@@ -0,0 +1,56 @@
+#helper class to clean data from influx and organise further
+
+
+
+
+from os import rename
+import pandas
+from sqlalchemy.testing.util import total_size
+from data_tools import query
+from data_tools.collections import TimeSeries
+from datetime import datetime, date, time, timezone
+import numpy as np
+import matplotlib.pyplot as plt
+import pandas as pd
+import dill
+import os
+import pytz
+from datetime import datetime, time, date
+
+def data_preprocessing():
+
+    def __init__(self, influx):
+        self.influx = influx
+
+    # #queries brake_pressed, accelerator position and speed
+    # def query_data(self, db, start_time, stop_time):
+    #     utc_offset_h = 7
+    #     start_utc = time(start_time)
+    #     stop_utc = time(stop_time)
+    #
+    #     date_start = date(2024, 7, 14)
+    #     date_stop = date(2024, 7, 16)
+    #
+    #     vancouver = pytz.timezone("America/Vancouver")
+    #
+    #     start_local = vancouver.localize(datetime.combine(date_start, start_utc))
+    #     stop_local = vancouver.localize(datetime.combine(date_stop, stop_utc))
+    #
+    #     start_time = start_local.astimezone(pytz.utc)
+    #     stop_time = stop_local.astimezone(pytz.utc)
+    #
+    #     client = query.DBClient()
+    #     mech_brake_pressed: TimeSeries = client.query_time_series(start_time, stop_time, field="MechBrakePressed")
+    #     accel_position: TimeSeries = client.query_time_series(start_time, stop_time, field="AcceleratorPosition")
+    #     speed_kph: TimeSeries = client.query_time_series(start_time, stop_time, "VehicleVelocity")
+
+
+    def combine_dfs(self, telemetry_names, index_common, all_dfs):
+        combined_df = pd.DataFrame(index=index_common)
+
+        for name, df in zip(telemetry_names, all_dfs):
+            #df_interp = self.resample(df, index_common)
+            combined_df[name] = pd.to_numeric(df).values
+
+        return combined_df
+

array_temp/pyproject.toml

@@ -0,0 +1,10 @@
+[project]
+name = "array-temp"
+version = "0.1.0"
+requires-python = ">=3.12"
+dependencies = [
+    "matplotlib>=3.10.8",
+    "numpy>=2.4.0",
+    "pytz>=2024.2",
+    "ubc-solar-data-tools>=1.9.1",
+]

control_model/DataPreprocessing.py

@@ -0,0 +1,191 @@
+#necessary imports
+from torch.utils.data import DataLoader
+from sklearn.preprocessing import StandardScaler
+from RNN_Dataset import RNN_Dataset
+from localization_roc import *
+from sklearn.preprocessing import MinMaxScaler
+from data_tools import query
+from data_tools import *
+import pandas as pd
+import numpy as np
+
+
+
+
+
+#this file will create a single dataframe to use for the RNN. Has multiple helper functions for:
+# scaling data
+# creating a testing/training split
+# Making individual sequences into a format feedable to the RNN.
+
+
+
+def combine_dfs(telemetry_names, index_common, all_dfs):
+    combined_df = pd.DataFrame(index=index_common)
+    combined_df.dropna()
+
+    for name, df in zip(telemetry_names, all_dfs):
+        combined_df[name] = df
+
+    return combined_df
+# get data from sunbeam and influx.
+# use sunbeam instead to save yourself a headache
+def make_df(source, event):
+    """
+    Method to query data from sunbeam, align timeseries together and make a single pandas dataframe.
+    :param source: str refers to the sunbeam data pipeline source.
+    :param event: str refers to the sunbeam data pipeline event.
+    :return: pandas dataframe consisting of queried data (Vehicle Velocity, Brake Pressed, Acceleration Position)
+    """
+    dfs = []
+    files = []
+    client = query.SunbeamClient()
+    for name in ["VehicleVelocity", "MechBrakePressed", "AcceleratorPosition"]:
+        file = client.get_file(
+            origin="production",
+            event=event,
+            source=source,
+            name=name
+        ).unwrap().data
+        files.append(file)
+
+
+    file_pos = client.get_file(
+            origin="production",
+            event=event,
+            source="localization",
+            name="TrackIndex"
+        ).unwrap().data
+
+    files = TimeSeries.align(files[0], files[1], files[2], file_pos)
+    last_idx = np.where(np.isnan(file_pos))[0][0]
+    file_pos = file_pos[0:last_idx]
+    files.append(file_pos)
+    files = TimeSeries.align(files[0], files[1], files[2], files[3]) # remember to align twice.
+    for file2 in files:
+            dfs.append(
+                pd.DataFrame(
+                    data=file2,
+                    index=file2.datetime_x_axis
+                )
+            )
+    return pd.concat(dfs).sort_index()
+
+def make_single_df():
+    day_dfs = []
+    radius_of_curvature = calculate_circular_track_curvature(coords, step=2)  # compute once
+
+    for event in ["FSGP_2024_Day_1", "FSGP_2024_Day_2", "FSGP_2024_Day_3"]:
+        speed_kph, mech_brake_pressed, accel_position, position = make_df(source="ingress", event=event)
+
+        scaler = MinMaxScaler(feature_range=(0, 1))
+        df_accel_position = pd.DataFrame(
+            scaler.fit_transform(accel_position),
+            index=accel_position.index
+        )
+
+        position_series = position.squeeze()
+        calculated_roc = position_series.map(
+            lambda pos: radius_of_curvature[int(pos) % len(radius_of_curvature)],
+            na_action='ignore'
+        )
+        calculated_roc_df = calculated_roc.to_frame(name='curvature').dropna()
+
+        day_df = pd.merge_asof(
+            mech_brake_pressed.sort_index(),
+            df_accel_position.sort_index(),
+            left_index=True,
+            right_index=True,
+            direction="nearest"
+        )
+        day_df = pd.merge_asof(
+            day_df.sort_index(),
+            speed_kph.sort_index().dropna(),
+            left_index=True,
+            right_index=True,
+            direction="nearest"
+        )
+        day_df = pd.merge_asof(
+            day_df.sort_index(),
+            calculated_roc_df.sort_index().dropna(),
+            left_index=True,
+            right_index=True,
+            direction="nearest"
+        )
+
+        day_df.columns = ["brake_pressed", "accel_position", "speed", "ROC"]
+        day_df = day_df.sort_index().ffill().dropna()
+        day_dfs.append(day_df)
+
+    final_df = pd.concat(day_dfs, axis=0).sort_index()
+    return final_df
+
+
+#given the raw dataframe, creates a testing / training split. Only training data is scaled.
+#create sequences of given length and feed to dataloaders (tensor conversions are done via class RNN_Dataset).
+# returns scaled training dataset, unscaled testing dataset, train_loader and test_loader (Dataloaders for iterating over the dataset and can return batches of samples).
+def make_sequence_datasets(
+    df_xy,
+    state_cols,
+    control_cols,
+    seq_len,
+    stride=50,
+    train_frac=0.8,
+    batch_size=64,
+):
+
+
+    cols_to_scale = state_cols + control_cols
+
+    # Train/test split (time-series safe)
+    n_total = len(df_xy)
+    train_len = int(train_frac * n_total)
+    df_xy = df_xy.dropna(subset=state_cols + control_cols).reset_index(drop=True)
+
+    df_train_raw = df_xy.iloc[:train_len].reset_index(drop=True)
+    df_test_raw  = df_xy.iloc[train_len:].reset_index(drop=True)
+
+    # Fit scaler only on training data
+    scaler = StandardScaler()
+    scaler.fit(df_train_raw[cols_to_scale])
+
+    # Apply scaling
+    df_train = df_train_raw.copy()
+    df_test  = df_test_raw.copy()
+
+    df_train[cols_to_scale] = scaler.transform(df_train_raw[cols_to_scale])
+    df_test[cols_to_scale]  = scaler.transform(df_test_raw[cols_to_scale])
+
+    # Create datasets
+    train_dataset = RNN_Dataset(
+        df_train,
+        state_cols,
+        control_cols,
+        seq_len,
+        stride
+    )
+
+    test_dataset = RNN_Dataset(
+        df_test,
+        state_cols,
+        control_cols,
+        seq_len,
+        stride
+    )
+
+    # DataLoaders
+    train_loader = DataLoader(
+        train_dataset,
+        batch_size=batch_size, num_workers=0,
+        shuffle=False, pin_memory = True
+    )
+
+    test_loader = DataLoader(
+        test_dataset,
+        batch_size=batch_size,num_workers=0,
+        shuffle=False, pin_memory = True
+    )
+
+    return train_dataset, test_dataset, train_loader, test_loader, scaler
+
+

control_model/RNN.py

@@ -0,0 +1,66 @@
+# necessary imports:
+import torch
+from torch import nn
+
+# Set to cuda/gpu if available, else default to cpu.
+device = torch.accelerator.current_accelerator().type if torch.accelerator.is_available() else "cpu"
+
+
+# Some general overview on RNNs:
+# - Essentially just a FNN with a non-linear output (hidden layer) that is passed onto the next. So there's an additional set of weights and biases.
+# - LSTMs (what is being defined below) is a type of RNN that is more capable of learning long-term dependencies.
+# Pytorch's LSTM module is hardcoded to follow tanh and sigmoid, unlike the RNN module which will let you choose between ReLU and tanh.
+
+class RNN(nn.Module):
+
+    def __init__(self, input_size, hidden_size, num_layers, seq_length, output_size):
+        """
+           Main class of RNN architecture. This LSTM has 2 input features (speed+curvature) and 2 output features (brake pressed and accelerator position).
+           Activation Sigmoid with two hidden layers i.e. a stacked LSTM where the second LSTM takes in the outputs of the first LSTM to compute final results.
+           Cell activation and final hidden state calculation is defaulted to tanh
+           Unidirectional LSTM.
+
+           :param input_size refers to the number of input variables
+           :param hidden_size refers to the dimension of memory inside the LSTM
+           :param num_layers refers to the number of recurrent layers
+           :param seq_length refers to the length of time sequence. In this use case, this will be a constant value of 15 seconds.
+           :param output_size refers to the number of output variables
+
+        """
+
+        # inherits from nn.Module
+        super(RNN, self).__init__()
+        self.hidden_size = hidden_size  # dim of memory inside lstm ie no of features in the hidden state that persists between timesteps
+        # a higher hidden size usually corresponds to complex dependencies
+        self.num_layers = num_layers  # stacked lstm layers
+        # lstm: long short term memory - looks at long term dependencies in sequential data
+        self.lstm = nn.LSTM(input_size, hidden_size, num_layers, dropout=0.1, batch_first=True)
+        self.dropout = nn.Dropout(0.1)
+        self.seq_length = seq_length  # no of timestamps to look at to predict the next control output
+
+        # num classes is the no of outputs predicted by the model
+        # to convert memory vector to outputs (shaping constraints)
+        self.fc = nn.Linear(hidden_size, output_size)
+
+    def forward(self, x, hidden=None):
+        """
+        Method to define the forward pass associated with the RNN.
+        :param x refers to input tensor of shape [batch_size, seq, input_size].
+        :param hidden refers to the previous hidden and cell states, defaults to None.
+        :return the output of shape [batch_size, seq,  output_size] and the updated hidden and cell state tensors.
+
+        """
+
+        if hidden is None:
+            # initialize hidden state with zeroes
+            h = torch.zeros(self.num_layers, x.size(0), self.hidden_size).to(x.device)
+            # cell state = long term memory, stores trends. Initialise with zeroes.
+            c = torch.zeros(self.num_layers, x.size(0), self.hidden_size).to(x.device)
+            # hidden = short term memory, current output of LSTM at a given time
+            # h and c are internal memory vectors
+            hidden = (h, c)
+        out, (h, c) = self.lstm(x, hidden)
+        out = self.dropout(
+            out)  # Prevents overfitting by randomly zeroing out elements of the input tensor with probability p during training.
+        out = self.fc(out)  # index the hidden state of the last time stamp.
+        return out, (h,c)  # return output of shape [batch_size, seq_length, hidden_size] and indexed hidden state of last timestep.

control_model/RNN_Dataset.py

@@ -0,0 +1,46 @@
+import os
+import torch
+from torch import nn
+from torch.utils.data import Dataset, TensorDataset, DataLoader
+
+
+class RNN_Dataset(torch.utils.data.Dataset):
+    """
+
+    This class creates a dataset for the Neural Network, specifically a Seq2Seq model. We encode an input sequence and generate a corresponding output sequence.
+    Sequence generation via sliding window  ( sequence of consecutive timesteps as input, the target is the value following the window).
+    Create sequences from data (seq_len timestamps as input - the next timestamp is the target)
+    Stride as an argument is used to control the overlap between input windows.
+
+
+    """
+
+    def __init__(self, df, state_cols, control_cols, seq_len, stride):
+        self.seq_len = seq_len  # length of input sequences
+        # Convert directly to tensors
+        self.states = torch.tensor(df[state_cols].values, dtype=torch.float32)
+        self.controls = torch.tensor(df[control_cols].values, dtype=torch.float32)
+        self.stride = stride  # the step between the start o consecutive sequences - to reduce overlapping between sequences being fed to the network.
+        self.total_size = self.states.size(0)  # total number of timestamps
+
+        # compute all possible start indices
+        self.indices = list(range(0, self.total_size - self.seq_len,
+                                  self.stride))  # first seq starts at t0, second at t0+stride, next at t0 + 2*stride, etc
+
+        # the target timestamp is: i+seq_len, so the input is from i:i+seq_len, so i<total_size - seq_len
+
+    def __len__(self):
+        return len(self.indices)
+
+    def __getitem__(self, index):
+        idx = self.indices[index]  # index of the first timestep of seq
+
+        # input: current states
+        current_state = self.states[idx:idx + self.seq_len]  # speed+ position
+        # target_state = self.states[idx +  self.seq_len] # state at t + seq_len
+        # current_control = self.controls[idx] #mech_brake + accelerator position
+        x_seq = torch.cat([current_state], dim=0)  # concatenate along feature dimension
+        # output: controls
+        y_seq = self.controls[idx:idx + self.seq_len]
+
+        return x_seq, y_seq