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)
[0, 100.0, 0], [0, 0, .01]) ) R = np.mat ( ([1000.0, 0], [0, 1000.0]) ) #Q = np.mat ( ([1.0, 0, 0], #[0, 100.0, 0], #[0, 0, .01]) ) #R = np.mat ( ([100.0, 0], #[0, 1000.0]) ) # Static H = np.mat ( ([1, 0, 0], [0, 1, 0]) ) B = np.zeros ((3,2)) pitch_filt = Orientation_EKF (B, Q, H, R, .01) roll_filt = Orientation_EKF (B, Q, H, R, .01) u = np.mat ( ([0], [0]) ) class vector: x = 0 y = 0 z = 0 accel_x = [] accel_y = [] accel_z = [] gyro_x = [] gyro_y = []