はじめに
こんにちは！組み込みとPLCの間を行ったり来たりしている境界限界プログラマのuniuniです。
早速ですがみなさまはカルマンフィルタ使っているでしょうか？
数式が難しく敷居の高さを感じている方も多いと思いますが、使ってみるとこれがなかなか便利なフィルタです。
とりあえず動かす。を目標に実践解説をしてみます。
今回の実践テーマ
今回は位置を測定し、真の位置・速度を推定するプログラムを実装します。
工場で使われているセンサは1次元の値を検出するものが多いですよね。
例えば距離m、圧力pa、温度℃、重量gなどです。
これらを位置信号として取り扱い、真の位置と速度（m/sec、pa/sec、℃/sec、g/sec）をカルマンフィルタで推定します。
単位はお好みのセンサに応じて読み替えてください。
カルマンフィルタの計算
カルマンフィルタといえば以下の式ですよね。
\hat{\mathbf{x}}_k^- = \mathbf{F}_k \hat{\mathbf{x}}_{k-1} + \mathbf{B}_k \mathbf{u}_k
\mathbf{P}_k^- = \mathbf{F}_k \mathbf{P}_{k-1} \mathbf{F}_k^\top + \mathbf{Q}_k
\mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^\top \left( \mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^\top + \mathbf{R}_k \right)^{-1}
\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k \left( \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_k^- \right)
\mathbf{P}_k = \left( \mathbf{I} - \mathbf{K}_k \mathbf{H}_k \right) \mathbf{P}_k^-
ああっ、ブラウザバックはちょっとお待ち下さい！
pythonで書けば以下のようになります。
x = F @ x
P = F @ P @ F.T + Q
y = z - H @ x
S = H @ P @ H.T + R
K = P @ H.T @ np.linalg.inv(S)
x = x + K @ y
I = np.eye(P.shape[0])
P = (I - K @ H) @ P
いや、pythonで書き直されたところでどちらにせよわからない？いやまったくその通りです。
この式・計算を見て「なるほどー」と思わえる人はごく僅かだと思います。
私もぽかーんとする側の人間です。
このカルマンフィルタの式は重要ですが、実際にフィルタリングするにあたって実は完全に理解する必要はありません。
（と言うとあらゆる方面から怒られそうなので、理解するに越したことはない。と付け加えておきます）
むしろ実用においては状態方程式などの設計の方が重要です。
ではどの部分を設計するのでしょうか？
ｚは観測値なのでセンサから取り込むだけです。
S,K,x,yについてもカルマンフィルタ内で計算される値です。
なので残りの状態遷移行列F、観測行列H、プロセス共分散Q、観測雑音共分散Rを設計し、カルマンフィルタに与えてあげる必要があります。
これらは要素数に応じた行列で定義されます。
移動平均であれば窓数と平均数、ローパスフィルタであれば時定数のみの決めてあげれば良かったですが、カルマンフィルタでは4パラメータ、しかも行列で設計する必要があります。
このあたりがカルマンフィルタの取っつき辛さの原因だと思います。
今回の記事では位置・速度のみに絞ってガッツリ簡略化したカルマンフィルタを実践します。
カルマンフィルタの設計
ようやく本題に入りました。さきほどのアバンでの説明の通り今回はシンプルに位置、速度の2要素とします。
位置を観測して、位置と速度の状態を推定します。
状態ベクトルx
ベクトルと聞いただけで身構えてしまいますが、なんということはありません。
予測する状態...、今回は位置・速度の２要素をベクトルで表現したものです。
\mathbf{x} =
\begin{bmatrix}
\text{位置} \\
\text{速度}
\end{bmatrix}
状態遷移行列F
そのまま状態がどのように遷移するか定める行列です。たとえば物理法則なんかを行列で表現します。
状態遷移行列Fを設計しましょ...ということで設計しました。
\mathbf{F} =
\begin{bmatrix}
1 & \Delta t \\
0 & 1
\end{bmatrix}
ではなぜこうなるのでしょうか？
順番が前後してしまったのですが、状態方程式の理解が必要です。
ということで状態方程式です。
\mathbf{x}_k = \mathbf{F}_k \mathbf{x}_{k-1} + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_k
Fはさきほど設計した状態遷移行列、xは状態ベクトル、Bは制御入力行列、uは制御入力、wはプロセスノイズです。
フワッとした説明をすると、
F=状態がどう振る舞うか係数・法則
x=推定値
u=操作量、MV値（例：モータの回転数、アクチュエータの操作量）
B=操作量がどう影響するか係数
W=ノイズ
になります。
今回は位置・速度だけなので操作量$Bu$項については考慮しないものとします。
またノイズ$W$についても除外します。すると、
\mathbf{x}_k = \mathbf{F}_k \mathbf{x}_{k-1}
になります。
添え字の k はタイムステップを表します。
プログラミング経験のある方であれば以下のような式も違和感がないと思います。
x = 100*x
そこで思い切って添字も削除します。
\mathbf{x} = \mathbf{F} \mathbf{x}
今回の状態方程式が完成しました。だいぶ読みやすくなりましたね。
ではこれを先ほどのF,xを当てはめて計算してみましょう。
\mathbf{F} \cdot \mathbf{x} =
\begin{bmatrix}
1 & \Delta t \\
0 & 1
\end{bmatrix}
\begin{bmatrix}
\text{位置} \\
\text{速度}
\end{bmatrix}
行列計算って忘れがちですよね。ということで少し丁寧に計算してみました。
\mathbf{F} \cdot \mathbf{x} =
\begin{bmatrix}
1 & \Delta t \\
0 & 1
\end{bmatrix}
\begin{bmatrix}
\text{位置} \\
\text{速度}
\end{bmatrix}
=
\begin{bmatrix}
(1 \cdot \text{位置}) + (\Delta t \cdot \text{速度}) \\
(0 \cdot \text{位置}) + (1 \cdot \text{速度})
\end{bmatrix}
=
\begin{bmatrix}
\text{位置} + \Delta t \cdot \text{速度} \\
\text{速度}
\end{bmatrix}
ここでポイントは
\text{位置} = \text{位置} + \Delta t \cdot \text{速度}
となることです。
速度で移動した分だけ位置が変わるわけですから、当たり前と言われれば当たり前ですね。
また、速度については
\text{速度} = \text{速度}
としています。
これは速度についてはステップ前後で特に変化しないという前提に基づいています。
今回は位置・速度を対象としたものなので上記式ですが、対象が変わればFも再設計が必要です。
フィルタ対象に応じてFを設計しないといけない理由はなんとなくわかったと思います。
カルマンフィルタの扱い辛さであると同時に自由度の高さでもあります。
観測ノイズ共分散行列R
測定値ｚの分散$(\sigma^2)$を入力します。
観測ノイズの分散がほしいので、安定時の測定値から標準偏差$(\sigma)$を計算します。
要はバラツキだけを入手したいとうことです。
たとえば、位置(メートル）の測定値の標準偏差が$5m$であれば、$R = 5^2 = 25m$となります。
今回は以下のようにしてみました。
R =
\begin{bmatrix}
\sigma_{\text{位置}}^2 & 0 \\
0 & \sigma_{\text{位置}}^2
\end{bmatrix}
=
\begin{bmatrix}
25 & 0 \\
0 & 25
\end{bmatrix}
$\Delta t$を考慮して以下のような形にしても良いかもしれません。
R =
\begin{bmatrix}
\sigma_{\text{位置}}^2 & 0 \\
0 & \sigma_{\text{位置}}^2 \cdot \Delta t
\end{bmatrix}
誤解を恐れず表現すると、
Rが大きい＝ノイズが多い＝観測値が信用できない＝フィルタ強め
Rが小さい＝ノイズが少ない＝観測値が信用できる＝フィルタ弱め
みたいなイメージです。
プロセスノイズ共分散行列Q
こちらはモデルがどれほど信用できるかを表す行列です。
参考記事に則り、運動方程式の白色ノイズモデルを採用します。
Q =
\begin{bmatrix}
\frac{\Delta t^3}{3} & \frac{\Delta t^2}{2} \\
\frac{\Delta t^2}{2} & \Delta t
\end{bmatrix} \phi
$\phi$は実際のデータにフィルタを掛けて決めます。
色々と理論はあるのかもしれませんが、実験的に決められる場合には所望のフィルタ性能になるまで数値を下げるという感じです。
こちらも誤解を恐れず表現すると、
Q（$\phi$）が大きい＝状態遷移モデルFが信用できない＝フィルタ弱め=観測値z重視
Q（$\phi$）が小さい＝状態遷移モデルFが信用できる＝フィルタ強め=モデルF重視
みたいなイメージです。
観測行列H
これは観測できる値を定義しています。
今回は位置のみ観測できます。
H =
\begin{bmatrix}
{\text{位置}} & {\text{速度}}
\end{bmatrix}
=
\begin{bmatrix}
1 & 0
\end{bmatrix}
1=可観測、0＝観測しない。という意味だと考えてください。
ただのマスキングですね。
カルマンフィルタの実装
さあ、これにて必要なパラメータは揃いました。実践タイムです！
ちょっと長かったので折りたたんでいます。
pythonでの実装コード
import numpy as np
import matplotlib.pyplot as plt
class KalmanFilter:
def __init__(self, F, H, Q, R, x0, P0):
"""
カルマンフィルタのパラメータを初期化
引数:
F : 状態遷移行列
H : 観測行列
Q : プロセス雑音共分散
R : 観測雑音共分散
x0 : 初期状態ベクトル
P0 : 初期誤差共分散行列
"""
self.F = F
self.H = H
self.Q = Q
self.R = R
self.x = x0
self.P = P0
def predict(self):
"""
予測ステップ:
状態予測: x = F * x
誤差共分散予測: P = F * P * F^T + Q
"""
self.x = self.F @ self.x
self.P = self.F @ self.P @ self.F.T + self.Q
return self.x
def update(self, z):
"""
更新ステップ:
観測 z に基づいて状態と誤差共分散を更新
引数:
z : 観測値
"""
# イノベーション（残差）
y = z - self.H @ self.x
# 観測共分散
S = self.H @ self.P @ self.H.T + self.R
# カルマンゲインの計算
K = self.P @ self.H.T @ np.linalg.inv(S)
# 状態の更新
self.x = self.x + K @ y
# 誤差共分散の更新
I = np.eye(self.P.shape[0])
self.P = (I - K @ self.H) @ self.P
return self.x
# --- パラメータの設定 ---
dt = 1.0 # サンプリング間隔
# 状態遷移行列 (定常速度モデル)
F = np.array([[1, dt],
[0, 1]])
# 観測行列 (位置のみ観測可能)
H = np.array([[1, 0]])
# プロセス雑音共分散 (適当な値を設定)
phi = 0.1
Q_base = np.array([[dt**3/3 , dt**2/2 ],
[dt**2/2 , dt]])
Q = Q_base * phi
# 観測雑音共分散
r = 25.0 # 5m^2
R = np.array([[r]])
# 初期状態とその誤差共分散
x0 = np.array([[0], # 位置の初期値
[0]]) # 速度の初期値
P0 = np.eye(2)
# カルマンフィルタのインスタンス作成
kf = KalmanFilter(F, H, Q, R, x0, P0)
# --- シミュレーションデータの生成 ---
num_steps = 200
# 真の初期状態 (例: 位置=0, 速度=1.0)
true_x = np.array([[0],
[1.0]])
true_states = [] # 真の状態の履歴
measurements = [] # 観測値の履歴
estimated_states = [] # 推定状態の履歴
np.random.seed(0) # 再現性のため乱数シードを固定
count = 0
for _ in range(num_steps):
# 真の状態更新
true_x = F @ true_x
true_states.append(true_x.copy())
# 観測値生成 (観測雑音を付加)
z = H @ true_x + np.random.normal(0, np.sqrt(r), size=(1, 1))
measurements.append(z.copy())
# カルマンフィルタの予測と更新
kf.predict()
kf.update(z)
estimated_states.append(kf.x.copy())
count += 1
# --- 結果のプロット ---
time = np.arange(num_steps)
true_positions = [state[0, 0] for state in true_states]
est_positions = [state[0, 0] for state in estimated_states]
measured_positions = [z[0, 0] for z in measurements]
plt.figure(figsize=(10, 6))
plt.plot(time, true_positions, label='True Position', linewidth=2, color='gray')
plt.plot(time, est_positions, label='Estimated Position', linewidth=2)
plt.scatter(time, measured_positions, label='Measured Position', color='red', marker='o')
plt.xlabel('Time Step')
plt.ylabel('Position')
plt.title('Kalman Filter: Position Estimation')
plt.legend()
plt.grid(True)
plt.show()
実行結果
線形の場合
phi=0.1の実行結果です。
続いてphi=0.001の実行結果です。
スムーズになっていますね。
Sin波形の場合
phi=0.1の実行結果です。
続いてphi=0.001の実行結果です。
速度は変化しないという前提でしたので、速度変化に対応できていませんね。
このような場合、大人しくphiを大きくする（観測値を重視する）か、非線形なカルマンフィルタを使う必要があります。あるいはローパスフィルタを使っても良いでしょう。
組み込みやすいカルマンフィルタを目指して
組み込み系などの制約がある場合、pythonコードはそのまま移植できませんね。
前回のローパスフィルタの記事と同様に今回も配列を使わず手計算スタイルで実装してみます。
配列を使わない手計算
# --- 初期値 ---
x1 = 0 # 初期位置
x2 = 0 # 初期速度
p11 = 1 # 初期誤差共分散行列の (1,1) 成分
p12 = 0 # 初期誤差共分散行列の (1,2) 成分
p21 = 0 # 初期誤差共分散行列の (2,1) 成分
p22 = 1 # 初期誤差共分散行列の (2,2) 成分
dt = 1.0 # サンプリング間隔
phi = 0.001 # プロセス雑音共分散の係数
r = 25.0 # 観測雑音共分散
# 状態遷移行列 F
F_11, F_12 = 1, dt
F_21, F_22 = 0, 1
# 観測行列 H
H_11 = 1
H_12 = 0
def kalman_filter(z, x1, x2, p11, p12, p21, p22):
# プロセス雑音共分散行列 Q
Q_11 = phi * (dt**3 / 3)
Q_12 = phi * (dt**2 / 2)
Q_21 = phi * (dt**2 / 2)
Q_22 = phi * dt
# 観測雑音共分散行列 R
R = r
# --- 予測ステップ ---
# 状態予測: x = F * x
x1_pred = F_11 * x1 + F_12 * x2
x2_pred = F_21 * x1 + F_22 * x2
# 誤差共分散予測: P = F * P * F^T + Q
p11_pred = F_11 * p11 * F_11 + F_11 * p12 * F_12 + F_12 * p21 * F_11 + F_12 * p22 * F_12 + Q_11
p12_pred = F_11 * p11 * F_21 + F_11 * p12 * F_22 + F_12 * p21 * F_21 + F_12 * p22 * F_22 + Q_12
p21_pred = F_21 * p11 * F_11 + F_21 * p12 * F_12 + F_22 * p21 * F_11 + F_22 * p22 * F_12 + Q_21
p22_pred = F_21 * p11 * F_21 + F_21 * p12 * F_22 + F_22 * p21 * F_21 + F_22 * p22 * F_22 + Q_22
# --- 更新ステップ ---
# イノベーション（残差）: y = z - H * x
y = z - (H_11 * x1_pred + H_12 * x2_pred)
# 観測共分散: S = H * P * H^T + R
S = H_11 * p11_pred * H_11 + H_11 * p12_pred * H_12 + H_12 * p21_pred * H_11 + H_12 * p22_pred * H_12 + R
# カルマンゲイン: K = P * H^T * S^(-1)
K1 = (p11_pred * H_11 + p12_pred * H_12) / S
K2 = (p21_pred * H_11 + p22_pred * H_12) / S
# 状態更新: x = x + K * y
x1_update = x1_pred + K1 * y
x2_update = x2_pred + K2 * y
# 誤差共分散更新: P = (I - K * H) * P
p11_update = (1 - K1 * H_11) * p11_pred - K1 * H_12 * p21_pred
p12_update = (1 - K1 * H_11) * p12_pred - K1 * H_12 * p22_pred
p21_update = -K2 * H_11 * p11_pred + (1 - K2 * H_12) * p21_pred
p22_update = -K2 * H_11 * p12_pred + (1 - K2 * H_12) * p22_pred
return x1_update, x2_update, p11_update, p12_update, p21_update, p22_update
for z in range(200):
x1, x2, p11, p12, p21, p22 = kalman_filter(z, x1, x2, p11, p12, p21, p22)
2要素ぐらいであればなんとかなりますね。
あとはSTでもCでもラダーでもお好きな言語に書き換えてください。
配列を使わない手計算（minimal)
行列の0要素はなにも計算していないので、0部分の計算を削ぎ落としていきます。
また、1の要素についても削除できそうですね。
# --- 初期値 ---
x1 = 0 # 初期位置
x2 = 0 # 初期速度
p11 = 1 # 初期誤差共分散行列の (1,1) 成分
p22 = 1 # 初期誤差共分散行列の (2,2) 成分
dt = 1.0 # サンプリング間隔
phi = 0.001 # プロセス雑音共分散の係数
r = 25.0 # 観測雑音共分散
# 状態遷移行列 F
F_12 = dt
def kalman_filter(z, x1, x2, p11, p22):
# プロセス雑音共分散行列 Q
Q_11 = phi * (dt**3 / 3)
Q_12 = phi * (dt**2 / 2)
Q_21 = phi * (dt**2 / 2)
Q_22 = phi * dt
# 観測雑音共分散行列 R
R = r
# --- 予測ステップ ---
# 状態予測: x = F * x
x1_pred = x1 + F_12 * x2
x2_pred = x2
# 誤差共分散予測: P = F * P * F^T + Q
p11_pred = p11 + F_12 * p22 * F_12 + Q_11
p12_pred = F_12 * p22 + Q_12
p21_pred = p22 * F_12 + Q_21
p22_pred = p22 + Q_22
# --- 更新ステップ ---
# イノベーション（残差）: y = z - H * x
y = z - x1_pred
# 観測共分散: S = H * P * H^T + R
S = p11_pred + R
# カルマンゲイン: K = P * H^T * S^(-1)
K1 = p11_pred / S
K2 = p21_pred / S
# 状態更新: x = x + K * y
x1_update = x1_pred + K1 * y
x2_update = x2_pred + K2 * y
# 誤差共分散更新: P = (I - K * H) * P
p11_update = (1 - K1) * p11_pred
p22_update = -K2 * p12_pred + p22_pred
return x1_update, x2_update, p11_update, p22_update
for z in range(200):
x1, x2, p11, p22 = kalman_filter(z, x1, x2, p11, p22)
かなりスッキリしましたね。
これならラダーにも実装できそうです（白目）
汎用性は皆無となりましたが限界民にはこのシンプルさはありがたいです。
異常検知
カルマンフィルタは実は異常検知器にもなります。
内部的に共分散 $S$（観測共分散）を持っていますので、これと残差$y$（イノベーション)を使ってマハラノビス距離による異常予測をしてみます。
マハラノビス距離
マハラノビス距離 d の定義は以下です。
d = \sqrt{y^T S^{-1} y}
実装はシンプルで、コードに以下の２行を追加するだけです。
def update(self, z):
# イノベーション（残差）の計算
y = z - self.H @ self.x
# 観測共分散 S の計算
S = self.H @ self.P @ self.H.T + self.R
+ # マハラノビス距離の計算 (平方距離として計算)
+ d2 = float(y.T @ np.linalg.inv(S) @ y)
+ d = np.sqrt(d2)
# カルマンゲインの計算
K = self.P @ self.H.T @ np.linalg.inv(S)
# 状態と誤差共分散の更新
self.x = self.x + K @ y
# 誤差共分散の更新
I = np.eye(self.P.shape[0])
self.P = (I - K @ self.H) @ self.P
return self.x, d
実行結果
ノコギリ波を準備してマハラノビス距離をプロットしてみました。
谷の部分でマハラノビス距離が大きくなっていますね。うまくいきました。
横線のTherseholdは3.84です。これは正規分布における95%信頼区間を示しています。2σ^2
配列を使わない手計算への追加
こちらもシンプルに以下の計算を行うだけです。非常にお手軽ですね。
# マハラノビス距離の計算:
d_mh = abs(y) / math.sqrt(S)
活用例
話を少し元に戻すとプロセスノイズ共分散行列$Q$は、実際に実験を行いながら小さくしていくと説明しました。
しかしノコギリ波のような異常値が連続する場合、$Q$を大きめに設定する必要があり、所望のフィルタ性能を達成できないことがあります。かといって非線形フィルタというのも難易度が高いですし、このような問題に対処できるかも未知数です。
↓phi=0.0001の例。まったく追従できていませんね。
このように、工場のセンサってほぼ線形な動きだけど、位置決めや繰り返し動作の切り替えの瞬間に上図のような非線形な動きとなることって、よくありますよね？
そこで対策としてQの値を動的に変更することを考えます。
異常値のときマハラノビス距離が大きくなるのはわかっているので、これをもとにQ値を決定します。
phi_base = 0.0001
Q_base = np.array([[dt ** 3 / 3, dt ** 2 / 2],
[dt ** 2 / 2, dt]])
#マハラノビス距離をもとにQの値を動的に変える。
#適当。対象特性によって変える。とりあえず2σ^2 = 1.96^2から外れたところから変える。
phi = phi_base * 10**(d_mh-3.84)
#制限リミット
if phi < phi_base:
phi = phi_base
if phi > 1.0:
phi = 1.0
Q = Q_base * phi
ということで、上記の計算を加えたときのグラフです。
うまく追従できましたね。
（非線形フィルタに片足を突っ込んでる気もしますが...）
正しいアプローチなのかわかりませんが、個人的には割と多用しています。
Qを変えるとマハラノビス距離も変わるのでそこが欠点でしょうか。それと調整が少し難しいです。
おわりに
線形カルマンフィルタの実践的コードを説明してみました。
制御入力$Bu$の説明ができなかったので、また次回にでも解説してみようと思います。
それではまた！
参考文献
- Pythonで学ぶベイズフィルタとカルマンフィルタ ←カルマンフィルタに対する基本的な解説。これ読んどけばok
- ストワのレーダーでカルマンフィルタ ←わかりやすくてオススメ。動画も良い