def __init__(self, bzrc):
self._kalman = Kalman()
self.aliveTime = time.time()
self.bzrc = bzrc
self.constants = self.bzrc.get_constants()
# print self.constants
self.commands = []
self.potentialFields = []
self.updates = []
self.flag_sphere = 400
self.obstacle_sphere = 1000
self.enemy_sphere = 100
self.explore_sphere = 50
self.prev_x = {}
self.prev_y = {}
self.stuck_ticks = {}
for x in xrange(20):
self.prev_x[x] = 0
self.prev_y[x] = 0
self.stuck_ticks[x] = 0
self.path_fields = []
self.grid = Grid(int(self.constants['worldsize']),
int(self.constants['worldsize']),
self.initialGridConstant, self.initialGridConstant)
def __init__(self, interval=1):
""" Constructor
:type interval: int
:param interval: Check interval, in seconds
"""
self.sensorfusion = Kalman()
print("Connecting to sensortag...")
self.tag = SensorTag(macAddress)
print("connected.")
self.tag.accelerometer.enable()
self.tag.magnetometer.enable()
self.tag.gyroscope.enable()
self.tag.battery.enable()
self.pitch = 0
self.angular_velocity = 0
self.linear_velocity = 0
self.acceleration = 0
time.sleep(1.0) # Loading sensors
self.prev_time = time.time()
thread = threading.Thread(target=self.run, args=())
thread.daemon = True # Daemonize thread
thread.start() # Start the execution
def main():
sensorfusion = Kalman()
print("Connecting to sensortag...")
tag = SensorTag(macAddress)
print("connected.")
tag.accelerometer.enable()
tag.magnetometer.enable()
tag.gyroscope.enable()
tag.battery.enable()
time.sleep(1.0) # Loading sensors
prev_time = time.time()
while True:
accelerometer_readings = tag.accelerometer.read()
gyroscope_readings = tag.gyroscope.read()
magnetometer_readings = tag.magnetometer.read()
ax, ay, az = accelerometer_readings
gx, gy, gz = gyroscope_readings
mx, my, mz = magnetometer_readings
curr_time = time.time()
dt = curr_time - prev_time
sensorfusion.computeAndUpdateRollPitchYaw(ax, ay, az, gx, gy, gz, mx,
my, mz, dt)
print(f"dt: {dt} pitch: {sensorfusion.pitch}")
prev_time = curr_time
print("Battery: ", tag.battery.read())
def __init__(self, bzrc, tank_index):
#print tank_index
self.agent_index = tank_index
self._kalman = Kalman()
self.aliveTime = time.time()
self.bzrc = bzrc
self.constants = self.bzrc.get_constants()
self.mytanks = self.bzrc.get_mytanks()
self.targetTank,self.targ = self.closest_enemy(self.mytanks[self.agent_index], self.bzrc.get_othertanks())
self._kalman.resetArrays(self.targetTank.x, self.targetTank.y)
# print self.constants
self.commands = []
self.potentialFields = []
self.updates = []
self.flag_sphere = 400
self.obstacle_sphere = 1000
self.enemy_sphere = 100
self.explore_sphere = 50
self.prev_x = {}
self.prev_y = {}
self.stuck_ticks = {}
for x in xrange(20):
self.prev_x[x] = 0
self.prev_y[x] = 0
self.stuck_ticks[x] = 0
self.path_fields = []
self.grid = Grid(int(self.constants['worldsize']),
int(self.constants['worldsize']),
self.initialGridConstant,
self.initialGridConstant)
class BlueToothThreading:
"""
Bluetooth Threading
The run() method will be started and it will run in the background
until the application exits.
"""
def __init__(self, interval=1):
""" Constructor
:type interval: int
:param interval: Check interval, in seconds
"""
self.sensorfusion = Kalman()
print("Connecting to sensortag...")
self.tag = SensorTag(macAddress)
print("connected.")
self.tag.accelerometer.enable()
self.tag.magnetometer.enable()
self.tag.gyroscope.enable()
self.tag.battery.enable()
self.pitch = 0
time.sleep(1.0) # Loading sensors
self.prev_time = time.time()
thread = threading.Thread(target=self.run, args=())
thread.daemon = True # Daemonize thread
thread.start() # Start the execution
def run(self):
""" Method that runs forever """
while True:
accelerometer_readings = self.tag.accelerometer.read()
gyroscope_readings = self.tag.gyroscope.read()
magnetometer_readings = self.tag.magnetometer.read()
ax, ay, az = accelerometer_readings
gx, gy, gz = gyroscope_readings
mx, my, mz = magnetometer_readings
curr_time = time.time()
dt = curr_time - self.prev_time
self.sensorfusion.computeAndUpdateRollPitchYaw(
ax, ay, az, gx, gy, gz, mx, my, mz, dt)
self.pitch = self.sensorfusion.pitch
self.prev_time = curr_time
print("Battery: ", self.tag.battery.read())
def __init__(
self,
set_point,
water_level,
maximum_flowrate,
initial_flowrate=0,
learning_rate=0.1,
last_layer_activation_function='tanh',
initial_error_covariance_matrix=initial_error_covariance_matrix,
transition_matrix=transition_matrix,
input_matrix=input_matrix,
input_vector=input_vector,
measurement_matrix=measurement_matrix,
disturbance=disturbance,
uncertainty=uncertainty):
initial_state = np.asarray([initial_flowrate]).T
self.kalman = Kalman(
initial_state=initial_state,
initial_error_covariance_matrix=initial_error_covariance_matrix,
transition_matrix=transition_matrix,
measurement_matrix=measurement_matrix,
input_matrix=input_matrix,
input_vector=input_vector,
measurement_noise=uncertainty,
process_noise=disturbance)
self.pidnn = PIDNN(
set_point=set_point,
actual_value=water_level,
prev_output=initial_flowrate,
learning_rate=learning_rate,
last_layer_activation_function=last_layer_activation_function)
self.maximum_flowrate = maximum_flowrate
def __init__(self, bzrc):
self._kalman = Kalman()
self.aliveTime = time.time()
self.bzrc = bzrc
self.constants = self.bzrc.get_constants()
# print self.constants
self.commands = []
self.potentialFields = []
self.updates = []
self.flag_sphere = 400
self.obstacle_sphere = 1000
self.enemy_sphere = 100
self.explore_sphere = 50
self.prev_x = {}
self.prev_y = {}
self.stuck_ticks = {}
for x in xrange(20):
self.prev_x[x] = 0
self.prev_y[x] = 0
self.stuck_ticks[x] = 0
self.path_fields = []
self.grid = Grid(int(self.constants['worldsize']),
int(self.constants['worldsize']),
self.initialGridConstant,
self.initialGridConstant)
def _filter_xs(self, s_0, s_ts, pi_0, sigma_0, As, Cs, Qs, Rs):
"""
Computes x_1|1, P_1|1, ..., x_T|T, P_T|T given observations, parameters and values of switching
variable at each time.
:returns:
-n_LF x T numpy array
Smoothed means
-T x n_LF x n_LF numpy array
Smoothed covariances
-T-1 x n_LF x n_LF numpy array
Smoothed lagged covariances (ie, cov[x_t, x_t-1])
"""
# Initialize Kalman filter using values of parameters at t = 0, even though they're never used
kf = Kalman(mu_0=pi_0.copy(), sigma_0=sigma_0.copy(), A=As[s_0], B=None, C=Cs[s_0], D=None,
Q=Qs[s_0], R=Rs[s_0])
# x_t|t, P_t|t, t = 1, ..., T
x_filts = np.zeros([self.T, self.n_LF])
P_filts = np.zeros([self.T, self.n_LF, self.n_LF])
# x_t|t-1, P_t|t-1. t = 1, ..., T. Indexed by t.
x_pred = np.zeros([self.T, self.n_LF])
P_pred = np.zeros([self.T, self.n_LF, self.n_LF])
# Filtering step
for t in range(0, self.T): # corresponds to t = 1, ..., T
# Change parameters. Never need to use A_{s_0}, etc.
kf.A = As[s_ts[t]]
kf.C = Cs[s_ts[t]]
kf.Q = Qs[s_ts[t]]
kf.R = Rs[s_ts[t]]
# Compute x_{t|t-1}, P_{t|t-1}
kf.predict()
x_pred[t] = kf.mu
P_pred[t] = kf.sigma
# Compute x_{t|t}, P_{t|t}
kf.update(self.y_ts[t])
x_filts[t] = kf.mu
P_filts[t] = kf.sigma
# TODO: run smoother to fill in missing data!!!
return x_filts, P_filts, x_pred, P_pred
def __init__(self):
rospy.init_node("IK_node", anonymous=False)
self.input_sub = rospy.Subscriber("/vx250/input_arm_pose", ArmPose,
self.callback)
self.marker_pub = rospy.Publisher("/vx250/joint_state_est",
Marker,
queue_size=1)
self.angles_pub = rospy.Publisher("/vx250/arm_controller/command",
JointTrajectory,
queue_size=1)
self.measurement = ArmState()
self.states = {
'Green':
Kalman(
np.ones((6, 1)) / 1000,
np.diag([
2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn(),
2.5 * randn(), 2.5 * randn()
]), 1 / LOOP_RATE),
'Red':
Kalman(
np.ones((6, 1)) / 1000,
np.diag([
2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn(),
2.5 * randn(), 2.5 * randn()
]), 1 / LOOP_RATE),
'Blue':
Kalman(
np.ones((6, 1)) / 1000,
np.diag([
2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn(),
2.5 * randn(), 2.5 * randn()
]), 1 / LOOP_RATE),
'Yellow':
Kalman(
np.ones((6, 1)) / 1000,
np.diag([
2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn(),
2.5 * randn(), 2.5 * randn()
]), 1 / LOOP_RATE)
}
self.prev_states = self.states
self.rate = rospy.Rate(LOOP_RATE)
def __init__(self):
self.A_k = None
self.B_k = None
self.dt = 0.02 # simulation time step
self.t_rc = 0.005 # membrane RC time constant
self.t_ref = 0.001 # refractory period
self.tau = 0.014 # synapse time constant for standard first-order lowpass filter synapse
self.N_A = 1000 # number of neurons in first population
self.rate_A = 200, 400 # range of maximum firing rates for population A
self.model = nengo.Network(label="Kalman SNN")
self.sim = None
self.kalman = Kalman()
self.output = None
self.testX = None
self.count = 0
self.chN = None
def __init__(self, robot_id):
# Does this need to be a node? Maybe it could be a tf?
rospy.init_node('Filter' + str(robot_id))
self.pub = rospy.Publisher("Filter" + str(robot_id) + "/state",
Odometry,
queue_size=1)
self.sub = rospy.Subscriber("Sensor/measurement" + str(robot_id),
Odometry, self.new_measurement)
self.kf = Kalman()
self.state_out = Odometry()
self.state = np.array([[0], [0], [0], [0], [0], [0]])
self.startTime = rospy.get_time()
self.oldTime = 0.0
def init_demo_kalman():
n = m = 2
A = np.array([[0.9, 0.2], [-0.3, 0.5]])
H = np.array([[1.0, 0.1], [-0.2, 1.1]])
R = 0.5 * np.eye(m)
Q = 1.4 * np.eye(n)
x0 = [-0.7, 0.9]
sys = LinearSystem(n, m, x0, A, H, R, Q)
kalman = Kalman(n, m, sys)
return kalman
def __init__(self, bzrc):
self.bzrc = bzrc
self.constants = self.bzrc.get_constants()
self.pos_noise = 5
self.commands = []
self.kalman = Kalman(self.pos_noise)
self.max_tank_speed = float(self.constants['tankspeed'])
self.linear_acceleration = float(self.constants['linearaccel'])
self.shot_speed = float(self.constants['shotspeed'])
mytanks, othertanks, flags, shots = self.bzrc.get_lots_o_stuff()
enemies = [tank for tank in othertanks if tank.color !=
self.constants['team']]
covariance_list = [[100, 0, 0, 0, 0, 0],
[ 0, 0.1, 0, 0, 0, 0],
[ 0, 0, 0.1, 0, 0, 0],
[ 0, 0, 0, 100, 0, 0],
[ 0, 0, 0, 0, 0.1, 0],
[ 0, 0, 0, 0, 0, 0.1]]
self.observed_states = []
self.states = []
self.state_covariances = []
for enemy in enemies:
new_observed = numpy.matrix(numpy.zeros((6, 1)))
new_observed.getA()[0][0] = enemy.x
new_observed.getA()[3][0] = enemy.y
self.observed_states.append(new_observed)
new_state = numpy.matrix(numpy.zeros((6, 1)))
self.states.append(new_state)
new_covariance = numpy.matrix(covariance_list)
self.state_covariances.append(new_covariance)
# PD control
self.old_angle = [0] * len(mytanks)
# initialize plot
plt.ion()
self.fig = plt.figure()
self.fignum = 0
self.graph = self.fig.add_subplot(111)
worldsize = float(self.constants['worldsize'])
worldlimit = worldsize / 2.0
self.graph.axis([-worldlimit, worldlimit, -worldlimit, worldlimit])
plt.show()
def __init__(self, initial_point, trackIdCount):
"""Initialize variables used by Track class
Args:
prediction: predicted centroids of clusters to be tracked
trackIdCount: identification of each track object
Return:
None
"""
self.track_id = trackIdCount # identification of each track object
self.KF = Kalman(initial_point) # KF instance to track this object
self.prediction = np.asarray(
initial_point) # predicted centroids (x,y)
self.skipped_frames = 0 # number of frames skipped undetected
self.last_detection_assigment = 0 # number of frames skipped undetected
self.age = 0 # the number of frames this tracker has lived for
self.trace = [] # trace path
def new_measurement(self, data):
self.startTime = rospy.get_time()
timestep = self.startTime - self.oldTime
self.oldTime = self.startTime
self.state = Kalman.update(
self.kf, self.state, data.twist.twist.linear.x,
data.twist.twist.angular.z, timestep,
[data.pose.pose.position.x, data.pose.pose.position.y])
self.state_out.pose.pose.position.x = self.state[0]
self.state_out.pose.pose.position.y = self.state[2]
self.state_out.twist.twist.linear.x = math.sqrt(self.state[1]**2 +
self.state[3]**2)
angle = 1 + self.state[4] - 1
quat = tf.transformations.quaternion_from_euler(0.0, 0.0, angle)
self.state_out.pose.pose.orientation.x = quat[0]
self.state_out.pose.pose.orientation.y = quat[1]
self.state_out.pose.pose.orientation.z = quat[2]
self.state_out.pose.pose.orientation.w = quat[3]
self.state_out.twist.twist.angular.z = self.state[5]
#print str(self.state)
self.pub.publish(self.state_out)
def __init__(self, trackId, rect, img, mask):
self.trackId = trackId
self.lastClass = 'unknown'
self.lastKeypoints = None
self.rect = []
self.searchWindow = rect
self.template = []
self.tracklet = []
self.trackletRects = []
self.direction = Enum(['LEFT', 'RIGHT', 'UNKNOWN'])
self.actions = []
self.actions.append('unknown')
self.classHistory = []
self.active = True
self.occluded = False
self.foundCount = 0
self.numTemplates = 6
self.outDimens = None
#self.trackOutputStream = None
self.doTrackWrite = False
self.savedFrames = []
self.outFps = 30
height, width = img.shape[:2]
self.imgBounds = (width, height)
x, y, w, h = rect
x, y, w, h = rect = self.resizeRect(rect, (1.0, 1.0), (0.0, 0.0))
#x,y,w,h = rect = self.resizeRect(rect,(1.0,0.75),(-0.15,-0.2))
#crop syntax is [starty:endy, startx:endx]
template = img[y:y + h, x:x + w]
#maskFiltered = mask[y:y+h, x:x+w]
# 3 Kalman filters per research document
self.kalmanFast = Kalman((x + (w / 2), y + (h / 2)))
self.kalmanArea = Kalman((w, h)) # ,0.003,0.3)
self.kalmanSlow = Kalman((x + (w / 2), y + (h / 2)), 0.003, 0.03)
self.updateTemplate(rect, template)
#x,y,w,h = rect = self.resizeRect(rect,(1.5,1.3),(0,0.5))
self.trackletSize = (0, 0)
self.updateTracklet(rect, img)
import numpy as np
import matplotlib.pyplot as plt
from kalman import Kalman
from scipy.stats import norm
from scipy.integrate import quad
## Parameters
theta = 10
A, G, Q, R = 1, 1, 0, 1
x_hat_0, Sigma_0 = 8, 1
epsilon = 0.1
## Initialize Kalman filter
kalman = Kalman(A, G, Q, R)
kalman.set_state(x_hat_0, Sigma_0)
T = 600
z = np.empty(T)
for t in range(T):
# Record the current predicted mean and variance, and plot their densities
m, v = kalman.current_x_hat, kalman.current_Sigma
m, v = float(m), float(v)
f = lambda x: norm.pdf(x, loc=m, scale=np.sqrt(v))
integral, error = quad(f, theta - epsilon, theta + epsilon)
z[t] = 1 - integral
# Generate the noisy signal and update the Kalman filter
kalman.update(theta + norm.rvs(size=1))
fig, ax = plt.subplots()
ax.set_ylim(0, 1)
ax.set_xlim(0, T)
ax.plot(range(T), z)
#!/usr/bin/env python
import flask
from random import randrange
from kalman import Kalman
import time
app = flask.Flask(__name__)
input_data_range = 5000*2
k_f = Kalman()
k_f.setAngle( 0.0 )
@app.route('/')
def index():
input_ = []
setpoint = []
output_ra = []
output_ma = []
output_kf = []
output_cf = []
# Raw data
for i in range(input_data_range):
input_.append( round(randrange(-4,4)/float(randrange(2,5)),1) )
setpoint.append( 0.0 )
# Moving Average Algorithm
m = input_[0]
output_ma.append(m)
class TrackBase:
def __init__(self, trackId, rect, img, mask):
self.trackId = trackId
self.lastClass = 'unknown'
self.lastKeypoints = None
self.rect = []
self.searchWindow = rect
self.template = []
self.tracklet = []
self.trackletRects = []
self.direction = Enum(['LEFT', 'RIGHT', 'UNKNOWN'])
self.actions = []
self.actions.append('unknown')
self.classHistory = []
self.active = True
self.occluded = False
self.foundCount = 0
self.numTemplates = 6
self.outDimens = None
#self.trackOutputStream = None
self.doTrackWrite = False
self.savedFrames = []
self.outFps = 30
height, width = img.shape[:2]
self.imgBounds = (width, height)
x, y, w, h = rect
x, y, w, h = rect = self.resizeRect(rect, (1.0, 1.0), (0.0, 0.0))
#x,y,w,h = rect = self.resizeRect(rect,(1.0,0.75),(-0.15,-0.2))
#crop syntax is [starty:endy, startx:endx]
template = img[y:y + h, x:x + w]
#maskFiltered = mask[y:y+h, x:x+w]
# 3 Kalman filters per research document
self.kalmanFast = Kalman((x + (w / 2), y + (h / 2)))
self.kalmanArea = Kalman((w, h)) # ,0.003,0.3)
self.kalmanSlow = Kalman((x + (w / 2), y + (h / 2)), 0.003, 0.03)
self.updateTemplate(rect, template)
#x,y,w,h = rect = self.resizeRect(rect,(1.5,1.3),(0,0.5))
self.trackletSize = (0, 0)
self.updateTracklet(rect, img)
# set up the ROI for tracking
#roi = template
#hsv_roi = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
#maskFiltered = cv2.inRange(hsv_roi, np.array((0., 150.,60.)), np.array((180.,255.,255.)))
#cv2.imshow('maskFiltered',maskFiltered)
#self.roi_hist = cv2.calcHist([hsv_roi],[0,1],maskFiltered,[36,50],[0,180,0,255])
#cv2.normalize(self.roi_hist,self.roi_hist,0,255,cv2.NORM_MINMAX)
# returns true if the provided point plus current track bounds go over the edge of the frame, false otherwise
def isInBounds(self, point, bounds):
bh, bw = bounds
px, py = point
x, y, w, h = self.getNewestRect()
if px + w / 2 > bw or px - w / 2 < 0:
return False
if py + h / 2 > bh or py - h / 2 < 0:
return False
return True
# search for this track in the provided frame by whatever concrete algorithm the track implements
def update(self, img):
self.savedFrames.append(img)
# always do predictions before measurements
pLoc = self.kalmanFast.predict()
self.kalmanSlow.predict()
self.kalmanArea.predict()
# make sure we haven't already gone off the frame
if not self.isInBounds(pLoc, img.shape[:2]):
self.active = False
print('Track #' + repr(self.trackId) + ' ' + repr(self.occluded) +
' ENDING TRACK: gone off frame ' + repr(pLoc))
return
newroi = (0, 0, 0, 0)
# search phase
if not self.occluded:
self.occluded, newroi = self.doNonOccludedSearch(img)
else:
self.occluded, newroi = self.doOccludedSearch(img)
# update phase
if not self.occluded:
self.doNonOccludedUpdate(newroi, img)
else:
self.doOccludedUpdate(newroi, img)
#if not self.trackOutputStream is None and self.doTrackWrite:
#w,h = self.outDimens
#resized = cv2.resize(self.getNewestTemplate(), (w,h))
# self.trackOutputStream.write(self.getNewestTracklet())
#print('Track #' + repr(self.trackId) + ' ' + repr(self.occluded) + ' ROI: ' + repr(self.rect[0]))
# perform search for tracked object with the assumption it was occluded in the previous frame
# default implementation provides a global template match for previously known good template
def doOccludedSearch(self, img):
# set search window so we're not performing a global search
roi = self.resizeRect(self.getNewestRect(), (4, 4))
x, y, w, h = roi
newroi = (0, 0, 0, 0)
self.searchWindow = roi
window = img[y:y + h, x:x + w]
temp = self.getOldestTemplate()
# make sure we haven't already gone off the frame
if window.shape[:2][0] <= 0 or window.shape[:2][0] < temp.shape[:2][
0] or window.shape[:2][1] <= 0 or window.shape[:2][
1] < temp.shape[:2][1]:
self.active = False
print('Track #' + repr(self.trackId) + ' ' + repr(self.occluded) +
' ENDING TRACK: match window off frame ' +
repr(window.shape[:2]))
return True, newroi
#print('Track #' + repr(self.trackId) + ' window:' + repr(window.shape[:2]) + ' temp:' + repr(temp.shape[:2]))
#cv2.imshow('template',temp)
# perform the template matching search
res = cv2.matchTemplate(window, temp, cv2.TM_CCOEFF_NORMED)
min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(res)
if max_val >= 0.7: #found match
px, py = max_loc
x, y, w, h = roi
x2, y2, w2, h2 = self.getOldestRect()
newroi = (px + x, py + y, w2, h2)
return False, newroi
else:
return True, newroi
#print('Track #' + repr(self.trackId) + ' ' + repr(self.occluded) + ' CORR: ' + repr(max_val))
# perform search for tracked object with the assumption it was not occluded in the previous frame
# all subclasses should implement this method with their specific tracking algorithm
def doNonOccludedSearch(self, img):
return
# updates the track when it was not found in the search frame using Kalman filter predictions
def doOccludedUpdate(self, roi, img):
if self.foundCount < 10:
pLocS = self.kalmanFast.getLastPrediction()
else:
pLocS = self.kalmanSlow.getLastPrediction()
x, y, w, h = self.getOldestRect()
newRoi = self.resizeRect(
(int(pLocS[0] - (w / 2)), int(pLocS[1] - (h / 2)), w, h),
(1.0, 1.0))
self.updateTemplate(newRoi, None)
self.updateTracklet(newRoi,
img) # don't want missing frames in tracklet
# updates the track with a real measurement when it was found in the search frame
def doNonOccludedUpdate(self, roi, img):
self.foundCount = self.foundCount + 1
x, y, w, h = roi
# correct Kalman filters
self.kalmanFast.correct((x + (w / 2), y + (h / 2)))
self.kalmanSlow.correct((x + (w / 2), y + (h / 2)))
self.kalmanArea.correct((w, h))
self.updateTemplate(roi, img[y:y + h, x:x + w])
self.updateTracklet(roi, img)
# TODO: remove when deemed no longer necessary
# callback when a potential seed roi is detected near this
def onFindIntersect(self, roi, img):
return
# returns true if provided roi is near this track position
def matches(self, roi, img):
if self.intersects(roi, 100):
self.onFindIntersect(roi, img)
return True
else:
return False
def updateTemplate(self, rect, template):
x, y, w, h = rect
#self.rect.insert( 0,rect )
if not rect is None:
self.rect.append(rect)
if len(self.rect) > self.numTemplates:
del self.rect[0]
#del self.rect[self.numTemplates-1]
#self.template.insert( 0,template )
if not template is None:
self.template.append(template)
if len(self.template) > self.numTemplates:
del self.template[0]
#del self.template[self.numTemplates-1]
def updateTracklet(self, rect, img):
#x,y,w,h = rect = self.resizeRect(rect,(1.3,1.5),(-0.15,0.25)) #wave
x, y, w, h = rect = self.resizeRect(rect, (1.2, 1.5), (0.0, 0.25))
if self.trackletSize[0] == 0:
self.trackletSize = (w, h)
else:
w, h = self.trackletSize
#x = rect[0]
#y = rect[1]
roiGray = cv2.cvtColor(img[y:y + h, x:x + w], cv2.COLOR_BGR2GRAY)
self.tracklet.append(roiGray)
self.trackletRects.append((x, y, w, h))
#if not self.trackOutputStream is None and self.doTrackWrite:
# self.trackOutputStream.write(img[y:y+h, x:x+w])
# returns true is the provided rectangle touches this track's known rectangle
# based on a circle drawn from the center
def intersects(self, rect, dmin=100):
x1, y1, w1, h1 = rect
x2, y2, w2, h2 = self.getNewestRect()
center1 = (int(x1 + w1 / 2), int(y1 + h1 / 2))
center2 = (int(x2 + w2 / 2), int(y2 + h2 / 2))
# radius of search area
radius1 = int(math.hypot(w1, h1) / 2)
radius2 = int(math.hypot(w2, h2) / 2)
# distance between centers
dist = int(math.hypot(center1[0] - center2[0],
center1[1] - center2[1]))
#cv2.line(img, center1, center2, (0,255,255),1)
#cv2.circle(img, center1, radius1, (0,0,255), 1)
#cv2.circle(img, center2, radius2, (0,255,255), 1)
return dist < (radius1 + radius2) or dist < dmin
# return the input rectangle resized by given area coefficient and bounds checked against provided bounds
def resizeRect(self, rect, scale=(1.0, 1.0), offsets=(0, 0)):
x, y, w, h = rect
width, height = self.imgBounds
if x < 0:
x = 0
sx = x - (w * (scale[0] - 1.0)) / 2
sx = sx + offsets[0] * w
if sx < 0:
sx = 0
if sx >= width:
sx = width - 1
ex = sx + (w * scale[0])
if ex >= width:
ex = width - 1
if y < 0:
y = 0
sy = y - (h * (scale[1] - 1.0)) / 2
sy = sy + offsets[1] * h
if sy < 0:
sy = 0
if sy >= height:
sy = height - 1
ey = sy + (h * scale[1])
if ey >= height:
ey = height - 1
return (int(sx), int(sy), int(ex - sx), int(ey - sy))
# Returns a measure of how close the provided image's histogram matches this one. Lower value = more similar
def histogramsMatch(self, img1, img2, thresh=0.6):
hsv_roi1 = cv2.cvtColor(img1, cv2.COLOR_BGR2HSV)
hist1 = cv2.calcHist([hsv_roi1], [0, 1], None, [36, 50],
[0, 180, 0, 255])
cv2.normalize(hist1, hist1, 0, 255, cv2.NORM_MINMAX)
hsv_roi2 = cv2.cvtColor(img2, cv2.COLOR_BGR2HSV)
hist2 = cv2.calcHist([hsv_roi2], [0, 1], None, [36, 50],
[0, 180, 0, 255])
cv2.normalize(hist2, hist2, 0, 255, cv2.NORM_MINMAX)
score = cv2.compareHist(hist1, hist2, cv2.HISTCMP_BHATTACHARYYA)
return score <= thresh, score
# TODO: resize templates more intelligently
# returns true if the provided images match, false otherwise
def templatesMatch(self, img1, img2, thresh=0.3):
#cv2.imshow('template',img)
h1, w1 = img1.shape[:2]
h2, w2 = img2.shape[:2]
w = min((w1, w2))
h = min((h1, h2))
if w <= 0 or h <= 0:
return False, 0.0
x1 = (w1 - w) / 2
y1 = (h1 - h) / 2
x2 = (w2 - w) / 2
y2 = (h2 - h) / 2
crop1 = img1[y1:y1 + h, x1:x1 + w]
crop2 = img2[y2:y2 + h, x2:x2 + w]
#resized = cv2.resize(img2, (w,h))
#print('Track #' + repr(self.trackId) + ' img1:' + repr(img1.shape[:2]) + ' img2:' + repr(img2.shape[:2]) + ' resize:' + repr(resized.shape[:2]))
#cv2.imshow('resized',resized)
res = cv2.matchTemplate(crop1, crop2, cv2.TM_CCOEFF_NORMED)
min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(res)
return max_val >= thresh, max_val
def getNewestRect(self):
#return self.rect[0]
return self.rect[-1]
def getTrackletRect(self):
return self.trackletRects[-1]
def getTrackletRects(self):
return self.trackletRects
def saveLastTracklet(self):
lasttracklet = self.getLastTracklet(45)
if not lasttracklet is None:
h, w = lasttracklet[0].shape[:2]
outputStream = cv2.VideoWriter(
'track' + repr(self.trackId) + '.avi',
cv2.VideoWriter_fourcc(*'DIVX'), self.outFps, (w, h))
for frame in lasttracklet:
outputStream.write(frame)
outputStream.release()
def getLastTracklet(self, numFrames):
if len(self.trackletRects) > numFrames:
h, w = self.savedFrames[0].shape[:2]
temptracklet = []
#temprects = []
xmax, xmin, ymax, ymin = (0, w, 0, h)
for rect in self.trackletRects[-numFrames:]:
x, y, w, h = rect
xmin = x if x < xmin else xmin
xmax = (x + w) if (x + w) > xmax else xmax
ymin = y if y < ymin else ymin
ymax = (y + h) if (y + h) > ymax else ymax
for frame in self.savedFrames[-numFrames:]:
temptracklet.append(frame[ymin:ymin + (ymax - ymin),
xmin:xmin + (xmax - xmin)])
return temptracklet
else:
return None
def getNewestTracklet(self):
return self.tracklet[-1]
def getOldestRect(self):
return self.rect[0]
#if len(self.rect) < self.numTemplates :
# return self.rect[len(self.rect)-1]
#else:
# return self.rect[self.numTemplates-1]
def getNewestTemplate(self):
return self.template[-1]
def getOldestTemplate(self):
return self.template[0]
#if len(self.template) < self.numTemplates :
# return self.template[len(self.template)-1]
#else:
# return self.template[self.numTemplates-1]
def getPath(self):
return self.kalmanFast.getPath()
def getPredicted(self):
return self.kalmanFast.getPredicted()
def getTrackId(self):
return self.trackId
def isOccluded(self):
return self.occluded
def getSearchWindow(self):
return self.searchWindow
def isActive(self):
return self.active
def updateClass(self, classType, kp):
self.lastKeypoints = kp
self.lastClass = classType
self.classHistory.append(classType)
try:
lc = mode(self.classHistory)
self.lastClass = lc
return self.lastClass
except StatisticsError:
return self.lastClass
def getLastClass(self):
return self.lastClass
def getLastKeypoints(self):
return self.lastKeypoints
def getTracklet(self):
return self.tracklet
def updateAction(self, action):
self.actions.append(action)
return self.actions[-1]
def getLastAction(self):
return self.actions[-1]
def setDoTrackWrite(self, doWrite):
self.doTrackWrite = doWrite
def setOutFps(self, fps):
self.outFps = fps
image_folder_path = "./images/"
measurements_path = "./measurements/ball_20.csv"
dt = 0.1
u = 0 # Acceleration
std_acc = 0.001 # Q
std_pos = 0.5 # R
image_names = os.listdir(image_folder_path)
image_names.sort()
measurements = np.genfromtxt(measurements_path, delimiter=",")
kalman = Kalman()
kalman.init(dt, u, std_acc, std_pos)
for i in range(len(image_names)):
image = cv2.imread(image_folder_path + image_names[i])
measurement = measurements[i].astype(np.int)
# Given measurement color BLUE
cv2.rectangle(image, (measurement[0], measurement[1]),
(measurement[0]+measurement[2], measurement[1]+measurement[3]), (255, 0, 0), 2)
(x, y, vx, vy) = kalman.predict()
# Predicted state color GREEN
cv2.rectangle(image, (x, y),
(x + measurement[2], y + measurement[3]), (0, 255, 0), 2)
update = np.array([[measurement[0]+(measurement[2]/2)], [measurement[1]+(measurement[3]/2)]])
from numpy.random import multivariate_normal
import matplotlib.pyplot as plt
from kalman import Kalman
# The prior density
Sigma = [[0.4, 0.3], [0.3, 0.45]]
Sigma = np.array(Sigma)
x_hat = np.array([0.2, -0.2])
# Define A, Q, G, R
G = np.eye(2)
R = 0.5 * Sigma
A = np.eye(2)
Q = np.zeros(2)
## Initialize Kalman filter
kn = Kalman(A, G, Q, R)
kn.set_state(x_hat, Sigma)
# The true value of the state
theta = np.zeros(2) + 4.0
T = 1000
z = np.empty(T)
for t in range(T):
# Measure the error
y = multivariate_normal(mean=theta, cov=R)
kn.update(y)
z[t] = np.sqrt(np.sum((theta - kn.current_x_hat)**2))
fig, ax = plt.subplots()
ax.plot(range(T), z)
# Create a socket object soc = socket.socket(socket.AF_INET, socket.SOCK_STREAM) time.sleep(3) # arduino will reset on serial open # let it calibrate, has to be still # Initialize orientation vectors acc = [0.0, 0.0, 0.0] mag = [0.0, 0.0, 0.0] # Reset rpy reset_condition = True # Kalman objects kalman_x = Kalman() kalman_y = Kalman() kalman_z = Kalman() ##################################### # YOU NEED TO ADJUST THESE (TASK 4) # ##################################### # Set Kalman sensor noise stdevs kalman_x.setSensorNoise(0.2, 0.03) # 0.2, 0.03 kalman_y.setSensorNoise(0.2, 0.03) # 0.2, 0.03 kalman_z.setSensorNoise(0.6, 0.03) # 0.6, 0.03 # kalman_x.setSensorNoise(0.01, 0.01) # kalman_y.setSensorNoise(0.01, 0.01) # kalman_z.setSensorNoise(0.01, 0.01)
import time
import numpy as np
from icm20948 import ICM20948
from kalman import Kalman
import csv
imu = ICM20948()
sensorfusion = Kalman()
imu.read_sensors()
imu.computeOrientation()
sensorfusion.roll = imu.roll
sensorfusion.pitch = imu.pitch
sensorfusion.yaw = imu.yaw
currTime = time.time()
while True:
imu.read_sensors()
imu.computeOrientation()
print(
f"roll: {round(imu.roll,3)}, pitch: {round(imu.pitch,3)}, yaw: {round(imu.yaw,3)}"
)
newTime = time.time()
dt = newTime - currTime
currTime = newTime
sensorfusion.computeAndUpdateRollPitchYaw(
imu.accel_data[0], imu.accel_data[1], imu.accel_data[2],
# === Define A, Q, G, R === #
G = np.eye(2)
R = 0.5 * np.eye(2)
A = [[0.5, 0.4],
[0.6, 0.3]]
Q = 0.3 * np.eye(2)
# === Define the prior density === #
Sigma = [[0.9, 0.3],
[0.3, 0.9]]
Sigma = np.array(Sigma)
x_hat = np.array([8, 8])
# === Initialize the Kalman filter === #
kn = Kalman(A, G, Q, R)
kn.set_state(x_hat, Sigma)
# === Set the true initial value of the state === #
x = np.zeros(2)
# == Print eigenvalues of A == #
print("Eigenvalues of A:")
print(eigvals(A))
# == Print stationary Sigma == #
S, K = kn.stationary_values()
print("Stationary prediction error variance:")
print(S)
# === Generate the plot === #
class KAgent(object):
"""Class handles all command and control logic for a teams tanks."""
initialGridConstant = 0
def __init__(self, bzrc):
self._kalman = Kalman()
self.aliveTime = time.time()
self.bzrc = bzrc
self.constants = self.bzrc.get_constants()
# print self.constants
self.commands = []
self.potentialFields = []
self.updates = []
self.flag_sphere = 400
self.obstacle_sphere = 1000
self.enemy_sphere = 100
self.explore_sphere = 50
self.prev_x = {}
self.prev_y = {}
self.stuck_ticks = {}
for x in xrange(20):
self.prev_x[x] = 0
self.prev_y[x] = 0
self.stuck_ticks[x] = 0
self.path_fields = []
self.grid = Grid(int(self.constants['worldsize']),
int(self.constants['worldsize']),
self.initialGridConstant,
self.initialGridConstant)
def tick(self, time_diff):
"""Some time has passed; decide what to do next."""
# don't need to know where the flags or shots are when exploring. Enemies are included in the 'othertanks' call
# pos,partialGrid = self.bzrc.get_occgrid()
# self.grid.updateGrid(pos,partialGrid)
self._kalman.setDT(time_diff)
self.mytanks = self.bzrc.get_mytanks()
# self.othertanks = self.bzrc.get_othertanks()
targetTank = self.bzrc.get_othertanks()[0]
self.commands = []
if targetTank.status == 'alive':
self.lock_on(targetTank)
else:
self.aliveTime = time.time()
stopMoving = Command(0,0,0,False)
self.commands.append(stopMoving)
self.bzrc.do_commands(self.commands)
self._kalman.resetArrays()
def lock_on(self, targetTank):
print str(targetTank.x)
agentTank = self.mytanks[0]
Zt = array([[targetTank.x],
[targetTank.y]])
self._kalman.updateKalmanFilter(Zt)
est = self._kalman.H.dot(self._kalman.mu)
self.updates.append(((int(est[0][0]), int(est[1][0])),self._kalman.sigmaT))
aimAngle,distance = self.take_aim((agentTank.x,agentTank.y), agentTank.angle)
command = Command(0,0,aimAngle*2,True)
if aimAngle < 1 and aimAngle > -1:
if distance < 350:
command = Command(0,0,aimAngle*2,True)
else:
command = Command(0,0,aimAngle*2,False)
else:
command = Command(0,0,aimAngle*2,False)
self.commands.append(command)
self.bzrc.do_commands(self.commands)
def take_aim(self, tankPosition, tankAngle):
xPos = self._kalman.mu[0][0]
xVel = self._kalman.mu[1][0]
xAcc = self._kalman.mu[2][0]
yPos = self._kalman.mu[3][0]
yVel = self._kalman.mu[4][0]
yAcc = self._kalman.mu[5][0]
tankX, tankY = tankPosition
velocity = math.sqrt(xVel**2 + yVel**2)
acceleration = math.sqrt(xAcc**2 + yAcc**2)
distance = self.dist(tankX,tankY,xPos,yPos)
timeToEnemy = 0
r = (100-velocity)**2 - 4*(acceleration/2)*distance*-1
plusRoot = (-(100-velocity) + math.sqrt(r)) / acceleration
minusRoot = (-(100-velocity) - math.sqrt(r)) / acceleration
if plusRoot > minusRoot:
timeToEnemy = plusRoot
else:
timeToEnemy = minusRoot
self._kalman.setDT(timeToEnemy)
projectionMatrix = self._kalman.projectPosition()
projectedX = projectionMatrix[0][0]
projectedY = projectionMatrix[3][0]
distance = self.dist(tankX,tankY,projectedX,projectedY)
angel = math.atan2(projectedY - tankY, projectedX - tankX)
return (self.normalize_angle(angel - tankAngle), distance)
def doUpdate(self):
# print len(self.updates)
for update in self.updates:
pos,partialGrid = update
self.grid.updateGrid(pos,partialGrid)
self.updates = []
def dist(self, x1, y1, x2, y2):
dist_result = math.sqrt((float(x1) - float(x2))**2 + (float(y1) - float(y2))**2)
#print dist_result
return dist_result
def normalize_angle(self, angle):
"""Make any angle be between +/- pi."""
angle -= 2 * math.pi * int (angle / (2 * math.pi))
if angle <= -math.pi:
angle += 2 * math.pi
elif angle > math.pi:
angle -= 2 * math.pi
return angle
gx_data = df['gx'].as_matrix()
gy_data = df['gy'].as_matrix()
gz_data = df['gz'].as_matrix()
offset_num = 500
ax_offset = np.mean(ax_data[0:offset_num])
ay_offset = np.mean(ay_data[0:offset_num])
az_offset = np.mean(az_data[0:offset_num])
gx_offset = np.mean(gx_data[0:offset_num])
gy_offset = np.mean(gy_data[0:offset_num])
gz_offset = np.mean(gz_data[0:offset_num])
fi_kf = Kalman(F=np.array([[1, 1], [0, 1]]),
P = np.array([[10, 0], [0, 10]]),
Q = np.array([[0.001, 0], [0, 0.001]]),
R = np.array([[1000]]),
H = np.array([[1, 0]]),
x = np.array([[0], [0]]),
u = np.array([[0], [0]]))
theta_kf = Kalman(F=np.array([[1, 1], [0, 1]]),
P = np.array([[1000, 0], [0, 1000]]),
Q = np.array([[0.1, 0], [0, 0.1]]),
R = np.array([[10]]),
H = np.array([[1, 0]]),
x = np.array([[0], [0]]),
u = np.array([[0], [0]]))
should_init_kf = True
fi_raw_list = []
theta_raw_list = []
def kalmanSmooth(Y, pi0, sigma0, A, C, Q, R, nLF):
"""
Runs Kalman filtering and smoothing step of dynamic factor model estimator.
Returns
-nLF x T numpy array
Smoothed means
-T x nLF x nLF numpy array
Smoothed covariances
-T-1 x nLF x nLF numpy array
Smoothed lagged covariances (ie, cov[x_t, x_t-1])
"""
N, T = Y.shape
# Initialize Kalman filter
kf = Kalman(mu_0=pi0.copy(), sigma_0=sigma0.copy(), A=A, B=np.zeros(2*[2*nLF]), C=C, D=None, Q=Q, R=R)
# sigma_t|t, mu_t|t
sigma_filt = np.zeros([T, nLF, nLF])
sigma_filt[0] = sigma0.copy()
mu_filt = np.zeros([T, nLF])
mu_filt[0] = pi0.copy()
# sigma_t|t-1, mu_t|t-1. NOTE: indexed by t-1!!!
sigma_pred = np.zeros([T-1, nLF, nLF])
mu_pred = np.zeros([T-1, nLF])
# Avoid printing repetitive errors
#printedPosSemidefErr = False
# Filtering step
for t in range(1, T):
kf.predict()
# Save mu_t|t-1 and sigma_t|t-1
sigma_pred[t-1, :, :] = kf.sigma
mu_pred[t-1] = kf.mu
# Update if we have a measurement. Nans are handled by Kalman.
kf.update(Y[:, t])
# Save filtered mean, covariance
sigma_filt[t, :, :] = kf.sigma
mu_filt[t] = kf.mu
# Make sure filtered covariance is positive semidefinite!
#eigs, _ = np.linalg.eig(sigma_filt[t])
#if len(np.where(eigs < 0)[0]) > 0 and not printedPosSemidefErr:
# print "\tsigma_filt[%i] is not positive semidefinite"%t
# printedPosSemidefErr = True
# Make sure filtered covariance is symmetric (with REALLY loose tolerance...)
#if np.allclose(sigma_filt[t], sigma_filt[t].T, rtol=1e-4) == False:
# print "\tsigma_filt[%i] is not symmetric..."%t
# sigma_t|T, mu_t|T
sigma_smooth = np.zeros((T, nLF, nLF))
mu_smooth = np.zeros((T, nLF))
# Initialize: sigma_T|T = sigma_T|T(filtered)
sigma_smooth[-1] = sigma_filt[-1]
# mu_T|T = mu_T|T(filtered)
mu_smooth[-1] = mu_filt[-1]
# Lagged covariance. Indexed by t-1.
sigmaLag_smooth = np.zeros((T-1, nLF, nLF))
# sigmaLag_{T,T-1} = (1 - K_T C) A V_{T-1|T-1}, where K_T is Kalman gain at last timestep.
K_T = np.dot(sigma_pred[-1], np.dot(kf.C.T, np.linalg.pinv(np.dot(kf.C, \
np.dot(sigma_pred[-1], kf.C.T)) + kf.R)))
sigmaLag_smooth[-1] = np.dot(np.dot((np.identity(nLF) - np.dot(K_T, kf.C)), kf.A), sigma_filt[-2])
# Backwards Kalman gain
J = np.zeros((T-1, nLF, nLF))
# Smoothing step. Runs from t=T-1 to t=0.
for t in range(T-2, -1, -1):
# Backward Kalman gain matrix
J[t] = np.dot(np.dot(sigma_filt[t], kf.A.T), np.linalg.pinv(sigma_pred[t]))
# Smoothed mean
mu_smooth[t] = mu_filt[t] + np.dot(J[t], mu_smooth[t+1] - mu_pred[t])
# Smoothed covariance. This is explicity symmetric.
sigma_smooth[t, :, :] = sigma_filt[t] + np.dot(np.dot(J[t], sigma_smooth[t+1] - sigma_pred[t]), J[t].T)
# Lagged smoothed covariance (NOT SYMMETRIC!). Pretty sure this is correct...
for t in range(T-3, -1, -1):
sigmaLag_smooth[t, :, :] = np.dot(sigma_filt[t+1], J[t].T) \
+ np.dot(np.dot(J[t+1], (sigmaLag_smooth[t+1] - np.dot(kf.A, sigma_filt[t+1]))), J[t].T)
# Fill in missing Y values
YImp = Y.copy()
nanRows, nanCols = np.where(np.isnan(YImp))
for s, t in zip(nanRows, nanCols):
YImp[s, t] = np.dot(C[s, :], mu_smooth[t, :])
return mu_smooth.T, sigma_smooth, sigmaLag_smooth, YImp, sigma_filt
import matplotlib.pyplot as plt
from kalman import Kalman
# === define prior density === #
Sigma = [[0.4, 0.3], [0.3, 0.45]]
Sigma = np.array(Sigma)
x_hat = np.array([0.2, -0.2])
# === define A, Q, G, R === #
G = np.eye(2)
R = 0.5 * Sigma
A = np.eye(2)
Q = np.zeros(2)
# === initialize Kalman filter === #
kn = Kalman(A, G, Q, R)
kn.set_state(x_hat, Sigma)
# === the true value of the state === #
theta = np.zeros(2) + 4.0
# === generate plot === #
T = 1000
z = np.empty(T)
for t in range(T):
# Measure the error
y = multivariate_normal(mean=theta, cov=R)
kn.update(y)
z[t] = np.sqrt(np.sum((theta - kn.current_x_hat)**2))
fig, ax = plt.subplots()
class KAgent(object):
"""Class handles all command and control logic for a teams tanks."""
initialGridConstant = 0
def __init__(self, bzrc):
self._kalman = Kalman()
self.aliveTime = time.time()
self.bzrc = bzrc
self.constants = self.bzrc.get_constants()
# print self.constants
self.commands = []
self.potentialFields = []
self.updates = []
self.flag_sphere = 400
self.obstacle_sphere = 1000
self.enemy_sphere = 100
self.explore_sphere = 50
self.prev_x = {}
self.prev_y = {}
self.stuck_ticks = {}
for x in xrange(20):
self.prev_x[x] = 0
self.prev_y[x] = 0
self.stuck_ticks[x] = 0
self.path_fields = []
self.grid = Grid(int(self.constants['worldsize']),
int(self.constants['worldsize']),
self.initialGridConstant, self.initialGridConstant)
def tick(self, time_diff):
"""Some time has passed; decide what to do next."""
# don't need to know where the flags or shots are when exploring. Enemies are included in the 'othertanks' call
# pos,partialGrid = self.bzrc.get_occgrid()
# self.grid.updateGrid(pos,partialGrid)
self._kalman.setDT(time_diff)
self.mytanks = self.bzrc.get_mytanks()
# self.othertanks = self.bzrc.get_othertanks()
targetTank = self.bzrc.get_othertanks()[0]
self.commands = []
if targetTank.status == 'alive':
self.lock_on(targetTank)
else:
self.aliveTime = time.time()
stopMoving = Command(0, 0, 0, False)
self.commands.append(stopMoving)
self.bzrc.do_commands(self.commands)
self._kalman.resetArrays()
def lock_on(self, targetTank):
print str(targetTank.x)
agentTank = self.mytanks[0]
Zt = array([[targetTank.x], [targetTank.y]])
self._kalman.updateKalmanFilter(Zt)
est = self._kalman.H.dot(self._kalman.mu)
self.updates.append(
((int(est[0][0]), int(est[1][0])), self._kalman.sigmaT))
aimAngle, distance = self.take_aim((agentTank.x, agentTank.y),
agentTank.angle)
command = Command(0, 0, aimAngle * 2, True)
if aimAngle < 1 and aimAngle > -1:
if distance < 350:
command = Command(0, 0, aimAngle * 2, True)
else:
command = Command(0, 0, aimAngle * 2, False)
else:
command = Command(0, 0, aimAngle * 2, False)
self.commands.append(command)
self.bzrc.do_commands(self.commands)
def take_aim(self, tankPosition, tankAngle):
xPos = self._kalman.mu[0][0]
xVel = self._kalman.mu[1][0]
xAcc = self._kalman.mu[2][0]
yPos = self._kalman.mu[3][0]
yVel = self._kalman.mu[4][0]
yAcc = self._kalman.mu[5][0]
tankX, tankY = tankPosition
velocity = math.sqrt(xVel**2 + yVel**2)
acceleration = math.sqrt(xAcc**2 + yAcc**2)
distance = self.dist(tankX, tankY, xPos, yPos)
timeToEnemy = 0
r = (100 - velocity)**2 - 4 * (acceleration / 2) * distance * -1
plusRoot = (-(100 - velocity) + math.sqrt(r)) / acceleration
minusRoot = (-(100 - velocity) - math.sqrt(r)) / acceleration
if plusRoot > minusRoot:
timeToEnemy = plusRoot
else:
timeToEnemy = minusRoot
self._kalman.setDT(timeToEnemy)
projectionMatrix = self._kalman.projectPosition()
projectedX = projectionMatrix[0][0]
projectedY = projectionMatrix[3][0]
distance = self.dist(tankX, tankY, projectedX, projectedY)
angel = math.atan2(projectedY - tankY, projectedX - tankX)
return (self.normalize_angle(angel - tankAngle), distance)
def doUpdate(self):
# print len(self.updates)
for update in self.updates:
pos, partialGrid = update
self.grid.updateGrid(pos, partialGrid)
self.updates = []
def dist(self, x1, y1, x2, y2):
dist_result = math.sqrt((float(x1) - float(x2))**2 +
(float(y1) - float(y2))**2)
#print dist_result
return dist_result
def normalize_angle(self, angle):
"""Make any angle be between +/- pi."""
angle -= 2 * math.pi * int(angle / (2 * math.pi))
if angle <= -math.pi:
angle += 2 * math.pi
elif angle > math.pi:
angle -= 2 * math.pi
return angle
from scipy.linalg import eigvals
from kalman import Kalman
# === Define A, Q, G, R === #
G = np.eye(2)
R = 0.5 * np.eye(2)
A = [[0.5, 0.4], [0.6, 0.3]]
Q = 0.3 * np.eye(2)
# === Define the prior density === #
Sigma = [[0.9, 0.3], [0.3, 0.9]]
Sigma = np.array(Sigma)
x_hat = np.array([8, 8])
# === Initialize the Kalman filter === #
kn = Kalman(A, G, Q, R)
kn.set_state(x_hat, Sigma)
# === Set the true initial value of the state === #
x = np.zeros(2)
# == Print eigenvalues of A == #
print("Eigenvalues of A:")
print(eigvals(A))
# == Print stationary Sigma == #
S, K = kn.stationary_values()
print("Stationary prediction error variance:")
print(S)
# === Generate the plot === #
# Create a socket object soc = socket.socket(socket.AF_INET, socket.SOCK_STREAM) time.sleep(3) # arduino will reset on serial open # let it calibrate, has to be still # Initialize orientation vectors acc = [0.0, 0.0, 0.0] mag = [0.0, 0.0, 0.0] # Reset rpy reset_condition = True # Kalman objects kalman_x = Kalman() kalman_y = Kalman() kalman_z = Kalman() ##################################### # YOU NEED TO ADJUST THESE (TASK 4) # ##################################### # Set Kalman sensor noise stdevs #kalman_x.setSensorNoise(0.2, 0.03) # 0.2, 0.03 #kalman_y.setSensorNoise(0.2, 0.03) # 0.2, 0.03 #kalman_z.setSensorNoise(0.6, 0.03) # 0.6, 0.03 ##kalman_x.setSensorNoise(0.01, 0.01) ##kalman_y.setSensorNoise(0.01, 0.01)
figdistr, axis = pl.subplots(3, 1)
figdistr.tight_layout()
init_area = 5
s = 2
# Desired altitude and heading
alt_d = 4
q1.yaw_d = -np.pi
q2.yaw_d = np.pi/2
q3.yaw_d = 0
# Kalman filter for position estimation
kf = Kalman()
for t in time:
# Simulation
X = np.append(q1.xyz[0:2], np.append(q2.xyz[0:2], q3.xyz[0:2]))
V = np.append(q1.v_ned[0:2], np.append(q2.v_ned[0:2], q3.v_ned[0:2]))
U = fc.u_acc(X, V)
q1.set_a_2D_alt_lya(U[0:2], -alt_d)
q2.set_a_2D_alt_lya(U[2:4], -alt_d)
q3.set_a_2D_alt_lya(U[4:6], -alt_d)
# relative velocities!
v2_relative = q2.v_ned - q1.v_ned
v3_relative = q3.v_ned - q1.v_ned
def test(noise=20,
time_max=10,
show_acceleration_plots=True,
filter_name='kalman'):
import sys
sys.path.append('../')
import numpy as np
import time
# Record Data
import time
import numpy as np
import pyautogui
def record_data(noise=20, time_max=10):
print("RECORDING DATA")
print("Move your cursor as if you were driving it.")
coords = []
coords_actual = []
timestamp = []
T = time.time()
while time.time() - T < time_max:
t = time.time()
while True:
if time.time() - t > .02:
timestamp.append(time.time() - T)
act = np.asarray(pyautogui.position())
pos = act + 2 * noise * np.asarray([
np.random.rand(), np.random.rand()
]) - noise * np.asarray(
[np.random.rand(), np.random.rand()])
coords.append(pos)
pos_actual = act
coords_actual.append(pos_actual)
break
print("RECORDING DATA COMPLETED")
return coords, coords_actual, timestamp
coords, coords_actual, timestamp = record_data(noise=noise,
time_max=time_max)
'''
FLOW:
INITIAL_STATE_VECTOR, INITIAL_ERROR_COVARIANCE_MATRIX
PRIORI CALCULATION, PRIORI_ERROR_COVARIANCE
OBSERVE_READINGS, EXPECTED_READINGS, EXPECTED_ERROR_COVARIANCE
KALMAN_GAIN
TAKES IN MEASUREMENT_NOISE, MEASUREMENT_MATRIX, NEW_ERROR_COVARIANCE
POSTERIORI CALCULATION
TAKES IN KALMAN_GAIN, OBSERVED_READINGS, PRIORI
POSTERIORI_ERROR_COVARIANCE
'''
'''
INPUT:
cursor coords
OUTPUT:
priori, posteriori
'''
# Dynamics
# initial_state = [x_coord, y_coord, x_velocity, y_velocity, x_acceleration, y_acceleration]
initial_state = np.asarray([coords[0][0], coords[0][1], 0, 0, 0, 0]).T
initial_error_covariance_matrix = 0 * np.eye(6)
dt = 0.02
transition_matrix = np.asarray([[1, 0, dt, 0, 0, 0], [0, 1, 0, dt, 0, 0],
[0, 0, 1, 0, dt, 0], [0, 0, 0, 1, 0, dt],
[0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]])
position_funciton = lambda position, velocity: position + velocity * dt
velocity_funciton = lambda velocity, acceleration: velocity + acceleration * dt
acceleration_function = lambda acceleration: acceleration
def transition_function(state_vector, input_vector):
# print('state_vector', state_vector.shape)
position = position_funciton(state_vector[0:2], state_vector[2:4])
velocity = velocity_funciton(state_vector[2:4], state_vector[4:6])
acceleration = acceleration_function(state_vector[4:6])
return_state = np.vstack((position, velocity, acceleration))
return np.reshape(return_state, (-1, 1)) + np.zeros(6).T.reshape(
state_vector.size, input_vector.shape[0]) * input_vector
def measurement_function(state_vector):
# print('state_vector', state_vector.shape)
return np.reshape(state_vector[0:2], (-1, 1))
# print(nd.Jacobian(transition_function)(initial_state))
input_matrix = np.zeros(6).T
input_vector = np.zeros((1, 1))
measurement_matrix = np.asarray([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]])
disturbance = 10 * np.eye(6)
uncertainty = noise * np.eye(2)
from kalman import Kalman
from extended_kalman import ExtendedKalman
kf = Kalman(
initial_state=initial_state,
initial_error_covariance_matrix=initial_error_covariance_matrix,
transition_matrix=transition_matrix,
measurement_matrix=measurement_matrix,
input_matrix=input_matrix,
input_vector=input_vector,
measurement_noise=uncertainty,
process_noise=disturbance)
ekf = ExtendedKalman(
initial_state=initial_state,
initial_error_covariance_matrix=initial_error_covariance_matrix,
transition_function=transition_function,
measurement_function=measurement_function,
input_matrix=input_matrix,
input_vector=input_vector,
measurement_noise=uncertainty,
process_noise=disturbance)
if filter_name == 'kalman':
k = kf
elif filter_name == 'extended':
k = ekf
# Visualization
length = 1920
breadth = 1080
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
fig, ax = plt.subplots()
cursor_x, cursor_y = [], []
actual, = plt.plot([], [], '-', label='actual')
xdata, ydata = [], []
recorded, = plt.plot([], [], '*', label='recorded')
priori_x, priori_y = [], []
priori_plot, = plt.plot([], [], 'g-', label='priori_plot')
posteriori_x, posteriori_y = [], []
posteriori_plot, = plt.plot([], [], 'r-', label='posteriori_plot')
if show_acceleration_plots:
fig_acc_x, acc_x = plt.subplots()
priori_acceleration_x = []
posteriori_acceleration_x = []
priori_acceleration_plot_x, = plt.plot([], [],
'g-',
label='priori_acc_x')
posteriori_acceleration_plot_x, = plt.plot([], [],
'r-',
label='posteriori_acc_x')
fig_acc_y, acc_y = plt.subplots()
priori_acceleration_y = []
posteriori_acceleration_y = []
priori_acceleration_plot_y, = plt.plot([], [],
'g-',
label='priori_acc_y')
posteriori_acceleration_plot_y, = plt.plot([], [],
'r-',
label='posteriori_acc_y')
timestamp_data = []
def init():
ax.set_xlim(0, length)
ax.set_ylim(0, breadth)
ax.legend()
if show_acceleration_plots:
acc_x.set_xlim(0, 10)
acc_x.set_ylim(-1000, 1000)
acc_x.legend()
acc_y.set_xlim(0, 10)
acc_y.set_ylim(-1000, 1000)
acc_y.legend()
return actual, recorded, priori_plot, posteriori_plot, priori_acceleration_plot_x, posteriori_acceleration_plot_x, priori_acceleration_plot_y, posteriori_acceleration_plot_y,
else:
return actual, recorded, priori_plot, posteriori_plot,
def update(frame):
n = frame
# print(xdata[-1], ydata[-1])
# print(n - 1000, n)
# print(np.asarray(coords[n - 100:n])[:, 0], 1080 - np.asarray(coords[n - 100:n])[:, 1])
pos_x = np.asarray(coords[n])[0]
pos_y = breadth - np.asarray(coords[n])[1]
# kpriori, kposteriori = kf(np.asarray([pos_x, pos_y]), None)
# epriori, eposteriori = ekf(np.asarray([pos_x, pos_y]), None)
priori, posteriori = k(np.asarray([pos_x, pos_y]), input_vector)
priori_x.append(priori[0])
priori_y.append(priori[1])
priori_plot.set_data(priori_x, priori_y)
xdata.append(pos_x)
ydata.append(pos_y)
recorded.set_data(xdata, ydata)
posteriori_x.append(posteriori[0])
posteriori_y.append(posteriori[1])
posteriori_plot.set_data(posteriori_x, posteriori_y)
cursor_x.append(np.asarray(coords_actual[n])[0])
cursor_y.append(breadth - np.asarray(coords_actual[n])[1])
actual.set_data(cursor_x, cursor_y)
if show_acceleration_plots:
timestamp_data.append(timestamp[n])
priori_acceleration_x.append(priori[4, 0])
priori_acceleration_plot_x.set_data(timestamp_data,
priori_acceleration_x)
posteriori_acceleration_x.append(posteriori[4, 0])
posteriori_acceleration_plot_x.set_data(timestamp_data,
posteriori_acceleration_x)
priori_acceleration_y.append(priori[5, 0])
priori_acceleration_plot_y.set_data(timestamp_data,
priori_acceleration_y)
posteriori_acceleration_y.append(posteriori[5, 0])
posteriori_acceleration_plot_y.set_data(timestamp_data,
posteriori_acceleration_y)
return actual, recorded, priori_plot, posteriori_plot, priori_acceleration_plot_x, posteriori_acceleration_plot_x, priori_acceleration_plot_y, posteriori_acceleration_plot_y,
else:
return actual, recorded, priori_plot, posteriori_plot,
ani = FuncAnimation(fig,
update,
frames=np.arange(1, len(coords), 2),
init_func=init,
blit=True,
interval=100)
plt.show()
def main():
print('Program Started')
sim.simxFinish(-1)
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print('Connected to remote API server')
else:
print('Failed connecting to remote API server')
returnCode, leftMotor = sim.simxGetObjectHandle(
clientID, "Pioneer_p3dx_leftMotor", sim.simx_opmode_oneshot_wait)
returnCode, rightMotor = sim.simxGetObjectHandle(
clientID, "Pioneer_p3dx_rightMotor", sim.simx_opmode_oneshot_wait)
robot = Robot(clientID, 2)
returnCode, robotHandle = sim.simxGetObjectHandle(
clientID, "Pioneer_p3dx", sim.simx_opmode_oneshot_wait)
returnCode, sensorData = sim.simxGetStringSignal(clientID, "scanRanges",
sim.simx_opmode_streaming)
l_rot_prev, r_rot_prev = 0, 0
# Initial state and initial covariance matrix
prevPosition = np.matrix(getPosition(clientID, robotHandle)).T
odPrevPos = np.matrix(getPosition(clientID, robotHandle)).T
prevErrorPosition = np.zeros((3, 3))
odPrevError = np.zeros((3, 3))
a = 0
g = 0.3
count = 0
while sim.simxGetConnectionId(clientID) != -1:
v = np.zeros((2, 1))
# speedMotors = robot.breit_controller(clientID)
# sim.simxSetJointTargetVelocity(clientID, leftMotor, speedMotors[0], sim.simx_opmode_streaming)
# sim.simxSetJointTargetVelocity(clientID, rightMotor, speedMotors[1], sim.simx_opmode_streaming)
if count == 100:
# Dead reckoning
dPhiL, dPhiR, l_rot_prev, r_rot_prev = readOdometry(
clientID, leftMotor, rightMotor, l_rot_prev, r_rot_prev)
# Initialize Kalman
kalman_filter = Kalman(dPhiL, dPhiR)
odometry = Kalman(dPhiL, dPhiR)
predPosition, predError = kalman_filter.prediction(
prevPosition, prevErrorPosition)
truePos = np.matrix(getPosition(clientID, robotHandle)).T
odPosition, odError = odometry.prediction(odPrevPos, odPrevError)
odPosition[2] = truePos[2]
odPrevPos = odPosition
odPrevError = odError
# Observations
observedFeatures = readObservations(clientID)
x, y = lidar.arrangeData(observedFeatures)
lidarInputs, nLidarInputs = lidar.split_and_merge(x, y)
d_ant = 10000000000
S = np.zeros((2, 2))
H = np.zeros((2, 3))
predPosition[2] = truePos[2]
for i in range(nLidarInputs):
for j in range(len(mapInputs)):
y, S, H = kalman_filter.getInnovation(
predPosition, predError, mapInputs[j, :],
lidarInputs[:, i])
d = y.T @ np.linalg.pinv(S) @ y
if d < g**2 and d < d_ant:
# print("=============== MATCH ================\n")
v = y
d_ant = d
estPosition, estError, y, S = kalman_filter.update(
predPosition, predError, v, S, H)
prevPosition = estPosition
prevErrorPosition = estError
print(
f'Odometry position (x) - Estimated position (x) = {float(odPosition[0] - estPosition[0])}\n'
)
print(
f'Odometry position (y) - Estimated position (y) = {float(odPosition[1] - estPosition[1])}\n'
)
count = 0
count += 1
import numpy as np
import matplotlib.pyplot as plt
from kalman import Kalman
from scipy.stats import norm
## Parameters
theta = 10
A, G, Q, R = 1, 1, 0, 1
x_hat_0, Sigma_0 = 8, 1
## Initialize Kalman filter
kalman = Kalman(A, G, Q, R)
kalman.set_state(x_hat_0, Sigma_0)
N = 5
fig, ax = plt.subplots()
xgrid = np.linspace(theta - 5, theta + 2, 200)
for i in range(N):
# Record the current predicted mean and variance, and plot their densities
m, v = kalman.current_x_hat, kalman.current_Sigma
m, v = float(m), float(v)
ax.plot(xgrid, norm.pdf(xgrid, loc=m, scale=np.sqrt(v)), label=r'$t=%d$' % i)
# Generate the noisy signal
y = theta + norm.rvs(size=1)
# Update the Kalman filter
kalman.update(y)
ax.set_title(r'First %d densities when $\theta = %.1f$' % (N, theta))
ax.legend(loc='upper left')
plt.show()
# - Position and velocity states and estimates vs time
# - Estimation error and error covariance vs time
# - Kalman gains vs time
if __name__ == "__main__":
# UUV parameters
x0 = np.zeros([1, 2]) # initial states
duration = 50 # model test duration
dt = 0.05 # time step
R = np.array([[0.01, 0], [0, 0.0001]]) # process covariance
Q = np.array([0.001]) # measurement covariance
uuv = UUV(x0, duration, dt, R, Q) # UUV model object
# Kalman Filter Init
kalman = Kalman(uuv.A, uuv.B, uuv.C, uuv.R, uuv.Q)
# plot data containers
two_sig_v = np.zeros([int(duration / dt) + 1, 2]) # two-sigma velocity boundary
two_sig_x = np.zeros([int(duration / dt) + 1, 2]) # two-sigma position boundary
mu = np.zeros([int(duration / dt) + 1, 2]) # state estimation vector
K = np.zeros([int(duration / dt) + 1, 2]) # Kalman gain vector
# Input Command Simulation
F = np.zeros([int(duration / dt)])
for i in range(int(duration / dt)):
if i < int(5 / dt):
F[i] = 500
elif i < int(25 / dt):
F[i] = 0
elif i < int(30 / dt):
import numpy as np
import matplotlib.pyplot as plt
from kalman import Kalman
from scipy.stats import norm
## Parameters
theta = 10
A, G, Q, R = 1, 1, 0, 1
x_hat_0, Sigma_0 = 8, 1
## Initialize Kalman filter
kalman = Kalman(A, G, Q, R)
kalman.set_state(x_hat_0, Sigma_0)
N = 5
fig, ax = plt.subplots()
xgrid = np.linspace(theta - 5, theta + 2, 200)
for i in range(N):
# Record the current predicted mean and variance, and plot their densities
m, v = kalman.current_x_hat, kalman.current_Sigma
m, v = float(m), float(v)
ax.plot(xgrid,
norm.pdf(xgrid, loc=m, scale=np.sqrt(v)),
label=r'$t=%d$' % i)
# Generate the noisy signal
y = theta + norm.rvs(size=1)
# Update the Kalman filter
kalman.update(y)
ax.set_title(r'First %d densities when $\theta = %.1f$' % (N, theta))
ax.legend(loc='upper left')
plt.show()
class BlueToothThreading:
"""
Bluetooth Threading
The run() method will be started and it will run in the background
until the application exits.
"""
def __init__(self, interval=1):
""" Constructor
:type interval: int
:param interval: Check interval, in seconds
"""
self.sensorfusion = Kalman()
print("Connecting to sensortag...")
self.tag = SensorTag(macAddress)
print("connected.")
self.tag.accelerometer.enable()
self.tag.magnetometer.enable()
self.tag.gyroscope.enable()
self.tag.battery.enable()
self.pitch = 0
self.angular_velocity = 0
self.linear_velocity = 0
self.acceleration = 0
time.sleep(1.0) # Loading sensors
self.prev_time = time.time()
thread = threading.Thread(target=self.run, args=())
thread.daemon = True # Daemonize thread
thread.start() # Start the execution
def run(self):
""" Method that runs forever """
while True:
accelerometer_readings = self.tag.accelerometer.read()
gyroscope_readings = self.tag.gyroscope.read()
magnetometer_readings = self.tag.magnetometer.read()
ax, ay, az = accelerometer_readings
gx, gy, gz = gyroscope_readings
mx, my, mz = magnetometer_readings
curr_time = time.time()
dt = curr_time - self.prev_time
self.sensorfusion.computeAndUpdateRollPitchYaw(
ax, ay, az, gx, gy, gz, mx, my, mz, dt)
pitch = self.sensorfusion.pitch * 0.0174533
dp = pitch - self.pitch
da = ax - self.acceleration
self.angular_velocity = dp / dt
self.linear_velocity = da * dt
self.pitch = pitch
self.acceleration = ax
self.prev_time = curr_time
print("Battery: ", self.tag.battery.read())
def take_observation(self):
return [
self.pitch, self.angular_velocity, self.linear_velocity,
self.acceleration
]
# return observation with manual calbiration
# expects list of length STATE_SIZE
def take_observation_calibrated(self, calibration):
return [
self.pitch - calibration[0],
self.angular_velocity - calibration[1],
self.linear_velocity - calibration[2],
self.acceleration - calibration[3]
]
"la": AccelerationState("la"),
"za": AccelerationState("za"),
"p": PhiState("p"),
"r": PhiState("r"),
"headr": PhiState("headr"),
"pr": ThetaState("pr", "p"),
"rr": ThetaState("rr", "r"),
"head": ThetaState("headr", "head"),
}
c = {
"t": 1.0
}
robot = Robot()
k = Kalman(sc, c)
imu = ImuSensor(k.state_keys)
axis("equal")
controls = [
(0.0, (2.0, 0.0)),
(10.0, (0.0, 0.2)),
(14.0, (0.0, 0.0))
]
controlIdx = 0
controlNext = controls[controlIdx][0]
seed(123)
t = 1.0
drift = 0.0
class Agent(object):
"""Class handles all command and control logic for a teams tanks."""
def __init__(self, bzrc):
self.bzrc = bzrc
self.constants = self.bzrc.get_constants()
self.pos_noise = 5
self.commands = []
self.kalman = Kalman(self.pos_noise)
self.max_tank_speed = float(self.constants['tankspeed'])
self.linear_acceleration = float(self.constants['linearaccel'])
self.shot_speed = float(self.constants['shotspeed'])
mytanks, othertanks, flags, shots = self.bzrc.get_lots_o_stuff()
enemies = [tank for tank in othertanks if tank.color !=
self.constants['team']]
covariance_list = [[100, 0, 0, 0, 0, 0],
[ 0, 0.1, 0, 0, 0, 0],
[ 0, 0, 0.1, 0, 0, 0],
[ 0, 0, 0, 100, 0, 0],
[ 0, 0, 0, 0, 0.1, 0],
[ 0, 0, 0, 0, 0, 0.1]]
self.observed_states = []
self.states = []
self.state_covariances = []
for enemy in enemies:
new_observed = numpy.matrix(numpy.zeros((6, 1)))
new_observed.getA()[0][0] = enemy.x
new_observed.getA()[3][0] = enemy.y
self.observed_states.append(new_observed)
new_state = numpy.matrix(numpy.zeros((6, 1)))
self.states.append(new_state)
new_covariance = numpy.matrix(covariance_list)
self.state_covariances.append(new_covariance)
# PD control
self.old_angle = [0] * len(mytanks)
# initialize plot
plt.ion()
self.fig = plt.figure()
self.fignum = 0
self.graph = self.fig.add_subplot(111)
worldsize = float(self.constants['worldsize'])
worldlimit = worldsize / 2.0
self.graph.axis([-worldlimit, worldlimit, -worldlimit, worldlimit])
plt.show()
def tick(self, time_diff):
"""Some time has passed; decide what to do next."""
mytanks, othertanks, flags, shots = self.bzrc.get_lots_o_stuff()
self.mytanks = mytanks
self.othertanks = othertanks
self.enemies = [tank for tank in othertanks if tank.color !=
self.constants['team']]
for index in xrange(len(self.enemies)):
# don't run filter if enemy is dead
if self.enemies[index].status != 'alive':
continue
# get the saved previous state
previous_observed = self.observed_states[index]
previous_state = self.states[index]
# get the observed state at this time
enemy = self.enemies[index]
observed_velocity_x = self.approximateDerivative(previous_observed.getA()[0][0], enemy.x, time_diff, self.max_tank_speed)
observed_velocity_y = self.approximateDerivative(previous_observed.getA()[3][0], enemy.y, time_diff, self.max_tank_speed)
observed_acceleration_x = self.approximateDerivative(previous_observed.getA()[1][0], observed_velocity_x, time_diff, self.linear_acceleration)
observed_acceleration_y = self.approximateDerivative(previous_observed.getA()[4][0], observed_velocity_y, time_diff, self.linear_acceleration)
observed_state = numpy.matrix(numpy.zeros((6, 1)))
observed_state.getA()[0][0] = enemy.x
observed_state.getA()[1][0] = observed_velocity_x
observed_state.getA()[2][0] = observed_acceleration_x
observed_state.getA()[3][0] = enemy.y
observed_state.getA()[4][0] = observed_velocity_y
observed_state.getA()[5][0] = observed_acceleration_y
self.observed_states[index] = observed_state
# get the saved previous state covariance
previous_covariance = self.state_covariances[index]
predicted_state, updated_covariance = self.kalman.predictState(
previous_state,
observed_state,
previous_covariance,
time_diff
)
x = numpy.arange(-400, 400)
y = numpy.arange(-400, 400)
X, Y = numpy.meshgrid(x, y)
Z = mlab.bivariate_normal(X, Y, updated_covariance.getA()[0][0], updated_covariance.getA()[3][3], predicted_state.getA()[0][0], predicted_state.getA()[3][0], updated_covariance.getA()[0][3])
self.graph.clear()
self.graph.plot([observed_state.getA()[0][0]], [observed_state.getA()[3][0]], 'ro')
self.graph.contour(X, Y, Z)
shots_x = [shot.x for shot in shots]
shots_y = [shot.y for shot in shots]
self.graph.plot(shots_x, shots_y, 'bo')
self.states[index] = predicted_state
self.state_covariances[index] = updated_covariance
# create tank commands (shoot stuff)
for tank in mytanks:
if len(self.states) > 0:
# get predicted enemy position
first_enemy_index = 0
predicted_state = self.states[first_enemy_index]
pred_x = predicted_state.getA()[0][0]
pred_y = predicted_state.getA()[3][0]
distance = math.sqrt((pred_x - tank.x) **2 + (pred_y - tank.y) **2)
secondsTilCollision = distance / self.shot_speed
future_state = self.kalman.predictFutureState(predicted_state, secondsTilCollision)
fut_x = future_state.getA()[0][0]
fut_y = future_state.getA()[3][0]
# move
self.create_command(tank, fut_x, fut_y, time_diff)
results = self.bzrc.do_commands(self.commands)
self.fig.canvas.draw()
#if self.fignum < 20:
# self.fig.savefig("plots/kalman_{0}.png".format(self.fignum), bbox_inches="tight")
# self.fignum += 1
def approximateDerivative(self, p1, p2, time_diff, cap):
difference = p2 - p1
derivative = difference / time_diff
absDerivative = abs(derivative)
if absDerivative > cap:
derivative = cap * ( derivative / absDerivative )
return derivative
def create_command(self, tank, target_x, target_y, time_diff):
"""Set command to move to given coordinates."""
angvel = 0
speed = 0
shoot = 0
if time_diff <= 0:
return
self.graph.plot([tank.x], [tank.y], 'gs')
self.graph.plot([target_x], [target_y], 'go')
# PD Controller - angle
target_angle = math.atan2(target_y - tank.y, target_x - tank.x)
angle_remaining = self.angle_remaining(tank.angle, target_angle)
angvel = self.pd_angvel(tank, target_angle, time_diff)
plot_line_length = 350
x_forward = plot_line_length * math.cos(tank.angle) + tank.x
y_forward = plot_line_length * math.sin(tank.angle) + tank.y
self.graph.plot([tank.x, x_forward], [tank.y, y_forward], color='r', linestyle='-', linewidth=1)
if angvel > 1:
angvel = 1
elif angvel < -1:
angvel = -1
if abs(angle_remaining) < 0.01:
shoot = 1
command = Command(tank.index, speed, angvel, shoot)
self.commands.append(command)
self.old_angle[tank.index] = tank.angle
def pd_angvel(self, tank, target_angle, time_diff):
"""PD Controller for the angular velocity of the tank."""
kp = 1.0
kd = -0.2
angle_remaining = self.angle_remaining(tank.angle, target_angle)
differential = self.angle_remaining(self.old_angle[tank.index], tank.angle) / time_diff
angvel = ( kp * angle_remaining ) + ( kd * differential )
return angvel
def angle_remaining(self, tank_angle, target_angle):
"""Find the angle remaining (in radians) between the tank and the target."""
tank_angle = self.normalize_angle(tank_angle)
target_angle = self.normalize_angle(target_angle)
# If the angles are on the same hemisphere, target - tank.
if tank_angle * target_angle > 0:
return target_angle - tank_angle
# Otherwise they are on opposite hemispheres.
positive_angle = max(tank_angle, target_angle)
negative_angle = min(tank_angle, target_angle)
tank_positive = True
if tank_angle < 0:
tank_positive = False
# Compare the angles to turn right and left.
right_angle = -1 * ( positive_angle - negative_angle )
left_angle = 2 * math.pi - positive_angle + negative_angle
if not tank_positive:
temp_angle = -1 * right_angle
right_angle = -1 * left_angle
left_angle = temp_angle
# Pick the smallest angle.
if abs(right_angle) <= abs(left_angle):
return right_angle
return left_angle
def normalize_angle(self, angle):
"""Make any angle be between +/- pi."""
angle -= 2 * math.pi * int (angle / (2 * math.pi))
if angle <= -math.pi:
angle += 2 * math.pi
elif angle > math.pi:
angle -= 2 * math.pi
return angle
