filter_init = 0
predictState = 1
len_data = len(data)
#data for plot
fusion_pose = copy.deepcopy(gps[:, 3:6])
fusion_speed = copy.deepcopy(data[:, 0:3])
fusion_heading = copy.deepcopy(vision_heading)
#len_data = 6000
for i in range(gps_buf, len_data):
#print math.sqrt(pow(acc[i][0],2) + pow(acc[i][1],2) + pow(acc[i][2],2))
if not filter_init:
pitch = math.asin(-np.mean(acc[100:150, 0]) / 9.80665)
roll = math.atan2(np.mean(acc[100:150, 1]), np.mean(acc[100:150, 2]))
yaw = gps_heading[i]
x_nom[6:10] = euler2q([roll, pitch, yaw])
rpy = q2euler(x_nom[0:4])
print "yaw = %f, rpy = %f, %f, %f" % (yaw, rpy[0], rpy[1], rpy[2])
#print x[0:4]
P = np.diag([
0.25, 0.25, 0.25, 0.01, 0.01, 0.01, 0, 0, 0,
pow(2.0 * DEG_TO_RAD_DOUBLE, 2),
pow(2.0 * DEG_TO_RAD_DOUBLE, 2),
pow(2.0 * DEG_TO_RAD_DOUBLE, 2),
pow(3.0e-3, 2),
pow(3.0e-3, 2),
pow(3.0e-3, 2)
])
P[6:9,
6:9] = np.diag([1e-4, 1e-4,
1e-4]) #P for err_state, use euler not quaternion.
predictState = 1
len_data = len(data)
#data for plot
fusion_pose = copy.deepcopy(gps[:, 3:6])
fusion_speed = copy.deepcopy(data[:, 0:3])
fusion_heading = copy.deepcopy(vision_heading)
#ppprint(gps)
#len_data = 6000
for i in range(gps_buf, len_data):
#print math.sqrt(pow(acc[i][0],2) + pow(acc[i][1],2) + pow(acc[i][2],2))
if not filter_init:
pitch = math.asin(-np.mean(acc[100:150, 0]) / 9.80665)
roll = math.atan2(np.mean(acc[100:150, 1]), np.mean(acc[100:150, 2]))
yaw = gps_heading[i]
x[0:4] = euler2q([roll, pitch, yaw])
rpy = q2euler(x[0:4])
print "yaw = %f, rpy = %f, %f, %f" % (yaw, rpy[0], rpy[1], rpy[2])
#print x[0:4]
p = np.diag([
0, 0, 0, 0, 0.01, 0.01, 0.01, 4, 4, 4,
pow(2.0 * DEG_TO_RAD_DOUBLE, 2),
pow(2.0 * DEG_TO_RAD_DOUBLE, 2),
pow(2.0 * DEG_TO_RAD_DOUBLE, 2),
pow(3.0e-3, 2),
pow(3.0e-3, 2),
pow(3.0e-3, 2)
])
rotation_vector_variance = [
pow(3.0 * DEG_TO_RAD_DOUBLE, 2),
pow(3.0 * DEG_TO_RAD_DOUBLE, 2),
filter_init = 0
predictState = 1
len_data = len(data)
#data for plot
fusion_pose = copy.deepcopy(gps[:, 3:6])
fusion_speed = copy.deepcopy(data[:, 0:3])
fusion_heading = copy.deepcopy(vision_heading)
#len_data = 6000
for i in range(gps_buf, len_data):
#print math.sqrt(pow(acc[i][0],2) + pow(acc[i][1],2) + pow(acc[i][2],2))
if not filter_init:
pitch = math.asin(-np.mean(acc[100:150, 0]) / 9.80665)
roll = math.atan2(np.mean(acc[100:150, 1]), np.mean(acc[100:150, 2]))
yaw = gps_heading[i]
x_nom[6:10] = euler2q([roll, pitch, yaw])
rpy = q2euler(x_nom[6:10])
print "yaw = %f, rpy = %f, %f, %f" % (yaw, rpy[0], rpy[1], rpy[2])
#print x[0:4]
P = np.diag([
0.25, 0.25, 0.25, 0.01, 0.01, 0.01, 0, 0, 0,
pow(2.0 * DEG_TO_RAD_DOUBLE, 2),
pow(2.0 * DEG_TO_RAD_DOUBLE, 2),
pow(2.0 * DEG_TO_RAD_DOUBLE, 2),
pow(3.0e-3, 2),
pow(3.0e-3, 2),
pow(3.0e-3, 2)
])
P[6:9,
6:9] = np.diag([1e-4, 1e-4,
1e-4]) #P for err_state, use euler not quaternion.