【基礎】カルマンフィルタ・パーティクルフィルタとは？(逐次モンテカルロ法の解説もあるよ)

2024/08/25

Category : 制御工学

はじめに

　本記事は時系列データにおけるフィルタリング技術全般を扱います。より詳細に理論的、厳密にカルマン・パーティクルフィルタを学ばれたいという方を対象としています。

　また本記事は”Beyond the Kalman Filter: Particle Filters for Tracking Applications”という本を参考に作成しており、この本を進行形でお読みになられている方にとっても参考になると思います。

　時系列解析はマーケッティングや製造、セキュリティ、気象予報といった分野での応用が期待されています。 データ解析をなさるという方にもお役立ちすると思います。

　本記事に限らず、当サイトのブログ記事には参考文献をつけております。 参考文献は私自身が学びやすいなと思ったサイトでもありますので、よろしければそちらも参照してみるとよろしいかと思います。

　前提として理工系学部4年生以上としていますが、初学者の方でもわかりやすいように記述しました。

こちらもどうぞ

実装を急ぐという方はパーティクルフィルタについての記事であればこちらにありますので、こちらもご参照ください。

また、カルマンフィルタの初学者向けに実装を踏まえつつ解説した記事もありますので、こちらもどうぞ。

ロボットの実装に使う方へ

カルマンフィルタやパーティクルフィルタで検索をかけるとロボットの作成における自己位置推定タスク目的という方が多く見受けられます。 本記事では時系列データの分析のためのフィルタリングを解説していますが、ロボットの位置というものもまた時系列データと言えます。

GNSSやLiDARといったセンサによる観測値とロボット自身の状態遷移モデルの２つを設定することで、本記事にて紹介されるフィルタの技術の応用が叶うでしょう。

実装をメインで紹介するわけではありませんが、どうぞお付き合い下さると幸いです。

ノイズの種類とその対処

時系列データには様々なノイズが乗ります。 ノイズの種類に対して、適切な手法を選択することが大切です。

以下はノイズの種類と対処についての簡易的な表です。

今回紹介するカルマンフィルタとパーティクルフィルタは共に再帰形フィルタ(より厳密にはベイジアンフィルタ)に分類される手法です。

もしも微小なノイズや欠損値にお困りでしたら、また違った手法をおすすめします。(もちろんベイジアンフィルタを用いた平滑化も可能ですが、何しろ実装が大変ですので…)

構成

まずはカルマンフィルタ・パーティクルフィルタに共通する基本的な考え方である”再帰形フィルタ”について紹介します。

その次にカルマンフィルタについて解説したのち、パーティクルフィルタのもとである逐次モンテカルロ法について解説します。

最後にパーティクルフィルタについて解説しますが、殆どが逐次モンテカルロ法の焼きましであり、実態はモンテカルロ法です。 そのため逐次モンテカルロ法がメインに扱われています。

再帰形フィルタ(Recursive Filter)

予想分布と観測方程式

まず状態変数を $\mathbf{x_k} \in \mathbb{R}^{n_x}$ と表現します。ただし$n_x$ は自然数で $\mathbf{x_k}$ の次元数を示し、$\mathbb{R}$ は実数集合を、$k \in \mathbb{N}$ は時系列データの連番を表します。

状態遷移

ただし$f_k$ は状態 $\mathbf{x_k}$ の関数で、予測分布とも呼ばれ、$\mathbf{v_{k-1}}$ はノイズを表す。

一般に、プロセスが進むにつれて推定精度が低下するのは、このノイズの影響によるものです。

観測方程式

パーティクルフィルタに代表される予測（prediction）と更新（update）から構成されるフィルタリングを、一般に再帰形フィルタリングと呼びます。更新には観測値 $\mathbf{z_k} \in \mathbb{R}^{n_z}$ を使用し、次のように表現されます。

ただし$h_k$ は既知の関数で、$\mathbf{w_k}$ はノイズ、$n_z$は観測値$\mathbf{z_k}$の次元数。

この方程式を観測方程式と呼びます。

なお、$\mathbf{v_{k-1}}, \mathbf{w_k}$ といったノイズがそれぞれ独立で確率的である場合、これらは白色ノイズとして扱うのが一般的です。もちろんノイズに特定の傾向が見られる場合は、それに応じた適切なノイズモデルを導入することが望ましいでしょう。

推定値

本稿では $X_K := \mathbf{\{x_0, x_1, \cdots, x_k\}}$、$Z_k := \mathbf{\{z_0, z_1, \cdots,z_k\}}$ と定義します。

さて、我々は観測値からシステムモデルの状態変数を推定するため、$p(\mathbf{x_k} | Z_k)$を導出する必要があります。 $p(\mathbf{x_0}) := p(\mathbf{x_0} | Z_0)$として、$p(\mathbf{x_{k-1}}| Z_{k-1})$が既知であると仮定すると、チャップマン＝コルモゴロフ方程式より以下のように再帰的に取得できます。

また、$p(\mathbf{x_k} | \mathbf{Z_k})$は以下のように表現されます。

いかにもベイズって感じですね！

なお、以降推定値は$\hat x_{a | b}$のように記述します。これは時刻$b$までの観測値を用いた時刻$a$における状態推定値という意味です。

推定値の取得基準

推定値の取得に際しては、様々な基準が存在します。例えば、最小平均二乗誤差（MMSE：Minimum Mean-Square Error）があり、以下のように表現されます。

あるいは、最大事後確率（MAP：Maximum a posteriori）という基準もあり、こちらは以下の式で求められます。

カルマンフィルタ(Kalman Filter)

「ロボットの実装でカルマンフィルタの触りをやってみよう！【カルマンフィルタ入門】」にても解説を行っていますが、ここでは更に厳密な解説を行っていくこととします。

カルマンフィルタは線形空間で用いるフィルタリングです。 かのアポロ計画においてロケットが月面に至るまでの自己位置推定に用いられた歴史があります。

得られる解はMMSEです。つまり最適解です。しかし条件がありまして、それを満たす必要があります。条件は後述のとおりです。

使用条件

カルマンフィルタは以下の条件を満たすときに使用できます。

• $p(\mathbf{x_k} | \mathbf{Z_k})$がガウシアンであること
• $v_k,\mathbf{w_k}$がともに既知のガウシアン分布であること
• $f_k,h_k$が線形であること

この条件を満たす時、ベイズ推定における最適性を獲得します。(最尤推定、最小分散推定)

定義

カルマンフィルタは上記の条件を満たすので

• 状態遷移モデル

• 観測モデル

と表記する事ができます。ただし$F_{k-1}$は$n_x\times n_x$次元の行列で、$H_k$は$n_z \times n_x$次元の行列です。 また先述の通り、ノイズがホワイトノイズであるとするのなら$\mathbf{v_{k-1}},\mathbf{w_k}$は平均0、誤差共分散$Q_{k-1},R_{k}$の標準正規分布とするのが一般です。

アルゴリズム

カルマンフィルタは

1. 予測ステップ：$Z_{k-1}$を用いた状態変数の予想
2. カルマンゲインの取得
3. 更新ステップ：$\mathbf{z_k}$を用いた状態変数の予測

という手続きからなります。

よってカルマンフィルタのアルゴリズムは以下の様に表現されます。

ただし、$\mathcal{N}(x;m,P)$は平均$m$,誤差共分散$P$とする正規分布の$x$における値で

を満たすものです。

予測ステップ

状態の予測と誤差共分散の値を時刻$k-1$までの情報から算出します。

更新ステップ

カルマンゲインを取得し、時刻$k$までの情報を使用します。

と表現されます。なお、$K_k$こそがカルマンゲインと呼ばれる値です。

カルマンフィルタは再帰的に平均と$p(\mathbf{x_k}|Z_k)$の共分散を算出する手法です。

先述の条件を満たすときに最適解を導きます。つまりガウシアンな条件下ではカルマンフィルタが最も良いフィルタになる訳ですね！

拡張カルマンフィルタ(Extended Kalman Filter)

EKFなどと略されます。 本手法は準最適非線形フィルタの一種です。 最大の特徴は非線形関数を線形に落とし込むことにあります。

状態推移モデルと観測方程式

が与えられた時、線形空間に落とし込んで

としてしまうのです。これはテイラー展開が背景にあります。

そして、状態方程式と観測方程式をそれぞれ一階のテイラー展開で近似します。これにより、非線形なモデルを線形化してカルマンフィルタの手法を適用できるようにします。

テイラー展開による線形化

状態方程式中の$f_{k-1}$ と観測方程式中の$h_k$をそれぞれ一階テイラー展開します。これにより、次のような線形モデルを得ます。

• 状態方程式の線形化:

• 観測方程式の線形化:

ここで、$F_{k-1}$ と $H_k$ はそれぞれ次のヤコビ行列です。

アルゴリズム

この線形化により、拡張カルマンフィルタは通常のカルマンフィルタと同じアルゴリズムを使用できるようになりました。やったね！

ただし、非線形な関数をテイラー展開で近似しているため、得られるのは最適ではなく準最適な解となる訳です。

予測ステップ

更新ステップ

カルマンゲインを計算し、状態と誤差共分散を更新します。

パーティクルフィルタ(Particle Filter)

すでに「理論ベースでパーティクルフィルタについて解説【実例あり】」にて解説済みではありますが、ここでは更に厳密な解説を行っていくこととします。理論よりも応用を急ぐという方はこちらの記事をおすすめします。

パーティクルフィルタはEKFと同じく準最適非線形フィルタの一種です。

分布密度に従う複数の粒子を用いた逐次モンテカルロ法(SMC：Sequential Monte Carlo)による推定がパーティクルフィルタです。 しばしばパーティクルフィルタと逐次モンテカルロ法が同義に扱われますが、後者のほうがより抽象的な話題を扱っており、パーティクルフィルタはより具体的な手法を指します。

したがってパーティクルフィルタはモンテカルロ法の一種であるので、粒子の数を多くすればするほど最適解に漸近するという性質があります。

本フィルタは1950年代にはあったそうですが、初めはあまり注目されていませんでした。 これはコンピュータの計算能力が足りなかった為だそうです。 思えばアポロ計画で使用された計算機のメモリはまだMB単位でしたね。(筆者はまだ生まれていません)

逐次モンテカルロ法(Sequential Monte Carlo)

パーティクルフィルタの背景である逐次モンテカルロ法について説明します。

モンテカルロ積分(Monte Carlo Integration)

モンテカルロ積分とはサンプリングを用いて積分を近似する計算方法を指します。

「なぜに積分？」と思われた方もいるでしょう。これは期待値というものが積分で求まるということを念頭に置いてくださればよろしいかと。

さて、ある積分を考えます。 $\mathbf{x}\in \mathcal{R}^{n_\mathbf{x}}$であるとき、

を満たすとします。ただし、両端は閉じていてもいなくてもどちらでも構いません。 $g(\mathbf{x})$はある確率密度関数$\pi(\mathbf{x})$を用いて

と表現することができます。したがって

と言い換える事ができる訳です。($\pi(\mathbf{x})$を確率、$f(\mathbf{x})$を確率変数と捉えるといかにも期待値ですよね)

次に$N$個の$\pi(\mathbf{x})$に従うサンプル$\left\{\mathbf{x_i},i = 1,2,\cdots N\right\}$を用意します。 これらサンプルを用いると積分は

のように近似できる訳ですね。ただし、$\mathbf{x_i}$は互いに独立しているものとします。

この時$I_N$は$I$の不偏推定量になり、$N$が大きければ多いほどより精密な推定に漸近します。

このようにサンプル(粒子)を用いて$\int$を$\sum$で近似する手法をモンテカルロ積分といいます。

重点サンプリング(Importance Sampling)

私達は理想を言えば$\pi(\mathbf{x})$に従うサンプルを用いて$I$の積分を行いたいのですが、実際に$\pi(\mathbf{x})$が完全にはわからないケースもあるでしょう。 そんなときには$\pi(\mathbf{x})$に似た分布$q(\mathbf{x})$を用いてサンプリングを行います。

なお

を満たすものとします。

この条件があれば0除算が起こらないので

と表記し直すことができます。

この時$I_N$は

として求める事ができる訳です。 ただし

とします。

実はこの$\~\omega$こそが重点サンプリングにおける重点(Importance weight)のことを指しています。

正規化重点サンプリング(Normal Importance Sampling)

$\pi(\mathbf{x})$の形状は知っているが、正規化定数（分布全体で積分すると1になるための係数）がわからない場合に使用します。

で推定することができます。ただし

で、正規化重み(normalized importance weight)などと呼ばれます。$\sum{\omega(\mathbf{X}^i)} = 1$ですから正規化なんですね。

連続重点サンプリング(SIS：Sequential Importance Sampling)

SISは逐次モンテカルロ法の一種で、モンテカルロ積分の一般的な計算方法です。

基本的な思想

粒子をいくつも用意し、観測データを得るたびに粒子の状態の更新と重みの更新を行うもので、これによって分布に近似するというものです。

説明しよう！

まず$X_k = \left\{\mathbf{x_1,x_2,\cdots,x_k}\right\}$を宣言します。 これは時刻$k$までの$\mathbf{x}$の順列で、状態遷移の歴史みたいなものです。

同様に $Z_k = \left\{\mathbf{z_1,z_2,\cdots,z_k}\right\}$ を宣言します。こちらは観測値の変遷です。

次に$\left\{X_k^i,\omega_k^i\right\}$は$p(X_k | Z_k)$の分布に従ってランダムな値を取るサンプル(粒子)とします。ただし$\sum_i{\omega_k^i}=1$を満たすものとします。

私達はこの粒子を用いて

の様に分布に近似させる訳です。 なお、$\delta$はディラックのデルタ関数です。

アルゴリズム

1. 時刻$k=0$で、$N$個の粒子$\mathbf{x}_0^i, (i \isin \left\{1,2,\cdots,N\right\})$を初期分布$p(\mathbf{x}_0|z_0)$からサンプリング
2. $k$の区間で以下を繰り返す
   1. 粒子の予測(状態更新)
      $p(X_k|Z_k)$に似た分布$q(X_k|Z_k)$を用いて $$\begin{array}{rcl} q(X_k|Z_k)& :=&q(\mathbf{x_k} | X_{k-1} ,Z_k)q(X_{k-1}|Z_{k-1})\\ \\ X_K^i &\approx&q(X_k|Z_k) \end{array}$$ として予測する.
   2. 重みの更新 $$\begin{array}{rcl} \omega_k^i &\propto& \frac{p(\mathbf{z_k|x_k^i})p(\mathbf{x_k^i|x_{k-1}^i})p(X_{k-1}^i|Z_{k-1})}{q(\mathbf{x}_k^i| X_{k-1}^i,Z_k)q(X_{k-1}^i |Z_{k-1})}\\ \\ & = & \omega_{k-1}^i \frac{p(\mathbf{z_k|x_k^i})p(\mathbf{x_k^i|x_{k-1}^i})}{q(\mathbf{x}_k^i| \mathbf{x}_{k-1} , \mathbf{z}_k)}\\ \\ \end{array}$$ $$\left(\because p(X_{k}|Z_{k}) \propto p(\mathbf{z}_k | \mathbf{x}_k)p(\mathbf{x}_k | \mathbf{x}_{k-1})p(X_{k-1} | {Z}_{k-1})\right)$$
   3. 重みの正規化 $$\omega_k^i = \frac{\omega_k^i}{\sum_j{\omega_k^j}}$$
3. 推論
   事後分布は以下のように近似されます。 $$p(X_k|Z_k) \approx \sum_{i=1}^N \omega_k^i \delta(X_k - X_k^i)$$ ここで、$\delta$はディラックのデルタ関数です。

サンプルコード

以下にPythonでの例を示します。

def SIS(N, T, init_distribution, transition_model, observation_model, proposal_distribution):
    """
    N: 粒子の数
    T: 時系列の長さ
    particles  : xに相当
    weights    : wに相当
    init_distribution: 初期分布
    transition_model: 状態遷移モデル p(x_k|x_{k-1})
    observation_model: 観測モデル p(z_k|x_k)
    proposal_distribution: 提案分布 q(x_k|x_{k-1}, z_k)
    """
    # 初期化
    particles = init_distribution.sample(N)
    weights = np.ones(N) / N
    
    for k in range(1, T+1):
        # 観測値を取得
        z_k = get_observation(k)
        
        for i in range(N):
            # 粒子の予測
            particles[i] = proposal_distribution.sample(particles[i], z_k)
            
            # 重みの更新
            weights[i] *= (observation_model.likelihood(z_k, particles[i]) * 
                           transition_model.probability(particles[i], particles[i]) / 
                           proposal_distribution.probability(particles[i], particles[i], z_k))
        
        # 重みの正規化
        weights /= np.sum(weights)
        
        # 状態の推定（ここでは重み付き平均とします）
        estimated_state = np.sum(particles * weights[:, np.newaxis], axis=0)
        
        # 結果の保存や表示
        save_or_display_results(k, estimated_state, particles, weights)
    
    return estimated_state, particles, weights

粒子の劣化(Degeneracy problem)

SISの問題として、$k$の上限が大きくなるほど少数のサンプルに重みが集中し、多数のサンプルの$p(\mathbf{x}_k|Z_k)$が0に、つまり重みが0に近づいていくというものがあります。

そこで我々は次節にて紹介する”リサンプリング”という方法を取るわけです。

有効サンプルサイズ(Effective Sample Size)

サンプル(粒子)の重みがどのくらい有効かどうかを図る指標としてESS(有効サンプルサイズ)と言うものがあります。

として表記されます。ただし、$\omega$は正規化されているものとします。

$\omega$が理想的な状態$(=\frac{1}{N})$であれば$\hat{N}_{eff} = N$になり、反対に劣化によって重みが0の粒子が増えるほど$\hat{N}_{eff} \rightarrow 1$となります。

リサンプリング

ESSが閾値を下回るような深刻な状態になった時、SISアルゴリズムはリサンプリングを実施する必要があります。 リサンプリングには様々な手法があります。

累積重み和(CSW：Cumulative Sum Weighting)

1. 正規化済みの重みを足し合わせます。
2. 一様分布に従うランダムな値を0から1の範囲で生成します
3. ランダムな値を用いて累積重みの範囲内のパーティクルを選択し、新しいサンプルとして取得します。

def csw_resample(particles, weights):
    N = len(particles)
    indices = np.zeros(N, dtype=int)
    
    # 重みの正規化
    weights = weights / np.sum(weights)
    
    # 累積重みの計算
    cumulative_sum = np.cumsum(weights)
    cumulative_sum[-1] = 1.0  # 数値誤差の修正
    
    # ランダムスタートポイントの決定
    r = np.random.uniform(0, 1/N)
    
    # 等間隔ポイントの生成とパーティクル選択
    for i in range(N):
        u = r + i / N
        j = np.searchsorted(cumulative_sum, u)
        indices[i] = j
    
    return particles[indices]
"""
new_particles = csw_resample(particles, weights)
のように使用できます。
"""

ルーレットホイールサンプリング(Roulette Wheel Sampling)

CSWよりも計算量が多く遅い手法ですが、遺伝的アルゴリズムによく用いられているので紹介します。

ルーレットを思い浮かべてください。ルーレットのマスの大きさが各サンプル(粒子)の重みに対応します。つまり重みが大きければ大きいほどルーレットで当たりやすくなるわけです。

以下にPythonでの例を示します。

import random
def rws_resample(particles,weights):
    num_particles = len(weights)
    
    # 重みの合計を計算
    total_weight = sum(weights)
    
    # 累積和を計算
    cumulative_weights = []
    cumulative_sum = 0
    for weight in weights:
        cumulative_sum += weight
        cumulative_weights.append(cumulative_sum)
    
    # RWSによるリサンプリング
    resampled_indices = []
    for _ in range(num_particles):
        rand = random.uniform(0, total_weight)
        for i, cumulative_weight in enumerate(cumulative_weights):
            if rand < cumulative_weight:
                resampled_indices.append(i)
                break
    
    return particles[resampled_indices]
"""
new_particles = rws_resample(particles, weights)
のように使用できます。
"""

なお、私は以前紹介したパーティクルフィルタの記事でこちらのリサンプリング手法を用いて実装していました。 パーティクルの数が多くなるほど、CSWの方がより高速になるのでおすすめです。

一般的パーティクルフィルタ：(generic particle filter)

重点サンプリングでは重みが減衰していくという問題がありました。 そこで私たちはESSとリサンプリングによって精度を保つのでしたね。 現在多くのパーティクルフィルタの実装・手法ではSISとリサンプリングを組み合わせています。 通常パーティクルフィルタというとこの手法を指す事が多いです。

以下に一般的なパーティクルフィルタの実装例を載せます。

def PF(N, T, init_distribution, transition_model, observation_model, proposal_distribution,eff_thre)
    """
    N: 粒子の数
    T: 時系列の長さ
    particles  : xに相当
    weights    : wに相当
    init_distribution: 初期分布
    transition_model: 状態遷移モデル p(x_k|x_{k-1})
    observation_model: 観測モデル p(z_k|x_k)
    proposal_distribution: 提案分布 q(x_k|x_{k-1}, z_k)
    get_eff : 有効サンプルサイズの取得
    eff_thre : リサンプリング条件となるESSの閾値
    """
    # 初期化
    particles = init_distribution.sample(N)
    weights = np.ones(N) / N
    
    for k in range(1, T+1):
        # 観測値を取得
        z_k = get_observation(k)
        
        for i in range(N):
            # 粒子の予測
            particles[i] = proposal_distribution.sample(particles[i], z_k)
            
            # 重みの更新
            weights[i] *= (observation_model.likelihood(z_k, particles[i]) * 
                           transition_model.probability(particles[i], particles[i]) / 
                           proposal_distribution.probability(particles[i], particles[i], z_k))
        
        # 重みの正規化
        weights /= np.sum(weights)

        # 有効サンプルサイズの評価
        if get_eff() < eff_thre:
          # リサンプリングの実行
          (particles,weights) = resampling(particles,weights)
        
        # 状態の推定（ここでは重み付き平均とします）
        estimated_state = np.sum(particles * weights[:, np.newaxis], axis=0)

最後に

おつかれさまでした。

ここまで読み切った方はカルマン・パーティクルフィルタをある程度理解したと言える所まで来たと思います。

実はパーティクルフィルタには更に他のアルゴリズムがあります。今回紹介したのはもっともオーソドックスな手法なのです。 この先を更に学習されたいという方は下記に示します参考文献をあたってみることをおすすめします。

共に楽しい時系列データ整形ライフを送っていきましょう♪ それでは、また。

参考文献

• B Ristic, S Arulampalam , N Gordon “Beyond the Kalman Filter: Particle Filters for Tracking Applications”
• Math Works “カルマンフィルター”
• Wikipedia “カルマンフィルター”
• Wikipedia “チャップマン＝コルモゴロフ方程式”
• 機械学習と情報技術 “【機械学習】モンテカルロ積分を理解する”
• 島田直希 “時系列解析　自己回帰型モデル・状態空間モデル・異常検知”