カルマンフィルタ

カルマンフィルタ

カルマンフィルタは、状態推定やシステム制御に広く使用される信号処理手法です。
以下では、Pythonでカルマンフィルタを実装して、状態推定の例をグラフ化します。

まずは、NumPyとMatplotlibを使用してカルマンフィルタを実装します。
以下のコードでは、1次元のシステムに対してカルマンフィルタを適用し、真の状態と推定された状態の推移をグラフ化します。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
import numpy as np
import matplotlib.pyplot as plt

def kalman_filter(initial_state, initial_estimate_error_covariance, process_variance, measurement_variance, measurements):
    # 初期状態の設定
    x_estimate = initial_state
    P_estimate = initial_estimate_error_covariance
    
    # 推定された状態と真の状態の履歴を保存するリスト
    x_history = []
    true_state_history = []
    
    # カルマンフィルタの更新
    for z in measurements:
        # 予測ステップ
        x_predict = x_estimate
        P_predict = P_estimate + process_variance
        
        # 更新ステップ
        K = P_predict / (P_predict + measurement_variance)
        x_estimate = x_predict + K * (z - x_predict)
        P_estimate = (1 - K) * P_predict
        
        # 結果を保存
        x_history.append(x_estimate)
        true_state_history.append(z)
    
    return x_history, true_state_history

# システムの真の状態を生成
np.random.seed(0)
true_state = np.cumsum(np.random.randn(100))  # ランダムウォークモデル

# ノイズのある観測値を生成
measurements = true_state + 0.5 * np.random.randn(100)

# カルマンフィルタのパラメータ設定
initial_state = 0
initial_estimate_error_covariance = 1
process_variance = 1e-5
measurement_variance = 0.5**2

# カルマンフィルタを適用
estimated_states, true_states = kalman_filter(initial_state, initial_estimate_error_covariance, process_variance, measurement_variance, measurements)

# 結果のプロット
plt.figure(figsize=(10, 6))
plt.plot(true_states, label='True State', color='blue', linestyle='--')
plt.plot(estimated_states, label='Estimated State', color='red')
plt.title('Kalman Filter Example')
plt.xlabel('Time Step')
plt.ylabel('State')
plt.legend()
plt.grid(True)
plt.show()

このコードでは、ランダムウォークモデルに従うシステムの真の状態を生成し、観測にノイズが加わった状態をシミュレートします。
その後、カルマンフィルタを用いて真の状態を推定し、推定された状態と真の状態の推移をグラフ化します。

カルマンフィルタは、システムの動的な状態を推定するために、システムモデルと観測モデルを組み合わせて効果的に利用されます。

この例では、1次元のシステムに対するカルマンフィルタの動作を示していますが、より複雑なシステムや高次元の状態推定にも応用することができます。

[実行結果]

ソースコード解説

ソースコードを説明します。

1. ライブラリのインポート

1
2
import numpy as np
import matplotlib.pyplot as plt

NumPyは数値計算や配列操作のためのライブラリであり、Matplotlibはグラフ描画のためのライブラリです。

2. カルマンフィルタ関数の定義

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
def kalman_filter(initial_state, initial_estimate_error_covariance, process_variance, measurement_variance, measurements):
    # 初期状態の設定
    x_estimate = initial_state
    P_estimate = initial_estimate_error_covariance
    
    # 推定された状態と真の状態の履歴を保存するリスト
    x_history = []
    true_state_history = []
    
    # カルマンフィルタの更新
    for z in measurements:
        # 予測ステップ
        x_predict = x_estimate
        P_predict = P_estimate + process_variance
        
        # 更新ステップ
        K = P_predict / (P_predict + measurement_variance)
        x_estimate = x_predict + K * (z - x_predict)
        P_estimate = (1 - K) * P_predict
        
        # 結果を保存
        x_history.append(x_estimate)
        true_state_history.append(z)
    
    return x_history, true_state_history

この関数はカルマンフィルタを実装します。
引数として初期状態、初期推定誤差共分散行列、プロセスの分散、観測の分散、観測値を受け取ります。
関数内ではカルマンフィルタのアルゴリズムに基づいて真の状態を推定します。

3. シミュレーション用データの生成

1
2
3
np.random.seed(0)
true_state = np.cumsum(np.random.randn(100))  # ランダムウォークモデル
measurements = true_state + 0.5 * np.random.randn(100)  # ノイズのある観測値

ランダムウォークモデルに従う真の状態 true_state と、観測ノイズを含んだ観測値 measurements を生成します。

4. カルマンフィルタのパラメータ設定

1
2
3
4
initial_state = 0
initial_estimate_error_covariance = 1
process_variance = 1e-5
measurement_variance = 0.5**2

カルマンフィルタの初期状態やパラメータを設定します。

5. カルマンフィルタの適用と結果のプロット

1
2
3
4
5
6
7
8
9
10
11
12
estimated_states, true_states = kalman_filter(initial_state, initial_estimate_error_covariance, process_variance, measurement_variance, measurements)

# 結果のプロット
plt.figure(figsize=(10, 6))
plt.plot(true_states, label='True State', color='blue', linestyle='--')
plt.plot(estimated_states, label='Estimated State', color='red')
plt.title('Kalman Filter Example')
plt.xlabel('Time Step')
plt.ylabel('State')
plt.legend()
plt.grid(True)
plt.show()

カルマンフィルタを適用し、真の状態 true_states と推定された状態 estimated_states の時間変化をグラフで表示します。
青の破線が真の状態を示し、赤の実線がカルマンフィルタによって推定された状態を示します。
推定された状態が真の状態に近づくことが期待されます。

このグラフはカルマンフィルタがシステムの状態を効果的に推定できることを示しています。

結果解説

[実行結果]

上記のカルマンフィルタのグラフは、真の状態とカルマンフィルタによって推定された状態の時間変化を示しています。
以下はグラフに表示される内容の詳細です：

True State (青の破線):

  • 真の状態を表します。
  • ランダムウォークモデルに従って生成されたデータで、時間とともにランダムに変化します。
  • ノイズが加わった観測値を元にカルマンフィルタが推定を行います。

Estimated State (赤の実線):

  • カルマンフィルタによって推定された状態を表します。
  • 真の状態と観測値を基にして、カルマンフィルタが状態を推定します。
  • 推定された状態は観測値と真の状態の間を滑らかに補完したものとなります。

Time Step (時間ステップ):

  • $x$軸は時間ステップを表します。
  • 各ステップで真の状態とその推定値が示されています。

State (状態):

  • $y$軸はシステムの状態を表します。
  • 真の状態と推定された状態の値が示されています。

このグラフを通じて、カルマンフィルタがシステムの真の状態を観測ノイズを考慮しながら効果的に推定している様子が確認できます。

推定された状態は真の状態に近い軌道を描いており、ノイズの影響を抑えつつシステムの状態を正確に追跡しています。