Earlier in this course, the dynamics of an \(n\)-DOF rigid robot manipulator were written as

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

where \( \mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}} \in \mathbb{R}^n \) are joint position, velocity, and acceleration, and \( \boldsymbol{\tau} \in \mathbb{R}^n \) collects the joint torques or forces.

Dynamic parameter identification assumes that the functional dependence on \( \mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}} \) is known (from rigid-body mechanics), but that the physical parameters (link masses, centers of mass, inertias, friction coefficients, etc.) are unknown. Least squares estimation uses sampled data \( (\mathbf{q}(t_k),\dot{\mathbf{q}}(t_k),\ddot{\mathbf{q}}(t_k), \boldsymbol{\tau}(t_k)) \) for \(k = 1,\dots,N\) to fit those parameters.

The key structural fact (studied in the inertia modeling chapter) is that manipulator dynamics are linear in the dynamic parameters: there exists a regressor matrix \( \mathbf{Y}(\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}) \) and a parameter vector \( \boldsymbol{\phi} \in \mathbb{R}^p \) such that

\[ \boldsymbol{\tau}(t) = \mathbf{Y}(\mathbf{q}(t),\dot{\mathbf{q}}(t),\ddot{\mathbf{q}}(t)) \boldsymbol{\phi}. \]

This allows us to treat dynamic identification as a (possibly large) linear regression problem, to which least squares provides a natural solution.

To make the structure concrete, consider a single revolute joint (a rotary pendulum) with link mass \(m\), center-of-mass distance \(\ell\) from the joint, scalar inertia about the joint \(I\), viscous friction \(b\), in gravity \(g\). Its equation of motion is

\[ (I + m\ell^2)\ddot{q}(t) + m g \ell \sin(q(t)) + b \dot{q}(t) = \tau(t). \]

Define the dynamic parameters

\[ a_1 = I + m\ell^2,\quad a_2 = m g \ell,\quad a_3 = b, \]

and collect them into \( \boldsymbol{\phi} = [a_1\ a_2\ a_3]^{\mathsf{T}} \). For each time instant \(t\),

\[ \tau(t) = \begin{bmatrix} \ddot{q}(t) & \sin(q(t)) & \dot{q}(t) \end{bmatrix} \boldsymbol{\phi}. \]

For a sequence of samples \(t_1,\dots,t_N\), define

\[ \mathbf{Y}_k = \begin{bmatrix} \ddot{q}(t_k) & \sin(q(t_k)) & \dot{q}(t_k) \end{bmatrix},\quad \tau_k = \tau(t_k). \]

Stacking all samples for this 1-DOF example yields

\[ \mathbf{T} = \mathbf{Y}\boldsymbol{\phi}, \quad \mathbf{T} = \begin{bmatrix} \tau_1\\ \vdots\\ \tau_N \end{bmatrix}, \quad \mathbf{Y} = \begin{bmatrix} \mathbf{Y}_1\\ \vdots\\ \mathbf{Y}_N \end{bmatrix} \in \mathbb{R}^{N \times 3}. \]

For an \(n\)-DOF manipulator, a similar stacking is applied to each joint equation, with a larger parameter vector \( \boldsymbol{\phi} \in \mathbb{R}^p \) (full or minimal inertial parameter set) and a regressor \( \mathbf{Y}_k \in \mathbb{R}^{n \times p} \).

In practice, measurements are noisy. We model

\[ \mathbf{T} = \mathbf{Y}\boldsymbol{\phi}^{\star} + \mathbf{e}, \]

where \( \boldsymbol{\phi}^{\star} \) are the true parameters and \( \mathbf{e} \) is a stacked error vector (sensor noise, unmodeled dynamics, discretization, etc.).

The batch least squares estimate \( \hat{\boldsymbol{\phi}} \) minimizes the sum of squared residuals:

\[ J(\boldsymbol{\phi}) = \frac{1}{2} \sum_{k=1}^{N} \left\| \mathbf{Y}_k \boldsymbol{\phi} - \boldsymbol{\tau}_k \right\|_2^2 = \frac{1}{2} \left\| \mathbf{Y}\boldsymbol{\phi} - \mathbf{T} \right\|_2^2. \]

Using vector calculus, the gradient w.r.t. \( \boldsymbol{\phi} \) is

\[ \nabla_{\boldsymbol{\phi}} J = \mathbf{Y}^{\mathsf{T}} (\mathbf{Y}\boldsymbol{\phi} - \mathbf{T}). \]

Setting \( \nabla_{\boldsymbol{\phi}} J = \mathbf{0} \) yields the normal equations:

\[ \mathbf{Y}^{\mathsf{T}}\mathbf{Y}\,\hat{\boldsymbol{\phi}} = \mathbf{Y}^{\mathsf{T}}\mathbf{T}. \]

If \( \mathbf{Y}^{\mathsf{T}}\mathbf{Y} \) is invertible (i.e., \(\mathbf{Y}\) has full column rank), the unique least squares solution is

\[ \hat{\boldsymbol{\phi}} = (\mathbf{Y}^{\mathsf{T}}\mathbf{Y})^{-1} \mathbf{Y}^{\mathsf{T}}\mathbf{T}. \]

In numerical implementations, one never forms the inverse explicitly; instead, decompositions such as QR, SVD, or Cholesky are used to solve the normal equations in a stable way.

For analysis, suppose \( \mathbf{e} \) has zero mean and covariance \( \operatorname{Cov}(\mathbf{e}) = \sigma^2 \mathbf{I} \), and that the regressor \(\mathbf{Y}\) is deterministic or independent of the noise. Then

\[ \hat{\boldsymbol{\phi}} = (\mathbf{Y}^{\mathsf{T}}\mathbf{Y})^{-1} \mathbf{Y}^{\mathsf{T}} (\mathbf{Y}\boldsymbol{\phi}^{\star} + \mathbf{e}) = \boldsymbol{\phi}^{\star} + (\mathbf{Y}^{\mathsf{T}}\mathbf{Y})^{-1} \mathbf{Y}^{\mathsf{T}}\mathbf{e}. \]

Taking expectation,

\[ \mathbb{E}[\hat{\boldsymbol{\phi}}] = \boldsymbol{\phi}^{\star} + (\mathbf{Y}^{\mathsf{T}}\mathbf{Y})^{-1} \mathbf{Y}^{\mathsf{T}}\mathbb{E}[\mathbf{e}] = \boldsymbol{\phi}^{\star}, \]

so the least squares estimator is unbiased under these assumptions. The covariance is

\[ \operatorname{Cov}(\hat{\boldsymbol{\phi}}) = (\mathbf{Y}^{\mathsf{T}}\mathbf{Y})^{-1} \mathbf{Y}^{\mathsf{T}} \operatorname{Cov}(\mathbf{e}) \mathbf{Y} (\mathbf{Y}^{\mathsf{T}}\mathbf{Y})^{-1} = \sigma^2 (\mathbf{Y}^{\mathsf{T}}\mathbf{Y})^{-1}. \]

Two key consequences for robotics:

• Identifiability. If \( \mathbf{Y} \) does not have full column rank, some directions in parameter space cannot be distinguished; the covariance matrix becomes singular and infinitely many \( \hat{\boldsymbol{\phi}} \) minimize the cost.
• Excitation quality. Even if \( \mathbf{Y}^{\mathsf{T}}\mathbf{Y} \) is invertible, a large condition number (ill-conditioning) inflates variances of some parameter combinations, making estimates numerically fragile.

For long data records, a common condition is

\[ \liminf_{N \to \infty} \frac{1}{N}\mathbf{Y}_N^{\mathsf{T}}\mathbf{Y}_N \succ \mathbf{0}, \]

where \( \mathbf{Y}_N \) is the regressor built from \(N\) samples and \( \succ \mathbf{0} \) denotes positive definiteness (all eigenvalues strictly > \(0\)). This is a formalization of persistent excitation of the trajectories designed in the previous lesson.

In robot identification, measurement errors may not have equal variance, and the regression may be ill-conditioned. Two important extensions of basic least squares address these issues.

5.1 Weighted Least Squares

Suppose \( \operatorname{Cov}(\mathbf{e}) = \mathbf{R} \) with symmetric positive definite \( \mathbf{R} \). The weighted least squares (WLS) cost is

\[ J_W(\boldsymbol{\phi}) = \frac{1}{2} (\mathbf{Y}\boldsymbol{\phi} - \mathbf{T})^{\mathsf{T}} \mathbf{R}^{-1} (\mathbf{Y}\boldsymbol{\phi} - \mathbf{T}). \]

Differentiating and setting the gradient to zero gives

\[ \hat{\boldsymbol{\phi}}_W = (\mathbf{Y}^{\mathsf{T}}\mathbf{R}^{-1}\mathbf{Y})^{-1} \mathbf{Y}^{\mathsf{T}}\mathbf{R}^{-1}\mathbf{T}. \]

In practice, \( \mathbf{R} \) is often chosen diagonal, weighting some samples or joints more heavily (e.g., higher-precision torque sensors).

5.2 Tikhonov (Ridge) Regularization

When \( \mathbf{Y}^{\mathsf{T}}\mathbf{Y} \) is nearly singular, small measurement noise can cause large parameter variations. A standard regularized cost is

\[ J_{\lambda}(\boldsymbol{\phi}) = \frac{1}{2} \left\| \mathbf{Y}\boldsymbol{\phi} - \mathbf{T} \right\|_2^2 + \frac{\lambda}{2} \left\| \boldsymbol{\phi} - \boldsymbol{\phi}_0 \right\|_2^2, \quad \lambda > 0, \]

where \( \boldsymbol{\phi}_0 \) encodes prior information (e.g., CAD parameters). The minimizer satisfies

\[ (\mathbf{Y}^{\mathsf{T}}\mathbf{Y} + \lambda \mathbf{I})\hat{\boldsymbol{\phi}}_{\lambda} = \mathbf{Y}^{\mathsf{T}}\mathbf{T} + \lambda \boldsymbol{\phi}_0. \]

The extra term \( \lambda \mathbf{I} \) shifts eigenvalues of \( \mathbf{Y}^{\mathsf{T}}\mathbf{Y} \) away from zero, improving conditioning at the cost of bias towards \( \boldsymbol{\phi}_0 \).

The high-level pipeline from data collection to estimated parameters is summarized below.

flowchart TD
  A["Plan exciting joint trajectories"] --> B["Collect samples of (q, qdot, qddot, tau)"]
  B --> C["For each sample, build regressor row Y_k"]
  C --> D["Stack rows into W and torques into T"]
  D --> E["Solve least squares: minimize ||W * phi - T||^2"]
  E --> F["Obtain parameter estimate phi_hat"]
  F --> G["Use identified model in later validation and control"]
        

We implement the batch least squares estimator for the 1-DOF pendulum model of Section 2 using NumPy. In a full manipulator setting, roboticstoolbox-python or symbolic tools can be used to build the regressor matrix.

import numpy as np

# Assume we have logged arrays of joint states and measured torque:
# q, qd, qdd, tau_meas are 1D NumPy arrays of length N
# q      : joint position [rad]
# qd     : joint velocity [rad/s]
# qdd    : joint acceleration [rad/s^2]
# tau_meas : measured motor torque [Nm]

def build_regressor_pendulum(q, qd, qdd, g=9.81):
    """
    Build the regressor matrix Y for the 1-DOF pendulum:
      tau = a1 * qdd + a2 * sin(q) + a3 * qd
    Parameters:
      q, qd, qdd : shape (N,)
    Returns:
      Y : shape (N, 3)
    """
    q = np.asarray(q)
    qd = np.asarray(qd)
    qdd = np.asarray(qdd)
    Y = np.column_stack([qdd, np.sin(q), qd])
    return Y

# Example: synthetic data with known parameters
N = 1000
t = np.linspace(0.0, 10.0, N)
q = 0.5 * np.sin(2.0 * t)  # simple excitation
qd = np.gradient(q, t)
qdd = np.gradient(qd, t)

phi_true = np.array([0.08, 2.5, 0.1])  # [a1, a2, a3]
Y = build_regressor_pendulum(q, qd, qdd)
tau_clean = Y @ phi_true
noise_std = 0.05
tau_meas = tau_clean + noise_std * np.random.randn(N)

# Least squares estimate using numpy.linalg.lstsq
phi_hat, residuals, rank, s = np.linalg.lstsq(Y, tau_meas, rcond=None)

print("True parameters:     ", phi_true)
print("Estimated parameters:", phi_hat)
print("Rank of Y:", rank)

# Predicted torques for diagnostics
tau_pred = Y @ phi_hat
rmse = np.sqrt(np.mean((tau_pred - tau_meas) ** 2))
print("RMSE on training data:", rmse)
      

For an \(n\)-DOF manipulator, build_regressor_pendulum would be replaced by a routine computing the full regressor \( \mathbf{Y}(\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}) \), and the torque vector would be stacked across joints.

In C++, the Eigen library is widely used in robotics for linear algebra. The following snippet performs least squares on the same 1-DOF pendulum regression:

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

// Build regressor for 1-DOF pendulum
Eigen::MatrixXd buildRegressorPendulum(const std::vector<double> &q,
                                       const std::vector<double> &qd,
                                       const std::vector<double> &qdd)
{
    const std::size_t N = q.size();
    Eigen::MatrixXd Y(N, 3);
    for (std::size_t k = 0; k < N; ++k) {
        Y(static_cast<int>(k), 0) = qdd[k];
        Y(static_cast<int>(k), 1) = std::sin(q[k]);
        Y(static_cast<int>(k), 2) = qd[k];
    }
    return Y;
}

int main()
{
    // Example: assume q, qd, qdd, tau_meas are filled from logs
    std::vector<double> q, qd, qdd, tau_meas;
    // ... fill data ...

    const std::size_t N = q.size();
    Eigen::MatrixXd Y = buildRegressorPendulum(q, qd, qdd);

    Eigen::VectorXd T(N);
    for (std::size_t k = 0; k < N; ++k) {
        T(static_cast<int>(k)) = tau_meas[k];
    }

    // Solve normal equations using a robust decomposition
    Eigen::MatrixXd YtY = Y.transpose() * Y;
    Eigen::VectorXd YtT = Y.transpose() * T;

    Eigen::Vector3d phi_hat =
        YtY.ldlt().solve(YtT);  // 3 parameters for the pendulum

    std::cout << "Estimated parameters (a1, a2, a3): "
              << phi_hat.transpose() << std::endl;

    return 0;
}
      

For multi-DOF robots, an analogous approach builds a larger \( \mathbf{Y} \) and torque vector \( \mathbf{T} \), with Eigen::VectorXd used for \( \boldsymbol{\phi} \) of dimension \(p\).

In Java, libraries such as Apache Commons Math or EJML provide linear algebra routines suited to least squares. Below is an example using Commons Math:

import org.apache.commons.math3.linear.MatrixUtils;
import org.apache.commons.math3.linear.RealMatrix;
import org.apache.commons.math3.linear.RealVector;
import org.apache.commons.math3.linear.DecompositionSolver;
import org.apache.commons.math3.linear.QRDecomposition;

public class PendulumLeastSquares {

    public static RealMatrix buildRegressorPendulum(double[] q,
                                                    double[] qd,
                                                    double[] qdd) {
        int N = q.length;
        double[][] data = new double[N][3];
        for (int k = 0; k < N; ++k) {
            data[k][0] = qdd[k];
            data[k][1] = Math.sin(q[k]);
            data[k][2] = qd[k];
        }
        return MatrixUtils.createRealMatrix(data);
    }

    public static void main(String[] args) {
        // Assume q, qd, qdd, tauMeas are filled from logs
        double[] q, qd, qdd, tauMeas;
        // ... allocate and fill arrays ...

        int N = q.length;
        RealMatrix Y = buildRegressorPendulum(q, qd, qdd);

        double[] tData = new double[N];
        for (int k = 0; k < N; ++k) {
            tData[k] = tauMeas[k];
        }
        RealVector T = MatrixUtils.createRealVector(tData);

        // Solve Y * phi = T in least squares sense via QR decomposition
        QRDecomposition qr = new QRDecomposition(Y);
        DecompositionSolver solver = qr.getSolver();
        RealVector phiHat = solver.solve(T);

        System.out.println("Estimated parameters (a1, a2, a3): "
                           + phiHat);
    }
}
      

For industrial Java-based robot controllers, this structure can be embedded in an offline identification tool that processes log files and exports \(\hat{\boldsymbol{\phi}}\) to the control software.

10.1 MATLAB/Simulink

MATLAB provides a concise syntax for solving least squares problems using the backslash operator. Assuming column vectors q, qd, qdd, and tau:

% Build regressor Y for 1-DOF pendulum
Y = [qdd, sin(q), qd];   % size N-by-3
T = tau;                 % size N-by-1

% Ordinary least squares estimate
phi_hat = Y \ T;         % 3-by-1 vector [a1; a2; a3]

% Predicted torque and residual analysis
tau_pred = Y * phi_hat;
residual = tau - tau_pred;
rmse = sqrt(mean(residual.^2));
disp('Estimated parameters [a1 a2 a3]^T:');
disp(phi_hat);
disp(['RMSE: ', num2str(rmse)]);
      

In Simulink, the identification procedure is usually implemented offline: log the signals \(q,\dot{q},\ddot{q},\tau\) during an excitation experiment using To Workspace blocks, then run the above script. More advanced workflows can integrate the System Identification Toolbox for validation in later lessons.

10.2 Wolfram Mathematica

Mathematica has built-in least squares functionality via LeastSquares or LinearModelFit:

(* q, qd, qdd, tau are lists of length N *)

Y = Transpose @ {
    qdd,
    Sin[q],
    qd
}; (* N x 3 regressor matrix *)

T = tau; (* N-element vector *)

phiHat = LeastSquares[Y, T]; (* {a1, a2, a3} *)

Print["Estimated parameters {a1, a2, a3}: ", phiHat];

(* Predicted torque and residual *)
tauPred = Y . phiHat;
residual = T - tauPred;
rmse = Sqrt[Mean[residual^2]];
Print["RMSE: ", rmse];
      

For symbolic robots, Mathematica can also derive \( \mathbf{Y}(\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}) \) symbolically and then generate C or MATLAB code for use in real-time control systems.

In this lesson we exploited the fact that rigid-body robot dynamics are linear in a set of inertial and friction parameters. This allowed us to cast dynamic parameter identification as a linear regression problem, derive the batch least squares estimator and its normal equations, and study unbiasedness, covariance, and identifiability via the rank of the regressor matrix. We then extended the basic formulation to weighted and regularized least squares and implemented the estimation pipeline for a 1-DOF pendulum example in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica. In the next lesson, these identified models will be evaluated using quantitative validation metrics.