パーティクルフィルタ 理論と特性
11.1 パーティクルフィルタの理論的導出 状態遷移とマルコフ性 p x k x 1:k 1, y 1:k 1 = f x k x k 1 p y k x 1:k, y 1:k 1 k = 0,1, = h y k x k x 1:k x 1, x 2,, x k y 1:k y 1, y 2,, y k 確率分布で表現される現時刻の状態が, 前時刻までの状態と観測の条件付き確率によって定まる. 特に, 前時刻のみの状態と観測によって現時刻が表現される場合この過程がマルコフ性を持つという. system model x k ~f x k x k 1 measurement model y k ~h y k x k
11.1 パーティクルフィルタの理論的導出 状態推定とは? 観測時系列 y 1:k が与えられた下で, 状態の事後分布 p x 1:k y 1:k を求めること. パーティクルフィルタ 粒子の遷移を決める分布 システムモデル, 観測モデル, プロポーザル分布に基づき多数の粒子 ( 状態の仮説 ) を時間遷移させながら状態の事後分布 p x 1:k y 1:k を求めるフィルタ. 粒子とは? ひとつの粒子は状態空間内のベクトルであり, スカラの重みを持つ. 多数の粒子を用いて, 状態の仮説の分布 ( 確率分布 ) を表現する. x k (i), wk (i) M i=1 ~p x1:k y 1:k
11.1 パーティクルフィルタの理論的導出 求めたい事後分布 p x 1:k y 1:k = p x 1:k, y k y 1:k 1 p y k y 1:k 1 p a b, c = p a, b c p b c = p x 1:k 1 y 1:k 1 p x k, y k x 1:k 1, y 1:k 1 p y k y 1:k 1 p a, b c = p b a, c p a c = p x 1:k 1 y 1:k 1 p x k x 1:k 1, y 1:k 1 p y k x 1:k 1, y 1:k 1 p y k y 1:k 1 システムモデル 観測モデル = p x 1:k 1 y 1:k 1 f x k x k 1 h y k x k p y k y 1:k 1
11.1 パーティクルフィルタの理論的導出 p x 1:k y 1:k q x 1:k y 1:k = p x 1:k 1 y 1:k 1 f x k x k 1 h y k x k p y k y 1:k 1 q x 1:k y 1:k プロポーザル分布 q x 1:k y 1:k = q x 1:k 1 y 1:k 1 q x k x k 1, y 1:k = p x 1:k 1 y 1:k 1 q x 1:k 1 y 1:k 1 f x k x k 1 h y k x k p y k y 1:k 1 q x k x k 1, y 1:k 重み w k w k x 1:k p x 1:k y 1:k q x 1:k y 1:k p y k y 1:k 1 : 全ての観測は同確率で出現すると仮定 w k w k 1 f x k x k 1 h y k x k q x k x k 1, y k
11.2 パーティクルフィルタの手順 1 予測 (i) (i) x k ~q xk x k 1, yk 推定したい分布 p(x) 2 重み更新 (i) (i) w k wk 1 3 w k (i) (i) (i) f x k xk 1 リサンプリング h y k x k (i) q x (i) k x (i) k 1, y k w k (i) M (i) i=1 w k 多数の粒子を用いて離散近似 x x k (i) ~ (1) x k with prob. (1) w k with prob. x k (M) w k (M) x
11.2 パーティクルフィルタの手順 k 0 1. 初期化 ( ) パーティクルフィルタの中で最も簡単なモンテカルロフィルタ 初期分布 p(x 0 ) に従って M 個の粒子 x 0 (i) i = 1,2,, M を無作為に発生させ k 1 とする. 2. 一期先予測 ( k 1 ) (i) (i) 粒子 x k 1 をf x k x k 1 に従って状態推移させ, 粒子集合 x k (i) i = 1,2,, M を発生させる. 3. ろ波 3.1 尤度計算 粒子 x k (i) の尤度 w k (i) = h yk x k (i) を計算する. f x k x k 1 = q x k x k 1, y k 3.2 重みの正規化 w k (i) 3.3 リサンプリング i=1 M w k (i) w k (i) (i) (i) 粒子 x k を wk に従った確率でリサンプリングし (i) 粒子集合 x k i = 1,2,, M を発生させる. 3.4 時刻更新 k k + 1 として 2. に戻る.
11.3 パーティクルフィルタの特長と利用の注意点 計算量 計算量削減の工夫の一つとして, 現時刻で過去の粒子を再利用する. 過去に㴑って計算しない為, 計算量を粒子数のオーダに抑えることができる. 計算量のオーダは O M であり, 時間推移に対して一定である. ESS (effective sample size) ESS = 1 l=1 M (l) w 2 k ESS = M : すべての粒子の重みが等しい場合 ( つまりすべての粒子が均等に利用されている場合 ) ESS = 1 : ひとつの粒子のみが非零の値を持つ場合 ( つまり一つの粒子のみが利用されている場合 ) ESS にしきい値を設定し, リサンプリングを行うタイミングを決定する
11.3 パーティクルフィルタの特長と利用の注意点 対数計算による高速化とアンダーフローの回避 実際に PF の重みの計算で必要なのは尤度そのものではなく尤度の比であることを考慮すれば, 以下のようにしてアンダーフローの影響を回避できる. ガウス分布などの場合に限られる. 1 2 全粒子のうち最大の対数尤度 l (K) を選定 ζ (i) = exp(l i l (K) ) 3 w (i) = ζ (i) ζ (i)
11.3 パーティクルフィルタの特長と利用の注意点 対数尤度の例 ( 尤度関数がガウス分布の場合 ) l = p(x 1 ) p x 2 p x n = i=1 n p(x i ) l = 1 2πσ exp (x 1 μ) 2 2 2σ 2 1 2πσ exp (x n μ) 2 2 2σ 2 = 1 2πσ 2 n exp i=1 n (x i μ) 2 2σ 2 log l = log p(x 1 ) + log p(x 2 ) + + log p(x n ) = log l = log 1 2πσ 2 n exp i=1 n (x i μ) 2 2σ 2 = n log 2π n log 2 2 σ2 1 log 2σ 2 i=1 n (x i μ) 2 n i=1 p(x i )
11.4 パーティクルフィルタの特性値外乱 目標値発生器 + 操作量 制御対象 状態量 観測 出力 + オブザーバ + 観測が困難な状態量を推定する機構 推定状態量が真の状態量に漸近する レギュレータ 推定状態量 観測ノイズ 11
11.4 パーティクルフィルタの特性値 外乱 w() t 目標値発生器 r() t + 操作量 制御対象 状態量 u () t x() t x( t) Ax( t) Bu( t) y( t) Cx( t) 出力 y() t 同一次元オブザーバ xˆ ( t) Axˆ ( t) Bu( t) k( yˆ y) yˆ( t) Cxˆ( t) + + 線形システムに対しては構成が容易 最適オブザーバはカルマンフィルタと一致する 推定状態量 xˆ( t) v() t 観測ノイズ F レギュレータ 12
11.4 パーティクルフィルタの特性値 外乱 w() t 目標値発生器 r() t + 操作量 制御対象 状態量 u () t x() t x( t) f ( x( t), u( t)) y( t) h( x( t)) 出力 y() t + 拡張カルマンフィルタ,UKF + 非線形システムに対しては構成が可能 ノイズは正規性を仮定する 多峰性確率分布は正確に表現できない F 推定状態量 xˆ( t) レギュレータ v() t 観測ノイズ 13
11.4 パーティクルフィルタの特性値 外乱 w() t 目標値発生器 r() t + 操作量 制御対象 状態量 u () t x() t x( t) f ( x( t), u( t)) y( t) h( x( t)) 出力 y() t 非線形システムであっても構成が容易 不確定要素の多いロボットの状態推定に向いている パーティクルフィルタ x k (i), wk (i) M i=1 特性値抽出器 ~p x1:k y 1:k 推定状態量 xˆ( t) + + v() t 観測ノイズ F レギュレータ 14
11.4 パーティクルフィルタの特性値 問題 パーティクルフィルタの推定結果から決定論的な特性値を抽出するには? PF の柔軟な近似能力が新たな問題を引き起こす 従来の解決方法 1. 2. 最大事後確率 (MAP) を利用する 粒子の重みつき平均値 (MMSE) を利用する
11.4 パーティクルフィルタの特性値 唯一の値 ( 特性値 ) の導出方法 ( 尤度評価の後 リサンプリングの前に計算する ) MMSE (minimum mean square error) estimation 最小平均自乗誤差推定 x k MMSE = M i=1 w k (i) xk (i) MAP (maximum a posteriori) estimation 最大事後確率推定 x MAP 1:k = argmax p x 1:k y 1:t x 1:k
11.4 パーティクルフィルタの特性値 唯一の値 ( 特性値 ) の導出方法 ( 尤度評価の後 リサンプリングの前に計算する ) MAP (maximum a posteriori) estimation ML (maximum likelihood) estimation x k MAP = max x k (i) EP-VGM (end point Viterbi-Godsill MAP) estimation x MAP 1:k = argmax (i) 1 k x k (i) ;i=1,2,,m j=1 x 1:k k log h y k x k + log f x k x k 1 pf-map (maximum a posteriori) estimation x k MAP = argmax x k (i) h y k x k (i) M j (i) (j) f x k xk 1 (j) w k 1
11.4 パーティクルフィルタの特性値 x MMSE(Minimum Mean Squared Error) x k MMSE = M i=1 w k (i) xk (i) MAP(Maximum A Posteriori) x MAP k n = argmax p x k y 1:n x k
11.5 研究内容の紹介 テスト関数 1 1 1 T 1 1 1 T 1 yk exp 12 μ1, k μ1, k exp μ2, k μ2, k 2 2 2 2 2 μi, k x Ai sin( k /180) diag(0.016 0.016) A1 diag(0.3 0.3) A2 diag(0.14 0.14) 二つのガウス分布が時間とともに移動する
11.5 研究内容の紹介 システムモデル x x d v d k k 1 k k k k ( k 1) sin sin 180 180 k ( k 1) sin sin 180 180 v N k (0, ) diag(0.016 0.016) f ( x x ) k k 1 観測モデル h y k x k = exp x k target xk 2 2σo 2 点は粒子を表す 粒子数は 2000.
11.5 研究内容の紹介 1. 最大事後確率を利用 2. 粒子の重みつき平均値を利用
11.5 研究内容の紹介 1. 最大事後確率 (MAP) を利用する 一様分布の場合, 観測信号に加わる外乱によって最大尤度を持つ粒子の位置が振動する 粒子の分布 MAP 推定値 分解能の低いセンサ信号 領域検出問題 多峰性分布の場合, 推定結果が複数の粒子クラスタ間を頻繁にジャンプする 複数センサ情報の統合において矛盾する情報が生じる場合 反射波の混入 (GPS, ソナー ) 複数の可能性を保持する必要がある場合 (SLAM, 環境変化への適応 )
11.5 研究内容の紹介 2. 粒子の重みつき平均値 (MMSE) を利用する 粒子が複雑な分布を形成する場合には望まない出力が得られる 粒子の分布 全粒子の重みつき平均値 オクルージョン ( 遮蔽 ) の存在 非線形システム 複数センサ情報の統合において矛盾する情報が生じる場合 反射波の混入 (GPS, ソナー ) 複数の可能性を保持する必要がある場合 (SLAM, 環境変化への適応 ) 特性値の導出過程で 粒子が持つ情報の多くが棄却される
11.5 研究内容の紹介 PF の柔軟な近似能力が新たな問題を引き起こす 問題 パーティクルフィルタの推定結果から決定論的な特性値を抽出するには? 解決方法 対象の確率分布の形状に関する情報を抽出し制御系を適応的に調整する
11.5 研究内容の紹介 ~ 確率分布の形状推定 ~ 対象の確率密度分布を多数の粒子を用いて離散近似 適応ベクトル量子化 (CRL) による粒子の情報の圧縮 粒子分布の形状や分布密度情報の抽出
11.5 研究内容の紹介 ~ ベクトル量子化 ~ 入力ベクトル集合を有限数の荷重ベクトル集合へ写像する 荷重ベクトルを用いて入力ベクトルを再構築した時の歪を測る 入力ベクトル空間 ( m) x k ( n) w k ( n) V k 入力ベクトル ( m) l x xk R ( m 1,, M) 時刻 k における確率密度 pk ( x) に従って発生するベクトル ( 提案手法では粒子 ) 荷重ベクトル ( n) l wk R ( n 1,2,, N) ベクトル量子化器が持つ有限個の記憶装置 ボロノイ領域 ボロノイ図 ( n) ( n) ( o) k xk xk wk xk wk, V o n 各荷重ベクトル ( n) w k の担当領域
11.5 研究内容の紹介 ~ ベクトル量子化 ~ 歪と部分歪 N 2 N 1 ( n) ( n) k ( n) k k k ( )d V k N k n 1 n 1 ボロノイ領域 D x w p x x D の部分歪 ( n) V k D D k ( n) k 最小化 均一化 これらを同時に満たすものが最適ベクトル量子化器となる ( 等歪み原理 ) ベクトル量子化手法 K-means 法 LGB 法 LVQ 法など多数の手法が存在 従来の VQ アルゴリズムの多くは勾配法に基づくため収束が遅く, 初期状態に依存して局所解に陥る 適応ベクトル量子化手法 : CRL(competitive re-initialization learning) CRL は再初期化処理によって荷重ベクトルを適応的に再配置するため収束が高速であり 初期状態に依存せず局所解を回避することが可能
11.5 研究内容の紹介 ~ 時変多峰性確率分布の形状推定 ~ テスト関数 1 1 1 T 1 1 1 T 1 yk exp 12 μ1, k μ1, k exp μ2, k μ2, k 2 2 2 2 2 μi, k x Ai sin( k /180) diag(0.016 0.016) A1 diag(0.3 0.3) A2 diag(0.14 0.14) 二つのガウス分布が時間とともに移動する
11.5 研究内容の紹介 ~ 時変多峰性確率分布の形状推定 ~ ボロノイ図 荷重ベクトル ボロノイ領域 面積が小さなボロノイ領域 CRL の荷重ベクトル数は任意に設定可能 CRL は PF の粒子の大幅な再配置に対して効率よく対応する ボロノイ領域の体積の逆数によって粒子の密度を知ることができる
11.5 研究内容の紹介 ~ 時変多峰性確率分布の形状推定 ~ ボロノイ図 荷重ベクトル ボロノイ領域 面積が小さなボロノイ領域 CRL の荷重ベクトル数は任意に設定可能 CRL は PF の粒子の大幅な再配置に対して効率よく対応する ボロノイ領域の体積の逆数によって粒子の密度を知ることができる
11.5 研究内容の紹介 ~ 時変多峰性確率分布の形状推定 ~ ドロネー図 荷重ベクトル ドロネー線. 一定以下の長さの線分のみを表示. ドロネー線はボロノイ境界の垂直 2 等分線であり ドロネー図とボロノイ図は双対の関係にある. ドロネー図より粒子の分布の形状を知ることができる
11.5 研究内容の紹介 ~ 時変多峰性確率分布の形状推定 ~ ドロネー図 荷重ベクトル ドロネー線. 一定以下の長さの線分のみを表示. ドロネー線はボロノイ境界の垂直 2 等分線であり ドロネー図とボロノイ図は双対の関係にある. ドロネー図より粒子の分布の形状を知ることができる
11.5 研究内容の紹介 ~ 時変多峰性確率分布の形状推定 ~ 前処理 1HSV 変換 2 色相値が 100~120 の画素を抽出し 2 値化 3 膨張収縮処理によりノイズを除去 入力画像 前処理画像 パーティクルフィルタ適用画像ボロノイ図ドロネー図
11.5 研究内容の紹介 ~ 時変多峰性確率分布の形状推定 ~ preprocessing 1. HSV transform 2. extract pixels with hue value from 100 to 120 3. banalization 4. Erosion and dilation for noise reduction. Input image Preprocessing image Particle filtering image Voronoi image Delaunay image