In robot control, we rarely measure the full state exactly. Joint encoders may be noisy or missing, velocities are often obtained by differentiating noisy positions, and end-effector pose estimates may come from cameras or IMUs with drift and delay. The controller, however, requires an estimate of the state vector \( \mathbf{x}_k \), typically \( \mathbf{x}_k = [\mathbf{q}_k^\top \; \dot{\mathbf{q}}_k^\top]^\top \) or augmented with end-effector pose and velocities.

For a nonlinear robot model, we write the discrete-time process and measurement equations as

\[ \mathbf{x}_{k+1} = \mathbf{f}(\mathbf{x}_k, \mathbf{u}_k) + \mathbf{w}_k, \qquad \mathbf{z}_k = \mathbf{h}(\mathbf{x}_k) + \mathbf{v}_k, \]

where \( \mathbf{u}_k \) is the control input (joint torques \( \boldsymbol{\tau}_k \) or commanded joint motions), \( \mathbf{w}_k \) and \( \mathbf{v}_k \) are zero-mean Gaussian noises with covariances \( \mathbf{Q}_k \) and \( \mathbf{R}_k \). The EKF replaces the nonlinear model locally by its first-order linearization, then applies the optimal linear Kalman filter to this local approximation.

flowchart TD
  Xk["x_hat(k|k)"] --> F["Nonlinear dynamics f(x_k,u_k)"]
  U["u_k (joint torques)"] --> F
  F --> XP["x_hat(k+1|k) (prediction)"]
  XP --> H["Measurement model h(x)"]
  Z["z_k+1 (encoders, ee sensors)"] --> INNOV["innovation: z_k+1 - h(x_hat)"]
  H --> INNOV
  INNOV --> K["compute gain K_k+1"]
  XP --> UPDATE["x_hat(k+1|k+1) (correction)"]
  K --> UPDATE
        

The EKF provides a recursive estimator suitable for on-line control: at each sampling instant the filter predicts the robot state forward using the dynamics, then corrects it using joint and end-effector measurements.

For an \(n\)-DOF manipulator, you already know the joint-space dynamics

\[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} + \mathbf{J}^\top(\mathbf{q})\mathbf{f}_{\text{ext}}, \]

where \( \mathbf{M}(\mathbf{q}) \) is the inertia matrix, \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) is the Coriolis/centrifugal matrix, \( \mathbf{g}(\mathbf{q}) \) is gravity, and \( \mathbf{J}(\mathbf{q}) \) is the geometric Jacobian.

For state estimation we adopt a joint-space state vector \( \mathbf{x} = [\mathbf{q}^\top \; \dot{\mathbf{q}}^\top]^\top \in \mathbb{R}^{2n} \). With sampling time \( \Delta t \), a simple discretization (forward Euler for illustration) gives

\[ \begin{aligned} \mathbf{q}_{k+1} &= \mathbf{q}_k + \Delta t \, \dot{\mathbf{q}}_k, \\ \dot{\mathbf{q}}_{k+1} &= \dot{\mathbf{q}}_k + \Delta t \, \ddot{\mathbf{q}}_k, \\ \ddot{\mathbf{q}}_k &= \mathbf{M}^{-1}(\mathbf{q}_k) \Big( \boldsymbol{\tau}_k - \mathbf{C}(\mathbf{q}_k,\dot{\mathbf{q}}_k)\dot{\mathbf{q}}_k - \mathbf{g}(\mathbf{q}_k) \Big). \end{aligned} \]

Collecting terms, we define the discrete-time process function \( \mathbf{f} : \mathbb{R}^{2n} \times \mathbb{R}^{n} \to \mathbb{R}^{2n} \) by

\[ \mathbf{x}_{k+1} = \mathbf{f}(\mathbf{x}_k, \boldsymbol{\tau}_k) + \mathbf{w}_k, \]

where \( \mathbf{w}_k \sim \mathcal{N}(\mathbf{0},\mathbf{Q}_k) \) models unmodeled dynamics and discretization errors.

For measurements we assume:

• Joint encoders providing positions \( \mathbf{z}^{(q)}_k = \mathbf{q}_k + \mathbf{v}^{(q)}_k \).
• An external sensor (e.g., camera, motion capture) providing end-effector pose \( \mathbf{x}^{\text{ee}}_k \), related to joints by forward kinematics \( \mathbf{x}^{\text{ee}}_k = \mathbf{p}(\mathbf{q}_k) \).

Stacking measurements yields a nonlinear measurement model

\[ \mathbf{z}_k = \begin{bmatrix} \mathbf{z}_k^{(q)} \\ \mathbf{z}_k^{(\text{ee})} \end{bmatrix} = \underbrace{ \begin{bmatrix} \mathbf{I} & \mathbf{0} \\ \mathbf{p}(\cdot) & \mathbf{0} \end{bmatrix}}_{\text{nonlinear in } \mathbf{q}} \mathbf{x}_k + \mathbf{v}_k \;\equiv\; \mathbf{h}(\mathbf{x}_k) + \mathbf{v}_k, \]

where \( \mathbf{v}_k \sim \mathcal{N}(\mathbf{0},\mathbf{R}_k) \) models sensor noise.

Assume at time \(k\) we have a Gaussian approximation of the state given all measurements up to \(k\): \( \mathbf{x}_k | \mathcal{Z}_k \sim \mathcal{N}(\hat{\mathbf{x}}_{k|k}, \mathbf{P}_{k|k}) \), where \( \mathcal{Z}_k = \{\mathbf{z}_0,\dots,\mathbf{z}_k\} \).

3.1 Prediction Step

The true state evolves as \( \mathbf{x}_{k+1} = \mathbf{f}(\mathbf{x}_k,\mathbf{u}_k) + \mathbf{w}_k \). The EKF approximates the predicted mean by pushing the estimate through the nonlinear dynamics:

\[ \hat{\mathbf{x}}_{k+1|k} = \mathbf{f}(\hat{\mathbf{x}}_{k|k},\mathbf{u}_k). \]

To propagate covariance we linearize \( \mathbf{f} \) around \( \hat{\mathbf{x}}_{k|k} \). Define the Jacobian

\[ \mathbf{F}_k = \left.\frac{\partial \mathbf{f}}{\partial \mathbf{x}}\right|_{\mathbf{x}=\hat{\mathbf{x}}_{k|k},\mathbf{u}=\mathbf{u}_k} \in \mathbb{R}^{2n \times 2n}. \]

Write the estimation error \( \tilde{\mathbf{x}}_k = \mathbf{x}_k - \hat{\mathbf{x}}_{k|k} \). A first-order Taylor expansion gives

\[ \mathbf{x}_{k+1} = \mathbf{f}(\hat{\mathbf{x}}_{k|k},\mathbf{u}_k) + \mathbf{F}_k \tilde{\mathbf{x}}_k + \mathbf{w}_k + \mathcal{O}(\|\tilde{\mathbf{x}}_k\|^2), \]

so that, neglecting higher-order terms,

\[ \tilde{\mathbf{x}}_{k+1} = \mathbf{x}_{k+1} - \hat{\mathbf{x}}_{k+1|k} \approx \mathbf{F}_k \tilde{\mathbf{x}}_k + \mathbf{w}_k. \]

Taking covariance on both sides and using independence \( \mathbb{E}[\tilde{\mathbf{x}}_k \mathbf{w}_k^\top] = \mathbf{0} \) yields the predicted covariance:

\[ \mathbf{P}_{k+1|k} = \mathbf{F}_k \mathbf{P}_{k|k} \mathbf{F}_k^\top + \mathbf{Q}_k. \]

3.2 Measurement Update

At time \( k+1 \) we obtain a measurement \( \mathbf{z}_{k+1} = \mathbf{h}(\mathbf{x}_{k+1}) + \mathbf{v}_{k+1} \). Linearize \( \mathbf{h} \) at the predicted mean:

\[ \mathbf{H}_{k+1} = \left.\frac{\partial \mathbf{h}}{\partial \mathbf{x}}\right|_{\mathbf{x}=\hat{\mathbf{x}}_{k+1|k}}. \]

Again define the prediction error \( \tilde{\mathbf{x}}_{k+1|k} = \mathbf{x}_{k+1} - \hat{\mathbf{x}}_{k+1|k} \). Then

\[ \mathbf{z}_{k+1} \approx \mathbf{h}(\hat{\mathbf{x}}_{k+1|k}) + \mathbf{H}_{k+1} \tilde{\mathbf{x}}_{k+1|k} + \mathbf{v}_{k+1}. \]

The innovation (or residual) is

\[ \boldsymbol{\nu}_{k+1} = \mathbf{z}_{k+1} - \mathbf{h}(\hat{\mathbf{x}}_{k+1|k}). \]

Approximating the innovation covariance by

\[ \mathbf{S}_{k+1} = \mathbf{H}_{k+1} \mathbf{P}_{k+1|k} \mathbf{H}_{k+1}^\top + \mathbf{R}_{k+1}, \]

the EKF chooses the Kalman gain

\[ \mathbf{K}_{k+1} = \mathbf{P}_{k+1|k} \mathbf{H}_{k+1}^\top \mathbf{S}_{k+1}^{-1}, \]

and updates mean and covariance as

\[ \begin{aligned} \hat{\mathbf{x}}_{k+1|k+1} &= \hat{\mathbf{x}}_{k+1|k} + \mathbf{K}_{k+1} \boldsymbol{\nu}_{k+1}, \\ \mathbf{P}_{k+1|k+1} &= (\mathbf{I} - \mathbf{K}_{k+1}\mathbf{H}_{k+1})\mathbf{P}_{k+1|k}. \end{aligned} \]

Numerically, it is often safer to use the Joseph stabilized form

\[ \mathbf{P}_{k+1|k+1} = (\mathbf{I} - \mathbf{K}_{k+1}\mathbf{H}_{k+1})\mathbf{P}_{k+1|k} (\mathbf{I} - \mathbf{K}_{k+1}\mathbf{H}_{k+1})^\top + \mathbf{K}_{k+1} \mathbf{R}_{k+1} \mathbf{K}_{k+1}^\top, \]

which preserves symmetry and positive semi-definiteness better in finite precision.

Consider state \( \mathbf{x}_k = [\mathbf{q}_k^\top \; \dot{\mathbf{q}}_k^\top]^\top \) and measurement \( \mathbf{z}_k^{(q)} = \mathbf{q}_k + \mathbf{v}_k^{(q)} \). If only encoders are used (no end-effector sensor), the measurement model is linear:

\[ \mathbf{h}^{(q)}(\mathbf{x}_k) = \begin{bmatrix}\mathbf{I}_n & \mathbf{0}_{n\times n}\end{bmatrix} \mathbf{x}_k, \quad \mathbf{H}^{(q)} = \begin{bmatrix}\mathbf{I}_n & \mathbf{0}_{n\times n}\end{bmatrix}. \]

In that case, the EKF reduces to a linear Kalman filter for the given nonlinear dynamics linearized around the trajectory.

To compute the Jacobian \( \mathbf{F}_k \) for the dynamics, write explicitly

\[ \mathbf{f}(\mathbf{x}_k,\boldsymbol{\tau}_k) = \begin{bmatrix} \mathbf{q}_k + \Delta t \, \dot{\mathbf{q}}_k \\ \dot{\mathbf{q}}_k + \Delta t \, \mathbf{M}^{-1}(\mathbf{q}_k) \big(\boldsymbol{\tau}_k - \mathbf{C}(\mathbf{q}_k,\dot{\mathbf{q}}_k)\dot{\mathbf{q}}_k - \mathbf{g}(\mathbf{q}_k) \big) \end{bmatrix}. \]

Splitting \( \mathbf{x}_k = [\mathbf{q}_k;\dot{\mathbf{q}}_k] \) we obtain a block Jacobian

\[ \mathbf{F}_k = \begin{bmatrix} \dfrac{\partial \mathbf{q}_{k+1}}{\partial \mathbf{q}_k} & \dfrac{\partial \mathbf{q}_{k+1}}{\partial \dot{\mathbf{q}}_k} \\ \dfrac{\partial \dot{\mathbf{q}}_{k+1}}{\partial \mathbf{q}_k} & \dfrac{\partial \dot{\mathbf{q}}_{k+1}}{\partial \dot{\mathbf{q}}_k} \end{bmatrix} \approx \begin{bmatrix} \mathbf{I}_n & \Delta t \,\mathbf{I}_n \\ \star & \star \end{bmatrix}, \]

where the lower blocks are obtained by differentiating the inverse dynamics. In practice, these derivatives are often computed by automatic differentiation or by using the robot dynamics library (e.g. Pinocchio, RBDL) which can provide analytic Jacobians.

Now we augment the measurement with a nonlinear end-effector pose \( \mathbf{x}^{\text{ee}}_k = \mathbf{p}(\mathbf{q}_k) \). Assume \( \mathbf{p}(\cdot) \) is the forward kinematics mapping from joint positions to position-orientation representation (e.g. position plus Euler angles or quaternions).

The measurement function becomes

\[ \mathbf{h}(\mathbf{x}_k) = \begin{bmatrix} \mathbf{q}_k \\ \mathbf{p}(\mathbf{q}_k) \end{bmatrix} = \begin{bmatrix} \mathbf{I}_n & \mathbf{0} \\ \mathbf{p}(\mathbf{q}_k) & \mathbf{0} \end{bmatrix} \mathbf{x}_k \quad \text{(nonlinear in } \mathbf{q}_k \text{)}. \]

The measurement Jacobian is block-structured:

\[ \mathbf{H}_k = \begin{bmatrix} \dfrac{\partial \mathbf{q}_k}{\partial \mathbf{q}_k} & \dfrac{\partial \mathbf{q}_k}{\partial \dot{\mathbf{q}}_k} \\ \dfrac{\partial \mathbf{p}(\mathbf{q}_k)}{\partial \mathbf{q}_k} & \dfrac{\partial \mathbf{p}(\mathbf{q}_k)}{\partial \dot{\mathbf{q}}_k} \end{bmatrix} = \begin{bmatrix} \mathbf{I}_n & \mathbf{0} \\ \mathbf{J}_p(\mathbf{q}_k) & \mathbf{0} \end{bmatrix}, \]

where \( \mathbf{J}_p(\mathbf{q}) = \frac{\partial \mathbf{p}}{\partial \mathbf{q}} \) is the position-orientation Jacobian (a subset or transformation of the standard geometric Jacobian).

flowchart TD
  X["state x_k = [q; dq]"] --> FK["forward kinematics p(q)"]
  X --> ENC["joint encoder model: \nz_q = q + noise"]
  FK --> EES["ee sensor model: \nz_ee approx p(q) + noise"]
  ENC --> STACK["stacked measurement z_k = [z_q; z_ee]"]
  EES --> STACK
  STACK --> EKF["EKF update (H_k from I and J_p(q))"]
  EKF --> XNEXT["updated x_hat(k|k) for controller"]
        

This construction fuses joint encoders (which are precise but local) with end-effector measurements (which are global but noisier), yielding a consistent estimate of both joint states and end-effector pose aligned in a common coordinate frame for control.

Covariance tuning.

• Process noise \( \mathbf{Q}_k \) reflects model uncertainty: friction, unmodeled flexibility, discretization error. A common choice is block-diagonal in position and velocity, with larger variance in velocities.
• Measurement noise \( \mathbf{R}_k \) is determined from sensor specifications or empirical variance estimates from static measurements.

Consistency and linearization limits.

• The EKF is only locally valid: if the initial error is large or the dynamics are highly nonlinear over a sampling interval, linearization can be inaccurate and the filter may become inconsistent or even diverge.
• For highly nonlinear orientation representations (e.g. Euler angles), it is often better to use minimal-error coordinates on the Lie group (e.g. local error on SO(3)), but this belongs to more advanced estimation on manifolds.

Computational considerations.

• EKF requires computing Jacobians and updating a \( 2n \times 2n \) covariance. For typical manipulators with \( n \leq 7 \), this is tractable at kHz rates in modern hardware.
• Use Cholesky-based linear algebra and exploit sparsity of Jacobians when available (e.g. block diagonal or banded structures).

Below is a Python/Numpy sketch for an EKF estimating \( \mathbf{x} = [\mathbf{q}^\top \; \dot{\mathbf{q}}^\top]^\top \) from joint encoders and end-effector pose measurements. Robot-specific functions robot_forward_dynamics and forward_kinematics must be provided (e.g., via a robotics library).

import numpy as np

class RobotEKF:
    def __init__(self, n_dof, dt):
        self.n = n_dof
        self.dt = dt
        dim_x = 2 * n_dof

        self.x = np.zeros((dim_x, 1))  # [q; dq]
        self.P = np.eye(dim_x) * 1e-2

        # Process and measurement noise covariances
        q_pos = 1e-6
        q_vel = 1e-4
        self.Q = np.block([
            [q_pos * np.eye(n_dof), np.zeros((n_dof, n_dof))],
            [np.zeros((n_dof, n_dof)), q_vel * np.eye(n_dof)]
        ])

        # Example: joint encoder + ee position (3d) noise
        r_enc = 1e-5
        r_ee = 1e-4
        self.dim_z = n_dof + 3
        self.R = np.diag(
            np.concatenate([
                r_enc * np.ones(n_dof),
                r_ee * np.ones(3)
            ])
        )

    def f(self, x, u):
        """Nonlinear process model x_{k+1} = f(x_k, u_k)."""
        n = self.n
        q = x[0:n].reshape(-1, 1)
        dq = x[n:2*n].reshape(-1, 1)

        # Robot forward dynamics: qddot = M^{-1}(q) * (tau - C(q,dq)dq - g(q))
        # This must be implemented using your robot model.
        qddot = robot_forward_dynamics(q, dq, u)

        q_next = q + self.dt * dq
        dq_next = dq + self.dt * qddot
        return np.vstack((q_next, dq_next))

    def F_jacobian(self, x, u, eps=1e-6):
        """Numerical Jacobian of f wrt x."""
        dim_x = x.shape[0]
        F = np.zeros((dim_x, dim_x))
        fx = self.f(x, u)
        for i in range(dim_x):
            dx = np.zeros_like(x)
            dx[i, 0] = eps
            f_plus = self.f(x + dx, u)
            F[:, i:i+1] = (f_plus - fx) / eps
        return F

    def h(self, x):
        """Measurement model: stacked joint positions and ee position."""
        n = self.n
        q = x[0:n].reshape(-1, 1)
        # Encoders measure q directly
        z_q = q

        # Forward kinematics: ee_pos in R^3
        ee_pos = forward_kinematics(q)  # shape (3,1)
        return np.vstack((z_q, ee_pos))

    def H_jacobian(self, x, eps=1e-6):
        """Numerical Jacobian of h wrt x."""
        dim_x = x.shape[0]
        z0 = self.h(x)
        dim_z = z0.shape[0]
        H = np.zeros((dim_z, dim_x))
        for i in range(dim_x):
            dx = np.zeros_like(x)
            dx[i, 0] = eps
            H[:, i:i+1] = (self.h(x + dx) - z0) / eps
        return H

    def predict(self, u):
        """EKF prediction step."""
        x = self.x
        P = self.P

        # Mean prediction
        self.x = self.f(x, u)

        # Jacobian wrt x
        F = self.F_jacobian(x, u)
        self.P = F @ P @ F.T + self.Q

    def update(self, z):
        """EKF update step with measurement z."""
        x_pred = self.x
        P_pred = self.P

        z_pred = self.h(x_pred)
        H = self.H_jacobian(x_pred)

        y = z.reshape(-1, 1) - z_pred                      # innovation
        S = H @ P_pred @ H.T + self.R                     # innovation covariance
        K = P_pred @ H.T @ np.linalg.inv(S)               # Kalman gain

        self.x = x_pred + K @ y
        I = np.eye(P_pred.shape[0])
        self.P = (I - K @ H) @ P_pred @ (I - K @ H).T + K @ self.R @ K.T

# Example placeholders for robot-specific functions:
def robot_forward_dynamics(q, dq, tau):
    # Implement using your dynamics library (Pinocchio, RBDL, etc.)
    # Here we simply use a dummy model with unit inertia and no coupling.
    return tau  # qddot = tau (very simplified)

def forward_kinematics(q):
    # Implement your forward kinematics to 3D ee position
    # Here, return a dummy linear function.
    return np.array([[q.sum()], [0.0], [0.0]])
      

The following C++ code uses Eigen to implement one EKF prediction–update cycle for a manipulator. Robot-specific dynamics and kinematics must be supplied by your chosen library.

#include <Eigen/Dense>

using Eigen::VectorXd;
using Eigen::MatrixXd;

struct RobotEKF {
    int n;          // number of joints
    double dt;      // sampling time
    VectorXd x;     // state [q; dq]
    MatrixXd P;     // covariance
    MatrixXd Q;     // process noise
    MatrixXd R;     // measurement noise

    RobotEKF(int n_dof, double dt_)
        : n(n_dof), dt(dt_)
    {
        int dim_x = 2 * n;
        x = VectorXd::Zero(dim_x);
        P = MatrixXd::Identity(dim_x, dim_x) * 1e-2;

        Q = MatrixXd::Zero(dim_x, dim_x);
        Q.block(0, 0, n, n) = 1e-6 * MatrixXd::Identity(n, n);
        Q.block(n, n, n, n) = 1e-4 * MatrixXd::Identity(n, n);

        int dim_z = n + 3;  // q encoders + 3D ee position
        R = MatrixXd::Zero(dim_z, dim_z);
        R.block(0, 0, n, n) = 1e-5 * MatrixXd::Identity(n, n);
        R.block(n, n, 3, 3) = 1e-4 * MatrixXd::Identity(3, 3);
    }

    VectorXd f(const VectorXd& xk, const VectorXd& u) const {
        int dim_x = xk.size();
        int n_local = n;
        VectorXd q = xk.segment(0, n_local);
        VectorXd dq = xk.segment(n_local, n_local);

        VectorXd qddot = robot_forward_dynamics(q, dq, u); // user-defined

        VectorXd x_next(dim_x);
        x_next.segment(0, n_local) = q + dt * dq;
        x_next.segment(n_local, n_local) = dq + dt * qddot;
        return x_next;
    }

    MatrixXd F_jacobian(const VectorXd& xk, const VectorXd& u) const {
        int dim_x = xk.size();
        MatrixXd F(dim_x, dim_x);
        VectorXd fx = f(xk, u);
        double eps = 1e-6;

        for (int i = 0; i < dim_x; ++i) {
            VectorXd dx = VectorXd::Zero(dim_x);
            dx(i) = eps;
            VectorXd f_plus = f(xk + dx, u);
            F.col(i) = (f_plus - fx) / eps;
        }
        return F;
    }

    VectorXd h(const VectorXd& xk) const {
        int dim_x = xk.size();
        int n_local = n;
        VectorXd q = xk.segment(0, n_local);
        VectorXd z(dim_z());
        // encoders
        z.segment(0, n_local) = q;
        // ee position
        VectorXd ee = forward_kinematics(q);  // size 3
        z.segment(n_local, 3) = ee;
        return z;
    }

    MatrixXd H_jacobian(const VectorXd& xk) const {
        int dim_x = xk.size();
        int dimz = dim_z();
        MatrixXd H(dimz, dim_x);
        double eps = 1e-6;
        VectorXd h0 = h(xk);

        for (int i = 0; i < dim_x; ++i) {
            VectorXd dx = VectorXd::Zero(dim_x);
            dx(i) = eps;
            VectorXd h_plus = h(xk + dx);
            H.col(i) = (h_plus - h0) / eps;
        }
        return H;
    }

    int dim_z() const { return n + 3; }

    void predict(const VectorXd& u) {
        VectorXd x_prev = x;
        MatrixXd P_prev = P;

        x = f(x_prev, u);
        MatrixXd F = F_jacobian(x_prev, u);
        P = F * P_prev * F.transpose() + Q;
    }

    void update(const VectorXd& z_meas) {
        VectorXd x_pred = x;
        MatrixXd P_pred = P;

        VectorXd z_pred = h(x_pred);
        MatrixXd H = H_jacobian(x_pred);
        VectorXd y = z_meas - z_pred;

        MatrixXd S = H * P_pred * H.transpose() + R;
        MatrixXd K = P_pred * H.transpose() * S.inverse();

        x = x_pred + K * y;
        MatrixXd I = MatrixXd::Identity(P_pred.rows(), P_pred.cols());
        P = (I - K * H) * P_pred * (I - K * H).transpose() + K * R * K.transpose();
    }
};

// Placeholders for robot functions.
VectorXd robot_forward_dynamics(const VectorXd& q,
                                const VectorXd& dq,
                                const VectorXd& tau)
{
    // TODO: implement using your robot dynamics library
    return tau; // qddot = tau (dummy)
}

VectorXd forward_kinematics(const VectorXd& q)
{
    // TODO: implement your forward kinematics
    VectorXd ee(3);
    ee.setZero();
    ee(0) = q.sum(); // dummy: x = sum of joint angles
    return ee;
}
      

Java does not have a standard matrix library; you can use EJML or similar. Here, for clarity, we show an interface assuming a small helper class Mat for matrix operations (to be implemented or mapped to EJML).

public class RobotEKFJava {
    private final int n;
    private final double dt;
    private Mat x;  // state [q; dq], column vector
    private Mat P;  // covariance
    private Mat Q;
    private Mat R;
    private final int dimZ;

    public RobotEKFJava(int nDof, double dt) {
        this.n = nDof;
        this.dt = dt;
        int dimX = 2 * n;

        this.x = Mat.zeros(dimX, 1);
        this.P = Mat.eye(dimX).scale(1e-2);

        this.Q = Mat.zeros(dimX, dimX);
        this.Q.setBlock(0, 0, n, n, Mat.eye(n).scale(1e-6));
        this.Q.setBlock(n, n, n, n, Mat.eye(n).scale(1e-4));

        this.dimZ = n + 3;
        this.R = Mat.zeros(dimZ, dimZ);
        this.R.setBlock(0, 0, n, n, Mat.eye(n).scale(1e-5));
        this.R.setBlock(n, n, 3, 3, Mat.eye(3).scale(1e-4));
    }

    // Process model
    private Mat f(Mat xk, Mat u) {
        int dimX = xk.rows();
        Mat q = xk.subMat(0, n, 0, 1);
        Mat dq = xk.subMat(n, 2 * n, 0, 1);

        Mat qddot = robotForwardDynamics(q, dq, u); // user-defined

        Mat qNext = q.add(dq.scale(dt));
        Mat dqNext = dq.add(qddot.scale(dt));

        Mat xNext = Mat.zeros(dimX, 1);
        xNext.setBlock(0, 0, n, 1, qNext);
        xNext.setBlock(n, 0, n, 1, dqNext);
        return xNext;
    }

    private Mat FJacobian(Mat xk, Mat u) {
        int dimX = xk.rows();
        Mat F = Mat.zeros(dimX, dimX);
        Mat fx = f(xk, u);
        double eps = 1e-6;

        for (int i = 0; i < dimX; ++i) {
            Mat dx = Mat.zeros(dimX, 1);
            dx.set(i, 0, eps);
            Mat fPlus = f(xk.add(dx), u);
            Mat col = fPlus.sub(fx).scale(1.0 / eps);
            F.setCol(i, col);
        }
        return F;
    }

    // Measurement model
    private Mat h(Mat xk) {
        Mat z = Mat.zeros(dimZ, 1);
        Mat q = xk.subMat(0, n, 0, 1);
        z.setBlock(0, 0, n, 1, q);

        Mat ee = forwardKinematics(q); // size 3x1
        z.setBlock(n, 0, 3, 1, ee);
        return z;
    }

    private Mat HJacobian(Mat xk) {
        int dimX = xk.rows();
        Mat H = Mat.zeros(dimZ, dimX);
        Mat h0 = h(xk);
        double eps = 1e-6;

        for (int i = 0; i < dimX; ++i) {
            Mat dx = Mat.zeros(dimX, 1);
            dx.set(i, 0, eps);
            Mat hPlus = h(xk.add(dx));
            Mat col = hPlus.sub(h0).scale(1.0 / eps);
            H.setCol(i, col);
        }
        return H;
    }

    public void predict(Mat u) {
        Mat xPrev = x.copy();
        Mat PPrev = P.copy();

        x = f(xPrev, u);
        Mat F = FJacobian(xPrev, u);
        P = F.mul(PPrev).mul(F.transpose()).add(Q);
    }

    public void update(Mat zMeas) {
        Mat xPred = x.copy();
        Mat PPred = P.copy();

        Mat zPred = h(xPred);
        Mat H = HJacobian(xPred);
        Mat y = zMeas.sub(zPred);

        Mat S = H.mul(PPred).mul(H.transpose()).add(R);
        Mat K = PPred.mul(H.transpose()).mul(S.inverse());

        x = xPred.add(K.mul(y));
        Mat I = Mat.eye(PPred.rows());
        P = (I.sub(K.mul(H)))
                .mul(PPred)
                .mul((I.sub(K.mul(H))).transpose())
                .add(K.mul(R).mul(K.transpose()));
    }

    // Robot-specific stubs:
    private Mat robotForwardDynamics(Mat q, Mat dq, Mat tau) {
        // Implement using your dynamics
        return tau; // qddot = tau (dummy)
    }

    private Mat forwardKinematics(Mat q) {
        Mat ee = Mat.zeros(3, 1);
        double sum = 0.0;
        for (int i = 0; i < q.rows(); ++i)
            sum += q.get(i, 0);
        ee.set(0, 0, sum);
        return ee;
    }

    public Mat getState() { return x; }
}
      

Here Mat is a placeholder for a matrix wrapper providing basic operations (add, sub, mul, inverse, etc.). In practice, you would map this code to a concrete library (e.g., EJML or Apache Commons Math).

MATLAB is well suited for prototyping EKF-based state estimation. A typical implementation uses a function to perform one predict–update step:

function [x_next, P_next] = robot_ekf_step(x, P, u, z, dt, Q, R)
% x, P : current state and covariance
% u    : control input (joint torques)
% z    : measurement [q_enc; ee_pos]
% dt   : sampling time
% Q, R : process and measurement noise covariances

n = numel(x) / 2;
q  = x(1:n);
dq = x(n+1:end);

% ----- Process model -----
qddot = robot_forward_dynamics(q, dq, u);  % user-supplied

q_pred  = q  + dt * dq;
dq_pred = dq + dt * qddot;
x_pred  = [q_pred; dq_pred];

F = numerical_jacobian(@(xx) f_process(xx, u, dt), x);

P_pred = F * P * F.' + Q;

% ----- Measurement model -----
z_pred = h_measurement(x_pred);

H = numerical_jacobian(@h_measurement, x_pred);

y = z - z_pred;
S = H * P_pred * H.' + R;
K = P_pred * H.' / S;

x_next = x_pred + K * y;
I = eye(size(P_pred));
P_next = (I - K * H) * P_pred * (I - K * H).' + K * R * K.';

end

function x_next = f_process(x, u, dt)
    % helper for numerical Jacobian
    n = numel(x) / 2;
    q  = x(1:n);
    dq = x(n+1:end);
    qddot = robot_forward_dynamics(q, dq, u);
    q_next  = q  + dt * dq;
    dq_next = dq + dt * qddot;
    x_next = [q_next; dq_next];
end

function z = h_measurement(x)
    global n_dof
    q  = x(1:n_dof);
    z_q  = q;
    ee   = forward_kinematics(q);  % 3x1
    z = [z_q; ee];
end

function J = numerical_jacobian(fun, x)
    fx = fun(x);
    m  = numel(fx);
    n  = numel(x);
    J  = zeros(m, n);
    eps = 1e-6;
    for i = 1:n
        dx = zeros(n,1);
        dx(i) = eps;
        f_plus = fun(x + dx);
        J(:, i) = (f_plus - fx) / eps;
    end
end
      

In Simulink, you typically:

• Use a Unit Delay or Memory block to store \( \hat{\mathbf{x}}_{k|k} \) and \( \mathbf{P}_{k|k} \).
• Implement the process and measurement models inside MATLAB Function blocks.
• Wire the EKF prediction to run with control \( \boldsymbol{\tau}_k \) and update with measurements \( \mathbf{z}_k \) at each sample.

Mathematica can implement an EKF by defining symbolic or numeric process and measurement functions and iterating the recursion. Below is a numeric example:

(* State dimension for n DOF: dimX = 2 n *)
nDof = 2;
dt   = 0.001;
dimX = 2 nDof;
dimZ = nDof + 3;

(* Process model f[x_, u_] *)
fProcess[x_, u_] := Module[
  {q, dq, qddot, qNext, dqNext},
  q  = x[[1 ;; nDof]];
  dq = x[[nDof + 1 ;; 2 nDof]];

  qddot = robotForwardDynamics[q, dq, u]; (* user-defined *)

  qNext  = q  + dt dq;
  dqNext = dq + dt qddot;
  Join[qNext, dqNext]
];

(* Measurement model h[x_] *)
hMeasurement[x_] := Module[
  {q, ee},
  q  = x[[1 ;; nDof]];
  ee = forwardKinematics[q];  (* 3-vector *)
  Join[q, ee]
];

(* Numerical Jacobian *)
jacobian[fun_, x_List] := Module[
  {fx, n, m, eps, cols},
  fx  = fun[x];
  m   = Length[fx];
  n   = Length[x];
  eps = 10.^(-6);
  cols = Table[
    Module[{dx = ConstantArray[0., n], fplus},
      dx[[i]] = eps;
      fplus = fun[x + dx];
      (fplus - fx)/eps
    ],
    {i, 1, n}
  ];
  Transpose[cols]
];

(* One EKF step *)
ekfStep[{x_, P_}, {u_, z_, Q_, R_}] := Module[
  {xPred, F, PPred, zPred, H, y, S, K, I, PPost, xPost},
  xPred = fProcess[x, u];
  F     = jacobian[fProcess[#, u] &, x];
  PPred = F . P . Transpose[F] + Q;

  zPred = hMeasurement[xPred];
  H     = jacobian[hMeasurement, xPred];
  y     = z - zPred;
  S     = H . PPred . Transpose[H] + R;
  K     = PPred . Transpose[H] . Inverse[S];

  xPost = xPred + K . y;
  I     = IdentityMatrix[Length[x]];
  PPost = (I - K . H) . PPred . Transpose[I - K . H] + K . R . Transpose[K];

  {xPost, PPost}
];

(* Example usage: fold over time steps *)
x0 = ConstantArray[0., dimX];
P0 = IdentityMatrix[dimX] 0.01;

(* robotForwardDynamics and forwardKinematics must be implemented *)

(* {uSeq, zSeq} are sequences of controls and measurements *)
stateCovSeq = FoldList[
  ekfStep,
  {x0, P0},
  MapThread[
    { #1, #2, Q, R } &,
    {uSeq, zSeq}
  ]
];
      

Here robotForwardDynamics and forwardKinematics are user-defined functions with access to the manipulator model; Q and R are covariance matrices chosen according to sensor and model uncertainty.

Problem 1 (Deriving the Prediction Covariance): Consider the nonlinear process \( \mathbf{x}_{k+1} = \mathbf{f}(\mathbf{x}_k) + \mathbf{w}_k \) with \( \mathbf{w}_k \sim \mathcal{N}(\mathbf{0},\mathbf{Q}) \). Let \( \hat{\mathbf{x}}_{k|k} \) be an estimate with error covariance \( \mathbf{P}_{k|k} \). Show that, under first-order linearization around \( \hat{\mathbf{x}}_{k|k} \), the predicted covariance is \( \mathbf{P}_{k+1|k} = \mathbf{F}_k \mathbf{P}_{k|k} \mathbf{F}_k^\top + \mathbf{Q} \), where \( \mathbf{F}_k = \frac{\partial \mathbf{f}}{\partial \mathbf{x}}|_{\hat{\mathbf{x}}_{k|k}} \).

Solution:

Define \( \tilde{\mathbf{x}}_k = \mathbf{x}_k - \hat{\mathbf{x}}_{k|k} \). Then

\[ \mathbf{x}_{k+1} = \mathbf{f}(\mathbf{x}_k) + \mathbf{w}_k \approx \mathbf{f}(\hat{\mathbf{x}}_{k|k}) + \mathbf{F}_k \tilde{\mathbf{x}}_k + \mathbf{w}_k. \]

The predicted mean is \( \hat{\mathbf{x}}_{k+1|k} = \mathbf{f}(\hat{\mathbf{x}}_{k|k}) \), so the new error is

\[ \tilde{\mathbf{x}}_{k+1} = \mathbf{x}_{k+1} - \hat{\mathbf{x}}_{k+1|k} \approx \mathbf{F}_k \tilde{\mathbf{x}}_k + \mathbf{w}_k. \]

Taking covariance and using \( \mathbb{E}[\tilde{\mathbf{x}}_k \mathbf{w}_k^\top] = \mathbf{0} \) yields

\[ \mathbf{P}_{k+1|k} = \mathbb{E}[\tilde{\mathbf{x}}_{k+1}\tilde{\mathbf{x}}_{k+1}^\top] \approx \mathbf{F}_k \mathbf{P}_{k|k}\mathbf{F}_k^\top + \mathbf{Q}. \]

Problem 2 (Measurement Jacobian for End-Effector Pose): For a planar 2-DOF arm with link lengths \( l_1, l_2 \) and joint angles \( \mathbf{q} = [q_1, q_2]^\top \), the end-effector position is

\[ \mathbf{p}(\mathbf{q}) = \begin{bmatrix} l_1 \cos q_1 + l_2 \cos(q_1+q_2) \\ l_1 \sin q_1 + l_2 \sin(q_1+q_2) \end{bmatrix}. \]

Derive the measurement Jacobian \( \mathbf{H}_k \) for the EKF when the state is \( \mathbf{x}_k = [q_1,q_2,\dot{q}_1,\dot{q}_2]^\top \) and the measurement consists of \( \mathbf{z}_k = \mathbf{p}(\mathbf{q}_k) \).

Solution:

The measurement function is \( \mathbf{h}(\mathbf{x}_k) = \mathbf{p}(\mathbf{q}_k) \), independent of velocities. Derivatives w.r.t. \( \dot{q}_1,\dot{q}_2 \) are zero. Therefore

\[ \mathbf{H}_k = \begin{bmatrix} \dfrac{\partial \mathbf{p}}{\partial q_1} & \dfrac{\partial \mathbf{p}}{\partial q_2} & \dfrac{\partial \mathbf{p}}{\partial \dot{q}_1} & \dfrac{\partial \mathbf{p}}{\partial \dot{q}_2} \end{bmatrix} = \begin{bmatrix} -l_1 \sin q_1 - l_2 \sin(q_1+q_2) & -l_2 \sin(q_1+q_2) & 0 & 0 \\ l_1 \cos q_1 + l_2 \cos(q_1+q_2) & l_2 \cos(q_1+q_2) & 0 & 0 \end{bmatrix}. \]

Problem 3 (Fusion of Joint Encoders and End-Effector Pose): Explain qualitatively how fusing joint encoder measurements and end-effector pose measurements in the EKF can help detect and compensate for slowly varying joint encoder biases.

Solution:

If only encoders are used, a slowly drifting bias in \( q_i \) cannot be distinguished from a true joint offset; the filter will treat it as part of the state. Adding end-effector pose measurements from an independent sensor introduces global geometric constraints: many combinations of joint angles yield end-effector positions inconsistent with the measured pose. If the state is augmented with bias variables, e.g. \( \mathbf{x} = [\mathbf{q}^\top, \dot{\mathbf{q}}^\top, \mathbf{b}^\top]^\top \), the EKF can attribute persistent discrepancies between forward kinematics and measured end-effector pose to the biases \( \mathbf{b} \), gradually correcting them. This is a special case of joint state and parameter estimation.

Problem 4 (Filter Stability vs. Tuning): You find that your joint/end-effector EKF becomes numerically unstable when you decrease \( \mathbf{R} \) while leaving \( \mathbf{Q} \) unchanged. Give a conceptual explanation.

Solution:

Decreasing \( \mathbf{R} \) means you trust the measurements more than before. The Kalman gain \( \mathbf{K}_k \) becomes larger, so the filter reacts more aggressively to innovations. If \( \mathbf{Q} \) is too small, the predicted covariance can become unrealistically small, and the filter may overreact to measurement noise, especially when the linearization is poor. The combination leads to large swings in the state estimate and covariance that can cause numerical instability or loss of positive definiteness. A consistent tuning maintains a reasonable ratio between \( \mathbf{Q} \) and \( \mathbf{R} \).

Problem 5 (Algorithmic Flow): Sketch the algorithmic flow for an EKF-based joint/end-effector estimator running in a control loop.

Solution (flow diagram):

flowchart TD
  START["start loop at time k"] --> READU["read control input u_k"]
  READU --> PRED["EKF predict with f(x_k,u_k)"]
  PRED --> READZ["read sensors: encoders, ee pose"]
  READZ --> UPDATE["EKF update: compute innovation, K_k, x_hat(k|k)"]
  UPDATE --> CTRL["use x_hat(k|k) in controller"]
  CTRL --> INC["increment k"]
  INC --> START
        

In this lesson we formulated the joint and end-effector state estimation problem for robot manipulators within a nonlinear state-space framework and derived the Extended Kalman Filter as a local linearization of the dynamics and kinematics. We specialized the process and measurement Jacobians to standard robot models, showed how to fuse joint encoders with end-effector sensors, and discussed covariance tuning and numerical stability. Finally, we provided implementation sketches in Python, C++, Java, MATLAB/Simulink, and Mathematica, preparing you for integrating EKF-based estimators tightly with robot controllers in subsequent lessons.