６. 線形カルマンフィルタ

第５回 確率システム制御特論

1

𝑏 𝑧−1𝑰

𝑨

𝒄𝑇

状態方程式

観測方程式

観測雑音

時系列(観測値)

＋

＋

＋＋ 𝑦(𝑘)𝑣(𝑘)

𝒙 𝑘 + 1 = 𝑨𝒙 𝑘 + 𝒃𝑣(𝑘)

離散時間状態方程式• 𝑨 ∈ ℝ𝑛×𝑛 および𝒃 ∈ ℝ𝑛

• 𝑣 𝑘 ∼ 𝒩 0, 𝜎𝑣2 ：システム雑音

• 𝒄 ∈ ℝ𝑛 ：観測係数ベクトル• 𝑤(𝑘) ∼ 𝒩 0, 𝜎𝑤

2 ：観測雑音． 𝑣(𝑘)と無相関．𝑦 𝑘 = 𝒄𝑇𝒙 𝑘 + 𝑤(𝑘)

観測方程式

𝒙(𝑘 + 1) 𝒙(𝑘)

𝑤(𝑘)

𝑨, 𝒃, 𝒄は時不変（確率定常過程）

𝑨，𝒃を𝑨 𝑘 ，𝒃 𝑘とすれば非定常過程

2

第５回 確率システム制御特論

6.1 カルマンフィルタリング問題

6.1 カルマンフィルタリング問題

カルマンフィルタリング問題

時系列データ 𝑦 𝑖 , 𝑖 = 1,2,⋯ , 𝑘 に基づいて，状態𝒙 𝑘 のMSEの最小値を与える推定値，すなわち最小平均二乗誤差（MMSE: minimum mean square error）を見つけることを，カルマンフィルタリング問題と呼ぶ．

𝒙filter 𝑘 = argmin 𝒙 (𝑘)

𝐽 𝑘

𝐽 𝑘 = E 𝒙𝟐(𝑘)

状態推定誤差の定義

𝒙 𝑘 ≜ 𝒙 𝑘 − 𝒙(𝑘)推定値真値

評価関数

？ スカラの観測値 𝑦 𝑘 から𝑛次元状態ベクトル 𝒙 𝑘 を推定できるだろうか？

3

第５回 確率システム制御特論

6.3 時系列信号に対するカルマンフィルタ

事前推定値

事後推定値

𝒙−(𝑘) = 𝒙 𝑘 𝑘 − 1 時刻 𝑘 − 1までに利用可能なデータに基づいた時刻 𝑘における 𝒙の予測推定値．

𝒙(𝑘) = 𝒙 𝑘 𝑘時刻 𝑘までに利用可能なデータすなわち 𝑦 𝑘 も用いた 𝒙のフィルタリング推定値．

𝑘𝑘 − 1

𝒙(𝑘 − 1) 𝒙−(𝑘)

𝑦(𝑘)

𝒙(𝑘)

事前推定値 事前推定値

𝑷(𝑘 − 1) 𝑷−(𝑘) 𝑷(𝑘)

予測ステップ（モデルを利用）

フィルタリングステップ（観測を利用）

4

第５回 確率システム制御特論

ベイズ予測の立場より事前推定値と観測値に関する線形予測器を仮定する．

𝒙 𝑘 = 𝑮 𝑘 𝒙− 𝑘 + 𝒈 𝑘 𝑦(𝑘)事前推定値 観測値事後推定値

𝑮 𝑘 = 𝑰 − 𝒈(𝑘)𝒄𝑇 が成り立つ．

𝑦 𝑖 (𝑖 = 1,2,⋯ , 𝑘 − 1)は事後状態推定誤差 𝒙 𝑘 と直交する．すなわち，

E 𝒙 𝑘 𝑦 𝑖 = 𝑰 − 𝒈 𝑘 𝒄𝑇 − 𝑮 𝑘 E[𝒙 𝑘 𝑦 𝑖 ] = 𝟎

一方で，E[𝒙 𝑘 𝑦 𝑖 ] ≠ 𝟎 より，

𝑰 − 𝒈 𝑘 𝒄𝑇 − 𝑮 𝑘 = 𝟎

𝑮 𝑘 と 𝒈 𝑘 はどうやって決定する？？

まず，𝒈 𝑘 だけに注目すれば良いことが判明した．

5

6.3 時系列信号に対するカルマンフィルタ

第５回 確率システム制御特論

𝒙 𝑘 = 𝑰 − 𝒈(𝑘)𝒄𝑇 𝒙− 𝑘 + 𝒈 𝑘 𝑦 𝑘

= 𝒙− 𝑘 + 𝒈 𝑘 𝑦 𝑘

𝑦 𝑘 = 𝑦 𝑘 − 𝑦− 𝑘

= 𝒙− 𝑘 + 𝒈 𝑘 𝑦 𝑘 − 𝒄𝑇 𝒙− 𝑘

= 𝒙− 𝑘 + 𝒈 𝑘 𝑦 𝑘 − 𝑦− 𝑘 𝑦− 𝑘 = 𝒄𝑇 𝒙− 𝑘

一段先予測値

𝑘 − 1までの出力が観測されたときの時刻 𝑘での出力の事前推定値

イノベーション過程

カルマンゲイン

= 𝒄𝑇 𝒙 𝑘 + 𝑤(𝑘) − 𝒄𝑇 𝒙− 𝑘

= 𝒄𝑇 𝒙− 𝑘 + 𝑤(𝑘)

現時刻 𝑘での観測値に含まれている最新の情報に基づく出力予測誤差．

状態量の更新に現時刻 𝑘の観測値をどの程度反映させるかを決める．

𝑮 𝑘 を消去してまとめると…

（事後推定）＝（事前推定）＋（カルマンゲイン）・（出力予測誤差）という形で表現されることが分かった！

6

第５回 確率システム制御特論

6.3 時系列信号に対するカルマンフィルタ

E 𝒙 𝑘 𝑦 𝑖 = E 𝒙 𝑘 − 𝒙 𝑘 𝑦(𝑖)

= E 𝒙 𝑘 − 𝑮 𝑘 𝒙− 𝑘 − 𝒈 𝑘 𝑦(𝑘) 𝑦(𝑖)

= E 𝒙 𝑘 − 𝑮 𝑘 𝒙− 𝑘 − 𝒈 𝑘 (𝒄𝑇𝒙 𝑘 − 𝑤 𝑘 ) 𝑦(𝑖)

= E 𝒙 𝑘 − 𝑮 𝑘 𝒙− 𝑘 − 𝒈 𝑘 𝒄𝑇𝒙 𝑘 − 𝒈 𝑘 𝑤(𝑘) 𝑦(𝑖) = 𝟎

E 𝑤 𝑘 𝑦 𝑖 = 0, 𝑖 = 1,2,⋯ , 𝑘 − 1 より

↪ E 𝒙 𝑘 − 𝑮 𝑘 𝒙− 𝑘 − 𝒈 𝑘 𝒄𝑇𝒙 𝑘 𝑦(𝑖) = 𝟎

↪ E 𝑰 − 𝒈 𝑘 𝒄𝑇 − 𝑮(𝑘)}𝒙 𝑘 𝑦(𝑖) + 𝑮 𝑘 {𝒙 𝑘 − 𝒙− 𝑘 𝑦(𝑖)

= E 𝑰 − 𝒈 𝑘 𝒄𝑇 − 𝑮(𝑘) 𝒙 𝑘 𝑦(𝑖) + 𝑮 𝑘 𝒙− 𝑘 𝑦(𝑖) = 𝟎

𝒙− 𝑘 = 𝒙 𝑘 − 𝒙− 𝑘 : 事前状態推定誤差

↪ E 𝑰 − 𝒈 𝑘 𝒄𝑇 − 𝑮 𝑘 𝒙 𝑘 𝑦(𝑖)

= 𝑰 − 𝒈 𝑘 𝒄𝑇 − 𝑮 𝑘 E 𝒙 𝑘 𝑦(𝑖) = 𝟎

𝟎

𝟎

「𝑮 𝑘 = 𝑰 − 𝒈(𝑘)𝒄𝑇 」の証明

7

第５回 確率システム制御特論

6.3 時系列信号に対するカルマンフィルタ

カルマンゲインの求め方

𝒙 𝑘 = 𝒙 𝑘 − 𝒙 𝑘 = 𝑰 − 𝒈 𝑘 𝒄𝑇 𝒙− 𝑘 − 𝒈 𝑘 𝑤 𝑘

𝑦 𝑘 = 𝑦 𝑘 − 𝑦− 𝑘 = 𝒄𝑇 𝒙− 𝑘 + 𝑤(𝑘)

E 𝒙 𝑘 𝑦 𝑘 = 𝟎

𝑰 − 𝒈 𝑘 𝒄𝑇 E 𝒙− 𝑘 𝒙− 𝑘𝑇𝒄 − 𝒈 𝑘 𝜎𝑤

2 = 𝟎

𝑷− 𝑘 とおく

𝒈 𝑘 =𝑷− 𝑘 𝒄

𝒄𝑇𝑷− 𝑘 𝒄 + 𝜎𝑤2

３つの関係式を利用する

カルマンゲインの算出方法が分かった！でも， 𝑷− 𝑘 はどうやって求める？8

第５回 確率システム制御特論

6.3 時系列信号に対するカルマンフィルタ

𝒙 𝑘 = 𝒙 𝑘 − 𝒙 𝑘 = 𝑰 − 𝒈 𝑘 𝒄𝑇 𝒙− 𝑘 − 𝒈 𝑘 𝑤 𝑘

𝑦 𝑘 = 𝑦 𝑘 − 𝑦− 𝑘 = 𝒄𝑇 𝒙− 𝑘 + 𝑤(𝑘)

E 𝒙 𝑘 𝑦 𝑘 = 𝟎

E 𝒙 𝑘 𝑦 𝑘 = E 𝑰 − 𝒈 𝑘 𝒄𝑇 𝒙− 𝑘 − 𝒈 𝑘 𝑤 𝑘 𝒄𝑇 𝒙− 𝑘 + 𝑤(𝑘) = 𝟎

↪ E 𝑰 − 𝒈 𝑘 𝒄𝑇 𝒙− 𝑘 𝒄𝑇 𝒙− 𝑘 + E 𝑰 − 𝒈 𝑘 𝒄𝑇 𝒙− 𝑘 𝑤 𝑘

−E 𝒈 𝑘 𝑤 𝑘 𝒄𝑇 𝒙− 𝑘 − E 𝒈 𝑘 𝑤2 𝑘 = 𝟎𝟎

𝟎↪ E 𝑰 − 𝒈 𝑘 𝒄𝑇 𝒙− 𝑘 𝒄𝑇 𝒙− 𝑘 − E 𝒈 𝑘 𝑤2 𝑘 = 𝟎

↪ E 𝑰 − 𝒈 𝑘 𝒄𝑇 𝒙− 𝑘 𝒙− 𝑘𝑇𝒄 − 𝒈 𝑘 E 𝑤2 𝑘 = 𝟎

↪ 𝑰 − 𝒈 𝑘 𝒄𝑇 E 𝒙− 𝑘 𝒙− 𝑘𝑇𝒄 − 𝒈 𝑘 𝜎𝑤

2 = 𝟎

𝑷− 𝑘 とおく

カルマンゲイン算出の詳細

9

第５回 確率システム制御特論

6.3 時系列信号に対するカルマンフィルタ

共分散行列の更新

前提 １時刻前の事後共分散行列 𝑷 𝑘 − 1 は得られている．

Step 1予測

Step 2フィルタリング

𝑷 𝑘 − 1 ⇒ 𝑷− 𝑘

𝑷− 𝑘 = E 𝒙− 𝑘 𝒙− 𝑘𝑇

= E 𝑨 𝒙 𝑘 − 1 + 𝒃𝑣(𝑘 − 1) 𝑨 𝒙 𝑘 − 1 + 𝒃𝑣(𝑘 − 1) 𝑇

= 𝑨E 𝒙 𝑘 − 1 𝒙𝑇 𝑘 − 1 𝑨𝑇 + 𝑨E 𝒙 𝑘 − 1 𝑣 𝑘 − 1 𝒃𝑇

+𝒃E 𝑣 𝑘 − 1 𝒙𝑇 𝑘 − 1 𝑨𝑇 + 𝒃E 𝑣2 𝑘 − 1 𝒃𝑇

= 𝑨𝑷(𝑘 − 1)𝑨𝑇 + 𝜎𝑣2𝒃𝒃𝑇

𝟎

𝟎𝑷(𝑘 − 1)

𝜎𝑣2

𝑷− 𝑘 ⇒ 𝑷 𝑘 𝑷 𝑘 = E 𝒙 𝑘 𝒙𝑇 𝑘 = ⋯ = 𝑰 − 𝒈 𝑘 𝒄𝑇 𝑷− 𝑘

すべての情報が揃った！10

6.3 時系列信号に対するカルマンフィルタ

第５回 確率システム制御特論

6.3 カルマンフィルタ ～c. 入力が白色ノイズのSISO系～

初期設定

状態推定値の初期値 𝒙(0) は 𝒩 𝟎, Σ0 に従う正規性確率ベクトルとする．

𝒙 0 = E 𝒙 0 = 𝒙0

𝑷 0 = E 𝒙 0 − E 𝒙(0) 𝒙 0 − E 𝒙(0) 𝑇 = 𝚺0

システム雑音の分散 𝜎𝑣2 と観測雑音の分散 𝜎𝑤

2 を設定する．

時間更新

①

②

① 予測ステップ 事前状態推定： 𝒙− 𝑘 = 𝑨 𝒙 𝑘 − 1

事前誤差共分散行列： 𝑷− 𝑘 = 𝑨𝑷 𝑘 − 1 𝑨𝑇 + 𝜎𝑣2𝒃𝒃𝑇

② フィルタリングステップ

カルマンゲイン： 𝒈 𝑘 =𝑷− 𝑘 𝒄

𝒄𝑇𝑷− 𝑘 𝒄 + 𝜎𝑤2

状態推定値：

𝑷 𝑘 = 𝑰 − 𝒈 𝑘 𝒄𝑇 𝑷− 𝑘

𝒙 𝑘 = 𝒙− 𝑘 + 𝒈(𝑘) 𝑦 𝑘 − 𝒄𝑇 𝒙− 𝑘

事後誤差共分散行列：

事前知識・計測中間変数推定値

𝚺0 = 𝑘𝑰 で良いが，𝑘が大きいと収束が速い代わりに初期の振動が大きくなる．

実験や事前知識により見積もる実験や事前知識により見積もる

11

第５回 確率システム制御特論

1 時系列モデルやシステムモデルの精度に推定精度が大きく依存する

3

2

シミュレーション結果と実装時の結果は大きく精度が違う．実装時の精度は一般に低くなる．

非線形および非ガウシアンの場合は状態推定が難しくなる

EKFやUKF, PFをなどの非線形フィルタを利用することで対処

注意点

6.3 線形カルマンフィルタ ～f. 注意点～

12

第５回 確率システム制御特論

4

事後誤差共分散行列： 𝑷 𝑘 = 𝑰 − 𝒈 𝑘 𝝋𝑇 𝑘 𝑷−(𝑘)

実装時は逆行列の計算を発生させない工夫が必要．

カルマンゲイン： 𝑮 𝑘 = 𝑷− 𝑘 𝑪 𝑪𝑇𝑷− 𝑘 𝑪 + 𝑹 −1

𝒇 = 𝑷− 𝑘 𝑪

𝛼 = 𝑪𝑇𝒇 + 𝑹

𝑮 𝑘 =𝒇 ∙ 1/𝛼

5

この値が小さくなりすぎると数値計算の精度の問題で，𝑷 𝑘 の妥当性が崩れる．

U-D分解フィルタ，SVDフィルタなどの改善手法が存在する．

6.3 線形カルマンフィルタ ～f. 注意点～

注意点

13

第５回 確率システム制御特論

初期設定

状態推定値の初期値 𝒙(0) は 𝒩 𝟎, Σ0 に従う正規性確率ベクトルとする．

𝒙 0 = E 𝒙 0 = 𝒙0

𝑷 0 = E 𝒙 0 − E 𝒙(0) 𝒙 0 − E 𝒙(0) 𝑇 = 𝚺0

システム雑音の分散 𝜎𝑣2 と観測雑音の分散 𝜎𝑤

2 を設定する．

時間更新

①

②

① 予測ステップ 事前状態推定： 𝒙− 𝑘 = 𝑨 𝒙 𝑘 − 1 + 𝒃𝑢𝑢(𝑘 − 1)

事前誤差共分散行列： 𝑷− 𝑘 = 𝑨𝑷 𝑘 − 1 𝑨𝑇 + 𝜎𝑣2𝒃𝒃𝑇

② フィルタリングステップ

カルマンゲイン： 𝒈 𝑘 =𝑷− 𝑘 𝒄

𝒄𝑇𝑷− 𝑘 𝒄 + 𝜎𝑤2

状態推定値：

𝑷 𝑘 = 𝑰 − 𝒈 𝑘 𝒄𝑇 𝑷− 𝑘

𝒙 𝑘 = 𝒙− 𝑘 + 𝒈(𝑘) 𝑦 𝑘 − 𝒄𝑇 𝒙− 𝑘

事後誤差共分散行列：

6.3 カルマンフィルタ ～e. 制御入力がある場合のSISO系～

自由系と異なる箇所

14

第５回 確率システム制御特論

初期設定

状態推定値の初期値 𝒙(0) は 𝒩 𝟎, Σ0 に従う正規性確率ベクトルとする．

𝒙 0 = E 𝒙 0 = 𝒙0

𝑷 0 = E 𝒙 0 − E 𝒙(0) 𝒙 0 − E 𝒙(0) 𝑇 = 𝚺0

システム雑音の分散 𝜎𝑣2(0) と観測雑音の分散 𝜎𝑤

2(0)を設定する．

時間更新

①

②

① 予測ステップ

事前状態推定： 𝒙− 𝑘 = 𝑨(𝑘 − 1) 𝒙 𝑘 − 1 + 𝒃𝑢(𝑘 − 1)𝑢(𝑘 − 1)

事前誤差共分散行列： 𝑷− 𝑘 = 𝑨(𝑘 − 1)𝑷 𝑘 − 1 𝑨𝑇(𝑘 − 1) + 𝜎𝑣2𝒃(𝑘 − 1)𝒃𝑇(𝑘 − 1)

② フィルタリングステップ

カルマンゲイン： 𝒈 𝑘 =𝑷− 𝑘 𝒄(𝑘)

𝒄𝑇(𝑘)𝑷− 𝑘 𝒄(𝑘) + 𝜎𝑤2(𝑘)

状態推定値：

𝑷 𝑘 = 𝑰 − 𝒈 𝑘 𝒄𝑇(𝑘) 𝑷− 𝑘

𝒙 𝑘 = 𝒙− 𝑘 + 𝒈(𝑘) 𝑦 𝑘 − 𝒄𝑇(𝑘) 𝒙− 𝑘

事後誤差共分散行列：

6.3 カルマンフィルタ ～f. 制御入力がある場合のSISO系非定常過程～

定常過程と異なる箇所

15

第５回 確率システム制御特論

6.3 カルマンフィルタ ～d. 入力が白色ノイズのMIMO系～

初期設定

状態推定値の初期値 𝒙(0) は 𝒩 𝟎, Σ0 に従う正規性確率ベクトルとする．

𝒙 0 = E 𝒙 0 = 𝒙0

𝑷 0 = E 𝒙 0 − E 𝒙(0) 𝒙 0 − E 𝒙(0) 𝑇 = 𝚺0

システム雑音の分散 𝑸 と観測雑音の分散 𝑹を設定する．

時間更新

①

②

① 予測ステップ 事前状態推定： 𝒙− 𝑘 = 𝑨 𝒙 𝑘 − 1

事前誤差共分散行列： 𝑷− 𝑘 = 𝑨𝑷 𝑘 − 1 𝑨𝑇 + 𝑩𝑸𝑩𝑇

② フィルタリングステップ

カルマンゲイン： 𝑮 𝑘 = 𝑷− 𝑘 𝑪𝑇 𝑪𝑷− 𝑘 𝑪𝑇 + 𝑹 −1

状態推定値：

𝑷 𝑘 = 𝑰 − 𝑮 𝑘 𝑪 𝑷− 𝑘

𝒙 𝑘 = 𝒙− 𝑘 + 𝑮(𝑘) 𝑦 𝑘 − 𝑪 𝒙− 𝑘

事後誤差共分散行列：

ＳＩＳＯと異なる箇所

16

第５回 確率システム制御特論

1. 線形カルマンフィルタ ～a. ARXモデル～

対象とする離散時間線形システム（ARXモデル）

𝑦 𝑘 + 𝑖=1𝑛 𝑎𝑖𝑦(𝑘 − 𝑖) = 𝑖=1

𝑛 𝑏𝑖𝑦(𝑘 − 𝑖) + 𝑤(𝑘)

𝑎1, 𝑎2, … 𝑎𝑛 ,𝑏1, 𝑏2, … , 𝑏𝑛

𝑢 𝑘 :時刻𝑘におけるシステムへの入力

𝑦 𝑘 :時刻𝑘におけるシステムの出力

𝑤 𝑘 ~𝒩 0, 𝜎𝑤2 ：正規性白色雑音

パラメータを推定したい！

𝐵(𝑧−1)

𝐴(𝑧−1)

1

𝐴(𝑧−1)

ARモデル

ARXモデル

eXogenous+

+

白色雑音

制御入力(外生入力)

𝑤(𝑘)

𝑢(𝑘) 𝑦(𝑘)

出力

𝐴 𝑧−1 = 1 + 𝑖=1𝑛 𝑎𝑖𝑧

−𝑖 : 出力𝑦の𝑧変換 𝐵 𝑧−1 = 𝑖=1𝑛 𝑏𝑖𝑧

−𝑖 : 入力𝑢の𝑧変換

17

第５回 確率システム制御特論

ARXモデルのブロック線図より 𝑦 𝑘 =𝐵 𝑧−1

𝐴 𝑧−1𝑢 𝑘 +

1

𝐴 𝑧−1𝑤(𝑘)

両辺に𝐴(𝑧−1 )をかけて， 𝐴 𝑧−1 = 1 +

𝑖=1

𝑛

𝑎𝑖𝑧−𝑖 , 𝐵 𝑧−1 =

𝑖=1

𝑛

𝑏𝑖𝑧−𝑖を代入すると

𝑦 𝑘 1 +

𝑖=1

𝑛

𝑎𝑖𝑧−𝑖 =

𝑖=1

𝑛

𝑏𝑖𝑧−𝑖 𝑢 𝑘 + 𝑤(𝑘)

ここで，推移演算子 𝑧−𝑗𝑦 𝑘 = 𝑦 𝑘 − 𝑗 , (𝑗 = 1, 2, 3, … ) を利用すれば，

𝑦 𝑘 +

𝑖=1

𝑛

𝑎𝑖𝑦(𝑘 − 𝑖) =

𝑖=1

𝑛

𝑏𝑖𝑢(𝑘 − 𝑖) + 𝑤(𝑘)

6.3 線形カルマンフィルタ ～a. ARXモデル～

18

第５回 確率システム制御特論

これを𝑦 𝑘 について整理すると，

𝑦 𝑘 = −

𝑖=1

𝑛

𝑎𝑖𝑦 𝑘 − 𝑖 +

𝑖=1

𝑛

𝑏𝑖𝑢 𝑘 − 𝑖 + 𝑤 𝑘

= −𝑦 𝑘 − 1 ⋯− 𝑦 𝑘 − 𝑛 𝑢 𝑘 − 1 ⋯𝑢 𝑘 − 𝑛

𝑎1⋮𝑎𝑛𝑏1⋮𝑏𝑛

+ 𝑤(𝑘)

= 𝝋𝑇 𝑘 𝜽 + 𝑤 𝑘

6.3 線形カルマンフィルタ ～a. ARXモデル～

19

第５回 確率システム制御特論

パラメータ推定問題の線形回帰モデル

𝝋 𝑘 =

−𝑦 𝑘 − 1⋮

−𝑦 𝑘 − 𝑛

𝑢 𝑘 − 1⋮

𝑢(𝑘 − 𝑛)

𝑨 𝑘 = 𝑰𝒃 𝑘 = 𝟎𝒄 𝑘 = 𝝋(𝑘)

𝜽 =

𝑎1⋮𝑎𝑛𝑏1⋮𝑏𝑛

状態空間モデルと対応付ければ，カルマンフィルタのアルゴリズムが見出せる！

𝑦 𝑘 = 𝝋𝑇 𝑘 𝜽 𝑘 + 𝑤(𝑘)

𝜽 𝑘 + 1 = 𝜽 𝑘

𝒙 𝑘 + 1 = 𝑨 𝑘 𝒙 𝑘 + 𝒃 𝑘 𝑣(𝑘)

𝑦 𝑘 = 𝒄𝑇 𝑘 𝒙 𝑘 + 𝑤 𝑘

6.3 線形カルマンフィルタ ～b. パラメータ推定問題～

20

第５回 確率システム制御特論

事前誤差共分散行列：

6.3 線形カルマンフィルタ ～c. パラメータ推定アルゴリズム（パラメータ時不変）～

初期設定

推定パラメータの初期値

システム雑音の分散 𝜎𝑣2 と観測雑音の分散 𝜎𝑤

2 を設定する．

時間更新

①

②

① 予測ステップ 事前状態推定：

事前誤差共分散行列：

② フィルタリングステップ

カルマンゲイン：

状態推定値：

事後誤差共分散行列：

𝜽 0 = 𝜽0

𝑷 0 = 𝛾𝑰, 𝛾 > 0

𝜽− 𝑘 = 𝜽(𝑘 − 1)

𝑷− 𝑘 = 𝑷 𝑘 − 1

𝒈 𝑘 =𝑷− 𝑘 𝝋 𝑘

𝝋𝑇 𝑘 𝑷− 𝑘 𝝋 𝑘 + 𝜎𝑤2

𝜽 𝑘 = 𝜽 𝑘 − 1 + 𝒈 𝑘 {𝑦 𝑘 − 𝝋𝑇 𝑘 𝜽 𝑘 − 1 }

𝑷 𝑘 = 𝑷 𝑘 − 1 −𝑷− 𝑘 𝝋 𝑘 𝝋𝑇 𝑘 𝑷− 𝑘

𝝋𝑇 𝑘 𝑷− 𝑘 𝝋 𝑘 + 𝜎𝑤2

21

第５回 確率システム制御特論

離散時間状態方程式

時間更新

事前状態推定値：

𝑷− 𝑘 = 𝑷 𝑘 − 1 + 𝜎𝑣2𝒃𝒃𝑇事前誤差共分散行列：

カルマンゲイン：

状態推定値：

事後誤差共分散行列： 𝑷 𝑘 = 𝑰 − 𝒈 𝑘 𝝋𝑇 𝑘 𝑷−(𝑘)

𝜽 𝑘 + 1 = 𝜽 𝑘 + 𝒃𝑣(𝑘)

𝑦 𝑘 = 𝝋𝑇 𝑘 𝜽 𝑘 + 𝑤 𝑘

𝜽− 𝑘 = 𝜽(𝑘 − 1)

𝜽 𝑘 = 𝜽 𝑘 − 1 + 𝒈 𝑘 {𝑦 𝑘 − 𝝋𝑇 𝑘 𝜽 𝑘 − 1 }

𝒈 𝑘 =𝑷− 𝑘 𝝋 𝑘

𝝋𝑇 𝑘 𝑷− 𝑘 𝝋 𝑘 + 𝜎𝑤2

6.3 線形カルマンフィルタ ～d. パラメータ推定アルゴリズム（パラメータ時変）～

22

第５回 確率システム制御特論

特徴

1 計算機を用いたオンライン処理に適している

3

2

4

カルマンフィルタは漸化式の形式であるため過去のデータをすべて記憶する必要がない

非定常時系列（時変システム）に対しても適用できる

プロセス雑音の分散𝜎𝑣2と観測雑音の分散𝜎𝑤

2が厳密な値でなくても動作する

推定問題を状態空間表現することによって，さまざまな問題をカルマンフィルタの枠組みで解くことが可能

6.3 線形カルマンフィルタ ～e. 特長～

23

第５回 確率システム制御特論