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

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




### Pythonコード


```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コード
```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コード

```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()
```