def calibration_mag():
data = sd_recieve.read_mag_file("magcalib.txt")
cx=0
cy=0
cz=0
for item in data:
cx+=item[0]
cy+=item[1]
cz+=item[2]
cx=cx/len(data)
cy=cy/len(data)
cz=cz/len(data)
return cx,cy,cz
if __name__ == "__main__":
global cx,cy,cz
global offset
offset=calibration_gyro()
cx,cy,cz=calibration_mag()
mad_filter=madgwickahrs.MadgwickAHRS(1/50,beta=.1)
reciever=uart_recieve.uart_reciever()
cube=Simulation()
cube.run(0,0,0)
gyr = []
mag = []
touch = []
c_timestamp = []
c_acc = []
c_gyr = []
c_mag = []
c_touch = []
saveQ = []
saveT = []
saveS = []
frames = Frames()
mad = madgwickahrs.MadgwickAHRS()
def draw(timestamp, gyr, acc, mag, touch):
while True:
if len(timestamp) > 1:
if threadLock.acquire(100):
c_timestamp = copy.deepcopy(timestamp)
c_gyr = copy.deepcopy(gyr)
c_acc = copy.deepcopy(acc)
# c_mag = copy.deepcopy(mag)
c_touch = copy.deepcopy(touch)
threadLock.release()
frames.read_serial(c_timestamp, c_gyr, c_acc, c_mag, c_touch)
frames.preprocess()
draw_frames(frames)
y_acc_min = -970.0
y_acc_max = 995.0
z_acc_min = -1030.0
z_acc_max = 970.0
x_bias = (x_acc_max+x_acc_min)/2.0
x_scale = (x_acc_max-x_acc_min)/2.0
y_bias = (y_acc_max+y_acc_min)/2.0
y_scale = (y_acc_max-y_acc_min)/2.0
z_bias = (z_acc_max+z_acc_min)/2.0
z_scale = (z_acc_max-z_acc_min)/2.0
acc_bias = np.array([x_bias, y_bias, z_bias])
acc_scale = np.array([x_scale, y_scale, z_scale])
ma = mg.MadgwickAHRS(sampleperiod = 1.0 / 1100, beta = 5)
sample_period = 1.0 / 1100
def read_data():
# Get the raw data
data_pds = pds.read_csv("data.csv")
data_np = data_pds[["ax","ay","az","gx","gy","gz","mx","my","mz"]].values
data_np = data_np.astype("float64")
f = open("data_index.csv")
lines = f.readlines()
data_processed = []
labels = []
for line in lines:
index = list(map(int, line.split(',')[1:]))
# data_no_g = remove_gravity(data_np[index[0]:index[1],:])
features = get_features(data_np[index[0]:index[1],:6])
def main():
mad_filter = madgwickahrs.MadgwickAHRS(1 / 20, beta=.1)
data1, data2 = read_data_file(file='out.txt')
data1 = calib_gyro(data1)
data2 = calibration_mag(data2)
data2 = coord_2_meters(data2)
data2 = center_meters(data2)
R = data_2_NWU(data1, data2)
k_filter = kalman_filter.KalmanFilter(200, 200, 5, 5, 2, 2, 0, 0, 0,
1 / 20)
k_filter.init_pos_vel_accel_x()
g_NWU = np.matmul(R, np.transpose(data1[0, 1:4]))
for a in range(0, len(data1)):
data1[a, 1:4] = np.matmul(R, np.transpose(data1[a, 1:4]))
data1[a, 4:7] = np.matmul(R, np.transpose(data1[a, 4:7]))
for a in range(0, len(data2)):
data2[a, 5:8] = np.matmul(R, np.transpose(data2[a, 5:8]))
orientation = np.empty((0, 3), float)
x = np.empty((0, 6), float)
a_NWU_aux = np.empty((0, 3), float)
data2_index = 0
for a in range(0, len(data1) - 100):
if data1[a, 7] == 0:
theta = updateEuler(data1[a, 1:4], data1[a, 4:7], data1[a, 7],
[0, 0, 0], mad_filter)
R_tot = allign_axis.eulerAnglesToRotationMatrix(
[theta[2], theta[1], theta[0]])
k_filter.init_accel_y()
k_filter.predict()
#a_NWU=np.linalg.inv(R_tot)@(data1[a,1:4])
a_NWU = R_tot @ (data1[a, 1:4])
k_filter.update(
np.array([[0], [0], [0], [0], [a_NWU[0]], [a_NWU[1]]]))
else:
theta = updateEuler(data1[a, 1:4], data1[a, 4:7], data1[a, 7],
data2[data2_index, 5:8], mad_filter)
R_tot = allign_axis.eulerAnglesToRotationMatrix(
[theta[2], theta[1], theta[0]])
data2_index += 1
k_filter.init_pos_accel_y()
k_filter.predict()
# a_NWU=np.linalg.inv(R_tot)@(data1[a,1:4])
a_NWU = R_tot @ (data1[a, 1:4])
k_filter.update(
np.array([[data2[data2_index, 3]], [-data2[data2_index, 2]],
[0], [0], [a_NWU[0]], [a_NWU[1]]]))
a_NWU_aux = np.append(a_NWU_aux, np.array([a_NWU]), 0)
temp, a = k_filter.get_state()
if temp[1] > 50:
print(temp)
x = np.append(x, temp.transpose(), 0)
orientation = np.append(orientation,
np.array([[theta[2], theta[1], theta[0]]]), 0)
fig13 = plt.figure()
fig12 = plt.figure()
fig11 = plt.figure()
fig10 = plt.figure()
kalman_pos = fig11.add_subplot(1, 1, 1)
kalman_pos.title.set_text("position")
kalman_pos.set_ylabel("East")
kalman_pos.set_xlabel("m")
kalman_pos.margins(x=0)
# kalman_pos.scatter(-x[:,1],x[:,0],c=data1[:7371,0])
kalman_pos.plot(-x[:, 1], x[:, 0])
orient = fig10.add_subplot(1, 1, 1)
orient.title.set_text("Orientation")
orient.set_ylabel("rad")
orient.set_xlabel("time (sec)")
orient.margins(x=0)
orient.plot(data1[:len(orientation), 0], orientation[:, 0], label="roll")
orient.plot(data1[:len(orientation), 0], orientation[:, 1], label="pitch")
orient.plot(data1[:len(orientation), 0], orientation[:, 2], label="yaw")
orient.legend()
corrected_accel = fig12.add_subplot(1, 1, 1)
corrected_accel.title.set_text("Corrected accel")
corrected_accel.set_ylabel("rad")
corrected_accel.set_xlabel("time (sec)")
corrected_accel.margins(x=0)
corrected_accel.plot(a_NWU_aux[:, 0], label="ax")
corrected_accel.plot(a_NWU_aux[:, 1], label="ay")
corrected_accel.plot(a_NWU_aux[:, 2], label="az")
corrected_accel.legend()
display_all(data1, data2)
pos_check = fig13.add_subplot(1, 1, 1)
pos_check.title.set_text("pos")
pos_check.set_ylabel("accel")
pos_check.set_xlabel("posistion")
pos_check.margins(x=0)
pos_check.plot(-x[:, 1], a_NWU_aux[:, 0], label="ax_corrected")
pos_check.plot(-x[:, 1], a_NWU_aux[:, 1], label="ay_corrected")
pos_check.plot(-x[:, 1], a_NWU_aux[:, 2], label="az_corrected")
# corrected_accel.plot(-x[:,1],data2[:,5],label="mx")
# corrected_accel.plot(-x[:,1],data2[:,6],label="my")
# corrected_accel.plot(-x[:,1],data2[:,7],label="mz")
# pos_check.plot(-x[:,1],data1[:7371,4],label="gx")
# pos_check.plot(-x[:,1],data1[:7371,5],label="gy")
# pos_check.plot(-x[:,1],data1[:7371,6],label="gz")
pos_check.legend()
plt.show()
print("\n".join(map(repr, trajectory)))
print("\nRange: [{}, {}], [{}, {}], [{}, {}]".format(
min(coord_by_axis[0]),
max(coord_by_axis[0]),
min(coord_by_axis[1]),
max(coord_by_axis[1]),
min(coord_by_axis[2]),
max(coord_by_axis[2]),
))
# Uncomment the statement below to print the double integration results
# when the program exits.
#atexit.register(print_trajectory)
ser = get_serial(None, 115200)
sample_period = 1.0 / 200
ma = mg.MadgwickAHRS(sampleperiod=sample_period)
count = 0
x_bias = 29.69433
y_bias = 3.20569
z_bias = -22.26908
l = 0.5
verticies = [[l, -l, -l],[l, l, -l],[-l, l, -l],[-l, -l, -l],[l, -l, l],[l, l, l],[-l, -l, l],[-l, l, l]]
# ini_qua = Quaternion(0.05, -0.116, 0.984, 0.001)
# for i in range(len(verticies)):
# verticies[i] = ini_qua.rotate(verticies[i])
final_verticies = [[1, -1, -1],[1, 1, -1],[-1, 1, -1],[-1, -1, -1],[1, -1, 1],[1, 1, 1],[-1, -1, 1],[-1, 1, 1]]
edges = [[0,1],[0,3],[0,4],[2,1],[2,3],[2,7],[6,3],[6,4],[6,7],[5,1],[5,4],[5,7]]