From kinematics and dynamics you know that an \(n\)-DOF robot can be modeled (after linearization around an operating point) as a linear state-space system with state vector \( \mathbf{x}(t) \in \mathbb{R}^{n_x} \), input (e.g. joint torques) \( \mathbf{u}(t) \in \mathbb{R}^{n_u} \) and measured outputs \( \mathbf{y}(t) \in \mathbb{R}^{n_y} \):

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

In robot control, a state-feedback law from previous chapters takes the form \( \mathbf{u}(t) = -\mathbf{K}\,\mathbf{x}(t) + \mathbf{u}_{\text{ff}}(t) \) (possibly with tracking and feedforward terms). However, sensors provide only \( \mathbf{y}(t) \), which is typically a subset of the state: joint encoders measure positions but not velocities; force-torque sensors measure contact wrenches but not full link momenta; cameras see end-effector pose but not full joint configuration.

Thus, there is a fundamental mismatch: controllers are designed in the state space, but hardware lives in the output space. State estimation is precisely the bridge between them: given a model and a stream of measurements, construct an estimate \( \hat{\mathbf{x}}(t) \) of the true (hidden) state \( \mathbf{x}(t) \).

flowchart TD
  R["Desired closed-loop behavior (tracking, stability)"]
    --> SF["State-space controller design (K)"]
  SF --> REQ["Requires full state x(t)"]
  REQ --> GAP["Sensors only give y(t)"]
  GAP --> EST["Introduce estimator to compute x_hat(t)"]
  EST --> CTRL["Implement u(t) = -K x_hat(t) + u_ff(t)"]
  CTRL --> R
        

In this lesson we do not design sophisticated estimators yet. Instead, we make precise:

• what it means to estimate a state mathematically,
• when a state can, in principle, be reconstructed from outputs, and
• how estimator dynamics affect closed-loop robot behavior.

Consider a single revolute joint of a robot around an operating point. A common linearized model uses the generalized coordinate \( q \) and its velocity \( \dot{q} \) as state:

\[ \mathbf{x}(t) = \begin{bmatrix} q(t) \\ \dot{q}(t) \end{bmatrix},\quad \dot{\mathbf{x}}(t) = \underbrace{ \begin{bmatrix} 0 & 1 \\ 0 & -d \end{bmatrix}}_{\mathbf{A}} \mathbf{x}(t) + \underbrace{ \begin{bmatrix} 0 \\ b \end{bmatrix}}_{\mathbf{B}} \mathbf{u}(t), \]

where \( d > 0 \) is damping and \( b > 0 \) scales control torque. The natural state-feedback law (for regulation) is

\[ \mathbf{u}(t) = -\mathbf{K}\,\mathbf{x}(t) = -\begin{bmatrix} k_1 & k_2 \end{bmatrix} \begin{bmatrix} q(t) \\ \dot{q}(t) \end{bmatrix}. \]

In real hardware, we often measure only the joint angle \( q(t) \):

\[ \mathbf{y}(t) = \mathbf{C}\,\mathbf{x}(t),\quad \mathbf{C} = \begin{bmatrix} 1 & 0 \end{bmatrix},\quad \mathbf{y}(t) = q(t). \]

A naïve idea is to replace velocities by finite differences of noisy encoder readings:

\[ \dot{q}(t_k) \approx \frac{q(t_k) - q(t_{k-1})}{T_s}, \]

with sampling period \( T_s \). If the encoder measurement is corrupted by random noise \( n_k \), then the discrete estimate actually uses \( q(t_k) + n_k \), and the noise component in the derivative estimate becomes

\[ \tilde{n}_k = \frac{n_k - n_{k-1}}{T_s}. \]

For uncorrelated noise with variance \( \operatorname{Var}(n_k) = \sigma_n^2 \), we have

\[ \operatorname{Var}(\tilde{n}_k) = \frac{\operatorname{Var}(n_k) + \operatorname{Var}(n_{k-1})}{T_s^2} = \frac{2\sigma_n^2}{T_s^2}, \]

which blows up as \( T_s \) decreases. So a naïve derivative-based estimator amplifies measurement noise dramatically, potentially destabilizing the feedback loop when \( k_2 \) is large. This illustrates that how we reconstruct unmeasured states (here, velocity) critically affects closed-loop performance.

Output-feedback laws of the form \( \mathbf{u}(t) = -\mathbf{F}\,\mathbf{y}(t) \) use only measurements but in general are less powerful than full state feedback: for an \(n_x\)-dimensional system, \(\mathbf{F} \in \mathbb{R}^{n_u \times n_y}\) has fewer degrees of freedom than \(\mathbf{K} \in \mathbb{R}^{n_u \times n_x}\) when \( n_y < n_x \). A state estimator allows us to recover the benefits of state feedback even when only partial measurements are available.

A realistic robot model must include unmodeled dynamics (e.g. friction, flexible couplings) and sensor noise. A standard continuous-time stochastic model is

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

where \( \mathbf{w}(t) \) is process noise (unmodeled disturbances) and \( \mathbf{v}(t) \) is measurement noise. Typically we assume zero-mean random processes with covariance matrices \( \mathbf{Q} \) and \( \mathbf{R} \):

\[ \mathbb{E}[\mathbf{w}(t)] = \mathbf{0},\quad \mathbb{E}[\mathbf{v}(t)] = \mathbf{0},\quad \mathbb{E}[\mathbf{w}(t)\mathbf{w}(\tau)^{\top}] = \mathbf{Q}\,\delta(t-\tau),\quad \mathbb{E}[\mathbf{v}(t)\mathbf{v}(\tau)^{\top}] = \mathbf{R}\,\delta(t-\tau). \]

An estimator produces an estimate \( \hat{\mathbf{x}}(t) \) based on past inputs and outputs:

\[ \hat{\mathbf{x}}(t) = \mathcal{E}\big( \{\mathbf{y}(\tau)\}_{0\leq \tau\leq t}, \{\mathbf{u}(\tau)\}_{0\leq \tau\leq t}\big). \]

The estimation error is \( \mathbf{e}(t) = \mathbf{x}(t) - \hat{\mathbf{x}}(t) \). Two key performance measures are:

• Bias: \( \mathbb{E}[\mathbf{e}(t)] \). An estimator is (asymptotically) unbiased if \( \mathbb{E}[\mathbf{e}(t)] \to \mathbf{0} \) as \( t \to \infty \).
• Error covariance: \( \mathbf{P}(t) = \mathbb{E}[\mathbf{e}(t)\mathbf{e}(t)^{\top}] \). Small \( \mathbf{P}(t) \) means that \( \hat{\mathbf{x}}(t) \) is tightly concentrated around \( \mathbf{x}(t) \).

In control-oriented design, a good estimator balances:

• Responsiveness to new measurements (fast error decay),
• Robustness to noise (not overly amplifying \( \mathbf{v}(t) \)), and
• Compatibility with the controller (closed-loop stability and performance).

Later lessons will construct specific estimators (observers, Kalman-type filters). In this lesson we focus on structural requirements: when is state reconstruction even possible, and how does it interact with feedback?

For linear time-invariant (LTI) systems, observability characterizes whether the initial state can be reconstructed from output measurements. Consider

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

with input \( \mathbf{u}(t) = \mathbf{0} \) (we can always subtract the known forced response due to \( \mathbf{u}(\cdot) \)). Define the observability matrix

\[ \mathcal{O} = \begin{bmatrix} \mathbf{C} \\ \mathbf{C}\mathbf{A} \\ \mathbf{C}\mathbf{A}^2 \\ \vdots \\ \mathbf{C}\mathbf{A}^{n_x-1} \end{bmatrix} \in \mathbb{R}^{n_x n_y \times n_x}. \]

Definition. The pair \( (\mathbf{A},\mathbf{C}) \) is observable if the only initial state that produces identically zero output \( \mathbf{y}(t) \equiv \mathbf{0} \) for all \( t\geq 0 \) is \( \mathbf{x}(0) = \mathbf{0} \).

Theorem (LTI observability test). The pair \( (\mathbf{A},\mathbf{C}) \) is observable if and only if \( \operatorname{rank}(\mathcal{O}) = n_x \).

Sketch of proof.

1. The solution of \( \dot{\mathbf{x}} = \mathbf{A}\mathbf{x} \) is \( \mathbf{x}(t) = e^{\mathbf{A} t}\mathbf{x}(0) \). Thus \( \mathbf{y}(t) = \mathbf{C} e^{\mathbf{A} t}\mathbf{x}(0) \).
2. Expand \( e^{\mathbf{A} t} \) as a power series around \( t=0 \): \[ \mathbf{y}(t) = \sum_{k=0}^{\infty} \frac{t^k}{k!}\,\mathbf{C}\mathbf{A}^k \mathbf{x}(0). \] If \( \mathbf{y}(t) \equiv \mathbf{0} \), then all derivatives at \( t=0 \) vanish, so \( \mathbf{C}\mathbf{A}^k \mathbf{x}(0) = \mathbf{0} \) for \( k=0,\dots,n_x-1 \).
3. Stacking these conditions yields \( \mathcal{O}\,\mathbf{x}(0) = \mathbf{0} \). Therefore, if \( \operatorname{rank}(\mathcal{O}) = n_x \), we must have \( \mathbf{x}(0) = \mathbf{0} \).
4. Conversely, if \( \operatorname{rank}(\mathcal{O}) < n_x \), there exists \( \mathbf{x}(0) \neq \mathbf{0} \) in the null space of \( \mathcal{O} \) that yields \( \mathbf{y}(t) \equiv \mathbf{0} \), so the system is not observable.

For robot control, observability has a direct interpretation: if \( (\mathbf{A},\mathbf{C}) \) is not observable, then no estimator based solely on \( \mathbf{y}(t) \) can reconstruct the full state. In that case, either additional sensors or a different control structure (e.g. purely output feedback with reduced performance) must be considered.

A standard deterministic observer (Luenberger-type) for an observable pair \( (\mathbf{A},\mathbf{C}) \) has the structure

\[ \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), \]

where \( \mathbf{L} \) is the observer gain. The term \( \mathbf{y}(t) - \mathbf{C}\hat{\mathbf{x}}(t) \) is the innovation (output prediction error). Design of \( \mathbf{L} \) is addressed in the next lesson; here we analyze its role in feedback.

Suppose we implement state feedback using the estimated state:

\[ \mathbf{u}(t) = -\mathbf{K}\,\hat{\mathbf{x}}(t). \]

The true plant evolves as

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

Define the estimation error \( \mathbf{e}(t) = \mathbf{x}(t) - \hat{\mathbf{x}}(t) \). Subtracting the observer dynamics from the plant dynamics yields

\[ \begin{aligned} \dot{\mathbf{e}}(t) &= \dot{\mathbf{x}}(t) - \dot{\hat{\mathbf{x}}}(t) \\ &= \big(\mathbf{A}\mathbf{x}(t) - \mathbf{B}\mathbf{K}\hat{\mathbf{x}}(t)\big) - \big(\mathbf{A}\hat{\mathbf{x}}(t) - \mathbf{B}\mathbf{K}\hat{\mathbf{x}}(t) + \mathbf{L}(\mathbf{C}\mathbf{x}(t) - \mathbf{C}\hat{\mathbf{x}}(t))\big) \\ &= (\mathbf{A} - \mathbf{L}\mathbf{C})\,\mathbf{e}(t). \end{aligned} \]

In the \((\mathbf{x},\mathbf{e})\) coordinates, the closed-loop dynamics are

\[ \begin{bmatrix} \dot{\mathbf{x}}(t) \\ \dot{\mathbf{e}}(t) \end{bmatrix} = \underbrace{ \begin{bmatrix} \mathbf{A} - \mathbf{B}\mathbf{K} & \mathbf{B}\mathbf{K} \\ \mathbf{0} & \mathbf{A} - \mathbf{L}\mathbf{C} \end{bmatrix}}_{\mathbf{M}} \begin{bmatrix} \mathbf{x}(t) \\ \mathbf{e}(t) \end{bmatrix}. \]

The matrix \( \mathbf{M} \) is block upper triangular. Therefore, its eigenvalues are precisely the eigenvalues of the diagonal blocks:

\[ \lambda(\mathbf{M}) = \lambda\big(\mathbf{A}-\mathbf{B}\mathbf{K}\big) \cup \lambda\big(\mathbf{A}-\mathbf{L}\mathbf{C}\big). \]

This has two fundamental consequences for robot control:

• If \( (\mathbf{A},\mathbf{B}) \) is controllable, we can choose \( \mathbf{K} \) so that all eigenvalues of \( \mathbf{A}-\mathbf{B}\mathbf{K} \) lie in the open left half-plane, ensuring desirable state-feedback dynamics.
• If \( (\mathbf{A},\mathbf{C}) \) is observable, we can choose \( \mathbf{L} \) so that all eigenvalues of \( \mathbf{A}-\mathbf{L}\mathbf{C} \) lie in the open left half-plane, ensuring fast decay of the estimation error.

Crucially, these designs can be carried out independently. This is known as the separation principle: for LTI systems with process/measurement noise modeled as in Section 3, optimal or stabilizing state-feedback and estimation design can be performed separately, yet the combined observer-based controller remains stable.

flowchart TD
  R["Reference / desired state"] --> C["Controller uses x_hat(t)"]
  C --> U["Control input u(t)"]
  U --> P["Robot dynamics (A,B)"]
  P --> Y["Sensors: y(t) = C x(t)"]
  Y --> O["Estimator with gain L"]
  O --> C
  P --> H["Hidden state x(t)"]
  O --> EH["Estimated state x_hat(t)"]
        

This block structure explains why estimation matters: poor estimator dynamics (e.g. unstable \( \mathbf{A}-\mathbf{L}\mathbf{C} \)) will destabilize or severely deteriorate the closed-loop behavior even if the state-feedback gain \( \mathbf{K} \) alone would yield excellent performance.

We now implement a simple simulation showing how estimation interacts with feedback for the 1-DOF joint model of Section 2. We use the LTI model:

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

Choose a stabilizing state-feedback gain \( \mathbf{K} = \begin{bmatrix} 3 & 2 \end{bmatrix} \) and an observer gain \( \mathbf{L}^{\top} = \begin{bmatrix} 5 & 6 \end{bmatrix} \). We discretize the dynamics with explicit Euler for simplicity: \( \mathbf{x}_{k+1} \approx \mathbf{x}_k + T_s \dot{\mathbf{x}}_k \).

6.1 Python (NumPy, typical in ROS-based stacks)

In Python, roboticists often combine numpy, scipy, and packages such as python-control or ROS-based simulation frameworks. Here we implement the basic loop with NumPy only.

import numpy as np

A = np.array([[0.0, 1.0],
              [0.0, -0.5]])
B = np.array([[0.0],
              [1.0]])
C = np.array([[1.0, 0.0]])
K = np.array([[3.0, 2.0]])      # state-feedback gain
L = np.array([[5.0],
              [6.0]])           # observer gain

Ts = 0.001
steps = 5000

x = np.array([[0.5],
              [0.0]])           # true initial state
x_hat = np.zeros((2, 1))        # initial estimate
rng = np.random.default_rng(0)

for k in range(steps):
    t = k * Ts

    # measurement with additive Gaussian noise
    y = C @ x + 0.01 * rng.standard_normal((1, 1))

    # control uses estimated state
    u = -K @ x_hat

    # true and estimated dynamics (Euler discretization)
    x_dot = A @ x + B @ u
    x_hat_dot = A @ x_hat + B @ u + L @ (y - C @ x_hat)

    x += Ts * x_dot
    x_hat += Ts * x_hat_dot

    if k % 1000 == 0:
        print(f"t={t:5.3f}  q={x[0,0]: .3f}  q_hat={x_hat[0,0]: .3f}")
      

6.2 C++ (Eigen, common in low-level robot control)

In C++ robot control stacks (e.g. ROS control, whole-body controllers), Eigen is a de facto standard for linear algebra.

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

int main() {
    using namespace Eigen;

    Matrix2d A;
    A << 0.0, 1.0,
          0.0, -0.5;
    Vector2d B;
    B << 0.0, 1.0;
    RowVector2d C;
    C << 1.0, 0.0;
    RowVector2d K;
    K << 3.0, 2.0;
    Vector2d L;
    L << 5.0, 6.0;

    double Ts = 0.001;
    int steps = 5000;

    Vector2d x;      x << 0.5, 0.0;
    Vector2d x_hat;  x_hat.setZero();

    std::mt19937 gen(0);
    std::normal_distribution<double> dist(0.0, 0.01);

    for (int k = 0; k < steps; ++k) {
        double t = k * Ts;

        double y = C * x + dist(gen);
        double y_hat = C * x_hat;
        double u = -K * x_hat;

        Vector2d x_dot = A * x + B * u;
        Vector2d x_hat_dot = A * x_hat + B * u + L * (y - y_hat);

        x += Ts * x_dot;
        x_hat += Ts * x_hat_dot;

        if (k % 1000 == 0) {
            std::cout << "t=" << t
                      << "  q=" << x(0)
                      << "  q_hat=" << x_hat(0)
                      << std::endl;
        }
    }
    return 0;
}
      

6.3 Java (manual matrices, or using EJML / Apache Commons Math)

Java-based robotic frameworks (e.g. some industrial arms, Android-based platforms) frequently use libraries like EJML or Apache Commons Math. Here we implement the 2D case explicitly.

public class JointObserverDemo {
    public static void main(String[] args) {
        double[][] A = { {0.0, 1.0},
                        {0.0, -0.5} };
        double[] B = {0.0, 1.0};
        double[] C = {1.0, 0.0};
        double[] K = {3.0, 2.0};
        double[] L = {5.0, 6.0};

        double Ts = 0.001;
        int steps = 5000;

        double[] x = {0.5, 0.0};
        double[] xHat = {0.0, 0.0};

        java.util.Random rng = new java.util.Random(0);

        for (int k = 0; k < steps; ++k) {
            double t = k * Ts;

            double y = C[0] * x[0] + C[1] * x[1]
                       + 0.01 * rng.nextGaussian();
            double yHat = C[0] * xHat[0] + C[1] * xHat[1];

            double u = -(K[0] * xHat[0] + K[1] * xHat[1]);

            double xdot0 = A[0][0] * x[0] + A[0][1] * x[1] + B[0] * u;
            double xdot1 = A[1][0] * x[0] + A[1][1] * x[1] + B[1] * u;

            double e = y - yHat;
            double xHatDot0 = A[0][0] * xHat[0] + A[0][1] * xHat[1]
                              + B[0] * u + L[0] * e;
            double xHatDot1 = A[1][0] * xHat[0] + A[1][1] * xHat[1]
                              + B[1] * u + L[1] * e;

            x[0] += Ts * xdot0;
            x[1] += Ts * xdot1;
            xHat[0] += Ts * xHatDot0;
            xHat[1] += Ts * xHatDot1;

            if (k % 1000 == 0) {
                System.out.printf("t=%.3f  q=%.3f  q_hat=%.3f%n",
                                  t, x[0], xHat[0]);
            }
        }
    }
}
      

6.4 MATLAB / Simulink

In MATLAB, the ss (state-space) object and lsim are convenient for simulation. In Simulink, the same system can be realized with a State-Space block connected to a discrete-time observer block.

A = [0 1; 0 -0.5];
B = [0; 1];
C = [1 0];
K = [3 2];
L = [5; 6];

Ts = 1e-3;
Tfinal = 5.0;
t = 0:Ts:Tfinal;
N = numel(t);

x = [0.5; 0.0];
x_hat = [0; 0];

q = zeros(1, N);
q_hat = zeros(1, N);

rng(0);

for k = 1:N
    y = C * x + 0.01 * randn;   % noisy measurement
    u = -K * x_hat;

    x_dot = A * x + B * u;
    x_hat_dot = A * x_hat + B * u + L * (y - C * x_hat);

    x = x + Ts * x_dot;
    x_hat = x_hat + Ts * x_hat_dot;

    q(k) = x(1);
    q_hat(k) = x_hat(1);
end

plot(t, q, t, q_hat, '--');
legend('q', 'q\_hat');
xlabel('time [s]');
ylabel('position [rad]');
title('Joint position with observer-based feedback');
      

6.5 Wolfram Mathematica

Mathematica has built-in support for state-space models via StateSpaceModel and NDSolve. Below we directly simulate the combined plant-observer system.

A = { {0., 1.}, {0., -0.5} };
B = { {0.}, {1.} };
C = { {1., 0.} };
K = { {3., 2.} };
L = { {5.}, {6.} };

Ts = 0.001;
tmax = 5.0;

(* Define continuous-time dynamics for x and xHat *)
eqns = {
  x1'[t] == A[[1, 1]] x1[t] + A[[1, 2]] x2[t] +
            B[[1, 1]] (-K[[1, 1]] xHat1[t] - K[[1, 2]] xHat2[t]),
  x2'[t] == A[[2, 1]] x1[t] + A[[2, 2]] x2[t] +
            B[[2, 1]] (-K[[1, 1]] xHat1[t] - K[[1, 2]] xHat2[t]),
  xHat1'[t] ==
     A[[1, 1]] xHat1[t] + A[[1, 2]] xHat2[t] +
     B[[1, 1]] (-K[[1, 1]] xHat1[t] - K[[1, 2]] xHat2[t]) +
     L[[1, 1]] (C[[1, 1]] x1[t] + C[[1, 2]] x2[t] -
                (C[[1, 1]] xHat1[t] + C[[1, 2]] xHat2[t])),
  xHat2'[t] ==
     A[[2, 1]] xHat1[t] + A[[2, 2]] xHat2[t] +
     B[[2, 1]] (-K[[1, 1]] xHat1[t] - K[[1, 2]] xHat2[t]) +
     L[[2, 1]] (C[[1, 1]] x1[t] + C[[1, 2]] x2[t] -
                (C[[1, 1]] xHat1[t] + C[[1, 2]] xHat2[t])),

  x1[0] == 0.5, x2[0] == 0.0,
  xHat1[0] == 0.0, xHat2[0] == 0.0
};

sol = NDSolve[eqns, {x1, x2, xHat1, xHat2}, {t, 0, tmax}][[1]];

Plot[{x1[t] /. sol, xHat1[t] /. sol}, {t, 0, tmax},
     PlotLegends -> {"q(t)", "qHat(t)"},
     AxesLabel -> {"t", "position"}]
      

Across all languages, the same structural pattern appears: the plant dynamics and the observer dynamics share the same model matrices, and the innovation term \( \mathbf{y} - \mathbf{C}\hat{\mathbf{x}} \) couples measurement information into the estimator. In full robot controllers (e.g. whole-body control, operational-space control), the state vector and matrices \( \mathbf{A},\mathbf{B},\mathbf{C} \) grow in dimension but the estimation-feedback pattern remains the same.

Problem 1 (Observability of a Joint with Encoder Only). Consider the joint model of Section 2 with \( \mathbf{A}, \mathbf{C} \) as given there and no input for simplicity. Show that the pair \( (\mathbf{A},\mathbf{C}) \) is observable.

Solution. We have

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

The observability matrix is

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

Clearly \( \operatorname{rank}(\mathcal{O}) = 2 = n_x \). Thus the system is observable: encoder measurements are sufficient to reconstruct both position and velocity (in the absence of noise).

Problem 2 (Observer Error Dynamics). For the general observer of Section 5, derive the error dynamics \( \dot{\mathbf{e}}(t) = (\mathbf{A} - \mathbf{L}\mathbf{C})\mathbf{e}(t) \) explicitly from the plant and observer equations.

Solution. Recall

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

Define \( \mathbf{e} = \mathbf{x} - \hat{\mathbf{x}} \). Then

\[ \begin{aligned} \dot{\mathbf{e}} &= \dot{\mathbf{x}} - \dot{\hat{\mathbf{x}}} \\ &= (\mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u}) - \big(\mathbf{A}\hat{\mathbf{x}} + \mathbf{B}\mathbf{u} + \mathbf{L}(\mathbf{C}\mathbf{x} - \mathbf{C}\hat{\mathbf{x}})\big) \\ &= \mathbf{A}(\mathbf{x} - \hat{\mathbf{x}}) - \mathbf{L}\mathbf{C}(\mathbf{x} - \hat{\mathbf{x}}) \\ &= (\mathbf{A} - \mathbf{L}\mathbf{C})\,\mathbf{e}. \end{aligned} \]

Thus the estimation error dynamics are autonomous and independent of the input \( \mathbf{u} \). This is key to the separation principle.

Problem 3 (Noise Amplification by Differentiation). Suppose encoder noise \( n_k \) at sampling times \( t_k \) is zero-mean, independent, with variance \( \sigma_n^2 \). Show that the variance of the finite-difference derivative estimate \( \dot{q}_k = (q_k - q_{k-1})/T_s \) has a noise component with variance \( 2\sigma_n^2 / T_s^2 \).

Solution. Write the measured position as \( q_k^{\text{meas}} = q_k^{\text{true}} + n_k \). The derivative estimate uses

\[ \dot{q}_k^{\text{meas}} = \frac{q_k^{\text{meas}} - q_{k-1}^{\text{meas}}}{T_s} = \frac{q_k^{\text{true}} - q_{k-1}^{\text{true}}}{T_s} + \frac{n_k - n_{k-1}}{T_s}. \]

The noise term is \( \tilde{n}_k = (n_k - n_{k-1})/T_s \). Since \( n_k, n_{k-1} \) are independent with variance \( \sigma_n^2 \),

\[ \operatorname{Var}(\tilde{n}_k) = \frac{\operatorname{Var}(n_k) + \operatorname{Var}(n_{k-1})}{T_s^2} = \frac{2\sigma_n^2}{T_s^2}. \]

Hence differentiation amplifies noise by a factor proportional to \( 1/T_s^2 \), explaining why high-gain derivative control is dangerous with noisy encoders.

Problem 4 (Need for Observability in Observer-Based Feedback). Let \( (\mathbf{A},\mathbf{B}) \) be controllable but \( (\mathbf{A},\mathbf{C}) \) not observable. Argue why we cannot, in general, choose an observer gain \( \mathbf{L} \) such that \( \mathbf{A}-\mathbf{L}\mathbf{C} \) is asymptotically stable.

Solution. If \( (\mathbf{A},\mathbf{C}) \) is not observable, the observability matrix \( \mathcal{O} \) has rank strictly smaller than \( n_x \), so there exists a nonzero vector \( \mathbf{x}_0 \) such that \( \mathbf{C}\mathbf{A}^k \mathbf{x}_0 = \mathbf{0} \) for all \( k=0,\dots,n_x-1 \). This vector spans an unobservable subspace. Any observer whose correction term uses only \( \mathbf{y} - \mathbf{C}\hat{\mathbf{x}} \) cannot distinguish between trajectories differing by components in this subspace. Consequently, the projection of the error dynamics onto that subspace cannot be influenced by \( \mathbf{L} \). If the corresponding eigenvalues of \( \mathbf{A} \) lie on or to the right of the imaginary axis, no choice of \( \mathbf{L} \) can make all eigenvalues of \( \mathbf{A}-\mathbf{L}\mathbf{C} \) strictly stable. Thus observability is a necessary condition for stabilizing the estimation error dynamics via an observer.

Problem 5 (Separation Principle and Closed-Loop Eigenvalues). For the block upper triangular matrix \( \mathbf{M} \) in Section 5, show that its characteristic polynomial factorizes as the product of the characteristic polynomials of \( \mathbf{A}-\mathbf{B}\mathbf{K} \) and \( \mathbf{A}-\mathbf{L}\mathbf{C} \).

Solution. Let \( \mathbf{M} \) be

\[ \mathbf{M} = \begin{bmatrix} \mathbf{A}-\mathbf{B}\mathbf{K} & \mathbf{B}\mathbf{K} \\ \mathbf{0} & \mathbf{A}-\mathbf{L}\mathbf{C} \end{bmatrix}. \]

Then

\[ \lambda \mathbf{I} - \mathbf{M} = \begin{bmatrix} \lambda \mathbf{I} - (\mathbf{A}-\mathbf{B}\mathbf{K}) & -\mathbf{B}\mathbf{K} \\ \mathbf{0} & \lambda \mathbf{I} - (\mathbf{A}-\mathbf{L}\mathbf{C}) \end{bmatrix}. \]

This matrix is block upper triangular; hence its determinant is the product of the determinants of the diagonal blocks:

\[ \det(\lambda \mathbf{I} - \mathbf{M}) = \det\big(\lambda \mathbf{I} - (\mathbf{A}-\mathbf{B}\mathbf{K})\big)\, \det\big(\lambda \mathbf{I} - (\mathbf{A}-\mathbf{L}\mathbf{C})\big). \]

But these determinants are precisely the characteristic polynomials of \( \mathbf{A}-\mathbf{B}\mathbf{K} \) and \( \mathbf{A}-\mathbf{L}\mathbf{C} \), respectively. Thus the closed-loop eigenvalues are the union of the state-feedback and observer eigenvalues, justifying the separation principle.

This lesson established the necessity of state estimation in robot control. We reviewed how state-feedback laws derived in earlier chapters require full knowledge of \( \mathbf{x}(t) \) while sensors provide only \( \mathbf{y}(t) = \mathbf{C}\mathbf{x}(t) + \mathbf{v}(t) \). Using linear system theory, we defined the estimation error, its covariance, and the observability matrix, proving that full-state reconstruction is possible if and only if \( \operatorname{rank}(\mathcal{O}) = n_x \).

We also introduced the canonical observer-based feedback structure and derived the coupled closed-loop dynamics, highlighting the separation between state-feedback and observer eigenvalues. Simple simulations in Python, C++, Java, MATLAB/Simulink, and Mathematica illustrated how an estimator can replace unsafe derivative approximations and enable robust state-feedback control using standard robotics software stacks.

The next lesson will exploit these structural insights to design observers (choose \( \mathbf{L} \)) for robotic systems and analyze their convergence rigorously.