def RunEst(self):
# Assign inputs
self.est_u[0:3] = self.sen_imu_mapLinAcc # linear acceleration input
self.est_u[3:6] = self.sen_imu_mapAngVel # gyroscope input
self.RbtToMap(
) # convert sensor measurements from robot frame into map frame
self.SenMeasArrays() # collect sensor measurements
self.MapLinPos() # compute map frame linear position
self.EstObsArray() # update observation matrix
# Instantiate EKF
xDim = self.est_dimState # number of states
x = self.est_x # last estimate from robot frame
P = self.est_P # last covariance matrix
Q = self.est_Q # process noise covariance
R = self.est_R # measurement noise covariance
H = self.est_H # measurement matrix
z = self.est_m[2:15] # measurement
u = self.est_u # control input vector
A = self.est_A # jacobian state matrix
state = ExtendedKalmanFilter(xDim, x, P, z, u, A, Q, R, H)
# Perform time update if condition is 0
if self.meas_update == 0:
self.TimePropagation()
self.est_P = state.predict(P)
# Perform measurement update if condition is 1
else:
self.EstMeasArray() # estimator measurement array
self.est_x, self.est_L, self.est_P = state.update(x, P, z)
# Output state, covariance, and control input
self.OutputEst()
class Tracker:
'''
The Tracker class is created everytime we detect a new object during our analysis from either
the LIDAR point cloud or the RADAR doppler. It contains the entire state of the tracked object.
'''
def __init__(self):
#ideally we'd be given this ID from the measurement_packets coming in.
#However, I currently don't know how multiple objects are Identified.
self.id = None
#this is where the magic happens.
self.__ekf = ExtendedKalmanFilter()
self.__is_initialized = False
self.__previous_timestamp = 0.
@property
def state(self):
return self.__ekf.current_estimate[0]
def process_measurement(self, measurement_packet):
# if this is first measurement_packet, then setup state vector.
if not self.__is_initialized:
x, y, vx, vy = 0, 0, 0, 0
if measurement_packet.sensor_type == SensorType.LIDAR:
x, y, vx, vy = measurement_packet.x_measured, measurement_packet.y_measured, 0, 0
elif measurement_packet.sensor_type == SensorType.RADAR:
#we have the polar space measurements; we need to transform to cart space.
x, y, vx, vy = polar_2_cart(measurement_packet.rho_measured,
measurement_packet.phi_measured,
measurement_packet.rhodot_measured)
if abs(x + y) <= 1e-4:
x = 1e-4
y = 1e-4
self.__ekf.init_state_vector(x, y, vx, vy)
self.__previous_timestamp = measurement_packet.timestamp
self.__is_initialized = True
return
#1st we calculate how much time has passed since our last measurement_packet in seconds
dt = (measurement_packet.timestamp -
self.__previous_timestamp) / 1000000.0
self.__previous_timestamp = measurement_packet.timestamp
#2nd set new F and Q using new dt
self.__ekf.recompute_F_and_Q(dt)
#3rd make a prediction
self.__ekf.predict()
#4th update prediction
if measurement_packet.sensor_type == SensorType.LIDAR:
self.__ekf.updateLidar(measurement_packet)
elif measurement_packet.sensor_type == SensorType.RADAR:
self.__ekf.updateRadar(measurement_packet)
def __init__(self):
#ideally we'd be given this ID from the measurement_packets coming in.
#However, I currently don't know how multiple objects are Identified.
self.id = None
#this is where the magic happens.
self.__ekf = ExtendedKalmanFilter()
self.__is_initialized = False
self.__previous_timestamp = 0.
def one_run(filter_type,
num_steps,
data_factor=1,
filter_factor=1,
num_particles=100,
seed=None):
if seed is not None:
np.random.seed(seed)
alphas = np.array([0.05**2, 0.005**2, 0.1**2, 0.01**2])
beta = np.diag([np.deg2rad(5)**2])
env = Field(data_factor * alphas, data_factor * beta)
policy = policies.OpenLoopRectanglePolicy()
initial_mean = np.array([180, 50, 0]).reshape((-1, 1))
initial_cov = np.diag([10, 10, 1])
if filter_type == 'none':
filt = None
elif filter_type == 'ekf':
filt = ExtendedKalmanFilter(initial_mean, initial_cov,
filter_factor * alphas,
filter_factor * beta)
elif filter_type == 'pf':
filt = ParticleFilter(initial_mean, initial_cov, num_particles,
filter_factor * alphas, filter_factor * beta)
# You may want to edit this line to run multiple localization experiments.
#mean_position_error= localize(env, policy, filt, initial_mean, num_steps,False)
mean_position_error, anees = localize(env, policy, filt, initial_mean,
num_steps, False)
return mean_position_error, anees
def __init__(self, listen_to_vicon=False, vicon_channel=None, publish_to_lcm=False, use_rpydot=False, use_ekf=False, use_ukf=False, delay_comp=False): self._tvlqr_counting = False self._last_time_update = time.time() self._current_dt = 0.0 Thread(target=self._estimator_watchdog).start() self.q = [1.0, 0.0, 0.0, 0.0] # quaternion of sensor frame relative to auxiliary frame self.integralFB = [0.0, 0.0, 0.0] # integral error terms scaled by Ki self._last_rpy = [0.0, 0.0, 0.0] self._last_gyro = [0.0, 0.0, 0.0] self._last_acc = [0.0, 0.0, 0.0] self._last_imu_update = time.time() self._last_xyz = [0.0, 0.0, 0.0] self._last_xyz_raw = [0.0, 0.0, 0.0] self._last_dxyz = [0.0, 0.0, 0.0] self._last_vicon_rpy = [0.0, 0.0, 0.0] self._last_vicon_update = time.time() self._valid_vicon = False self._vicon_alpha_pos = .8 self._vicon_alpha_vel = .7 self._vicon_yaw = 0.0 self._use_rpydot = use_rpydot self._publish_to_lcm = publish_to_lcm if publish_to_lcm: self._xhat_lc = lcm.LCM() self._listen_to_vicon = listen_to_vicon self._vicon_channel = vicon_channel if listen_to_vicon: Thread(target=self._vicon_listener).start() self._input_log = list() self._last_input = [0.0, 0.0, 0.0, 0.0] self._last_input_time = time.time() self._delay_comp = delay_comp if delay_comp: self._cf_model = Crazyflie2() self._delay = 0.028 # delay in the control loop in seconds self._use_ekf = use_ekf self._use_ukf = use_ukf self._use_kalman = use_ekf or use_ukf if self._use_kalman: # states: x y z dx dy dz accxbias accybias acczbias # inputs: roll pitch yaw accx accy accz # measurements: x y z self._plant = DoubleIntegrator() self._last_kalman_update = time.time() if use_ekf: self._kalman = ExtendedKalmanFilter(dim_x=9, dim_z=3, plant=self._plant) elif use_ukf: self._kalman = UnscentedKalmanFilter(dim_x=9, dim_z=3, plant=self._plant)
def update_and_predict(measurement, OTHER = None):
"""Return the predicted x and P of target."""
dt = 1
# initialization stage: 0 measurements
# warming-up stage: 0-2 measurements
if OTHER == None or OTHER[0] == 0:
if OTHER == None:
OTHER = []
OTHER.append(0)
data = ExtendedKalmanFilter()
data.set_x(measurement[0], measurement[1], 0., 0., 0.)
data.set_P(1000.)
data.set_dt(dt)
OTHER.append(data)
if len(OTHER) == 3:
OTHER[0] = 1
# transition stage: 3 measurements
elif OTHER[0] == 1:
x_prev1,y_prev1 = OTHER[1].get_coordinates()
x_prev2,y_prev2 = OTHER[2].get_coordinates()
x_prev3,y_prev3 = measurement[0], measurement[1]
distance = distance_between((x_prev2,y_prev2),(x_prev3,y_prev3))
heading = angle_trunc(atan2(y_prev3-y_prev2,x_prev3-x_prev2))
turning = angle_trunc(atan2(y_prev3-y_prev2,x_prev3-x_prev2) - \
atan2(y_prev2-y_prev1,x_prev2-x_prev1))
data = ExtendedKalmanFilter()
data.set_x(x_prev3, y_prev3, distance, heading, turning)
data.set_P(1000.)
data.set_dt(dt)
data.update(measurement)
data.predict()
OTHER = [data]
# predicting stage: > 3 measurements
else:
OTHER[0].update(measurement)
OTHER[0].predict()
return OTHER
alphas = np.array([0.05**2, 0.005**2, 0.1**2, 0.01**2])
beta = np.diag([np.deg2rad(5)**2])
env = Field(args.data_factor * alphas, args.data_factor * beta)
policy = policies.OpenLoopRectanglePolicy()
initial_mean = np.array([180, 50, 0]).reshape((-1, 1))
initial_cov = np.diag([10, 10, 1])
if args.filter_type == 'none':
filt = None
elif args.filter_type == 'ekf':
filt = ExtendedKalmanFilter(
initial_mean,
initial_cov,
args.filter_factor * alphas,
args.filter_factor * beta
)
elif args.filter_type == 'pf':
filt = ParticleFilter(
initial_mean,
initial_cov,
args.num_particles,
args.filter_factor * alphas,
args.filter_factor * beta
)
# You may want to edit this line to run multiple localization experiments.
localize(env, policy, filt, initial_mean, args.num_steps, args.plot)
est_k] # last state estimate vector from robot frame
P = est_rbt_P[:, :, est_k] # last covariance matrix
Q = est_rbt_Q # process noise covariance matrix
# R = est_rbt_R # measurement noise covariance
R = est_rbt_s[:, :, est_k] # measurement noise covariance
H = est_rbt_H # observation matrix
z = est_rbt_m[:, est_k] # measurement vector
u = matrix(
[[0], [0], [0]]
) # control input vector (don't give kalman filter knowledge about thruster inputs)
# Discrete EKF
A = est_rbt_A
B = est_rbt_B
state = ExtendedKalmanFilter(x, P, z, u, A, B, Q, R, H)
x, P = state.predict(x, P, u)
x, K, P = state.update(x, P, z)
# print('x', x)
# # print('P', P)
# # print('Q', Q)
# # print('R', R)
# # print('H', H)
# # print('z', z)
# # print('u', u)
# # print('K', K)
# Store state estimate
est_rbt_x[:, est_k + 1] = x
est_rbt_L[:, :, est_k + 1] = K
def estimate_next_pos(measurement, OTHER = None):
"""Estimate the next (x, y) position of the wandering Traxbot
based on noisy (x, y) measurements."""
"""OTHER has the following data structure:
[flag,ExtendedKalmanFilter]"""
dt = 1
if OTHER == None or OTHER[0] == 0:
if OTHER == None:
OTHER = []
OTHER.append(0)
data = ExtendedKalmanFilter()
data.set_x(measurement[0], measurement[1], 0., 0., 0.)
data.set_P(1000.)
data.set_dt(dt)
xy_estimate = measurement[0], measurement[1]
OTHER.append(data)
if len(OTHER) == 3:
OTHER[0] = 1
return xy_estimate, OTHER
elif OTHER[0] == 1:
x_prev1,y_prev1 = OTHER[1].get_coordinates()
x_prev2,y_prev2 = OTHER[2].get_coordinates()
x_prev3,y_prev3 = measurement[0], measurement[1]
distance = distance_between((x_prev2,y_prev2),(x_prev3,y_prev3))
heading = angle_trunc(atan2(y_prev3-y_prev2,x_prev3-x_prev2))
turning = angle_trunc(atan2(y_prev3-y_prev2,x_prev3-x_prev2) - atan2(y_prev2-y_prev1,x_prev2-x_prev1))
data = ExtendedKalmanFilter()
data.set_x(x_prev3, y_prev3, distance, heading, turning)
data.set_P(1000.)
data.set_dt(dt)
data.update(measurement)
data.predict()
x_predict,y_predict = data.get_coordinates()
xy_estimate = (x_predict, y_predict)
OTHER[0] = 2
del OTHER[1:3]
OTHER.append(data)
return xy_estimate, OTHER # You must return xy_estimate (x, y), and OTHER (even if it is None)
else:
OTHER[1].update(measurement)
OTHER[1].predict()
x_predict,y_predict = OTHER[1].get_coordinates()
xy_estimate = (x_predict, y_predict)
return xy_estimate, OTHER # You must return xy_estimate (x, y), and OTHER (even if it is None)
# ax[j,i].set_ylabel("Radians")
if j == 1:
ax[j, i].set_xlabel("Seconds")
for i in iterations:
# Approximate the initial state
xs = np.array([i, i, angle])
angle += angle_increment
# Run the iekf
iekf = InvariantEKF(sys, xs, np.eye(3))
mus_iekf, sigmas = iekf.iterate(u, z)
# Run the ekf
z_trunc = z[:, :2]
ekf = ExtendedKalmanFilter(sys, xs, np.eye(3))
mus_ekf, sigmas = ekf.iterate(u, z_trunc)
# mus_ekf[:,2] = (mus_ekf[:,2] + np.pi) % (2 * np.pi) - np.pi # Convert angles to be between -pi and pi
mus_ekf[:, 2] = shift_to_final(angle_final, np.unwrap(mus_ekf[:, 2]))
# plot x y and thetas
ax[0, 0].plot(xaxis, mus_ekf[:, 0], '--')
ax[0, 1].plot(xaxis, mus_ekf[:, 1], '--')
ax[0, 2].plot(xaxis, mus_ekf[:, 2], '--')
ax[1, 0].plot(xaxis, mus_iekf[:, 0, 2], '--')
ax[1, 1].plot(xaxis, mus_iekf[:, 1, 2], '--')
ax[1, 2].plot(
xaxis,
shift_to_final(
angle_final,
def read_traceXY(self, file, type, verb, plot, psum, init_x, init_y):
with open(file) as f:
op = None
s = f.readlines()
i = 0
read_num = False
read_locs = False
read_uncertainties = 0
num = 0
init_xy = [[0, 0], [0, 0]]
static_xy = [[0, 0], [0, 0]]
m_var = [0, 0]
a_var = [0, 0]
d_var = [0, 0]
h_var = [0, 0]
self.r0_est_pts_x = []
self.r0_est_pts_y = []
self.r0_gt_pts_x = []
self.r0_gt_pts_y = []
self.r1_est_pts_x = []
self.r1_est_pts_y = []
self.r1_gt_pts_x = []
self.r1_gt_pts_y = []
self.r0_sim_x = []
self.r0_sim_y = []
self.r1_sim_x = []
self.r1_sim_y = []
self.err0 = 0
self.err0_count = 0
self.err0_final = 0
self.err1 = 0
self.err1_count = 0
self.err1_final = 0
while (i < len(s)):
if (len(s[i]) <= 1 or s[i][0] == '#'):
i = i + 1
continue
if (not (read_num)):
num = int(s[i].split()[0])
read_num = True
i = i + 1
continue
if (not (read_locs)):
x = s[i].split()
x0 = float(x[0])
y0 = float(x[1])
x = s[i + 1].split()
x1 = float(x[0])
y1 = float(x[1])
init_xy = [[x0, y0], [init_x, init_y]]
static_xy = [[x0, y0], [init_x, init_y]]
self.r0_gt_pts_x.extend([x0])
self.r0_gt_pts_y.extend([y0])
self.r0_est_pts_x.extend([x0])
self.r0_est_pts_y.extend([y0])
self.r1_gt_pts_x.extend([x1])
self.r1_gt_pts_y.extend([y1])
self.r1_est_pts_x.extend([init_x])
self.r1_est_pts_y.extend([init_y])
self.r0_sim_x.extend([[], x0])
self.r0_sim_y.extend([[], y0])
self.r1_sim_x.extend([[], x1])
self.r1_sim_y.extend([[], y1])
read_locs = True
i = i + 2
continue
if (read_uncertainties < self.NUM_UNCERTAINTIES):
x = s[i].split()
if (x[0] == 'O'):
m_var = [float(x[1]), float(x[2])]
elif (x[0] == 'A'):
a_var = [
math.radians(math.sqrt(float(x[1])))**2,
math.radians(math.sqrt(float(x[2])))**2
]
elif (x[0] == 'D'):
d_var = [float(x[1]), float(x[2])]
elif (x[0] == 'H'):
h_var = [
math.radians(math.sqrt(float(x[1])))**2,
math.radians(math.sqrt(float(x[2])))**2
]
read_uncertainties = read_uncertainties + 1
i = i + 1
continue
if (read_num and read_uncertainties == self.NUM_UNCERTAINTIES
and read_locs):
break
if (type == 0):
op = OdometryLocalize(init_xy, m_var, a_var, d_var, h_var)
elif (type == 1):
op = ExtendedKalmanFilter(init_xy, m_var, a_var, d_var, h_var)
#elif(type == 2):
# op = GridLocalize(init_xy, m_var, a_var, d_var, h_var)
elif (type == 3):
op = UnscentedKalmanFilter(init_xy, m_var, a_var, d_var, h_var)
elif (type == 4):
op = ParticleLocalize(init_xy, m_var, a_var, d_var, h_var)
# elif(type == 5):
# op = ExtendedKalmanFilterWPH(init_xy, m_var, a_var, d_var, h_var)
else:
print "Unknown filter type"
sys.exit()
counter = 0
while (i < len(s)):
line = s[i]
if (len(line) <= 1 or line[0] == '#'):
i = i + 1
continue
x = line.split()
if x[0] == 'M':
d0 = float(x[1])
h0 = math.radians(float(x[2]))
d1 = float(x[3])
h1 = math.radians(float(x[4]))
op.measure_movement([d0, d1], [h0, h1])
(x0, y0) = op.get_possible_pose_estimates(0)
(x1, y1) = op.get_possible_pose_estimates(1)
self.r0_sim_x.extend([x0])
self.r0_sim_y.extend([y0])
self.r1_sim_x.extend([x1])
self.r1_sim_y.extend([y1])
elif x[0] == 'D':
d0 = float(x[1])
d1 = float(x[2])
ph0 = math.radians(float(x[3]))
ph1 = math.radians(float(x[4]))
op.measure_distance([d0, d1], [ph0, ph1])
(x0, y0) = op.get_pose_estimate(0)
(x1, y1) = op.get_pose_estimate(1)
self.r0_sim_x.extend([x0])
self.r0_sim_y.extend([y0])
self.r1_sim_x.extend([x1])
self.r1_sim_y.extend([y1])
elif x[0] == 'C':
(x0, y0) = (float(x[1]), float(x[2]))
(x1, y1) = (float(x[3]), float(x[4]))
(pred_x0, pred_y0) = op.get_pose_estimate(0)
(pred_x1, pred_y1) = op.get_pose_estimate(1)
if verb or (i >= len(s) - 7 and psum):
print "Rover 0 abs. pose %d: (%f, %f)" % (counter, x0,
y0)
print "Rover 0 est. pose %d: (%f, %f)" % (
counter, pred_x0, pred_y0)
pd0 = math.sqrt((pred_x0 - x0)**2 + (pred_y0 - y0)**2)
d0 = math.sqrt((x0 - static_xy[0][0])**2 +
(y0 - static_xy[0][1])**2)
self.err0_count += 1
self.err0_final = (100.0 * (abs(pd0) / d0))
self.err0 += self.err0_final
if verb or (i >= len(s) - 7 and psum):
print "Rover 0 error: %f%%" % (100.0 * (abs(pd0) / d0))
self.r0_gt_pts_x.extend([x0])
self.r0_gt_pts_y.extend([y0])
self.r0_est_pts_x.extend([pred_x0])
self.r0_est_pts_y.extend([pred_y0])
if verb or (i >= len(s) - 7 and psum):
print "Rover 1 abs. pose %d: (%f, %f)" % (counter, x1,
y1)
print "Rover 1 est. pose %d: (%f, %f)" % (
counter, pred_x1, pred_y1)
pd1 = math.sqrt((pred_x1 - x1)**2 + (pred_y1 - y1)**2)
d1 = math.sqrt((x1 - static_xy[1][0])**2 +
(y1 - static_xy[1][1])**2)
self.err1_count += 1
self.err1_final = (100.0 * (abs(pd1) / d1))
self.err1 += self.err1_final
if verb or (i >= len(s) - 7 and psum):
print "Rover 1 error: %f%%" % (100.0 * (abs(pd1) / d1))
print "\n"
self.r1_gt_pts_x.extend([x1])
self.r1_gt_pts_y.extend([y1])
self.r1_est_pts_x.extend([pred_x1])
self.r1_est_pts_y.extend([pred_y1])
counter = counter + 1
else:
print "Invalid trace file"
i = i + 1
if verb:
print self.r0_gt_pts_x
print self.r0_gt_pts_y
if plot:
# self.plot_cont(len(self.r0_sim_x) / 2, 500.0, self.r0_sim_x, self.r0_sim_y, self.r0_gt_pts_x, self.r0_gt_pts_y, self.r1_sim_x, self.r1_sim_y, self.r1_gt_pts_x, self.r1_gt_pts_y)
self.plot_points(self.r0_gt_pts_x, self.r0_gt_pts_y,
self.r0_est_pts_x, self.r0_est_pts_y,
self.r1_gt_pts_x, self.r1_gt_pts_y,
self.r1_est_pts_x, self.r1_est_pts_y)