Go to Qiita Advent Calendar 2024 Top
LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@qwer123123

カルマンフィルタ　システム応答

Last updated at Posted at 2024-10-03

カルマンフィルタをPythonで実装し、プロットするコードを以下に示します。このコードは、1次元の状態推定を行い、カルマンフィルタの推定値と実際の観測データを可視化します。

Pythonコード

import numpy as np
import matplotlib.pyplot as plt

# カルマンフィルタのパラメータ設定
dt = 0.1  # 時間間隔
F = np.array([[1, dt], [0, 1]])  # 状態遷移行列
H = np.array([[1, 0]])  # 観測行列
Q = np.array([[1, 0], [0, 3]])  # プロセスノイズ共分散行列
R = np.array([[10]])  # 観測ノイズ共分散行列
B = np.array([[0.5 * dt**2], [dt]])  # 制御行列
u = 2  # 制御入力 (例：定数加速度)

# 初期状態と共分散行列
x = np.array([[0], [1]])  # 初期状態（位置と速度）
P = np.eye(2)  # 共分散行列

# シミュレーション時間
num_steps = 50

# 観測データを生成
np.random.seed(42)  # 再現性のための乱数シード
true_positions = []
observations = []
for _ in range(num_steps):
    # 実際の位置の更新
    x = F @ x + B * u
    true_positions.append(x[0, 0])
    
    # 観測値の生成 (ノイズを含む)
    z = H @ x + np.random.normal(0, np.sqrt(R[0, 0]), (1, 1))
    observations.append(z[0, 0])

# カルマンフィルタによる状態推定
estimated_positions = []
for z in observations:
    # 予測
    x = F @ x + B * u
    P = F @ P @ F.T + Q

    # カルマンゲインの計算
    K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)

    # 更新
    x = x + K @ (z - H @ x)
    P = (np.eye(2) - K @ H) @ P

    # 推定された位置を保存
    estimated_positions.append(x[0, 0])

# プロット
plt.figure(figsize=(10, 6))
plt.plot(true_positions, label='True Position', marker='o')
plt.plot(observations, label='Observations', linestyle='dotted', marker='x')
plt.plot(estimated_positions, label='Kalman Filter Estimate', linestyle='--', marker='s')
plt.xlabel('Time Step')
plt.ylabel('Position')
plt.title('Kalman Filter Position Estimation')
plt.legend()
plt.grid()
plt.show()

コードの説明

  1. パラメータ設定:

    • dt は時間ステップです。
    • F は状態遷移行列で、システムの状態が次の時刻にどう変化するかを定義します。
    • H は観測行列で、システムの状態がどのように観測されるかを表します。
    • Q はプロセスノイズの共分散行列で、システムにかかる外乱を表しています。
    • R は観測ノイズの共分散行列で、観測値の誤差の大きさを表します。

  2. シミュレーションと観測データの生成:

    • 各時間ステップでの実際の位置を計算し、観測ノイズを加えて観測値を生成しています。

  3. カルマンフィルタの適用:

    • 予測ステップと更新ステップを繰り返すことで、状態を推定します。
    • K はカルマンゲインを表し、観測値をどの程度信頼するかを決定します。

  4. 結果のプロット:

    • 実際の位置、観測値、カルマンフィルタによる推定値をプロットし、比較します。

これにより、カルマンフィルタを用いた状態推定の効果を視覚的に確認することができます。観測ノイズが多い場合でも、カルマンフィルタは実際の位置に近い推定を行えることがわかります。

画像に基づいて、カルマンフィルタに関連する状態推定の条件付き平均と分散の予測について説明します。この内容をPythonコードで実装し、状態推定の推移をプロットするコードを以下に示します。

条件付き平均と分散の予測

カルマンフィルタの予測ステップを実装します。

  • 条件付き平均 (Conditional Mean) の予測:
    式: ( \hat{X}(n | n-1) = F \hat{X}(n-1 | n-1) )

  • 条件付き分散 (Conditional Variance) の予測:
    式: ( P(n | n-1) = F P(n-1 | n-1) F^T + G U G^T )

Pythonコード

import numpy as np
import matplotlib.pyplot as plt

# カルマンフィルタのパラメータ設定
dt = 0.1  # 時間ステップ
F = np.array([[1, dt], [0, 1]])  # 状態遷移行列
H = np.array([[1, 0]])  # 観測行列
G = np.array([[0.5 * dt**2], [dt]])  # 制御行列
Q = np.array([[1, 0], [0, 1]])  # プロセスノイズ共分散行列
R = np.array([[5]])  # 観測ノイズ共分散行列
U = np.eye(1)  # プロセスノイズ共分散（画像で示されているGUG^Tに相当）

# 初期状態と共分散行列
x = np.array([[0], [0.5]])  # 初期状態（位置と速度）
P = np.eye(2)  # 初期共分散行列

# シミュレーション時間
num_steps = 60

# 実際の位置と観測値の生成
true_positions = []
observations = []
for _ in range(num_steps):
    # 実際の位置の更新
    x = F @ x
    true_positions.append(x[0, 0])
    
    # 観測値の生成 (ノイズを含む)
    z = H @ x + np.random.normal(0, np.sqrt(R[0, 0]), (1, 1))
    observations.append(z[0, 0])

# カルマンフィルタによる状態推定
x_est = np.array([[0], [0.5]])  # 推定初期状態
P_est = np.eye(2)  # 初期推定共分散行列
estimated_positions = []

# 推定値と分散の保存
predicted_positions = []
predicted_variances = []

for z in observations:
    # 予測ステップ
    x_pred = F @ x_est
    P_pred = F @ P_est @ F.T + G @ U @ G.T

    # 観測とカルマンゲイン
    K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R)

    # 状態の更新
    x_est = x_pred + K @ (z - H @ x_pred)
    P_est = (np.eye(2) - K @ H) @ P_pred

    # 推定された位置を保存
    estimated_positions.append(x_est[0, 0])
    predicted_positions.append(x_pred[0, 0])
    predicted_variances.append(P_pred[0, 0])

# プロット
plt.figure(figsize=(12, 8))
plt.plot(true_positions, label='True Position', marker='o', color='black')
plt.plot(observations, label='Observations', linestyle='dotted', marker='x', color='red')
plt.plot(estimated_positions, label='Kalman Filter Estimate', linestyle='--', marker='s', color='blue')
plt.plot(predicted_positions, label='Predicted Position', linestyle='-', marker='d', color='green')
plt.fill_between(range(num_steps), 
                 np.array(predicted_positions) - 2 * np.sqrt(predicted_variances), 
                 np.array(predicted_positions) + 2 * np.sqrt(predicted_variances), 
                 color='green', alpha=0.2, label='Predicted Position ± 2σ')
plt.xlabel('Time Step')
plt.ylabel('Position')
plt.title('Kalman Filter Position Estimation with Prediction and Uncertainty')
plt.legend()
plt.grid()
plt.show()

コードの説明

  1. パラメータの設定:

    • 状態遷移行列F、観測行列H、プロセスノイズQ、観測ノイズR、制御行列Gを設定しています。
    • プロセスノイズ共分散行列Uは、画像にあるGUG^TのUに相当します。

  2. 観測データの生成:

    • 実際の位置を更新し、観測ノイズを加えて観測値を生成しています。

  3. カルマンフィルタの適用:

    • 予測ステップでは、条件付き平均x_predと分散P_predを計算します。
    • 観測値zとカルマンゲインKを使って、更新された状態と共分散行列を計算します。

  4. プロット:

    • 実際の位置、観測値、カルマンフィルタによる推定値、および予測された位置をプロットします。
    • 予測された位置の分散（±2σの範囲）を塗りつぶしで表現し、不確実性を視覚化します。

Pythonで、MDOFシステム（多自由度系）の応答をシミュレートし、その動作をプロットするコードを以下に示します。具体的には、2自由度系 (2DOF) の質量-ばね-ダンパーシステムを例にして、各質点の応答を求め、プロットします。

システム同定のPythonコード

import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import odeint

# 2DOFシステムのパラメータ設定
m1 = 1.0  # 質点1の質量 [kg]
m2 = 1.5  # 質点2の質量 [kg]
k1 = 20.0  # 質点1と固定点間のばね定数 [N/m]
k2 = 30.0  # 質点1と質点2間のばね定数 [N/m]
c1 = 2.0   # 質点1のダンパー係数 [Ns/m]
c2 = 2.5   # 質点1と質点2間のダンパー係数 [Ns/m]

# 状態空間表現の定義
def system_dynamics(y, t):
    # y = [x1, x1_dot, x2, x2_dot]（x1: 質点1の位置, x1_dot: 質点1の速度, x2: 質点2の位置, x2_dot: 質点2の速度）
    x1, x1_dot, x2, x2_dot = y

    # 質量行列の逆行列を求めて方程式を解く
    M = np.array([[m1, 0], [0, m2]])  # 質量行列
    C = np.array([[c1 + c2, -c2], [-c2, c2]])  # ダンパー行列
    K = np.array([[k1 + k2, -k2], [-k2, k2]])  # ばね行列
    
    # 運動方程式: M * [x1_ddot, x2_ddot].T = -C * [x1_dot, x2_dot].T - K * [x1, x2].T
    F = -C @ np.array([x1_dot, x2_dot]) - K @ np.array([x1, x2])
    x_ddot = np.linalg.inv(M) @ F  # x_ddot = [x1_ddot, x2_ddot]

    return [x1_dot, x_ddot[0], x2_dot, x_ddot[1]]

# シミュレーション時間
t = np.linspace(0, 10, 1000)  # 0から10秒までを1000ステップで計算

# 初期条件（質点1と質点2の初期位置・初期速度）
y0 = [0.1, 0.0, -0.1, 0.0]  # [x1初期位置, x1初期速度, x2初期位置, x2初期速度]

# システムの応答を計算
response = odeint(system_dynamics, y0, t)

# 各質点の位置を取得
x1_response = response[:, 0]  # 質点1の位置
x2_response = response[:, 2]  # 質点2の位置

# プロット
plt.figure(figsize=(12, 6))

# 質点1の応答
plt.plot(t, x1_response, label='Mass 1 (x1) Response', linestyle='-', color='blue')
# 質点2の応答
plt.plot(t, x2_response, label='Mass 2 (x2) Response', linestyle='--', color='green')

# グラフのラベル
plt.xlabel('Time [s]')
plt.ylabel('Displacement [m]')
plt.title('Response of 2DOF System')
plt.legend()
plt.grid()
plt.show()

コードの説明

  1. システムのパラメータ設定:

    • m1 と m2 は各質点の質量。
    • k1 と k2 は各ばねのばね定数。
    • c1 と c2 は各ダンパーのダンパー係数。

  2. 状態空間表現の定義:

    • system_dynamics(y, t) はシステムの状態変数とその時間変化を表す関数です。
    • 運動方程式を解くために、質量行列 M、ダンパー行列 C、およびばね行列 K を設定し、加速度 x_ddot を計算しています。

  3. システム応答の計算:

    • odeint 関数を使ってシステムの運動方程式を数値的に解き、各質点の位置を時間の関数として求めます。

  4. プロット:

    • x1_response は質点1の位置、x2_response は質点2の位置を表します。
    • 両質点の応答を同時にプロットし、システムの動的な振る舞いを視覚化します。
import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import multivariate_normal

# パラメータ設定
mean_true = [0, 0]  # 真の平均ベクトル
cov_true = [[1, 0.8], [0.8, 1]]  # 真の共分散行列

# サンプル生成 (観測データ)
np.random.seed(0)
sample_size = 300
samples = np.random.multivariate_normal(mean_true, cov_true, sample_size)

# 最尤推定 (MLE) を使用した平均と共分散行列の推定
mean_est = np.mean(samples, axis=0)
cov_est = np.cov(samples, rowvar=False)

# グリッドを作成して確率密度関数 (PDF) をプロット
x, y = np.mgrid[-3:3:.01, -3:3:.01]
pos = np.dstack((x, y))

# 真の分布の確率密度
rv_true = multivariate_normal(mean_true, cov_true)
pdf_true = rv_true.pdf(pos)

# 推定された分布の確率密度
rv_est = multivariate_normal(mean_est, cov_est)
pdf_est = rv_est.pdf(pos)

# プロット
plt.figure(figsize=(12, 6))

# サンプルの散布図
plt.subplot(1, 2, 1)
plt.scatter(samples[:, 0], samples[:, 1], s=5, label='Samples')
plt.contour(x, y, pdf_true, colors='blue', linestyles='dashed', label='True PDF')
plt.contour(x, y, pdf_est, colors='red', linestyles='solid', label='Estimated PDF')
plt.title('Samples and True vs. Estimated PDF')
plt.xlabel('X1')
plt.ylabel('X2')
plt.legend()
plt.grid()

# 推定された共分散行列のヒートマップ
plt.subplot(1, 2, 2)
plt.imshow(cov_est, cmap='hot', interpolation='nearest')
plt.colorbar()
plt.title('Estimated Covariance Matrix')
plt.xlabel('X1')
plt.ylabel('X2')

plt.tight_layout()
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from scipy.linalg import inv

# システムパラメータの設定
m = 1.0  # 質量 [kg]
c = 0.5  # 減衰係数 [Ns/m]
k = 2.0  # ばね定数 [N/m]

# カルマンフィルタのパラメータ設定
dt = 0.1  # 時間ステップ
F = np.array([[1, dt], [-k*dt/m, 1-c*dt/m]])  # 状態遷移行列
B = np.array([0, dt/m]).reshape(-1, 1)  # 制御行列
H = np.array([[1, 0]])  # 観測行列
Q = np.array([[1e-5, 0], [0, 1e-5]])  # プロセスノイズ共分散行列
R = np.array([[1e-2]])  # 観測ノイズ共分散行列

# 初期状態と共分散行列
x_est = np.array([0, 0]).reshape(-1, 1)  # 初期状態（位置と速度）
P_est = np.eye(2)  # 初期共分散行列

# シミュレーション時間と制御入力の設定
t = np.linspace(0, 20, 200)  # 0～20秒を200ステップでシミュレーション
u = np.sin(2 * np.pi * 0.5 * t)  # 0.5Hzの正弦波を外力として加える

# 観測データを生成（シミュレーション）
true_state = np.zeros((2, len(t)))
observations = np.zeros(len(t))
true_state[:, 0] = [0.5, 0]  # 初期位置と初期速度
for i in range(1, len(t)):
    true_state[:, i] = F @ true_state[:, i-1] + B.flatten() * u[i-1] + np.random.multivariate_normal([0, 0], Q)
    observations[i] = H @ true_state[:, i] + np.random.normal(0, np.sqrt(R[0, 0]))

# カルマンフィルタによる推定
estimated_states = np.zeros((2, len(t)))
for i in range(1, len(t)):
    # 予測ステップ
    x_pred = F @ x_est + B * u[i-1]
    P_pred = F @ P_est @ F.T + Q

    # カルマンゲインの計算
    K = P_pred @ H.T @ inv(H @ P_pred @ H.T + R)

    # 更新ステップ
    x_est = x_pred + K @ (observations[i] - H @ x_pred)
    P_est = (np.eye(2) - K @ H) @ P_pred

    # 結果の保存
    estimated_states[:, i] = x_est.flatten()

# プロット
plt.figure(figsize=(12, 8))

# 位置のプロット（真の値と推定値）
plt.subplot(2, 1, 1)
plt.plot(t, true_state[0, :], label='True Position', linestyle='-', color='blue')
plt.plot(t, estimated_states[0, :], label='Estimated Position', linestyle='--', color='red')
plt.scatter(t, observations, label='Observations', color='green', s=10)
plt.xlabel('Time [s]')
plt.ylabel('Position [m]')
plt.title('Position Estimation using Kalman Filter')
plt.legend()
plt.grid()

# 速度のプロット（真の値と推定値）
plt.subplot(2, 1, 2)
plt.plot(t, true_state[1, :], label='True Velocity', linestyle='-', color='blue')
plt.plot(t, estimated_states[1, :], label='Estimated Velocity', linestyle='--', color='red')
plt.xlabel('Time [s]')
plt.ylabel('Velocity [m/s]')
plt.title('Velocity Estimation using Kalman Filter')
plt.legend()
plt.grid()

plt.tight_layout()
plt.show()
0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?