In earlier chapters, we designed controllers assuming a reasonably accurate dynamics model. In practice, unmodeled friction, payload changes, flexible elements, and calibration errors introduce a residual dynamics term \( d(\cdot) \) that degrades tracking performance.

A learning-augmented controller keeps the nominal model-based structure (e.g., computed-torque) but augments it with a learned estimate of this residual:

\[ u = u_{\text{nom}}(x, r) + \hat d(x, r), \]

where \( x \) collects measured robot states (e.g., joint positions and velocities), \( r \) collects reference signals (desired joint trajectories), and \( \hat d \) is a learned function. A Gaussian process provides:

• A flexible, nonparametric regression model for \( d(\cdot) \).
• A distribution over functions, producing both a mean estimate and predictive variance (uncertainty).
• A natural way to incorporate prior smoothness and scale assumptions via kernel functions.

At a high level, a GP-based error compensation pipeline looks like:

flowchart TD
  A["Robot + nominal controller"] --> B["Collect (state, reference, torque) data"]
  B --> C["Compute residual = measured torque - nominal torque"]
  C --> D["Form dataset X: inputs, y: residuals"]
  D --> E["Train Gaussian process model"]
  E --> F["Deploy GP: online residual prediction"]
  F --> G["Command torque = nominal term + GP residual"]
        

In the remainder of this lesson, we formalize the GP model, derive the posterior, and embed \( \hat d \) into a tracking controller for robot manipulators.

Consider a scalar residual function \( f : \mathbb{R}^d \to \mathbb{R} \), where \( x \in \mathbb{R}^d \) encodes robot states and references. We observe noisy samples \( y_i = f(x_i) + \varepsilon_i \) with \( \varepsilon_i \sim \mathcal{N}(0,\sigma_n^2) \).

A Gaussian process prior specifies that any finite collection of function values is jointly Gaussian:

\[ f(\cdot) \sim \mathcal{GP}(m(\cdot), k(\cdot,\cdot)), \]

where \( m(x) \) is the mean function (often taken as \( 0 \)) and \( k(x,x') \) is a positive semidefinite kernel encoding similarity. A common choice is the squared-exponential (RBF) kernel:

\[ k(x,x') = \sigma_f^2 \exp\!\Big( -\tfrac{1}{2}(x - x')^\top \Lambda^{-1} (x - x') \Big), \]

with diagonal length-scale matrix \( \Lambda = \operatorname{diag}(\ell_1^2,\dots,\ell_d^2) \), signal variance \( \sigma_f^2 \), and noise variance \( \sigma_n^2 \).

2.1 Joint prior and posterior

Stack \( N \) training inputs into \( X = [x_1,\dots,x_N]^\top \in \mathbb{R}^{N \times d} \) and outputs into \( \mathbf{y} = [y_1,\dots,y_N]^\top \). Let \( \mathbf{f} = [f(x_1),\dots,f(x_N)]^\top \). Then

\[ \mathbf{f} \sim \mathcal{N}(\mathbf{0}, \mathbf{K}), \quad \mathbf{K}_{ij} = k(x_i,x_j), \]

\[ \mathbf{y} \mid \mathbf{f} \sim \mathcal{N}(\mathbf{f}, \sigma_n^2 \mathbf{I}_N). \]

The marginal prior over \( \mathbf{y} \) is then

\[ \mathbf{y} \sim \mathcal{N}\big(\mathbf{0}, \mathbf{K}_y\big), \quad \mathbf{K}_y = \mathbf{K} + \sigma_n^2 \mathbf{I}_N. \]

For a test input \( x_\ast \), define \( f_\ast = f(x_\ast) \), kernel vector \( \mathbf{k}_\ast = [k(x_1,x_\ast),\dots,k(x_N,x_\ast)]^\top \), and scalar \( k_{\ast\ast} = k(x_\ast,x_\ast) \). The joint prior of \( [\mathbf{y}^\top, f_\ast]^\top \) is

\[ \begin{bmatrix} \mathbf{y} \\ f_\ast \end{bmatrix} \sim \mathcal{N}\!\left( \begin{bmatrix} \mathbf{0} \\ 0 \end{bmatrix}, \begin{bmatrix} \mathbf{K}_y & \mathbf{k}_\ast \\ \mathbf{k}_\ast^\top & k_{\ast\ast} \end{bmatrix}\right). \]

2.2 Posterior mean and variance (derivation)

Using the conditioning formula for multivariate Gaussians, the posterior distribution of \( f_\ast \) given the data is

\[ f_\ast \mid X,\mathbf{y},x_\ast \sim \mathcal{N}\big(\mu_\ast, \sigma_\ast^2\big), \]

\[ \mu_\ast = \mathbf{k}_\ast^\top \mathbf{K}_y^{-1} \mathbf{y}, \quad \sigma_\ast^2 = k_{\ast\ast} - \mathbf{k}_\ast^\top \mathbf{K}_y^{-1} \mathbf{k}_\ast. \]

Proof sketch. For a jointly Gaussian vector \( \begin{bmatrix} a \\ b \end{bmatrix} \) with zero mean and covariance \( \begin{bmatrix} \Sigma_{aa} & \Sigma_{ab} \\ \Sigma_{ba} & \Sigma_{bb} \end{bmatrix} \), the conditional distribution of \( b \mid a \) is Gaussian with

\[ \mathbb{E}[b \mid a] = \Sigma_{ba}\Sigma_{aa}^{-1} a, \quad \operatorname{Cov}(b \mid a) = \Sigma_{bb} - \Sigma_{ba}\Sigma_{aa}^{-1}\Sigma_{ab}. \]

Identifying \( a \equiv \mathbf{y} \), \( b \equiv f_\ast \), \( \Sigma_{aa} \equiv \mathbf{K}_y \), \( \Sigma_{ab} \equiv \mathbf{k}_\ast \), \( \Sigma_{bb} \equiv k_{\ast\ast} \), we obtain the claimed expressions.

2.3 Equivalence to kernel ridge regression

Defining \( \boldsymbol{\alpha} = \mathbf{K}_y^{-1} \mathbf{y} \), the posterior mean can be written as

\[ \mu_\ast = \mathbf{k}_\ast^\top \boldsymbol{\alpha} = \sum_{i=1}^N \alpha_i k(x_i,x_\ast). \]

This coincides with the predictor of kernel ridge regression with regularization parameter \( \lambda = \sigma_n^2 \). Thus, a GP with Gaussian noise can be viewed as kernel ridge regression together with a principled predictive variance \( \sigma_\ast^2 \).

We recall the standard rigid-body manipulator dynamics (which you know from kinematics and dynamics):

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}, \]

where \( \mathbf{q} \in \mathbb{R}^n \) are joint positions, \( \dot{\mathbf{q}} \) joint velocities, \( \ddot{\mathbf{q}} \) joint accelerations, \( \mathbf{M}(\mathbf{q}) \) is the inertia matrix, \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) collects Coriolis and centripetal terms, \( \mathbf{g}(\mathbf{q}) \) is gravity, and \( \boldsymbol{\tau} \) the commanded joint torques.

3.1 Nominal model and residual torque

Suppose we have a nominal model \( \mathbf{M}_0(\mathbf{q}), \mathbf{C}_0(\mathbf{q},\dot{\mathbf{q}}), \mathbf{g}_0(\mathbf{q}) \) from CAD or identification. Then the nominal torque needed to produce the measured motion is

\[ \boldsymbol{\tau}_{\text{nom,inv}} = \mathbf{M}_0(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}_0(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}_0(\mathbf{q}). \]

The torque residual is then

\[ \mathbf{d}(\mathbf{x}) = \boldsymbol{\tau} - \boldsymbol{\tau}_{\text{nom,inv}} = \boldsymbol{\tau} - \big( \mathbf{M}_0(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}_0(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}_0(\mathbf{q}) \big), \]

where the input vector \( \mathbf{x} \) typically includes \( \mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}} \), possibly desired quantities and task variables. For GP modeling we choose, for example,

\[ \mathbf{x} = \begin{bmatrix} \mathbf{q}^\top & \dot{\mathbf{q}}^\top & \ddot{\mathbf{q}}^\top \end{bmatrix}^\top \in \mathbb{R}^{3n}. \]

3.2 Vector-valued outputs via independent GPs

The residual torque \( \mathbf{d}(\mathbf{x}) \in \mathbb{R}^n \) is vector-valued. A simple and common approximation is to model each joint residual independently with its own scalar GP:

\[ d_j(\cdot) \sim \mathcal{GP}\big(0, k_j(\cdot,\cdot)\big), \quad j = 1,\dots,n, \]

trained on the same input vectors \( \mathbf{x} \) but different outputs \( y^{(j)}_i = d_j(\mathbf{x}_i) \). This ignores cross-correlation among joints but is significantly simpler and often effective in practice.

More advanced constructions use multi-output kernels to jointly model correlations between joint residuals, but we will stay with independent GPs here.

Consider the standard computed-torque / inverse-dynamics control law (with approximate model) for tracking a desired trajectory \( \mathbf{q}_d(t) \), \( \dot{\mathbf{q}}_d(t) \), \( \ddot{\mathbf{q}}_d(t) \). Define tracking error \( \mathbf{e} = \mathbf{q}_d - \mathbf{q} \), \( \dot{\mathbf{e}} = \dot{\mathbf{q}}_d - \dot{\mathbf{q}} \) and choose positive definite gain matrices \( \mathbf{K}_p, \mathbf{K}_d \).

The nominal computed-torque law based on the approximate model is

\[ \boldsymbol{\tau}_{\text{nom}} = \mathbf{M}_0(\mathbf{q}) \big( \ddot{\mathbf{q}}_d + \mathbf{K}_d \dot{\mathbf{e}} + \mathbf{K}_p \mathbf{e} \big) + \mathbf{C}_0(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}_0(\mathbf{q}). \]

If the nominal model were exact, the resulting error dynamics would (approximately) satisfy

\[ \ddot{\mathbf{e}} + \mathbf{K}_d \dot{\mathbf{e}} + \mathbf{K}_p \mathbf{e} = \mathbf{0}, \]

which is exponentially stable for appropriate gains. With model mismatch, there is an additional disturbance term.

4.1 Error dynamics with residual compensation

Let the true dynamics be

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}. \]

Substitute the GP-augmented input

\[ \boldsymbol{\tau} = \boldsymbol{\tau}_{\text{nom}} + \hat{\mathbf{d}}(\mathbf{x}), \]

where \( \hat{\mathbf{d}}(\mathbf{x}) \) is the vector of GP posterior means for each joint. After standard manipulations (similar to the computed-torque derivation) one can show

\[ \mathbf{M}(\mathbf{q}) \big(\ddot{\mathbf{e}} + \mathbf{K}_d \dot{\mathbf{e}} + \mathbf{K}_p \mathbf{e}\big) = \mathbf{d}(\mathbf{x}) - \hat{\mathbf{d}}(\mathbf{x}), \]

where \( \mathbf{d}(\mathbf{x}) \) is the true residual torque defined in Section 3. Thus the closed-loop error dynamics are nominally linear and exponentially stable, but driven by the residual prediction error \( \mathbf{d} - \hat{\mathbf{d}} \).

4.2 Practical stability with bounded GP error

Assume the standard manipulator properties (e.g., bounded, symmetric positive definite inertia matrix) so that there exist constants \( 0 < m_{\min} \leq m_{\max} \) such that

\[ m_{\min} \|\mathbf{v}\|^2 \leq \mathbf{v}^\top \mathbf{M}(\mathbf{q}) \mathbf{v} \leq m_{\max} \|\mathbf{v}\|^2 \quad \forall \mathbf{q}, \mathbf{v}. \]

Consider the Lyapunov function

\[ V(\mathbf{e},\dot{\mathbf{e}}) = \tfrac{1}{2} \dot{\mathbf{e}}^\top \mathbf{M}(\mathbf{q}) \dot{\mathbf{e}} + \tfrac{1}{2} \mathbf{e}^\top \mathbf{K}_p \mathbf{e}. \]

Using standard manipulator properties (skew-symmetry of \( \dot{\mathbf{M}} - 2\mathbf{C} \)), one can derive

\[ \dot{V} = - \dot{\mathbf{e}}^\top \mathbf{K}_d \dot{\mathbf{e}} + \dot{\mathbf{e}}^\top \big( \mathbf{d}(\mathbf{x}) - \hat{\mathbf{d}}(\mathbf{x}) \big). \]

Applying the Cauchy–Schwarz inequality,

\[ \dot{V} \leq - \lambda_{\min}(\mathbf{K}_d) \|\dot{\mathbf{e}}\|^2 + \|\dot{\mathbf{e}}\| \big\| \mathbf{d}(\mathbf{x}) - \hat{\mathbf{d}}(\mathbf{x}) \big\|. \]

If the GP prediction error is bounded, \( \big\| \mathbf{d}(\mathbf{x}) - \hat{\mathbf{d}}(\mathbf{x}) \big\| \leq \bar{\delta} \) in the region of interest, then outside a small ball of radius proportional to \( \bar{\delta} \) the negative quadratic term dominates, and the closed loop is practically exponentially stable (tracking error converges to a small neighborhood of zero). This shows how improved GP accuracy directly reduces steady-state tracking error.

GP behavior is controlled by kernel hyperparameters \( \theta = (\sigma_f^2, \ell_1,\dots,\ell_d, \sigma_n^2) \). Given training data \( (X,\mathbf{y}) \), the log marginal likelihood is

\[ \log p(\mathbf{y}\mid X,\theta) = -\tfrac{1}{2} \mathbf{y}^\top \mathbf{K}_y^{-1} \mathbf{y} -\tfrac{1}{2} \log|\mathbf{K}_y| -\tfrac{N}{2}\log(2\pi), \]

which can be maximized with respect to \( \theta \) using gradient-based methods. The gradients have closed forms in terms of \( \partial \mathbf{K}_y / \partial \theta_i \).

However, exact GP inference scales as \( \mathcal{O}(N^3) \) in the number of data points \( N \) due to matrix inversion. For real-time robot control at high servo rates, one typically:

• Limits \( N \) (e.g., uses a sliding window of recent data).
• Employs sparse GPs (inducing points) or structured approximations (beyond this lesson).
• Precomputes \( \mathbf{K}_y^{-1} \) offline and only evaluates \( \mu_\ast \) online.

A typical implementation flow is:

flowchart TD
  A["Offline: collect batch data"] --> B["Tune GP hyperparameters by marginal likelihood"]
  B --> C["Precompute inverse of Ky"]
  C --> D["Store GP parameters on controller"]
  D --> E["Online: each control cycle"]
  E --> F["Form input x from sensors + reference"]
  F --> G["Evaluate GP mean for residual"]
  G --> H["Add residual to nominal torque and send to actuators"]
        

We implement a simple GP-based residual compensator for a 1-DOF rotary joint using scikit-learn and numpy. The nominal dynamics assume a simple pendulum model; the GP learns unmodeled friction and disturbances.

import numpy as np
from numpy.linalg import norm
from sklearn.gaussian_process import GaussianProcessRegressor
from sklearn.gaussian_process.kernels import RBF, WhiteKernel

# Physical parameters (unknown to the controller, but used for data generation)
m_true = 1.0
l_true = 0.5
g = 9.81
b_true = 0.2  # viscous friction coefficient

def true_dynamics(q, dq, tau):
    """
    1-DOF rotary joint:
        I * ddq + b_true * dq + m_true * g * l_true * sin(q) = tau
    """
    I = m_true * l_true**2
    ddq = (tau - b_true * dq - m_true * g * l_true * np.sin(q)) / I
    return ddq

# Nominal model used in controller (misses friction)
m_nom = 0.9       # slightly wrong mass
l_nom = 0.45
def nominal_inverse_dynamics(q, dq, ddq):
    I0 = m_nom * l_nom**2
    tau_nom = I0 * ddq + m_nom * g * l_nom * np.sin(q)
    return tau_nom

# Generate training data under some excitation trajectory
def generate_training_data(T=5.0, dt=0.01):
    t = np.arange(0.0, T, dt)
    N = t.size
    q = 0.5 * np.sin(2.0 * t)
    dq = np.gradient(q, dt)
    ddq_des = np.gradient(dq, dt)

    X = []
    y = []

    q_curr = 0.0
    dq_curr = 0.0
    for k in range(N):
        # Nominal control: inverse dynamics for desired ddq plus PD
        e = q[k] - q_curr
        de = dq[k] - dq_curr
        kp = 50.0
        kd = 10.0
        v = ddq_des[k] + kd * de + kp * e
        tau_nom = nominal_inverse_dynamics(q_curr, dq_curr, v)

        # Apply to true plant and simulate one step
        ddq_true = true_dynamics(q_curr, dq_curr, tau_nom)
        dq_curr = dq_curr + ddq_true * dt
        q_curr = q_curr + dq_curr * dt

        # Compute residual torque that would make nominal model exact
        tau_inv_nom = nominal_inverse_dynamics(q_curr, dq_curr, ddq_true)
        d = tau_nom - tau_inv_nom  # torque residual

        x_k = np.array([q_curr, dq_curr, ddq_des[k]])
        X.append(x_k)
        y.append(d)

    return np.asarray(X), np.asarray(y)

X_train, y_train = generate_training_data()

# Fit GP on residual torque
kernel = 1.0 * RBF(length_scale=np.ones(3)) + WhiteKernel(noise_level=1e-4)
gp = GaussianProcessRegressor(kernel=kernel, alpha=0.0, normalize_y=True)
gp.fit(X_train, y_train)

print("Trained GP kernel:", gp.kernel_)

# Control loop with GP compensation
def control_with_gp(q0, dq0, qd_fun, dqd_fun, ddqd_fun, T=3.0, dt=0.002):
    t_grid = np.arange(0.0, T, dt)
    q = q0
    dq = dq0
    traj = {"t": [], "q": [], "qd": [], "dq": [], "tau": []}

    kp = 80.0
    kd = 20.0

    for t in t_grid:
        qd = qd_fun(t)
        dqd = dqd_fun(t)
        ddqd = ddqd_fun(t)

        e = qd - q
        de = dqd - dq

        v = ddqd + kd * de + kp * e
        tau_nom = nominal_inverse_dynamics(q, dq, v)

        x = np.array([[q, dq, ddqd]])
        d_hat, sigma = gp.predict(x, return_std=True)
        tau = tau_nom + d_hat[0]

        ddq_true = true_dynamics(q, dq, tau)
        dq = dq + ddq_true * dt
        q = q + dq * dt

        traj["t"].append(t)
        traj["q"].append(q)
        traj["qd"].append(qd)
        traj["dq"].append(dq)
        traj["tau"].append(tau)

    for key in traj:
        traj[key] = np.asarray(traj[key])
    return traj

# Example: track a slow sinusoid
qd_fun = lambda t: 0.3 * np.sin(1.0 * t)
dqd_fun = lambda t: 0.3 * np.cos(1.0 * t)
ddqd_fun = lambda t: -0.3 * np.sin(1.0 * t)

traj_gp = control_with_gp(0.0, 0.0, qd_fun, dqd_fun, ddqd_fun)

print("Final position error:", traj_gp["qd"][-1] - traj_gp["q"][-1])
      

In practice, you would compare the tracking performance of the nominal controller with and without GP compensation, evaluate the reduction in steady tracking error and sensitivity to disturbances, and check real-time computation time of the GP prediction.

In C++, robotics projects often rely on Eigen for linear algebra and libraries such as RBDL or KDL for rigid-body dynamics. Below is a minimal GP regression class for scalar residuals using an RBF kernel. It can be used inside a joint-space controller that already computes nominal torque from a dynamics library.

#include <Eigen/Dense>
#include <vector>

class SimpleGP1D {
public:
    using Vec = Eigen::VectorXd;
    using Mat = Eigen::MatrixXd;

    SimpleGP1D(double length_scale,
               double sigma_f,
               double sigma_n)
        : ell_(length_scale),
          sigma_f_(sigma_f),
          sigma_n_(sigma_n),
          trained_(false) {}

    void setTrainingData(const Mat& X, const Vec& y) {
        X_ = X;
        y_ = y;
        const int N = static_cast<int>(y_.size());
        Mat K(N, N);
        for (int i = 0; i < N; ++i) {
            for (int j = 0; j < N; ++j) {
                K(i, j) = kernel(X_.row(i), X_.row(j));
            }
        }
        Mat Ky = K
               + (sigma_n_ * sigma_n_) * Mat::Identity(N, N);

        // Precompute Ky^{-1} y using LDLT
        Eigen::LDLT<Mat> ldlt(Ky);
        alpha_ = ldlt.solve(y_);
        trained_ = true;
    }

    double predict(const Vec& x_star, double* var_out = nullptr) const {
        assert(trained_);
        const int N = static_cast<int>(y_.size());
        Vec k_star(N);
        for (int i = 0; i < N; ++i) {
            k_star(i) = kernel(X_.row(i), x_star.transpose());
        }
        double mean = k_star.dot(alpha_);

        if (var_out) {
            // For simplicity, we omit efficient reuse of the factorization and
            // compute Ky^{-1} k_star directly.
            const int Nloc = N;
            Mat K(Nloc, Nloc);
            for (int i = 0; i < Nloc; ++i) {
                for (int j = 0; j < Nloc; ++j) {
                    K(i, j) = kernel(X_.row(i), X_.row(j));
                }
            }
            Mat Ky = K
                   + (sigma_n_ * sigma_n_) * Mat::Identity(Nloc, Nloc);
            Eigen::LDLT<Mat> ldlt(Ky);
            Vec v = ldlt.solve(k_star);
            double k_ss = kernel(x_star.transpose(), x_star.transpose());
            *var_out = k_ss - k_star.dot(v);
        }

        return mean;
    }

private:
    double kernel(const Eigen::RowVectorXd& x1,
                  const Eigen::RowVectorXd& x2) const {
        // Isotropic RBF kernel
        double r2 = (x1 - x2).squaredNorm();
        return sigma_f_ * sigma_f_ * std::exp(-0.5 * r2 / (ell_ * ell_));
    }

    Mat X_;
    Vec y_;
    Vec alpha_;      // Ky^{-1} y
    double ell_;
    double sigma_f_;
    double sigma_n_;
    bool trained_;
};

// Example use inside a joint controller loop
double computeTorqueWithGP(
    const Eigen::VectorXd& q,
    const Eigen::VectorXd& dq,
    const Eigen::VectorXd& qd,
    const Eigen::VectorXd& dqd,
    const Eigen::VectorXd& ddqd,
    SimpleGP1D& gp,
    double tau_nom)  // scalar nominal torque for 1-DOF
{
    Eigen::VectorXd x(3);
    x(0) = q(0);
    x(1) = dq(0);
    x(2) = ddqd(0);
    double d_hat = gp.predict(x);
    double tau = tau_nom + d_hat;
    return tau;
}
      

In a full robot codebase, the nominal torque would be computed from a rigid-body dynamics library, and the GP input vector would include the relevant joint states and references. For multi-DOF robots, one can maintain one SimpleGP1D instance per joint.

In Java-based robotics stacks (for example, some ROS clients or custom middleware), one can implement GP regression using a numerical linear algebra library such as EJML. Below is a minimal sketch of a scalar GP that can be called from a joint controller:

import org.ejml.simple.SimpleMatrix;

public class SimpleGP1D {
    private SimpleMatrix X;       // N x d
    private SimpleMatrix y;       // N x 1
    private SimpleMatrix alpha;   // N x 1, Ky^{-1} y

    private double ell;
    private double sigmaF;
    private double sigmaN;
    private boolean trained = false;

    public SimpleGP1D(double ell, double sigmaF, double sigmaN) {
        this.ell = ell;
        this.sigmaF = sigmaF;
        this.sigmaN = sigmaN;
    }

    private double kernel(SimpleMatrix x1, SimpleMatrix x2) {
        SimpleMatrix diff = x1.minus(x2);
        double r2 = diff.transpose().mult(diff).get(0);
        return sigmaF * sigmaF * Math.exp(-0.5 * r2 / (ell * ell));
    }

    public void setTrainingData(double[][] Xdata, double[] ydata) {
        int N = ydata.length;
        int d = Xdata[0].length;
        X = new SimpleMatrix(N, d);
        y = new SimpleMatrix(N, 1);

        for (int i = 0; i < N; ++i) {
            y.set(i, 0, ydata[i]);
            for (int j = 0; j < d; ++j) {
                X.set(i, j, Xdata[i][j]);
            }
        }

        SimpleMatrix K = new SimpleMatrix(N, N);
        for (int i = 0; i < N; ++i) {
            for (int j = 0; j < N; ++j) {
                SimpleMatrix xi = X.extractVector(true, i);
                SimpleMatrix xj = X.extractVector(true, j);
                K.set(i, j, kernel(xi, xj));
            }
        }
        SimpleMatrix Ky = K.plus(SimpleMatrix.identity(N).scale(sigmaN * sigmaN));
        alpha = Ky.solve(y);
        trained = true;
    }

    public double predict(double[] xStar) {
        if (!trained) {
            throw new IllegalStateException("GP not trained");
        }
        int N = X.numRows();
        SimpleMatrix kStar = new SimpleMatrix(N, 1);
        SimpleMatrix xStarMat = new SimpleMatrix(1, xStar.length, true, xStar);
        for (int i = 0; i < N; ++i) {
            SimpleMatrix xi = X.extractVector(true, i);
            kStar.set(i, 0, kernel(xi, xStarMat));
        }
        double mean = kStar.transpose().mult(alpha).get(0);
        return mean;
    }
}

// Example usage inside a joint controller
public class JointController {
    private SimpleGP1D gp;
    // Nominal dynamics and PD gains are assumed to be implemented elsewhere.

    public JointController(SimpleGP1D gpModel) {
        this.gp = gpModel;
    }

    public double computeTorque(double q, double dq,
                                double qd, double dqd, double ddqd,
                                double tauNominal) {
        double[] x = new double[] { q, dq, ddqd };
        double dHat = gp.predict(x);
        return tauNominal + dHat;
    }
}
      

Here the robotics-specific part (e.g., kinematics, dynamics, ROS messaging) is decoupled from the GP code, which only requires linear algebra.

MATLAB has built-in support for Gaussian process regression via fitrgp (Statistics and Machine Learning Toolbox). Together with Robotics System Toolbox, one can easily integrate GP residuals into a Simulink controller.

% X_train: N x d matrix of inputs [q, dq, ddq_des, ...]
% y_train: N x 1 vector of torque residuals
load("training_data.mat","X_train","y_train");

gprMdl = fitrgp(X_train, y_train, ...
    "KernelFunction","squaredexponential", ...
    "KernelScale","auto", ...
    "Sigma",1e-3, ...
    "Standardize",true);

% Example: online control step (inside a control loop or Simulink MATLAB Function block)
function tau = gp_augmented_torque(q, dq, qd, dqd, ddqd)
    % Nominal computed-torque (for 1-DOF example)
    m_nom = 0.9;
    l_nom = 0.45;
    g = 9.81;
    kp = 80;
    kd = 20;

    e = qd - q;
    de = dqd - dq;
    v = ddqd + kd * de + kp * e;

    I0 = m_nom * l_nom^2;
    tau_nom = I0 * v + m_nom * g * l_nom * sin(q);

    % GP input vector must match training features
    x = [q, dq, ddqd];
    d_hat = predict(gprMdl, x);

    tau = tau_nom + d_hat;
end
      

In Simulink, one can either:

• Use a MATLAB Function block calling predict(gprMdl,...), or
• Use the Regression GP block (if available) and feed it the same features used in training.

In Wolfram Mathematica, a convenient way to perform GP regression is via Predict with method "GaussianProcess". Below is a basic residual model for a single joint.

(* Training data: list of rules {x_i -> y_i} where x_i is a feature vector *)
trainingInputs = {
  {q1, dq1, ddq1},
  {q2, dq2, ddq2},
  (* ... *)
};
trainingTargets = {d1, d2, (* ... *)};

trainingData = Thread[trainingInputs -> trainingTargets];

gpModel = Predict[
  trainingData,
  Method -> "GaussianProcess"
];

(* Nominal torque (for 1-DOF example) *)
nominalTorque[q_, dq_, qd_, dqd_, ddqd_] :=
 Module[{mNom = 0.9, lNom = 0.45, g = 9.81, kp = 80., kd = 20.,
   e, de, v, I0, tauNom},
  e = qd - q;
  de = dqd - dq;
  v = ddqd + kd*de + kp*e;
  I0 = mNom*lNom^2;
  tauNom = I0*v + mNom*g*lNom*Sin[q];
  tauNom
 ]

(* GP-augmented torque *)
gpAugmentedTorque[q_, dq_, qd_, dqd_, ddqd_] :=
 Module[{x, dHat, tauNom},
  x = {q, dq, ddqd};
  dHat = gpModel[x];
  tauNom = nominalTorque[q, dq, qd, dqd, ddqd];
  tauNom + dHat
 ]
      

The function gpAugmentedTorque can be called from a numerical integration of the manipulator dynamics, or from an external interface to a robotics simulator, to study GP error compensation effects.

Problem 1 (Posterior mean derivation): Let \( f(\cdot) \sim \mathcal{GP}(0,k(\cdot,\cdot)) \) and noisy observations \( y_i = f(x_i) + \varepsilon_i \) with \( \varepsilon_i \sim \mathcal{N}(0,\sigma_n^2) \) independently. Show that the posterior mean at a test input \( x_\ast \) is \( \mu_\ast = \mathbf{k}_\ast^\top \mathbf{K}_y^{-1} \mathbf{y} \), with notation as in Section 2.

Solution: The joint Gaussian distribution of \( [\mathbf{y}^\top, f_\ast]^\top \) is

\[ \begin{bmatrix} \mathbf{y} \\ f_\ast \end{bmatrix} \sim \mathcal{N}\!\left( \begin{bmatrix} \mathbf{0} \\ 0 \end{bmatrix}, \begin{bmatrix} \mathbf{K}_y & \mathbf{k}_\ast \\ \mathbf{k}_\ast^\top & k_{\ast\ast} \end{bmatrix}\right). \]

Using the general Gaussian conditioning formula (see Section 2.2), we have

\[ \mu_\ast = \mathbb{E}[f_\ast \mid \mathbf{y}] = \mathbf{k}_\ast^\top \mathbf{K}_y^{-1} \mathbf{y}, \]

which is exactly the claimed expression.

Problem 2 (Kernel ridge interpretation): Consider kernel ridge regression with loss

\[ J(\boldsymbol{\alpha}) = \big\| \mathbf{y} - \mathbf{K}\boldsymbol{\alpha} \big\|_2^2 + \lambda \boldsymbol{\alpha}^\top \mathbf{K} \boldsymbol{\alpha}. \]

Show that the optimal coefficient vector is \( \boldsymbol{\alpha}^\star = (\mathbf{K} + \lambda \mathbf{I})^{-1}\mathbf{y} \) and the predictor \( \hat f(x_\ast) = \mathbf{k}_\ast^\top \boldsymbol{\alpha}^\star \) equals the GP posterior mean with \( \lambda = \sigma_n^2 \).

Solution: Differentiating \( J \) with respect to \( \boldsymbol{\alpha} \) and setting to zero:

\[ \frac{\partial J}{\partial \boldsymbol{\alpha}} = -2\mathbf{K}^\top(\mathbf{y} - \mathbf{K}\boldsymbol{\alpha}) + 2\lambda \mathbf{K}\boldsymbol{\alpha} = -2\mathbf{K}(\mathbf{y} - \mathbf{K}\boldsymbol{\alpha}) + 2\lambda \mathbf{K}\boldsymbol{\alpha} = \mathbf{0}. \]

Because \( \mathbf{K} \) is symmetric, \( \mathbf{K}\mathbf{y} - \mathbf{K}^2\boldsymbol{\alpha} + \lambda \mathbf{K}\boldsymbol{\alpha} = \mathbf{0} \), so

\[ \mathbf{K}\mathbf{y} = \mathbf{K}(\mathbf{K} + \lambda \mathbf{I})\boldsymbol{\alpha}. \]

Assuming \( \mathbf{K} \) is invertible, we obtain \( \mathbf{y} = (\mathbf{K} + \lambda \mathbf{I})\boldsymbol{\alpha} \), hence

\[ \boldsymbol{\alpha}^\star = (\mathbf{K} + \lambda \mathbf{I})^{-1}\mathbf{y}. \]

Setting \( \lambda = \sigma_n^2 \) and noting that \( \mathbf{K}_y = \mathbf{K} + \sigma_n^2 \mathbf{I} \), we have \( \boldsymbol{\alpha}^\star = \mathbf{K}_y^{-1} \mathbf{y} \), so the predictor

\[ \hat f(x_\ast) = \mathbf{k}_\ast^\top \boldsymbol{\alpha}^\star = \mathbf{k}_\ast^\top \mathbf{K}_y^{-1} \mathbf{y} \]

matches the GP posterior mean \( \mu_\ast \).

Problem 3 (Residual-based error dynamics): For an \( n \)-DOF manipulator with GP residual compensation as in Section 4, show that the error dynamics satisfy

\[ \ddot{\mathbf{e}} + \mathbf{K}_d \dot{\mathbf{e}} + \mathbf{K}_p \mathbf{e} = \mathbf{M}^{-1}(\mathbf{q}) \big( \mathbf{d}(\mathbf{x}) - \hat{\mathbf{d}}(\mathbf{x}) \big). \]

Solution: Starting from the true dynamics \( \mathbf{M}\ddot{\mathbf{q}} + \mathbf{C}\dot{\mathbf{q}} + \mathbf{g} = \boldsymbol{\tau} \), substitute \( \boldsymbol{\tau} = \boldsymbol{\tau}_{\text{nom}} + \hat{\mathbf{d}}(\mathbf{x}) \), with

\[ \boldsymbol{\tau}_{\text{nom}} = \mathbf{M}_0 \big( \ddot{\mathbf{q}}_d + \mathbf{K}_d \dot{\mathbf{e}} + \mathbf{K}_p \mathbf{e} \big) + \mathbf{C}_0\dot{\mathbf{q}} + \mathbf{g}_0. \]

After adding and subtracting the true model terms and rearranging, one finds

\[ \mathbf{M}\big( \ddot{\mathbf{q}}_d + \mathbf{K}_d \dot{\mathbf{e}} + \mathbf{K}_p \mathbf{e} - \ddot{\mathbf{q}} \big) = \mathbf{d}(\mathbf{x}) - \hat{\mathbf{d}}(\mathbf{x}), \]

where \( \mathbf{d}(\mathbf{x}) \) collects modeling errors. Using \( \ddot{\mathbf{e}} = \ddot{\mathbf{q}}_d - \ddot{\mathbf{q}} \) and left multiplying by \( \mathbf{M}^{-1} \) yields the desired equation.

Problem 4 (Predictive variance at a point): Consider a scalar GP with RBF kernel and noise variance \( \sigma_n^2 \). Suppose we have two training points \( x_1, x_2 \in \mathbb{R} \) with outputs \( y_1, y_2 \), and we predict at \( x_\ast = x_1 \). Show that the predictive variance satisfies

\[ \sigma_\ast^2 = k(x_1,x_1) - \mathbf{k}_\ast^\top \mathbf{K}_y^{-1} \mathbf{k}_\ast \leq \sigma_n^2, \]

where equality holds only if \( x_2 \) is sufficiently far from \( x_1 \) (so that \( k(x_1,x_2) \approx 0 \)).

Solution: At \( x_\ast = x_1 \), the prior variance is \( k(x_1,x_1) \). Observing \( y_1 \) at the same location reduces uncertainty to at most the noise variance \( \sigma_n^2 \) because the GP must explain the noisy observation. The RBF kernel decays with distance, so as \( |x_2 - x_1| \) grows, the off-diagonal term \( k(x_1,x_2) \) becomes negligible, and the information from \( x_2 \) does not further reduce variance at \( x_1 \). A direct computation of \( \mathbf{K}_y^{-1} \) for the \( 2 \times 2 \) case confirms that the variance is bounded above by \( \sigma_n^2 \).

Problem 5 (Real-time complexity): Suppose you have \( N \) training points and a GP with exact inference. The controller runs at \( 1 \,\text{kHz} \), and each GP prediction costs \( \mathcal{O}(N) \) after precomputing \( \mathbf{K}_y^{-1} \), but updating the model with new data requires \( \mathcal{O}(N^3) \). Discuss a strategy for using GP error compensation in real time without violating the control rate.

Solution: A typical strategy is:

• Perform heavy computations offline or at a slow rate: periodically collect batches of data, retrain the GP (including matrix inversions) at \( \mathcal{O}(N^3) \), and then freeze the parameters for online use.
• During the high-rate control loop, use only the precomputed \( \mathbf{K}_y^{-1} \) to evaluate posterior means in \( \mathcal{O}(N) \) time per cycle.
• Constrain \( N \) (e.g., via a fixed-size buffer or inducing points) so that GP prediction time remains well below the sampling period.

This separation between slow model updates and fast prediction keeps the learning component compatible with real-time constraints.

In this lesson we introduced Gaussian processes as a probabilistic function approximator for residual robot dynamics. We derived the GP posterior mean and variance from the joint Gaussian prior, related them to kernel ridge regression, and embedded a GP residual model into computed-torque control to compensate unmodeled torques. A Lyapunov-based argument shows that as the GP residual prediction error becomes small and bounded, the closed-loop error dynamics approach those of the nominal controller, yielding practically exponential tracking. Finally, we explored concrete implementations in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, highlighting real-time considerations for deploying GP-based error compensation in robot controllers.