def residual_x(self, a, b):
y = a - b
y[4], y[5], y[6] = normalize_angle(y[4]), normalize_angle(
y[5]), normalize_angle(y[6])
return y
def ode_int(self):
a_t = np.arange(0.0, self.move_time, 1. / self.rate)
ini_val = [
normalize_angle(self.r_caster_angle - np.pi),
normalize_angle(self.l_caster_angle - np.pi)
]
asol = odeint(self.solvr, ini_val, a_t)
self.asol = asol
def save_data(self):
np.savetxt('data_cwo.csv', np.c_[self.l_caster_data,
self.r_caster_data])
sol = self.solve_est()
sol[:, 0] = self.al_to_th(sol[:, 0])
sol[:, 1] = self.al_to_th(sol[:, 1])
x0 = [normalize_angle(item) for item in sol[:, 0].tolist()]
x1 = [normalize_angle(item) for item in sol[:, 1].tolist()]
np.savetxt('data_est_cwo.csv', np.c_[x0, x1])
def plot_data(self):
self.ode_int()
for i in xrange(len(self.asol)):
self.sol_alpha1.append(normalize_angle(self.asol[i][0]))
self.sol_alpha2.append(normalize_angle(self.asol[i][1]))
self.sol_alpha1 = [
normalize_angle((angle + np.pi)) for angle in self.sol_alpha1
]
self.sol_alpha2 = [
normalize_angle((angle + np.pi)) for angle in self.sol_alpha2
]
self.calc_error()
plt.figure(1)
plt.subplot(221)
plt.title("R Caster Orientation (rad)")
xaxis = [x / 100. for x in xrange(len(self.sol_alpha1))]
plt.plot(xaxis, self.sol_alpha1, label="est")
xaxis = [x / 100. for x in xrange(len(self.r_caster_data))]
plt.plot(xaxis, self.r_caster_data, label="actual")
plt.legend()
plt.subplot(222)
plt.title("L Caster Orientation (rad)")
xaxis = [x / 100. for x in xrange(len(self.sol_alpha2))]
plt.plot(xaxis, self.sol_alpha2, label="est")
xaxis = [x / 100. for x in xrange(len(self.l_caster_data))]
plt.plot(xaxis, self.l_caster_data, label="actual")
plt.legend()
plt.subplot(223)
plt.title("Error R Caster Orientation (rad)")
xaxis = [x / 100. for x in xrange(len(self.error_alpha1))]
plt.plot(xaxis, self.error_alpha1)
plt.subplot(224)
plt.title("Error L Caster Orientation (rad)")
xaxis = [x / 100. for x in xrange(len(self.error_alpha2))]
plt.plot(xaxis, self.error_alpha2)
plt.show()
def save_data(self):
np.savetxt(
'data_model.csv',
np.c_[self.pose_x_data, self.pose_y_data, self.pose_th_data,
self.l_caster_data, self.r_caster_data])
sol = self.solve_est()
sol[:, 5] = self.al_to_th(sol[:, 5])
sol[:, 6] = self.al_to_th(sol[:, 6])
x00 = [item for item in sol[:, 0].tolist()]
x11 = [item for item in sol[:, 1].tolist()]
x22 = [-item for item in sol[:, 2].tolist()]
x33 = [item for item in sol[:, 3].tolist()]
x44 = [normalize_angle(item) for item in sol[:, 4].tolist()]
x55 = [normalize_angle(item) for item in sol[:, 5].tolist()]
x66 = [normalize_angle(item) for item in sol[:, 6].tolist()]
np.savetxt('data_est_model.csv', np.c_[x00, x11, x22, x33, x44, x55,
x66])
def dynamic_model(self, x0, alpha1, alpha2):
a_t = np.arange(0.0, self.dt, 0.001)
ini_val = x0
self.alpha1 = alpha1
self.alpha2 = alpha2
asol = odeint(self.solvr_dynamic_model, ini_val, a_t)
sol = asol[-1]
sol[4] = normalize_angle(sol[4])
return sol
def save_data(self):
rospy.loginfo("Saving data...")
np.savetxt(
'data.csv',
np.c_[self.pose_x_data, self.pose_y_data, self.pose_th_data,
self.l_caster_data, self.r_caster_data])
ukf_data = self.xs
ukf_data[:, 5] = self.al_to_th(ukf_data[:, 5])
ukf_data[:, 6] = self.al_to_th(ukf_data[:, 6])
x0 = [item for item in ukf_data[:, 0].tolist()]
x1 = [item for item in ukf_data[:, 1].tolist()]
x2 = [-item for item in ukf_data[:, 2].tolist()]
x3 = [item for item in ukf_data[:, 3].tolist()]
x4 = [normalize_angle(item) for item in ukf_data[:, 4].tolist()]
x5 = [normalize_angle(item) for item in ukf_data[:, 5].tolist()]
x6 = [normalize_angle(item) for item in ukf_data[:, 6].tolist()]
np.savetxt('data_ukf.csv', np.c_[x0, x1, x2, x3, x4, x5, x6])
sol = self.solve_est()
sol[:, 5] = self.al_to_th(sol[:, 5])
sol[:, 6] = self.al_to_th(sol[:, 6])
x00 = [item for item in sol[:, 0].tolist()]
x11 = [item for item in sol[:, 1].tolist()]
x22 = [-item for item in sol[:, 2].tolist()]
x33 = [item for item in sol[:, 3].tolist()]
x44 = [normalize_angle(item) for item in sol[:, 4].tolist()]
x55 = [normalize_angle(item) for item in sol[:, 5].tolist()]
x66 = [normalize_angle(item) for item in sol[:, 6].tolist()]
np.savetxt('data_est.csv', np.c_[x00, x11, x22, x33, x44, x55, x66])
def hx(x):
return np.array([x[3], x[2], normalize_angle(x[4])])
def residual_z(self, a, b):
y = a - b
y[2] = normalize_angle(y[2])
return y
def fx(x, dt):
x[0], x[1] = normalize_angle(x[0]), normalize_angle(x[1])
self.prev_alpha1 = x[0]
self.prev_alpha2 = x[1]
return self.caster_model_ukf(x, dt)
def ukf_move_wheelchair(self):
while rospy.get_time() == 0.0:
continue
rospy.sleep(1)
def fx(x, dt):
x[0], x[1] = normalize_angle(x[0]), normalize_angle(x[1])
self.prev_alpha1 = x[0]
self.prev_alpha2 = x[1]
return self.caster_model_ukf(x, dt)
def hx(x):
# print "2: ", self.prev_alpha1
delta_alpha1 = x[0] - self.prev_alpha1
delta_alpha2 = x[1] - self.prev_alpha2
alpha1dot = delta_alpha1 / self.dt
alpha2dot = delta_alpha2 / self.dt
sol = self.meas_model(x[0], x[1], alpha1dot, alpha2dot)
return sol
self.ini_pose = [
self.wheel_cmd.angular.z, -self.wheel_cmd.linear.x, -self.odom_y,
self.odom_x, self.odom_th
]
self.save_pose_data.append(
[self.ini_pose[2], self.ini_pose[3], self.ini_pose[4]])
points = MerweScaledSigmaPoints(n=2, alpha=.1, beta=1., kappa=-1.)
kf = UKF(dim_x=2,
dim_z=2,
dt=self.dt,
fx=fx,
hx=hx,
points=points,
sqrt_fn=None,
x_mean_fn=self.state_mean,
z_mean_fn=self.meas_mean,
residual_x=self.residual_x,
residual_z=self.residual_z)
# self.ini_val = [normalize_angle(self.l_caster_angle-np.pi), normalize_angle(self.r_caster_angle-np.pi)]
self.ini_val = [3.1, -3.14]
self.save_caster_data.append(self.ini_val)
kf.x = np.array(self.ini_val) # initial mean state
kf.P *= 0.0001 # kf.P = eye(dim_x) ; adjust covariances if necessary
# kf.R *= 0
# kf.Q *= 0
zs = []
xs = []
# xs.append(self.ini_val)
count = 0
# print "Est1: ", normalize_angle(kf.x[0]+np.pi), normalize_angle(kf.x[1]+np.pi)
rospy.loginfo("Moving robot...")
last_odom_x = self.odom_x
last_odom_th = self.odom_th
start = rospy.get_time()
self.r.sleep()
while (rospy.get_time() - start <
self.move_time) and not rospy.is_shutdown():
curr_odom_x, curr_odom_th = self.odom_x, self.odom_th
delta_x, delta_th = curr_odom_x - last_odom_x, curr_odom_th - last_odom_th
z = np.array([delta_x / self.dt, delta_th / self.dt])
# z = np.array([self.odom_vx, self.odom_vth])
if count % self.factor == 0:
zs.append(z)
kf.predict()
kf.update(z)
xs.append([
normalize_angle(kf.x[0] + np.pi),
normalize_angle(kf.x[1] + np.pi)
])
# print "Est: ", normalize_angle(kf.x[0]+np.pi), normalize_angle(kf.x[1]+np.pi)
# print "Act: ", normalize_angle(self.l_caster_angle), normalize_angle(self.r_caster_angle)
self.save_caster_data.append(
[self.l_caster_angle, self.r_caster_angle])
self.save_pose_data.append(
[self.odom_x, self.odom_y, self.odom_th])
# print len(self.save_caster_data)
self.pub_twist.publish(self.wheel_cmd)
count += 1
last_odom_x, last_odom_th = curr_odom_x, curr_odom_th
self.r.sleep()
# Stop the robot
self.pub_twist.publish(Twist())
self.caster_sol_ukf = np.array(xs)
self.caster_sol_act = np.array(self.save_caster_data)
self.pose_act = np.array(self.save_pose_data)
# self.pose_act = np.reshape(self.pose_act, (len(self.save_pose_data), 3))
rospy.sleep(1)
def plot_data(self):
plt.figure(1)
# Plot left caster
plt.subplot(221)
plt.title("L Caster Orientation (rad)")
data = self.generate_data(self.caster_sol, 'left')
data_est_l = [normalize_angle(angle + np.pi) for angle in data]
xaxis = [x / 100. for x in xrange(len(data_est_l))]
plt.plot(xaxis, data_est_l, label="est")
data_ukf_l = self.generate_data(self.caster_sol_ukf, 'left')
xaxis = [x / 5. for x in xrange(len(data_ukf_l))]
plt.plot(xaxis, data_ukf_l, label="ukf")
data_act = self.generate_data(self.caster_sol_act, 'left')
xaxis = [x / self.rate for x in xrange(len(data_act))]
plt.plot(xaxis, data_act, label="act")
plt.legend()
plt.subplot(223)
plt.title("Error L Caster Orientation (rad)")
error_data = self.calc_error(data_act, data_ukf_l)
xaxis = [x / 5. for x in xrange(len(error_data))]
plt.plot(xaxis, error_data, label="act - ukf")
# plt.subplot(325)
# plt.title("Error L Caster Orientation (rad)")
error_data = self.calc_error2(data_act, data_est_l)
xaxis = [x / 50. for x in xrange(len(error_data))]
plt.plot(xaxis, error_data, label="act - est")
plt.legend()
# Plot right caster
plt.subplot(222)
plt.title("R Caster Orientation (rad)")
data = self.generate_data(self.caster_sol, 'right')
data_est_r = [normalize_angle(angle + np.pi) for angle in data]
xaxis = [x / 100. for x in xrange(len(data_est_r))]
plt.plot(xaxis, data_est_r, label="est")
data_ukf_r = self.generate_data(self.caster_sol_ukf, 'right')
xaxis = [x / 5. for x in xrange(len(data_ukf_r))]
plt.plot(xaxis, data_ukf_r, label="ukf")
data_act = self.generate_data(self.caster_sol_act, 'right')
xaxis = [x / self.rate for x in xrange(len(data_act))]
plt.plot(xaxis, data_act, label="act")
plt.legend()
plt.subplot(224)
plt.title("Error R Caster Orientation (rad)")
error_data = self.calc_error(data_act, data_ukf_r)
xaxis = [x / 5. for x in xrange(len(error_data))]
plt.plot(xaxis, error_data, label="act - ukf")
# plt.subplot(326)
# plt.title("Error R Caster Orientation (rad)")
error_data = self.calc_error2(data_act, data_est_r)
xaxis = [x / 50. for x in xrange(len(error_data))]
plt.plot(xaxis, error_data, label="act - est")
plt.legend()
est_poses = self.get_est_pose(data_est_l, data_est_r, 20)
ukf_poses = self.get_est_pose(data_ukf_l, data_ukf_r, 1)
x_est = [est_poses[i, 3] for i in xrange(len(est_poses))]
y_est = [-est_poses[i, 2] for i in xrange(len(est_poses))]
th_est = [est_poses[i, 4] for i in xrange(len(est_poses))]
x_ukf = [ukf_poses[i, 3] for i in xrange(len(ukf_poses))]
y_ukf = [-ukf_poses[i, 2] for i in xrange(len(ukf_poses))]
th_ukf = [ukf_poses[i, 4] for i in xrange(len(ukf_poses))]
x_act = [self.pose_act[i, 0] for i in xrange(len(self.pose_act))]
y_act = [self.pose_act[i, 1] for i in xrange(len(self.pose_act))]
th_act = [self.pose_act[i, 2] for i in xrange(len(self.pose_act))]
plt.figure(2)
plt.subplot(231)
plt.title("Pose x (m)")
xaxis = [x / 5. for x in xrange(len(x_est))]
plt.plot(xaxis, x_est, label="est")
xaxis = [x / 5. for x in xrange(len(x_ukf))]
plt.plot(xaxis, x_ukf, label="ukf")
xaxis = [x / self.rate for x in xrange(len(x_act))]
plt.plot(xaxis, x_act, label="act")
plt.legend()
plt.subplot(234)
plt.title("Error pose x (m)")
error_data = self.calc_error3(x_act, x_ukf)
xaxis = [x / 5. for x in xrange(len(error_data))]
plt.plot(xaxis, error_data, label="act - ukf")
error_data = self.calc_error3(x_act, x_est)
xaxis = [x / 5. for x in xrange(len(error_data))]
plt.plot(xaxis, error_data, label="act - est")
plt.legend()
plt.subplot(232)
plt.title("Pose y (m)")
xaxis = [x / 5. for x in xrange(len(y_est))]
plt.plot(xaxis, y_est, label="est")
xaxis = [x / 5. for x in xrange(len(y_ukf))]
plt.plot(xaxis, y_ukf, label="ukf")
xaxis = [x / self.rate for x in xrange(len(y_act))]
plt.plot(xaxis, y_act, label="act")
plt.legend()
plt.subplot(235)
plt.title("Error pose y (m)")
error_data = self.calc_error3(y_act, y_ukf)
xaxis = [x / 5. for x in xrange(len(error_data))]
plt.plot(xaxis, error_data, label="act - ukf")
error_data = self.calc_error3(y_act, y_est)
xaxis = [x / 5. for x in xrange(len(error_data))]
plt.plot(xaxis, error_data, label="act - est")
plt.legend()
plt.subplot(233)
plt.title("Orientation (rad)")
xaxis = [x / 5. for x in xrange(len(th_est))]
plt.plot(xaxis, th_est, label="est")
xaxis = [x / 5. for x in xrange(len(th_ukf))]
plt.plot(xaxis, th_ukf, label="ukf")
xaxis = [x / self.rate for x in xrange(len(th_act))]
plt.plot(xaxis, th_act, label="act")
plt.legend()
plt.subplot(236)
plt.title("Error orientation (rad)")
error_data = self.calc_error3(th_act, th_ukf)
xaxis = [x / 5. for x in xrange(len(error_data))]
plt.plot(xaxis, error_data, label="act - ukf")
error_data = self.calc_error3(th_act, th_est)
xaxis = [x / 5. for x in xrange(len(error_data))]
plt.plot(xaxis, error_data, label="act - est")
plt.legend()
plt.show()
def delta(self, alpha):
return normalize_angle(-alpha)