diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile
index d85a7bcec..7fc073865 100644
--- a/.devcontainer/Dockerfile
+++ b/.devcontainer/Dockerfile
@@ -1,4 +1,4 @@
-FROM ghcr.io/ubcsailbot/sailbot_workspace/dev:mongodb-cpp
+FROM ghcr.io/ubcsailbot/sailbot_workspace/dev:refactor-images-v2
 
 # Copy configuration files (e.g., .vimrc) from config/ to the container's home directory
 ARG USERNAME=ros
diff --git a/.devcontainer/base-dev/base-dev.Dockerfile b/.devcontainer/base-dev/base-dev.Dockerfile
index 5119c1bf2..ce7ebcee5 100644
--- a/.devcontainer/base-dev/base-dev.Dockerfile
+++ b/.devcontainer/base-dev/base-dev.Dockerfile
@@ -1,4 +1,4 @@
-FROM ghcr.io/ubcsailbot/sailbot_workspace/pre-base:ros_humble-ompl_4c86b2f as base
+FROM ghcr.io/ubcsailbot/sailbot_workspace/pre-base:ros_humble-ompl_4c86b2f-mongo_367-v2 as base
 
 # install base apt dependencies
 ENV DEBIAN_FRONTEND=noninteractive
@@ -70,23 +70,11 @@ RUN apt-get update \
     && apt-get install -y --no-install-recommends \
         mongodb-database-tools \
         mongodb-mongosh \
-        libmongoc-dev \
     && apt-get autoremove -y \
     && apt-get clean -y \
     && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/*
 ENV DEBIAN_FRONTEND=
 
-# setup MongoDB C++ Packages
-ENV DEBIAN_FRONTEND=noninteractive
-# mongo-cxx-driver version must match libmongoc-dev version - see https://mongocxx.org/mongocxx-v3/installation/linux/
-RUN wget https://github.com/mongodb/mongo-cxx-driver/releases/download/r3.6.7/mongo-cxx-driver-r3.6.7.tar.gz \
-    && tar -xzf mongo-cxx-driver-r3.6.7.tar.gz \
-    && cd mongo-cxx-driver-r3.6.7/build \
-    && cmake .. -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr/local \
-    && cmake --build . \
-    && cmake --build . --target  install
-ENV DEBIAN_FRONTEND=
-
 FROM local-base as ros-dev
 
 # From https://github.com/athackst/dockerfiles/blob/32a872348af0ad25ec4a6e6184cb803357acb6ab/ros2/humble.Dockerfile
diff --git a/.devcontainer/config/sailbot_workspace.code-workspace b/.devcontainer/config/sailbot_workspace.code-workspace
index 954e8fe15..2ff328e96 100644
--- a/.devcontainer/config/sailbot_workspace.code-workspace
+++ b/.devcontainer/config/sailbot_workspace.code-workspace
@@ -6,6 +6,9 @@
         {
             "path": "../../src/boat_simulator"
         },
+        {
+            "path": "../../src/controller"
+        },
         {
             "path": "../../src/custom_interfaces"
         },
@@ -18,6 +21,9 @@
         {
             "path": "../../src/network_systems"
         },
+        {
+            "path": "../../src/notebooks"
+        },
         {
             "path": "../../src/raye-local-pathfinding"
         },
@@ -125,6 +131,8 @@
             "editor.formatOnSave": true,
             "editor.rulers": [99],
         },
+        "notebook.formatOnCellExecution": true,
+        "notebook.formatOnSave.enabled": true,
         "python.analysis.diagnosticMode": "workspace",
         "python.analysis.extraPaths": [
             "/workspaces/sailbot_workspace/install/lib/python3.10/site-packages",
@@ -412,9 +420,9 @@
             // Workspace editing tasks
             {
                 "label": "clone source repositories",
-                "detail": "Clone the repositories specified in src/new_project.repos to src/",
+                "detail": "Clone the repositories specified in src/polaris.repos to src/",
                 "type": "shell",
-                "command": "vcs import < src/new_project.repos src --skip-existing",
+                "command": "vcs import < src/polaris.repos src --skip-existing",
                 "problemMatcher": []
             },
             {
diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json
index 30c9233b5..e89285d55 100644
--- a/.devcontainer/devcontainer.json
+++ b/.devcontainer/devcontainer.json
@@ -66,6 +66,7 @@
 				"ms-python.black-formatter",
 				"ms-python.isort",
 				"ms-python.vscode-pylance",
+				"ms-toolsai.jupyter",
 				"ms-vsliveshare.vsliveshare",
 				"njpwerner.autodocstring",
 				"streetsidesoftware.code-spell-checker",
diff --git a/.devcontainer/pre-base/build-pre-base.sh b/.devcontainer/pre-base/build-pre-base.sh
index 121e458f9..7cdba38e5 100755
--- a/.devcontainer/pre-base/build-pre-base.sh
+++ b/.devcontainer/pre-base/build-pre-base.sh
@@ -2,6 +2,6 @@
 
 docker buildx build . \
     --file pre-base.Dockerfile \
-    --tag ghcr.io/ubcsailbot/sailbot_workspace/pre-base:ros_humble-ompl_4c86b2f \
+    --tag ghcr.io/ubcsailbot/sailbot_workspace/pre-base:ros_humble-ompl_4c86b2f-mongo_367-v2 \
     --platform linux/arm64,linux/amd64 \
     --push
diff --git a/.devcontainer/pre-base/pre-base.Dockerfile b/.devcontainer/pre-base/pre-base.Dockerfile
index b73beb1a1..66c342830 100644
--- a/.devcontainer/pre-base/pre-base.Dockerfile
+++ b/.devcontainer/pre-base/pre-base.Dockerfile
@@ -1,4 +1,15 @@
-FROM ubuntu:22.04 AS ompl-source
+FROM ubuntu:22.04 AS fix-certificates
+
+ENV DEBIAN_FRONTEND=noninteractive
+RUN apt-get update \
+    && apt-get install -y --no-install-recommends \
+        ca-certificates \
+    && apt-get autoremove -y \
+    && apt-get clean -y \
+    && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/*
+ENV DEBIAN_FRONTEND=
+
+FROM fix-certificates AS ompl-source
 
 ENV DEBIAN_FRONTEND=noninteractive
 RUN apt-get update \
@@ -11,7 +22,7 @@ WORKDIR /ompl
 RUN git reset --hard 4c86b2f
 
 # From https://github.com/athackst/dockerfiles/blob/32a872348af0ad25ec4a6e6184cb803357acb6ab/ros2/humble.Dockerfile
-FROM ubuntu:22.04 AS ros-pre-base
+FROM fix-certificates AS ros-pre-base
 
 ENV DEBIAN_FRONTEND=noninteractive
 
@@ -55,7 +66,7 @@ ENV ROS_VERSION=2
 ENV DEBIAN_FRONTEND=
 
 # Based on https://github.com/ompl/ompl/blob/4c86b2fecf7084ae9073bf6a837176d0be169721/scripts/docker/ompl.Dockerfile
-FROM ros-pre-base AS ompl-builder
+FROM fix-certificates AS ompl-builder
 # avoid interactive configuration dialog from tzdata, which gets pulled in
 # as a dependency
 ENV DEBIAN_FRONTEND=noninteractive
@@ -99,6 +110,30 @@ RUN cmake \
     && ninja -j `nproc` \
     && ninja install
 
+FROM fix-certificates AS mongo-cxx-driver-builder
+
+ENV DEBIAN_FRONTEND=noninteractive
+RUN apt-get update \
+    && apt-get install -y --no-install-recommends \
+        build-essential \
+        cmake \
+        git \
+        libmongoc-dev \
+        wget \
+    && apt-get autoremove -y \
+    && apt-get clean -y \
+    && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/*
+
+# setup MongoDB C++ Packages
+# mongo-cxx-driver version must match libmongoc-dev version - see https://mongocxx.org/mongocxx-v3/installation/linux/
+RUN wget https://github.com/mongodb/mongo-cxx-driver/releases/download/r3.6.7/mongo-cxx-driver-r3.6.7.tar.gz \
+    && tar -xzf mongo-cxx-driver-r3.6.7.tar.gz \
+    && cd mongo-cxx-driver-r3.6.7/build \
+    && cmake .. -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr/local \
+    && cmake --build . \
+    && cmake --build . --target  install
+ENV DEBIAN_FRONTEND=
+
 FROM ros-pre-base as pre-base
 LABEL org.opencontainers.image.source = "https://github.com/UBCSailbot/sailbot_workspace"
 
@@ -115,6 +150,7 @@ RUN apt-get update \
         libboost-system-dev \
         libeigen3-dev \
         libflann-dev \
+        libmongoc-dev \
         libode-dev \
         libtriangle-dev \
         ninja-build \
@@ -129,3 +165,4 @@ RUN apt-get update \
 ENV DEBIAN_FRONTEND=
 
 COPY --from=ompl-builder /usr /usr
+COPY --from=mongo-cxx-driver-builder /usr/local /usr/local
diff --git a/.github/actions/ament-lint/run.sh b/.github/actions/ament-lint/run.sh
index 1959f196b..813997678 100755
--- a/.github/actions/ament-lint/run.sh
+++ b/.github/actions/ament-lint/run.sh
@@ -47,5 +47,5 @@ source /opt/ros/${ROS_DISTRO}/setup.bash
 cd src
 
 # Exclude repos and files we don't want to lint
-VALID_SRC_DIRS=$(ls | grep -v -e virtual_iridium -e docs -e raye-local-pathfinding -e new_project.repos)
+VALID_SRC_DIRS=$(ls | grep -v -e virtual_iridium -e docs -e raye-local-pathfinding -e polaris.repos)
 lint ${VALID_SRC_DIRS}
diff --git a/.github/workflows/build-images.yml b/.github/workflows/build-images.yml
index 163020234..15519bdd1 100644
--- a/.github/workflows/build-images.yml
+++ b/.github/workflows/build-images.yml
@@ -19,22 +19,22 @@ jobs:
 
     steps:
       - name: Set up QEMU
-        uses: docker/setup-qemu-action@v2
+        uses: docker/setup-qemu-action@v3
 
       - name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@v2
+        uses: docker/setup-buildx-action@v3
         with:
           platforms: linux/arm64,linux/amd64
 
       - name: Login to GitHub Container Registry
-        uses: docker/login-action@v2
+        uses: docker/login-action@v3
         with:
           registry: ghcr.io
           username: ${{ github.actor }}
           password: ${{ secrets.PAT_TOKEN }}
 
       - name: Build and push base
-        uses: docker/build-push-action@v4
+        uses: docker/build-push-action@v5
         with:
           context: "{{defaultContext}}:.devcontainer/base-dev"
           file: base-dev.Dockerfile
@@ -46,7 +46,7 @@ jobs:
           target: base
 
       - name: Build and push local-base
-        uses: docker/build-push-action@v4
+        uses: docker/build-push-action@v5
         with:
           context: "{{defaultContext}}:.devcontainer/base-dev"
           file: base-dev.Dockerfile
@@ -58,7 +58,7 @@ jobs:
           target: local-base
 
       - name: Build and push dev
-        uses: docker/build-push-action@v4
+        uses: docker/build-push-action@v5
         with:
           context: "{{defaultContext}}:.devcontainer/base-dev"
           file: base-dev.Dockerfile
diff --git a/.gitignore b/.gitignore
index 7af5573e6..31103c815 100644
--- a/.gitignore
+++ b/.gitignore
@@ -6,7 +6,7 @@ site/*
 # vcstool packages
 src/*
 !src/global_launch
-!src/new_project.repos
+!src/polaris.repos
 
 # configuration files
 .devcontainer/config/*
diff --git a/build.sh b/build.sh
index c6165525c..ab28ca2f8 100755
--- a/build.sh
+++ b/build.sh
@@ -41,6 +41,9 @@ else
     STATIC_ANALYSIS="ON"
     UNIT_TEST="ON"
 fi
+if [[ $UNIT_TEST = "ON" ]]; then
+    BUILD_TYPE="Debug"
+fi
 
 # Build ROS packages in src directory
 colcon build \
diff --git a/setup.sh b/setup.sh
index 192222e0a..2810ac16e 100755
--- a/setup.sh
+++ b/setup.sh
@@ -10,7 +10,7 @@ function warn() {
 # Import all project repositories
 if [[ $DISABLE_VCS != "true" ]]; then
     echo "Importing project repositories..."
-    vcs import < src/new_project.repos src
+    vcs import < src/polaris.repos src --skip-existing
 else
     warn "VCS disabled. Skipping project repository imports..."
 fi
diff --git a/src/new_project.repos b/src/polaris.repos
similarity index 87%
rename from src/new_project.repos
rename to src/polaris.repos
index f0c32dbaa..7ba1a9f74 100644
--- a/src/new_project.repos
+++ b/src/polaris.repos
@@ -22,6 +22,11 @@ repositories:
     url: https://github.com/UBCSailbot/boat_simulator
     version: main
 
+  controller:
+    type: git
+    url: https://github.com/UBCSailbot/controller
+    version: main
+
   custom_interfaces:
     type: git
     url: https://github.com/UBCSailbot/custom_interfaces
@@ -42,6 +47,11 @@ repositories:
     url: https://github.com/UBCSailbot/network_systems
     version: main
 
+  notebooks:
+    type: git
+    url: https://github.com/UBCSailbot/notebooks
+    version: main
+
   raye-local-pathfinding:
     type: git
     url: https://github.com/UBCSailbot/raye-local-pathfinding