1import numpy as np
2import pandas as pd
3
4# カルマンフィルターの初期化
5def initialize_filter():
6    # 初期状態推定値 (クォータニオン)
7    x = np.array([1.0, 0.0, 0.0, 0.0])  # 初期の姿勢は単位クォータニオン（姿勢なし）
8    
9    # 初期推定誤差共分散行列
10    P = np.eye(4)
11
12    # プロセスノイズの共分散行列 (システムノイズ)
13    Q = np.eye(4) * 0.01  # 適切な値を設定する必要があります
14
15    # 観測ノイズの共分散行列
16    R = np.eye(3) * 0.1  # 適切な値を設定する必要があります
17
18    return x, P, Q, R
19
20# クォータニオンを角速度に変換する関数
21def quaternion_to_rotation_rate(q, gyro_data):
22    q_conj = np.array([q[0], -q[1], -q[2], -q[3]])
23    omega = 2.0 * R.from_quat(q_conj).inv().as_rotvec() * gyro_data
24    return omega
25
26# カルマンフィルターの更新ステップ
27def update_filter(x, P, Q, R, accel_data, gyro_data, dt):
28    # 予測ステップ
29    A = np.eye(4) + 0.5 * dt * np.array([
30        [0, -gyro_data[0], -gyro_data[1], -gyro_data[2]],
31        [gyro_data[0], 0, gyro_data[2], -gyro_data[1]],
32        [gyro_data[1], -gyro_data[2], 0, gyro_data[0]],
33        [gyro_data[2], gyro_data[1], -gyro_data[0], 0]
34    ])
35    x_pred = A.dot(x)
36    P_pred = A.dot(P).dot(A.T) + Q
37
38    # カルマンゲインの計算
39    H = np.eye(3)
40    K = P_pred.dot(H.T).dot(np.linalg.inv(H.dot(P_pred).dot(H.T) + R))
41
42    # 角速度をクォータニオンに変換
43    omega = quaternion_to_rotation_rate(x, gyro_data)
44
45    # 観測更新
46    x_updated = x_pred + K.dot(accel_data - H.dot(x_pred))
47    P_updated = (np.eye(4) - K.dot(H)).dot(P_pred)
48
49    return x_updated, P_updated
50
51# メインプログラム
52if __name__ == "__main__":
53    # データをDataFrameから読み込む (仮のデータ)
54    data = pd.read_csv('sensor_data.csv')  # ファイル名は実際のファイルに合わせて変更
55
56    # カルマンフィルターの初期化
57    x, P, Q, R = initialize_filter()
58
59    # サンプリング時間
60    dt = data['timestamp'].diff().mean()
61
62    # データフレームをイテレートし、カルマンフィルターを更新
63    for index, row in data.iterrows():
64        accel_data = np.array([row['accel_x'], row['accel_y'], row['accel_z']])
65        gyro_data = np.array([row['gyro_x'], row['gyro_y'], row['gyro_z']])
66        x, P = update_filter(x, P, Q, R, accel_data, gyro_data, dt)
67
68    # 推定された姿勢をクォータニオンからオイラー角に変換
69    euler_angles = R.from_quat(x).as_euler('xyz')
70
71    print(f"推定されたロール角度: {euler_angles[0]} rad")
72    print(f"推定されたピッチ角度: {euler_angles[1]} rad")
73    print(f"推定されたヨー角度: {euler_angles[2]} rad")