class FusionEKF:
"""
A class that gets sensor measurements from class DataPoint
and predicts the next state of the system using an extended Kalman filter algorithm
The state variables we are considering in this system are the position and velocity
in x and y cartesian coordinates, in essence there are 4 variables we are tracking.
In particular, an instance of this class gets measurements from both lidar and radar sensors
lidar sensors measure positions in cartesian coordinates (2 values)
radar sensors measure position and velocity in polar coordinates (3 values)
lidar sensor are linear and radar sensors are non-linear, so we use the jacobian algorithm
to compute the state transition matrix H unlike a simple kalman filter.
"""
def __init__(self, d):
self.initialized = False
self.timestamp = 0
self.n = d['number_of_states']
self.P = d['initial_process_matrix']
self.F = d['inital_state_transition_matrix']
self.Q = d['initial_noise_matrix']
self.radar_R = d['radar_covariance_matrix']
self.lidar_R = d['lidar_covariance_matrix']
self.lidar_H = d['lidar_transition_matrix']
self.a = (d['acceleration_noise_x'], d['acceleration_noise_y'])
self.kalmanFilter = KalmanFilter(self.n)
def updateQ(self, dt):
dt2 = dt * dt
dt3 = dt * dt2
dt4 = dt * dt3
x, y = self.a
r11 = dt4 * x / 4
r13 = dt3 * x / 2
r22 = dt4 * y / 4
r24 = dt3 * y / 2
r31 = dt3 * x / 2
r33 = dt2 * x
r42 = dt3 * y / 2
r44 = dt2 * y
Q = np.matrix([[r11, 0, r13, 0], [0, r22, 0, r24], [r31, 0, r33, 0],
[0, r42, 0, r44]])
self.kalmanFilter.setQ(Q)
def update(self, data):
dt = time_difference(self.timestamp, data.get_timestamp())
self.timestamp = data.get_timestamp()
self.kalmanFilter.updateF(dt)
self.updateQ(dt)
self.kalmanFilter.predict()
z = np.matrix(data.get_raw()).T
x = self.kalmanFilter.getx()
if data.get_name() == 'radar':
px, py, vx, vy = x[0, 0], x[1, 0], x[2, 0], x[3, 0]
rho, phi, drho = cartesian_to_polar(px, py, vx, vy)
H = calculate_jacobian(px, py, vx, vy)
Hx = (np.matrix([[rho, phi, drho]])).T
R = self.radar_R
elif data.get_name() == 'lidar':
H = self.lidar_H
Hx = self.lidar_H * x
R = self.lidar_R
self.kalmanFilter.update(z, H, Hx, R)
def start(self, data):
self.timestamp = data.get_timestamp()
x = np.matrix([data.get()]).T
self.kalmanFilter.start(x, self.P, self.F, self.Q)
self.initialized = True
def process(self, data):
if self.initialized:
self.update(data)
else:
self.start(data)
def get(self):
return self.kalmanFilter.getx()
def kalman_filter(data,
col_num,
threshold,
Q: float,
EGM: bool,
EGM_window_width=800,
verbose=True):
'''
----------------
DESCRIPTION
----------------
Simple implementation of a Kalman filter based on:
http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
'''
Measurement = effectiv_trans.gradient_filter(data=data,
col_num=col_num,
threshold=threshold,
verbose=verbose)
P = np.diag(np.array(1.0).reshape(-1))
F = np.matrix(1.0)
H = F
R = np.matrix(0.1**2)
Q = Q
G = np.matrix(1.0)
Q = G * (G.T) * Q
Z = np.matrix(Measurement[0])
X = Z
kf = KalmanFilter(X, P, F, Q, Z, H, R)
X_ = [X[0, 0]]
for i in tqdm(range(1, len(Measurement)),
desc="Kalman filter...",
ascii=False,
ncols=75):
# Predict
(X, P) = kf.predict(X, P, w=0)
# Update
(X, P) = kf.update(X, P, Z)
Z = np.matrix(Measurement[i])
X_.append(X[0, 0])
if EGM:
EGM = [X_[0]]
n = EGM_window_width
for i in tqdm(range(1,
len(Measurement) - n + 1),
desc="EGM filter...",
ascii=False,
ncols=80):
EGM.append(np.mean(X_[i:i + n]))
EGM.extend(X_[len(Measurement) - n + 1:])
return EGM
else:
return X_
class PoseEstimator:
def __init__(self):
self.mode = rospy.get_param("~mode", default=MODE.CMD_VEL_RGBD)
rospy.loginfo("Using mode: " + str(self.mode))
if self.mode == MODE.CMD_VEL_SCAN or self.mode == MODE.CMD_VEL_RGBD:
rospy.Subscriber("cmd_vel", Twist, self.on_cmdvel, queue_size=1)
elif self.mode == MODE.POSE_SCAN or self.mode == MODE.POSE_RGBD:
rospy.Subscriber("pose", PoseStamped, self.on_pose, queue_size=1)
else:
rospy.logerr("Unrecognized / invalid mode: " + self.mode)
exit(1)
rospy.Subscriber("scan", LaserScan, self.on_scan, queue_size=1)
self.state_pub = rospy.Publisher("state_estimate",
PoseWithCovarianceStamped,
queue_size=1)
self.tf_broadcaster = tf.TransformBroadcaster()
outfile = rospy.get_param("~outfile", default="output.csv")
self.outfile = open(outfile, "w")
self.outfile.write("time, location, uncertainty\n")
self.kfilter = KalmanFilter(INITIAL_BELIEF, INITIAL_UNCERTAINTY,
STATE_EVOLUTION, MOTION_NOISE, SENSE_NOISE)
self.prev_cmdvel_t = None
self.prev_cmdvel = None
self.cmdvel_count = 0
self.prev_pose = None
self.pose_count = 0
self.scan_count = 0
self.prev_wall_loc = None
def spin(self):
rospy.spin()
def on_cmdvel(self, msg):
curtime = rospy.Time.now()
self.cmdvel_count += 1
rospy.loginfo("cmdvel #{0} linX:{1}".format(
self.cmdvel_count, msg.linear.x))
if self.prev_cmdvel_t is not None:
dt = curtime - self.prev_cmdvel_t
avg_vel = 0.5 * (msg.linear.x + self.prev_cmdvel.linear.x)
rospy.loginfo("\tavg_vel: {0} dt:{1}".format(avg_vel, dt))
self.kfilter.propogate(action_model=numpy.matrix([dt.to_sec()]),
control=numpy.matrix([avg_vel]))
self.publish_state_estimation(curtime)
self.prev_cmdvel_t = curtime
self.prev_cmdvel = msg
def on_pose(self, msg):
self.pose_count += 1
rospy.loginfo("pose #{0} location:{1}".format(
self.pose_count, msg.pose.position.x))
if self.prev_pose is not None:
dx = msg.pose.position.x - self.prev_pose.pose.position.x
rospy.loginfo("\tdx: {0}".format(dx))
self.kfilter.propogate(action_model=numpy.matrix([1]),
control=numpy.matrix([dx]))
self.publish_state_estimation(msg.header.stamp)
self.prev_pose = msg
def on_scan(self, msg):
self.scan_count += 1
cur_reading = None
if self.mode == MODE.POSE_SCAN or self.mode == MODE.CMD_VEL_SCAN:
# 0th range measurment is forward using the builtin lidar
cur_reading = msg.ranges[0]
elif self.mode == MODE.POSE_RGBD or self.mode == MODE.CMD_VEL_RGBD:
# for RGBD conversion, the middle range measurement is forward
# > for increased robustness, look at the middle few messages
tries = 0
midI = int(len(msg.ranges) / 2)
loffset = 0
roffset = 0
while cur_reading is None or math.isnan(cur_reading):
if tries % 2 == 0:
cur_reading = msg.ranges[midI + loffset]
loffset -= 1
if tries % 2 == 1:
cur_reading = msg.ranges[midI + roffset]
roffset += 1
tries += 1
if tries > 10:
break
rospy.loginfo("scan #{0} dist:{1}".format(self.scan_count,
cur_reading))
msg = LaserScan()
if math.isnan(cur_reading):
rospy.loginfo("\tUnusable scan reading. Ignoring scan msg")
return
if self.prev_wall_loc is not None:
# Need to jump through some hoops to get sensor model,
# because we choose to make 0 be the starting point of the robot
robot_x = self.kfilter.belief.item(0)
expected_wall_distance = self.prev_wall_loc - robot_x
self.kfilter.update(expected_reading=expected_wall_distance,
reading=cur_reading)
self.publish_state_estimation(msg.header.stamp)
# Setting odom origin as 0, the wall is located at (scan_reading), offset by
# our current location (stored in self.kfilter.belief)
self.prev_wall_loc = self.kfilter.belief.item(0) + cur_reading
rospy.loginfo("\twall location: {0}".format(self.prev_wall_loc))
def publish_state_estimation(self, stamp):
robot_x = self.kfilter.belief.item(0)
uncertainty = self.kfilter.uncertainty.item(0)
pose = PoseWithCovarianceStamped()
pose.pose.pose.position.x = robot_x
pose.pose.covariance = numpy.matrix([[uncertainty, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0]])
self.state_pub.publish(pose)
self.tf_broadcaster.sendTransform(
(robot_x, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0),
stamp, "odom_kf", "base_footprint")
rospy.loginfo("\tkfilter. pos: {0} uncertainty: {1}".format(
self.kfilter.belief, self.kfilter.uncertainty))
stamp = rospy.Time.now()
self.outfile.write("{0},{1},{2}\n".format(stamp.to_sec(), robot_x,
uncertainty))
def __del__(self):
self.outfile.close()
