Adaptive Informative Path Planning¶

This tutorial will walk you through Adaptive Informative Path Planning (AIPP) using the sgptools library. In many real-world scenarios, a robot doesn't know everything about its environment from the start. AIPP addresses this by allowing the robot to update its understanding and re-plan its path on the fly as it gathers new data.

This tutorial will demonstrate an AIPP scenario where:

1. The robot has a non-point Field of View (FoV), specifically a square-shaped sensor whose size changes with its altitude, similar to a camera. This is much more realistic than assuming a simple point sensor.
2. The robot adaptively re-plans its path. After visiting each waypoint, it uses the newly collected data to update its internal model of the world and then re-optimizes the remainder of its path.

We will walk through the entire process, from setting up the environment to running the simulation and visualizing the robot's adaptive behavior step-by-step.

Setup and Imports¶

We begin by importing the necessary libraries and modules.

• os: Used to set environment variables to avoid potential conflicts with underlying libraries.
• numpy: For numerical operations.
• tensorflow: The backend for GPflow.
• matplotlib.pyplot: For plotting the results.
• sgptools modules: We import specific classes and functions from the library that we'll be using.

In [ ]:

import os
os.environ["TF_ENABLE_ONEDNN_OPTS"] = "0"

import numpy as np
import tensorflow as tf
from time import time

# Import necessary components from sgptools
from sgptools.utils.metrics import get_reconstruction, get_rmse
from sgptools.utils.data import Dataset
from sgptools.core.osgpr import init_osgpr
from sgptools.utils.tsp import run_tsp
from sgptools.utils.gpflow import get_model_params, optimize_model
from sgptools.methods import ContinuousSGP
from sgptools.core.transformations import IPPTransform, SquareHeightTransform
from gpflow.utilities import print_summary
from sgptools.utils.misc import get_inducing_pts

# Plotting libraries for visualization
from matplotlib import colors
import matplotlib.pyplot as plt
from matplotlib.patches import Polygon
import mpl_toolkits.mplot3d.art3d as art3d
from scipy.optimize import linear_sum_assignment
from sklearn.metrics import pairwise_distances

# Set random seeds for reproducibility
np.random.seed(1234)
tf.random.set_seed(1234)

import os os.environ["TF_ENABLE_ONEDNN_OPTS"] = "0" import numpy as np import tensorflow as tf from time import time # Import necessary components from sgptools from sgptools.utils.metrics import get_reconstruction, get_rmse from sgptools.utils.data import Dataset from sgptools.core.osgpr import init_osgpr from sgptools.utils.tsp import run_tsp from sgptools.utils.gpflow import get_model_params, optimize_model from sgptools.methods import ContinuousSGP from sgptools.core.transformations import IPPTransform, SquareHeightTransform from gpflow.utilities import print_summary from sgptools.utils.misc import get_inducing_pts # Plotting libraries for visualization from matplotlib import colors import matplotlib.pyplot as plt from matplotlib.patches import Polygon import mpl_toolkits.mplot3d.art3d as art3d from scipy.optimize import linear_sum_assignment from sklearn.metrics import pairwise_distances # Set random seeds for reproducibility np.random.seed(1234) tf.random.set_seed(1234)

Helper Functions for Plotting¶

To make our results easy to understand, we'll create two helper functions. These will handle the complexities of plotting our 3D environment and the robot's state within it.

In [2]:

def get_vertices(Xu, X_fov):
    """
    A utility function to calculate the corner vertices of the square FoV for plotting.
    It takes the central waypoint coordinates (`Xu`) and the expanded FoV points (`X_fov`)
    and returns the coordinates of the four corners of the square.
    """
    X_fov = X_fov.reshape(len(Xu), -1, 2)
    vertices = np.zeros((len(Xu), 5, 2))
    vertices[:, 0] = X_fov.min(axis=1)
    vertices[:, 1] = np.array([X_fov[:, :, 0].min(axis=1),
                              X_fov[:, :, 1].max(axis=1)]).T
    vertices[:, 2] = X_fov.max(axis=1)
    vertices[:, 3] = np.array([X_fov[:, :, 0].max(axis=1),
                              X_fov[:, :, 1].min(axis=1)]).T
    vertices[:, 4] = X_fov.mean(axis=1)

    dists = pairwise_distances(vertices[:, 4], Y=Xu[:, :2],
                               metric='euclidean')
    _, idx = linear_sum_assignment(dists)

    vertices[:, 4] = Xu[idx][:, :2]
    return vertices

def plot_results(path, dataset, noise_variance, kernel,
                 current_idx=0, fov_points=None):
    """
    This function visualizes the state of the simulation at a single time step.
    It creates a 3D plot showing:
    - The robot's reconstructed map of the environment.
    - The path the robot has traveled so far.
    - The path the robot plans to travel next.
    - The sensor's Field of View (FoV) at each waypoint.
    """
    # Get sensor data from the waypoints visited so far
    current_idx += 1 # Adjust for zero-based indexing
    num_fov_pts = len(fov_points)//len(path)
    sol_X, sol_y = dataset.get_sensor_data(fov_points[:num_fov_pts*(current_idx)], 
                                           max_samples=1500)
    # Create a GP model to reconstruct the data field from the gathered sensor data
    y_pred, _ = get_reconstruction((sol_X, sol_y), dataset.get_test()[0],
                                   noise_variance, kernel)

    # Setup 3D plot
    fig = plt.figure(figsize=(5, 5))
    ax = fig.add_subplot(111, projection='3d', computed_zorder=False)

    # Plot the reconstructed environment state as a colored surface on the floor of the plot
    vmin = min(dataset.y.min(), y_pred.min())
    vmax = max(dataset.y.max(), y_pred.max())
    norm = colors.Normalize(vmin, vmax)
    data_shape = dataset.y.shape
    _ = ax.plot_surface(dataset.get_test()[0][:, 0].reshape(data_shape),
                        dataset.get_test()[0][:, 1].reshape(data_shape),
                        np.atleast_2d(-0.1),
                        facecolors=plt.cm.jet(norm(y_pred.reshape(data_shape))),
                        shade=False,
                        alpha=0.8,
                        zorder=0)

    # Plot the solution path: visited waypoints in red, planned waypoints in green
    ax.scatter(path[:current_idx, 0],
               path[:current_idx, 1],
               path[:current_idx, 2], c='C3')
    ax.scatter(path[current_idx:, 0],
               path[current_idx:, 1],
               path[current_idx:, 2], c='C2')
    ax.plot(path[:, 0], path[:, 1], path[:, 2], 'k-')

    # Plot the square Field of View at each waypoint
    vertices = get_vertices(path, fov_points)
    for i in range(vertices.shape[0]):
        color = 'C3' if i < current_idx else 'C2'
        verts = []
        verts.append([vertices[i, 0], vertices[i, 1],
                      vertices[i, 2], vertices[i, 3]])
        fov = Polygon(np.array(verts)[0, :, :2],
                      linewidth=1.5,
                      edgecolor=color,
                      facecolor=color,
                      fill=False,
                      zorder=15)
        ax.add_patch(fov)
        art3d.pathpatch_2d_to_3d(fov)

    # Configure plot labels, limits, and view angle
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')

    ax.set_xlim(np.min(dataset.get_train()[0][:, 0])-0.5, np.max(dataset.get_train()[0][:, 0])+0.5)
    ax.set_ylim(np.min(dataset.get_train()[0][:, 1])-0.5, np.max(dataset.get_train()[0][:, 1])+0.5)
    ax.set_zlim(0, 14)

    ax.view_init(elev=30, azim=45+180)
    ax.set_title(f'Waypoint: {current_idx}, RMSE: {get_rmse(y_pred, dataset.get_test()[1]):.2f}\nData Field Reconstruction', y=0.99)

    plt.tight_layout()
    plt.show()

def get_vertices(Xu, X_fov): """ A utility function to calculate the corner vertices of the square FoV for plotting. It takes the central waypoint coordinates (`Xu`) and the expanded FoV points (`X_fov`) and returns the coordinates of the four corners of the square. """ X_fov = X_fov.reshape(len(Xu), -1, 2) vertices = np.zeros((len(Xu), 5, 2)) vertices[:, 0] = X_fov.min(axis=1) vertices[:, 1] = np.array([X_fov[:, :, 0].min(axis=1), X_fov[:, :, 1].max(axis=1)]).T vertices[:, 2] = X_fov.max(axis=1) vertices[:, 3] = np.array([X_fov[:, :, 0].max(axis=1), X_fov[:, :, 1].min(axis=1)]).T vertices[:, 4] = X_fov.mean(axis=1) dists = pairwise_distances(vertices[:, 4], Y=Xu[:, :2], metric='euclidean') _, idx = linear_sum_assignment(dists) vertices[:, 4] = Xu[idx][:, :2] return vertices def plot_results(path, dataset, noise_variance, kernel, current_idx=0, fov_points=None): """ This function visualizes the state of the simulation at a single time step. It creates a 3D plot showing: - The robot's reconstructed map of the environment. - The path the robot has traveled so far. - The path the robot plans to travel next. - The sensor's Field of View (FoV) at each waypoint. """ # Get sensor data from the waypoints visited so far current_idx += 1 # Adjust for zero-based indexing num_fov_pts = len(fov_points)//len(path) sol_X, sol_y = dataset.get_sensor_data(fov_points[:num_fov_pts*(current_idx)], max_samples=1500) # Create a GP model to reconstruct the data field from the gathered sensor data y_pred, _ = get_reconstruction((sol_X, sol_y), dataset.get_test()[0], noise_variance, kernel) # Setup 3D plot fig = plt.figure(figsize=(5, 5)) ax = fig.add_subplot(111, projection='3d', computed_zorder=False) # Plot the reconstructed environment state as a colored surface on the floor of the plot vmin = min(dataset.y.min(), y_pred.min()) vmax = max(dataset.y.max(), y_pred.max()) norm = colors.Normalize(vmin, vmax) data_shape = dataset.y.shape _ = ax.plot_surface(dataset.get_test()[0][:, 0].reshape(data_shape), dataset.get_test()[0][:, 1].reshape(data_shape), np.atleast_2d(-0.1), facecolors=plt.cm.jet(norm(y_pred.reshape(data_shape))), shade=False, alpha=0.8, zorder=0) # Plot the solution path: visited waypoints in red, planned waypoints in green ax.scatter(path[:current_idx, 0], path[:current_idx, 1], path[:current_idx, 2], c='C3') ax.scatter(path[current_idx:, 0], path[current_idx:, 1], path[current_idx:, 2], c='C2') ax.plot(path[:, 0], path[:, 1], path[:, 2], 'k-') # Plot the square Field of View at each waypoint vertices = get_vertices(path, fov_points) for i in range(vertices.shape[0]): color = 'C3' if i < current_idx else 'C2' verts = [] verts.append([vertices[i, 0], vertices[i, 1], vertices[i, 2], vertices[i, 3]]) fov = Polygon(np.array(verts)[0, :, :2], linewidth=1.5, edgecolor=color, facecolor=color, fill=False, zorder=15) ax.add_patch(fov) art3d.pathpatch_2d_to_3d(fov) # Configure plot labels, limits, and view angle ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z') ax.set_xlim(np.min(dataset.get_train()[0][:, 0])-0.5, np.max(dataset.get_train()[0][:, 0])+0.5) ax.set_ylim(np.min(dataset.get_train()[0][:, 1])-0.5, np.max(dataset.get_train()[0][:, 1])+0.5) ax.set_zlim(0, 14) ax.view_init(elev=30, azim=45+180) ax.set_title(f'Waypoint: {current_idx}, RMSE: {get_rmse(y_pred, dataset.get_test()[1]):.2f}\nData Field Reconstruction', y=0.99) plt.tight_layout() plt.show()

Data Loading and Hyperparameter Learning¶

Before we can start planning paths, we need to:

1. Load a dataset: We'll use the Dataset class to generate a synthetic elevation map. This map will serve as the "ground truth" environment that our robot(s) will explore.
2. Learn GP hyperparameters: We'll train a Gaussian Process (GP) model on a small sample of the data to learn the kernel's hyperparameters (like lengthscales and variance). These parameters describe the spatial correlation of the data and are crucial for evaluating the reconstructions from the solution.

In [3]:

# Get the synthetic elevation data
dataset = Dataset(shape=(100, 100), 
                  num_test=10000, 
                  random_seed=0,
                  verbose=False)
X_train, y_train = dataset.get_train()
X_test, y_test = dataset.get_test()

# Train a Gaussian Process (GP) model and get the optimized kernel parameters
# `get_model_params` returns loss_values, noise_variance, kernel_object
_, noise_variance_opt, kernel_opt = get_model_params(X_train, y_train, 
                                             lengthscales=[1.0, 1.0], # Initial lengthscales for the RBF kernel
                                             optimizer='scipy.L-BFGS-B') # Use SciPy's L-BFGS-B optimizer

# Plot the ground truth data
plt.figure()
im1 = plt.imshow(y_test.reshape(100, 100).T,
                 cmap='jet', origin='lower',
                 extent=[np.min(X_test[:, 0]), np.max(X_test[:, 0]),
                         np.min(X_test[:, 1]), np.max(X_test[:, 1])])
ax1 = plt.gca()
ax1.set_title('Ground Truth')
ax1.set_xlabel('X-coordinate')
ax1.set_ylabel('Y-coordinate')
ax1.set_aspect('equal', adjustable='box')
ax1.grid(True)

# Get the synthetic elevation data dataset = Dataset(shape=(100, 100), num_test=10000, random_seed=0, verbose=False) X_train, y_train = dataset.get_train() X_test, y_test = dataset.get_test() # Train a Gaussian Process (GP) model and get the optimized kernel parameters # `get_model_params` returns loss_values, noise_variance, kernel_object _, noise_variance_opt, kernel_opt = get_model_params(X_train, y_train, lengthscales=[1.0, 1.0], # Initial lengthscales for the RBF kernel optimizer='scipy.L-BFGS-B') # Use SciPy's L-BFGS-B optimizer # Plot the ground truth data plt.figure() im1 = plt.imshow(y_test.reshape(100, 100).T, cmap='jet', origin='lower', extent=[np.min(X_test[:, 0]), np.max(X_test[:, 0]), np.min(X_test[:, 1]), np.max(X_test[:, 1])]) ax1 = plt.gca() ax1.set_title('Ground Truth') ax1.set_xlabel('X-coordinate') ax1.set_ylabel('Y-coordinate') ax1.set_aspect('equal', adjustable='box') ax1.grid(True)

2025-06-30 14:01:13.690761: E external/local_xla/xla/stream_executor/cuda/cuda_platform.cc:51] failed call to cuInit: INTERNAL: CUDA error: Failed call to cuInit: UNKNOWN ERROR (303)

╒═════════════════════════╤═══════════╤══════════════════╤═════════╤═════════════╤═════════╤═════════╤══════════════════════╕
│ name                    │ class     │ transform        │ prior   │ trainable   │ shape   │ dtype   │ value                │
╞═════════════════════════╪═══════════╪══════════════════╪═════════╪═════════════╪═════════╪═════════╪══════════════════════╡
│ GPR.kernel.variance     │ Parameter │ Softplus         │         │ True        │ ()      │ float64 │ 0.7356971010336169   │
├─────────────────────────┼───────────┼──────────────────┼─────────┼─────────────┼─────────┼─────────┼──────────────────────┤
│ GPR.kernel.lengthscales │ Parameter │ Softplus         │         │ True        │ (2,)    │ float64 │ [2.16791 2.14484]    │
├─────────────────────────┼───────────┼──────────────────┼─────────┼─────────────┼─────────┼─────────┼──────────────────────┤
│ GPR.likelihood.variance │ Parameter │ Softplus + Shift │         │ True        │ ()      │ float64 │ 0.020845990883794487 │
╘═════════════════════════╧═══════════╧══════════════════╧═════════╧═════════════╧═════════╧═════════╧══════════════════════╛

Setting Up the Adaptive IPP Mission¶

Now, we configure the parameters for our AIPP mission. This includes setting the number of waypoints, defining the robot's sensor model, and a distance budget.

In [ ]:

num_waypoints = 12
pts_per_side = 5  # The FoV will be approximated by a pts_per_sidexpts_per_side grid of points

# We start with all the hyperparameters set to unit values. These will be updated online.
print('Initial AIPP Hyperparameters')
_, noise_variance, kernel = get_model_params(X_train, y_train,
                                             lengthscales=[1.0, 1.0],
                                             max_steps=0)

# Generate an initial path for the robot. We use `get_inducing_pts` to select
# waypoints and then `run_tsp` to order them into a logical path.
# The path is in 3D (x, y, altitude), so we initialize the altitude to 2.0.
Xu_init = get_inducing_pts(X_train, num_waypoints)
# Initialize the height dimension for all points with random values
height = np.random.normal(loc = 2, size=(Xu_init.shape[0], 1))
Xu_init = np.concatenate((Xu_init, height), axis=1)
paths, _ = run_tsp(Xu_init[:, :2])
if paths:
    Xu_init[:, :2] = paths[0]

# Define the sensor model using `SquareHeightTransform`. This transform models an
# FoV that changes size with altitude. We wrap it in an `IPPTransform` to integrate
# it into our planning framework.
fov_transform = SquareHeightTransform(pts_per_side=pts_per_side)
transform = IPPTransform(num_dim=3, # Defaults to 2
                         sensor_model=fov_transform,
                         aggregate_fov=False,
                         distance_budget=120., # Distance budget for the robot
                         constraint_weight=500.) # Weight for the distance constraint in optimization


# Initialize the IPP model using the `ContinuousSGP` method. This is the core
# component that will optimize the robot's path.
ipp_model = ContinuousSGP(
    num_sensing=num_waypoints,
    X_objective=X_train,
    noise_variance=noise_variance,
    kernel=kernel,
    transform=transform,
    X_init=Xu_init,
)

num_waypoints = 12 pts_per_side = 5 # The FoV will be approximated by a pts_per_sidexpts_per_side grid of points # We start with all the hyperparameters set to unit values. These will be updated online. print('Initial AIPP Hyperparameters') _, noise_variance, kernel = get_model_params(X_train, y_train, lengthscales=[1.0, 1.0], max_steps=0) # Generate an initial path for the robot. We use `get_inducing_pts` to select # waypoints and then `run_tsp` to order them into a logical path. # The path is in 3D (x, y, altitude), so we initialize the altitude to 2.0. Xu_init = get_inducing_pts(X_train, num_waypoints) # Initialize the height dimension for all points with random values height = np.random.normal(loc = 2, size=(Xu_init.shape[0], 1)) Xu_init = np.concatenate((Xu_init, height), axis=1) paths, _ = run_tsp(Xu_init[:, :2]) if paths: Xu_init[:, :2] = paths[0] # Define the sensor model using `SquareHeightTransform`. This transform models an # FoV that changes size with altitude. We wrap it in an `IPPTransform` to integrate # it into our planning framework. fov_transform = SquareHeightTransform(pts_per_side=pts_per_side) transform = IPPTransform(num_dim=3, # Defaults to 2 sensor_model=fov_transform, aggregate_fov=False, distance_budget=120., # Distance budget for the robot constraint_weight=500.) # Weight for the distance constraint in optimization # Initialize the IPP model using the `ContinuousSGP` method. This is the core # component that will optimize the robot's path. ipp_model = ContinuousSGP( num_sensing=num_waypoints, X_objective=X_train, noise_variance=noise_variance, kernel=kernel, transform=transform, X_init=Xu_init, )

Initial AIPP Hyperparameters
╒═════════════════════════╤═══════════╤══════════════════╤═════════╤═════════════╤═════════╤═════════╤═════════════════════╕
│ name                    │ class     │ transform        │ prior   │ trainable   │ shape   │ dtype   │ value               │
╞═════════════════════════╪═══════════╪══════════════════╪═════════╪═════════════╪═════════╪═════════╪═════════════════════╡
│ GPR.kernel.variance     │ Parameter │ Softplus         │         │ True        │ ()      │ float64 │ 1.0                 │
├─────────────────────────┼───────────┼──────────────────┼─────────┼─────────────┼─────────┼─────────┼─────────────────────┤
│ GPR.kernel.lengthscales │ Parameter │ Softplus         │         │ True        │ (2,)    │ float64 │ [1. 1.]             │
├─────────────────────────┼───────────┼──────────────────┼─────────┼─────────────┼─────────┼─────────┼─────────────────────┤
│ GPR.likelihood.variance │ Parameter │ Softplus + Shift │         │ True        │ ()      │ float64 │ 0.09999999999999999 │
╘═════════════════════════╧═══════════╧══════════════════╧═════════╧═════════════╧═════════╧═════════╧═════════════════════╛

Simulating the Adaptive IPP Mission¶

This is the heart of the tutorial. We will now simulate the AIPP mission step-by-step in a loop. At each step, the robot will:

1. "Visit" a waypoint: We'll simulate this by taking the current solution path.
2. Gather data: The robot collects new sensor readings from its current location and FoV.
3. Update its world model: The new data is used to update an Online Sparse GP model (OSGPR). This model efficiently incorporates new information without retraining from scratch, which is key for adaptive planning. The kernel hyperparameters are re-optimized.
4. Re-plan its path: The ContinuousSGP planner is updated with the new hyperparameters from the OSGPR. It then re-optimizes the remaining waypoints in its path.
5. Visualize: We plot the robot's current state and its new plan.

We repeat this process until the robot has visited all waypoints.

In [5]:

total_time_param = 0
total_ipp_time = 0
curr_sol = Xu_init.copy().reshape(1, -1, 3)

# Get the initial expanded path for the first plot
expanded_path = ipp_model.transform.expand(curr_sol.reshape(-1, 3), 
                                           expand_sensor_model=False)
expanded_path = expanded_path.numpy()

# Plot the initial state before the mission starts
plot_results(expanded_path,
             dataset, noise_variance_opt, kernel_opt,
             fov_points=ipp_model.transform.expand(curr_sol.reshape(-1, 3)).numpy())

# Initialize the Online Sparse GP model (`OSGPR`) for hyperparameter updates
init_kernel, init_noise_variance = ipp_model.get_hyperparameters()
param_model = init_osgpr(X_train,
                         num_inducing=40,
                         kernel=init_kernel,
                         noise_variance=init_noise_variance)

# Main simulation loop
for time_step in range(num_waypoints):
    print(f"\n--- Time Step {time_step + 1}/{num_waypoints} ---")

    # Get a new batch of data from the last visited waypoint
    last_visited = curr_sol[:, time_step].copy()
    data_pts = ipp_model.transform.sensor_model.expand(last_visited).numpy()
    data_X_batch, data_y_batch = dataset.get_sensor_data(data_pts)

    # Skip param and path update for the last waypoint
    if time_step == num_waypoints - 1:
        break

    # Update the Online GP model (`OSGPR`) with the new data
    print("Updating the online GP model model with new data...")
    param_model.update((np.array(data_X_batch),
                        np.array(data_y_batch)),
                        update_inducing=True)
    # Optimize the kernel and noise hyperparameters of the online model
    start_time = time()
    optimize_model(param_model)
    end_time = time()
    total_time_param += end_time - start_time
    print("Updated Kernel and Likelihood:")
    print_summary(param_model.kernel)
    print_summary(param_model.likelihood)

    # Update and re-plan the path with the `ContinuousSGP` model
    print("Re-planning the rest of the path...")
    # Fix the waypoints that have already been visited
    Xu_visited = curr_sol.copy()[:, :time_step + 1]
    ipp_model.transform.update_Xu_fixed(Xu_visited)
    # Update the planner with the new hyperparameters from the online model
    ipp_model.update(param_model.kernel, 
                     param_model.likelihood.variance.numpy())
    
    # Optimize the remaining part of the path
    start_time = time()
    curr_sol = ipp_model.optimize(verbose=False)
    end_time = time()
    total_ipp_time += end_time - start_time
    print("Path re-optimized.")

    # Visualize the current state
    expanded_path = ipp_model.transform.expand(curr_sol.reshape(-1, 3), 
                                               expand_sensor_model=False)
    expanded_path = expanded_path.numpy()

    plot_results(expanded_path,
                 dataset, noise_variance_opt, kernel_opt,
                 current_idx=time_step + 1,
                 fov_points=ipp_model.transform.expand(curr_sol.reshape(-1, 3)).numpy())

total_time_param = 0 total_ipp_time = 0 curr_sol = Xu_init.copy().reshape(1, -1, 3) # Get the initial expanded path for the first plot expanded_path = ipp_model.transform.expand(curr_sol.reshape(-1, 3), expand_sensor_model=False) expanded_path = expanded_path.numpy() # Plot the initial state before the mission starts plot_results(expanded_path, dataset, noise_variance_opt, kernel_opt, fov_points=ipp_model.transform.expand(curr_sol.reshape(-1, 3)).numpy()) # Initialize the Online Sparse GP model (`OSGPR`) for hyperparameter updates init_kernel, init_noise_variance = ipp_model.get_hyperparameters() param_model = init_osgpr(X_train, num_inducing=40, kernel=init_kernel, noise_variance=init_noise_variance) # Main simulation loop for time_step in range(num_waypoints): print(f"\n--- Time Step {time_step + 1}/{num_waypoints} ---") # Get a new batch of data from the last visited waypoint last_visited = curr_sol[:, time_step].copy() data_pts = ipp_model.transform.sensor_model.expand(last_visited).numpy() data_X_batch, data_y_batch = dataset.get_sensor_data(data_pts) # Skip param and path update for the last waypoint if time_step == num_waypoints - 1: break # Update the Online GP model (`OSGPR`) with the new data print("Updating the online GP model model with new data...") param_model.update((np.array(data_X_batch), np.array(data_y_batch)), update_inducing=True) # Optimize the kernel and noise hyperparameters of the online model start_time = time() optimize_model(param_model) end_time = time() total_time_param += end_time - start_time print("Updated Kernel and Likelihood:") print_summary(param_model.kernel) print_summary(param_model.likelihood) # Update and re-plan the path with the `ContinuousSGP` model print("Re-planning the rest of the path...") # Fix the waypoints that have already been visited Xu_visited = curr_sol.copy()[:, :time_step + 1] ipp_model.transform.update_Xu_fixed(Xu_visited) # Update the planner with the new hyperparameters from the online model ipp_model.update(param_model.kernel, param_model.likelihood.variance.numpy()) # Optimize the remaining part of the path start_time = time() curr_sol = ipp_model.optimize(verbose=False) end_time = time() total_ipp_time += end_time - start_time print("Path re-optimized.") # Visualize the current state expanded_path = ipp_model.transform.expand(curr_sol.reshape(-1, 3), expand_sensor_model=False) expanded_path = expanded_path.numpy() plot_results(expanded_path, dataset, noise_variance_opt, kernel_opt, current_idx=time_step + 1, fov_points=ipp_model.transform.expand(curr_sol.reshape(-1, 3)).numpy())

--- Time Step 1/12 ---
Updating the online GP model model with new data...
Updated Kernel and Likelihood:
╒═════════════════════════════════╤═══════════╤═════════════╤═════════╤═════════════╤═════════╤═════════╤═════════════════════════╕
│ name                            │ class     │ transform   │ prior   │ trainable   │ shape   │ dtype   │ value                   │
╞═════════════════════════════════╪═══════════╪═════════════╪═════════╪═════════════╪═════════╪═════════╪═════════════════════════╡
│ SquaredExponential.variance     │ Parameter │ Softplus    │         │ True        │ ()      │ float64 │ 0.2327835934018852      │
├─────────────────────────────────┼───────────┼─────────────┼─────────┼─────────────┼─────────┼─────────┼─────────────────────────┤
│ SquaredExponential.lengthscales │ Parameter │ Softplus    │         │ True        │ (2,)    │ float64 │ [0.98424141 1.14156   ] │
╘═════════════════════════════════╧═══════════╧═════════════╧═════════╧═════════════╧═════════╧═════════╧═════════════════════════╛
╒═══════════════════╤═══════════╤══════════════════╤═════════╤═════════════╤═════════╤═════════╤═══════════╕
│ name              │ class     │ transform        │ prior   │ trainable   │ shape   │ dtype   │     value │
╞═══════════════════╪═══════════╪══════════════════╪═════════╪═════════════╪═════════╪═════════╪═══════════╡
│ Gaussian.variance │ Parameter │ Softplus + Shift │         │ True        │ ()      │ float64 │ 0.0105753 │
╘═══════════════════╧═══════════╧══════════════════╧═════════╧═════════════╧═════════╧═════════╧═══════════╛
Re-planning the rest of the path...
Path re-optimized.

--- Time Step 2/12 ---
Updating the online GP model model with new data...
Updated Kernel and Likelihood:
╒═════════════════════════════════╤═══════════╤═════════════╤═════════╤═════════════╤═════════╤═════════╤════════════════════╕
│ name                            │ class     │ transform   │ prior   │ trainable   │ shape   │ dtype   │ value              │
╞═════════════════════════════════╪═══════════╪═════════════╪═════════╪═════════════╪═════════╪═════════╪════════════════════╡
│ SquaredExponential.variance     │ Parameter │ Softplus    │         │ True        │ ()      │ float64 │ 0.3909714420310825 │
├─────────────────────────────────┼───────────┼─────────────┼─────────┼─────────────┼─────────┼─────────┼────────────────────┤
│ SquaredExponential.lengthscales │ Parameter │ Softplus    │         │ True        │ (2,)    │ float64 │ [6.45418 3.54678]  │
╘═════════════════════════════════╧═══════════╧═════════════╧═════════╧═════════════╧═════════╧═════════╧════════════════════╛
╒═══════════════════╤═══════════╤══════════════════╤═════════╤═════════════╤═════════╤═════════╤═════════╕
│ name              │ class     │ transform        │ prior   │ trainable   │ shape   │ dtype   │   value │
╞═══════════════════╪═══════════╪══════════════════╪═════════╪═════════════╪═════════╪═════════╪═════════╡
│ Gaussian.variance │ Parameter │ Softplus + Shift │         │ True        │ ()      │ float64 │ 0.18264 │
╘═══════════════════╧═══════════╧══════════════════╧═════════╧═════════════╧═════════╧═════════╧═════════╛
Re-planning the rest of the path...
Path re-optimized.

--- Time Step 3/12 ---
Updating the online GP model model with new data...
Updated Kernel and Likelihood:
╒═════════════════════════════════╤═══════════╤═════════════╤═════════╤═════════════╤═════════╤═════════╤═══════════════════╕
│ name                            │ class     │ transform   │ prior   │ trainable   │ shape   │ dtype   │ value             │
╞═════════════════════════════════╪═══════════╪═════════════╪═════════╪═════════════╪═════════╪═════════╪═══════════════════╡
│ SquaredExponential.variance     │ Parameter │ Softplus    │         │ True        │ ()      │ float64 │ 1.95838           │
├─────────────────────────────────┼───────────┼─────────────┼─────────┼─────────────┼─────────┼─────────┼───────────────────┤
│ SquaredExponential.lengthscales │ Parameter │ Softplus    │         │ True        │ (2,)    │ float64 │ [5.15126 3.71883] │
╘═════════════════════════════════╧═══════════╧═════════════╧═════════╧═════════════╧═════════╧═════════╧═══════════════════╛
╒═══════════════════╤═══════════╤══════════════════╤═════════╤═════════════╤═════════╤═════════╤══════════╕
│ name              │ class     │ transform        │ prior   │ trainable   │ shape   │ dtype   │    value │
╞═══════════════════╪═══════════╪══════════════════╪═════════╪═════════════╪═════════╪═════════╪══════════╡
│ Gaussian.variance │ Parameter │ Softplus + Shift │         │ True        │ ()      │ float64 │ 0.110979 │
╘═══════════════════╧═══════════╧══════════════════╧═════════╧═════════════╧═════════╧═════════╧══════════╛
Re-planning the rest of the path...
Path re-optimized.

--- Time Step 4/12 ---
Updating the online GP model model with new data...
Updated Kernel and Likelihood:
╒═════════════════════════════════╤═══════════╤═════════════╤═════════╤═════════════╤═════════╤═════════╤═══════════════════╕
│ name                            │ class     │ transform   │ prior   │ trainable   │ shape   │ dtype   │ value             │
╞═════════════════════════════════╪═══════════╪═════════════╪═════════╪═════════════╪═════════╪═════════╪═══════════════════╡
│ SquaredExponential.variance     │ Parameter │ Softplus    │         │ True        │ ()      │ float64 │ 0.73042140691213  │
├─────────────────────────────────┼───────────┼─────────────┼─────────┼─────────────┼─────────┼─────────┼───────────────────┤
│ SquaredExponential.lengthscales │ Parameter │ Softplus    │         │ True        │ (2,)    │ float64 │ [4.41423 4.14698] │
╘═════════════════════════════════╧═══════════╧═════════════╧═════════╧═════════════╧═════════╧═════════╧═══════════════════╛
╒═══════════════════╤═══════════╤══════════════════╤═════════╤═════════════╤═════════╤═════════╤══════════╕
│ name              │ class     │ transform        │ prior   │ trainable   │ shape   │ dtype   │    value │
╞═══════════════════╪═══════════╪══════════════════╪═════════╪═════════════╪═════════╪═════════╪══════════╡
│ Gaussian.variance │ Parameter │ Softplus + Shift │         │ True        │ ()      │ float64 │ 0.058691 │
╘═══════════════════╧═══════════╧══════════════════╧═════════╧═════════════╧═════════╧═════════╧══════════╛
Re-planning the rest of the path...
Path re-optimized.

--- Time Step 5/12 ---
Updating the online GP model model with new data...
Updated Kernel and Likelihood:
╒═════════════════════════════════╤═══════════╤═════════════╤═════════╤═════════════╤═════════╤═════════╤════════════════════╕
│ name                            │ class     │ transform   │ prior   │ trainable   │ shape   │ dtype   │ value              │
╞═════════════════════════════════╪═══════════╪═════════════╪═════════╪═════════════╪═════════╪═════════╪════════════════════╡
│ SquaredExponential.variance     │ Parameter │ Softplus    │         │ True        │ ()      │ float64 │ 0.2286429758847512 │
├─────────────────────────────────┼───────────┼─────────────┼─────────┼─────────────┼─────────┼─────────┼────────────────────┤
│ SquaredExponential.lengthscales │ Parameter │ Softplus    │         │ True        │ (2,)    │ float64 │ [3.35848 1.88981]  │
╘═════════════════════════════════╧═══════════╧═════════════╧═════════╧═════════════╧═════════╧═════════╧════════════════════╛
╒═══════════════════╤═══════════╤══════════════════╤═════════╤═════════════╤═════════╤═════════╤══════════╕
│ name              │ class     │ transform        │ prior   │ trainable   │ shape   │ dtype   │    value │
╞═══════════════════╪═══════════╪══════════════════╪═════════╪═════════════╪═════════╪═════════╪══════════╡
│ Gaussian.variance │ Parameter │ Softplus + Shift │         │ True        │ ()      │ float64 │ 0.126621 │
╘═══════════════════╧═══════════╧══════════════════╧═════════╧═════════════╧═════════╧═════════╧══════════╛
Re-planning the rest of the path...
Path re-optimized.

--- Time Step 6/12 ---
Updating the online GP model model with new data...
Updated Kernel and Likelihood:
╒═════════════════════════════════╤═══════════╤═════════════╤═════════╤═════════════╤═════════╤═════════╤════════════════════╕
│ name                            │ class     │ transform   │ prior   │ trainable   │ shape   │ dtype   │ value              │
╞═════════════════════════════════╪═══════════╪═════════════╪═════════╪═════════════╪═════════╪═════════╪════════════════════╡
│ SquaredExponential.variance     │ Parameter │ Softplus    │         │ True        │ ()      │ float64 │ 0.3813628961175243 │
├─────────────────────────────────┼───────────┼─────────────┼─────────┼─────────────┼─────────┼─────────┼────────────────────┤
│ SquaredExponential.lengthscales │ Parameter │ Softplus    │         │ True        │ (2,)    │ float64 │ [2.44825 4.90212]  │
╘═════════════════════════════════╧═══════════╧═════════════╧═════════╧═════════════╧═════════╧═════════╧════════════════════╛
╒═══════════════════╤═══════════╤══════════════════╤═════════╤═════════════╤═════════╤═════════╤═════════╕
│ name              │ class     │ transform        │ prior   │ trainable   │ shape   │ dtype   │   value │
╞═══════════════════╪═══════════╪══════════════════╪═════════╪═════════════╪═════════╪═════════╪═════════╡
│ Gaussian.variance │ Parameter │ Softplus + Shift │         │ True        │ ()      │ float64 │ 0.11031 │
╘═══════════════════╧═══════════╧══════════════════╧═════════╧═════════════╧═════════╧═════════╧═════════╛
Re-planning the rest of the path...
Path re-optimized.

--- Time Step 7/12 ---
Updating the online GP model model with new data...
Updated Kernel and Likelihood:
╒═════════════════════════════════╤═══════════╤═════════════╤═════════╤═════════════╤═════════╤═════════╤════════════════════╕
│ name                            │ class     │ transform   │ prior   │ trainable   │ shape   │ dtype   │ value              │
╞═════════════════════════════════╪═══════════╪═════════════╪═════════╪═════════════╪═════════╪═════════╪════════════════════╡
│ SquaredExponential.variance     │ Parameter │ Softplus    │         │ True        │ ()      │ float64 │ 0.6266021176837389 │
├─────────────────────────────────┼───────────┼─────────────┼─────────┼─────────────┼─────────┼─────────┼────────────────────┤
│ SquaredExponential.lengthscales │ Parameter │ Softplus    │         │ True        │ (2,)    │ float64 │ [3.47098 3.60558]  │
╘═════════════════════════════════╧═══════════╧═════════════╧═════════╧═════════════╧═════════╧═════════╧════════════════════╛
╒═══════════════════╤═══════════╤══════════════════╤═════════╤═════════════╤═════════╤═════════╤══════════╕
│ name              │ class     │ transform        │ prior   │ trainable   │ shape   │ dtype   │    value │
╞═══════════════════╪═══════════╪══════════════════╪═════════╪═════════════╪═════════╪═════════╪══════════╡
│ Gaussian.variance │ Parameter │ Softplus + Shift │         │ True        │ ()      │ float64 │ 0.122391 │
╘═══════════════════╧═══════════╧══════════════════╧═════════╧═════════════╧═════════╧═════════╧══════════╛
Re-planning the rest of the path...
Path re-optimized.

--- Time Step 8/12 ---
Updating the online GP model model with new data...
Updated Kernel and Likelihood:
╒═════════════════════════════════╤═══════════╤═════════════╤═════════╤═════════════╤═════════╤═════════╤════════════════════╕
│ name                            │ class     │ transform   │ prior   │ trainable   │ shape   │ dtype   │ value              │
╞═════════════════════════════════╪═══════════╪═════════════╪═════════╪═════════════╪═════════╪═════════╪════════════════════╡
│ SquaredExponential.variance     │ Parameter │ Softplus    │         │ True        │ ()      │ float64 │ 0.9663967915248568 │
├─────────────────────────────────┼───────────┼─────────────┼─────────┼─────────────┼─────────┼─────────┼────────────────────┤
│ SquaredExponential.lengthscales │ Parameter │ Softplus    │         │ True        │ (2,)    │ float64 │ [4.44781 3.97362]  │
╘═════════════════════════════════╧═══════════╧═════════════╧═════════╧═════════════╧═════════╧═════════╧════════════════════╛
╒═══════════════════╤═══════════╤══════════════════╤═════════╤═════════════╤═════════╤═════════╤═════════╕
│ name              │ class     │ transform        │ prior   │ trainable   │ shape   │ dtype   │   value │
╞═══════════════════╪═══════════╪══════════════════╪═════════╪═════════════╪═════════╪═════════╪═════════╡
│ Gaussian.variance │ Parameter │ Softplus + Shift │         │ True        │ ()      │ float64 │ 0.24431 │
╘═══════════════════╧═══════════╧══════════════════╧═════════╧═════════════╧═════════╧═════════╧═════════╛
Re-planning the rest of the path...
Path re-optimized.

--- Time Step 9/12 ---
Updating the online GP model model with new data...
Updated Kernel and Likelihood:
╒═════════════════════════════════╤═══════════╤═════════════╤═════════╤═════════════╤═════════╤═════════╤════════════════════╕
│ name                            │ class     │ transform   │ prior   │ trainable   │ shape   │ dtype   │ value              │
╞═════════════════════════════════╪═══════════╪═════════════╪═════════╪═════════════╪═════════╪═════════╪════════════════════╡
│ SquaredExponential.variance     │ Parameter │ Softplus    │         │ True        │ ()      │ float64 │ 0.6044801308260008 │
├─────────────────────────────────┼───────────┼─────────────┼─────────┼─────────────┼─────────┼─────────┼────────────────────┤
│ SquaredExponential.lengthscales │ Parameter │ Softplus    │         │ True        │ (2,)    │ float64 │ [2.88835 3.75912]  │
╘═════════════════════════════════╧═══════════╧═════════════╧═════════╧═════════════╧═════════╧═════════╧════════════════════╛
╒═══════════════════╤═══════════╤══════════════════╤═════════╤═════════════╤═════════╤═════════╤═══════════╕
│ name              │ class     │ transform        │ prior   │ trainable   │ shape   │ dtype   │     value │
╞═══════════════════╪═══════════╪══════════════════╪═════════╪═════════════╪═════════╪═════════╪═══════════╡
│ Gaussian.variance │ Parameter │ Softplus + Shift │         │ True        │ ()      │ float64 │ 0.0974913 │
╘═══════════════════╧═══════════╧══════════════════╧═════════╧═════════════╧═════════╧═════════╧═══════════╛
Re-planning the rest of the path...
Path re-optimized.

--- Time Step 10/12 ---
Updating the online GP model model with new data...
Updated Kernel and Likelihood:
╒═════════════════════════════════╤═══════════╤═════════════╤═════════╤═════════════╤═════════╤═════════╤═════════════════════╕
│ name                            │ class     │ transform   │ prior   │ trainable   │ shape   │ dtype   │ value               │
╞═════════════════════════════════╪═══════════╪═════════════╪═════════╪═════════════╪═════════╪═════════╪═════════════════════╡
│ SquaredExponential.variance     │ Parameter │ Softplus    │         │ True        │ ()      │ float64 │ 0.17318604418364328 │
├─────────────────────────────────┼───────────┼─────────────┼─────────┼─────────────┼─────────┼─────────┼─────────────────────┤
│ SquaredExponential.lengthscales │ Parameter │ Softplus    │         │ True        │ (2,)    │ float64 │ [2.29347 3.45906]   │
╘═════════════════════════════════╧═══════════╧═════════════╧═════════╧═════════════╧═════════╧═════════╧═════════════════════╛
╒═══════════════════╤═══════════╤══════════════════╤═════════╤═════════════╤═════════╤═════════╤═══════════╕
│ name              │ class     │ transform        │ prior   │ trainable   │ shape   │ dtype   │     value │
╞═══════════════════╪═══════════╪══════════════════╪═════════╪═════════════╪═════════╪═════════╪═══════════╡
│ Gaussian.variance │ Parameter │ Softplus + Shift │         │ True        │ ()      │ float64 │ 0.0740142 │
╘═══════════════════╧═══════════╧══════════════════╧═════════╧═════════════╧═════════╧═════════╧═══════════╛
Re-planning the rest of the path...
Path re-optimized.

--- Time Step 11/12 ---
Updating the online GP model model with new data...
Updated Kernel and Likelihood:
╒═════════════════════════════════╤═══════════╤═════════════╤═════════╤═════════════╤═════════╤═════════╤═════════════════════╕
│ name                            │ class     │ transform   │ prior   │ trainable   │ shape   │ dtype   │ value               │
╞═════════════════════════════════╪═══════════╪═════════════╪═════════╪═════════════╪═════════╪═════════╪═════════════════════╡
│ SquaredExponential.variance     │ Parameter │ Softplus    │         │ True        │ ()      │ float64 │ 0.37144769581607917 │
├─────────────────────────────────┼───────────┼─────────────┼─────────┼─────────────┼─────────┼─────────┼─────────────────────┤
│ SquaredExponential.lengthscales │ Parameter │ Softplus    │         │ True        │ (2,)    │ float64 │ [1.81332 5.9557 ]   │
╘═════════════════════════════════╧═══════════╧═════════════╧═════════╧═════════════╧═════════╧═════════╧═════════════════════╛
╒═══════════════════╤═══════════╤══════════════════╤═════════╤═════════════╤═════════╤═════════╤═══════════╕
│ name              │ class     │ transform        │ prior   │ trainable   │ shape   │ dtype   │     value │
╞═══════════════════╪═══════════╪══════════════════╪═════════╪═════════════╪═════════╪═════════╪═══════════╡
│ Gaussian.variance │ Parameter │ Softplus + Shift │         │ True        │ ()      │ float64 │ 0.0585437 │
╘═══════════════════╧═══════════╧══════════════════╧═════════╧═════════════╧═════════╧═════════╧═══════════╛
Re-planning the rest of the path...
Path re-optimized.

--- Time Step 12/12 ---

The simulation is now complete! Let's review the total and average time taken for the two key adaptive steps: updating the online GP model and re-planning the path. In a real-world robotic system, these times are critical for ensuring the robot can make decisions in real-time.

In [6]:

print("\n--- Mission Performance Summary ---")
print(f"Solution Path Total Length: {ipp_model.transform.sensor_model.distance(curr_sol.reshape(-1, 3)).numpy():.4f}")

print(f'Total Hyperparameter Update Time:   {total_time_param:.2f}s')
print(f'Total IPP Update Time:              {total_ipp_time:.2f}s')

print(f'Average Hyperparameter Update Time: {total_time_param/num_waypoints:.2f}s')
print(f'Average IPP Update Time:            {total_ipp_time/num_waypoints:.2f}s')

print("\n--- Mission Performance Summary ---") print(f"Solution Path Total Length: {ipp_model.transform.sensor_model.distance(curr_sol.reshape(-1, 3)).numpy():.4f}") print(f'Total Hyperparameter Update Time: {total_time_param:.2f}s') print(f'Total IPP Update Time: {total_ipp_time:.2f}s') print(f'Average Hyperparameter Update Time: {total_time_param/num_waypoints:.2f}s') print(f'Average IPP Update Time: {total_ipp_time/num_waypoints:.2f}s')

--- Mission Performance Summary ---
Solution Path Total Length: 120.0000
Total Hyperparameter Update Time:   77.32s
Total IPP Update Time:              90.67s
Average Hyperparameter Update Time: 6.44s
Average IPP Update Time:            7.56s

Conclusion¶

In this tutorial, we've demonstrated a complete Adaptive Informative Path Planning (AIPP) loop. We've seen how a robot can start with a basic understanding of its environment, and then intelligently update both its world model and its future plans as it gathers new information.

Here are the key takeaways:

• The Power of Adaptation: By re-planning at each step, the robot can react to what it has learned. You can see in the visualizations how the planned path (in green) changes based on the data gathered from the visited waypoints (in red). This allows the robot to focus its efforts on areas of high uncertainty or interest, leading to a much more efficient mission.

• Modeling Realistic Sensors: We've gone beyond simple point sensors and modeled a more realistic altitude-dependent Field of View. The sgptools library makes it straightforward to define and incorporate such custom sensor models into the planning process.

• Efficient Online Updates: The use of an Online Sparse GP (OSGPR) is crucial for the adaptive component. It allows the robot's world model to be updated efficiently without the need to retrain from scratch.

We encourage you to experiment with the parameters in this tutorial—try different numbers of waypoints, different sensor models, or even different environments—to further explore the capabilities of adaptive planning.