線形システム𝒙(𝑘)

𝒈(𝑘)

モデルに基づくカルマンフィルタ

+

+

－

+

𝑣(𝑘)

初期値

𝑷(0)

𝒙(𝑘)

事後状態推定値

𝑦−(𝑘)

𝑦(𝑘)

時系列

𝑤(𝑘)

カルマンゲイン

𝑦(𝑘)

イノベーション

システムのモデルをコピー

𝒙(0)

6.3.5 データとカルマンフィルタの関係

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

出力

時刻

𝑘 − 4 𝑘 − 3 𝑘 − 2 𝑘 − 1 𝑘

推定値 𝑥(𝑘)

観測値𝑦(𝑘)

6.3.5 分散行列の重要性

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

推定値

観測値

互いに独立な正規性白色雑音

6.3.6 多変数時系列に対するカルマンフィルタ

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

𝑨 ∈ ℝ𝑛×𝑛 𝑩 ∈ ℝ𝑛×𝑟 𝑪 ∈ ℝ𝑝×𝑛

𝒙 𝑘 + 1 = 𝑨𝒙 𝑘 + 𝑩𝒗 𝑘𝒚 𝑘 = 𝑪𝒙 𝑘 + 𝒘(𝑘)

𝒚(𝑘) ∈ ℝ𝑝𝒙(𝑘) ∈ ℝ𝑛

𝒗(𝑘) ∈ ℝ𝑟 ∼ 𝒩 𝟎,𝑸

𝒘(𝑘) ∈ ℝ𝑝 ∼ 𝒩 𝟎,𝑹

システム行列

信号

6.3.6 多変数時系列に対するカルマンフィルタ

初期設定

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

𝒙 0 = E 𝒙 0 = 𝒙0

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

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

時間更新

①

②

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

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

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

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

状態推定値：

𝑷 𝑘 = 𝑰 − 𝑮 𝑘 𝑪 𝑷− 𝑘

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

事後誤差共分散行列：

ＳＩＳＯと異なる箇所

4

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

∫𝑣(𝑡) 𝑥(𝑡)

白色雑音 ウィナー過程

𝑥 𝑘 + 1 = 𝑥 𝑘 + 𝑣 𝑘 , 𝑥 0 = 0

𝑦 𝑘 = 𝑥 𝑘 + 𝑤(𝑘)

互いに独立を仮定

6.4 数値シミュレーション例

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

𝑣(𝑘) ∼ 𝒩 0, 𝜎𝑣2

観測雑音

システム雑音

𝑤(𝑘) ∼ 𝒩 0, 𝜎𝑤2

状態方程式の一般系

と比較すると…

𝑨 = 𝑩 = 𝑪 = 1 に対応

𝒙 𝑘 + 1 = 𝑨𝒙 𝑘 + 𝑩𝒗 𝑘𝒚 𝑘 = 𝑪𝒙 𝑘 + 𝒘(𝑘)

𝑥 𝑘 + 1 = 𝑥 𝑘 + 𝑣 𝑘

𝑥 𝑘 = 𝑥 𝑘 − 1 + 𝑣 𝑘 − 1 = 𝑥 𝑘 − 2 + 𝑣 𝑘 − 2 + 𝑣 𝑘 − 1 = ･･･

= 𝑥 0 + 𝑣 1 + ･･･ + 𝑣 𝑘 − 1

=

𝑖=0

𝑘−1

𝑣(𝑖)

入力される白色雑音を積分している

6.4 数値シミュレーション例

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

𝑦 𝑘 = 𝑥 𝑘 + 𝑤(𝑘)

𝑥 0 = 0

例題6.1

𝑥 𝑘 + 1 = 𝑥 𝑘 + 𝑣 𝑘 , 𝑥 0 = 0

𝑦 𝑘 = 𝑥 𝑘 + 𝑤(𝑘)

𝑦 𝑘 𝑥 𝑘カルマンフィルタにより推定する．

・分散 システム雑音：𝜎𝑣2 = 1

観測雑音：𝜎𝑤2 = 2

・初期値 𝑥 0 = 0𝑝 0 = 0

6.4 数値シミュレーション例

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

pp. 112-117 参照

状態方程式

𝑥 𝑘 + 1 = 𝑥 𝑘 ，𝑥 0 = 1

𝑦 𝑘 = 𝑥 𝑘 + 𝑤(𝑘)

𝑘 = 1から𝑘 = 3に対するカルマンフィルタの時間更新式を導き，それから状態推定値の漸化式を導出せよ．ただし，𝑤(𝑘)は平均値0，分散𝑟 = 𝜎𝑤

2の正規性白色雑音， 𝑥 0 = 0，𝑝 0 = 1とする．

(6.62)

(6.63)

6.4 数値シミュレーション例

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

例題6.2

pp. 117-119 参照

𝑥 𝑘 + 1 = 𝐴𝑥 𝑘 + 𝑏𝑣 𝑘

𝑦 𝑘 = 𝑐𝑇𝑥 𝑘 + 𝑤(𝑘)

𝑥 𝑘 + 1 = 𝑥 𝑘 ，𝑥 0 = 1

𝑦 𝑘 = 𝑥 𝑘 + 𝑤(𝑘)

𝐴 = 1，𝑏 = 0，𝑐 = 1

一般式 例題

𝜎𝑣2 = 0，𝜎𝑤

2 = 𝑟

比較

𝑥− 𝑘 = 𝑥 𝑘 − 1 ， 𝑝− 𝑘 = 𝑝(𝑘 − 1)

6.4 数値シミュレーション例

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

𝑔 𝑘 =𝑝(𝑘 − 1)

𝑝 𝑘 − 1 + 𝑟

𝑥 𝑘 = 𝑥 𝑘 − 1 + 𝑔 𝑘 𝑦 𝑘 − 𝑥 𝑘 − 1

= 1 − 𝑔 𝑘 𝑥 𝑘 − 1 + 𝑔 𝑘 𝑦(𝑘)

𝑝 𝑘 = 1 − 𝑔 𝑘 𝑝 𝑘 − 1 =𝑟

𝑝 𝑘 − 1 + 𝑟𝑝 𝑘 − 1

• 𝑘 = 1のとき

𝑔 1 =1

1 + 𝑟

𝑥 1 =1

1 + 𝑟𝑦(1)

𝑝 1 =𝑟

1 + 𝑟

• 𝑘 = 2のとき

𝑔 2 =1

2 + 𝑟

𝑥 2 =1

2 + 𝑟(𝑦 1 + 𝑦 2 )

𝑝 1 =𝑟

2 + 𝑟

• 𝑘 = 3のとき

𝑔 2 =1

3 + 𝑟

𝑥 2 =1

3 + 𝑟(𝑦 1 + 𝑦 2 + 𝑦(3))

𝑝 1 =𝑟

3 + 𝑟

(6.64)

(6.65)

(6.66)

カルマンフィルタの時間更新式

6.4 数値シミュレーション例

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

時刻𝑘における状態推定値

𝑥 𝑘 =1

𝑘 + 𝑟

𝑖=1

𝑘

𝑦(𝑖)

平均値

ローパスフィルタ

𝑟は小さくなっていく

(6.67)

6.4 数値シミュレーション例

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

カルマンゲイン

𝑔 𝑘 =1

𝑘 + 𝑟

カルマンフィルタの漸化式

= 𝑥 𝑘 − 1 +1

𝑘 + 𝑟(𝑦 𝑘 − 𝑥(𝑘 − 1))

𝑥 𝑘 = 𝑥 𝑘 − 1 + 𝑔 𝑘 𝑦 𝑘 − 𝑥 𝑘 − 1

lim𝑘→∞

𝑔 𝑘 = 0

状態推定値が一定値に収束する

(6.68)

6.4 数値シミュレーション例

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

𝑥 𝑘 =𝑘 + 𝑟 − 1

𝑘 + 𝑟 𝑥 𝑘 − 1 +

1

𝑘 + 𝑟𝑦(𝑘)

𝑚 𝑘 =𝑘 − 1

𝑘 𝑚 𝑘 − 1 +

1

𝑘𝑦(𝑘)

同様の形式

(6.70)

(6.8)

時系列の平均を計算している

6.4 数値シミュレーション例

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

例題6.3

𝑧−1 + 0.5𝑧−2

1 + 1.5𝑧−1 + 0.7𝑧−2

𝑣(𝑘)

𝑤(𝑘)

𝑦(𝑘)++

この伝達関数モデルを可観測正準形の形式の状態空間モデルに変換せよ．ただし，システム雑音は𝒩(0，1)に従う正規性白色雑音，観測雑音は 𝒩(0，0.1) に従うシステム雑音と独立な正規性白色雑音とする．また， 𝑥 0 = 𝟎，𝑷 0 = 𝑰とする．

6.4 数値シミュレーション例

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

pp. 119-121 参照

8

時系列

𝐺 𝑧−1 =𝑧−1 + 0.5𝑧−2

1 + 1.5𝑧−1 + 0.7𝑧−2

𝑦 𝑘 = 𝐺 𝑧−1 𝑣 𝑘 + 𝑤(𝑘)

ただし

𝑥1(𝑘 + 1)𝑥2(𝑘 + 1)

=0 −0.71 −1.5

𝑥1(𝑘)𝑥2(𝑘)

+0.51

𝑣(𝑘)

𝑦 𝑘 = 0 1𝑥1(𝑘)𝑥2(𝑘)

+ 𝑤(𝑘)

可観測正準形に変換

6.4 数値シミュレーション例

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

時系列データ（確率過程）のフィルタリング問題

制御入力𝑢(𝑘)が存在する

システムの状態推定が必要

6.5 システム制御のためのカルマンフィルタ

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

離散時間状態方程式

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

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

(6.73)

(6.74)

𝒃𝑢

𝑢(𝑘)

𝒃 𝑧−1𝑰

𝑨

𝑣(𝑘)+

+ +𝒙 𝑘 + 1 𝒙 𝑘

+

既知

𝒄𝑇

++

𝑦(𝑘)

観測雑音 𝑤(𝑘)

観測値観測方程式

システム雑音

状態方程式

6.5 システム制御のためのカルマンフィルタ

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

𝒃𝑢

𝑢(𝑘)

𝒃

𝑧−1𝑰

𝑨

𝑣(𝑘)

++ +

𝒙 𝑘 + 1 𝒙 𝑘

+𝒄𝑇

++

𝑦(𝑘)

観測雑音 𝑤(𝑘)

観測値

プラント（制御対象）

システム雑音

制御入力（操作量）

図： システム制御の観点から書き直したブロック線図

6.5 システム制御のためのカルマンフィルタ

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

初期設定

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

𝒙 0 = E 𝒙 0 = 𝒙0

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

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

2 を設定する．

時間更新

①

②

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

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

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

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

𝒄𝑇𝑷− 𝑘 𝒄 + 𝜎𝑤2

状態推定値：

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

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

事後誤差共分散行列：

6.5 システム制御のためのカルマンフィルタ ～制御入力がある場合のSISO系～

自由系と異なる箇所

19

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

図： 制御入力がある場合のカルマンフィルタのブロック線図

𝒈 𝑘

𝑢(𝑘)

𝑰 − 𝒈 𝑘 𝒄𝑇

𝑧−1𝑰

𝑨

++𝑦(𝑘)

𝒃𝑢

𝒙− 𝑘

𝒙 𝑘 − 1

+

𝒙 𝑘

6.5 システム制御のためのカルマンフィルタ

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

制御対象をカルマンフィルタを用いて推定する手順

Step 1 プラントモデリング

• 第一原理モデル，システム同定法を用いて，制御対象（プラント）をモデリングする．EX. 力学システム（バネ，マス，ダンパシステム）

• 状態方程式𝒙 𝑘 + 1 = 𝑨𝒙 𝑘 + 𝒃𝑢𝑢(𝑘) + 𝒃𝑣 𝑘

のシステム雑音𝑣 𝑘 の分散を設定する．

Step 2 カルマンフィルタによる状態推定

• 制御入力がある場合のカルマンフィルタのアルゴリズムを用いて状態を推定する．

Step 3 コントローラの構成• 現代制御を用いて状態フィードバックにより制御則を決定する

Step 2で求めた状態推定値を用いてコントローラを構成する．

6.5 システム制御のためのカルマンフィルタ

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

𝑧−1 + 0.5𝑧−2

1 + 1.5𝑧−1 + 0.7𝑧−2

𝑣(𝑘)

𝑤(𝑘)

𝑦(𝑘)+++

+

𝑢(𝑘) 制御入力

システム雑音（入力雑音）

観測雑音

出力

この伝達関数モデルを可観測正準形の形式の状態空間モデルに変換せよ．

ただし，システム雑音は𝑁(0，0.01)に従う正規性白色雑音，観測雑音は𝑁(0，0.1)に従うシステム雑音と独立な正規性白色雑音とする．

また， 𝑥 0 = 𝟎，𝑷 0 = 𝑰とする．制御入力𝑢(𝑘)は±1の値を不規則にとる信号とする．

6.5 システム制御のためのカルマンフィルタ

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

例題6.4

時系列

𝐺 𝑧−1 =𝑧−1 + 0.5𝑧−2

1 + 1.5𝑧−1 + 0.7𝑧−2

𝑦 𝑘 = 𝐺 𝑧−1 𝑣 𝑘 + 𝐺 𝑧−1 𝑢 𝑘 + 𝑤(𝑘)

𝑥1(𝑘 + 1)𝑥2(𝑘 + 1)

=0 −0.71 −1.5

𝑥1(𝑘)𝑥2(𝑘)

+0.51

𝑢 𝑘 +0.51

𝑣(𝑘)

𝑦 𝑘 = 0 1𝑥1(𝑘)𝑥2(𝑘)

+ 𝑤(𝑘)

可観測正準形に変換

6.5 システム制御のためのカルマンフィルタ

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