16. Stochastic State Estimation and Kalman Filter

A concrete motion-control example — estimating a servo motor’s angular velocity — motivates the Kalman filter, the optimal recursive estimator for state estimation under noise.

Lecture videos

Slides

Download Kalman filter

Interactive example

Steady-state Kalman filter

link to download code

The example sets up a discrete-time, second-order stochastic system (A, C) driven by process noise entering through Bw. la.solve_discrete_lyapunov(A, Bw @ Bw.T) solves for the system’s steady-state open-loop state covariance Xss, and X11 is the variance of the first state — used below to scale the measurement noise relative to the process itself.

V = r**2 * X11 sets the measurement-noise covariance as a multiple of that state variance, and ct.dare(...) solves the discrete algebraic Riccati equation for the steady-state Kalman filter gain F. The slider controls r (the measurement-noise ratio): a larger r means noisier, less trustworthy measurements, so the filter gain drops and it leans more on the model’s prediction; a smaller r means cleaner measurements, so the gain rises and the filter trusts the sensor more.

import numpy as np
import scipy.linalg as la
import control.matlab as ct
import json

A = np.array([[0, 1], [0, 0.7114]])
Bw = np.array([[0.0384], [0.0722]])
C = np.array([[1, 0]])
W = np.array([[1]])

Xss = la.solve_discrete_lyapunov(A, Bw @ Bw.T)
X11 = Xss[0, 0]
V = float(param_value) ** 2 * X11
M, _, F = ct.dare(A.T, C.T, Bw @ W @ Bw.T, V)
F = F.T
json.dumps({"steady_state_gain_F": F.flatten().tolist(), "process_cov_X11": float(X11)})

Steady-state Kalman filter (mcimp/codes/kf/second_order_kf.py)

Measurement noise ratio r: Run

Click "Run" to execute.

← All chapters