非線形カルマンフィルタ～a. 問題設定～離散時間非...

第８回非線形確率システム制御

非線形カルマンフィルタ～a. 問題設定～

𝒙 𝑘 + 1 = 𝒇 𝒙 𝑘 + 𝒃𝑣 𝑘

離散時間非線形状態空間表現

𝑦 𝑘 = ℎ 𝒙 𝑘 + 𝑤 𝑘

𝑓 ∙ : ベクトル値をとる𝒙 𝑘 の非線形関数

ℎ ∙ : スカラ値をとる𝒙 𝑘 の非線形関数

𝑤 𝑘 ∶ 観測雑音(平均値 0，分散𝜎𝑤2 𝑘 )

𝑣 𝑘 ∶ システム雑音(平均値 0，分散𝜎𝑣2 𝑘 ) 𝒙 𝑘 + 1 = 𝒇 𝒙 𝑘 ，𝑣 𝑘

𝑦 𝑘 = ℎ 𝒙 𝑘 ，𝑤 𝑘

システム雑音と観測雑音の項も非線形関数に含めるのが一般的．

1

確率分布を非線形変換すると，その分布の3次以上の高次モーメントが最初の二つのモーメントを変化させてしまう．

非線形カルマンフィルタ



𝑓 𝑥

𝑥 𝑘 + 1

𝑥 𝑘

𝑓 𝑥

𝑥 𝑘 + 1

𝑥 𝑘非線形システムによる分布の変換線形システムによる分布の変換（スカラシステム）

非線形カルマンフィルタにおける中心的な課題

1. 正規分布に従う確率変数が，曲線である非線形システムによってどのような分布に変換されるか

2. その分布からどのようにして状態推定値を計算するか

→非線形フィルタリングでは，平均値と共分散行列という二つのモーメントだけを用いて正確な状態推定を行うことができなくなる．

2

平均値 𝒙と共分散行列𝑷の非線形変換𝒈 ∙ を近似するために，


代表的な近似法

線形化（ＥＫＦ）

統計的サンプリング法（ＵＫＦ）

非線形関数のテイラー級数展開を偏微分（ヤコビアン）を用いて計算し級数を１次で打ち切る方法

1. 元の空間でサンプルΞ𝑖を選び，2. それらを𝒈で非線形変換して， 𝒈 Ξ𝑖 を得て，3. その𝒈 Ξ𝑖 に基づいて変換先での平均値と共分散行列を推定


3

方法状態方程式確率分布

線形カルマンフィルタ線形正規性

EKF（拡張カルマンフィルタ）非線形正規性

UKF（シグマポイントカルマンフィルタ）非線形正規性

パーティクルフィルタ非線形非正規性

多数のサンプル点を用いて分布を近似


接線による曲線の直線化小数個のサンプル点を用いて分布を近似

𝑓 𝑥𝑥 𝑘 + 1

𝑥 𝑘

𝑥 𝑘 + 1

𝑓 𝑥

𝑥 𝑘

UKFの考え方EKFの考え方モンテカルロ法の考え方

𝑥 𝑘

𝑥 𝑘 + 1𝑓 𝑥


4


拡張カルマンフィルタは，1. 非線形システムを各時刻において線形化し，2. それぞれの時刻において時変カルマンフィルタを適用する

という考えに基づく（次の４段階）．

これらの非線形関数をテイラー級数展開を用いて線形近似すると，

時刻𝑘，𝑘 + 1において，それぞれ事前状態推定値 𝒙− 𝑘 が利用可能であるという仮定のもとで，

𝒙 𝑘 + 1 = 𝒇 𝒙 𝑘 + 𝒃𝑣 𝑘

𝑦 𝑘 = ℎ 𝒙 𝑘 + 𝑤 𝑘

ℎ 𝒙 𝑘 = ℎ 𝒙− 𝑘 + 𝒄𝑇 𝑘 𝒙 𝑘 − 𝒙− 𝑘

𝒇 𝒙 𝑘 = 𝒇 𝒙− 𝑘 + 𝑨 𝑘 𝒙 𝑘 − 𝒙− 𝑘

が得られる．ただし，

𝑨 𝑘 = 𝜕𝒇(𝒙)

𝜕𝒙𝑥= 𝒙− 𝑘

𝒄𝑇 𝑘 = 𝜕ℎ(𝒙)

𝜕𝒙𝑥= 𝒙− 𝑘

非線形カルマンフィルタ～b. 拡張カルマンフィルタ～

１

２

5

非線形システムを線形化したものは，制御入力𝒖 𝑘 を含んだ線形システムと同じ形式であることがわかる．（ただし，係数行列𝑨と係数ベクトル𝒄はともに時変）


拡張カルマンフィルタ～a. 特長と注意点～

𝒙 𝑘 + 1 = 𝑨 𝑘 𝒙 𝑘 + 𝒃𝑣 𝑘 + 𝒇 𝒙− 𝑘 − 𝑨 𝑘 𝒙− 𝑘

𝑦 𝑘 = 𝒄𝑇 𝑘 𝒙 𝑘 + 𝑤 𝑘 + ℎ 𝒙− 𝑘 − 𝒄𝑇 𝑘 𝒙− 𝑘

𝒖 𝑘 ≜ 𝒇 𝒙− 𝑘 − 𝑨 𝑘 𝒙− 𝑘

𝑧 𝑘 ≜ 𝑦 𝑘 − ℎ 𝒙− 𝑘 + 𝒄𝑇 𝑘 𝒙− 𝑘

𝒙 𝑘 + 1 = 𝑨 𝑘 𝒙 𝑘 + 𝒃𝑣 𝑘 + 𝒖 𝑘

𝑧 𝑘 = 𝒄𝑇 𝑘 𝒙 𝑘 + 𝑤 𝑘

３

４

6


時間更新

事前状態推定値：

線形近似：

事前誤差共分散行列：

カルマンゲイン：

状態推定値：

事後誤差共分散行列：

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

① 予測ステップ

𝒙− 𝑘 = 𝒇 𝒙 𝑘 − 1

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

𝒈 𝑘 =𝑷− 𝑘 𝒄 𝑘

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

𝒙 𝑘 = 𝒙− 𝑘 + 𝒈 𝑘 𝒚 𝑘 − ℎ 𝒙− 𝑘

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

𝑨 𝑘 − 1 = 𝜕𝒇(𝒙)

𝜕𝒙𝑥= 𝒙 𝑘−1

𝒄𝑇 𝑘 − 1 = 𝜕ℎ(𝒙)

𝜕𝒙𝑥= 𝒙 𝑘−1

初期設定

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

𝒙 0 = E 𝒙 0 = 𝒙0

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

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

2 を設定する．

①

②

非線形カルマンフィルタ～b. 拡張カルマンフィルタ～

7

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

拡張カルマンフィルタ

例題 7.1

スカラの非線形状態方程式に対するEKFを構成せよ．

𝑥 𝑘 + 1 = 𝑥 𝑘 + 3 cos𝑥(𝑘)

10+ 𝑣(𝑘)

𝑥 0 = 10

𝑦 𝑘 = 𝑥3 𝑘 + 𝑤(𝑘)

𝑣(𝑘)~𝒩 0,1

𝑤(𝑘) ~𝒩 0,100 𝑥 0 = 11

𝑝 0 = 1


7.2 拡張カルマンフィルタ

𝑓 𝑥 ≜ 𝑥 + 3 cos𝑥

10ℎ 𝑥 ≜ 𝑥3

𝜕𝑓 𝑥

𝜕𝑥= 1 −

3

10sin

𝑥

10

𝜕ℎ 𝑥

𝜕𝑥= 3𝑥2

これらを𝑥(𝑘)について偏微分すると

これらを非線形カルマンフィルタのアルゴリズムに代入すると，

時間更新


線形近似：


カルマンゲイン：

状態推定値：

事後誤差共分散行列：

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

① 予測ステップ 𝑥− 𝑘 = 𝑥 𝑘 − 1 + 3 cos

𝑥 𝑘 − 1

10

𝑝− 𝑘 = 𝑎2 𝑘 − 1 𝑝 𝑘 − 1 + 1

𝑔 𝑘 =𝑝− 𝑘 𝑐 𝑘

𝑐2 𝑘 𝑝− 𝑘 + 100

𝑥 𝑘 = 𝑥− 𝑘 + 𝑔 𝑘 𝑦 𝑘 − 𝑥− 𝑘 3

𝑝 𝑘 = 1 − 𝑔 𝑘 𝑐 𝑘 𝑝− 𝑘

𝑎 𝑘 − 1 = 1 −3

10sin

𝑥− 𝑘

10𝑐 𝑘 = 3 𝑥− 𝑘 2

10


7.2 拡張カルマンフィルタ


• 非線形カルマンフィルタの一種• 線形化を行わない（EKFの問題点の一つを解決できる）

UKFの特徴

基本的な考え方

非線形システムの各時刻における線形近似ではなく，確率密度関数を正規分布で近似する．

標準偏差に対応するシグマポイント（σ点）と呼ばれる少数個のサンプル点を選び，集合平均的に確率分布を近似する統計的サンプリング法の一種．

非線形カルマンフィルタ～c. アンセンテッド（無香）カルマンフィルタ～

𝑥 𝑘 + 1

𝑓 𝑥

𝑥 𝑘

11


U変換の利用によるシグマポイントの算出

確率変数ベクトル𝒙 ∈ ℝ𝒏を，ある非線形関数𝑓: ℝ𝒏 → ℝ𝒏によって，確率ベクトル𝒚 ∈ ℝ𝒏に変換する問題を考える．

この変換（状態推移）で２次モーメント（分散）までの統計量を，計算量を抑えて精度良く保存するにはどうすればよいか？

𝑛次元確率密度関数の形状を少数個のサンプル点で近似

𝒚 = 𝒇(𝒙)


𝒚 = 𝒇(𝒙)

𝒚 = 𝒇( 𝒙)

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

𝑷 𝑘

𝒴𝑖− 𝑘 = ℎ 𝝌𝑖

−(𝑘)

12


シグマポイント（sigma point）を用いて平均値 𝒙に関して対称にサンプリングを行う

シグマポイント 𝝌𝑖 , 𝑖 = 0,1,2, ⋯ , 2𝑛 の選び方

𝝌0 = 𝒙

𝝌𝑖 = 𝒙 + 𝑛 + 𝜅 𝑷𝑥 𝑖, 𝑖 = 1,2, ⋯ , 𝑛

𝝌𝑛+𝑖 = 𝒙 − 𝑛 + 𝜅 𝑷𝑥 𝑖, 𝑖 = 1,2, ⋯ , 𝑛

シグマポイントの重み

𝑤0 =𝜅

𝑛 + 𝜅𝑤𝑖 =

1

2 𝑛 + 𝜅, 𝑖 = 1,2, ⋯ , 2𝑛

𝑖=0

2𝑛

𝑤𝑖 = 1ただし


𝝌𝑖は平均値と標準偏差に対応する2𝑛個の点（合計2𝑛 + 1個）のサンプルのこと

𝜅 スケーリングパラメータ（デフォルトは0）

𝑷𝑥 𝑖

共分散行列𝑷𝑥の平方根行列の𝑖番目の列

13


𝑛 × 𝑛正定値対称行列𝑨は，

のように分解することができ，これをコレスキー分解という。

行列𝑨が与えられたとき，右のアルゴリズムによって，平方根行列𝑺を求めることができる．

𝑨 = 𝑺𝑺𝑇

for 𝑖 = 1,⋯ , 𝑛

𝑆𝑖𝑖 = 𝐴𝑖𝑖 −

𝑗=1

𝑖−1

𝑆𝑖𝑗2

for 𝑗 = 1,⋯ , 𝑛

𝑆𝑗𝑖 =

0, 𝑗 < 𝑖

1

𝑆𝑖𝑖𝐴𝑗𝑖 −

𝑘=1

𝑖−1

𝑆𝑗𝑘𝑆𝑖𝑘 , 𝑗 ≥ 𝑖

next 𝑗

next 𝑖

コレスキー分解の他に，

• UD分解（UD factorization）

• 特異値分解（SVD: Singular Value Decomposition）

などがある。これらの行列分解を共分散行列に適用することによって，カルマンフィルタの

数値的安定性を向上させることができる。


コレスキー分解（平方根行列を求める）

14

初期設定

時間更新

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


𝒙 0 = E 𝒙(0) = 𝒙0

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

① シグマポイントの計算

𝝌0(𝑘 − 1) = 𝒙 (𝑘 − 1)

𝝌𝑖(𝑘 − 1) = 𝒙(𝑘 − 1) + 𝑛 + 𝜅 𝑷(𝑘 − 1)𝑖, 𝑖 = 1,2, ⋯ , 𝑛

𝝌𝑛+𝑖(𝑘 − 1) = 𝒙 − 𝑛 + 𝜅 𝑷(𝑘 − 1)𝑖, 𝑖 = 1,2, ⋯ , 𝑛

𝑤0 =𝜅

𝑛 + 𝜅𝑤𝑖 =

1

2 𝑛 + 𝜅, 𝑖 = 1,2, ⋯ , 2𝑛

𝑖=0

2𝑛

𝑤𝑖 = 1ただし

また，重みを次のように計算する．

1時刻前に得られた状態推定値 𝒙 𝑘 − 1 と共分散行列𝑷 𝑘 − 1 を用いて，2𝑛 − 1個のシグマポイントを計算

1-1

1-2


15


時間更新

② 予測ステップ

シグマポイントの更新：



𝝌𝑖− 𝑘 = 𝒇 𝝌𝑖 𝑘 − 1 , 𝑖 = 1,2, ⋯ , 𝑛

𝒙− 𝑘 =

𝑖=0

2𝑛

𝑤𝑖𝝌𝑖− 𝑘

𝑷− 𝑘 =

𝑖=0

2𝑛

𝑤𝑖 𝝌𝑖− 𝑘 − 𝒙− 𝑘 𝝌𝑖

− 𝑘 − 𝒙− 𝑘 𝑇 + 𝜎𝑣2𝒃𝒃𝑇

2-1

2-2

2-3


16


ＵＫＦ～サンプリング～

時間更新

シグマポイントの再計算：2-4

出力のシグマポイントの計算：2-5

事前出力値の計算：2-6

𝝌0(𝑘) = 𝒙− (𝑘)

𝝌𝑖−(𝑘) = 𝒙− 𝑘 + 𝑛 + 𝜅 𝑷− 𝑘

𝑖, 𝑖 = 1,2, ⋯ , 𝑛

𝝌𝑛+𝑖− (𝑘) = 𝒙− 𝑘 − 𝑛 + 𝜅 𝑷− 𝑘

𝑖, 𝑖 = 1,2, ⋯ , 𝑛

𝒴𝑖− 𝑘 = ℎ 𝝌𝑖

−(𝑘) , 𝑖 = 1,2, ⋯ , 𝑛

𝑦− 𝑘 =

𝑖=0

2𝑛

𝑤𝑖𝒴𝑖−

事前出力誤差共分散行列：2-7

𝑃𝑦𝑦− (𝑘) =

𝑖=0

2𝑛

𝑤𝑖 𝒴𝑖− 𝑘 − 𝑦−(𝑘) 2

17


時間更新

事前状態・出力誤差共分散行列：2-8

カルマンゲイン2-9

𝑷𝑥𝑦− (𝑘) =

𝑖=0

2𝑛

𝑤𝑖 𝝌𝑖−(𝑘) − 𝒙−(𝑘) 𝒴𝑖

− 𝑘 − 𝑦−(𝑘)

𝒈 𝑘 =𝑷𝑥𝑦

− (𝑘)

𝑃𝑦𝑦− 𝑘 + 𝜎𝑤

2

状態推定値：3-1

事後誤差共分散行列：3-2

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

𝑷 𝑘 = 𝑷− 𝑘 − 𝑔(𝑘) 𝑷𝑥𝑦− (𝑘)

𝑇

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


18


非線形カルマンフィルタ～c. SPKFの亜種～

Central Difference KF (CDKF)

Square-Root UKF (SRUKF)

Square-Root CDKF (SRCDKF)

M. Norgaard, N. Poulsen, and O. Ravn, “New Developments in State Estimation for Nonlinear Systems,” Automatica, vol. 36, pp. 1627–1638, November 2000.

R. van der Merwe and E. Wan, “Efficient Derivative-Free Kalman Filters for Online Learning,” in Proceedings of the 9th European Symposium on Artificial Neural Networks (ESANN), (Bruges, Belgium), pp. 205–210, Apr 2001.

R. van der Merwe and E. Wan, “The Square-Root Unscented Kalman Filter for State- and Parameter-Estimation,” in Proceedings of the IEEE International Conference on Acoustics, Speech, and Signal Processing (ICASSP), vol. 6, (Salt Lake City, UT), pp. 3461–3464, May 2001.

19

非線形カルマンフィルタ～a. 問題設定～ 離散時間非...

Documents

非線形カルマンフィルタ～a. 問題設定～離散時間非...