制御工学で最も重要な課題の一つは「システムの状態を知ること」です。しかし現実のセンサは完璧ではありません。GPSの位置情報はノイズだらけで、車の速度計にも誤差があります。しかし、カーナビはGPSと速度計の両方の情報を賢く組み合わせて、滑らかで正確な位置推定を実現しています。この「ノイズまみれの情報から最も確からしい状態を推定する」技術がカルマンフィルタ（Kalman filter）です。
カルマンフィルタの核心的なアイデアは、身近な場面でも見ることができます。たとえば、天気予報を考えてみましょう。明日の天気を予測するとき、2つの情報源があります。1つは気象モデルによるシミュレーション結果（予測）、もう1つは最新の観測データ（気温、気圧、衛星画像など）です。どちらにも誤差がありますが、両方を賢く組み合わせることで、どちらか一方だけを使うよりも正確な予報が可能になります。カルマンフィルタはまさにこの「モデルの予測」と「観測データ」を最適に統合する数学的枠組みです。
前の記事で学んだルエンバーガオブザーバは、ノイズのない理想的な状況を前提としていました。カルマンフィルタは、プロセスノイズ（モデルの不確実性）と観測ノイズ（センサの誤差）を明示的に考慮し、推定誤差の分散を最小化する最適なオブザーバゲインを自動的に計算します。
カルマンフィルタは1960年にルドルフ・E・カルマン（Rudolf E. Kalman）によって発表されました。その後すぐにNASAのアポロ計画に採用され、月への航行に不可欠な技術となりました。地球から月までの約38万kmの旅路で、限られたセンサ情報から宇宙船の位置と速度を正確に推定し続けたのがカルマンフィルタです。以来60年以上にわたり、カルマンフィルタは航法、制御、信号処理の分野で最も広く使われるアルゴリズムの一つであり続けています。
カルマンフィルタを理解すると、以下のような応用が開けます。
- 航法・測位: GPS/INS統合航法、ロケットの誘導制御、衛星軌道決定
- ロボティクス: 自律走行車の自己位置推定（SLAM）、ドローンの姿勢推定
- 金融工学: 隠れた市場状態の推定、時系列の状態空間モデリング
- LQG制御: カルマンフィルタとLQR制御を組み合わせた最適制御
- 画像処理: 物体追跡、映像の手ブレ補正
本記事の内容
- 確率的な状態空間モデルの構成
- カルマンフィルタの導出と直感的理解（予測と更新のサイクル）
- カルマンゲインの最適性の数学的根拠
- 定常カルマンフィルタとリカッチ方程式
- 制御応用としてのLQG制御への接続
- 具体的な数値計算例
- Pythonでの実装と検証
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
確率的な状態空間モデル
ノイズを含むモデル
決定論的な状態空間モデル $\bm{x}_{k+1} = \bm{A}\bm{x}_k + \bm{B}\bm{u}_k$ では、システムの挙動が完全にモデルで記述できると仮定していました。しかし現実のシステムでは、モデルの不確実性（風乱、摩擦変動、モデル化誤差など）とセンサノイズが必ず存在します。
たとえば、衛星の軌道を追跡する場面を考えましょう。軌道力学の方程式でおおよその位置は予測できますが、太陽輻射圧、大気の抵抗（低軌道の場合）、重力場の高次項など、モデルに含まれていない微小な力が常に作用しています。これがプロセスノイズです。一方、レーダーで衛星の距離を測定するとき、電波の伝搬遅延のゆらぎや受信機の熱雑音が測定値に誤差を与えます。これが観測ノイズです。
これらを明示的に含んだ離散時間モデルは次のように書けます。
$$ \begin{align} \bm{x}_{k+1} &= \bm{A}\bm{x}_k + \bm{B}\bm{u}_k + \bm{w}_k \\ \bm{y}_k &= \bm{C}\bm{x}_k + \bm{v}_k \end{align} $$
ここで各記号の意味を整理します。
- $\bm{x}_k \in \mathbb{R}^n$: 時刻 $k$ における状態ベクトル（推定したい量）
- $\bm{u}_k \in \mathbb{R}^m$: 制御入力ベクトル（既知）
- $\bm{y}_k \in \mathbb{R}^p$: 観測ベクトル（センサで得られる量）
- $\bm{w}_k \sim \mathcal{N}(\bm{0}, \bm{Q})$: プロセスノイズ（白色ガウスノイズ）
- $\bm{v}_k \sim \mathcal{N}(\bm{0}, \bm{R})$: 観測ノイズ（白色ガウスノイズ）
$\bm{Q}$ と $\bm{R}$ はそれぞれの共分散行列で、ノイズの大きさと相関構造を表します。カルマンフィルタの重要な仮定として、$\bm{w}_k$ と $\bm{v}_k$ は互いに無相関であり、すべての時刻間でも無相関（白色ノイズ）であることを要求します。
なぜガウスノイズを仮定するのでしょうか。これには2つの理由があります。第一に、中心極限定理により、多数の微小な独立な誤差の和はガウス分布に収束するため、多くの実システムでガウスノイズの仮定は妥当です。第二に、ガウス分布は平均と共分散で完全に特徴づけられるため、推定問題が大幅に簡単になります。
状態空間モデルが定まったところで、次にこのモデルのもとで最適な状態推定を行うカルマンフィルタのアルゴリズムを導出します。
カルマンフィルタのアルゴリズム
カルマンフィルタは「予測」と「更新」の2ステップを繰り返します。この2ステップの構造は、先ほどの天気予報のアナロジーと対応しています。予測ステップは「気象モデルによる明日の天気のシミュレーション」、更新ステップは「最新の観測データによる補正」に相当します。
予測ステップ（時間更新）
まず、時刻 $k-1$ までの情報に基づいて、時刻 $k$ の状態を予測します。
状態の予測: $$ \hat{\bm{x}}_{k|k-1} = \bm{A}\hat{\bm{x}}_{k-1|k-1} + \bm{B}\bm{u}_{k-1} $$
ここで $\hat{\bm{x}}_{k|k-1}$ は「時刻 $k-1$ までの情報に基づく時刻 $k$ の状態の予測」を意味します。この予測はモデル方程式をそのまま使っていますが、プロセスノイズ $\bm{w}_{k-1}$ は未知なので含まれていません。
誤差共分散の予測: $$ \bm{P}_{k|k-1} = \bm{A}\bm{P}_{k-1|k-1}\bm{A}^T + \bm{Q} $$
この式の物理的意味を考えましょう。第1項 $\bm{A}\bm{P}_{k-1|k-1}\bm{A}^T$ は、前の時刻の推定誤差がシステムダイナミクスを通じてどのように伝播するかを表しています。第2項 $\bm{Q}$ はプロセスノイズにより新たに追加される不確実性です。つまり、予測ステップでは不確実性は常に増大します。観測データなしにはエントロピーは増えるだけなのです。
更新ステップ（観測更新）
次に、時刻 $k$ の観測 $\bm{y}_k$ を用いて予測を修正します。
カルマンゲイン: $$ \begin{equation} \bm{K}_k = \bm{P}_{k|k-1}\bm{C}^T(\bm{C}\bm{P}_{k|k-1}\bm{C}^T + \bm{R})^{-1} \end{equation} $$
状態の更新: $$ \hat{\bm{x}}_{k|k} = \hat{\bm{x}}_{k|k-1} + \bm{K}_k(\bm{y}_k – \bm{C}\hat{\bm{x}}_{k|k-1}) $$
誤差共分散の更新: $$ \bm{P}_{k|k} = (\bm{I} – \bm{K}_k\bm{C})\bm{P}_{k|k-1} $$
更新ステップの核心は、イノベーション（innovation）$\bm{e}_k = \bm{y}_k – \bm{C}\hat{\bm{x}}_{k|k-1}$ にあります。これは「予測された観測と実際の観測の差」であり、「新しい情報」を表します。もしモデルの予測が完璧であれば、イノベーションは観測ノイズのみを含みます。イノベーションが大きければ、予測が外れたことを意味し、カルマンゲインに応じて推定値を修正します。
カルマンゲインの直感的理解
$\bm{K}_k$ はカルマンゲインで、観測情報をどの程度信頼するかを最適に調整します。カルマンゲインの振る舞いを2つの極端なケースで考えてみましょう。
ケース1: $\bm{R} \to \bm{0}$（観測が完全に正確なとき）
$$ \bm{K}_k \to \bm{P}_{k|k-1}\bm{C}^T(\bm{C}\bm{P}_{k|k-1}\bm{C}^T)^{-1} $$
このとき $\hat{\bm{x}}_{k|k}$ は観測と完全に一致するように更新されます。観測が正確なら、観測をそのまま信じるのが最適だということです。
ケース2: $\bm{P}_{k|k-1} \to \bm{0}$（予測が完全に正確なとき）
$$ \bm{K}_k \to \bm{0} $$
このとき $\hat{\bm{x}}_{k|k} = \hat{\bm{x}}_{k|k-1}$ となり、観測は無視されます。予測が正確なら、わざわざノイズの乗った観測で修正する必要はないということです。
一般的には、$\bm{R}$ が小さい（観測が正確）ときはカルマンゲインが大きくなり観測を重視し、$\bm{Q}$ が小さい（モデルが正確）ときはカルマンゲインが小さくなりモデルの予測を重視します。
カルマンゲインの最適性
カルマンゲインが「最適」であることを数学的に示しましょう。推定誤差を $\tilde{\bm{x}}_k = \bm{x}_k – \hat{\bm{x}}_{k|k}$ と定義し、任意のゲイン $\bm{K}$ を使った更新式
$$ \hat{\bm{x}}_{k|k} = \hat{\bm{x}}_{k|k-1} + \bm{K}(\bm{y}_k – \bm{C}\hat{\bm{x}}_{k|k-1}) $$
を考えます。更新後の誤差共分散 $\bm{P}_{k|k} = E[\tilde{\bm{x}}_k \tilde{\bm{x}}_k^T]$ を計算すると、
$$ \bm{P}_{k|k} = (\bm{I} – \bm{K}\bm{C})\bm{P}_{k|k-1}(\bm{I} – \bm{K}\bm{C})^T + \bm{K}\bm{R}\bm{K}^T $$
これを展開して $\bm{K}$ について整理すると、
$$ \bm{P}_{k|k} = \bm{P}_{k|k-1} – \bm{K}\bm{C}\bm{P}_{k|k-1} – \bm{P}_{k|k-1}\bm{C}^T\bm{K}^T + \bm{K}(\bm{C}\bm{P}_{k|k-1}\bm{C}^T + \bm{R})\bm{K}^T $$
$\bm{S}_k = \bm{C}\bm{P}_{k|k-1}\bm{C}^T + \bm{R}$（イノベーション共分散）と定義し、$\text{tr}(\bm{P}_{k|k})$ を $\bm{K}$ で偏微分してゼロとおくと、
$$ -\bm{P}_{k|k-1}\bm{C}^T + \bm{K}\bm{S}_k = \bm{0} $$
これを解いて $\bm{K}_k = \bm{P}_{k|k-1}\bm{C}^T\bm{S}_k^{-1}$ が得られます。これがまさにカルマンゲインの式であり、推定誤差のトレース（各成分の分散の和）を最小にするゲインが自動的に決まることが確認できました。
定常カルマンフィルタ
時間が十分経つと $\bm{P}_{k|k-1}$ は定常値 $\bm{P}_\infty$ に収束し、カルマンゲインも定常値 $\bm{K}_\infty$ に収束します。この収束は、システムが可観測かつ可制御であり、$\bm{Q}$ が半正定値、$\bm{R}$ が正定値であれば保証されます。
定常状態の誤差共分散 $\bm{P}_\infty$ は離散代数リカッチ方程式（DARE）の解です。
$$ \bm{P}_\infty = \bm{A}\bm{P}_\infty\bm{A}^T – \bm{A}\bm{P}_\infty\bm{C}^T(\bm{C}\bm{P}_\infty\bm{C}^T + \bm{R})^{-1}\bm{C}\bm{P}_\infty\bm{A}^T + \bm{Q} $$
定常カルマンフィルタはルエンバーガオブザーバの特殊な場合であり、ゲイン $\bm{L} = \bm{K}_\infty$ がノイズ特性に対して最適になっています。ルエンバーガオブザーバではゲインを設計者が手動で極配置法などにより決定しましたが、カルマンフィルタではノイズの統計的性質（$\bm{Q}$ と $\bm{R}$）に基づいて自動的に最適なゲインが計算されます。
カルマンフィルタとベイズ推定の関係
カルマンフィルタは、ベイズ推定のフレームワークの中に自然に位置づけることができます。この視点はカルマンフィルタの深い理解につながります。
ベイズの定理は次のように書けます。
$$ p(\bm{x}_k | \bm{y}_{1:k}) = \frac{p(\bm{y}_k | \bm{x}_k) \, p(\bm{x}_k | \bm{y}_{1:k-1})}{p(\bm{y}_k | \bm{y}_{1:k-1})} $$
左辺の $p(\bm{x}_k | \bm{y}_{1:k})$ は事後分布（全観測データを考慮した状態の確率分布）です。右辺の $p(\bm{x}_k | \bm{y}_{1:k-1})$ は事前分布（予測ステップの結果）、$p(\bm{y}_k | \bm{x}_k)$ は尤度（観測モデル）です。
カルマンフィルタが美しいのは、すべての分布がガウス分布であるため、事後分布もガウス分布になり、その平均と共分散を閉じた形で更新できるという点です。予測ステップは事前分布の計算に対応し、更新ステップはベイズの定理による事前分布から事後分布への変換に対応しています。
非線形システムや非ガウスノイズの場合、この美しい性質は失われます。そのような場合には、拡張カルマンフィルタ（EKF）、無香カルマンフィルタ（UKF）、パーティクルフィルタなどの拡張手法が用いられます。
理論的な基盤が整ったところで、次にPythonで実装して動作を確認してみましょう。
Pythonでの実装と検証
import numpy as np
import matplotlib.pyplot as plt
np.random.seed(42)
# 離散時間システム
dt = 0.1
A = np.array([[1, dt], [0, 1]]) # 等速直線運動
B = np.array([[0.5*dt**2], [dt]])
C = np.array([[1, 0]]) # 位置のみ観測
Q = np.array([[0.01, 0], [0, 0.1]]) # プロセスノイズ
R = np.array([[1.0]]) # 観測ノイズ
n_steps = 200
x_true = np.zeros((2, n_steps))
x_est = np.zeros((2, n_steps))
y_obs = np.zeros((1, n_steps))
P = np.eye(2) * 10 # 初期推定誤差共分散
x_true[:, 0] = [0, 1] # 初期位置0、速度1
x_est[:, 0] = [2, 0] # 推定の初期値（誤差あり）
kalman_gains = []
p_traces = []
for k in range(1, n_steps):
# 真の状態の更新
u = np.array([[0.1 * np.sin(0.05 * k)]])
w = np.random.multivariate_normal([0, 0], Q)
x_true[:, k] = A @ x_true[:, k-1] + (B @ u).flatten() + w
# 観測
v = np.random.normal(0, np.sqrt(R[0, 0]))
y_obs[:, k] = C @ x_true[:, k] + v
# カルマンフィルタ: 予測
x_pred = A @ x_est[:, k-1] + (B @ u).flatten()
P_pred = A @ P @ A.T + Q
# カルマンフィルタ: 更新
S = C @ P_pred @ C.T + R
K_gain = P_pred @ C.T @ np.linalg.inv(S)
innovation = y_obs[:, k] - C @ x_pred
x_est[:, k] = x_pred + (K_gain @ innovation).flatten()
P = (np.eye(2) - K_gain @ C) @ P_pred
kalman_gains.append(K_gain.flatten())
p_traces.append(np.trace(P))
t = np.arange(n_steps) * dt
fig, axes = plt.subplots(2, 2, figsize=(14, 9))
# 位置の推定
ax = axes[0, 0]
ax.plot(t, x_true[0], linewidth=2, color="steelblue", label="True position")
ax.scatter(t, y_obs[0], s=5, color="gray", alpha=0.3, label="Noisy obs.")
ax.plot(t, x_est[0], linewidth=2, color="red", linestyle="--",
label="KF estimate")
ax.set_xlabel("Time [s]", fontsize=11)
ax.set_ylabel("Position", fontsize=11)
ax.set_title("Position Estimation", fontsize=12)
ax.legend(fontsize=9)
ax.grid(True, alpha=0.3)
# 速度の推定
ax = axes[0, 1]
ax.plot(t, x_true[1], linewidth=2, color="steelblue", label="True velocity")
ax.plot(t, x_est[1], linewidth=2, color="red", linestyle="--",
label="KF estimate")
ax.set_xlabel("Time [s]", fontsize=11)
ax.set_ylabel("Velocity", fontsize=11)
ax.set_title("Velocity Estimation (unobserved)", fontsize=12)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
# 推定誤差
ax = axes[1, 0]
err_pos = x_true[0] - x_est[0]
err_vel = x_true[1] - x_est[1]
ax.plot(t, err_pos, linewidth=1.5, label="Position error")
ax.plot(t, err_vel, linewidth=1.5, label="Velocity error")
ax.axhline(0, color="black", linestyle="--", alpha=0.4)
ax.set_xlabel("Time [s]", fontsize=11)
ax.set_ylabel("Error", fontsize=11)
ax.set_title("Estimation Error", fontsize=12)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
# カルマンゲインの収束
ax = axes[1, 1]
kg = np.array(kalman_gains)
ax.plot(t[1:], kg[:, 0], linewidth=2, label="$K_1$ (position gain)")
ax.plot(t[1:], kg[:, 1], linewidth=2, label="$K_2$ (velocity gain)")
ax.set_xlabel("Time [s]", fontsize=11)
ax.set_ylabel("Kalman Gain", fontsize=11)
ax.set_title("Kalman Gain Convergence", fontsize=12)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig("kalman_filter.png", dpi=150, bbox_inches="tight")
plt.show()
この結果から、カルマンフィルタの推定性能が確認できます。
-
位置推定（左上）: ノイズの多い観測データ（灰色の点、$R = 1.0$ の標準偏差で散らばっている）にもかかわらず、カルマンフィルタの推定（赤い破線）が真の位置（青い実線）に滑らかに追従しています。初期推定値を真の値から $2$ だけずらして設定しましたが（真値 $x_1(0) = 0$、推定初期値 $\hat{x}_1(0) = 2$）、約2秒で初期誤差が修正されています。これはカルマンフィルタの初期値に対する頑健性を示しています
-
速度推定（右上）: 速度は直接観測されていません（$\bm{C} = [1, 0]$ なので位置のみ観測）にもかかわらず、カルマンフィルタが正確に推定できています。これは可観測性条件が満たされているためです。位置の時系列変化から速度の情報を抽出できるのは、システムモデル $\bm{A}$ の中に位置と速度の関係（$x_1(k+1) = x_1(k) + dt \cdot x_2(k)$）が組み込まれているからです
-
推定誤差（左下）: 初期の大きな誤差（位置誤差が約2、速度誤差が約1）が約2秒で急速に収束し、その後は小さなノイズ起因の変動のみが残っています。定常状態の推定誤差の大きさは $\bm{P}_\infty$ の対角成分の平方根程度であり、これはプロセスノイズと観測ノイズのバランスによって決まります
-
カルマンゲイン（右下）: カルマンゲインが約1秒で定常値に収束する様子が見えます。初期は推定の不確実性が大きい（$\bm{P}_0 = 10\bm{I}$）ため、大きなゲインで観測を強く信頼します。収束後は適切なバランスに落ち着き、定常カルマンフィルタとして動作しています。$K_1$（位置ゲイン）が $K_2$（速度ゲイン）より大きいのは、位置が直接観測されているためです
-
全体的な考察: カルマンフィルタの真価は「直接観測できない状態変数の推定」にあります。このシミュレーションでは速度がその例です。モデルの力学的関係と観測を最適に統合することで、センサの数以上の情報を引き出しているのです
次に、具体的な数値例を通じてカルマンフィルタのパラメータ設計を深く理解しましょう。
数値計算例
例1：1次元位置推定でのカルマンゲインの手計算
最も簡単なケースとして、1次元の位置推定を手計算で追跡してみましょう。
システム: $x_{k+1} = x_k + w_k$（ランダムウォーク）、$y_k = x_k + v_k$
パラメータ: $A = 1$、$C = 1$、$Q = 0.1$、$R = 1.0$、初期値 $\hat{x}_0 = 0$、$P_0 = 10$
ステップ1（$k=1$）の予測:
$$ \hat{x}_{1|0} = A \hat{x}_{0|0} = 1 \times 0 = 0 $$
$$ P_{1|0} = A P_0 A^T + Q = 1 \times 10 \times 1 + 0.1 = 10.1 $$
ステップ1の更新: 観測 $y_1 = 3.5$ が得られたとします。
$$ K_1 = \frac{P_{1|0} C^T}{C P_{1|0} C^T + R} = \frac{10.1}{10.1 + 1.0} = 0.910 $$
$$ \hat{x}_{1|1} = \hat{x}_{1|0} + K_1(y_1 – C\hat{x}_{1|0}) = 0 + 0.910 \times (3.5 – 0) = 3.185 $$
$$ P_{1|1} = (1 – K_1 C)P_{1|0} = (1 – 0.910) \times 10.1 = 0.909 $$
初期の不確実性が大きい（$P_0 = 10$）ため、カルマンゲインは $0.910$ と非常に大きく、ほぼ観測値をそのまま採用しています。
ステップ2（$k=2$）の予測:
$$ P_{2|1} = 0.909 + 0.1 = 1.009 $$
ステップ2の更新: 観測 $y_2 = 3.2$ が得られたとします。
$$ K_2 = \frac{1.009}{1.009 + 1.0} = 0.502 $$
2ステップ目では $P$ が小さくなったため、カルマンゲインも $0.502$ に減少し、観測とモデル予測を約半々で組み合わせるようになっています。
定常状態では $P_\infty = A P_\infty A + Q – \frac{(A P_\infty C)^2}{C P_\infty C + R}$ を解いて $P_\infty \approx 0.270$、$K_\infty = 0.213$ が得られます。
例2：$\bm{Q}$ と $\bm{R}$ の比が推定に与える影響
プロセスノイズ共分散 $Q$ と観測ノイズ共分散 $R$ の比率が、カルマンフィルタの特性を決定します。同じ1次元系で比較してみましょう。
ケースA: $Q = 0.01$、$R = 1.0$（$Q/R = 0.01$、モデルが正確）
定常ゲイン $K_\infty \approx 0.067$ と非常に小さくなります。フィルタはモデルの予測を強く信頼し、観測はわずかな修正にしか使いません。推定値の軌跡は非常に滑らかですが、真の状態の急変に追従しにくくなります。
ケースB: $Q = 1.0$、$R = 1.0$（$Q/R = 1.0$、バランス型）
$K_\infty \approx 0.618$ となります。黄金比に近い値になるのは偶然ですが、モデル予測と観測をバランスよく組み合わせる設定です。
ケースC: $Q = 10$、$R = 1.0$（$Q/R = 10$、モデルが不正確）
$K_\infty \approx 0.916$ と非常に大きくなります。フィルタはモデル予測をほとんど信頼せず、観測値にほぼそのまま追従します。応答は速いですが、観測ノイズがそのまま推定値に乗ってしまいます。
このように、$Q/R$ の比はフィルタの「帯域幅」を決めるパラメータであり、信号処理におけるローパスフィルタのカットオフ周波数に対応する役割を果たしています。
例3：可観測性とカルマンフィルタの収束
カルマンフィルタが正しく機能するためには、システムが可観測（observable）である必要があります。可観測性行列は次のように定義されます。
$$ \bm{O} = \begin{bmatrix} \bm{C} \\ \bm{C}\bm{A} \\ \vdots \\ \bm{C}\bm{A}^{n-1} \end{bmatrix} $$
この行列のランクが $n$（状態次元）に等しいとき、システムは可観測です。
上のシミュレーションの例では、$\bm{A} = \begin{bmatrix} 1 & 0.1 \\ 0 & 1 \end{bmatrix}$、$\bm{C} = [1, 0]$ であるから、
$$ \bm{O} = \begin{bmatrix} 1 & 0 \\ 1 & 0.1 \end{bmatrix} $$
$\det(\bm{O}) = 0.1 \neq 0$ なので、ランクは2であり可観測です。位置の観測から速度を推定できることが、代数的にも確認できました。
もし $\bm{C} = [0, 0]$（何も観測しない）であれば $\bm{O}$ はゼロ行列となり、可観測ではありません。この場合、カルマンフィルタの誤差共分散 $\bm{P}_k$ は発散し、定常解は存在しません。
例4：LQG制御への接続
カルマンフィルタはLQG制御の構成要素として重要な役割を果たします。LQG制御では、カルマンフィルタの推定値 $\hat{\bm{x}}_{k|k}$ をLQRコントローラへの入力として使います。
$$ \bm{u}_k = -\bm{K}_{\text{LQR}} \hat{\bm{x}}_{k|k} $$
分離定理により、カルマンフィルタのゲイン $\bm{K}_{\text{KF}}$ とLQRのゲイン $\bm{K}_{\text{LQR}}$ は独立に設計できます。カルマンフィルタの設計は $(\bm{A}, \bm{C}, \bm{Q}, \bm{R})$ のみで決まり、LQRの重み行列 $(\bm{Q}_{\text{lqr}}, \bm{R}_{\text{lqr}})$ とは無関係です。この独立性がLQG制御の実用上の大きな利点であり、制御器の設計と推定器の設計をそれぞれの専門家が別々に行うことができます。
まとめ
本記事では、カルマンフィルタの理論的基盤から実装、制御応用への接続まで包括的に解説しました。要点を振り返ります。
- カルマンフィルタは、プロセスノイズ $\bm{Q}$ と観測ノイズ $\bm{R}$ を明示的に考慮した最適状態推定器であり、推定誤差の分散を最小化するゲインを自動的に計算します
- アルゴリズムは予測ステップ（モデルに基づく状態と誤差共分散の外挿）と更新ステップ（観測による修正）の2段階で構成されます。予測で不確実性が増大し、更新で不確実性が減少するというサイクルが繰り返されます
- カルマンゲイン $\bm{K}_k$ は、推定誤差のトレースを最小化する最適なゲインとして導出されます。$\bm{Q}/\bm{R}$ の比がフィルタの帯域幅を決め、モデルと観測の信頼度を自動的にバランスします
- カルマンフィルタはベイズ推定のガウス版であり、予測ステップが事前分布の計算、更新ステップがベイズの定理による事後分布の計算に対応しています
- 定常カルマンフィルタはルエンバーガオブザーバの最適化版であり、離散代数リカッチ方程式（DARE）の解からゲインが決定されます
- LQG制御の構成要素として、カルマンフィルタの推定値をLQR制御器に入力することで、ノイズ環境下での最適制御が実現されます
次のステップとして、以下の記事も参考にしてください。
- LQR制御の理論と実装 — 最適制御器
- LQG制御 — LQRとカルマンフィルタの統合
- オブザーバの設計 — 決定論的オブザーバ