import numpy as np
import matplotlib.pyplot as plt
from vqf import VQF, BasicVQF, PyVQF

# generate simple dummy data
gyr = np.deg2rad(1)*np.ones((6000, 3))  # in rad/s
acc = 9.81/np.sqrt(3)*np.ones((6000, 3))  # in m/s²
Ts = 0.01  # sampling time (100 Hz)

# run orientation estimation
vqf = VQF(Ts)
# alternative: vqf = PyVQF(Ts)
out = vqf.updateBatch(gyr, acc)

# plot the quaternion
plt.figure()
plt.subplot(211)
plt.plot(out['quat6D'])
plt.title('quaternion (full version)')
plt.grid()

# run the basic version with the same data
params = dict(
    motionBiasEstEnabled=False,
    restBiasEstEnabled=False,
    magDistRejectionEnabled=False,
)
vqf2 = VQF(Ts, **params)
# alternative: vqf2 = BasicVQF(Ts)
# alternative: vqf2 = PyVQF(Ts, **params)
out2 = vqf2.updateBatch(gyr, acc)

# plot quaternion (notice the difference due to disabled bias estimation)
plt.subplot(212)
plt.plot(out2['quat6D'])
plt.grid()
plt.title('quaternion (basic version)')
plt.tight_layout()
plt.show()