We start from a continuous-time LTI model of a robot joint or a linearized manipulator around a nominal trajectory:

\[ \dot{\mathbf{x}}(t) = \mathbf{A}\mathbf{x}(t) + \mathbf{B}\mathbf{u}(t), \quad \mathbf{y}(t) = \mathbf{C}\mathbf{x}(t) + \mathbf{v}(t), \]

where \( \mathbf{x}(t)\in\mathbb{R}^n \) is the state (e.g. joint positions and velocities), \( \mathbf{u}(t) \) is the control torque/force, and \( \mathbf{v}(t) \) is measurement noise. In the ideal observer-based feedback (without delay or drift), we use an estimate \( \hat{\mathbf{x}}(t) \):

\[ \mathbf{u}(t) = -\mathbf{K}\hat{\mathbf{x}}(t), \qquad \dot{\hat{\mathbf{x}}}(t) = \mathbf{A}\hat{\mathbf{x}}(t) + \mathbf{B}\mathbf{u}(t) + \mathbf{L}\big(\mathbf{y}(t) - \mathbf{C}\hat{\mathbf{x}}(t)\big). \]

Under the assumptions of previous lessons, if \( (\mathbf{A},\mathbf{B}) \) is stabilizable and \( (\mathbf{A},\mathbf{C}) \) is detectable, the separation principle guarantees that the poles of the closed loop are given by the union of \( \mathbf{A}-\mathbf{B}\mathbf{K} \) and \( \mathbf{A}-\mathbf{L}\mathbf{C} \). However, delay and drift break some of these clean properties.

flowchart TD
  R["Reference r(t)"] --> C["State Feedback Gain K"]
  C --> U["Control u(t)"]
  U --> P["Robot Dynamics xdot = A x + B u"]
  P --> Ytrue["True state x(t)"]
  Ytrue --> S["Sensors (delay, noise, drift)"]
  S --> E["Estimator (observer / EKF)"]
  E --> Xhat["State estimate x_hat(t)"]
  Xhat --> C
        

In this lesson we explicitly model:

• Measurement delay, e.g. camera processing latency or communication delay in joint encoders.
• Drift/bias, e.g. gyroscope bias or force-torque sensor offset.
• Sensor fusion, combining heterogeneous sensors (encoders, IMU, vision) with different latencies and noise levels.

Consider a scalar linearized joint model (e.g. small motions around an equilibrium):

\[ \dot{x}(t) = a x(t) + b u(t), \quad y(t) = x(t-h) + v(t), \]

where \( h > 0 \) is a constant measurement delay. Suppose the controller uses the delayed measurement directly:

\[ u(t) = -k y(t) = -k x(t-h). \]

Substituting this into the plant yields the delay-differential equation

\[ \dot{x}(t) = a x(t) - b k x(t-h). \]

Seeking exponential solutions \( x(t) = e^{s t} \) leads to the characteristic equation

\[ s - a + b k e^{-s h} = 0. \]

Even for this scalar case, the spectrum is infinite (countable) because of the exponential factor \( e^{-s h} \). Stability requires all roots to satisfy \( \operatorname{Re}(s) < 0 \).

To capture the delay margin (maximal tolerable delay) we often look for purely imaginary roots \( s = j\omega \). Separating real and imaginary parts:

\[ j\omega - a + b k (\cos(\omega h) - j \sin(\omega h)) = 0. \]

Therefore:

\[ \begin{cases} -a + b k \cos(\omega h) = 0,\\[4pt] \omega + b k \sin(\omega h) = 0. \end{cases} \]

Eliminating \( \cos(\omega h) \) using the first equation yields:

\[ \cos(\omega h) = \dfrac{a}{b k}, \quad \omega = - b k \sin(\omega h). \]

A real solution exists only if \( \left|\dfrac{a}{b k}\right|\le 1 \). The smallest positive solution for \( \omega \) gives the critical delay \( h_{\max} \):

\[ h_{\max} = \dfrac{\arccos\!\big(\frac{a}{b k}\big)}{\sqrt{(b k)^2 - a^2}}, \quad \text{for } |b k| > |a|. \]

For \( 0 < h < h_{\max} \), the delayed feedback remains asymptotically stable. This illustrates the key trade-off: increasing feedback gain \( k \) speeds up response but decreases the tolerable delay \( h_{\max} \).

In robot control, delays arise in:

• Camera-based pose estimation pipelines.
• Networked joint encoders over field buses or wireless links.
• Filtering and averaging of IMU data.

In practice, observers (e.g. EKF) attempt to predict the current state from delayed measurements, effectively replacing \( x(t-h) \) with an estimate of \( x(t) \) by propagating the dynamics between \( t-h \) and \( t \). Residual unmodeled delay still modifies the closed-loop poles similarly to the scalar analysis above.

Many robot sensors exhibit bias and drift. Let \( b(t) \) denote a slowly varying bias, e.g. gyroscope bias or force-torque offset. A typical model is:

\[ \dot{\mathbf{x}}(t) = \mathbf{A}\mathbf{x}(t) + \mathbf{B}\mathbf{u}(t),\quad \dot{\mathbf{b}}(t) = \mathbf{0},\quad \mathbf{y}(t) = \mathbf{C}\mathbf{x}(t) + \mathbf{D}\mathbf{b}(t) + \mathbf{v}(t), \]

with \( \mathbf{b}(t)\in\mathbb{R}^{p} \) representing constant or very slow bias. The standard move is to form the augmented state \( \mathbf{x}_a(t) = \begin{bmatrix}\mathbf{x}(t)\\\mathbf{b}(t)\end{bmatrix} \):

\[ \dot{\mathbf{x}}_a(t) = \underbrace{\begin{bmatrix} \mathbf{A} & \mathbf{0}\\ \mathbf{0} & \mathbf{0} \end{bmatrix}}_{\mathbf{A}_a} \mathbf{x}_a(t) + \underbrace{\begin{bmatrix} \mathbf{B}\\ \mathbf{0} \end{bmatrix}}_{\mathbf{B}_a}\mathbf{u}(t),\quad \mathbf{y}(t) = \underbrace{\begin{bmatrix} \mathbf{C} & \mathbf{D} \end{bmatrix}}_{\mathbf{C}_a} \mathbf{x}_a(t) + \mathbf{v}(t). \]

The observability of \( (\mathbf{A}_a,\mathbf{C}_a) \) determines whether bias can be estimated from measurements. A typical case:

• Joint encoders with unknown constant offset in position.
• IMU angular velocity with additive gyroscope bias.

For example, consider a 1-DOF joint with state \( x = [q\; \dot{q}]' \) and encoder \( y = q + b \), where \( b \) is constant. Then

\[ \mathbf{A}_a = \begin{bmatrix} 0 & 1 & 0\\ 0 & 0 & 0\\ 0 & 0 & 0 \end{bmatrix},\quad \mathbf{C}_a = \begin{bmatrix}1 & 0 & 1\end{bmatrix}. \]

The pair \( (\mathbf{A}_a,\mathbf{C}_a) \) is observable, so an observer (or EKF) can jointly estimate \( q, \dot{q}, b \).

If bias is not modeled, the control law \( u = -\mathbf{K}\hat{\mathbf{x}} \) is based on biased states, leading to steady-state tracking errors that no linear feedback (without integral action or bias estimation) can remove.

flowchart TD
  X["State x(t)"] --> M1["Sensor 1: y1 = C1 x + b1 + v1"]
  X --> M2["Sensor 2: y2 = C2 x + b2 + v2"]
  BIAS["Bias states b1, b2 (slow)"] --> M1
  BIAS --> M2
  M1 --> F["Bias-aware estimator (augmented state)"]
  M2 --> F
  F --> XHAT["Estimated x_hat, b_hat"]
  XHAT --> CTRL["Controller uses x_hat, optionally b_hat"]
        

Suppose we have two sensors measuring the same quantity \( z \), e.g. end-effector position:

\[ y_1 = z + v_1,\quad v_1 \sim \mathcal{N}(0,\sigma_1^2), \qquad y_2 = z + v_2,\quad v_2 \sim \mathcal{N}(0,\sigma_2^2), \]

with mutually independent noise. The optimal linear unbiased estimator (BLUE) of \( z \) from \( y_1,y_2 \) is

\[ \hat{z} = w_1 y_1 + w_2 y_2,\quad w_1 = \dfrac{\sigma_2^{-2}}{\sigma_1^{-2} + \sigma_2^{-2}},\quad w_2 = \dfrac{\sigma_1^{-2}}{\sigma_1^{-2} + \sigma_2^{-2}}. \]

Equivalently,

\[ \hat{z} = \dfrac{\sigma_2^2 y_1 + \sigma_1^2 y_2}{\sigma_1^2 + \sigma_2^2},\quad \operatorname{Var}(\hat{z} - z) = \dfrac{\sigma_1^2 \sigma_2^2}{\sigma_1^2 + \sigma_2^2}. \]

In a Kalman filter, this fusion appears as the measurement update (assuming linear measurement model and Gaussian noise). For state estimates, one fuses information from multiple sensors to reduce the posterior covariance \( \mathbf{P}_{k|k} \). The controller then acts on \( \hat{\mathbf{x}}_{k|k} \).

The closed-loop state with estimator-based control in discrete time is schematically

\[ \mathbf{x}_{k+1} = (\mathbf{A} - \mathbf{B}\mathbf{K})\mathbf{x}_k + \mathbf{B}\mathbf{K}(\mathbf{x}_k - \hat{\mathbf{x}}_{k|k}), \]

where the estimation error \( \mathbf{e}_k = \mathbf{x}_k - \hat{\mathbf{x}}_{k|k} \) is itself a random process governed by sensor noise, drift, and delay. Sensor fusion that reduces \( \mathbf{P}_{k|k} = \mathbb{E}[\mathbf{e}_k \mathbf{e}_k'] \) directly reduces the variance of the stochastic perturbation term \( \mathbf{B}\mathbf{K}\mathbf{e}_k \), leading to tighter tracking and lower control effort variance.

However, fusion introduces its own challenges:

• Asynchronous sensors with different timestamps require temporal alignment, often with interpolation or prediction to a common time.
• Sensors may have correlated noise; naive fusing assuming independence underestimates uncertainty.
• Faulty sensors can dominate the estimate if not robustly down-weighted.

In practice, EKF-based sensor fusion for robots (e.g. joint encoders + IMU + vision) is tuned so that the resulting estimator dynamics are significantly faster than the closed-loop controller dynamics, approximating the separation principle despite delays and nonlinearities.

We simulate a 1-DOF joint with state \( x = [q,\dot{q},b]' \), where \( b \) is an encoder bias. The measurement is delayed by \( h \) seconds. We use a simple EKF that:

1. Propagates the state between samples using dynamic model.
2. Applies a measurement update when a delayed encoder sample arrives.

import numpy as np

# Robot joint parameters (small-angle linearized)
J = 0.05   # inertia
d = 0.01   # viscous friction
Kp = 50.0
Kd = 2.0 * np.sqrt(Kp * J)  # critical-ish damping

# Discretization
dt = 0.001   # controller/estimator step (1 kHz)
h  = 0.02    # encoder delay = 20 ms
delay_steps = int(h / dt)

# State: x = [q, qd, b]^T
A = np.array([[0.0, 1.0, 0.0],
              [0.0, -d / J, 0.0],
              [0.0, 0.0, 0.0]])
B = np.array([[0.0],
              [1.0 / J],
              [0.0]])
C = np.array([[1.0, 0.0, 1.0]])  # y = q + b + noise

# EKF covariances (tuned)
Q = np.diag([1e-6, 1e-4, 1e-8])   # process noise
R = np.array([[1e-4]])            # meas noise

# Reference trajectory
def q_ref(t):
    return 0.5 * np.sin(2.0 * np.pi * 0.5 * t)  # 0.5 rad at 0.5 Hz

# Circular buffer for delayed measurements
max_steps = 50000
meas_buffer = [None] * (delay_steps + 1)

# Initialization
x_true = np.array([0.0, 0.0, 0.05])  # true bias 0.05 rad
x_hat  = np.array([0.0, 0.0, 0.0])   # initial bias estimate 0
P = np.eye(3) * 1e-2

def step_true(x, u):
    # exact discrete-time update via Euler for simplicity
    return x + dt * (A @ x + B.flatten() * u)

def ekf_predict(xh, Ph, u):
    Ad = np.eye(3) + dt * A
    Bd = dt * B
    xh = Ad @ xh + Bd.flatten() * u
    Ph = Ad @ Ph @ Ad.T + Q
    return xh, Ph

def ekf_update(xh, Ph, y):
    S = C @ Ph @ C.T + R
    K = Ph @ C.T @ np.linalg.inv(S)
    innov = y - C @ xh
    xh = xh + (K @ innov).flatten()
    Ph = (np.eye(3) - K @ C) @ Ph
    return xh, Ph

T_final = 10.0
N = int(T_final / dt)
t = 0.0

q_log = []
q_hat_log = []
b_hat_log = []

for k in range(N):
    # reference at current time
    qr = q_ref(t)
    # PD control using estimated state
    e = qr - x_hat[0]
    ed = 0.0 - x_hat[1]
    u = Kp * e + Kd * ed

    # plant propagation
    x_true = step_true(x_true, u)

    # generate measurement (delayed)
    y_now = x_true[0] + x_true[2] + np.random.randn() * np.sqrt(R[0, 0])
    meas_buffer[k % len(meas_buffer)] = y_now

    # EKF predict
    x_hat, P = ekf_predict(x_hat, P, u)

    # EKF update with delayed measurement (if available)
    # Here we approximate alignment by feeding measurement produced h seconds ago.
    idx_delay = (k - delay_steps) % len(meas_buffer)
    if k >= delay_steps:
        y_delayed = meas_buffer[idx_delay]
        x_hat, P = ekf_update(x_hat, P, y_delayed)

    # logs
    q_log.append(x_true[0])
    q_hat_log.append(x_hat[0])
    b_hat_log.append(x_hat[2])
    t += dt

print("Final bias estimate b_hat:", b_hat_log[-1])
      

This example demonstrates that even with measurement delay and bias, the augmented-state EKF can recover the bias and support stable PD control as long as the sampling time and delay are within the stability margin.

In C++, a typical implementation uses Eigen for linear algebra and, in practice, ROS or a similar middleware for timing and sensor I/O. Below is a simplified control-loop sketch assuming a fixed-rate timer and an encoder topic with known delay:

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

using Eigen::Vector3d;
using Eigen::Matrix3d;
using Eigen::RowVector3d;

struct Sample {
  double t;
  double y;   // encoder reading
};

int main() {
  double J = 0.05;
  double d = 0.01;
  double Kp = 50.0;
  double Kd = 2.0 * std::sqrt(Kp * J);

  double dt = 0.001;
  double h  = 0.02;
  int delaySteps = static_cast<int>(h / dt);

  Matrix3d A;
  A << 0.0, 1.0, 0.0,
         0.0, -d / J, 0.0,
         0.0, 0.0,  0.0;
  Eigen::Vector3d B;
  B << 0.0, 1.0 / J, 0.0;
  RowVector3d C;
  C << 1.0, 0.0, 1.0;

  Matrix3d Q = Matrix3d::Zero();
  Q(0,0) = 1e-6; Q(1,1) = 1e-4; Q(2,2) = 1e-8;
  double R = 1e-4;

  Vector3d x_hat(0.0, 0.0, 0.0);
  Matrix3d P = Matrix3d::Identity() * 1e-2;

  auto q_ref = [](double t) {
    return 0.5 * std::sin(2.0 * M_PI * 0.5 * t);
  };

  std::deque<Sample> meas_buffer;
  double t = 0.0;
  double x_true_q = 0.0;
  double x_true_qd = 0.0;
  double b_true = 0.05;

  for (int k = 0; k < 10000; ++k) {
    double qr = q_ref(t);
    double e  = qr - x_hat(0);
    double ed = 0.0 - x_hat(1);
    double u  = Kp * e + Kd * ed;

    // True dynamics (Euler)
    double qdd = (-d * x_true_qd + u) / J;
    x_true_q  += dt * x_true_qd;
    x_true_qd += dt * qdd;

    // Measurement at time t (will be used delayed)
    double noise = std::sqrt(R) * ((double)std::rand() / RAND_MAX - 0.5);
    double y = x_true_q + b_true + noise;
    meas_buffer.push_back({t, y});
    if (meas_buffer.size() > static_cast<size_t>(delaySteps + 2)) {
      meas_buffer.pop_front();
    }

    // EKF predict
    Matrix3d Ad = Matrix3d::Identity() + dt * A;
    Vector3d Bd = dt * B;
    x_hat = Ad * x_hat + Bd * u;
    P = Ad * P * Ad.transpose() + Q;

    // EKF update with delayed measurement
    if ((int)meas_buffer.size() > delaySteps) {
      Sample y_delayed = meas_buffer[meas_buffer.size() - delaySteps - 1];
      double innov = y_delayed.y - C * x_hat;
      double S = (C * P * C.transpose())(0,0) + R;
      Vector3d K_gain = P * C.transpose() / S;
      x_hat += K_gain * innov;
      P = (Matrix3d::Identity() - K_gain * C) * P;
    }

    t += dt;
  }

  std::cout << "Estimated bias: " << x_hat(2) << std::endl;
  return 0;
}
      

In a ROS-based system, the encoder callback would populate the measurement buffer, and the control loop would run in a real-time thread using the latest delayed-aligned sample.

For some robot joints, one can combine a fast but drifting IMU (angular velocity) with a slow but absolute encoder. A simple discrete-time complementary filter is

\[ \hat{q}_{k+1}^{-} = \hat{q}_{k} + T_s \omega_{k}^{\text{imu}},\quad \hat{q}_{k+1} = \alpha \hat{q}_{k+1}^{-} + (1-\alpha) q_{k+1}^{\text{enc}}, \]

where \( T_s \) is sample time and \( \alpha\in(0,1) \) weights integration vs absolute encoder reading. A larger \( \alpha \) trusts IMU more (fast response but more drift); a smaller \( \alpha \) trusts encoder more (slower but less drift).

Below is a Java implementation using basic types (a real system may use org.ejml or similar for full state-space models):

public class JointComplementaryFilter {
    private double Ts;      // sample time
    private double alpha;   // fusion weight
    private double qHat;    // filtered angle

    public JointComplementaryFilter(double Ts, double alpha, double q0) {
        this.Ts = Ts;
        this.alpha = alpha;
        this.qHat = q0;
    }

    public double update(double omegaImu, double qEnc) {
        // prediction (integrate IMU)
        double qPred = qHat + Ts * omegaImu;
        // fusion with encoder
        qHat = alpha * qPred + (1.0 - alpha) * qEnc;
        return qHat;
    }

    public static void main(String[] args) {
        double Ts = 0.001;
        double alpha = 0.95;
        JointComplementaryFilter filter =
                new JointComplementaryFilter(Ts, alpha, 0.0);

        double qEnc = 0.0;
        double omegaImu = 0.0;

        for (int k = 0; k < 10000; ++k) {
            // here we would read encoder and IMU from hardware
            // qEnc = readEncoder();
            // omegaImu = readImuGyro();

            double qFiltered = filter.update(omegaImu, qEnc);
            // controller would use qFiltered instead of raw sensor values
        }
    }
}
      

This simple fusion reduces the impact of IMU drift while preserving high-frequency responsiveness, improving the effective measurement for state feedback.

In MATLAB, we can create a continuous-time state-space model of the augmented joint system and then simulate a controller with a transport delay in the measurement path. Simulink provides a dedicated Transport Delay block.

J = 0.05;
d = 0.01;

A = [0 1 0;
     0 -d/J 0;
     0 0 0];
B = [0; 1/J; 0];
C = [1 0 1];
D = 0;

sys_aug = ss(A, B, C, D);

% PD gains
Kp = 50;
Kd = 2 * sqrt(Kp * J);
K  = [Kp Kd 0];

% Continuous-time closed-loop (assuming perfect estimator)
Acl = A - B * K;
sys_cl = ss(Acl, B, C, D);

% Discretization for implementation (zero-order hold)
Ts = 0.001;
sysd = c2d(sys_aug, Ts, 'zoh');

Ad = sysd.A;
Bd = sysd.B;
Cd = sysd.C;

% Luenberger observer for augmented state (without delay)
L = place(Ad', Cd', [0.1 0.09 0.08]).';

% Simulink hint:
%  - Create a State-Space block implementing the augmented dynamics.
%  - Add a PD controller block using estimated q and qd.
%  - Insert a "Transport Delay" block before the observer input.
%  - Implement the observer using a Discrete State-Space block with matrices:
%       x_hat(k+1) = Ad*x_hat + Bd*u + L*(y_delayed - Cd*x_hat)
%  - Scope joint position, estimated bias, and control effort.
      

Simulink allows systematic exploration of delay magnitude and observer gains, making it easy to visualize instability induced as delay approaches the theoretical delay margin.

Mathematica can symbolically or numerically study delay-differential equations and estimator error covariances. Below, we compute the spectrum of the scalar delayed system and simulate the estimation error covariance for a linear Kalman filter in discrete time.

(* Parameters *)
a = -2.0;
b = 1.0;
k = 3.0;
h = 0.05;

(* Characteristic equation: s - a + b k Exp[-s h] == 0 *)
charEq[s_] := s - a + b k Exp[-s h];

(* Plot of level curves Re(charEq) and Im(charEq) to inspect roots *)
ContourPlot[
  {
    Re[charEq[s]], Im[charEq[s]]
  } /. s -> x + I y,
  {x, -20, 5}, {y, -50, 50},
  Contours -> {0},
  ContourStyle -> {Red, Blue}
]

(* Discrete-time Kalman error covariance for a 1D position *)
A = 1.0;  (* x_{k+1} = x_k + w_k *)
C = 1.0;  (* y_k = x_k + v_k *)
q = 1.0*10^-4;
r = 1.0*10^-3;

kalmanStep[P_] := Module[{Ppred, K, Pnew},
  Ppred = A P A + q;
  K = Ppred C / (C Ppred C + r);
  Pnew = (1 - K C) Ppred;
  Pnew
];

P0 = 1.0;
plist = NestList[kalmanStep, P0, 100];

ListLinePlot[plist, AxesLabel -> {"k", "P_k"}, PlotRange -> All]
      

By varying h, a, and k in the characteristic equation, one can numerically approximate the delay margin. The covariance recursion illustrates how sensor noise levels and model uncertainty shape steady-state estimation error, which in turn propagates into closed-loop tracking error variance.

Problem 1 (Delay Margin for Scalar Feedback): Consider the scalar system \( \dot{x}(t) = a x(t) + b u(t) \) with delayed output \( y(t) = x(t-h) \) and control \( u(t) = -k y(t) \), with \( a < 0, b k > 0 \). Derive the expression for the critical delay \( h_{\max} \) above which the closed loop loses stability.

Solution: Substituting \( u(t) = -k x(t-h) \) yields

\[ \dot{x}(t) = a x(t) - b k x(t-h). \]

Using the ansatz \( x(t) = e^{s t} \) gives the characteristic equation

\[ s - a + b k e^{-s h} = 0. \]

At the boundary of stability, assume purely imaginary roots \( s = j\omega \). Plugging in:

\[ j\omega - a + b k\big(\cos(\omega h) - j\sin(\omega h)\big) = 0. \]

Equating real and imaginary parts:

\[ -a + b k \cos(\omega h) = 0,\quad \omega + b k \sin(\omega h) = 0. \]

From the first, \( \cos(\omega h) = \dfrac{a}{b k} \), which requires \( |a| \le |b k| \). Using \( \sin^2 + \cos^2 = 1 \), we find

\[ \sin(\omega h) = \pm \sqrt{1 - \frac{a^2}{b^2 k^2}},\quad \omega = -b k \sin(\omega h). \]

The smallest positive solution corresponds to \( \omega = \sqrt{b^2 k^2 - a^2} \) and \( \omega h = \arccos\!\big(\frac{a}{b k}\big) \). Therefore the critical delay is

\[ h_{\max} = \dfrac{\arccos\!\big(\frac{a}{b k}\big)} {\sqrt{b^2 k^2 - a^2}}, \]

and the system is asymptotically stable for \( 0 < h < h_{\max} \).

Problem 2 (Observability with Bias): For the 1-DOF joint with state \( \mathbf{x}_a = [q,\dot{q},b]' \) and measurement \( y = q + b \), show that the augmented pair \( (\mathbf{A}_a,\mathbf{C}_a) \) is observable, where

\[ \mathbf{A}_a = \begin{bmatrix} 0 & 1 & 0\\ 0 & 0 & 0\\ 0 & 0 & 0 \end{bmatrix},\quad \mathbf{C}_a = \begin{bmatrix}1 & 0 & 1\end{bmatrix}. \]

Solution: The observability matrix is

\[ \mathcal{O} = \begin{bmatrix} \mathbf{C}_a\\ \mathbf{C}_a \mathbf{A}_a\\ \mathbf{C}_a \mathbf{A}_a^2 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 1\\ 0 & 1 & 0\\ 0 & 0 & 0 \end{bmatrix}. \]

The rank of \( \mathcal{O} \) is 2, which is less than 3, so the system appears unobservable. However, note that the physical dynamics for \( \dot{q} \) (second state) are not fully captured in \( \mathbf{A}_a \) as given; we must include the effect of torque and friction to have the true system matrix. If we consider the more complete dynamics \( \dot{q} = \dot{q}, \dot{\dot{q}} = f(q,\dot{q},u) \) linearized around a trajectory, the corresponding \( \mathbf{A}_a \) will have nonzero entries in the second row, making \( \mathcal{O} \) full rank.

In other words, with realistic dynamics and \( y = q + b \), the bias is observable because motion couples \( q \) and \( \dot{q} \), allowing excitation that separates \( q \) and \( b \).

Problem 3 (Effect of Sensor Fusion on Closed-Loop Variance): For the discrete-time closed loop \( \mathbf{x}_{k+1} = (\mathbf{A} - \mathbf{B}\mathbf{K})\mathbf{x}_k + \mathbf{B}\mathbf{K}\mathbf{e}_k \), assume \( \mathbf{e}_k \) is zero-mean with covariance \( \mathbf{P} \), independent of \( \mathbf{x}_k \). Show that the steady-state state covariance \( \mathbf{\Sigma} = \mathbb{E}[\mathbf{x}_k\mathbf{x}_k'] \) satisfies a Lyapunov equation involving \( \mathbf{P} \).

Solution: Let \( \mathbf{F} = \mathbf{A} - \mathbf{B}\mathbf{K} \). Then

\[ \mathbf{x}_{k+1} = \mathbf{F}\mathbf{x}_k + \mathbf{B}\mathbf{K}\mathbf{e}_k. \]

Taking covariance and using independence and zero mean:

\[ \mathbf{\Sigma}_{k+1} = \mathbf{F}\mathbf{\Sigma}_k \mathbf{F}' + \mathbf{B}\mathbf{K}\mathbf{P}\mathbf{K}'\mathbf{B}'. \]

At steady state \( \mathbf{\Sigma}_{k+1} = \mathbf{\Sigma}_k = \mathbf{\Sigma} \), so

\[ \mathbf{\Sigma} = \mathbf{F}\mathbf{\Sigma}\mathbf{F}' + \mathbf{B}\mathbf{K}\mathbf{P}\mathbf{K}'\mathbf{B}'. \]

This is a discrete-time Lyapunov equation. If sensor fusion reduces \( \mathbf{P} \) (e.g. by combining redundant sensors), the forcing term decreases, leading to a smaller steady-state covariance \( \mathbf{\Sigma} \), i.e. lower state fluctuation variances.

Problem 4 (Complementary Filter Frequency Response): For the complementary filter \( \hat{q}(z) = \alpha z^{-1} \hat{q}(z) + (1-\alpha) q_{\text{enc}}(z) + \alpha T_s \Omega_{\text{imu}}(z) \) in the Z-domain (with \( z \) the shift operator), derive the transfer functions from encoder and IMU signals to \( \hat{q} \).

Solution: Rearranging:

\[ \hat{q}(z) - \alpha z^{-1} \hat{q}(z) = (1-\alpha) q_{\text{enc}}(z) + \alpha T_s \Omega_{\text{imu}}(z), \]

so

\[ \hat{q}(z) = \dfrac{1-\alpha}{1-\alpha z^{-1}} q_{\text{enc}}(z) + \dfrac{\alpha T_s}{1-\alpha z^{-1}} \Omega_{\text{imu}}(z). \]

Thus the transfer functions are

\[ H_{\text{enc}}(z) = \dfrac{1-\alpha}{1-\alpha z^{-1}},\quad H_{\text{imu}}(z) = \dfrac{\alpha T_s}{1-\alpha z^{-1}}. \]

For frequencies where \( |z| \approx 1 \), this implements a low-pass filter on encoder noise and a high-pass behavior on IMU integration errors, achieving complementary frequency responses.

Problem 5 (Effect of Unmodeled Delay on Observer): Suppose the observer in Section 1 is designed assuming \( y(t) = \mathbf{C}\mathbf{x}(t) + \mathbf{v}(t) \), but the true measurement is delayed: \( y(t) = \mathbf{C}\mathbf{x}(t-h) + \mathbf{v}(t) \). Qualitatively explain how this affects the estimation error dynamics and why increasing \( \|\mathbf{L}\| \) (observer gain) does not arbitrarily speed up convergence in presence of delay.

Solution: The observer uses \( y(t) - \mathbf{C}\hat{\mathbf{x}}(t) \) as an innovation. With delay, \( y(t) = \mathbf{C}\mathbf{x}(t-h) + \mathbf{v}(t) \), so the innovation becomes

\[ \mathbf{C}\mathbf{x}(t-h) - \mathbf{C}\hat{\mathbf{x}}(t) + \mathbf{v}(t), \]

which mixes the estimator state at time \( t \) with the plant state at time \( t-h \). The resulting error dynamics are a delay-differential system; high observer gains \( \mathbf{L} \) amplify the effect of the mismatch between \( \mathbf{x}(t-h) \) and \( \hat{\mathbf{x}}(t) \), leading to potential oscillations or instability. There is an observer delay margin beyond which increasing \( \|\mathbf{L}\| \) degrades instead of improving performance, analogous to controller delay margins.

We analyzed how measurement delay, sensor drift, and sensor-fusion strategies affect observer-based robot control. For scalar delayed feedback, we derived an explicit delay margin expression, illustrating the trade-off between feedback gain and tolerable delay. Bias was modeled as an augmented state, enabling joint estimation of physical states and sensor offsets, which is crucial for eliminating steady-state errors. Sensor fusion was framed both as weighted least squares and in the Kalman filter context, connecting estimator error covariance to closed-loop state variance. Finally, multi-language implementations demonstrated how these concepts manifest in practical robotic software stacks.