def solve_ukf(self):
def fx(x, dt):
# x[4], x[5], x[6] = x[4], normalize_angle(x[5]), normalize_angle(x[6])
sol = self.ode2(x)
# sol = self.ode_int(x)
return np.array(sol)
def hx(x):
return np.array([x[3], x[2], normalize_angle(x[4])])
# points = MerweScaledSigmaPoints(n=7, alpha=.00001, beta=2., kappa=-4.)
points = JulierSigmaPoints(n=7, kappa=-4., sqrt_method=None)
# points = SimplexSigmaPoints(n=7)
kf = UKF(dim_x=7,
dim_z=3,
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)
x0 = np.array(self.ini_val)
# x0 = np.reshape(x0, (1,7))
kf.x = x0 # initial mean state
kf.Q *= np.diag([.0001, .0001, .0001, .0001, .0001, .01, .01])
kf.P *= 0.000001 # kf.P = eye(dim_x) ; adjust covariances if necessary
kf.R *= 0.0001
Ms, Ps = kf.batch_filter(self.zs)
Ms[:, 5] = self.al_to_th(Ms[:, 5])
Ms[:, 6] = self.al_to_th(Ms[:, 6])
return Ms
def move_wheelchair(self):
self.ini_cwo_l = 2 * np.pi * np.random.random_sample() * -np.pi
self.ini_cwo_r = 2 * np.pi * np.random.random_sample() * -np.pi
while rospy.get_time() == 0.0:
continue
rospy.sleep(1)
# self.ini_val = [self.wheel_cmd.angular.z, -self.wheel_cmd.linear.x, -self.odom_y, self.odom_x, self.odom_th, self.th_to_al(self.l_caster_angle), self.th_to_al(self.r_caster_angle)]
self.ini_val = [
0.0, 0.0, 0.0, 0.0, 0.0, self.ini_cwo_l, self.ini_cwo_r
]
# self.ini_val = np.random.uniform(low=-1.0, high=1.0, size=(7,)).tolist()
# UKF initialization
def fx(x, dt):
sol = self.ode2(x)
return np.array(sol)
def hx(x):
return np.array([x[3], x[2], normalize_angle(x[4])])
# points = MerweScaledSigmaPoints(n=7, alpha=.00001, beta=2., kappa=-4.)
points = JulierSigmaPoints(n=7, kappa=-4., sqrt_method=None)
# points = SimplexSigmaPoints(n=7)
kf = UKF(dim_x=7,
dim_z=3,
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)
x0 = np.array(self.ini_val)
kf.x = x0 # initial mean state
kf.Q *= np.diag([.0001, .0001, .0001, .0001, .0001, .01, .01])
kf.P *= 0.000001 # kf.P = eye(dim_x) ; adjust covariances if necessary
kf.R *= 0.0001
count = 0
rospy.loginfo("Moving robot...")
start = rospy.get_time()
self.r.sleep()
while (rospy.get_time() - start <
self.move_time) and not rospy.is_shutdown():
self.pub_twist.publish(self.wheel_cmd)
z = np.array([self.odom_x, -self.odom_y, self.odom_th])
self.zs.append(z)
kf.predict()
kf.update(z)
self.xs.append(kf.x)
self.pose_x_data.append(self.odom_x)
self.pose_y_data.append(self.odom_y)
self.pose_th_data.append(self.odom_th)
self.l_caster_data.append(self.l_caster_angle)
self.r_caster_data.append(self.r_caster_angle)
count += 1
self.r.sleep()
# Stop the robot
self.pub_twist.publish(Twist())
self.xs = np.array(self.xs)
rospy.sleep(1)
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)