Recall from previous lessons that the dynamics of an \( n \)-DOF manipulator can be written in the regressor form

\[ \boldsymbol{\tau}(t) = \mathbf{Y}\big(\mathbf{q}(t),\dot{\mathbf{q}}(t),\ddot{\mathbf{q}}(t)\big)\, \boldsymbol{\pi}, \quad \boldsymbol{\tau}(t)\in\mathbb{R}^n,\; \boldsymbol{\pi}\in\mathbb{R}^p,\; \mathbf{Y}(t)\in\mathbb{R}^{n\times p}. \]

The vector \( \boldsymbol{\pi} \) collects (possibly minimal) inertial and friction parameters. In a practical lab we do not have continuous-time signals, but sampled data at instants \( t_k = k T_s \), \( k=1,\dots,N \). Stacking all samples into one large system yields

\[ \underbrace{ \begin{bmatrix} \boldsymbol{\tau}(t_1) \\ \boldsymbol{\tau}(t_2) \\ \vdots \\ \boldsymbol{\tau}(t_N) \end{bmatrix} }_{\boldsymbol{\tau}_{\mathrm{all}}\in\mathbb{R}^{Nn}} = \underbrace{ \begin{bmatrix} \mathbf{Y}(t_1) \\ \mathbf{Y}(t_2) \\ \vdots \\ \mathbf{Y}(t_N) \end{bmatrix} }_{\mathbf{Y}_{\mathrm{all}}\in\mathbb{R}^{Nn\times p}} \boldsymbol{\pi} \;+\; \boldsymbol{\varepsilon}, \]

where \( \boldsymbol{\varepsilon} \) collects measurement noise and unmodeled effects. The identification task is then to compute an estimator \( \hat{\boldsymbol{\pi}} \) from \( \mathbf{Y}_{\mathrm{all}} \) and \( \boldsymbol{\tau}_{\mathrm{all}} \), and to validate it on separate data.

flowchart TD
  A["Design excitation trajectory (Lesson 2)"] --> B["Collect sampled data: q_k, dq_k, ddq_k, tau_k"]
  B --> C["Preprocess: filtering, finite differences"]
  C --> D["Build stacked regressor Y_all"]
  D --> E["Solve LS: pi_hat = argmin ||Y_all pi - tau_all||_2^2"]
  E --> F["Validate on separate trajectories (Lesson 4 metrics)"]
  F --> G["Update model database for controller design/use"]
        

To make the lab concrete, consider a single revolute joint (e.g., a simple robot arm or pendulum) with dynamics

\[ \tau(t) = \underbrace{(I + m\ell^2)}_{\pi_1}\ddot{q}(t) + \underbrace{m g \ell}_{\pi_2}\cos\big(q(t)\big) + \underbrace{b}_{\pi_3}\dot{q}(t), \]

where \( q(t) \) is the joint angle, \( \ell \) a known geometric length, \( I \) the link inertia about the joint, \( m \) its mass, \( b \) a viscous friction coefficient, and \( g \) the gravity constant. Notice that this equation is linear in the grouped parameters

\[ \boldsymbol{\pi} = \begin{bmatrix} \pi_1 \\ \pi_2 \\ \pi_3 \end{bmatrix} = \begin{bmatrix} I + m\ell^2 \\ m g \ell \\ b \end{bmatrix}. \]

For a single sample at time \( t_k \), the regressor row is

\[ Y_k = \big[\,\ddot{q}(t_k)\;\;\; \cos(q(t_k))\;\;\; \dot{q}(t_k)\,\big] \in \mathbb{R}^{1\times 3}, \]

so that \( \tau(t_k) = Y_k \boldsymbol{\pi} \). Stacking \( N \) samples gives

\[ \boldsymbol{\tau} = \begin{bmatrix} \tau(t_1) \\ \tau(t_2) \\ \vdots \\ \tau(t_N) \end{bmatrix} = \underbrace{ \begin{bmatrix} Y_1 \\ Y_2 \\ \vdots \\ Y_N \end{bmatrix} }_{\mathbf{Y}\in\mathbb{R}^{N\times 3}} \boldsymbol{\pi} + \boldsymbol{\varepsilon}. \]

For multi-DOF arms, the same structure holds with more columns in \( \mathbf{Y} \) and with joint-level rows gathered appropriately (each joint equation contributes one row per sample).

Suppose we record position samples \( q_k \approx q(t_k) \) at period \( T_s \). If velocity and acceleration measurements are not directly available, they can be approximated by finite differences (with appropriate filtering to reduce noise):

\[ \dot{q}_k \approx \frac{q_{k+1} - q_{k-1}}{2 T_s}, \quad \ddot{q}_k \approx \frac{q_{k+1} - 2q_k + q_{k-1}}{T_s^2}, \quad k=2,\dots,N-1. \]

For each usable index \( k \), we form the regressor row \( Y_k = [\,\ddot{q}_k\;\;\cos(q_k)\;\;\dot{q}_k\,] \) and collect the corresponding measured joint torque \( \tau_k \). The discrete-time regressor matrix is then

\[ \mathbf{Y} = \begin{bmatrix} \ddot{q}_2 & \cos(q_2) & \dot{q}_2 \\ \ddot{q}_3 & \cos(q_3) & \dot{q}_3 \\ \vdots & \vdots & \vdots \\ \ddot{q}_{N-1} & \cos(q_{N-1}) & \dot{q}_{N-1} \end{bmatrix}, \quad \boldsymbol{\tau} = \begin{bmatrix} \tau_2 \\ \tau_3 \\ \vdots \\ \tau_{N-1} \end{bmatrix}. \]

In practice, before computing differences, we typically pass the raw position data through a low-pass filter to attenuate high-frequency measurement noise that would otherwise be amplified by numerical differentiation.

flowchart TD
  Qraw["q_k (raw)"] --> F["Filter / smoothing"]
  F --> Q["q_k (filtered)"]
  Q --> D1["Central difference -> dq_k"]
  Q --> D2["Second difference -> ddq_k"]
  D1 --> Y["Build rows Y_k = [ddq_k, cos(q_k), dq_k]"]
  D2 --> Y
  T["Measured tau_k"] --> STACK["Stack tau vector and Y matrix"]
  Y --> STACK
        

With the stacked model \( \boldsymbol{\tau} = \mathbf{Y}\boldsymbol{\pi}^\star + \boldsymbol{\varepsilon} \), the standard least-squares (LS) estimator is

\[ \hat{\boldsymbol{\pi}} = \arg\min_{\boldsymbol{\pi}} \big\|\mathbf{Y}\boldsymbol{\pi} - \boldsymbol{\tau}\big\|_2^2 = \big(\mathbf{Y}^\top \mathbf{Y}\big)^{-1} \mathbf{Y}^\top \boldsymbol{\tau}, \]

provided \( \mathbf{Y}^\top\mathbf{Y} \) is invertible (i.e., \( \mathbf{Y} \) has full column rank \( p \)). Writing \( \boldsymbol{\tau} = \mathbf{Y}\boldsymbol{\pi}^\star + \boldsymbol{\varepsilon} \) with \( \mathbb{E}[\boldsymbol{\varepsilon}] = \mathbf{0} \) and covariance \( \mathbb{E}[\boldsymbol{\varepsilon}\boldsymbol{\varepsilon}^\top] = \sigma^2\mathbf{I} \), we can prove unbiasedness:

\[ \begin{aligned} \mathbb{E}[\hat{\boldsymbol{\pi}}] &= \mathbb{E}\Big[ \big(\mathbf{Y}^\top \mathbf{Y}\big)^{-1} \mathbf{Y}^\top \big(\mathbf{Y}\boldsymbol{\pi}^\star + \boldsymbol{\varepsilon}\big) \Big] \\ &= \big(\mathbf{Y}^\top \mathbf{Y}\big)^{-1} \mathbf{Y}^\top \mathbf{Y}\boldsymbol{\pi}^\star + \big(\mathbf{Y}^\top \mathbf{Y}\big)^{-1} \mathbf{Y}^\top \mathbb{E}[\boldsymbol{\varepsilon}] \\ &= \boldsymbol{\pi}^\star. \end{aligned} \]

The covariance of the estimator is

\[ \operatorname{Cov}(\hat{\boldsymbol{\pi}}) = \sigma^2 \big(\mathbf{Y}^\top \mathbf{Y}\big)^{-1}. \]

Thus, persistently exciting trajectories (Lesson 2) that make \( \mathbf{Y}^\top\mathbf{Y} \) well-conditioned are essential: a large condition number of \( \mathbf{Y}^\top\mathbf{Y} \) leads to highly sensitive parameter estimates. When numerical issues arise, a regularized estimator such as

\[ \hat{\boldsymbol{\pi}}_\lambda = \big(\mathbf{Y}^\top \mathbf{Y} + \lambda \mathbf{I}\big)^{-1} \mathbf{Y}^\top \boldsymbol{\tau}, \quad \lambda > 0, \]

can improve robustness at the price of a small bias (ridge regression).

We now construct a simple Python lab that synthesizes data for the single-link model, adds noise, and recovers the parameters using LS. We also hint at robotics-specific tooling via roboticstoolbox-python.

import numpy as np

# Sampling grid
T_s = 0.002  # 500 Hz
T_end = 8.0
t = np.arange(0.0, T_end, T_s)
N = t.size

# True (grouped) parameters: pi_1, pi_2, pi_3
pi_true = np.array([0.35, 2.1, 0.08])

# Exciting joint trajectory (smooth, multi-sine)
q = 0.6 * np.sin(2.0 * np.pi * 0.4 * t) + 0.4 * np.sin(2.0 * np.pi * 0.9 * t)

# Compute derivatives analytically for clean ground truth
dq = 0.6 * 2.0 * np.pi * 0.4 * np.cos(2.0 * np.pi * 0.4 * t) \
   + 0.4 * 2.0 * np.pi * 0.9 * np.cos(2.0 * np.pi * 0.9 * t)
ddq = -0.6 * (2.0 * np.pi * 0.4)**2 * np.sin(2.0 * np.pi * 0.4 * t) \
    - 0.4 * (2.0 * np.pi * 0.9)**2 * np.sin(2.0 * np.pi * 0.9 * t)

# Build noiseless torque from regressor form: tau = Y pi
Y = np.column_stack([ddq, np.cos(q), dq])
tau_clean = Y @ pi_true

# Add measurement noise (e.g. encoder and current sensor noise)
rng = np.random.default_rng(1)
tau_meas = tau_clean + 0.05 * rng.standard_normal(N)

# Estimate parameters using least squares
# Use a burn-in at both ends to avoid transient filtering issues
Y_id = Y[50:-50, :]
tau_id = tau_meas[50:-50]

# Normal equations solution
YTY = Y_id.T @ Y_id
YTT = Y_id.T @ tau_id
pi_hat = np.linalg.solve(YTY, YTT)

print("True parameters :", pi_true)
print("Estimated       :", pi_hat)
print("Relative error  :", (pi_hat - pi_true) / pi_true)

# Simple validation on held-out subset
Y_val = Y[:50, :]
tau_val_true = tau_clean[:50]
tau_val_pred = Y_val @ pi_hat
rms_val = np.sqrt(np.mean((tau_val_pred - tau_val_true)**2))
print("Validation RMS torque error:", rms_val)

# (Optional, robotics toolbox) - structure for a real robot
# from roboticstoolbox import DHRobot, RevoluteDH
# robot = DHRobot([RevoluteDH(a=0.5, alpha=0.0, d=0.0)])
# For each time sample: robot.rne(qk, dqk, ddqk) gives tau_k using built-in dynamics.
      

For a real experiment, q, dq, ddq, and tau_meas would be loaded from log files rather than generated synthetically, but the LS identification code is unchanged.

In C++, Eigen provides efficient linear algebra. For robotics-specific dynamics, the Rigid Body Dynamics Library (RBDL) can construct \( \mathbf{Y} \) or directly evaluate \( \tau \). Below is a minimal LS skeleton for the single-link example using Eigen; extending to RBDL models mainly changes how the regressor rows are computed.

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

// For full robots, you could also include RBDL:
// #include <rbdl/rbdl.h>

int main() {
    using Eigen::VectorXd;
    using Eigen::MatrixXd;

    const double Ts = 0.002;
    const double Tend = 8.0;
    const int N = static_cast<int>(Tend / Ts);

    std::vector<double> t(N);
    for (int k = 0; k < N; ++k) {
        t[k] = k * Ts;
    }

    // Generate exciting trajectory
    VectorXd q(N), dq(N), ddq(N);
    const double pi = 3.141592653589793;
    for (int k = 0; k < N; ++k) {
        double tk = t[k];
        q(k) = 0.6 * std::sin(2.0 * pi * 0.4 * tk)
             + 0.4 * std::sin(2.0 * pi * 0.9 * tk);
        dq(k) = 0.6 * 2.0 * pi * 0.4 * std::cos(2.0 * pi * 0.4 * tk)
              + 0.4 * 2.0 * pi * 0.9 * std::cos(2.0 * pi * 0.9 * tk);
        ddq(k) = -0.6 * std::pow(2.0 * pi * 0.4, 2) * std::sin(2.0 * pi * 0.4 * tk)
               - 0.4 * std::pow(2.0 * pi * 0.9, 2) * std::sin(2.0 * pi * 0.9 * tk);
    }

    // True parameters
    VectorXd pi_true(3);
    pi_true << 0.35, 2.1, 0.08;

    // Build regressor and torque
    MatrixXd Y(N, 3);
    VectorXd tau(N);
    for (int k = 0; k < N; ++k) {
        Y(k, 0) = ddq(k);
        Y(k, 1) = std::cos(q(k));
        Y(k, 2) = dq(k);
    }
    tau = Y * pi_true;

    // Least-squares estimate: pi_hat = (Y^T Y)^(-1) Y^T tau
    MatrixXd YTY = Y.transpose() * Y;
    VectorXd YTtau = Y.transpose() * tau;
    VectorXd pi_hat = YTY.ldlt().solve(YTtau);

    std::cout << "pi_hat = " << pi_hat.transpose() << std::endl;
    return 0;
}
      

For a full multi-DOF robot using RBDL, one would build a model from a URDF, compute inverse dynamics at each sample to obtain \( \tau_k \), and then form regressors either by analytic expressions or via built-in RBDL functions that expose parameter sensitivities.

Java does not have as many dedicated robotics libraries, but general linear-algebra toolkits like EJML can be used to implement LS identification. The regressor computation is identical to the Python and C++ examples; only the matrix operations change.

import org.ejml.simple.SimpleMatrix;

public class SingleLinkIdentification {

    public static void main(String[] args) {
        double Ts = 0.002;
        double Tend = 8.0;
        int N = (int) (Tend / Ts);
        double[] t = new double[N];

        for (int k = 0; k < N; ++k) {
            t[k] = k * Ts;
        }

        double[] q = new double[N];
        double[] dq = new double[N];
        double[] ddq = new double[N];
        double piVal = Math.PI;

        for (int k = 0; k < N; ++k) {
            double tk = t[k];
            q[k] = 0.6 * Math.sin(2.0 * piVal * 0.4 * tk)
                 + 0.4 * Math.sin(2.0 * piVal * 0.9 * tk);
            dq[k] = 0.6 * 2.0 * piVal * 0.4 * Math.cos(2.0 * piVal * 0.4 * tk)
                  + 0.4 * 2.0 * piVal * 0.9 * Math.cos(2.0 * piVal * 0.9 * tk);
            ddq[k] = -0.6 * Math.pow(2.0 * piVal * 0.4, 2) * Math.sin(2.0 * piVal * 0.4 * tk)
                    -0.4 * Math.pow(2.0 * piVal * 0.9, 2) * Math.sin(2.0 * piVal * 0.9 * tk);
        }

        // Build Y and tau
        SimpleMatrix Y = new SimpleMatrix(N, 3);
        SimpleMatrix tau = new SimpleMatrix(N, 1);
        double[] piTrue = {0.35, 2.1, 0.08};

        for (int k = 0; k < N; ++k) {
            double row0 = ddq[k];
            double row1 = Math.cos(q[k]);
            double row2 = dq[k];
            Y.set(k, 0, row0);
            Y.set(k, 1, row1);
            Y.set(k, 2, row2);
            double tauVal = row0 * piTrue[0] + row1 * piTrue[1] + row2 * piTrue[2];
            tau.set(k, 0, tauVal);
        }

        // pi_hat = (Y^T Y)^(-1) Y^T tau
        SimpleMatrix YTY = Y.transpose().mult(Y);
        SimpleMatrix YTtau = Y.transpose().mult(tau);
        SimpleMatrix piHat = YTY.invert().mult(YTtau);

        System.out.println("Estimated parameters:");
        piHat.print();
    }
}
      

For more complex robots, Java-based simulators (e.g., those used in educational robotics platforms) can be used to generate the data, while EJML or Apache Commons Math handles the identification step.

MATLAB is widely used for robotic identification, with Simscape Multibody and Robotics System Toolbox providing high-fidelity models. Below we outline a script that assumes you have logged data q_k, dq_k, ddq_k, and tau_k from Simulink or from hardware.

% Suppose we have vectors q, dq, ddq, tau_meas of equal length N
% (e.g., exported from a Simulink model of the joint).
N = numel(q);

% Build regressor for single-link example
Y = [ddq(:), cos(q(:)), dq(:)];    % N-by-3
tau_vec = tau_meas(:);            % N-by-1

% Remove a few samples from both ends to avoid differentiation artifacts
idx = 10:(N-10);
Y_id = Y(idx, :);
tau_id = tau_vec(idx);

% Least-squares estimate (normal equations)
pi_hat = (Y_id' * Y_id) \ (Y_id' * tau_id);

fprintf('Estimated parameters: pi_1 = %.4f, pi_2 = %.4f, pi_3 = %.4f\n', ...
        pi_hat(1), pi_hat(2), pi_hat(3));

% Validation on a different trajectory data set q_val, dq_val, ddq_val
Y_val = [ddq_val(:), cos(q_val(:)), dq_val(:)];
tau_pred = Y_val * pi_hat;
rms_val = sqrt(mean((tau_pred - tau_val(:)).^2));
fprintf('Validation RMS torque error: %.4f\n', rms_val);

% In a Simulink workflow:
% 1) Model the joint with Simscape Multibody, including inertia and friction.
% 2) Drive q(t) with a multi-sine excitation via a trajectory block.
% 3) Log q, dq, ddq, tau (e.g., from the joint actuator block).
% 4) Export the log to the workspace and run this identification script.
      

For multi-DOF robots, robotics.RigidBodyTree can be used to generate symbolic or numeric dynamics, and the regressor can be built joint-by-joint before solving the global LS problem.

Mathematica is well suited for combining symbolic derivation of the regressor with numeric LS estimation. For the single-link example, we can derive \( Y_k \) symbolically and then evaluate it on measured data.

(* Symbolic regressor for single link *)
Clear[q, dq, ddq];
Ysym = {ddq, Cos[q], dq};   (* Y_k = [ddq_k, cos(q_k), dq_k] *)

(* Suppose data has been imported as lists qData, dqData, ddqData, tauData *)
Ynum = Transpose[{ddqData, Cos[qData], dqData}];
tauVec = tauData;

(* Convert to matrices *)
Ymat = N @ Ynum;
taumat = N @ tauVec;

(* Least-squares estimate *)
piHat = LinearSolve[Transpose[Ymat].Ymat, Transpose[Ymat].taumat]

(* Validation on separate trajectory *)
Yval = Transpose[{ddqVal, Cos[qVal], dqVal}];
tauPred = Yval.piHat;
rmsVal = Sqrt[Mean[(tauPred - tauVal)^2]]
      

For multi-DOF manipulators, symbolic tools such as TensorReduce and Simplify help derive minimal parameter sets and verify linearity of the dynamics with respect to those parameters before numerical estimation.

In this lab we implemented a full dynamic parameter identification pipeline for a single robot joint, starting from sampled motion and torque data. We showed how to construct the regressor \( \mathbf{Y} \) from filtered trajectories, formulated the LS identification problem, and analyzed unbiasedness and covariance of the estimator. Practical implementations in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica illustrated how the same mathematical structure is realized across platforms, and how cross-validation metrics from the previous lesson can be used to judge model quality. The same principles extend directly to multi-DOF manipulators and floating-base robots once the appropriate regressor structure is derived.