Let \( \mathbf{q}(t) \in \mathbb{R}^n \) denote the joint coordinates of a serial manipulator and \( \mathbf{x}(t) \in \mathbb{R}^m \) a task-space quantity (e.g., end-effector position) with kinematics

\[ \mathbf{x}(t) = \mathbf{f}(\mathbf{q}(t)). \]

In many applications we only have sampled measurements \( \mathbf{q}_k \approx \mathbf{q}(t_k) \) from encoders at discrete times \( t_k = t_0 + k h \) with sampling period \( h > 0 \). From these samples we want:

• Joint velocities \( \dot{\mathbf{q}}(t_k) \) and accelerations \( \ddot{\mathbf{q}}(t_k) \).
• Task-space velocities \( \dot{\mathbf{x}}(t_k) \) and accelerations \( \ddot{\mathbf{x}}(t_k) \).

Two main strategies are:

1. Numerical differentiation: apply finite-difference formulas directly to the sampled data \( \mathbf{q}_k \) and \( \mathbf{x}_k \).
2. Model-based differentiation: use known kinematic and dynamic models to compute derivatives analytically from smooth parameterizations of \( \mathbf{q}(t) \) or from torques and state variables.

At a high level, numerical differentiation is simple and model-free but highly sensitive to measurement noise, whereas model-based differentiation is less sensitive to sampling noise but depends on model accuracy and parameter identification.

flowchart TD
  A["Sampled encoders q_k"] --> B["Preprocess (filter / resample)"]
  B --> C["Numerical diff: finite differences on q_k"]
  B --> D["Fit smooth trajectory model q(t)"]
  D --> E["Model-based diff: analytic dq/dt, ddq/dt2"]
  C --> F["Task-space via kinematics"]
  E --> F
  F --> G["Robot velocities/accelerations used in control, analysis"]
        

Consider a scalar component \( q(t) \) of the joint trajectory, sampled as \( q_k = q(t_k) \) with uniform step size \( h \). Classical finite-difference approximations of \( \dot{q}(t_k) \) are:

\[ \begin{aligned} \text{Forward difference:}\quad \dot{q}_k^{\text{f}} &= \frac{q_{k+1} - q_k}{h}, \\ \text{Backward difference:}\quad \dot{q}_k^{\text{b}} &= \frac{q_k - q_{k-1}}{h}, \\ \text{Central difference:}\quad \dot{q}_k^{\text{c}} &= \frac{q_{k+1} - q_{k-1}}{2h}. \end{aligned} \]

Assume \( q \) is sufficiently smooth (three times continuously differentiable). By Taylor expansion around \( t_k \) we obtain

\[ \begin{aligned} q(t_k + h) &= q(t_k) + h\dot{q}(t_k) + \frac{h^2}{2}\ddot{q}(t_k) + \frac{h^3}{6}q^{(3)}(\xi_1), \\ q(t_k - h) &= q(t_k) - h\dot{q}(t_k) + \frac{h^2}{2}\ddot{q}(t_k) - \frac{h^3}{6}q^{(3)}(\xi_2), \end{aligned} \]

for some \( \xi_1, \xi_2 \) between \( t_k - h \) and \( t_k + h \). Subtracting these equations gives

\[ q(t_k + h) - q(t_k - h) = 2h\dot{q}(t_k) + \frac{h^3}{6}\bigl(q^{(3)}(\xi_1) + q^{(3)}(\xi_2)\bigr). \]

Hence the central difference approximation satisfies

\[ \dot{q}_k^{\text{c}} = \frac{q_{k+1} - q_{k-1}}{2h} = \dot{q}(t_k) + \mathcal{O}(h^2), \]

meaning the truncation error decays quadratically with \( h \) for smooth trajectories. Forward and backward differences have only first-order accuracy: \( \dot{q}_k^{\text{f}} = \dot{q}(t_k) + \mathcal{O}(h) \), similarly for \( \dot{q}_k^{\text{b}} \).

For second derivatives, a standard central difference formula is

\[ \ddot{q}_k^{\text{c2}} = \frac{q_{k+1} - 2 q_k + q_{k-1}}{h^2} = \ddot{q}(t_k) + \mathcal{O}(h^2). \]

For vector-valued joints \( \mathbf{q}(t) \in \mathbb{R}^n \) these formulas act componentwise. In matrix form, central differences can be written as

\[ \dot{\mathbf{q}}^{\text{c}} = \mathbf{D}_c \mathbf{q}, \quad \ddot{\mathbf{q}}^{\text{c2}} = \mathbf{D}_{c2} \mathbf{q}, \]

where \( \mathbf{D}_c \) and \( \mathbf{D}_{c2} \) are banded difference matrices implementing the above stencils (with suitable one-sided formulas at the boundaries).

Let encoder measurements be corrupted by additive noise: \( y_k = q_k + \eta_k \) with \( \mathbb{E}[\eta_k] = 0 \) and \( \mathbb{E}[\eta_k^2] = \sigma_\eta^2 \), independent across \( k \). Applying the central difference to \( y_k \) yields

\[ \dot{y}_k^{\text{c}} = \frac{y_{k+1} - y_{k-1}}{2h} = \dot{q}_k^{\text{c}} + \frac{\eta_{k+1} - \eta_{k-1}}{2h}. \]

The noise component is a linear combination of independent noises. Its variance is

\[ \operatorname{Var}\!\left[\frac{\eta_{k+1} - \eta_{k-1}}{2h}\right] = \frac{1}{4h^2}\bigl(\operatorname{Var}[\eta_{k+1}] + \operatorname{Var}[\eta_{k-1}]\bigr) = \frac{\sigma_\eta^2}{2 h^2}. \]

Thus, decreasing \( h \) reduces truncation error but amplifies noise variance as \( \mathcal{O}(1/h^2) \). This is the fundamental trade-off of numerical differentiation from noisy measurements.

From a signal-processing perspective, central difference differentiation is a discrete-time filter with impulse response kernel

\[ k = \frac{1}{2h}\begin{bmatrix} -1 & 0 & 1 \end{bmatrix}, \]

which emphasizes high-frequency components (including noise). In practice, numerical differentiation in robotics is almost always combined with smoothing, low-pass filtering, or fitting a smooth trajectory model (splines, polynomials) prior to differentiation.

For a manipulator with differentiable forward kinematics \( \mathbf{x} = \mathbf{f}(\mathbf{q}) \), the task-space velocity is given by the Jacobian \( \mathbf{J}(\mathbf{q}) \):

\[ \dot{\mathbf{x}} = \mathbf{J}(\mathbf{q}) \, \dot{\mathbf{q}}, \quad \mathbf{J}(\mathbf{q}) = \frac{\partial \mathbf{f}}{\partial \mathbf{q}}(\mathbf{q}) \in \mathbb{R}^{m \times n}. \]

Differentiating again yields the task-space acceleration:

\[ \ddot{\mathbf{x}} = \mathbf{J}(\mathbf{q}) \, \ddot{\mathbf{q}} + \dot{\mathbf{J}}(\mathbf{q},\dot{\mathbf{q}})\, \dot{\mathbf{q}}, \quad \dot{\mathbf{J}}(\mathbf{q},\dot{\mathbf{q}}) = \sum_{i=1}^{n} \frac{\partial \mathbf{J}}{\partial q_i}(\mathbf{q}) \, \dot{q}_i. \]

If we have a smooth analytic parameterization of the joint trajectory, e.g. polynomial splines or trigonometric functions, then \( \dot{\mathbf{q}}(t) \) and \( \ddot{\mathbf{q}}(t) \) can be computed exactly from those parameterizations, and model-based differentiation gives closed-form expressions for \( \dot{\mathbf{x}}(t) \) and \( \ddot{\mathbf{x}}(t) \).

Similarly, from dynamics, the joint accelerations satisfy

\[ \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{M}(\mathbf{q}) \) is the inertia matrix, \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) collects Coriolis/centrifugal effects, \( \mathbf{g}(\mathbf{q}) \) is gravity, and \( \boldsymbol{\tau} \) is the vector of joint torques. If the model is known and \( \mathbf{M}(\mathbf{q}) \) is invertible, we can compute

\[ \ddot{\mathbf{q}} = \mathbf{M}(\mathbf{q})^{-1} \bigl(\boldsymbol{\tau} - \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} - \mathbf{g}(\mathbf{q}) \bigr), \]

and obtain task-space accelerations by substitution into the kinematic formula. In this way, we differentiate trajectories implicitly using the equations of motion instead of applying finite differences to noisy signals.

The error of model-based differentiation is dominated by model uncertainty and parametric errors. If \( \mathbf{p} \) denotes a vector of inertial parameters and \( \ddot{\mathbf{q}} = \mathbf{f}(\mathbf{q},\dot{\mathbf{q}},\boldsymbol{\tau};\mathbf{p}) \), then for small parameter error \( \Delta\mathbf{p} \):

\[ \ddot{\mathbf{q}}(\mathbf{p} + \Delta\mathbf{p}) \approx \ddot{\mathbf{q}}(\mathbf{p}) + \frac{\partial \mathbf{f}}{\partial \mathbf{p}}(\mathbf{q},\dot{\mathbf{q}},\boldsymbol{\tau};\mathbf{p}) \, \Delta\mathbf{p}, \]

so the derivative error is approximately linear in the parameter error, in contrast to the strong nonlinear dependence on sampling step and noise found in numerical differentiation.

Consider a simple 2R planar manipulator with link lengths \( \ell_1, \ell_2 > 0 \) and joint coordinates \( \mathbf{q} = [q_1\ q_2]^\top \). The forward kinematics for end-effector position \( \mathbf{x} = [x\ y]^\top \) are

\[ \begin{aligned} x(\mathbf{q}) &= \ell_1 \cos q_1 + \ell_2 \cos(q_1 + q_2), \\ y(\mathbf{q}) &= \ell_1 \sin q_1 + \ell_2 \sin(q_1 + q_2). \end{aligned} \]

The Jacobian is

\[ \mathbf{J}(\mathbf{q}) = \begin{bmatrix} -\ell_1 \sin q_1 - \ell_2 \sin(q_1 + q_2) & -\ell_2 \sin(q_1 + q_2) \\ \ \ \ell_1 \cos q_1 + \ell_2 \cos(q_1 + q_2) & \ \ \ell_2 \cos(q_1 + q_2) \end{bmatrix}. \]

Suppose the joint trajectory is given analytically by \( q_1(t) = \sin(\omega t) \) and \( q_2(t) = \cos(\omega t) \). Then

\[ \dot{q}_1(t) = \omega \cos(\omega t), \quad \dot{q}_2(t) = -\omega \sin(\omega t). \]

The model-based task-space velocity is \( \dot{\mathbf{x}}(t) = \mathbf{J}(\mathbf{q}(t))\,\dot{\mathbf{q}}(t) \). Alternatively, we can compute \( \mathbf{x}(t) \) from the forward kinematics and then approximate \( \dot{\mathbf{x}}(t_k) \) via central differences:

\[ \dot{\mathbf{x}}^{\text{num}}(t_k) = \frac{\mathbf{x}(t_{k+1}) - \mathbf{x}(t_{k-1})}{2h}. \]

For sufficiently small \( h \) and noise-free evaluation of \( \mathbf{x}(t_k) \), both methods approximate the same continuous-time derivative. However, if \( \mathbf{q}(t) \) is measured with noise, numerical differentiation on \( \mathbf{x} \) amplifies this noise, whereas the model-based method using analytic \( \dot{\mathbf{q}}(t) \) preserves the smoothness of the analytic trajectory.

6.1 Python (NumPy, optional roboticstoolbox)

The Python example below computes joint and task-space velocities for the planar 2R arm using both central differences and the analytic Jacobian. Libraries such as roboticstoolbox-python can provide ready-made kinematic and dynamic models; here we implement the kinematics explicitly to keep the dependence on previous lessons clear.

import numpy as np

def planar2r_fk(q, l1, l2):
    q1, q2 = q
    x = l1 * np.cos(q1) + l2 * np.cos(q1 + q2)
    y = l1 * np.sin(q1) + l2 * np.sin(q1 + q2)
    return np.array([x, y])

def planar2r_jacobian(q, l1, l2):
    q1, q2 = q
    s1 = np.sin(q1)
    c1 = np.cos(q1)
    s12 = np.sin(q1 + q2)
    c12 = np.cos(q1 + q2)
    J = np.array([
        [-l1 * s1 - l2 * s12, -l2 * s12],
        [ l1 * c1 + l2 * c12,  l2 * c12]
    ])
    return J

def central_diff(x, h):
    x = np.asarray(x)
    dx = np.zeros_like(x)
    # one-sided at boundaries
    dx[0]  = (x[1] - x[0]) / h
    dx[-1] = (x[-1] - x[-2]) / h
    for k in range(1, len(x) - 1):
        dx[k] = (x[k + 1] - x[k - 1]) / (2.0 * h)
    return dx

# trajectory definition
l1, l2 = 1.0, 0.7
omega = 1.5
T = 5.0
N = 501
t = np.linspace(0.0, T, N)
h = t[1] - t[0]

q = np.zeros((N, 2))
for k in range(N):
    q[k, 0] = np.sin(omega * t[k])
    q[k, 1] = np.cos(omega * t[k])

# joint velocities: numerical (componentwise)
dq_num = np.zeros_like(q)
dq_num[:, 0] = central_diff(q[:, 0], h)
dq_num[:, 1] = central_diff(q[:, 1], h)

# joint velocities: analytic
dq_ana = np.zeros_like(q)
dq_ana[:, 0] = omega * np.cos(omega * t)
dq_ana[:, 1] = -omega * np.sin(omega * t)

# task-space trajectories and velocities
x = np.zeros((N, 2))
xd_num = np.zeros((N, 2))
xd_model = np.zeros((N, 2))
for k in range(N):
    x[k] = planar2r_fk(q[k], l1, l2)

# numerical task-space velocity
xd_num[:, 0] = central_diff(x[:, 0], h)
xd_num[:, 1] = central_diff(x[:, 1], h)

# model-based task-space velocity (using analytic dq_ana)
for k in range(N):
    J = planar2r_jacobian(q[k], l1, l2)
    xd_model[k] = J @ dq_ana[k]

# simple RMS error comparison
rms_err_num = np.sqrt(np.mean((xd_num - xd_model) ** 2))
print("RMS difference between numerical and model-based xdot:", rms_err_num)

# Note: with noise added to q, xd_num will deviate much more than xd_model.
      

6.2 C++ (Eigen and Central Differences)

In C++, libraries such as Eigen, RBDL, or Pinocchio are commonly used for matrix computations and rigid-body dynamics. The snippet below uses Eigen to implement central differences and the planar 2R Jacobian.

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

using Vec2 = Eigen::Vector2d;
using Mat2 = Eigen::Matrix2d;

Vec2 planar2rFK(const Vec2& q, double l1, double l2) {
    double q1 = q(0);
    double q2 = q(1);
    double x = l1 * std::cos(q1) + l2 * std::cos(q1 + q2);
    double y = l1 * std::sin(q1) + l2 * std::sin(q1 + q2);
    return Vec2(x, y);
}

Mat2 planar2rJ(const Vec2& q, double l1, double l2) {
    double q1 = q(0);
    double q2 = q(1);
    double s1 = std::sin(q1);
    double c1 = std::cos(q1);
    double s12 = std::sin(q1 + q2);
    double c12 = std::cos(q1 + q2);

    Mat2 J;
    J(0, 0) = -l1 * s1 - l2 * s12;
    J(0, 1) = -l2 * s12;
    J(1, 0) =  l1 * c1 + l2 * c12;
    J(1, 1) =  l2 * c12;
    return J;
}

void centralDiff(const std::vector<Vec2>& x, double h, std::vector<Vec2>& dx) {
    std::size_t N = x.size();
    dx.resize(N);
    if (N < 2) {
        return;
    }
    dx[0].setZero();
    dx[N - 1].setZero();
    for (std::size_t k = 1; k + 1 < N; ++k) {
        dx[k] = (x[k + 1] - x[k - 1]) / (2.0 * h);
    }
}

int main() {
    double l1 = 1.0;
    double l2 = 0.7;
    double omega = 1.5;
    double T = 5.0;
    std::size_t N = 501;
    double h = T / (static_cast<double>(N - 1));

    std::vector<Vec2> q(N), dq_ana(N), x(N), xd_num(N), xd_model(N);

    for (std::size_t k = 0; k < N; ++k) {
        double t = static_cast<double>(k) * h;
        q = std::sin(omega * t);
        q = std::cos(omega * t);

        dq_ana = omega * std::cos(omega * t);
        dq_ana = -omega * std::sin(omega * t);

        x[k] = planar2rFK(q[k], l1, l2);
    }

    // numerical differentiation of x
    centralDiff(x, h, xd_num);

    // model-based xdot
    for (std::size_t k = 0; k < N; ++k) {
        Mat2 J = planar2rJ(q[k], l1, l2);
        xd_model[k] = J * dq_ana[k];
    }

    // (RMS error computation omitted for brevity)
    return 0;
}
      

6.3 Java (EJML)

In Java, matrix libraries such as EJML or Apache Commons Math can be used to implement the same operations. Below we show a skeletal implementation using EJML (org.ejml.simple.SimpleMatrix).

import org.ejml.simple.SimpleMatrix;
import java.util.ArrayList;
import java.util.List;

public class Planar2RDiff {

    static SimpleMatrix fk(SimpleMatrix q, double l1, double l2) {
        double q1 = q.get(0);
        double q2 = q.get(1);
        double x = l1 * Math.cos(q1) + l2 * Math.cos(q1 + q2);
        double y = l1 * Math.sin(q1) + l2 * Math.sin(q1 + q2);
        return new SimpleMatrix(2, 1, true, x, y);
    }

    static SimpleMatrix jacobian(SimpleMatrix q, double l1, double l2) {
        double q1 = q.get(0);
        double q2 = q.get(1);
        double s1 = Math.sin(q1);
        double c1 = Math.cos(q1);
        double s12 = Math.sin(q1 + q2);
        double c12 = Math.cos(q1 + q2);

        double[] data = {
            -l1 * s1 - l2 * s12, -l2 * s12,
             l1 * c1 + l2 * c12,  l2 * c12
        };
        return new SimpleMatrix(2, 2, true, data);
    }

    static List<SimpleMatrix> centralDiff(List<SimpleMatrix> samples, double h) {
        int N = samples.size();
        List<SimpleMatrix> deriv = new ArrayList<>(N);
        for (int k = 0; k < N; ++k) {
            deriv.add(new SimpleMatrix(samples.get(0).numRows(), 1));
        }
        if (N < 2) {
            return deriv;
        }
        deriv.set(0, new SimpleMatrix(samples.get(0).numRows(), 1));
        deriv.set(N - 1, new SimpleMatrix(samples.get(0).numRows(), 1));
        for (int k = 1; k + 1 < N; ++k) {
            SimpleMatrix num = samples.get(k + 1).minus(samples.get(k - 1));
            deriv.set(k, num.divide(2.0 * h));
        }
        return deriv;
    }

    public static void main(String[] args) {
        double l1 = 1.0;
        double l2 = 0.7;
        double omega = 1.5;
        double T = 5.0;
        int N = 501;
        double h = T / (N - 1.0);

        List<SimpleMatrix> q = new ArrayList<>(N);
        List<SimpleMatrix> dqAna = new ArrayList<>(N);
        List<SimpleMatrix> x = new ArrayList<>(N);

        for (int k = 0; k < N; ++k) {
            double t = k * h;
            SimpleMatrix qk = new SimpleMatrix(2, 1, true,
                    Math.sin(omega * t),
                    Math.cos(omega * t));
            q.add(qk);

            SimpleMatrix dqk = new SimpleMatrix(2, 1, true,
                    omega * Math.cos(omega * t),
                    -omega * Math.sin(omega * t));
            dqAna.add(dqk);

            x.add(fk(qk, l1, l2));
        }

        List<SimpleMatrix> xdNum = centralDiff(x, h);
        List<SimpleMatrix> xdModel = new ArrayList<>(N);
        for (int k = 0; k < N; ++k) {
            SimpleMatrix J = jacobian(q.get(k), l1, l2);
            xdModel.add(J.mult(dqAna.get(k)));
        }

        // Comparison of xdNum vs xdModel can be added here.
    }
}
      

6.4 MATLAB/Simulink

MATLAB offers both basic differentiation tools (diff, gradient) and robotics-specific functions in the Robotics System Toolbox. The script below compares numerical differentiation with a model-based velocity using the planar 2R arm. In Simulink, equivalent behavior can be built with Derivative blocks and Robot Kinematics blocks using a rigidBodyTree model.

% Planar 2R parameters
l1 = 1.0;
l2 = 0.7;
omega = 1.5;
T = 5.0;
N = 501;
t = linspace(0, T, N);
h = t(2) - t(1);

q1 = sin(omega * t);
q2 = cos(omega * t);

% joint velocities: numerical (central differences)
dq1_num = zeros(1, N);
dq2_num = zeros(1, N);
dq1_num(1) = (q1(2) - q1(1)) / h;
dq1_num(end) = (q1(end) - q1(end - 1)) / h;
dq2_num(1) = (q2(2) - q2(1)) / h;
dq2_num(end) = (q2(end) - q2(end - 1)) / h;
for k = 2:N-1
    dq1_num(k) = (q1(k + 1) - q1(k - 1)) / (2 * h);
    dq2_num(k) = (q2(k + 1) - q2(k - 1)) / (2 * h);
end

% joint velocities: analytic
dq1_ana = omega * cos(omega * t);
dq2_ana = -omega * sin(omega * t);

% task-space position
x = l1 * cos(q1) + l2 * cos(q1 + q2);
y = l1 * sin(q1) + l2 * sin(q1 + q2);

% task-space velocity: numerical differentiation of x,y
xd_num = zeros(1, N);
yd_num = zeros(1, N);
xd_num(1) = (x(2) - x(1)) / h;
xd_num(end) = (x(end) - x(end - 1)) / h;
yd_num(1) = (y(2) - y(1)) / h;
yd_num(end) = (y(end) - y(end - 1)) / h;
for k = 2:N-1
    xd_num(k) = (x(k + 1) - x(k - 1)) / (2 * h);
    yd_num(k) = (y(k + 1) - y(k - 1)) / (2 * h);
end

% task-space velocity: model-based via Jacobian
xd_model = zeros(1, N);
yd_model = zeros(1, N);
for k = 1:N
    s1 = sin(q1(k));
    c1 = cos(q1(k));
    s12 = sin(q1(k) + q2(k));
    c12 = cos(q1(k) + q2(k));
    J = [ -l1 * s1 - l2 * s12, -l2 * s12;
           l1 * c1 + l2 * c12,  l2 * c12 ];
    dq = [dq1_ana(k); dq2_ana(k)];
    v = J * dq;
    xd_model(k) = v(1);
    yd_model(k) = v(2);
end

% Simple comparison plot
figure;
subplot(2, 1, 1);
plot(t, xd_num, 'b', t, xd_model, 'r--');
legend('xd num', 'xd model');
xlabel('t');
ylabel('xd');

subplot(2, 1, 2);
plot(t, yd_num, 'b', t, yd_model, 'r--');
legend('yd num', 'yd model');
xlabel('t');
ylabel('yd');
      

6.5 Wolfram Mathematica (Symbolic Model-Based Differentiation)

Mathematica is particularly convenient for symbolic differentiation of kinematic models and automatic generation of analytic expressions for Jacobians and accelerations.

(* Define kinematics *)
l1 = 1.0; l2 = 0.7;
x[q1_, q2_] := l1 Cos[q1] + l2 Cos[q1 + q2];
y[q1_, q2_] := l1 Sin[q1] + l2 Sin[q1 + q2];

(* Jacobian J(q) *)
J[q1_, q2_] := D[{x[q1, q2], y[q1, q2]}, {{q1, q2}}];

(* Trajectory definition *)
omega = 1.5;
q1traj[t_] := Sin[omega t];
q2traj[t_] := Cos[omega t];

(* Task-space position as function of time *)
xtraj[t_] := x[q1traj[t], q2traj[t]];
ytraj[t_] := y[q1traj[t], q2traj[t]];

(* Model-based task-space velocity (symbolic) *)
xdotModel[t_] := D[{xtraj[t], ytraj[t]}, t] // Simplify;

(* Numerical differentiation of sampled trajectory *)
t0 = 0; t1 = 5.0; n = 200;
ts = Subdivide[t0, t1, n];
h = ts[[2]] - ts[[1]];
samples = Table[{ts[[k]], xtraj[ts[[k]]], ytraj[ts[[k]]]}, {k, 1, Length[ts]}];

centralDiff[list_] := Module[{N = Length[list], deriv},
  deriv = Table[{list[[k, 1]], 0.0}, {k, 1, N}];
  Do[
    deriv[[k, 2]] = (list[[k + 1, 2]] - list[[k - 1, 2]])/(2 h),
    {k, 2, N - 1}
  ];
  deriv
];

xSamples = samples[[All, {1, 2}]];
xVelNum = centralDiff[xSamples];

(* xdotModel evaluated at grid points *)
xVelModel = Table[{ts[[k]], First[xdotModel[ts[[k]]]]}, {k, 1, Length[ts]}];
      

Using the symbolic expression xdotModel[t], high-accuracy numerical evaluations can be obtained without resorting to finite differences on noisy data, which illustrates the strength of model-based differentiation.

In practice, both numerical and model-based differentiation are used, often in combination. Some guidelines:

• Use model-based differentiation for offline analysis and simulation when accurate models and smooth trajectory parameterizations are available.
• Use numerical differentiation with smoothing when only raw encoder data are available and models are incomplete, but tune the sampling period and filtering to balance truncation and noise amplification.
• When task-space quantities are needed, prefer differentiating in joint space (model-based) and mapping via Jacobians, rather than differentiating already noisy task-space reconstructions.
• At high sampling rates with moderate noise, higher-order schemes (e.g., central differences on filtered trajectories) often outperform low-rate analytic models with large modeling errors.

flowchart TD
  S["Have kinematic/dynamic model?"] -->|yes| M1["Fit smooth q(t) or use commanded q(t)"]
  S -->|no| N1["Only measured noisy q_k"]
  M1 --> M2["Compute dq/dt, ddq/dt2 analytically"]
  M2 --> M3["Map to task space via J(q)"]
  N1 --> N2["Filter q_k (low-pass / spline)"]
  N2 --> N3["Apply finite differences on filtered data"]
  N3 --> M3
  M3 --> OUT["Use velocities/accelerations in analysis"]
        

In this lesson we studied numerical and model-based differentiation of robot trajectories. We derived finite-difference formulas and their truncation errors, and analyzed how noise variance is amplified by discrete differentiation. We then contrasted this with kinematic and dynamic model-based formulas for velocities and accelerations expressed via Jacobians and equations of motion. Through a planar 2R arm example and multi-language implementations, we saw how both methods appear in practice and how their error characteristics differ. These insights are crucial when deciding how to obtain reliable trajectory derivatives for later control and estimation tasks.