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")

1import numpy as np
2import pandas as pd
3import matplotlib.pyplot as plt
4from scipy.spatial.transform import Rotation
5
6
7def initialize_filter():
8    """
9    カルマンフィルターを初期化する
10    P, Q, R は適切な値を設定する必要がある
11    """
12
13    # 初期状態推定値 (クォータニオン)
14    # 初期の姿勢は単位クォータニオン（姿勢なし）
15    x = np.array([1.0, 0.0, 0.0, 0.0])
16
17    # 初期推定誤差共分散行列
18    P = np.eye(4)
19
20    # プロセスノイズの共分散行列
21    Q = np.eye(4) * 0.01
22
23    # 観測ノイズの共分散行列
24    R = np.eye(3) * 0.1
25
26    return x, P, Q, R
27
28
29def quaternion_to_rotation_rate(q, gyro_data):
30    """
31    クォータニオンを角速度に変換する
32
33    Args:
34        q (np.array): クォータニオン
35        gyro_data (np.array): ジャイロセンサーの値
36    """
37
38    # クォータニオンの共役を計算
39    # ax + bi (iは虚数)の共役は ax - bi みたいな感じ
40    q_conj = np.array([q[0], -q[1], -q[2], -q[3]])
41
42    # クォータニオンを回転ベクトルに変換
43    # 2倍するのは、回転ベクトルの長さが角速度になるようにするため
44    # 逆回転のためにinv()を使う
45    # inv で逆行列を求める
46    # as_rotvec で回転ベクトルに変換
47    # gyro_data で回転ベクトルの長さを決める
48    omega = 2.0 * Rotation.from_quat(q_conj).inv().as_rotvec() * gyro_data
49    return omega
50
51
52def update_filter(x, P, Q, R, accel_data, gyro_data, dt):
53    """
54    カルマンフィルターを更新する
55
56    Args:
57        x (np.array): 状態推定値
58        P (np.array): 推定誤差共分散行列
59        Q (np.array): プロセスノイズの共分散行列
60        R (np.array): 観測ノイズの共分散行列
61        accel_data (np.array): 加速度センサーの値
62        gyro_data (np.array): ジャイロセンサーの値
63        dt (float): サンプリング時間
64    """
65
66    # 予測ステップ
67    x_gyro = gyro_data[0]
68    y_gyro = gyro_data[1]
69    z_gyro = gyro_data[2]
70
71    # 角速度を表す行列
72    omega = np.array([
73        [0     , -x_gyro, -y_gyro, -z_gyro],
74        [x_gyro,       0,  z_gyro, -y_gyro],
75        [y_gyro, -z_gyro,       0,  x_gyro],
76        [z_gyro,  y_gyro, -x_gyro,       0]
77    ])
78
79    # np.eye(4) は 4x4 の単位行列を生成する
80    # 0.5 はサンプリング時間の半分
81    # dt はサンプリング時間
82    A = np.eye(4) + (0.5 * dt * omega)
83    del omega
84
85    # 角速度xサンプリング時間 を掛けることで予測値を求める
86    x_pred = A.dot(x)
87    P_pred = A.dot(P).dot(A.T) + Q
88
89    # カルマンゲインの計算
90    # 3x4のゼロ行列を作成
91    H = np.zeros((3, 4))
92
93    # H行列の適切な要素に値を設定
94    H[0, 1] = 1  # x方向の加速度とx軸回りの姿勢の関係
95    H[1, 2] = 1  # y方向の加速度とy軸回りの姿勢の関係
96    H[2, 3] = 1  # z方向の加速度とz軸回りの姿勢の関係
97
98    # カルマンゲインを求める
99    K = P_pred.dot(H.T).dot(np.linalg.inv(H.dot(P_pred).dot(H.T) + R))
100
101    # 角速度をクォータニオンに変換
102    omega = quaternion_to_rotation_rate(x, gyro_data)
103
104    # 観測更新
105    # 状態推定値(x_pred) と カルマンゲイン(K) を足す
106    x_updated = x_pred + K.dot(accel_data - H.dot(x_pred))
107    P_updated = (np.eye(4) - K.dot(H)).dot(P_pred)
108
109    return x_updated, P_updated
110
111
112# メインプログラム
113if __name__ == "__main__":
114    # データをDataFrameから読み込む (Accelerometer.csv と Gyroscope.csv に変更)
115    file_path = "./logs/run_kururu/"
116    accel_data = pd.read_csv(f'{file_path}Accelerometer.csv')
117    gyro_data = pd.read_csv(f'{file_path}Gyroscope.csv')
118
119    # カルマンフィルターの初期化
120    x, P, Q, R = initialize_filter()
121
122    # サンプリング時間
123    dt = accel_data['Time (s)'].diff().mean()
124
125    # データフレームをイテレートし、カルマンフィルターを更新
126    length = accel_data.shape[0] if accel_data.shape[0] < gyro_data.shape[0] else gyro_data.shape[0]
127
128    datum = []
129    for index in range(length - 1):
130        accel_data_point = np.array([
131            accel_data.at[index, 'Acceleration x (m/s^2)'],
132            accel_data.at[index, 'Acceleration y (m/s^2)'],
133            accel_data.at[index, 'Acceleration z (m/s^2)']
134        ])
135        gyro_data_point = np.array([
136            gyro_data.at[index, 'Gyroscope x (rad/s)'],
137            gyro_data.at[index, 'Gyroscope y (rad/s)'],
138            gyro_data.at[index, 'Gyroscope z (rad/s)']
139        ])
140        x, P = update_filter(x, P, Q, R, accel_data_point, gyro_data_point, dt)
141
142        # 推定された姿勢をクォータニオンからオイラー角に変換
143        euler_angles = Rotation.from_quat(x).as_euler('xyz')
144        datum.append([euler_angles[0], euler_angles[1], euler_angles[2]])
145
146    # datumをプロット
147    datum = np.array(datum)
148    plt.plot(datum[:, 0], label='roll')
149    plt.plot(datum[:, 1], label='pitch')
150    plt.plot(datum[:, 2], label='yaw')
151    plt.legend()
152    plt.show()