import numpy as np
from EKF import Orientation_EKF
from math import atan2
B = np.zeros ((3,2))
Q = np.mat ( ([.1, 0, 0],
[0, .1, 0],
[0, 0, .1]) )
H = np.mat ( ([1, 0, 0],
[0, 1, 0]) )
R = np.mat ( ([.1, 0],
[0, .1]) )
pitch_filt = Orientation_EKF (B, Q, H, R)
roll_filt = Orientation_EKF (B, Q, H, R)
acc_pitch = atan2 (0, -9.8)
acc_roll = -atan2 (0, -9.8)
Z_pitch = np.mat ( ([acc_pitch],
[ 10 ]) )
Z_roll = np.mat ( ([acc_roll],
[ 10 ]) )
u = np.mat ( ([0],
[0]) )
print pitch_filt.compute (Z_pitch, u)
print roll_filt.compute (Z_roll, u)
#print acc_roll
pitch_record_raw.append (acc_pitch)
roll_record_raw.append (acc_roll)
# Start filtering here
Z_pitch = np.mat ( ([acc_pitch],
[gyro.x]) )
Z_roll = np.mat ( ([acc_roll],
[gyro.y]) )
#pitch = float (pitch_filt.compute (Z_pitch, u)[0][0])
#roll = float (roll_filt.compute (Z_roll, u)[0][0])
pitch = float (pitch_filt.compute (Z_pitch, u)[0])
roll = float (roll_filt.compute (Z_roll, u)[0])
# Storing filtered data
pitch_record.append (pitch)
roll_record.append (roll)
sleep (.01)
except ValueError or KeyError:
pass
except KeyboardInterrupt:
ser.close ()
plt.plot (pitch_record, label="pitch")
plt.plot (roll_record, label="roll")
