カルマンフィルタの初期値の影響について

# 書いていないこと
本記事はカルマンフィルタの初期値が状態の推定にどのような影響を与えるかについてのみ考えます。下記のような内容には触れませんのでご注意ください。

- パラメータの推定
- 状態空間モデルの表現

# カルマンフィルタとは

状態空間モデルにおいて、推定しなければいけないものに「状態」と「パラメータ」があります。隼本によると、状態推定→「カルマンフィルタ」、パラメータ推定→「最尤法」がもっとも有名な組み合わせらしいです[^1]。

すなわちカルマンフィルタとは、状態を効率よく推定するためのアルゴリズムということになります。

# カルマンフィルタの計算方法

カルマンフィルタの計算はこちらに詳しいです。

https://qiita.com/IshitaTakeshi/items/740ac7e9b549eee4cc04#%E3%82%AB%E3%83%AB%E3%83%9E%E3%83%B3%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF%E3%81%AB%E3%82%88%E3%82%8B%E5%80%A4%E3%81%AE%E6%9B%B4%E6%96%B0

バッチのようにまとめて計算するわけではなく、逐次計算を行うということがリンク先の画像でわかるかと思います。

本記事でわかっておいてほしいのはカルマンゲインの計算式です。隼本より

$$
カルマンゲイン = \frac{状態の予測誤差の分散}{状態の予測誤差の分散 + 観測誤差の分散}
$$

これがわかれば初期値の話も理解できます。カルマンゲインとは状態を観測値を用いて補正するのですが、その補正をどれほど効かせるかというパラメータです。

# 初期値の影響

カルマンフィルタはアルゴリズムの計算上いくつかの初期値を与えないといけません。隼本では、Rのパッケージである`dlm`を例に挙げています。`dlm` では　__状態の初期値は0__ 、__状態の予測誤差の分散の初期値は10000000__ となっているそうです。

なぜこのような適当な値が割り当てられているか考えてみましょう。

## 状態の予測誤差の分散

状態の初期値は $100$ 、状態の予測誤差の分散を, $10^5$ と $10^{-5}$ と設定しました。
画像の青線は観測値で、オレンジ線が推定した状態です。

![image (2).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1272355/05f01284-e320-8756-8933-4272d8b4398e.png)
![image (1).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1272355/bacb8c3c-559f-f2ed-0e4f-94a63eb6a145.png)

画像を見ますと、分散が大きいほうがより状態の初期値を大きく修正していることがわかります。分散が小さいほうは初期値に引っ張られています。

先ほどみた、Rのパッケージの分散の初期値が大きいのは、状態の初期値に関わらず、すぐに状態が補正されることを期待してのことだったことがわかります。

カルマンゲインの計算式から考えてみると、状態の予測誤差の分散を十分に大きくすると、カルマンゲインは大きな値になります。つまり状態の補正が大きく行われるということです。これは画像で見た結果と一致します。

## 過程誤差と観測誤差

状態空間モデルには状態方程式に含まれる過程誤差と、観測方程式に含まれる観測誤差があります。これら誤差の分散が状態推定にどのような影響を与えるのかもついでに見ていきましょう。

$w_0$ が過程誤差で $v_0$ が観測誤差です。

![image (6).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1272355/be332601-6423-cdc7-9885-c8782e9c3d02.png)
![image (7).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1272355/6280c921-07b8-a2f0-146a-d4480ea6fa67.png)

画像を見ると、観測誤差が大きい場合、状態の補正はほとんど行われていないことがわかります。逆に過程誤差が小さい場合、推定した状態のグラフは観測値とほとんど等しくなることがわかります。

カルマンゲインの計算式に戻ると、観測誤差はカルマンゲインの分母にくるので、観測誤差が大きい→分母が大きくなることに等しく、カルマンゲインが小さくなることとなります。



# まとめ

以上がカルマンフィルタの初期値の影響でした。ちなみにパラメータの初期値を適当に設定しないようにする、散漫カルマンフィルタという手法もありますが、これはまたの機会に。

# コード

今までに出てきたグラフの画像は、`plot_state_init.jl` と `plot_error_init.jl` の出力から抜粋したものです。

【注意】シードを設定するのを忘れていたので、同じようなグラフが作成できるとは限りません。

```kalmanfilter.jl

using Distributions
using Plots

# 観測値作成
f(x) = 0.2 + 0.5x + rand(Normal())
Y = begin 
    Y = zeros(100)
    for i in 1:100
        Y[i] = f(Y[i])
    end
    Y
end

# カルマンフィルタ
x : 状態
y : 観測値
P : 状態の予測誤差の分散
w : 過程誤差
v : 観測誤差
function kalmanfillter( x, y, P, w, v )
    # 状態の予測
    xₚᵣₑ = x
    # 状態の予測誤差の分散
    Pₚᵣₑ = P + w
    # 観測値の予測
    yₚᵣₑ = xₚᵣₑ
    # 観測値の予測誤差の分散
    Fₚᵣₑ = Pₚᵣₑ + v
    
    # カルマンゲイン
    kalman_gain = Pₚᵣₑ / ( Pₚᵣₑ + v ) # same as Pₚᵣₑ / Fₚᵣₑ 
    # フィルタ化推定量
    x_updated = xₚᵣₑ + kalman_gain * ( y -  yₚᵣₑ )
    # フィルタ化推定量の分散
    P_updated = ( 1 - kalman_gain ) * Pₚᵣₑ
    
    x_updated, P_updated
end

# 状態を推定
function est_state( x₀, P₀, w₀, v₀ )
    state = zeros( length(Y) + 1 )

    # 状態の初期値
    state[1] = x₀
    # 状態の予測誤差初期値
    P = P₀
    # 過程誤差の分散
    w = w₀
    # 観測誤差の分散
    v = v₀

    for ( i, y ) in enumerate(Y)
        x, P = kalmanfillter( state[i], y, P, w, v )
        #@show kalmanfillter( state[i], y, P, w, v )
        state[i+1] = x
    end
    state 
end
```

```plot_state_init.jl

w = 1.0
v = 1.0
x₀ = [ 0.0, 1.0, 10.0, 1e2 ]
P₀ = [ 1e-5 ,0.1, 1.0, 1e2, 1e5 ]
p = []
for x in x₀, P in P₀
    plot_obs = plot(Y)
    push!( 
        p,
        plot!(
            plot_obs,
            0:100,
            est_state(x, P, w, v),
            title="x₀: $x P₀: $P",
        )
    )
end
plot( p..., legend=false, size=(1500,1000) )

```

```plot_error_init.jl

w₀ = [ 1e-5, 1e-1, 1.0, 1e1, 1e2, 1e5 ]
v₀ = [ 1e-5, 1e-1, 1.0, 1e1, 1e2, 1e5 ]
x = 0.0
P = 1.0
p = []
for w in w₀, v in v₀
    plot_obs = plot(Y)
    push!( 
        p,
        plot!(
            plot_obs,
            0:100,
            est_state(x, P, w, v),
            title="w₀: $w v₀: $v"
        )
    )
end

# オレンジが推定した状態
# 青が観測値
plot( p..., legend=false, size=(1500,1000) )
```


# 参考
- [時系列分析と状態空間モデルの基礎 RとStanで学ぶ理論と実装](https://books.rakuten.co.jp/rb/15329315/?scid=af_pc_etc&sc2id=af_103_0_10000991)
- [カルマンフィルタってなに？](https://qiita.com/IshitaTakeshi/items/740ac7e9b549eee4cc04)

[^1]: 隼本では、ほかの組み合わせとして、状態推定→「ベイズ推論」、パラメータ推定→「HMC法」が紹介されています。