def __init__(self):
self.c = carController.carController()
self.box = boundingBox.boundingBox()
self.lastbox1 = boundingBox.boundingBox()
self.lastbox2 = boundingBox.boundingBox()
self.lastbox3 = boundingBox.boundingBox()
self.lastbox4 = boundingBox.boundingBox()
self.lastbox5 = boundingBox.boundingBox()
self.empty = boundingBox.boundingBox()
self.empty.setPrediction(1)
self.car = boundingBox.carCharacteristics()
self.flag = 0
self.pred = ""
#min_area=10000.0
#max_area=42000.0
self.check = 0
self.time = rospy.get_time()
self.rtime = 0.0
self.input = ""
self.count = 0
self.sub = rospy.Subscriber("/detectnet/detections", String,
self.callback)
self.manualControl = 0
self.sub2 = rospy.Subscriber("joy", Joy, self.callback2)
self.sub3 = rospy.Subscriber("depthwitherror", Float32MultiArray,
self.callback3)
self.kf = kalman_filter.kalman_filter(1023, 0.125, 32, 0)
def fake_path_trick(alpha_kalman, sigma_e, sigma_o, sigma_u, sigma_r, sigma_v,
sigma_w, p_a, p_c, L, L_train, S, t_c_fixed, c_fixed,
t_r_fixed, r_fixed):
L_train = len(alpha_kalman)
mu_0_fp = 50 * np.random.rand()
delta_0_fp = 0.
gamma_0_fp = 0.5 * np.random.rand(S - 1) - 0.25
(y_fp, mu_fp, delta_fp, gamma_fp, mu_with_initial_values_fp,
delta_with_initial_values_fp, gamma_with_initial_values_fp, gamma_vec_fp,
z_a_fp, z_c_fp) = generative_procedure_with_evaluation_period(
mu_0_fp, delta_0_fp, gamma_0_fp, sigma_e, sigma_o, sigma_u, sigma_r,
sigma_v, sigma_w, p_a, p_c, L_train, L_train, S, t_c_fixed, c_fixed,
t_r_fixed, r_fixed)
d = 1000.
a_1 = kalman_filter.gen_a_1(S, y_fp)
P_1 = kalman_filter.gen_P_1(S, d)
Z_t = kalman_filter.gen_Z_t(S)
T_t = kalman_filter.gen_T_t(S)
R_t = kalman_filter.gen_R_t(S)
H_e = sigma_e**2
H_o = sigma_o**2
Q_eta = kalman_filter.gen_Q_eta(sigma_r, sigma_v, sigma_w)
Q_xi = kalman_filter.gen_Q_eta(sigma_u, sigma_v, sigma_w)
a_t, P_t, K_t, F_t, v_t = kalman_filter.kalman_filter(
L_train, y_fp, a_1, P_1, Z_t, p_a, p_c, T_t, H_e, H_o, Q_eta, Q_xi,
R_t)
alpha_hat, V = kalman_smoothing(L_train, v_t, a_t, F_t, K_t, P_t, T_t, Z_t)
alpha_fp = []
for i in range(len(y_fp)):
alpha_fp_t = torch.zeros((S + 1, 1), dtype=torch.float64)
alpha_fp_t[0] = mu_fp[i]
alpha_fp_t[1] = delta_fp[i]
alpha_fp_t[2:, 0] = gamma_vec_fp[i]
alpha_fp.append(alpha_fp_t)
return [
a_fp - a_hat + a_kalman
for a_fp, a_hat, a_kalman in zip(alpha_fp, alpha_hat, alpha_kalman)
]
def run_tracking_demo(filename, plot=True):
ss = 4 # state size
os = 2 # observation size
A = np.matrix('1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1')
C = np.matrix('1 0 0 0; 0 1 0 0')
Q = 0.1 * np.identity(ss)
R = 1 * np.identity(os)
init_x = np.matrix([10, 10, 1, 0]).T
init_V = 10 * np.identity(ss)
T = 15
# Generate new data
np.random.seed(0)
x, y = sample_lds(A, C, Q, R, init_x, T)
# # Use mat file to reproduce Murphy's example
# mat = io.loadmat(filename)
# x = mat['x']
# y = mat['y']
# A = mat['F'][:,:,0]
# C = mat['H'][:,:,0]
# Q = mat['Q'][:,:,0]
# R = mat['R']
# init_x = mat['initx']
# init_V = mat['initV']
xfilt, Vfilt, VVfilt, loglik = kalman_filter(y, A, C, Q, R, init_x, init_V)
xsmooth, Vsmooth, VVsmooth, loglik = kalman_smoother(y, A, C, Q, R, init_x, init_V)
dfilt = x[[0,1],:] - xfilt[[0,1],:]
mse_filt = np.sqrt(np.sum(dfilt**2))
print "mse_filt: %f" % mse_filt
dsmooth = x[[0,1],:] - xsmooth[[0,1],:]
mse_dsmooth = np.sqrt(np.sum(dsmooth**2))
print "mse_dsmooth: %f" % mse_dsmooth
# Plot
if plot:
f, (ax1, ax2) = plt.subplots(1, 2, sharex=True,sharey=True)
ax1.plot(x[0,:], x[1,:], 'ks-')
ax1.plot(xfilt[0,:], xfilt[1,:], 'rx:')
ax1.set_title('Kalman Filter')
plot_2d_contours(ax1, xfilt, Vfilt)
ax2.plot(x[0,:], x[1,:], 'ks-')
ax2.plot(xsmooth[0,:], xsmooth[1,:], 'rx:')
ax2.set_title('Kalman Smoother')
plot_2d_contours(ax2, xsmooth, Vsmooth)
plt.show()
def usbl_move(pos):
broadcaster = tf.TransformBroadcaster()
if usbl_move.start:
dt = 2
loc = np.matrix([[0],#v_x
[0],#v_y
[pos.pose.position.x],#x
[pos.pose.position.y]])#z
A = np.matrix([[1, 0, 0, 0,],
[0, 1, 0, 0,],
[dt, 0, 1, 0,],
[0, dt, 0, 1,]])
B = np.matrix([0])
C = np.eye(loc.shape[0])
Q = np.eye(loc.shape[0])*0.5
R = np.eye(loc.shape[0])*5000
P = np.eye(loc.shape[0])
U = np.matrix( [[0]])
Z = np.matrix( [[0],[0],[0],[0]])
usbl_move.kalman = kalman_filter.kalman_filter(A,B,C,Q,P,R,loc)
usbl_move.md_filter = md_filter.md_filter(2, [1.75, 1.6], 10, [0, 1])
usbl_move.start = 0
if usbl_move.md_filter.update( [pos.pose.position.x,pos.pose.position.y] ):
#update.ax.scatter(pos.position.x,pos.position.y,-1*current.position.z,color='b')
current.position.x = pos.pose.position.x
current.position.y = pos.pose.position.y
#current.position.z = pos.pose.position.z
#update('b')
Z = np.matrix( [[0],[0],[pos.pose.position.x],[pos.pose.position.y] ])
U = np.matrix( [[0]])
usbl_move.kalman.move(U,Z)
kalman_pos = usbl_move.kalman.getState()
current.position.y = kalman_pos[2]
current.position.x = kalman_pos[3]
broadcaster.sendTransform( (current.position.x,current.position.y,current.position.z),
(current.orientation.x,current.orientation.y,current.orientation.z,current.orientation.w),
rospy.Time.now(), "body", "odom" )
def main():
"""
For the Kalman filter, find and plot the true and estimated signal
and the true and predicted error
"""
y = generate_obs_seq()
x_true = np.zeros(n0)
x_pred = np.zeros(n0)
P_true = np.zeros(n0)
P_pred = np.zeros(n0)
x_true[0] = y[0]
x_pred[0] = y[0]
P_true[0] = 1
P_pred[0] = 1
for i in range(1, n0):
x_true[i], x_pred[i], P_true[i], P_pred[i] = kalman_filter(
y[i], sigma_v, sigma_w, x_true[i - 1], P_true[i - 1])
# Plot and save the required plots
plt.figure()
plt.plot(x_true, label="True x[n]")
plt.plot(x_pred, label="Estimated x[n]")
plt.xlabel("n")
plt.ylabel("x[n]")
plt.title("True and Estimated signal")
plt.legend()
plt.savefig('./results/Kalman_1.png')
plt.figure()
plt.plot(P_true, label="True Error")
plt.plot(P_pred, label="Estimated Error")
plt.xlabel("n")
plt.ylabel("Error")
plt.title("True and Predicted error")
plt.legend()
plt.savefig('./results/Kalman_2.png')
0,
0,
dt,
0,
0,
1,
]])
B = np.matrix([0])
C = np.eye(pos.shape[0])
Q = np.eye(pos.shape[0]) * .05
R = np.eye(pos.shape[0]) * 50000
P = np.eye(pos.shape[0])
U = np.matrix([[0]])
Z = np.matrix([[0], [0], [0], [0], [0], [0]])
kalman = kalman_filter.kalman_filter(A, B, C, Q, P, R, pos)
md_filter = md_filter.md_filter(3, [1.6, 1.5], 70, [-30, 30])
out_x = []
out_y = []
out_z = []
new_x = []
new_y = []
new_z = []
last = pos
outliers = 0
for i in xrange(len(t)):
if (md_filter.update([x[i], y[i], z[i]])):
def load_frame(obj_name, trial_times, GtDict, roi_mask, start_id, end_id, ParamOptions):
"""
Get the formatted sub-image_list, append in polar or cart subimage list.
data_name given the source binary data file name.
frame_id, is indexed for several sequences in sequential time.
GT_dict give all the ground truth information for a specific target
roi_mask is the region of interesting for a tracking environment
"""
print ('Tracking %s:...' % obj_name)
resPath = '../results/Res_KF/'
#*_tbbs.txt for averaged boundingbox only
#*_blob.txt for matched blob
tracker_res_name = resPath + obj_name + '_blobs_%02d_option.txt' % trial_times
tracker_tbb_name = resPath + obj_name + '_KF_Tbbs.txt'
track_res_file = open(tracker_res_name, 'w')
tracker_tbbs_file = open(tracker_tbb_name, 'w')
# ParamOptions= {'BlobSeg': {'MorphIter':2, 'DilateIter':3, 'DistThreash':0.6, 'SubWindow':(128,64)},
# 'VotePsr':5,
# 'VoteDistThreash':100,
# 'PrioSigmaFactor':3./4,
# 'BlobScore':0.2
# }
segBlobParam = ParamOptions['BlobSeg']
# VotePsrThreash= ParamOptions['VotePsr']
# VoteDistThreash = ParamOptions['VoteDistThreash']
# BlobScoreThreash= ParamOptions['BlobScore']
# PrioSigmaFactor = ParamOptions['PrioSigmaFactor']
# NumberOfTrackers = ParamOptions['NumberOfTrackers']
data_name = []
frame = np.array([])
#polar frame in float32
dframe = np.array([])
#polar frame in uint8
uframe = np.array([])
#cartesian frame in float32
dispmat = np.array([])
#udispmat frame in uint8
udispmat = np.array([])
#pre and cur frame for flow computing
preframe = np.array([])
curframe = np.array([])
#tracker_dict = {}
tracker_list = []
track_rect_list = []
#ignore the damaged frames
damaged_frames = [1, 116, 232, 348]
frame_id = 0
# starting frame_id
#start_id = 60
frame_counter = 0
total_time_consum = 0
frame_mode = 'polar'
# reading the sequential 4 files in a loop
for i in np.arange(4):
file_prefix = '/Users/yizhou/Radar_Datasets/Zhicheng/zhicheng_20130808_22.'
file_suffix = '_5minutes_600x2048.double.data'
data_name = '%s%d%s' % (file_prefix, 28 + i * 5, file_suffix)
print (data_name)
fdata_obj = open(data_name, 'rb')
# loading file data and to convert from polar to cartesian
while True:
if frame_id > end_id:
#using empty frame to break the while loop
frame = np.array([])
break
else:
# read a frame from local file
frame = np.fromfile(fdata_obj, 'float64', 600 * 2048)
if frame.size != 0:
frame_id += 1
if frame_id >= start_id and frame_id not in damaged_frames:
key = 'frame %d' % frame_id
objGtElem = {}
if key not in GtDict:
print ('%s is not in GT files!!!' % key)
else:
objGtElem = GtDict[key]
frame = frame.reshape([2048, 600])
frame = frame.T
# 通过计算均值mean
# fmean = frame.mean(axis=1)
# mfm = np.tile(fmean,(600,1)).transpose()
frame = frame - 4000 # frame.mean()
# 归一化操作
# dframe is for computing in double float
dframe = uti.frame_normalize(frame)
# #uframe is for displaying,optical flow computing and finding blob
uframe = (dframe * 255).astype(np.uint8)
canvas_polar = cv2.cvtColor(uframe, cv2.COLOR_GRAY2BGR)
cv2.putText(canvas_polar, obj_name+' frame ' + str(frame_id), org=(10, 50),
fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1, color=(0, 255, 255),
thickness=2, lineType=cv2.LINE_AA)
#cv2.imshow('polar', canvas_polar)
# for cartesian-coordinates
dispmat = corcon.polar2disp_njit(dframe, np.array([]))
dispmat = uti.frame_normalize(dispmat)
udispmat = (dispmat * 255).astype(np.uint8)
canvas_disp = cv2.cvtColor(udispmat, cv2.COLOR_GRAY2BGR)
cv2.putText(canvas_disp, 'frame ' + str(frame_id), org=(10, 50),
fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1, color=(0, 255, 255),
thickness=2, lineType=cv2.LINE_AA)
#cv2.imshow('disp', canvas_disp)
# cv2.moveWindow('polar',0,0)
#cv2.waitKey(1)
if frame_mode == 'polar':
tframe = dframe * roi_mask
#echoThreash plays a role of CFAR.
echoThreash = np.mean(tframe)
tuframe = uframe
canvas_frame = canvas_polar
else:
tframe = dispmat
tuframe = udispmat
canvas_frame = canvas_disp
if frame_id == start_id :
# init previous frame
preframe = tuframe
obj_bbox = objGtElem['BoundingBox'] # frame60 Alice
# Uncomment the line below to select a different bounding box
# bbox = cv2.selectROI(frame, False)
# Initialize tracker with first frame and bounding box
#tracker = KCF_ST.KCFTracker_status()
cx = obj_bbox[0] + obj_bbox[2]/2
cy = obj_bbox[1] + obj_bbox[3]/2
tracker = KF.kalman_filter()
ok = tracker.init(cx,cy)
X_ = tracker.X0
P_ = tracker.P0
blob_list, seg_roi, sub_blob_list = uti.blob_seg(tframe, obj_bbox)
obj_blob, blob_score, blob_id = uti.vote_blob(blob_list, [obj_bbox])
if obj_blob=={}:
print ('No blob is found in first frame!!!')
else:
blob_key = 'frame ' + str(frame_id)
linedict = {}
linedict[blob_key] = obj_blob
track_res_file.writelines(str(linedict) + '\n')
track_res_file.flush()
bb_linedict = {}
tbb_key = 'frame ' + str(frame_id)
bb_linedict[tbb_key] = {'BoundingBox':obj_bbox}
tracker_tbbs_file.writelines(str(bb_linedict) + '\n')
track_rect_list.append(obj_bbox)
uti.draw_rect(canvas_frame, obj_bbox, (0, 0, 255), 2)
if frame_id > start_id:
# only counting the tracking time
#timer = uti.msTimer()
#ms_start = timer.start()
#tstart = time.clock()
tstart = time.clock()
#seg window is initialized by last frame's boundingbox
blob_list, seg_roi, sub_blob_list = uti.blob_seg(tframe, obj_bbox,
segBlobParam['MorphIter'], segBlobParam['DilateIter'], segBlobParam['DistThreash'],
segBlobParam['SubWindow'])
cx = obj_bbox[0] + obj_bbox[2]/2
cy = obj_bbox[1] + obj_bbox[3]/2
dist_min = 200
dist = 0
for blob in blob_list:
(bcx,bcy) = blob['Centroid']
dist = np.sqrt((cx-bcx)**2+(cy-bcy)**2)
if dist < dist_min:
dist_min = dist
z_x = bcx
z_y = bcy
if dist_min < 200:
(X_, P_, X_pr) = tracker.update(X_, P_, z_x, z_y)
obj_bbox = (int(X_[0][0] - obj_bbox[2]/2), int(X_[2][0]-obj_bbox[3]/2), obj_bbox[2], obj_bbox[3])
#print ('speed x direction %f, y direction %f\n' % (X_[1][0], X_[3][0]))
cv2.circle(canvas_frame, (int(X_pr[0][0]), int(X_pr[2][0])),2,(0,255,255),2)
cv2.circle(canvas_frame, (int(X_[0][0]), int(X_[2][0]) ),2,(0, 0, 255),2)
else:
#no blob selected, using prior_estimated information
#computing offset based on the last 4 frame's obj_bbox'center.
#using the average center shift as the (offset_x, offset_y)
if len(track_rect_list)<4:
prio_offset = (0,0)
else:
dif_rect = []
for iter in [-1,-2,-3]:
dif_rect.append(np.array(track_rect_list[iter]) - np.array(track_rect_list[iter-1]))
offset_rect = np.mean(dif_rect,0)
prio_offset = (offset_rect[0] + offset_rect[2]/2, offset_rect[1] + offset_rect[3]/2)
print( 'offset is (%d, %d)' %(prio_offset[0], prio_offset[1]))
prio_obj_bbox = (int(obj_bbox[0] + prio_offset[0]), int(obj_bbox[1] + prio_offset[1]), obj_bbox[2], obj_bbox[3])
obj_bbox = prio_obj_bbox
track_rect_list.append(obj_bbox)
total_time_consum = total_time_consum + time.clock() - tstart
frame_counter += 1.
if(DETAIL_MODE==True):
print ('frame %d cost %.4f seconds' % (frame_id, (time.clock() - tstart)))
print ('frame %d cost %.4f seconds' % (frame_id, mscost))
uti.draw_rect(canvas_frame, obj_bbox, (0, 0, 255), 2)
bb_linedict = {}
tbb_key = 'frame ' + str(frame_id)
bb_linedict[tbb_key] = {'BoundingBox':obj_bbox}
tracker_tbbs_file.writelines(str(bb_linedict) + '\n')
# copy seg_roi to the canvas_frame
roih, roiw = seg_roi.shape[0:2]
canvas_frame[420:420 + roih, 770:770 + roiw, :] = seg_roi
uti.draw_rect(canvas_frame, (770, 420, roiw, roih), (255, 255, 255))
if frame_mode == 'polar':
cv2.imshow('polar', canvas_frame)
if frame_counter == 1:
cv2.moveWindow('polar', 0, 0)
else:
cv2.imshow('disp', canvas_frame)
cv2.waitKey(1)
img_res_name = '%s%s\\%04d.jpg' % (resPath,obj_name, frame_id)
#cv2.imwrite(img_res_name, canvas_frame)
else:#readed file is empty
break
#closing dataset file
fdata_obj.close()
track_res_file.close()
time_per_frame = total_time_consum/frame_counter
print ('%s: average time consum per frame %fs \n' % (obj_name, time_per_frame))
return time_per_frame
observations = []
bar = ProgressBar()
for sim in bar(range(num_sims)):
# Create Random Physical Landmarks
landmarksx = np.empty(num_landmarks)
landmarksy = np.empty(num_landmarks)
seed()
for i in range(num_landmarks):
landmarksx[i] = uniform(-landmark_range, landmark_range)
landmarksy[i] = uniform(-landmark_range, landmark_range)
# Initialize Car and Kalman Filter
robot = robotcar(2, 0.5, num_landmarks, ts=ts)
kf = kalman_filter(robot)
# Initialize empty arrays for plotting
t = np.empty(n)
e = np.empty(n)
x = np.empty(n)
y = np.empty(n)
# features in an observation include:
# left_encoder
# right_encoder
# x_odom
# y_odom
# theta_odom
# for each landmark:
# bearing
[0, 1, 0, 0, 0, 0, 0, 0, ],
[0, 0, 1, 0, 0, 0, 0, 0, ],
[0, 0, 0, 1, 0, 0, 0, 0, ],
[dt, 0, 0, 0, 1, 0, 0, 0, ],
[0, dt, 0, 0, 0, 1, 0, 0, ],
[0, 0, dt, 0, 0, 0, 1, 0, ],
[0, 0, 0, dt, 0, 0, 0, 1, ]])
B = np.matrix([0])
C = np.eye(loc.shape[0])
Q = np.eye(loc.shape[0]) * q
R = np.eye(loc.shape[0]) * r
P = np.eye(loc.shape[0])
U = np.matrix([[0]])
Z = np.matrix([[0], [0], [0], [0], [0], [0], [0], [0] ])
kalman = kalman_filter.kalman_filter(A, B, C, Q, P, R, loc)
#loop through data and apply Kalman filter
for i in xrange(len(t)):
kalman.move(U, Z)
temp = kalman.getState()
new_x.append(temp[5])
new_y.append(temp[4]*-1)
new_z.append(temp[6]*-1)
new_w.append(temp[7])
Z = np.matrix([[0], [0], [0], [0], [my_x[i]], [my_y[i]], [my_z[i]], [my_w[i]] ])
U = np.matrix([[0]])
#print raw and filtered data
# initial state variance parameters
var_px0 = 10.
var_py0 = 100.
var_vx0 = 1.
var_vy0 = 1.
# initial state covariance
PI = NP.array([[var_px0, 0, 0, 0],
[ 0, var_vx0, 0, 0],
[ 0, 0, var_py0, 0],
[ 0, 0, 0, var_vy0]])
# execute filter
x_hat, P = kalman_filter(y, H, R, F, Q, mu, PI, z=z)
# gather results and plot
px_hat = [x_hat_i[0] for x_hat_i in x_hat]
py_hat = [x_hat_i[2] for x_hat_i in x_hat]
fig = PL.figure(figsize=(11, 4))
ax = fig.add_subplot(111)
PL.plot([x_i[0] for x_i in x],
[x_i[2] for x_i in x],
label='Truth')
PL.plot(px_hat,
py_hat,
color='r',
ls='-',
mec='None',
def pose_move(pos):
#pos.position.z is in kPa, has to be convereted to depth
# P = P0 + pgz ----> pos.position.z = P0 + pg*z_real
#z_real = -(1000*pos.position.z-101.325)/9.81
z_real = -pos.position.z
toDegree = 180 / math.pi
current.position.z = z_real
broadcaster = tf.TransformBroadcaster()
#set up the Kalman Filter
#tf.transformations.quaternion_from_euler()
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
-1 * pos.orientation.y, pos.orientation.x, -1 * pos.orientation.z,
pos.orientation.w
])
roll = roll * toDegree
pitch = pitch * toDegree
yaw = yaw * toDegree
s = 'The value of roll is ' + repr(roll) + ', and pitch is ' + repr(
pitch) + ', yaw = ' + repr(yaw)
print s
if pose_move.start:
dt = .02
pos = np.matrix([
[0], # v_x
[0], # v_y
[0], # v_z
[roll], # x
[pitch], # y
[yaw]
]) # z
A = np.matrix([[
1,
0,
0,
0,
0,
0,
], [
0,
1,
0,
0,
0,
0,
], [
0,
0,
1,
0,
0,
0,
], [
dt,
0,
0,
1,
0,
0,
], [
0,
dt,
0,
0,
1,
0,
], [
0,
0,
dt,
0,
0,
1,
]])
B = np.matrix([0])
C = np.eye(pos.shape[0])
Q = np.eye(pos.shape[0]) * .5
R = np.eye(pos.shape[0]) * 500
P = np.eye(pos.shape[0])
U = np.matrix([[0]])
Z = np.matrix([[0], [0], [0], [0], [0], [0]])
pose_move.kalman = kalman_filter.kalman_filter(A, B, C, Q, P, R, pos)
pose_move.start = 0
Z = np.matrix([[0], [0], [0], [roll], [pitch], [yaw]])
U = np.matrix([[0]])
pose_move.kalman.move(U, Z)
pos = pose_move.kalman.getState()
roll = pos[3]
pitch = pos[4]
yaw = pos[5]
# quad = tf.transformations.quaternion_from_euler(roll/toDegree, pitch/toDegree, yaw/toDegree )
quad = tf.transformations.quaternion_from_euler(0, 0, yaw / toDegree)
current.orientation.x = quad[0]
current.orientation.y = quad[1]
current.orientation.z = quad[2]
current.orientation.w = quad[3]
broadcaster.sendTransform(
(current.position.x, current.position.y, current.position.z),
(current.orientation.x, current.orientation.y, current.orientation.z,
current.orientation.w), rospy.Time.now(), "body", "odom")
img = np.array(image, dtype=np.uint8)
img.resize([resolution[1], resolution[0], 3])
vision_sensor_output = cv2.rotate(img, cv2.ROTATE_180)
try:
parsing_data = frame_buffer(vision_sensor_output)
elapsed_time = time.time() - starting_time
fps = frame_id / elapsed_time
#Unpacking float datas from gyro and accel
floatValues1 = vrep.simxUnpackFloats( gyro_out)
floatValues2 = vrep.simxUnpackFloats( accel_out)
#parsing data to alpha trimmmer
CF_out = ComplementaryFilter(floatValues2, floatValues1, eulerAngles, RPY)
#parsing data to alpha trimmmer
KF_Out = kalman_filter(filtered_data, 0, 0, 0)
display_frame = info_frame(parsing_data, filtered_data, fps, KF_Out, CF_out[2], position, position2)
cv2.imshow("Image", display_frame)
# print("Detection successful")
cv2.waitKey(100)
except:
cv2.imshow("Image", vision_sensor_output)
elif ERRORCODE == vrep.simx_return_novalue_flag:
pass
else:
print(ERRORCODE)
else:
vrep.simxFinish(clientID)
cv2.destroyAllWindows()
def usbl_move(pos):
broadcaster = tf.TransformBroadcaster()
if usbl_move.start:
dt = 2
loc = np.matrix([
[0], #v_x
[0], #v_y
[pos.pose.position.x], #x
[pos.pose.position.y]
]) #z
A = np.matrix([[
1,
0,
0,
0,
], [
0,
1,
0,
0,
], [
dt,
0,
1,
0,
], [
0,
dt,
0,
1,
]])
B = np.matrix([0])
C = np.eye(loc.shape[0])
Q = np.eye(loc.shape[0]) * 0.5
R = np.eye(loc.shape[0]) * 5000
P = np.eye(loc.shape[0])
U = np.matrix([[0]])
Z = np.matrix([[0], [0], [0], [0]])
usbl_move.kalman = kalman_filter.kalman_filter(A, B, C, Q, P, R, loc)
usbl_move.md_filter = md_filter.md_filter(2, [1.75, 1.6], 10, [0, 1])
usbl_move.start = 0
if usbl_move.md_filter.update([pos.pose.position.x, pos.pose.position.y]):
#update.ax.scatter(pos.position.x,pos.position.y,-1*current.position.z,color='b')
current.position.x = pos.pose.position.x
current.position.y = pos.pose.position.y
#current.position.z = pos.pose.position.z
#update('b')
Z = np.matrix([[0], [0], [pos.pose.position.x], [pos.pose.position.y]])
U = np.matrix([[0]])
usbl_move.kalman.move(U, Z)
kalman_pos = usbl_move.kalman.getState()
current.position.y = kalman_pos[2]
current.position.x = kalman_pos[3]
broadcaster.sendTransform(
(current.position.x, current.position.y, current.position.z),
(current.orientation.x, current.orientation.y, current.orientation.z,
current.orientation.w), rospy.Time.now(), "body", "odom")
def pose_move(pos): #pos.position.z is in kPa, has to be convereted to depth # P = P0 + pgz ----> pos.position.z = P0 + pg*z_real #z_real = -(1000*pos.position.z-101.325)/9.81 z_real = -pos.position.z toDegree = 180/math.pi current.position.z = z_real broadcaster = tf.TransformBroadcaster() #set up the Kalman Filter #tf.transformations.quaternion_from_euler() (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([-1*pos.orientation.y, pos.orientation.x, -1*pos.orientation.z, pos.orientation.w]) roll = roll*toDegree pitch = pitch*toDegree yaw = yaw*toDegree s = 'The value of roll is ' + repr(roll) + ', and pitch is ' + repr(pitch) + ', yaw = ' + repr(yaw) print s if pose_move.start: dt = .02 pos = np.matrix([[0], # v_x [0], # v_y [0], # v_z [roll], # x [pitch], # y [yaw]]) # z A = np.matrix([[1, 0, 0, 0, 0, 0, ], [0, 1, 0, 0, 0, 0, ], [0, 0, 1, 0, 0, 0, ], [dt, 0, 0, 1, 0, 0, ], [0, dt, 0, 0, 1, 0, ], [0, 0, dt, 0, 0, 1, ]]) B = np.matrix([0]) C = np.eye(pos.shape[0]) Q = np.eye(pos.shape[0]) * .5 R = np.eye(pos.shape[0]) * 500 P = np.eye(pos.shape[0]) U = np.matrix([[0]]) Z = np.matrix([[0], [0], [0], [0], [0], [0] ]) pose_move.kalman = kalman_filter.kalman_filter(A,B,C,Q,P,R,pos) pose_move.start = 0 Z = np.matrix([[0], [0], [0], [roll], [pitch], [yaw]]) U = np.matrix([[0]]) pose_move.kalman.move(U, Z) pos = pose_move.kalman.getState() roll = pos[3] pitch = pos[4] yaw = pos[5] # quad = tf.transformations.quaternion_from_euler(roll/toDegree, pitch/toDegree, yaw/toDegree ) quad = tf.transformations.quaternion_from_euler(0, 0, yaw/toDegree ) current.orientation.x = quad[0] current.orientation.y = quad[1] current.orientation.z = quad[2] current.orientation.w = quad[3] broadcaster.sendTransform( (current.position.x,current.position.y,current.position.z), (current.orientation.x,current.orientation.y,current.orientation.z, current.orientation.w), rospy.Time.now(), "body", "odom" )
controller = LQR(K, max_input=20, max_input_on=False)
""" OBSERVER REPRESENTATION """
# Convert to discrete time
inverse_pendulum_plant_d_kalman = inverse_pendulum_plant.to_discrete(
dt_control)
# Extract discrete state matrices
A_discrete_k = inverse_pendulum_plant_d_kalman.A
B_discrete_k = inverse_pendulum_plant_d_kalman.B
C_discrete_k = inverse_pendulum_plant_d_kalman.C
D_discrete_k = inverse_pendulum_plant_d_kalman.D
# Kalman Filter
kf = kalman_filter(A_discrete_k, B_discrete_k, C_discrete_k, Q_kalman,
R_kalman)
# initilize states and covariance
kf.set_x_last(states)
kf.set_P_last(np.eye(np.size(A, 1)) * noise_val**2)
#kf = kalman_filter(A, B, C, Q, R)
""" UTILITY FUNCTIONS """
# Take a state input and add random noise
def add_noise(states):
for i in range(len(states - 1)):
noise = (random.random() * 2 - 1) * noise_val
states[i] += noise
def pos_move(pos, R, correction): # Note pos and current position have x and y swapped. # -> pos.position.x corresponds to current.position.y # Check that the values aren't crazy if correction: max = 5 # The pos is in world coordinates need to translate to kalman filter coordinates (x->y, y->x) a = quaternion_matrix([pos.pose.orientation.x, pos.pose.orientation.y, pos.pose.orientation.z, pos.pose.orientation.w]) a[0,3] = pos.pose.position.x a[1,3] = pos.pose.position.y b = np.array([current.position.x, current.position.y, 0, 1]) c = a.dot(b) pos.pose.position.x = c[1] pos.pose.position.y = c[0] else: max = 10 if(abs(pos.pose.position.x - current.position.y) > max or abs(pos.pose.position.y - current.position.x) > max ): print "CRAZY POSITION" return broadcaster = tf.TransformBroadcaster() if pos_move.start: dt = 0.0 loc = np.matrix([[0],#v_x [0],#v_y [pos.pose.position.x],#x [pos.pose.position.y]])#y A = np.matrix([[1, 0, 0, 0,], [0, 1, 0, 0,], [dt, 0, 1, 0,], [0, dt, 0, 1,]]) B = np.matrix([0]) C = np.eye(loc.shape[0]) Q = np.eye(loc.shape[0])*0.5 # R = np.eye(loc.shape[0])*5000 P = np.eye(loc.shape[0]) U = np.matrix( [[0]]) Z = np.matrix( [[0],[0],[0],[0]]) pos_move.kalman = kalman_filter.kalman_filter(A,B,C,Q,P,R,loc) pos_move.md_filter = md_filter.md_filter(2, [1.75, 1.6], 10, [0, 1]) pos_move.path = Path() pos_move.path.header.frame_id="odom" pos_move.start = 0 if not correction: pos.pose.position.x += pos_move.xd pos.pose.position.y += pos_move.yd # if pos_move.md_filter.update( [pos.pose.position.x,pos.pose.position.y] ) or correction or not correction: Z = np.matrix( [[0],[0],[pos.pose.position.x],[pos.pose.position.y] ]) U = np.matrix( [[0]]) pos_move.kalman.move(U,Z, R) kalman_pos = pos_move.kalman.getState() if correction: # We are subscribing to the inverse translation, thus the minus sign. pos_move.xd += kalman_pos.item(2) - current.position.y pos_move.yd += kalman_pos.item(3) - current.position.x # print "addition: ", pose_move.yaw_addition # print "pos:", "x:", current.position.x, "y:", current.position.y # print "mx: ", pos_move.xd # print "my: ", pos_move.yd # print "yaw:", pose_move.yaw current.position.y = kalman_pos.item(2) current.position.x = kalman_pos.item(3) # else: # print "NOT ACCEPTED" path_now = PoseStamped() path_now.pose.position.x = current.position.x path_now.pose.position.y = current.position.y path_now.pose.position.z = 0 # from odom (parent) to body (child) broadcaster.sendTransform( (current.position.x,current.position.y,current.position.z), (current.orientation.x,current.orientation.y,current.orientation.z,current.orientation.w), rospy.Time.now(), "body", "odom" ) # from buffer (parent) to odom (child) broadcaster.sendTransform((-pos_move.yd, -pos_move.xd, 0), tf.transformations.quaternion_from_euler(0, 0, -pose_move.yaw/180*math.pi), rospy.Time.now(),"odom","buffer") pos_move.path.poses.append(path_now) path.publish(pos_move.path) pose_kalman = PoseStamped(); pose_kalman.pose.position.x = current.position.x pose_kalman.pose.position.y = current.position.y pose_kalman.pose.position.z = current.position.z pose_kalman.pose.orientation.x = current.orientation.x pose_kalman.pose.orientation.y = current.orientation.y pose_kalman.pose.orientation.z = current.orientation.z pose_kalman.pose.orientation.w = current.orientation.w pose_kalman.header.frame_id = "odom" pub_pose.publish(pose_kalman)
def pose_move(pos, R, correction): toDegree = 180/math.pi broadcaster = tf.TransformBroadcaster() (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([-1*pos.orientation.y, pos.orientation.x, -1*pos.orientation.z, pos.orientation.w]) roll = roll*toDegree pitch = pitch*toDegree yaw = yaw*toDegree if correction: if abs(yaw) > 45: print "CRAZY YAW" return roll = pose_move.kalman.getState().item(3) pitch = pose_move.kalman.getState().item(4) yaw = pose_move.kalman.getState().item(5) - yaw if pose_move.start: dt = .00 position = np.matrix([[0], # v_x [0], # v_y [0], # v_z [roll], # x [pitch], # y [yaw]]) # z A = np.matrix([[1, 0, 0, 0, 0, 0, ], [0, 1, 0, 0, 0, 0, ], [0, 0, 1, 0, 0, 0, ], [dt, 0, 0, 1, 0, 0, ], [0, dt, 0, 0, 1, 0, ], [0, 0, dt, 0, 0, 1, ]]) B = np.matrix([0]) C = np.eye(position.shape[0]) Q = np.eye(position.shape[0]) * .5 R = np.eye(position.shape[0]) * 50 P = np.eye(position.shape[0]) U = np.matrix([[0]]) Z = np.matrix([[0], [0], [0], [0], [0], [0] ]) pose_move.kalman = kalman_filter.kalman_filter(A,B,C,Q,P,R,position) pose_move.start = 0 if not correction: yaw += pose_move.yaw current.position.z = -pos.position.z # current.position.z = 0 # Z = np.matrix([[0], [0], [0], [roll], [pitch], [yaw+pose_move.yaw_addition]]) Z = np.matrix([[0], [0], [0], [roll], [pitch], [yaw]]) # Z = np.matrix([[0], [0], [0], [0], [0], [yaw]]) U = np.matrix([[0]]) tmp_yaw = pose_move.kalman.getState().item(5) pose_move.kalman.move(U, Z, R) pos = pose_move.kalman.getState() if correction: # print "yaw input:", yaw # print "yaw before:", tmp_yaw # print "yaw after:", pos[5] # print "yaw change:", yaw - tmp_yaw pose_move.yaw += pos.item(5) - tmp_yaw # print "yaw addition:", pose_move.yaw roll = pos[3] pitch = pos[4] yaw = pos[5] quad = tf.transformations.quaternion_from_euler(roll/toDegree, pitch/toDegree, yaw/toDegree ) # quad = tf.transformations.quaternion_from_euler(0, 0, yaw/toDegree ) current.orientation.x = quad[0] current.orientation.y = quad[1] current.orientation.z = quad[2] current.orientation.w = quad[3] broadcaster.sendTransform( (current.position.x,current.position.y,current.position.z), (current.orientation.x,current.orientation.y,current.orientation.z, current.orientation.w), rospy.Time.now(), "body", "odom" ) broadcaster.sendTransform((-pos_move.yd,-pos_move.xd, 0), tf.transformations.quaternion_from_euler(0, 0, -pose_move.yaw/180*math.pi), rospy.Time.now(),"odom","buffer")