<jupyter_start><jupyter_text>Naive Integration of Gyro<jupyter_code>euler_g = euler_from_gyro(ts, gyro)
plot_xyz(ts, euler_g*180/np.pi, "Euler attitude from naive integration of gyro")<jupyter_output><empty_output><jupyter_text>Attitude from Accelerometer Only<jupyter_code>euler_a = euler_from_acce(acce)
plot_xyz(ts, euler_a[:, :2] * 180 / np.pi, "Euler attitude from accelerometer only with stationary assumption")
plot_xyz(ts, euler_from_acce2(acce)[:, :2] * 180 / np.pi, "Euler attitude from accelerometer only ver2")<jupyter_output><empty_output><jupyter_text>Sensor fusion with Kalman Filter<jupyter_code>def gyro_to_quat_system_mat(gyro):
    p, q, r = gyro.T
    zeros = np.zeros_like(p)
    return np.stack([
        zeros, -p, -q, -r,
        p, zeros, r, -q,
        q, -r, zeros, p,
        r, q, -p, zeros,
    ], axis=1).reshape(-1, 4, 4) / 2

def euler_to_quat(euler):
    phi, theta, psi = euler.T
    
    sin_phi = np.sin(phi / 2)
    cos_phi = np.cos(phi / 2)
    sin_theta = np.sin(theta / 2)
    cos_theta = np.cos(theta / 2)
    sin_psi = np.sin(psi / 2)
    cos_psi = np.cos(psi / 2)
    
    return np.stack([
        cos_phi * cos_theta * cos_psi + sin_phi * sin_theta * sin_psi,
        sin_phi * cos_theta * cos_psi - cos_phi * sin_theta * sin_psi,
        cos_phi * sin_theta * cos_psi + sin_phi * cos_theta * sin_psi,
        cos_phi * cos_theta * sin_psi - sin_phi * sin_theta * cos_psi,
    ], axis=1)

def quat_to_euler(quat):
    w, x, y, z = quat.T
    phi = np.arctan2(2*(y*z + w*x), 1 - 2*(x**2 + y**2))
    theta = -np.arcsin(2*(x*z - w*y))
    psi = np.arctan2(2*(x*y + w*z), 1 - 2*(y**2 + z**2))
    return np.stack([phi, theta, psi], axis=1)

system_mat = gyro_to_quat_system_mat(gyro)
attitude_meas_from_acce = euler_to_quat(euler_a)
attitude_pred = np.zeros((len(ts), 4))

kf = KalmanFilter(
    x0 = np.array([[1, 0, 0, 0]]).T,
    P0 = np.eye(4),
    A = None,
    H = np.eye(4),
    Q = np.eye(4) * 1e-4,
    R = np.eye(4) * 10,
)

for i, (s, a) in enumerate(zip(system_mat, attitude_meas_from_acce)):
    kf.A = np.eye(4) + dt * s
    kf.update(a.reshape(4, 1))
    attitude_pred[i] = kf.get().reshape(-1)
    
attitude_pred_euler = quat_to_euler(attitude_pred) * 180 / np.pi
plot_xyz(ts, attitude_pred_euler[:, :2], "Attitude prediction from Kalman Filter")<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.1550321199143469, 'fraction_numerical': 0.026552462526766595, 'mean_word_length': 3.3745318352059925, 'pattern_counts': {'":': 0, '<': 13, '<?xml version=': 0, '>': 13, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '3', 'dir': 'jupyter-structured-clean-dedup', 'id': '313671', 'license': 'Unlicense', 'n_tokens_mistral': 1069, 'n_tokens_neox': 1016, 'n_words': 207, 'path': '11.ipynb', 'repo_name': 'jinhwanlazy/kalman-filter-isnt-hard', 'size': '2335'}