def __init__(self):
rospy.init_node('wheelVel_filter_node', anonymous=True)
self.odomFreq = float(rospy.get_param("~odom_freq", "20"))
self.sub_vw_r = rospy.Subscriber("v_wheel_right", Float32,
self.updateVW_R)
self.sub_vw_l = rospy.Subscriber("v_wheel_left", Float32,
self.updateVW_L)
# self.sub_imu = rospy.Subscriber("/imu", Float32, self.updateIMU)
# self.pub_raw_vel = rospy.Publisher("/raw_vel", Float32MultiArray, queue_size=1)
self.pub_filter_vel = rospy.Publisher("filter_vel",
Float32MultiArray,
queue_size=1)
# self.pub_filter_yaw = rospy.Publisher("/filter_yaw", Float32, queue_size=1)
self.timer_v_wheel = rospy.Timer(rospy.Duration(1.0 / self.odomFreq),
self.timerVWheelCB)
self.filterR = KalmanFilter(1, 30)
self.filterL = KalmanFilter(1, 30)
# self.filterYaw = KalmanFilter(0.01, 2)
self.filter_yaw = 0.0
self.yaw = 0.0
self.vw_r = 0.0
self.vw_l = 0.0
self.filter_vw_r = 0.0
self.filter_vw_l = 0.0
def LimpiezaDatosCSV (ruta):
i=0
valores_baliza1=[]
valores_baliza2=[]
valores_baliza3=[]
with open (ruta, newline='') as File:
reader = csv.reader(File)
for row in reader:
if i>0:
valores_baliza1 = valores_baliza1 + [float(row[6])]
valores_baliza2 = valores_baliza2 + [float(row[7])]
valores_baliza3 = valores_baliza3 + [float(row[8])]
i=i+1
#Una vez tenemos todos los valores de potencia de todas las balizas, eliminamos los cero por desconexion de la baliza
# y le aplicamos un filtro de Kalman y una media para tener un único valor de potencia por baliza
Balizas=[valores_baliza1, valores_baliza2, valores_baliza3]
RSSI_balizas=[]
for baliza in Balizas:
repeticiones = baliza.count(0.0)
for i in range (repeticiones):
baliza.remove(0.0)
valores_filtrados = KalmanFilter(baliza)
RSSI_balizas = RSSI_balizas + [sum(valores_filtrados)/len(valores_filtrados)]
return RSSI_balizas
def main():
# Create opencv video capture object
VideoCap = cv2.VideoCapture('video/randomball.avi')
#Variable used to control the speed of reading the video
ControlSpeedVar = 100 #Lowest: 1 - Highest:100
HiSpeed = 100
#Create KalmanFilter object KF
#KalmanFilter(dt, u_x, u_y, std_acc, x_std_meas, y_std_meas)
KF = KalmanFilter(0.1, 1, 1, 1, 0.1, 0.1)
debugMode = 1
while (True):
# Read frame
ret, frame = VideoCap.read()
# Detect object
centers = detect(frame, debugMode)
# If centroids are detected then track them
if (len(centers) > 0):
# Draw the detected circle
cv2.circle(frame, (int(centers[0][0]), int(centers[0][1])), 10,
(0, 191, 255), 2)
# Predict
(x, y) = KF.predict()
# Draw a rectangle as the predicted object position
cv2.rectangle(frame, (int(x) - 15, int(y) - 15),
(int(x) + 15, int(y) + 15), (255, 0, 0), 2)
# Update
(x1, y1) = KF.update(centers[0])
# Draw a rectangle as the estimated object position
cv2.rectangle(frame, (int(x1) - 15, int(y1) - 15),
(int(x1) + 15, int(y1) + 15), (0, 0, 255), 2)
cv2.putText(frame, "Estimated Position",
(int(x1) + 15, int(y1) + 10), 0, 0.5, (0, 0, 255), 2)
cv2.putText(frame, "Predicted Position", (int(x) + 15, int(y)), 0,
0.5, (255, 0, 0), 2)
cv2.putText(frame, "Measured Position",
(int(centers[0][0]) + 15, int(centers[0][1]) - 15), 0,
0.5, (0, 191, 255), 2)
cv2.imshow('image', frame)
if cv2.waitKey(2) & 0xFF == ord('q'):
VideoCap.release()
cv2.destroyAllWindows()
break
cv2.waitKey(HiSpeed - ControlSpeedVar + 1)
def __init__(self, theta, emax, cachelen):
self.cache = []
self.readings = []
self.totaltrans = 0
self.cacheLen = cachelen
self.theta = theta
self.emax = emax
self.counter = 0
self.correcteddata = []
self.totalTransferredData = 0
r_std, q_std = 1., 0.003
self.kf = KalmanFilter(dim_x=2, dim_z=1)
self.kf.x = np.array([[19], [0]]) # position, velocity
self.kf.F = np.array([[1, 1], [0, 1]])
self.kf.R = np.array([[r_std**2]]) # input noise
self.kf.H = np.array([[1., 0.]])
self.kf.P = np.diag([0.001**2, 0])
self.kf.Q = Q_discrete_white_noise(2, 1, q_std**2)
self.est_list = []
self.counter = 0
self.batCap = 3.6
self.variance = 0
self.V = 3.6
self.Itx = ((17.4 + 18.8 + 0.5 + 0.426) / 32768)
self.DetaT = 5
def ball_kf_noay(x, y, omega, v0, dt, r=0.5, q=0.02):
g = 9.8 # gravitational constant
f1 = KalmanFilter(dim_x=5, dim_z=2)
ay = .5 * dt**2
f1.F = np.mat([
[1, dt, 0, 0, 0], # x = x0+dx*dt
[0, 1, 0, 0, 0], # dx = dx
[0, 0, 1, dt, 0], # y = y0 +dy*dt
[0, 0, 0, 1, dt], # dy = dy0 + ddy*dt
[0, 0, 0, 0, 1]
]) # ddy = -g.
f1.H = np.mat([[1, 0, 0, 0, 0], [0, 0, 1, 0, 0]])
f1.R *= r
f1.Q *= q
omega = radians(omega)
vx = cos(omega) * v0
vy = sin(omega) * v0
f1.x = np.mat([x, vx, y, vy, -9.8]).T
return f1
def __init__(self, bbox):
"""
Initialises a tracker using initial bounding box.
"""
#define constant velocity model
self.kf = KalmanFilter(dim_x=7, dim_z=4)
self.kf.F = np.array([[1, 0, 0, 0, 1, 0, 0], [0, 1, 0, 0, 0, 1, 0],
[0, 0, 1, 0, 0, 0, 1], [0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 1]])
self.kf.H = np.array([[1, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0]])
self.kf.R[2:, 2:] *= 10.
self.kf.P[
4:,
4:] *= 1000. #give high uncertainty to the unobservable initial velocities
self.kf.P *= 10.
self.kf.Q[-1, -1] *= 0.01
self.kf.Q[4:, 4:] *= 0.01
self.kf.x[:4] = convert_bbox_to_z(bbox)
self.time_since_update = 0
self.id = KalmanBoxTracker.count
KalmanBoxTracker.count += 1
self.history = []
self.hits = 0
self.hit_streak = 0
self.age = 0
def example1():
"""
Test Kalman Filter implementation
:return:
"""
dt = 1.0 / 60
F = np.array([[1, dt, 0], [0, 1, dt], [0, 0, 1]])
H = np.array([1, 0, 0]).reshape(1, 3)
Q = np.array([[0.05, 0.05, 0.0], [0.05, 0.05, 0.0], [0.0, 0.0, 0.0]])
R = np.array([0.5]).reshape(1, 1)
x = np.linspace(-10, 10, 100)
measurements = -(x**2 + 2 * x - 2) + np.random.normal(0, 2, 100)
kf = KalmanFilter(F=F, H=H, Q=Q, R=R)
predictions = []
for z in measurements:
predictions.append(np.dot(H, kf.predict())[0])
kf.update(z)
plt.plot(range(len(measurements)), measurements, label='Measurements')
plt.plot(range(len(predictions)),
np.array(predictions),
label='Kalman Filter Prediction')
plt.legend()
plt.show()
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
self.pos_goal = pos_goal
# YOUR CODE AFTER THIS
#-------------------------------------------#
self.time = rospy.get_time()
self.controlOut = (0.0, 0.0, False)
self.count_noMeasurement = 0
#-------------------------------------------#
self.markers = world_map
self.idx_target_marker = 0
# Calculate the optimal path
# From pos_init to pos_goal
self.path_2D = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init,
pos_goal)
self.idx_path = 0
self.size_path = self.path_2D.shape[0]
print "path.shape[0]", self.size_path
# Generate the 3D path (include "theta")
self.path = np.zeros((self.size_path, 3))
theta = 0.0
for idx in range(self.size_path - 1):
delta = self.path_2D[(idx + 1), :] - self.path_2D[idx, :]
theta = atan2(delta[1], delta[0])
if theta > np.pi:
theta -= np.pi * 2
elif theta < -np.pi:
theta += np.pi * 2
self.path[idx, :] = np.concatenate(
(self.path_2D[idx, :], np.array([theta])), axis=1)
self.path[self.size_path - 1, 0:2] = self.path_2D[self.size_path - 1,
0:2]
self.path[self.size_path - 1, 2] = pos_goal[2] # theta
#
self.path[0, 2] = pos_init[2] # theta
print "3D path:"
print self.path
# Uncomment as completed
# Kalman filter
self.kalman_filter = KalmanFilter(world_map)
self.kalman_filter.mu_est = pos_init # 3*pos_init # For test
# Differential drive
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
self.wayPointFollowing = wayPointFollowing(max_speed, max_omega)
#
self.task_done = False
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# TODO for student: Comment this when running on the robot
#self.robot_sim = RobotSim(world_map, occupancy_map, pos_init, pos_goal,
# max_speed, max_omega, x_spacing, y_spacing)
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
# speed control variables
self.v = 0.1 # allows persistent cmds through detection misses
self.omega = -0.1 # allows persistent cmds through detection misses
self.last_detect_time = rospy.get_time() #TODO on bot only
self.missed_vision_debounce = 1
self.start_time = 0
# generate the path assuming we know our start location, goal, and environment
self.path = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init,
pos_goal)
self.path_idx = 0
self.mission_complete = False
self.carrot_distance = 0.22
# Uncomment as completed
self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def run(self):
obj_xyzvels = self.get_obj_list()
for bd_id, xyzvel in obj_xyzvels.items():
cur_pos = xyzvel[['center_x', 'center_y']].iloc[0].values
cur_vel = xyzvel[['speed_x', 'speed_y']].iloc[0].values
pred_duration = 5
kf = KalmanFilter()
kf.motion_model(cur_pos, cur_vel, pred_duration)
return
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# TODO for student: Comment this when running on the robot
self.robot_sim = RobotSim(world_map, occupancy_map, pos_init, pos_goal,
max_speed, max_omega, x_spacing, y_spacing)
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
#self.ros_interface = ROSInterface(t_cam_to_body)
# YOUR CODE AFTER THIS
self.pos_goal = pos_goal
self.world_map = world_map
self.vel = np.array([0., 0.])
self.imu_meas = np.array([])
self.meas = []
self.max_speed = max_speed
self.max_omega = max_omega
self.goals = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init,
pos_goal)
# print(self.goals)
self.total_goals = self.goals.shape[0]
self.cur_goal = 2
self.end_goal = self.goals.shape[0] - 1
self.est_pose = None
# Uncomment as completed
self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
def __init__(self, detection, trackId):
super(Tracks, self).__init__()
self.KF = KalmanFilter()
self.KF.predict()
self.KF.correct(np.matrix(detection).reshape(2, 1))
self.trace = deque(maxlen=500)
self.nframe = deque(maxlen=500)
self.prediction = detection.reshape(1, 2)
self.trackId = trackId
self.skipped_frames = 0
def __init__(self, initialPosition, initialWidth, initialHeight, frame,
parametersNew):
#########################################
#########################################
self.initFrame = frame
self.initPos = initialPosition
self.initW = initialWidth
self.initH = initialHeight
self.KM = KalmanFilter()
self.MF = MaskingFilter()
self.KM.setStatePost(
np.array([initialPosition[0], initialPosition[1], 0.,
0.]).reshape(4, 1))
self.selectionWidth = initialWidth
self.selectionHeight = initialHeight
self.prevFrameGray = None
self.frameCounter = 0 #Shape = {tuple}: (x, 1, 2)
#Por ejemplo: [[[x1 y1]]\n\n [[x2 y2]]\n\n [[x3 y3]]]
#es decir una matriz de x filas y 1 columna, donde cada elemento
#de la unica columna es una coordenada [x y].
if self.MF.mask is not self.MF.maskingType["FILTER_OFF"]:
self.MF.calculateNewMask(
frame, frame[int(initialPosition[1] -
initialHeight / 2):int(initialPosition[1] +
initialHeight / 2),
int(initialPosition[0] -
initialWidth / 2):int(initialPosition[0] +
initialWidth / 2)])
frame = self.MF.filterFrame(frame)
self.prevFrameGray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
self.SC = Searcher(self.initFrame, initialHeight, initialWidth,
initialPosition[0], initialPosition[1],
cv.cvtColor(frame, cv.COLOR_BGR2GRAY))
self.SC.features, self.SC.trackingError = self.SC.ST.recalculateFeatures(
self.prevFrameGray[int(initialPosition[1] -
initialHeight / 2):int(initialPosition[1] +
initialHeight / 2),
int(initialPosition[0] -
initialWidth / 2):int(initialPosition[0] +
initialWidth / 2)])
self.SC.features = self.SC.featureTranslate(
initialPosition[0] - initialWidth / 2,
initialPosition[1] - initialHeight / 2, self.SC.features)
self.SC.LK.prevFeatures = self.SC.features
#OPTIMIZACIÓN
self.bins_var = 1
self.kernel_blur_var = 1
self.mask_blur_var = 1
self.low_pth_var = 200
def __init__(self, dt):
self.filter = KalmanFilter()
self.filter.F = np.kron(np.array([[1., dt], [0., 1.]]), np.eye(2))
self.filter.H = np.kron(np.array([1., 0.]), np.eye(2))
self.filter.P = np.diag([1., 1., 1e-9, 1e-9])
self.filter.Q = np.kron(np.diag([1e-9] * 2), np.eye(2))
self.filter.R = np.diag([5.0] * 2)
self.filter.xhat = np.array([0., 0., 0., 0.])
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# TODO for student: Comment this when running on the robot
self.robot_sim = RobotSim(world_map, occupancy_map, pos_init, pos_goal,
max_speed, max_omega, x_spacing, y_spacing)
# TODO for student: Use this when transferring code to robot
# Handles all the ROS related items
#self.ros_interface = ROSInterface(t_cam_to_body)
# Uncomment as completed
self.kalman_filter = KalmanFilter(world_map)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
plan = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init,
pos_goal)
self.state_tol = 0.1
self.path = plan.tolist()
print("Path: ", self.path, type(self.path))
self.path.reverse()
self.path.pop()
self.state = pos_init
self.goal = self.path.pop()
self.x_offset = x_spacing
self.vw = (0, 0, False)
# self.goal[0] += self.x_offset/2
# self.goal[1] += y_spacing
print("INIT GOAL: ", self.goal)
def main(args):
assert args.path_to_video is not None
KFilter = KalmanFilter(args.Q, args.R)
MAnly = MotionAnalyzer(args.path_to_video)
Vcorr = VideoCorrector(args.path_to_video)
Tx, Ty, _ = MAnly.process()
res_x, res_y = KFilter.filter(Tx), KFilter.filter(Ty)
delta_x = np.array(Tx) - np.array(res_x)
delta_y = np.array(Ty) - np.array(res_x)
Vcorr.correct(zip(delta_x, delta_y))
def run(self):
# Create the Kalman filter tracker
A = np.eye(6)
deltaT = np.array(2e-3)
A2 = np.eye(6, k=3) * deltaT
A = A + A2
R = np.eye(3) * 100**2
H = np.eye(3, 6)
P = np.eye(6) * 100**2
B = np.eye(6)
kalman = KalmanFilter(A, R, H, P, B)
first = 0
oldDataNdx = np.zeros(1, dtype=np.uint64)
tmp = np.array(np.zeros(6))
while (1):
navData = navQueue.get()
dataNdx = navQueue.get()
numMeas = np.size(navData, 1)
if first == 0:
kalman.initialize(np.hstack((navData[0:3, 0], 0, 0, 0)))
# Loop over each position measurement
for measNdx in range(0, numMeas):
if measNdx == 0 and first != 0:
deltaT = np.array(np.float(dataNdx - oldDataNdx) * 1e-3)
A2 = np.eye(6, k=3) * deltaT
kalman.A = A + A2
else:
deltaT = np.array(1e-3)
A2 = np.eye(6, k=3) * deltaT
kalman.A = A + A2
kalman.run(navData[0:3, measNdx])
tmp = np.vstack((tmp, kalman.x))
print('%i' % (dataNdx - oldDataNdx))
oldDataNdx = deepcopy(dataNdx)
navQueue.task_done()
navQueue.task_done()
first = first + 1
# Convert from ECEF to lat/lon
(lat, lon, height) = ecef2lla(kalman.x[0], kalman.x[1],
kalman.x[2])
mapQueue.put((lat, lon))
def predict(self, obj_xyzvels):
#TODO: Write a function that given the data, predits the position of an object at given time period.
res = {}
for bd_id, xyzvel in obj_xyzvels.items():
cur_pos = xyzvel[['center_x', 'center_y']].iloc[0].values
cur_vel = xyzvel[['speed_x', 'speed_y']].iloc[0].values
cur_time = xyzvel[['frame_id']].iloc[0].values[0]
pred_duration = 5
kf = KalmanFilter()
pred_xys = kf.motion_model(cur_pos, cur_vel, pred_duration,
cur_time)
res[bd_id] = pred_xys
return res
def initialise_filter(self):
filter_ = KalmanFilter(
dim_x=4,
dim_z=2) # need to instantiate every time to reset all fields
filter_.F = self.matrix_a
filter_.H = self.matrix_c
filter_.B = self.matrix_g
if KalmanParams.use_noise_in_kalman:
u = np.random.normal(loc=0, scale=KalmanParams.var_kalman, size=2)
filter_.u = u
# u = Q_discrete_white_noise(dim=2, var=1)
filter_.Q = self.q
filter_.R = self.r
return filter_
def test_ball_path():
y = 15
x = 0
omega = 0.
noise = [1, 1]
v0 = 100.
ball = BallPath(x0=x, y0=y, omega_deg=omega, velocity=v0, noise=noise)
dt = 1
f1 = KalmanFilter(dim_x=6, dim_z=2)
dt = 1 / 30. # time step
ay = -.5 * dt**2
f1.F = np.mat([
[1, dt, 0, 0, 0, 0], # x=x0+dx*dt
[0, 1, dt, 0, 0, 0], # dx = dx
[0, 0, 0, 0, 0, 0], # ddx = 0
[0, 0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2
[0, 0, 0, 0, 1, dt], # dy = dy0 + ddy*dt
[0, 0, 0, 0, 0, 1]
]) # ddy = -g
f1.H = np.mat([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]])
f1.R = np.eye(2) * 5
f1.Q = np.eye(6) * 0.
omega = radians(omega)
vx = cos(omega) * v0
vy = sin(omega) * v0
f1.x = np.mat([x, vx, 0, y, vy, -9.8]).T
f1.P = np.eye(6) * 500.
z = np.mat([[0, 0]]).T
count = 0
markers.MarkerStyle(fillstyle='none')
np.set_printoptions(precision=4)
while f1.x[3, 0] > 0:
count += 1
#f1.update (z)
f1.predict()
plt.scatter(f1.x[0, 0], f1.x[3, 0], color='green')
def DynPC(X, p, sig2, k_, option=None):
# This function calibrates the parameters of the Kalman filter model by
# using a two-step approach, statistical and regression LFM's
#INPUTS
# X :[matrix](n_ x t_end) dataset of observations
# p :[vector](1 x t_end) flexible probabilities
# sig2 :[matrix](n_ x n_) metric matrix
# k_ :[scalar] number of factors recovered via PC LFM
# option :[struct] structure specifying the initial values (needed for extraction) of hidden factors Z_KF and their uncertainty.
# In particular:
# option.Z :[vector](k_ x 1) initial value of hidden factor
# option.p2 :[matrix](k_ x k_) uncertainty on the initial value Z of hidden factors
#OPS
# Z_KF :[matrix](k_ x t_end) dataset of extracted factors
# alpha :[vector](n_ x 1) shifting term in the observation equation
# beta :[matrix](n_ x k_) parameter of the observation equation
# s2 :[matrix](n_ x n_) covariance of invariants in the observation equation
# alpha_z :[vector](k_ x 1) shifting term in the transition equation
# beta_z :[matrix](k_ x k_) parameter of the transition equation
# s2_z :[matrix](k_ x k_) covariance of the invariants in the transition equation
# For details on the exercise, see here .
# Step 1
# Estimation of statistical LFM
alpha_PC, beta_PC, gamma_PC, s2_U_PC = PrincCompFP(X, p, sig2, k_)
# set outputs
alpha = alpha_PC
beta = beta_PC
s2 = s2_U_PC
# compute hidden factors
Z = gamma_PC @ X
# Step 2
# Estimation of regression LFM
alpha_OLSFP, beta_OLSFP, s2_OLSFP, _ = OrdLeastSquareFPNReg(
Z[:, 1:], Z[:, :-1], p[[0], :-1])
# set outputs
alpha_z = alpha_OLSFP
beta_z = beta_OLSFP
s2_z = s2_OLSFP
# Step 3
# Extraction of hidden factors
Z_KF = KalmanFilter(X, alpha, beta, s2, alpha_z, beta_z, s2_z, option)
return Z_KF, alpha, beta, s2, alpha_z, beta_z, s2_z
def __init__(self, **filter_kwargs):
"""Initialize the class with the Kalman filter kwargs. Inherits the
RaceTrack() class variables so that Kalman filter estimates and
uncertainty can be plotted on the race track.
"""
super().__init__() # Inherit RaceTrack class variables
self.kalman_rotation = self.path_angles.copy()
self.init_rotation() # Calculate pixel-by-pixel car orientation
self.kf = KalmanFilter(**filter_kwargs) # Instantiate Kalman filter
# Get initial estimate and estimate uncertainty values
self.estimate = (self.kf.x[0, 0], self.kf.x[1, 0])
self.estimate_uncertainty = (self.kf.P[0, 0], self.kf.P[1, 1],
self.kf.P[2, 2], self.kf.P[3, 3])
self.estimates = [self.estimate]
self.uncertainties = [self.estimate_uncertainty]
# Transparent surface used to blit estimate uncertainty circle
self.surface = pygame.Surface((self.game_width, self.game_height),
pygame.SRCALPHA)
def __init__(self):
self.validSpeedUpdated = False
self.lastPoint = 0.
self.distanceThreshold = 0.05
self.speedUpdateTime = []
self.speedWindowSize = 10
self.keyMeasPairs = deque(maxlen=80)
self.speedWindow = []
self.filtedRange = []
self.curSpeed = 0.0
self.speedRecord = []
self.rangeKF = KalmanFilter(dim_x=1, dim_z=1)
self.rangeKF.x = np.array([0.])
self.rangeKF.F = np.array([[1.]])
self.rangeKF.H = np.array([[1.]])
self.rangeKF.P *= 100.
self.rangeKF.R = 0.1
self.rangeKF.Q = 0.001
self.rangeKF.initialized = False
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
self.kalman_filter = KalmanFilter(world_map)
print("INITSTATE", self.kalman_filter.state)
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
self.vel = 0
self.omega = 0
self.curInd = 0
self.path = dijkstras(occupancy_map, x_spacing, y_spacing, pos_init,
pos_goal)
print(self.path)
self.curGoal = self.path[0]
self.done = False
def __init__(self):
self.IMAGE_WIDTH = 320
self.IMAGE_HEIGHT = 240
# PID parameters
self.KP = 0.035
self.KI = 0.045
self.KD = 0.005
self.prev_errx = 0
self.prev_erry = 0
self.integral_x = 0
self.integral_y = 0
self.curr_yaw = 90
self.curr_pitch = 90
self.last_obs = time.time()
# Open the camera
self.capture = cv.CreateCameraCapture(0)
cv.SetCaptureProperty(self.capture, cv.CV_CAP_PROP_FRAME_WIDTH,
self.IMAGE_WIDTH)
cv.SetCaptureProperty(self.capture, cv.CV_CAP_PROP_FRAME_HEIGHT,
self.IMAGE_HEIGHT)
# Union-Find Connected Comonent Labeling
self.detector = BlobDetector()
# Kalman filter
self.filter = KalmanFilter()
# Open the serial port to the arduino
print 'Opening serial port ...'
self.serial = serial.Serial('/dev/ttyACM0', 19200)
time.sleep(2)
print 'Moving servos to initial position ...'
self.serial.write('90s90t')
time.sleep(1)
def __init__(self, markers, occupancy_map, pos_init, pos_goal, max_speed,
min_speed, max_omega, x_spacing, y_spacing, t_cam_to_body,
mode):
"""
Initialize the class
"""
# plan a path around obstacles using dijkstra's algorithm
print('Planning path...')
path = findShortestPath(occupancy_map,
x_spacing,
y_spacing,
pos_init[0:2],
pos_goal[0:2],
dilate=2)
print('Done!')
self.path_manager = PathManager(path)
self.kalman_filter = KalmanFilter(markers, pos_init)
self.diff_drive_controller = DiffDriveController(
max_speed, min_speed, max_omega)
if 'HARDWARE' in mode:
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
elif 'SIMULATE' in mode:
self.robot_sim = RobotSim(markers, occupancy_map, pos_init,
pos_goal, max_speed, max_omega,
x_spacing, y_spacing,
self.path_manager.path, mode)
self.user_control = UserControl()
self.vel = 0 # save velocity to use for kalman filter
self.goal = self.path_manager.getNextWaypoint() # get first waypoint
# for logging postion data to csv file
self.stateSaved = []
self.tagsSaved = []
self.waypoints = []
def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed,
max_omega, x_spacing, y_spacing, t_cam_to_body):
"""
Initialize the class
Inputs: (all loaded from the parameter YAML file)
world_map - a P by 4 numpy array specifying the location, orientation,
and identification of all the markers/AprilTags in the world. The
format of each row is (x,y,theta,id) with x,y giving 2D position,
theta giving orientation, and id being an integer specifying the
unique identifier of the tag.
occupancy_map - an N by M numpy array of boolean values (represented as
integers of either 0 or 1). This represents the parts of the map
that have obstacles. It is mapped to metric coordinates via
x_spacing and y_spacing
pos_init - a 3 by 1 array specifying the initial position of the robot,
formatted as usual as (x,y,theta)
pos_goal - a 3 by 1 array specifying the final position of the robot,
also formatted as (x,y,theta)
max_speed - a parameter specifying the maximum forward speed the robot
can go (i.e. maximum control signal for v)
max_omega - a parameter specifying the maximum angular speed the robot
can go (i.e. maximum control signal for omega)
x_spacing - a parameter specifying the spacing between adjacent columns
of occupancy_map
y_spacing - a parameter specifying the spacing between adjacent rows
of occupancy_map
t_cam_to_body - numpy transformation between the camera and the robot
(not used in simulation)
"""
# Handles all the ROS related items
self.ros_interface = ROSInterface(t_cam_to_body)
self.kalman_filter = KalmanFilter(world_map)
self.prev_v = 0
self.est_pose = np.array([[0], [0], [0]])
self.diff_drive_controller = DiffDriveController(max_speed, max_omega)
self.prev_imu_meas = np.array([[0], [0], [0], [0], [0]])
def __init__(self, x, P, Q, R, F, H, hx):
"""
:param x: list
:param P: list, sqrt of diagnose of P, std of each variable
:param Q: 2d ndarray
:param R: list, sqrt of diagnose of R, std of each measurement
:param F: (x, dt) => 2d ndarray
:param H: (x) => 2d ndarray
:param hx: (x) => 1d ndarray
"""
self.R = np.diag(R)**2
self.hx = hx
self.b = np.zeros((len(R), ))
self.ekf = KalmanFilter(x,
P,
Q,
R,
F=F,
H=H,
hx=lambda x, b: hx(x) + b)
self.residual_set = []
self.b_conv = np.array([0., 0.])
def __init__(self): self.filterL = KalmanFilter(1, 30) self.filterR = KalmanFilter(1, 30) self.filter_vw_r = 0.0 self.filter_vw_l = 0.0
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]
], np.float32) * 0.001
measurementStateMatrix = np.zeros((6, 1), np.float32)
observationMatrix = np.array([
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]
], np.float32)
measurementNoiseCov = np.array(
[[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]],
np.float32) * 1
kalman = KalmanFilter(X=stateMatrix,
P=estimateCovariance,
F=transitionMatrix,
Q=processNoiseCov,
Z=measurementStateMatrix,
H=observationMatrix,
R=measurementNoiseCov)
# function that is called whenever either an aruco marker or charuco board needs to be tracked by Realsense and the transformed pose is streamed to MagicLeap
def startTracking(kalman_flag, marker_type, MLRSTr, n_sq_y, n_sq_x, squareLen,
markerLen, aruco_dict, socket_connect, c):
RSTr = None
img_idx = 0
img_sent = 0
reinit_thresh = 10
skip_frame_reinit = 120 #after every 150 frames, reinitialize detection
skip_frame_send = 4
#if Charuco board is the marker type, execute following if statement
