相補フィルタの理論とPython実装：ジャイロスコープと加速度計のセンサフュージョン

import numpy as np
import matplotlib.pyplot as plt
from scipy import signal

# --- パラメータ ---
fs = 100.0           # サンプリング周波数 [Hz]
dt = 1.0 / fs
tau = 1.0            # 時定数 [s]
alpha = tau / (tau + dt)

# --- 伝達関数の定義 ---
# LPF: H_LPF(z) = (1-alpha) / (1 - alpha*z^{-1})
b_lpf = [1 - alpha]
a_lpf = [1, -alpha]

# HPF: H_HPF(z) = (1 - z^{-1}) / (1 - alpha*z^{-1})
b_hpf = [1, -1]
a_hpf = [1, -alpha]

# --- 周波数応答 ---
w_lpf, h_lpf = signal.freqz(b_lpf, a_lpf, worN=4096, fs=fs)
w_hpf, h_hpf = signal.freqz(b_hpf, a_hpf, worN=4096, fs=fs)

# 合計の周波数応答
h_sum = h_lpf + h_hpf

fc = 1.0 / (2 * np.pi * tau)  # クロスオーバー周波数

# --- プロット ---
fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 8))

# ゲイン特性
ax1.semilogx(w_lpf, 20 * np.log10(np.abs(h_lpf) + 1e-10),
             label='LPF (Accelerometer)', linewidth=2)
ax1.semilogx(w_hpf, 20 * np.log10(np.abs(h_hpf) + 1e-10),
             label='HPF (Gyroscope)', linewidth=2)
ax1.semilogx(w_lpf, 20 * np.log10(np.abs(h_sum) + 1e-10),
             label='Sum (HPF + LPF)', linewidth=2, linestyle='--', color='black')
ax1.axvline(fc, color='r', linestyle=':', alpha=0.7,
            label=f'$f_c$ = {fc:.3f} Hz')
ax1.axhline(-3, color='gray', linestyle=':', alpha=0.5, label='-3 dB')
ax1.set_xlabel('Frequency [Hz]')
ax1.set_ylabel('Gain [dB]')
ax1.set_title(f'Complementary Filter Frequency Response (τ={tau} s, α={alpha:.4f})')
ax1.legend()
ax1.grid(True, which='both', alpha=0.3)
ax1.set_xlim([0.001, fs / 2])
ax1.set_ylim([-40, 5])

# 位相特性
ax2.semilogx(w_lpf, np.angle(h_lpf, deg=True), label='LPF', linewidth=2)
ax2.semilogx(w_hpf, np.angle(h_hpf, deg=True), label='HPF', linewidth=2)
ax2.axvline(fc, color='r', linestyle=':', alpha=0.7)
ax2.set_xlabel('Frequency [Hz]')
ax2.set_ylabel('Phase [degrees]')
ax2.set_title('Phase Response')
ax2.legend()
ax2.grid(True, which='both', alpha=0.3)
ax2.set_xlim([0.001, fs / 2])

plt.tight_layout()
plt.savefig('complementary_frequency_response.png', dpi=150, bbox_inches='tight')
plt.show()

import numpy as np
import matplotlib.pyplot as plt

# --- シミュレーションパラメータ ---
fs = 100.0           # サンプリング周波数 [Hz]
dt = 1.0 / fs
T = 30.0             # シミュレーション時間 [s]
t = np.arange(0, T, dt)
N = len(t)

# --- 真の角度（テスト信号）---
# ゆっくりした正弦波運動 + ステップ変化
theta_true = (
    15.0 * np.sin(2 * np.pi * 0.1 * t) +     # 0.1 Hz の揺動
    5.0 * np.sin(2 * np.pi * 0.5 * t) +       # 0.5 Hz の揺動
    10.0 * (t > 15)                             # t=15s でステップ変化
)

# --- 真の角速度 ---
omega_true = np.gradient(theta_true, dt)

# --- ジャイロスコープ出力（角速度 + バイアスドリフト + ノイズ）---
gyro_bias = 0.5      # バイアス [deg/s]（ドリフトの原因）
gyro_noise_std = 2.0  # ノイズ標準偏差 [deg/s]

gyro_output = omega_true + gyro_bias + gyro_noise_std * np.random.randn(N)

# --- 加速度計出力（角度 + 振動ノイズ）---
accel_noise_std = 5.0  # ノイズ標準偏差 [deg]

accel_output = theta_true + accel_noise_std * np.random.randn(N)

# --- ジャイロのみによる角度推定（積分）---
theta_gyro = np.zeros(N)
theta_gyro[0] = theta_true[0]
for i in range(1, N):
    theta_gyro[i] = theta_gyro[i - 1] + gyro_output[i] * dt

# --- 相補フィルタ ---
tau = 1.0            # 時定数 [s]
alpha = tau / (tau + dt)

theta_comp = np.zeros(N)
theta_comp[0] = theta_true[0]
for i in range(1, N):
    # 式 (5) の実装
    theta_comp[i] = alpha * (theta_comp[i - 1] + gyro_output[i] * dt) \
                  + (1 - alpha) * accel_output[i]

# --- 比較プロット ---
fig, axes = plt.subplots(4, 1, figsize=(12, 14), sharex=True)

# 真の角度
axes[0].plot(t, theta_true, 'k-', linewidth=2, label='True angle')
axes[0].set_ylabel('Angle [deg]')
axes[0].set_title('True Angle')
axes[0].legend()
axes[0].grid(True, alpha=0.3)

# ジャイロのみ
axes[1].plot(t, theta_gyro, 'r-', alpha=0.8, label='Gyro only (integration)')
axes[1].plot(t, theta_true, 'k--', alpha=0.5, label='True')
axes[1].set_ylabel('Angle [deg]')
axes[1].set_title('Gyroscope Only (drift accumulation)')
axes[1].legend()
axes[1].grid(True, alpha=0.3)

# 加速度計のみ
axes[2].plot(t, accel_output, 'b-', alpha=0.5, label='Accelerometer only')
axes[2].plot(t, theta_true, 'k--', alpha=0.5, label='True')
axes[2].set_ylabel('Angle [deg]')
axes[2].set_title('Accelerometer Only (noisy)')
axes[2].legend()
axes[2].grid(True, alpha=0.3)

# 相補フィルタ
axes[3].plot(t, theta_comp, 'g-', linewidth=2, label=f'Complementary (α={alpha:.3f})')
axes[3].plot(t, theta_true, 'k--', alpha=0.5, label='True')
axes[3].set_ylabel('Angle [deg]')
axes[3].set_xlabel('Time [s]')
axes[3].set_title('Complementary Filter')
axes[3].legend()
axes[3].grid(True, alpha=0.3)

plt.tight_layout()
plt.savefig('complementary_filter_comparison.png', dpi=150, bbox_inches='tight')
plt.show()

# --- RMSE の計算 ---
rmse_gyro = np.sqrt(np.mean((theta_gyro - theta_true) ** 2))
rmse_accel = np.sqrt(np.mean((accel_output - theta_true) ** 2))
rmse_comp = np.sqrt(np.mean((theta_comp - theta_true) ** 2))

print(f"RMSE (Gyro only):          {rmse_gyro:.2f} deg")
print(f"RMSE (Accelerometer only): {rmse_accel:.2f} deg")
print(f"RMSE (Complementary):      {rmse_comp:.2f} deg")

# --- 異なる α（τ）での比較 ---
tau_values = [0.1, 0.5, 1.0, 5.0, 10.0]

fig, axes = plt.subplots(2, 1, figsize=(12, 10))

for tau_val in tau_values:
    alpha_val = tau_val / (tau_val + dt)
    theta_est = np.zeros(N)
    theta_est[0] = theta_true[0]
    for i in range(1, N):
        theta_est[i] = alpha_val * (theta_est[i - 1] + gyro_output[i] * dt) \
                      + (1 - alpha_val) * accel_output[i]

    rmse = np.sqrt(np.mean((theta_est - theta_true) ** 2))
    axes[0].plot(t, theta_est, label=f'τ={tau_val}s (α={alpha_val:.4f}, RMSE={rmse:.2f}°)')

axes[0].plot(t, theta_true, 'k--', linewidth=2, alpha=0.7, label='True')
axes[0].set_ylabel('Angle [deg]')
axes[0].set_xlabel('Time [s]')
axes[0].set_title('Effect of Time Constant τ on Complementary Filter')
axes[0].legend(fontsize=8)
axes[0].grid(True, alpha=0.3)

# τ vs RMSE のプロット
tau_range = np.logspace(-2, 2, 100)
rmse_list = []
for tau_val in tau_range:
    alpha_val = tau_val / (tau_val + dt)
    theta_est = np.zeros(N)
    theta_est[0] = theta_true[0]
    for i in range(1, N):
        theta_est[i] = alpha_val * (theta_est[i - 1] + gyro_output[i] * dt) \
                      + (1 - alpha_val) * accel_output[i]
    rmse_list.append(np.sqrt(np.mean((theta_est - theta_true) ** 2)))

axes[1].semilogx(tau_range, rmse_list, 'b-', linewidth=2)
axes[1].set_xlabel('Time Constant τ [s]')
axes[1].set_ylabel('RMSE [deg]')
axes[1].set_title('RMSE vs Time Constant τ')
axes[1].grid(True, which='both', alpha=0.3)

# 最適τの表示
optimal_idx = np.argmin(rmse_list)
optimal_tau = tau_range[optimal_idx]
optimal_rmse = rmse_list[optimal_idx]
axes[1].axvline(optimal_tau, color='r', linestyle='--',
                label=f'Optimal τ={optimal_tau:.2f}s (RMSE={optimal_rmse:.2f}°)')
axes[1].legend()

plt.tight_layout()
plt.savefig('alpha_effect.png', dpi=150, bbox_inches='tight')
plt.show()