非線形カルマンフィルタ入門

Transcription

1 確率システムの状態推定入門 東京工業大学大学院機械制御システム専攻 山北昌毅

2 内容 最小分散推定値 Kalman Filter 線形カルマンフィルタ 拡張カルマンフィルタ Unscented Kalman Filter (UKF) 例題を用いた状態推定比較 UKF と RHC の併用例 Kalman-Bucy Filter-UKBF 状態拘束 非ガウス性外乱に対する対 混合ガウス分布 アンサンブルカルマンフィルター まとめ

3 期待値 分散 X E p( ) d ( X ) ( ) p( d E ) 次統計モーメント E ( X ) ( X ) p( ) d 次モーメント : 平均 次モーメント : 分散 3 次モーメント : 歪度 4 次モーメント : 尖度 3

4 次元 p( ) ガウス分布 ep 平均 : E 分散 : E X X 奇数次モーメント : E 偶数次モーメント : E n X 0 n n X 3 n 4

5 5 多次元 n R X X E X X p n / / ep ) ( 次元

6 次元分布 ( 相関による変化 )

7 Fact 正規分布を持つ確率変数の Affine 変換された確率変数の確率分布は正規分布となる つまり を正規性の確率変数として y を次の式で定義する y A b R, y R, AR, br n m m n m y は正規性確率変数となり平均 分散は次の式で計算される E{ y}: y AE{ } b A b E{( y y)( y y) }: E{ A( )( ) A } AE{( )( ) } A AA yy 7

8 最小分散推定値 推定したいパラメータ 評価関数 E p( y ) ˆ 観測量 y を最小にする推定値 推定ルール g(y) 推定量 ˆ 条件付き期待値 ˆ E y ( の分布の種類によらず ) と等価 8

9 9 証明 (, ) (, ) (, ) (, ) ( ) :, ( ) E f X Y f y p y ddy p y p y py dy y d p y p y f ) ( ) ( ), ( Y Y X f E E ), ( Y X Y g E E X Y g E ) ( ) ( Y X X Y E X Y E Y g E E ) ( Y X g X g E E y y y y Y X g X g E E y y y y 期待値をとると 0

10 0 続き Y X Y g E E X Y g E y y ) ( ) ( Y X E Y Y g E E y y ) ( これが最小になるのは y Y g ) ( の時 X Y E Y g ) ( が最小分散推定値となるつまり (Y) g と無関係

11 ここまでのまとめ 最小分散推定値は条件付期待値である どうやって条件付期待値を求めるのか 条件付確率密度関数が得られても計算が困難 ガウス分布を仮定すると容易に計算可能

12 ガウス分布の条件付期待値 y y yy E E E y y y y E y y y y A 0 yy y A 0 y yy ˆ E y A y y E 0 E y E y y yy A 0 y 0 より上の共分散は A y A yy A0 0 注 : ガウス分布でないとき線形最小分散推定値となる ˆ Ay b

13 証明 補題 ], is nonsingular, is nonsingular. X : Y : Z ( X Y ) Z Y X Y X.det( ) det( X)det( Z) X Y ( 証明 ) In = 0 I n 0 3

14 条件付確率密度関数 p X Ep X X ( ) (det( )) ( ) ( ( ) ( )) n / / X : n Ep( ( ) ( )) d ( ) (det( )) n/ / X p X p( X X ), X Ep( ( ) ( n n)/ / ( ) (det( )) X X X ( ) ( ) X X X X Y X X X ( X Y ) Z Y X Y X X ( )) X ( X Y ( X )) X ( X Y ( X )) ( X ) Z ( X ) 4

15 p( X ) p(, X ) d Ep( ( X ( )/ / ) Z ( X )) n ( ) (det( Z)) n / ( ) (det( n n ( ) (det( )) Ep(( X Y ( X )) X ( X Y ( X )) d Ep( ( X / / ) Z ( X )) p( X X ) p( X, X ) px ( ) X )) / Ep ( ( X Y ( X )) X ( X Y ( X ))) E{ X X } Y( X ) Var{ X X } X 5

16 確率密度関数のレベル集合との関係 ˆ ( y, ) 大きく の効果が小さく yy y y 6

17 拘束条件付ノルム最小化と直行条件 Min ˆ : e s. t. c d e, y ˆ 0 y ˆ 7

18 偏差と観測値の 直交条件 E{( ˆ ) y }? ˆ Y ( y y), Y E{( Y ( y y)) y } E{( Y ( y y))( y y y) } 0 Y Y0 0 逆にYを知らなくとも Y 0 より Y が出る 8

19 種々の手法の推定法の概略. カルマンフィルター (KF) p ( (0)) をガウス分布に従うとして 解析的に条件付期待値を計算. 拡張カルマンフィルター (EKF) 誤差の分布がガウス分布に従うと近似して 解析的に条件付期待値を計算 3. UKF( 無香料カルマンフィルター ) 状態の分布を次の統計量まで近似して 解析的に条件付期待値を計算 4. アンサンブルカルマンフィルター (EnKF) 初期状態分布に基づき代表点を生成し 各代表点の値を解析的に更新する 条件付期待値は各点より集合平均的に数値計算する 5. パーティクルフィルター ( 粒子フィルター ) 分布を代表点の数の分布で近似し 条件付確率分布を解析的に近似する 条件付確率分布に従って粒子を再サンプリングし 条件付期待値を集合平均で求める 9

20 . カルマンフィルター 種々の手法のイメージ () t.ukf t 0

21 種々の手法のイメージ (). アンサンブルカルマンフィルター t. パーティクルフィルター t

22 パーティクルフィルター n p f (, w ),, w R, y, v R y h(, v ) ( ただし v h (, y ) が存在 ). の分布に従って ( i,, N) を生成 ( 0) () i 0 0. 以下を繰り返す( ) ( i) w の分布に従ってw () i ( ii) f (, w ) を計算 ( i,, N) ( i) ( i) ( i) を生成 ( i) ( iii) p( y ) を次の式によって計算 h (, y) vp vp y dv dy ( dv : dv dv d, dy : dy dy y ) h (, y) y = p( v ) dv p( v ) dy より h (, y ) p( y ) p( h (, y )) : c ( i) y ( iv) の頻度 ( 確率 ) は代表点 ( 粒子 ) の数で表しているので つの粒子の () i 確率は全てである よってp( y) は次式で計算される N () i ( v) 条件付期待値は () i の単純平均 p ( i) ( y ) p y p c N p y p c / N ( i) ( i) ( i) ( ) ( ) / N N ( i) ( i) ( i) ( ) ( ) i i ( i) ( i) () i () i N () i c i このp( y ) に基づいて を再サンプリングする ( の分布はp( y ) の分布となる ) c

23 3 線形カルマンフィルタ 対象モデル n R ] 状態ベクトル ] ] ] ] ] ] ] ] w C y v A 観測ベクトル l y R ] 状態外乱ベクトル n v R ] 観測外乱ベクトル l w R ] ] 0 0 ] ] ] ] R Q E v w w v

24 4 線形カルマンフィルタ初期推定値とその予測誤差共分散 0 ˆ P 0 R C P C C P W ) ˆ ( ˆ ˆ C y W A ˆ ˆ. カルマンゲインを計算. 前回の予測推定値を観測値との誤差で修正 4. 推定誤差共分散行列を更新 3. 予測推定値を計算 P W C P P Q A A P P

25 証明 ] ] A ] I 0 v ] y ] C ] 0 I w ] 両辺の条件無しの期待値をとる (- 時刻までの情報による期待値 ) ˆ ] ˆ A ˆ ˆ y C ] 誤差ベクトルの同時確率密度関数の分散は以下のように計算される ] : ], y ] : y ] C ] ] A ] ] v ] P E y ] C ] ] w ] A ] A : { ] ]} ] ] ] y ] A P A Q A P C E y ] ] y ] y ] C P A C P C R ただし, P はそれぞれy j] まで観測された i j ij ときのi ] の条件付期待値及び条件付分散である ] y y ] と考える 5

26 ここでX=+],Y=y] と考えると= ˆ ˆ = Aˆ y C ˆ C P C R yy A P y として z ( y ] y) y C yy + A ˆ P C C P C R y y は ( ( ) ( ] )) zz A P A Q A P C CP C R CP A ( ) A ( P W C P ) A Q A P P C C P C R C P A Q 6

27 7 従ってゲインを次のように定義し R C P C C P W ) ˆ ( ˆ ˆ C y W A ˆ ˆ 最適な推定値は次式で与えられる P W C P P Q A A P P ˆ P を次の式で定義すると

28 8 線形カルマンフィルタ ( 入力あり ) 対象モデル m R u 入力ベクトルほとんど同様に計算できる R C P C C P W ) ˆ ( ˆ ˆ C y W P W C P P Q A A P P A B u v y C w ˆ ˆ A B u ( 条件なし期待値の計算 )

29 白色化フィルター ( イノベーションプロセス ) 元のシステム A B u v y C w 状態推定器 ˆ ˆ W ( y C ˆ ) ˆ A ˆ B u A ˆ W ( y C ˆ ) B u A ˆ B u A W ( y C ˆ ) : ˆ y C K : AW システムの別表現 ˆ ˆ A B u K y ˆ C A, B, C が一定の場合 v が白色信号であることが示せる 9

30 30 AR モデルのパラメータ推定 () 3 次の AR モデルのパラメータ推定 w y a y a y a y 3 3 a a a 3 w y y y y 3 状態空間モデル

31 AR モデルのパラメータ推定 ( ) a.76 a.539 a R.0 P0 I ˆ0 0 â â 3 â 観測回数 3

32 拡張カルマンフィルタ ] f ] u ] v (, ) ] ] ( ] ) ] y h w ( ˆ, ) ( ˆ ) ] ] f ] u ] A ] ] ] v y ] h( ˆ ] ) C ] ( ] ˆ ] ) w ] A ] f ( ) ˆ ] C ] h( ) ˆ ] 3

33 33 拡張カルマンフィルタの更新式 ), ( ˆ ), ( ˆ ˆ ] ] ] ] ] ] ] u h y W u f ] ] ] ] ] ] ] ] R C P C C P A W ] ] ] ] ] ] ] ] ] ] W R C P C W A P A P ] ] ] ] ] ] ] ] ˆ ˆ (, ) ( ) ] ˆ ˆ ] : (, ) ˆ ] ] ] ], ] : ] ] f u A v f u A v

34 予測出力を用いた非線形カルマンフィルタ () (UKF の考え方も同じ ) p(, y) の同時分布の考え方で予測出力を用いた場合 ] ] 今までは y y ] y y ] と P が既知 y と yy y g() P を推定する : ] : ˆ ] E{ ]} E{ f ( ],, v ]) P : P ] E{( ] ˆ ])( ] ˆ ]) } y : y ] y : yˆ ] E{ y ]} P : E{( y ] yˆ ])( y ] yˆ ]) } yy y ] h( ],, w ]) : g( ]) 34

35 予測出力を用いた非線形カルマンフィルタ () ] f ( ],, v ]) y ] h( ],, w ]) ˆ( ) ˆ( ) W( ) v( ) P( ) P( ) W ( ) Pvv ( ) W ( ) W( ) Py ( ) Pvv ( ) v ] y ] y ] システムを逐次線形近似 (EKF) 統計モーメントを近似 (UKF) 35

36 予測出力を用いた拡張カルマンフィルタ ] f ] u ] v y (, ) ] ] h( ] ) w ] ( ˆ, ) ( ˆ ) ] ] f ] u ] A ] ] ] v ( ( ˆ ) ]) ] y ] h f ], u ]) A( )( ] ˆ ] v w h( f ( ˆ C( ) ( ˆ ) w ], u ])) A( ) ] ] ] C( ) v ] A ] f ( ) C ˆ ] ] h ( ) f ( ˆ ], u ]) 等価外乱 36 36

37 Unscented Kalman Filter(UKF) 拡張カルマンフィルタの問題点 線形近似する際にヤコビアンを計算しなければならない ( 不連続なシステム Hard Nonlinearity) 推定値にバイアスが乗ることがある 発散することもある ( 平均値の変換は変換後の平均値になると仮定している ) システムを近似するより統計量を近似するほうが容易 数カ所のサンプル点 (Sigma Points) を選び 集合平均的に統計量を近似する と P が既知 y と yy y g() P を推定する Unscented ransformation (U 変換 ) 37

38 拡張カルマンフィルターの問題点 y y f ( ) py ( ) p'( y) p() y p ( ) 38

39 Sigma Points の考え方 n R を平均値 分散行列 の確率変数ベクトルとする に対して n+ 個の代表点 i (i=0,,,n) 離散点の生起確率を W とする ただし i i n n を考えて それぞれの の集合的統計的性質は 次の モーメントまでは一致させる, W W i i i i i i0 i=0 y g( ) g( ) i i y y n n y yˆ : W, ˆ : yˆ yˆ W i i yy yy i i i i0 i=0 39

40 Sigma Points を用いた推定の性質 ŷ. は新の平均値を 次の order まで近似 y (EKF は 次の order まで近似 ) ˆ yy yy. はを 3 次の order まで近似 ( これは EKF と同じ ) 3. はチューニングパラメータ が Gaussian の場合 n 3 と選ぶのが良い 40

41 UKF の計算手順. 適切なサンプル点 (Sigma Points) を推定値と共分散から選ぶ.Sigma Points を基に予測値 を 計算する P( ) 3. 予測共分散 P y ( ) ( ) を計算する W ( ) P yy ˆ ( ) 4. カルマンゲインを計算する y( ) 5. 観測値が得られる yˆ ( ˆ( ) P( ) ˆ ( ) 6. 推定値と共分散を更新する ) 4

42 UKF:Sigma Points X 0 W 0 ] ˆ ] κ n κ i,,,n κr X i ] ˆ ] ( n ) P ] W i ( n ) i 番目の列ベクトル i i M N M NN とすると である X i n W i n ] ˆ ] ( n ) P ] ( n ) 4

43 UKF:Sigma Points の性質 平均 分散 P n i0 n i0 n W W i n i ix i i ] ˆ ] ] ˆ ] ] ˆ ] X i X i W ( n ) P( ) P( ) i P( ) P( ) P( ) i i i 平均 分散は一致している点の集合 i 44

44 45 補足 ( ) ( ) n n i i P v v v v v v v P とすると ( ), n n n n j j j n n n n n v v P v v v v v v v v v v vv v v vv

45 UKF: 共分散行列の更新.Sigma Points を状態遷移関数で遷移させる X ˆ P i f ( X ], u ]) ] ] n i0 n i0 W i W i. 遷移させた Sigma Points の集合平均で予測平均を近似する X i ] 3. 遷移させた Sigma Points の集合分散で予測分散を近似する ] ] ˆ i X i X i ] ] ˆ ] 46

46 4.Sigma Points を観測関数で遷移させる Y i ] h( X ], u ] ) i 5. 遷移させた点の集合平均で予測観測値を近似する 6. 遷移させた Sigma Points の集合分散で予測分散を近似する P P yˆ yy y ] n i0 n i0 W i W ] ] ˆ n i0 W i Y i Y i ] ] ] ˆ i X i Y Y i i y ] ] y ˆ ] ] ] y ˆ ] 47

47 7. イノベーションの予測共分散を計算する y yˆ y v ] ] ] ] w ] P vv R P ] ] ] yy 8. カルマンゲインを計算する W ] P y ] P vv ] y 9. 観測値 ] から推定値を更新する ˆ ] ˆ ] W ] v ] 0. 予測誤差共分散を更新する P P W P vv W ] ] ] ] ] 48

48 ノイズがアフィンでない場合 上記の説明では観測ノイズが状態変数と独立で アフィンな形で加わっていた ( ノイズの影響は分散行列の和として計算可能 ) ノイズがアフィンでない場合は状態とノイズの拡大した変数を考えてシグマポイントを生成して同様の計算を行う ( ただし その分計算量が大きくなる また 状態と両ノイズに相関がないので 平均の状態にノイズが加わった形での評価となる ) ] f ( ], v ]) : fa( a ]) y ] h( f ( ], v ]), w ])) : ha( a ]) ] n p a ] : v ] R w ] ˆ ] P ] a 0, P ] a Q a 0 R ]

49 共分散行列の予測 EKF による共分散の予測 UKF による共分散の予測 50

50 EKF と UKF の比較 () 5

51 EKF と UKF の比較 () 3 4 y y l cos( ) l cos( ) n 平面 リンクマニピュレータ 009/08/4 5

52 EKF と UKF の比較 () 009/08/4 53

53 EKF と UKF の比較 () 009/08/4 54

54 状態とパラメータの同時推定 () 状態方程式 観測方程式 y(t) を観測 d ( t) a ( t) b u( t ) dt y( t) ( t) 4sin( ( t)) w( t) : 既知 a b: 未知パラメータ 状態 ( t) 同時に推定する と未知パラメータ を a b 55

55 状態とパラメータの同時推定 () 観測方程式 ( モデル ) センサの脈動を 表したモデル y 56

56 状態とパラメータの同時推定 (3) R 0 Q 0 P 0 diag ˆ パラメータ â パラメータ bˆ 57

57 状態とパラメータの同時推定 (3) 離散化の影響 y 00msec] sin( ) ˆ ˆ パラメータ â パラメータ bˆ

58 59 逐次最小二乗法 ] ˆ ] ] ] ] ] ] ] ] ˆ ] ˆ y P P y ] ] 出力 入力から計算される非線形関数ベクトル未知パラメータベクトル出力 ] ] ] ] ] ] ] ] ] P P P P P 0 ] P

59 シミュレーション結果 P ] ˆ パラメータ â パラメータ bˆ 60

60 適応オブザーバ y a A bu,0,,0 A: 既知の n( n ) 行列 a, b: 未知の n ベクトル y A ay bu h : A が漸近安定になるように決める y K h g y bu g b を推定する 6

61 6 適応オブザーバ ˆ ˆ ˆ ˆ v v bu y g K ˆ ˆ ˆ h y 0 0,, 0 e,,, v v となるようにを決める b b g g e ˆ, ˆ, ˆ ˆ ˆ ˆ, ˆ ˆ ˆ b b b g a オブザーバ誤差方程式 v v u y K e e ˆ e

62 シミュレーション結果 d 0.0 ˆ パラメータ â パラメータ bˆ 63

63 UKF を用いたバックラッシュ系のモデル予測制御 モデル予測制御 制約条件を考慮したシステマティックな制御系設計 問題点 計算時間 状態の観測 ロバストパーフォーマンス 64

64 バックラッシュ 多くの機械システムに存在する 制御性能の悪化 振動現象 バックラッシュを考慮した制御は実用上非常に重要 一般には 全ての状態が直接観測されない バックラッシュを含むシステムの状態推定 微分方程式はスムーズでない UKF の適用 バックラッシュ補償 システマティックに最適制御系を実現したい バックラッシュ系は非線形系 ( 非線形最適制御フィードバック制御 ) 非線形モデル予測制御の適用 65

65 66 モデル v y ) ( ) ( D M D u M c c 状態方程式ダンパ要素バネ要素伝達トルク ) :, ( ) : (, ) :, ( ) ( 4 3 G F G F c v: センサノイズ観測方程式 M M 回転系と直動系の両方を表現可能

66 67 モデルダンパ定数バネ定数バックラッシュ幅相対距離 0 : 0 : 0 : ) ( 0 ) ( ), ( D K B B D B K B B D B K c 伝達トルク ( 力 )

67 推定シミュレーション シミュレーション条件 ( 回転系 ) M M K 000 D 0.0 D B , D 0. 観測値 ( ノイズ : 標準偏差 0.05 の正規外乱 ) u (rad) 駆動部位置 (rad) 被駆動部位置 Nm] 入力 (±00Nm] の矩形波 ) モデルパラメータ 0. gm ] gm ] Nm/rad] Nms/rad] Nms/rad] (rad) モータの慣性モーメントアームの慣性モーメント接触トルクのばね係数接触トルクのダンパ係数シャフトの摩擦係数バックラッシュ幅 68

68 69 推定シミュレーション結果 モデル誤差無し 平均共分散 EKF を とした誤差平均 共分散の絶対値の比率 Offset EKF UKF Variance EKF UKF

69 70 推定シミュレーション結果 モデルのバックラッシュ幅に誤差が -0% 平均共分散 EKF を とした誤差平均 共分散の絶対値の比率 Offset EKF UKF Variance EKF UKF

70 7 制御則 評価関数 終端コスト関数 コスト関数 : 参照軌道との二乗誤差 (Δτ で離散化したシステムを考える ) 0 * * * * )) ( ), ( ( ) ( J N i i i N t v t L t } ) ( ) {( : ) ( ) ( : Rv v Q L S r r rn N f rn N

71 シミュレーション RHC 制御周期 0 ms] 予測ステップ 0 (00ms]) 評価関数のパラメータは PSO などを用いて探索 LQR 制御と比較 ただし LQR では 質点は常に接続状態であるとして フィードバックゲインを求める (RHC の場合の重みとは異なる ) RHC と同程度の立ち上がり時間の応答で比較 状態推定にはともに UKF を用いる 制御周期は ms] 7

72 参照軌道 制御則 目標状態までの軌道 u ] r r r r3 r 4 r 駆動側の参照軌道 被駆動部に対し接触面で相対速度 0 r sgn( r ) B r3 4 被駆動部の参照軌道 被駆動部の目標位置をステップ関数で与える r r

73 シミュレーション M M K 000 D 0.0 D B , D 0.04 観測値 ( ノイズ : 定常偏差 0.05の正規外乱 ) (rad) (rad) 駆動部位置被駆動部位置 u Nm] 入力モデルパラメータ gm ] モータの慣性モーメント gm ] アームの慣性モーメント Nm/rad] 接触トルクのばね係数 Nms/rad] 接触トルクのダンパ係数 0.0 Nms/rad] シャフトの摩擦係数 (rad) バックラッシュ幅 74

74 シミュレーション結果 被駆動部の応答と伝達トルク 被駆動部の応答 伝達トルク 75

75 シミュレーション結果 モデルのバックラッシュ幅に ±0% の誤差 被駆動部の応答 伝達トルク 76

76 77 モデルパラメータ観測値モータの位置アームの位置入力トルクモデルパラメータモータの慣性モーメントアームの慣性モーメント接触トルクのばね係数接触トルクのダンパ係数シャフトの摩擦係数バックラッシュ幅 ) ( ) ( rad rad ) ( 0. ] / , ] / 0.00 ] / 00 ] 0.05 ] rad B rad Nms D D rad Nms D rad Nm K gm M gm M Nm] u

77 シミュレーション結果 モデルパラメータ 78

78 シミュレーション結果 入力 79

79 ハイブリッド UKF 連続時間での状態の予測 + 離散時間での更新 対象のシステム d( t) fc( ( t), t) dt L( t) d( t) dy( t) hc ( ( t), t) dt V ( t) d( t) d( t) fc( ( t), t) dt L( t) d( t) y h( ( t ), t ) r 列毎の写像の変換 f : R n n X R, Y R Y i i R f ( X ) i i m m Y Y,, Y ] R, X X,, X ] R Y f( X) md d d nd 80

80 伊藤の確率微分方程式 超入門 ( t t) ( t) fc( ( t)) t L( t) ( t) o( t), ( t) R y( t t) y( t) hc ( ( t)) t V ( t) ( t) o( t), y( t) R が任意の小さな正のtについて成り立つ時 n p d( t) fc( ( t), t) dt L( t) d ( t) dy( t) hc ( ( t), t) dt V ( t) d( t) と表現する ただし ( t), ( t) は独立なブラウン運動で以下を満たす d ( t), E{ ( t)} 0, E{ ( t) ( t)} Q( t), Q( t) diag( q( t),, qn( t)) d ( t), E{ ( t)} 0, E{ ( t) ( t)} R( t), R( t) diag( r ( t),, rp( t)) ( t) ~N(0, Q( t) t), ( t) ~N(0, Rt ( ) t) o( t) lim 0 t 0 t

81 伊藤の公式 t ( ) が伊藤の確率微分方程式を満たす時 d( t) f ( ( t), t) dt L( t) d ( t) c y f ( ) とするとyの微分方程式は次式となる f f dy d d d ただし 式を展開した後次の関係を利用 dt 0, dtdi 0, did j 0( i j) di qidt ( 関数が 次の係数を持つ場合 確率的要素が確定的成分に!) 8

82 連続系の Kalman Filter() P( t) : E{ ( t) ( t)}, ( t) : ( t) ˆ ( t) ˆ ( t t) : ˆ ( t) f ( ˆ c ( t), t) ( t 状態の予測値 ) yˆ ( t t) : h ( ˆ c ( t t), t t) ( t 出力の予測値 : 変化分 ) P ( t t) : E{( ( t t) ˆ ( t t))( ( t t) ˆ ( t t)) (} 予測の共分散 ) S( t t) : E{( y( t ) yˆ ( t t))( y( t ) yˆ ( t t)) } C( t t) : E{(( ( t t) ˆ ( t t))( y( t ) yˆ ( t t)) } K t t C t t S t t ( ) : ( ) ( ) ˆ ( t t) ˆ ( t t) K( t t)( y( t) yˆ ( t t)), y( t) : y( t t) y( t)

83 連続系の Kalman Filter() P ( t t) : E{( ( t t) ˆ ( t t))( ( t t) ˆ ( t t)) } E{( ( t) ˆ ( t) ( f ( ( t), t) f ( ˆ ( t), t)) t L( t) ( t) o( t))( ( t) ) } c t () f () t c c E{ ( t) ( t)} E{ ( t) f ( t) f ( t) ( t)} t E{ ( t)( L( t) ( t)) L( t) ( t) ( t)} E{ L( t) ( t)( L( t) ( t)) } o( t) c c 0 P( t) E{ ( t) f ( t) f ( t) ( t)} t L( t) Q( t) L ( t) t o( t) c c S( t t) : E{( y( t ) yˆ ( t t))( y( t ) yˆ ( t t)) } y( tt) E{(( h ( ( t), t) h ( ˆ ( t), t)) t V ( t) ( t) o( t))( h ) } c c c h () t V ( t) R( t) V ( t) t o( t) c C( t t) E{ ( t t) y( t t) } E{ ( t t) h ( t)} t o( t) c 84

84 連続系の Kalman Filter(3) K t t C t t S t t ( ) ( ) ( ) ( E{ ( t t) h ( t)} t o( t))( V ( t) R( t) V ( t) t o( t)) c o( t) o( t) ( E{ ( t t) hc ( t)} )( V ( t) R( t) V ( t) ) t t o( t) E{ ( t t) hc ( t)}( V ( t) R( t) V ( t)) t K( t) lim K( t t) E{ ( t t) h ( t)}( V ( t) R( t) V ( t)) t 0 c ˆ ( t t) ˆ ( t t) K( t t)( y( t) yˆ ( t t)) ˆ ( t) f ( ˆ ( t), t) t K( t t)( y( t) yˆ ( t t)) c dˆ( t) f ( ˆ c ( t), t) K( t)( z( t) hc( ( t), t)), z( t) : dt dy dt 85

85 連続系の Kalman Filter(4) P( t t) P ( t t) K( t t) S( t t) K ( t t) P( t) E{ ( t) f ( t) f ( t) ( t)} t L( t) Q( t) L ( t) t K( t t) S( t t) K ( t t) o( t) c c dp() t E t f t f t t t L t Q t L t t K t V t R t V t K t dt { ( ) c( ) c( ) ( )} ( ) ( ) ( ) ( )( ( ) ( ) ( )) ( ) 86

86 連続系の LI システムの Kalman Filter d( t) ( A( t) Bu( t)) dt L( t) d ( t) dy C( t) dt V ( t) d( t) E{ ( t) f ( t) f ( t) ( t)} E{ ( t)( A( t)) A( t) ( t)} c P( t) A AP( t) E{ ( t) h ( t)} E{ ( t)( C( t)) } P( t) C K( t) P( t) C ( V ( t) R( t) V ( t)) c c dp() t P( t) A AP( t) L( t) Q( t) L ( t) K( t)( V ( t) R( t) V ( t)) K( t) dt dp() t P( t) A AP( t) L( t) Q( t) L ( t) P( t) C V ( t) R( t) V ( t) K( t) CP( t) dt 87

87 次形式最適制御との双対性 () Kalman Filter 対象のシステム d( t) ( A( t) Bu( t)) dt L( t) d ( t) dy C( t) dt V ( t) d( t) 誤差の共分散行列の更新式 dp() t P( t) A AP( t) L( t) Q( t) L ( t) P( t) C V ( t) R( t) V ( t) K( t) CP( t) dt 次形式最適制御問題対象のシステム d() t A( t) Bu( t) dt 次形式評価関数 J ( ) 0 Ricatti方程式 ( t) Q( t) u ( t) Ru( t) dt dp() t dt P t A A P t Q P t BR B P t ( ) ( ) ( ) ( )

88 次形式最適制御との双対性 () 誤差の共分散行列の更新式 dp() t P( t) A AP( t) L( t) Q( t) L ( t) P( t) C V ( t) R( t) V ( t) K( t) CP( t) dt Ricatti方程式 dp() t P t A A P t Q P t BR B P t dt ( ) ( ) ( ) ( ) 双対関係制御 A B 推定 Q L( t) Q( t) L ( t) R V ( t) R( t) V ( t) A C

89 行列表現の U w W0, W,, W n] W ( I w,, w]) diag( W0,, Wn )) ( I w,, w]) c n Pˆ Pˆ n m,, m] c0, P, P], ˆ w g( ), yˆ w W W 90

90 証明 ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ, : i i n n n n n i n n i W W W W W w w W W I w w I w w W W W I w w W W P n I w w W

91 行列表現の UKF アルゴリズム 予測 ( ) ˆ ( ),, ˆ ( )] c0, P( ), P( )] ( ) f ( ( ), ) ˆ( ) ( ) w P( ) ( ) W ( ) Q( ) 更新 ( ) ˆ ( ),, ˆ ( )] c0, P( ), P( )] ( ) h( ( )) yˆ( ) ( ) w P ( ) ( ) W ( )] R( ) P ( ) ( ) W ( )] W ( ) P ( ) P ( ) ˆ ( ) ˆ ( ) W ( )( y( ) yˆ ( )) P( ) P( ) W ( ) P ( ) W ( ) 9

92 連続系の UKF アルゴリズム () 一般の式で次の近似を用いる (UKBF) E{ ( t) fc ( t)} X ( t) Wfc ( X ( t), t) E{ ( t) hc ( t)} X ( t) Whc ( X ( t), t) アルゴリズム dm() t f ( X ( t), t) w K( t) z( t) h( X ( t), t) w] dt dp() t X ( t) Wf ( X ( t), t) f ( X ( t), t) WX ( t) L( t) Qc( t) L ( t) K( t) V ( t) Rc( t) V ( t) K ( t) dt K( t) X ( t) Wh ( X ( t), t) V ( t) Rc ( t) V ( t))] X ( t) m( t),, m( t)] c0, P( t), P ( t)] 93

93 ハイブリッド UKF アルゴリズム R () t c dm() t f ( X ( t), t) w dt dp() t X ( t) Wf ( X ( t), t) f ( X ( t), t) WX ( t) L( t) Qc ( t) L ( t) dt 更新のアルゴリズムは離散の場合と同じ 94

94 領域拘束を考慮した状態推定 ( ) f ( ( ), u( ), w( ), ) y( ) h( ( ), v( ), ) a( ) ( ) b( ), ( ) R ns i ( ) ( ) ai ( ), ci if i ( ) ( ) ai ( ) No constraint, ci 0 if ai ( ) i ( ) ( ) bi ( ) i ( ) ( ) bi ( ), ci if bi ( ) i ( ) ( ) 95

95 runcated UKF PDF の打ち切り (shimada,98) 拘束なしの PDF UKF への応用 拘束ありの PDF ˆ( ), P( ) ( ), P( ) ˆ ( ( ), ( ) P ( ( ), ( )) PDF truncation 通常の UKF P ( ˆ ( ), P( ) Generate Sigma points UKF Update 96

96 Step : Initialization Step UKF Based Proposal method Constrained Unscented Gaussian Sum Filter Approimate を混合ガウス分布で近似する H terms Process noise I terms Measurement noise Gaussian sum approimate EM-Algorithm G terms 009//6 IEEE CDC

97 Step : runcated Unscented Kalman Filtering UKF Based Proposal method Constrained Unscented Gaussian Sum Filter Calculate constrained Gaussian PDF by PDF truncation for each Gaussian PDFs. unconstrained Gaussian PDF PDF truncation constrained Gaussian PDF H terms I terms Process noise Measurement noise PDF truncation PDF truncation G terms PDF truncation 009//6 IEEE CDC

98 Constrained Unscented Gaussian Sum Filter() GHI 個の UKF を異なるノイズによって並列に計算 GHI 個のガウス分布が得られる are derived H terms I terms Process noise Measurement noise PDF truncation ime update Measurement update PDF truncation ime update Measurement update G terms PDF truncation ime update Measurement update GHI UKFs 99

99 Constrained Unscented Gaussian Sum Filter() 推定値はそれらの平均値の重み付平均値で求める H terms I terms Process noise Measurement noise PDF truncation ime update Measurement update PDF truncation ime update Measurement update G terms PDF truncation ime update Measurement update Miture GHI terms 00

100 Constrained Unscented Gaussian Sum Filter(3) 数値的計算量の指数的増大の問題 : 混合する分布の数が指数的に増大する 回目の推定 回目の推定 3 回目の推定 N 回目の推定 GHI GHI HI GHI (HI) GHI (HI) n- 計算量を抑えるために pruning ( 枝狩り ) を行う一定以下の重みを持つ要素を捨てる 例 ) Constrained UKF Constrained UKF 次の推定に用いる Constrained UKF 0.03 この要素は捨てる 0

101 アンサンブルカルマンフィルター (EnKF) 予測 ( i) ( i) ( i) ˆ f ˆ u w ˆ ( ) ( ( ), ( ), ( ), ) ( ) ( ˆ ( ), ( ), ), (,, ) ( i) ( i) ( i) y h v i N 更新 N () i ˆ( ) ( ) N i N () i yˆ ( ) yˆ ( ) N i P y y P y y y y N ( i) ( i) ( ) { ˆ( ) ˆ ( )} { ˆ( ) ˆ ( )} N i N ( i) ( i) ( ) { ˆ( ) ˆ ( )} { ˆ( ) ˆ ( )} N i K( ) P ( )( P ( )) ( i) ( i) ( i) ˆ ( ) ˆ ( ) K( )( y( ) yˆ ( )) 0

102 計算の簡略化 : 粒子のプロジェクション 状態拘束を考え pdf truncation を用いると計算量が増大する 分布のパラメータを修正するより 粒子の方を修正する ( ) min ( ( ) ˆ ( )) I( ( ) ˆ ( )) ( i) ( i) ( i) ( i) ( i) ( i ) ( ) s.t. () i ci ( i ( ) ( ) ai ( )) 0 c b i s ] () i i ( i ( ) ( ) i ( )) 0)(,, ) 03

103 数値例 () 対象の非線形システム 追跡したい軌道 幅 m] ノイズの性質 mean variance 状態拘束 04

104 数値例 () 50 回のモンテカルロシミュレーションの結果 評価指標 計算時間 推定精度 CPU time アルゴリズム UKF CUGSF CEnKF(50) E-CEnKF(50) E-CEnKF(00) RMSE of m] RMSE of m] ime ms] (50),(00) は粒子数を表す 非ガウス性ノイズを陽に仮定する CUGSF, CEnKF E-CEnKF は UKF よりも良い推定精度を持つ EnKF を基本とするアルゴリズムは CUGSF よりも良い推定を与える E-CEnKF は CEnKF の推定精度は同程度であるが非常に高速である 05

105 数値例 (3) UKF 高速であるが非ガウス性ノイズをどの程度扱えるか? 観測ノイズだけを 0.8 から.6 ま 0., 刻みで変えて 50- 回のモンテカルロシミュレーションを行う UKF は R を増加させると性能が非常に悪くなる CUGSF,E-CEnKF は性能の劣化は小さい 06

106 数値例 : 同時推定 () 状態とパラメータの一部を同時に推定する問題を考える 状態と同時にパラメータ b も推定する 公称値 :0.5, 不確定性の範囲 :±0% 拡大系を考える X 3 の状態拘束 注意 : フィルターの安定性を確保するために観測方程式を修正している 07

107 数値例 : 同時推定 () 00 ステップのパラメータ推定結果 ( 横軸 : 真値 縦軸 : 推定値 ) RMSE of RMSE of UKF CUGSF ECEnKF E-CEnKF が状態だけでなくパラメータに関しても良い推定を与えている 08

108 まとめ 本講義では 線形システムに対する状態推定の基本と非線形システムに対する状態推定手法を UKF UKBF を中心に解説した 非ガウス性のノイズや状態拘束に対する対処についても説明した その応用例を数値シミュレーションにより示した 実際の適用に当たっては パーティクルフィルターなど他の手法との比較 組み合わせが重要である 09

109 その他のアプリケーション EnKF を無駄時間観測や観測順序が乱れた信号からの推定 (IEEE Aero-space Conference, 0) U 変換を利用した確率システムの制御 (SICE 第 回プラントモデリングシンポジウム,0) 0

110 参考文献 () 片山徹著 : 新版応用カルマンフィルタ朝倉書店 A.H.Jazwinsi: Stochastic Processes and Filtering heory, New Yor:Academic, 970 G.C.Goodwin and K.S.Sin :ADAPIVE FILERING PREDICION AND CONROL, PRENICE-HALL (984) C.Chui and G.Chen: Kalman Filtering with Real-ime Applications, 4th ed., Springer (009) S.J.Julier and J.K.Uhmann :A New Method for the Nonlinear ransformation of Means and Covariances in Filters and Estimators,IEEE rans.autom.contr. Vol.45,No.3 (000).Lefebvre,et.al Comment on A New Method for the Nonlinear ransformation of Means and Covariances in Filters and Estimators S.J.Julier and J.K.Uhmann A General Method for Approimating Nonlinear ransformation of Probability Distributions Online]996 E.A.Wan and R. Merwe : he Square-Root Unscented Kalman Filter for State and Parameter Estimation, Proc. Of Int. Conf. on Acoustics, Speech, and Signal Processing (00) S.Julier and J. Uhlmann : Unscented Filtering and Nonlinear Estimation, Proceedings of he IEEE, Vol. 9, No. 3, (004) M.Yamaita et. al. : Comparative Study of Simultaneous Parameter-State Estimations, Proc. of CCA 004 (004) 山北 :UKF って何?,, システム制御情報学会 (006) M.Saito, M.Yamaita: MPC for a Simplified ransmission Model with Baclash Using UKF, Proc. of CCA006, pp.57/53 (006) S.Sara: On Unscented Kalman Filtering for Sate Estimation of Continuous-ime Nonlinear Systems, IEEE rans. Autom Contr., Vol.5, No.9 (007)

111 参考文献 () S.Ishihara, M.Yamaita: Efficient Unscented Filtering for Nonlinear Systems with State Constraints, Proc. of ECC09 (009) S.Ishihara, M.Yamaita: Constrained State Estimation for Nonlinear Systems with non-gaussian Noise, Proc. of CDC09 (009) 石原新士 山北昌毅 : 非ガウス雑音を受ける領域拘束付き非線形システムの状態推定, 電気学会論文誌 C, Vol. 9,No. (009) D.Simon and L.China: Kalman filtering with state equality constraints, IEEE rans. On Aerospace and Electronic Systems, vol. 38, No., pp.8/36 (00)