第1回 制御とは何か：目的とノイズの問題
- フィードバック制御の目的
- 状態変数と出力の関係
- ノイズと不確実性の発生要因
- PID制御と現代制御の違い
- Python演習：ノイズ付き制御信号のシミュレーション
第2回 高校数学からの数学基礎復習
- 微分・積分と指数関数
- ベクトル・行列・固有値
- 確率変数・期待値・分散・共分散
- Pythonで行列演算・乱数生成
第3回 状態空間表現と制御モデル
-
状態方程式
ẋ = A x + B u y = C x -
次元・安定性・可制御性・可観測性
-
NumPyで行列シミュレーション
-
可観測性行列 rank(𝒪) の確認
第4回 ノイズを含むシステム（状態空間＋確率）
-
ノイズ付き状態方程式
ẋ = A x + B u + w y = C x + v -
w, v の統計的性質（平均0, 分散Q,R）
-
Python演習：ノイズ付きシステムのシミュレーション
第5回 オブザーバ理論（Luenberger Observer）
以下で要点→式→Python演習。
【モデルと統計】
連続時間（CT）ノイズ付き状態空間
ẋ(t) = A x(t) + B u(t) + w(t)
y(t) = C x(t) + v(t)
w(t): 平均0, 連続時間共分散密度 Q（E[w(t) w(τ)ᵀ] = Q δ(t-τ)）
v(t): 平均0, 連続時間共分散密度 R（E[v(t) v(τ)ᵀ] = R δ(t-τ)）
w と v は互いに独立、初期状態 x(0) は平均 μ₀, 共分散 P₀
離散化（サンプル時間 Δt）
x_{k+1} = A_d x_k + B_d u_k + w_k
y_k = C x_k + v_k
A_d = expm(A Δt)
B_d = ∫₀^{Δt} expm(A τ) dτ B dτ
w_k ~ N(0, Q_d), v_k ~ N(0, R_d)
Q_d = ∫₀^{Δt} expm(A τ) Q expm(Aᵀ τ) dτ
R_d = R / Δt （連続時間の密度Rを離散化。白色雑音の積分として幅Δtで分散が比例）
（実装では Van Loan 行列で Q_d, B_d を同時に厳密計算）
【Python演習：LTI連続モデル→厳密離散化→ノイズ付きシミュレーション】
import numpy as np
from numpy.linalg import cholesky
from scipy.linalg import expm
import matplotlib.pyplot as plt
# ---------------------------
# Parameters (centralized)
# ---------------------------
# Plant dimensions
n = 2 # state dim
m = 1 # input dim
p = 1 # output dim
# Continuous-time matrices (example: 2nd-order mass-spring-damper in controllable form)
A = np.array([[0.0, 1.0],
[-1.0, -0.4]]) # natural freq ~1 rad/s, damping ~0.2
B = np.array([[0.0],
[1.0]])
C = np.array([[1.0, 0.0]])
# Continuous-time noise intensities (density)
Q = np.diag([0.02, 0.02]) # process noise density
R = np.array([[0.5]]) # measurement noise density
# Initial condition statistics
x0_mean = np.array([0.5, 0.0])
P0 = np.diag([0.1, 0.1])
# Input signal parameters
u_amp = 1.0
u_freq = 0.3 # Hz
# Discretization
dt = 0.01 # s
T = 10.0 # total time [s]
N = int(T/dt)
# Random seed (reproducibility)
rng = np.random.default_rng(42)
# ---------------------------
# Exact discretization (Van Loan) for A_d, Q_d, B_d
# ---------------------------
def discretize_exact(A, B, Q, dt):
"""
Compute Ad, Bd, Qd for continuous LTI: x_dot = A x + B u + w, E[w w^T] = Q δ
Using Van Loan method.
"""
n = A.shape[0]
M = np.zeros((2*n, 2*n))
# Van Loan block for Qd
# M = [[-A, Q],
# [0 , A.T]]
M[:n, :n] = -A
M[:n, n:] = Q
M[n:, n:] = A.T
Md = expm(M * dt)
# Extract blocks
Phi12 = Md[:n, n:]
Phi22 = Md[n:, n:]
Qd = Phi22.T @ Phi12
# Ad and Bd
Ad = expm(A * dt)
# Compute Bd = ∫0^dt exp(A τ) dτ B
# Use augmented matrix for integral of exp(A τ):
# Aug = [[A, B],
# [0, 0]]
Aug = np.block([[A, B],
[np.zeros((1, n+1))]]) # m=1 generalize below
# For general m, build properly:
Aug = np.block([[A, B],
[np.zeros((B.shape[1], A.shape[0] + B.shape[1]))]])
Ed = expm(Aug * dt)
Ad_int = Ed[:n, :n]
Int = Ed[:n, n:]
Bd = Int # equals ∫ exp(A τ) B dτ
return Ad, Bd, Qd
Ad, Bd, Qd = discretize_exact(A, B, Q, dt)
Rd = R / dt # discrete measurement covariance
# Cholesky factors for sampling
S_Q = cholesky(Qd + 1e-12*np.eye(n)) # jitter for numerical PD
S_R = cholesky(Rd + 1e-12*np.eye(p))
S_P0 = cholesky(P0 + 1e-12*np.eye(n))
# ---------------------------
# Input, state, output simulation
# ---------------------------
t = np.arange(N+1) * dt
u = u_amp * np.sin(2*np.pi*u_freq * t) # deterministic input
x = np.zeros((n, N+1))
y = np.zeros((p, N+1))
# Sample initial state from N(x0_mean, P0)
x[:, 0] = x0_mean + S_P0 @ rng.standard_normal(n)
# Step through time
for k in range(N):
wk = S_Q @ rng.standard_normal(n)
vk = S_R @ rng.standard_normal(p)
# State update
x[:, k+1] = Ad @ x[:, k] + Bd @ u[k] + wk
# Measurement
y[:, k] = C @ x[:, k] + vk
# last measurement
y[:, N] = C @ x[:, N] + (S_R @ rng.standard_normal(p))
# ---------------------------
# Plots (English labels)
# ---------------------------
plt.figure(figsize=(9,4))
plt.plot(t, x[0,:], label="state x1")
plt.plot(t, x[1,:], label="state x2")
plt.xlabel("Time [s]")
plt.ylabel("States")
plt.title("Noisy State Trajectory (Discrete Simulation)")
plt.legend()
plt.grid(True)
plt.figure(figsize=(9,4))
plt.plot(t, y[0,:], label="measurement y")
plt.plot(t, C @ x, label="true output Cx", linestyle="--")
plt.xlabel("Time [s]")
plt.ylabel("Output")
plt.title("Measurement with Noise")
plt.legend()
plt.grid(True)
plt.show()
# ---------------------------
# Sanity checks (prints)
# ---------------------------
print("Ad shape:", Ad.shape, "Bd shape:", Bd.shape, "Qd shape:", Qd.shape, "Rd shape:", Rd.shape)
print("Qd eig (min, max):", np.linalg.eigvalsh(Qd)[0], np.linalg.eigvalsh(Qd)[-1])
print("Sample x mean approx:", x.mean(axis=1))
【ポイント】
-
R の離散化
連続時間の測定雑音 v(t) は密度 R（単位：出力²/秒）
離散観測 v_k は区間平均に相当し分散が Δt に反比例
よって Rd = R / dt -
Q_d, B_d の厳密離散化
Van Loan 法で Q_d を安定かつ厳密に算出
B_d は拡大行列の指数で ∫ exp(A τ) B dτ を同時評価 -
乱数生成
w_k = S_Q ξ_k, v_k = S_R η_k（ξ, η は標準正規）
数値上の半正定性崩れを避けるため微小jitterを加えてCholesky -
モデル置換
A, B, C, Q, R, x0_mean, P0 を入れ替えるだけで任意のLTIを再現
第6回 カルマンフィルタ（理論導入）
【1】確率空間とL²空間
確率空間 (Ω, F, P)
Ω : 標本空間
F : σ加法族
P : 確率測度（P(Ω)=1）
L²(Ω, F, P) = { Z : E[Z²] < ∞ }
内積: = E[Z1 * Z2]
ノルム: ||Z|| = sqrt(E[Z²])
条件付き期待値:
E[Z | G] = L²(G) 上の直交射影
最小二乗性: E[(Z - E[Z|G])²] = min_{Z'∈L²(G)} E[(Z - Z')²]
意味:
確率変数を信号とみなすと、L²空間は有限エネルギー信号空間。
条件付き期待値は観測情報Gで得られる最良推定。
【2】フィルトレーションと適合過程
フィルトレーション:
F_t は時刻tまでの情報
s < t のとき F_s ⊂ F_t
標準フィルトレーション:
F_t = ∩_{ε>0} σ(F_{t+ε} ∪ null sets)
適合過程:
X_t が F_t 可測であるとき「適合」と呼ぶ。
意味：未来の情報を含まない観測可能な過程。
【3】マルチンゲール
条件:
E[|M_t|] < ∞
E[M_t | F_s] = M_s （すべての s < t）
意味:
将来の平均が現在の値と等しい過程（予測不能信号）
サブマルチンゲール:
E[M_t | F_s] ≥ M_s
スーパーマルチンゲール:
E[M_t | F_s] ≤ M_s
例:
ブラウン運動 V_t はマルチンゲール。
V_t² - t もマルチンゲール。
定理:
Doob収束定理: L¹有界な M_t は M_t → M_∞ に収束。
マルチンゲール表現定理: M_t = M_0 + ∫₀ᵗ φ_s dV_s
【4】ブラウン運動（Wiener過程）
定義:
V_0 = 0
E[V_t] = 0
Var(V_t) = t
V_t - V_s ~ N(0, t - s)
E[V_t * V_s] = min(t, s)
性質:
連続パスをもち、独立増分を持つ。
形式的に dV_t/dt = ξ_t と書くと、ξ_t はホワイトノイズ。
【5】Itô積分と等距同型
Itô積分:
I_T = ∫₀ᵀ φ_t dV_t
条件:
E[∫₀ᵀ φ_t² dt] < ∞
Itô等距:
E[I_T²] = E[∫₀ᵀ φ_t² dt]
意味:
ノイズの入力エネルギーと積分の出力エネルギーが等しい（エネルギー保存）。
【6】Itôの補題
確率微分方程式:
dX_t = b(X_t) dt + σ(X_t) dV_t
関数 f(X_t) に対して:
df(X_t) = f'(X_t) dX_t + 1/2 * f''(X_t) * (dX_t)²
(dV_t)² = dt より:
df = [ f'(X_t) * b(X_t) + 1/2 * f''(X_t) * σ(X_t)² ] dt + f'(X_t) * σ(X_t) dV_t
【7】条件付き期待値による推定
π_t(φ) = E[φ(X_t) | Y_t]
E[(φ - π_t)²] ≤ E[(φ - ẑ)²] （任意のẑに対して）
意味:
π_t は観測Y_tに基づく最良推定（平均二乗誤差最小）。
【8】状態空間モデル（連続時間）
状態方程式:
dX_t = A X_t dt + G dV_t
観測方程式:
dY_t = C X_t dt + dW_t
ノイズ共分散:
E[dV dVᵀ] = Q dt
E[dW dWᵀ] = R dt
E[dV dWᵀ] = 0
意味:
X_t は状態（内部変数）
Y_t は観測値
V_t は外乱（process noise）
W_t は測定ノイズ（measurement noise）
【9】カルマン–ブシィフィルタ（連続時間）
推定値:
x̂_t = E[X_t | Y_t]
誤差共分散:
P_t = E[(X_t - x̂_t)(X_t - x̂_t)ᵀ]
イノベーション:
dν_t = dY_t - C x̂_t dt
カルマン方程式:
dx̂_t = A x̂_t dt + K_t dν_t
K_t = P_t Cᵀ R⁻¹
dP_t/dt = A P_t + P_t Aᵀ + G Q Gᵀ - P_t Cᵀ R⁻¹ C P_t
【10】定常状態（代数リカッチ方程式）
定常時（dP_t/dt = 0）:
A P + P Aᵀ + G Q Gᵀ - P Cᵀ R⁻¹ C P = 0
条件:
A - K∞C が安定なら、P は有限で収束。
【11】離散カルマンフィルタ
予測ステップ:
x̂⁻ = A x̂⁺
P⁻ = A P⁺ Aᵀ + G Q Gᵀ
更新ステップ:
K = P⁻ Cᵀ (C P⁻ Cᵀ + R)⁻¹
x̂⁺ = x̂⁻ + K (y - C x̂⁻)
P⁺ = (I - K C) P⁻
【12】数値実装上の注意
離散化:
Q_d = ∫₀^{Δt} e^{Aτ} G Q Gᵀ e^{Aᵀτ} dτ
正定性保持:
P⁺ = (I - K C) P⁻ (I - K C)ᵀ + K R Kᵀ
数値安定化:
P = S Sᵀ としてスクエアルート法で更新
観測欠損時:
更新ステップをスキップ
相関ノイズ:
拡大状態ベクトルで処理
【13】物理的意味
V_t : システム外乱（プロセスノイズ）
W_t : 観測ノイズ
x̂_t : 状態推定（条件付き平均）
P_t : 推定誤差共分散
K_t : カルマンゲイン（信号対ノイズ比）
Q : モデル不確実性の大きさ
R : センサノイズの大きさ
パラメータの傾向:
Q が大きい → モデルを信頼できない → 観測重視（K↑）
R が大きい → センサがノイジー → モデル重視（K↓）
第7回 カルマンフィルタ（Python実装）
- 1次系モデルでKF実装
- ノイズ強度Q,Rの調整と挙動比較
- 出力 vs 推定値プロット
第8回 連続時間の確率過程とSDE
-
確率過程 X(t)
-
ブラウン運動とホワイトノイズ
-
伊藤積分
∫σ(X_t)dV_t -
確率微分方程式
dX_t = f(X_t)dt + σ(X_t)dV_t -
Euler–Maruyama法の数値例
第9回 確率フィルタリング理論（Zakai, Kushner–Stratonovich）
- 条件付き期待値 π_t(φ) = E[φ(X_t)|Y_t]
- Zakai方程式（非正規化形）
- Kushner–Stratonovich方程式（正規化形）
- 線形・ガウス系ではKFと一致することの証明
第10回 拡張カルマンフィルタ（EKF）
-
非線形系の線形近似
ẋ = f(x,u) + w y = h(x) + v -
ヤコビ行列による線形化
-
EKFアルゴリズム（予測・更新）
-
Python実装：非線形システムの推定
第11回 粒子フィルタ（PF）
- サンプルベースの近似
- リサンプリング手法（systematic / stratified）
- Python実装：粒子群による状態推定
- KF/EKFとの比較
第12回 最適制御理論（LQR）
-
コスト関数
J = ∫ (xᵀQx + uᵀRu) dt -
リカッチ方程式の導出
-
最適ゲインK = R⁻¹BᵀP
-
PythonでLQR制御を設計
第13回 LQG制御（LQR + Kalman Filter）
-
現代制御の最終形
-
ノイズ環境下の最適制御
u = -K x̂ x̂ ← KFにより推定 -
分離定理：推定と制御の独立性
-
PythonでLQG制御シミュレーション
第14回 拡張応用（非線形・実時間制御）
- EKF制御、UKF制御
- モデル予測制御（MPC）との比較
- 非線形車両モデルでの実装例
- 数値安定性と実装注意点
第15回 総合演習：確率制御システムの設計
- RC回路、質量–ばね、ドローン姿勢のいずれかを選択
- PythonでKF→LQR→LQGを実装し比較
- 理論→数式→制御信号→グラフの一貫理解
- 発表・レポート課題
学習成果
| 到達段階 | 理解・実践内容 |
|---|---|
| 概念理解 | 確率と制御の関係を説明できる |
| 数式理解 | 状態方程式・KF・SDEを展開できる |
| 実装力 | PythonでKF/EKF/LQGを動作可能 |
| 応用力 | ノイズを含む制御システム設計が可能 |
このまま 「8週間集中スケジュール（理論＋Python課題付き）」 にも再編可能。
どちらを希望しますか？（①15回大学講義形式のまま、②8週間集中講座形式）