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 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)
class VelocityFilter:
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 predict(self):
return self.filter.predict()
def update(self, meas):
return self.filter.update(meas)
def run(self, meas):
self.update(meas)
return self.predict()
pos = (i, i)
ra, rb = d.range_of(pos)
rx, ry = d.range_of((i + f1.x[0, 0], i + f1.x[2, 0]))
print('range =', ra, rb)
z = np.mat([[ra - rx], [rb - ry]])
print('z =', z)
f1.H = H_of(pos, pos_a, pos_b)
print('H =', f1.H)
##f1.update (z)
print(f1.x)
xs.append(f1.x[0, 0] + i)
ys.append(f1.x[2, 0] + i)
pxs.append(pos[0])
pys.append(pos[1])
f1.predict()
print(f1.H * f1.x)
print(z)
print(f1.x)
f1.update(z)
print(f1.x)
p1, = plt.plot(xs, ys, 'r--')
p2, = plt.plot(pxs, pys)
plt.legend([p1, p2], ['filter', 'ideal'], 2)
plt.show()
pos = (i, i)
ra, rb = d.range_of(pos)
rx, ry = d.range_of((i + f1.x[0, 0], i + f1.x[2, 0]))
print('range =', ra, rb)
z = np.mat([[ra - rx], [rb - ry]])
print('z =', z)
f1.H = H_of(pos, pos_a, pos_b)
print('H =', f1.H)
##f1.update (z)
print(f1.x)
xs.append(f1.x[0, 0] + i)
ys.append(f1.x[2, 0] + i)
pxs.append(pos[0])
pys.append(pos[1])
f1.predict()
print(f1.H * f1.x)
print(z)
print(f1.x)
f1.update(z)
print(f1.x)
p1, = plt.plot(xs, ys, 'r--')
p2, = plt.plot(pxs, pys)
plt.legend([p1, p2], ['filter', 'ideal'], 2)
plt.show()
from KalmanFilter import KalmanFilter import time kf = KalmanFilter() kf.update(5) print kf.predict() time.sleep(.2) kf.update(5.5) print kf.predict() time.sleep(.2) kf.update(6) print kf.predict() time.sleep(.2) kf.update(6.5) print kf.predict() time.sleep(.2) kf.update(7) print kf.predict() time.sleep(.2) kf.update(15) print kf.predict() time.sleep(.2) kf.update(8.5) print kf.predict() time.sleep(.2) kf.update(7)
def main():
params = check_params(parse_args(sys.argv[1:]))
kf = KalmanFilter()
if params['file']:
template = read_template(params)
surf, flann = init_surf()
kp, des = surf.detectAndCompute(template, None)
#Start up Video processing thread
vs = PiVideoStream(resolution=(params['xRes'], params['yRes'])).start()
#Wait for camera to warm up
time.sleep(3.0)
#Used for calculating FPS
startTime = time.time()
if params['capTime'] == 0:
endTime = startTime + 1000000000
else:
endTime = startTime + params['capTime']
frames = 0
# Open up serial port with Pixhawk
if params['sendToPX4']:
port = serial.Serial('/dev/ttyAMA0', 57600)
mav = mavlink.MAVLink(port)
# Typically only need to search within a small Y-range
yRes = params['yRes']
(cropYmin, cropYmax) = (yRes * .25, yRes * .70)
#Take weighted average of last # of distances to filter out noise
notFoundCount = 1000
while time.time() < endTime:
frames += 1
frame = vs.read()
frame = frame[cropYmin : cropYmax, 0:params['xRes']]
found, [x,y], frame = FindingFuncs.findPatternSURF(frame, surf, kp, des, template, flann, params['preview'])
# Count how many frames it has been since the RPi has not found anything
if not found:
notFoundCount += 1
else:
notFoundCount = 0
kf.update(x)
# How many frames until you assume to keep going straight.
if notFoundCount > 100:
kf.reset()
x = params['xRes'] / 2
else:
x = kf.predict()
loc = boundFloat(2.0 * x / params['xRes'] - 1, -1., 1.)
if params['sendToPX4']:
message = mavlink.MAVLink_duck_leader_loc_message(loc, 5.0)
mav.send(message)
if params['debug'] :
print str(time.time() - startTime) + ", " + str(loc)
if params['preview']:
# Draw a circle on frame where the Kalman filter predicts.
cv2.circle(frame, (int(x), 10), 4, (0, 0, 255), 6)
frame = imutils.resize(frame, width=1000)
cv2.imshow("Preview", frame)
#Check for keypress, ending if Q is entered
key = cv2.waitKey(1) & 0xFF
if (key == ord("q")):
break;
totalTime = time.time() - startTime
cv2.destroyAllWindows()
vs.stop()
print "Average main FPS: " + str(float(frames) / totalTime) + "fps"
def main():
# Create opencv video capture object
# VideoCap = cv2.VideoCapture('./video/randomball.avi')
VideoCap = cv2.VideoCapture('./video/multi.mp4')
# VideoCap = cv2.VideoCapture('./video/cars2.mp4')
frame_width = int(VideoCap.get(3))
frame_height = int(VideoCap.get(4))
fps = VideoCap.get(cv2.CAP_PROP_FPS)
outVid = cv2.VideoWriter('trackedObjects.avi',
cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'), fps,
(frame_width, frame_height))
largestDistance = math.sqrt(frame_width**2 + frame_height**2)
# 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)
# debugMode = 1
debugMode = 0
KFs = []
KFs_used = []
j = 0
while (True):
j += 1
# Read frame
ret, frame = VideoCap.read()
if not ret and cv2.waitKey(2) & 0xFF == ord('q'):
outVid.release()
VideoCap.release()
cv2.destroyAllWindows()
break
# Detect object
try:
centers = detect(frame, debugMode)
except:
outVid.release()
VideoCap.release()
cv2.destroyAllWindows()
break
# If centroids are detected then track them
if (len(centers) > 0):
# getting rid of unused Kalman Filters
temp = KFs.copy()
KFs = []
for i in range(len(temp)):
if (KFs_used[i]):
KFs.append(temp[i])
gc.collect()
# predict all KFs
predicted_x, predicted_y = predictKFs(KFs)
KFs_used = [False] * len(KFs)
for i in range(len(centers)):
# Draw the detected circle
cv2.circle(frame, (int(centers[i][0]), int(centers[i][1])), 10,
(0, 191, 255), 2)
curr_KF = getClosestKF(centers[i][0], centers[i][1],
predicted_x, predicted_y,
largestDistance / 24)
if (curr_KF < 0):
KF = KalmanFilter(1.0 / fps, 1, 1, 1, 0.1, 0.1)
x, y = KF.predict()
KFs.append(KF)
KFs_used.append(True)
else:
KF = KFs[curr_KF]
KFs_used[curr_KF] = True
x, y = predicted_x[curr_KF], predicted_y[curr_KF]
# Draw a rectangle as the predicted object position
x, y = int(x), int(y)
cv2.rectangle(frame, (x - 15, y - 15), (x + 15, y + 15), (
255,
0,
), 2)
# Update
(x1, y1) = KF.update(centers[i])
x1, y1 = int(x1), int(y1)
# Draw a rectangle as the estimated object position
cv2.rectangle(frame, (x1 - 15, y1 - 15), (x1 + 15, y1 + 15),
(0, 0, 255), 2)
cv2.putText(frame, "Estimated Position" + str(i),
(x1 + 15, y1 + 10), 0, 0.5, (0, 0, 255), 2)
cv2.putText(frame, "Predicted Position" + str(i), (x + 15, y),
0, 0.5, (255, 0, 0), 2)
cv2.putText(frame, "Measured Position" + str(i),
(int(centers[i][0] + 15), int(centers[i][1] - 15)),
0, 0.5, (0, 191, 255), 2)
cv2.imshow('image', frame)
outVid.write(frame)
cv2.waitKey(HiSpeed - ControlSpeedVar + 1)
class KalmanAgent(Agents.Agent):
def __init__(self, tank_index, target_color, state):
self.state = state
self.target_color = target_color
self.tank_index = tank_index
self.pdc = PDController()
self.timer_id = Timer.add_task(self.update)
self.kvis = KalmanVisualizer(800, 800)
self.kfilter = KalmanFilter()
def update(self):
# update state
self.state.update_mytanks()
self.state.update_othertanks()
tank = self.state.mytanks['0']
angvel = self.getAngvelToTarget(tank)
tank.set_angvel(angvel)
def getAngvelToTarget(self, tank):
"""
Calculates the vector that we should be facing to kill the target, then uses the PDController to find the
angular velocity required to face the target.
This calculation is performed like this:
1. Use the Kalman Filter to get the predicted location of the target.
2. Add to that vector the distance that the target will travel while the bullet fires.
3. Pass the vector into the PDController to find the angular velocity that we need to face the right way
:return float: the angular velocity that we need in order to correctly face the target.
"""
# get newest data about target
target = self.state.othertanks[self.target_color][0]
# get the angle to the target
# step 1: Kalman Filter
ut, width, height = self.kfilter.update(npy.array([[target.x], [target.y]]))
xpos, xvel, xacc, ypos, yvel, yacc = ut
# step 2: bullet travel time
t = math.sqrt((tank.x - target.x) ** 2 + (tank.y - target.y) ** 2) / 100
hitzone_x = xpos + xvel*t + 0.5 # * xacc * t**2 + 100 * math.cos(tank.x) * t
hitzone_y = ypos + yvel*t + 0.5 # * yacc * t**2 + 100 * math.sin(tank.y) * t
self.kvis.update(xpos, ypos, width, target.x, target.y, hitzone_x, hitzone_y)
# step 3: PDController to find angular velocity
ang = self.ang(hitzone_x, hitzone_y)
# print ang
if abs(math.pi + tank.angle - ang) < 0.1:
tank.shoot()
target_vec = [-math.cos(ang), -math.sin(ang)]
next_action = self.pdc.get_next_action(self.state.mytanks[self.tank_index], target_vec)
return next_action['angvel']
def ang(self, x, y):
tank = self.state.mytanks['0']
angle = math.atan2(tank.y - y, tank.x - x)
'''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
class KalmanBoxTracker(object):
"""
This class represents the internel state of individual tracked objects observed as bbox.
"""
count = 0
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 update(self, bbox):
"""
Updates the state vector with observed bbox.
"""
self.time_since_update = 0
self.history = []
self.hits += 1
self.hit_streak += 1
self.kf.update(convert_bbox_to_z(bbox))
def predict(self):
"""
Advances the state vector and returns the predicted bounding box estimate.
"""
if ((self.kf.x[6] + self.kf.x[2]) <= 0):
self.kf.x[6] *= 0.0
self.kf.predict()
self.age += 1
if (self.time_since_update > 0):
self.hit_streak = 0
self.time_since_update += 1
self.history.append(convert_x_to_bbox(self.kf.x))
return self.history[-1]
def get_state(self):
"""
Returns the current bounding box estimate.
"""
return convert_x_to_bbox(self.kf.x)
class KalmanFilterSimulatorWindow(AnimationWindow):
"""
The mouse motion tracking window and some GUI input widgets.
"""
def helperWidget(self):
"""
GUI designer function,
The Canvas is the main window, Some helper Widget is added
on the side of the canvas.
"""
# Intall Pause button, default button, restart button and a check button
# on the frame to better organise the interface.
self.frameTop = tk.Frame()
self.frameTop.pack(anchor=tk.NW, side=tk.TOP, pady=25)
self.pauseButton = tk.Button(master=self.frameTop,
text="Pause",
width=8,
command=self.pauseClick)
self.pauseButton.pack(anchor=tk.NE, side=tk.LEFT, padx=5)
self.defaultButton = tk.Button(master=self.frameTop,
text="Restore Default Values",
command=self.restoreClick)
self.defaultButton.pack(anchor=tk.NW, side=tk.LEFT, padx=5)
self.restartButton = tk.Button(master=self.frameTop,
text="Restart",
command=self.restartClick)
self.restartButton.pack(anchor=tk.NW, side=tk.LEFT, padx=5)
self.showMouseTraceButtonOn = tk.BooleanVar()
self.showMouseTraceButton = tk.Checkbutton(
master=self.frameTop,
text="Show Mouse Trace",
variable=self.showMouseTraceButtonOn,
command=self.showMouseTrace)
self.showMouseTraceButton.pack(anchor=tk.NW, side=tk.LEFT, padx=5)
# setup the instance of Entry
self.entries = OrderedDict()
self.entries["A"] = [[0] * 4 for i in range(4)]
self.entries["B"] = [[0] * 4 for i in range(4)]
self.entries["H"] = [[0] * 4 for i in range(4)]
self.entries["Q"] = [[0] * 4 for i in range(4)]
self.entries["R"] = [[0] * 4 for i in range(4)]
self.entries["N"] = [[0] * 4 for i in range(4)]
self.code2entries = {0: "A", 1: "B", 2: "H", 3: "Q", 4: "R", 5: "N"}
# Install Entries of Matrix A, B, H on the medium top frame beneath the top frame.
self.frameMediumTop = tk.Frame()
self.frameMediumTop.pack(anchor=tk.NW, side=tk.TOP, pady=10)
for _ in range(3):
# Frame1 hold entries of matrix A, B, H in each iteration
self.frame1 = tk.Frame(master=self.frameMediumTop)
self.frame1.pack(anchor=tk.NW, side=tk.LEFT, padx=10)
for i in range(5):
self.frame2 = tk.Frame(master=self.frame1)
self.frame2.pack(anchor=tk.NW, side=tk.TOP)
if i == 4:
if _ == 0:
label = tk.Label(master=self.frame1,
text="A matrix\nState Transition")
label.pack(anchor=tk.NW, side=tk.LEFT, padx=2)
elif _ == 1:
label = tk.Label(master=self.frame1,
text="B matrix\nInput Control")
label.pack(anchor=tk.NW, side=tk.LEFT, padx=2)
elif _ == 2:
label = tk.Label(master=self.frame1,
text="H matrix\nMeasurement")
label.pack(anchor=tk.NW, side=tk.LEFT, padx=2)
else:
for j in range(4):
self.entries[self.code2entries[_]][i][j] = tk.Entry(
master=self.frame2, width=3)
self.entries[self.code2entries[_]][i][j].pack(
anchor=tk.NW, side=tk.LEFT, padx=1)
# Install Entries of Matrix Q, R, N on the medium top frame beneath the medium top frame.
self.frameMediumBottom = tk.Frame()
self.frameMediumBottom.pack(anchor=tk.NW, side=tk.TOP)
for _ in range(3, 6):
# Frame1 hold entries of matrix Q, R, N in each iteration
self.frame1 = tk.Frame(master=self.frameMediumBottom)
self.frame1.pack(anchor=tk.NW, side=tk.LEFT, padx=10)
for i in range(5):
self.frame2 = tk.Frame(master=self.frame1)
self.frame2.pack(anchor=tk.NW, side=tk.TOP)
if i == 4:
if _ == 3:
label = tk.Label(master=self.frame1,
text="Q matrix\nAction Uncertainty")
label.pack(anchor=tk.NW, side=tk.LEFT, padx=2)
elif _ == 4:
label = tk.Label(master=self.frame1,
text="R matrix\nSensor Noise")
label.pack(anchor=tk.NW, side=tk.LEFT, padx=2)
elif _ == 5:
label = tk.Label(master=self.frame1,
text="N matrix\nDistrubation")
label.pack(anchor=tk.NW, side=tk.LEFT, padx=2)
else:
for j in range(4):
self.entries[self.code2entries[_]][i][j] = tk.Entry(
master=self.frame2, width=3)
self.entries[self.code2entries[_]][i][j].pack(
anchor=tk.NW, side=tk.LEFT, padx=1)
# Install a slider bar on the bottom frame
self.frameBottom = tk.Frame()
self.frameBottom.pack(anchor=tk.NW, side=tk.TOP, pady=50)
label = tk.Label(master=self.frameBottom, text="Fade-Out Time (sec):")
label.pack(anchor=tk.NW, side=tk.TOP, padx=10, pady=0)
# Life time for the fading-out points
self.lifetime = 4
self.lifetimeScale = tk.Scale(self.frameBottom,
from_=0.1,
to=8,
length=200,
orient=tk.HORIZONTAL,
resolution=0.1,
command=self.onScale)
# initialize the scale
self.lifetimeScale.set(self.lifetime)
self.lifetimeScale.pack(anchor=tk.NW, side=tk.TOP, padx=10, pady=0)
def setup(self):
"""
Setup the intial condition for the Kalman Filter,
"""
self.mouseX, self.mouseY = 0, 0
self.premouseX, self.premouseY = 0, 0
# delta_t
self.A = np.array([[1, 0, self.frame_time, 0],
[0, 1, 0, self.frame_time], [0, 0, 1, 0],
[0, 0, 0, 1]])
# no control unit
self.B = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
# measurement unit
self.H = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
# model noise
self.Q = np.array([[0.01, 0, 0, 0], [0, 0.01, 0, 0], [0, 0, 0.01, 0],
[0, 0, 0, 0.01]])
# measurement noise
self.R = np.array([[0.1, 0, 0, 0], [0, 0.1, 0, 0], [0, 0, 0.1, 0],
[0, 0, 0, 0.1]])
# Noise to disturb the true position of the mouse
self.N = np.array([[100, 0, 0, 0], [0, 100, 0, 0], [0, 0, 0, 0],
[0, 0, 0, 0]])
# current state
self.cur_x = np.array([self.mouseX, self.mouseY, 0, 0])
# current error covariance matrix
self.cur_P = np.array([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0],
[0, 0, 0, 0]])
# Record the default value.
self.Adefault = self.A.copy()
self.Bdefault = self.B.copy()
self.Hdefault = self.H.copy()
self.Qdefault = self.Q.copy()
self.Rdefault = self.R.copy()
self.Ndefault = self.N.copy()
# control unit, For this case there is no control unit
self.control = np.array([0, 0, 0, 0])
# rPoint: record the measurement, the measurement is created
# by the true hidden state with Gaussian noise
# kPoint: store the positions of points from estimation of the Kalman filter
# tPoint: record true position of the mouse
self.rPoints = deque()
self.kPoints = deque()
self.tPoints = deque()
# self.pPoints = deque()
self.running = True
# I commented out the prediction line as it seems not very useful from my point of view.
# But I add the true mouse position.
# self.predictionOn = True
self.mousepositionOn = self.showMouseTraceButtonOn.get()
# Instance of the Kalman filter,
self.kfmodel = KalmanFilter(self.A, self.B, self.H, self.Q, self.R,
self.cur_x, self.cur_P)
# Intialize the entries,
# TO THINK: there is a better way? better to put it in a helperWidget() fucntion,
# as this function is used to design graphic interface...
self.setMatrix("A", self.Adefault)
self.setMatrix("B", self.Bdefault)
self.setMatrix("H", self.Hdefault)
self.setMatrix("Q", self.Qdefault)
self.setMatrix("R", self.Rdefault)
self.setMatrix("N", self.Ndefault)
def draw(self):
"""
Update the canvas in each frame per second.
"""
if not self.running:
return
# Clean the Canvas first.
self.canvas.delete(tk.ALL)
# Receive the values from the entries of each matrix.
# Is there a better to do that? As if I don't modify the matrix,
# I don't need to read the entries every time.
# Is there a callback function of the Entry object? NO...,
# https://stackoverflow.com/questions/6548837/how-do-i-get-an-event-callback-when-a-tkinter-entry-widget-is-modified
# There is a solution, But it adds some complexities. I don't like it.
self.modifyMatrix("A")
self.modifyMatrix("B")
self.modifyMatrix("H")
self.modifyMatrix("Q")
self.modifyMatrix("R")
self.modifyMatrix("N")
self.kfmodel.A = self.A
self.kfmodel.B = self.B
self.kfmodel.H = self.H
self.kfmodel.Q = self.Q
self.kfmodel.R = self.R
# the core of this code... ....
#**************** Kalman Filter *******************#
# Hidden State
self.curState = np.array([
self.mouseX, self.mouseY,
(self.mouseX - self.premouseX) / self.frame_time,
(self.mouseY - self.premouseY) / self.frame_time
])
# Gaussian Noise
self.measureNoise = np.random.multivariate_normal([0, 0, 0, 0], self.N)
# Apply measurement, z_k = H_k * x_k + V_k
self.measureState = np.dot(self.H, self.curState) + self.measureNoise
self.kfmodel.update(self.measureState, self.control)
#*************** END ******************#
# Store the measured, estimated, and mouse position points
# to the deque
self.rPoints.append(
Point(self, self.measureState[0], self.measureState[1],
[255, 255, 255], self.lifetime))
self.kPoints.append(
ConnectedPoint(self, self.kfmodel.cur_x[0], self.kfmodel.cur_x[1],
[0, 255, 0], self.lifetime, self.kfmodel.last_x[0],
self.kfmodel.last_x[1]))
if self.mousepositionOn:
self.tPoints.append(
ConnectedPoint(self, self.mouseX, self.mouseY, [0, 0, 255],
self.lifetime, self.premouseX, self.premouseY))
# Draw points in the deque, pop out those points which are dead
def drawPoints(pointDeque):
count_dead = 0
for point in pointDeque:
if point.isAlive():
point.draw()
else:
count_dead += 1
for _ in range(count_dead):
pointDeque.popleft()
drawPoints(self.rPoints)
drawPoints(self.kPoints)
if self.mousepositionOn:
drawPoints(self.tPoints)
###############################
# Not very useful... to show the status of state.
#self.showStatus()
# keep track of the current mouse position as previous mouse position
self.premouseX, self.premouseY = self.mouseX, self.mouseY
def mousemotion(self, mouseposition):
"""
store the mouse position
"""
self.mouseX, self.mouseY = mouseposition[0], mouseposition[1]
def pauseClick(self):
"""
pause or resume animation
"""
self.running = not self.running
if self.running:
self.pauseButton.config(relief=tk.RAISED, text="Pause")
else:
self.pauseButton.config(relief=tk.SUNKEN, text="Resume")
def restartClick(self):
"""
restart the animation
"""
self.setup()
def restoreClick(self):
"""
restore to the default value...
"""
self.lifetime = 4
self.lifetimeScale.set(self.lifetime)
self.setMatrix("A", self.Adefault)
self.setMatrix("B", self.Bdefault)
self.setMatrix("H", self.Hdefault)
self.setMatrix("Q", self.Qdefault)
self.setMatrix("R", self.Rdefault)
self.setMatrix("N", self.Ndefault)
def showMouseTrace(self):
"""
toggle the trace of mouse
"""
if self.showMouseTraceButtonOn.get() == True:
self.mousepositionOn = True
else:
self.mousepositionOn = False
def onScale(self, value):
"""
Fade out time
"""
self.lifetime = float(value)
def setMatrix(self, id, vals):
"""
set the values of the matrix
"""
for i in range(len(vals)):
for j in range(len(vals[0])):
self.entries[id][i][j].delete(0, tk.END)
self.entries[id][i][j].insert(0, vals[i, j])
def modifyMatrix(self, id):
"""
modify the matrix according to the GUI input.
"""
switcher = {
"A": self.A,
"B": self.B,
"H": self.H,
"Q": self.Q,
"R": self.R,
"N": self.N
}
for i in range(4):
for j in range(4):
# The following state make sure even the entry is empty, it
# can still work. Alphabic characters are not alowed.
if self.entries[id][i][j].get() == "":
switcher[id][i, j] = 1.0
else:
switcher[id][i, j] = float(self.entries[id][i][j].get())
def showStatus(self):
"""
Print the status of the mouse (position and velocity),
the Kalman filter estimation of the status of the mouse
"""
statustext = """Mouse status, (x, y, velx, vely) = ({0:7.0f}, {1:7.0f}, {2:7.2f}, {3:7.2f})\n
Kalman filter estimation = ({4:7.0f}, {5:7.0f}, {6:7.2f}, {7:7.2f})\n""".format(
self.curState[0], self.curState[1], self.curState[2],
self.curState[3], self.kfmodel.cur_x[0], self.kfmodel.cur_x[1],
self.kfmodel.cur_x[2], self.kfmodel.cur_x[3])
self.canvas.create_text(0,
150,
text=statustext,
fill="green4",
anchor=tk.NW)
VideoCap=cv2.VideoCapture(0)
KF=KalmanFilter(0.1, [0, 0])
while(True):
ret, frame=VideoCap.read()
points, mask=detect_inrange(frame, 800)
#points, mask=detect_visage(frame)
etat=KF.predict().astype(np.int32)
cv2.circle(frame, (int(etat[0]), int(etat[1])), 2, (0, 255, 0), 5)
cv2.arrowedLine(frame,
(etat[0], etat[1]), (etat[0]+etat[2], etat[1]+etat[3]),
color=(0, 255, 0),
thickness=3,
tipLength=0.2)
if (len(points)>0):
cv2.circle(frame, (points[0][0], points[0][1]), 10, (0, 0, 255), 2)
KF.update(np.expand_dims(points[0], axis=-1))
cv2.imshow('image', frame)
if mask is not None:
cv2.imshow('mask', mask)
if cv2.waitKey(1)&0xFF==ord('q'):
VideoCap.release()
cv2.destroyAllWindows()
break
[0, 0, 1, dt],
[0, 0, 0, 1]])
f.H = np.mat ([[1, 0, 0, 0],
[0, 0, 1, 0]])
f.Q *= 4.
f.R = np.mat([[50,0],
[0, 50]])
f.x = np.mat([0,0,0,0]).T
f.P *= 100.
xs = []
ys = []
count = 200
for i in range(count):
z = np.mat([[i+random.randn()*1],[i+random.randn()*1]])
f.predict ()
f.update (z)
xs.append (f.x[0,0])
ys.append (f.x[2,0])
plt.plot (xs, ys)
plt.show()
class speedEstimator():
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 vel_from_dis(self, l_0, l_1, l_2, t0, t1, t2):
t_1 = t1 - t0
t_2 = t2 - t1
if (l_2 - l_1) * (l_1 - l_0) > 0:
d = abs(l_2 * l_2 - l_1 * l_1 -
(l_1 * l_1 - l_0 * l_0) * t_2 / t_1)
tl = t_1 * t_2 + t_2 * t_2
return math.sqrt(d / tl)
else:
return False
def range_key_pairs_maintaince(self, range, time):
if abs(range - self.lastPoint) > self.distanceThreshold:
self.lastPoint = range
self.keyMeasPairs.append([range, time])
return True
else:
return False
def estimate_speed(self, range, time, interval):
fdragne = self.filter_range(range)
self.speedWindowSize = 5 + 0.1 * range
if self.range_key_pairs_maintaince(
fdragne, time) and len(self.keyMeasPairs) >= 2 * interval:
tempresult = self.vel_from_dis(self.keyMeasPairs[-2 * interval][0],
self.keyMeasPairs[-interval][0],
self.keyMeasPairs[-1][0],
self.keyMeasPairs[-2 * interval][1],
self.keyMeasPairs[-interval][1],
self.keyMeasPairs[-1][1])
if tempresult:
self.speedWindow.append(tempresult)
if len(self.speedWindow) > (self.speedWindowSize - 1):
self.curSpeed = np.median(
self.speedWindow
) # Estimation of this linear motion speed
self.speedRecord.append(self.curSpeed)
self.speedUpdateTime.append(time)
self.validSpeedUpdated = True
while (len(self.speedWindow) > (self.speedWindowSize - 1)):
self.speedWindow.pop(0)
else:
self.validSpeedUpdated = False
def filter_range(self, range):
if self.rangeKF.initialized == False:
self.rangeKF.x = np.array([range])
self.filtedRange.append(range)
self.rangeKF.initialized = True
else:
self.rangeKF.predict()
self.rangeKF.update(range)
self.filtedRange.append(self.rangeKF.x)
return self.filtedRange[-1]
def get_vel(self):
return self.curSpeed
class DroneVideoDisplay(QtGui.QMainWindow):
StatusMessages = {
DroneStatus.Emergency : 'Emergency',
DroneStatus.Inited : 'Initialized',
DroneStatus.Landed : 'Landed',
DroneStatus.Flying : 'Flying',
DroneStatus.Hovering : 'Hovering',
DroneStatus.Test : 'Test (?)',
DroneStatus.TakingOff : 'Taking Off',
DroneStatus.GotoHover : 'Going to Hover Mode',
DroneStatus.Landing : 'Landing',
DroneStatus.Looping : 'Looping (?)'
}
DisconnectedMessage = 'Disconnected'
UnknownMessage = 'Unknown Status'
def __init__(self):
# Construct the parent class
super(DroneVideoDisplay, self).__init__()
# Setup our very basic GUI - a label which fills the whole window and holds our image
self.setWindowTitle('AR.Drone Video Feed')
self.imageBox = QtGui.QLabel(self)
self.setCentralWidget(self.imageBox)
self.controller = BasicDroneController()
self.counter = 0
# Subscribe to the /ardrone/navdata topic, of message type navdata, and call self.ReceiveNavdata when a message is received
self.subNavdata = rospy.Subscriber('/ardrone/navdata',Navdata,self.ReceiveNavdata)
# Subscribe to the drone's video feed, calling self.ReceiveImage when a new frame is received
self.subVideo = rospy.Subscriber('/ardrone/image_raw',Image,self.ReceiveImage)
'''BEGIN CHANGES'''
#Define Kalman Filter constants
time = GUI_UPDATE_PERIOD
time2 = time*time
#1D case for velocity in the x-direction
dimension = 1
A = np.identity(dimension)
B = np.matrix(time)
H = np.identity(dimension)
P = np.identity(dimension)
Q = np.identity(dimension)
R = np.identity(dimension)
#tweak covariance matrices
Q = np.dot(1,Q)
R = np.dot(0.1, R)
#create the Kalman Filter instance
self.kfilter = KalmanFilter(A, P, R, Q, H, B, dimension)
#rospy.loginfo("INIT")
#create empty array to house our estimates
self.state_estimate = []
self.state_real = []
#### Computer vision code
self.x_pix = 320
self.y_pix = 240
self.x_ang = np.radians(54.4)
self.y_ang = np.radians(37.8)
self.prev_frame = None
self.prev_points = None
self.prev_time = None
self.vel = []
plt.ion()
'''END CHANGES'''
# Holds the image frame received from the drone and later processed by the GUI
self.image = None
self.imageLock = Lock()
self.tags = []
self.tagLock = Lock()
# Holds the status message to be displayed on the next GUI update
self.statusMessage = ''
# Tracks whether we have received data since the last connection check
# This works because data comes in at 50Hz but we're checking for a connection at 4Hz
self.communicationSinceTimer = False
self.connected = False
# A timer to check whether we're still connected
self.connectionTimer = QtCore.QTimer(self)
self.connectionTimer.timeout.connect(self.ConnectionCallback)
self.connectionTimer.start(CONNECTION_CHECK_PERIOD)
# A timer to redraw the GUI
self.redrawTimer = QtCore.QTimer(self)
self.redrawTimer.timeout.connect(self.RedrawCallback)
self.redrawTimer.start(GUI_UPDATE_PERIOD)
# Called every CONNECTION_CHECK_PERIOD ms, if we haven't received anything since the last callback, will assume we are having network troubles and display a message in the status bar
def ConnectionCallback(self):
self.connected = self.communicationSinceTimer
self.communicationSinceTimer = False
def RedrawCallback(self):
if self.image is not None:
# We have some issues with locking between the display thread and the ros messaging thread due to the size of the image, so we need to lock the resources
self.imageLock.acquire()
try:
'''
TODO:
1. Create Kalman Filter instance in constructor. DONE
2. Create optical flow instance in constructor. DONE
3. Retrieve controller navdata here. DONE
4. Retrieve image matrix here. Conver to cv matrix. DONE
5. Run optical flow alg. on image. DONE
6. Make prediction with controller data. DONE
7. Update prediction with image data. DONE
8. Plot estimate vs real continuously DONE
9. GetHeight() function in drone_controller.py INCOMPLETE
10. GetTime() function in here INCOMPLETE
'''
'''BEGIN CHANGES'''
#convert the ROS image to OpenCV and apply some processing. then convert back to ROS
openimage = ToOpenCV(self.image)
# make picture 2D
frame = cv2.cvtColor(openimage, cv2.COLOR_BGR2GRAY)
''' TODO: Implement GetHeight in drone_controller.py '''
feature_params = dict( maxCorners = 100,
qualityLevel = 0.3,
minDistance = 7,
blockSize = 7 )
if self.prev_frame is None:
self.prev_frame = frame
self.prev_points = cv2.goodFeaturesToTrack(self.prev_frame, mask=None, **feature_params)
self.prev_time = GUI_UPDATE_PERIOD #self.GetTime() is set to a constant for now
else:
h = self.controller.GetHeight()
xdist = (h * np.tan(self.x_ang / 2.0))
ydist = (h * np.tan(self.y_ang / 2.0))
pix2mx = self.x_pix / xdist
pix2my = self.y_pix / ydist
curr_frame = frame
''' Implement GetTime() '''
curr_time = GUI_UPDATE_PERIOD #self.GetTime() is set to constant for now
new_points, status, error = cv2.calcOpticalFlowPyrLK(self.prev_frame, curr_frame, self.prev_points)
good_new = new_points[status==1]
good_old = self.prev_points[status==1]
assert good_new.shape == good_old.shape
### Calculate velocity components
sum_x = 0.0
sum_y = 0.0
ptslen = len(good_new)
xcomp = 0
ycomp = 0
for x in range(ptslen):
xcomp = ((good_new[x][1] - good_old[x][1]) / self.x_pix) / (curr_time / 1000.0) #- self.prev_time
ycomp = ((good_new[x][0] - good_old[x][0]) / self.y_pix) / (curr_time / 1000.0)
sum_y += ycomp
sum_x += xcomp
avg_x = np.divide(xcomp, ptslen)
avg_y = np.divide(ycomp, ptslen)
self.vel.append(avg_x) #only x for now
# iterate next frames
self.prev_frame = curr_frame
self.prev_points = new_points
self.prev_time = curr_time
#Convert to ROS
ROSimage = ToRos(openimage)
##############################################
### Change velocity to get correct one here ##
##############################################
estimated_velocity = self.vel[-1:] ######
##############################################
if estimated_velocity == []:
estimated_velocity = 0
#rospy.loginfo("estimated_velocity")
#rospy.loginfo(estimated_velocity)
u_k = 0
real_velocity = 0
#update the measured accelerations and velocities
real_velocity = self.controller.GetVelocity()
rospy.loginfo("real_velocity")
rospy.loginfo(real_velocity)
u_k = self.controller.GetAcceleration()
z_k = estimated_velocity
rospy.loginfo("z_k")
rospy.loginfo(estimated_velocity)
#Kalman Filter step
self.kfilter.predictState(u_k)
self.kfilter.getKalmanGain()
#rospy.loginfo("kalman_gain")
#rospy.loginfo(self.kfilter.kalmanGain)
self.kfilter.update(estimated_velocity)
self.state_estimate.append(self.kfilter.x_k.item(0))
self.state_real.append(real_velocity)
#plot everything here, estimates not plotting atm
if self.counter > 100:
plt.plot(self.state_estimate[-90:], label = "estimated velocity", color = 'black')
plt.plot(self.state_real[-90], color = 'yellow')
else:
plt.plot(self.state_estimate, label = "estimated velocity", color = 'black')
plt.plot(self.state_real, color = 'yellow')
self.counter += 1
#rospy.loginfo("estimate")
#rospy.loginfo(self.state_estimate)
#rospy.loginfo("real state")
#rospy.loginfo(self.state_real)
plt.draw()
'''END CHANGES'''
# Convert the ROS image into a QImage which we can display
image = QtGui.QPixmap.fromImage(QtGui.QImage(ROSimage.data, ROSimage.width, ROSimage.height, QtGui.QImage.Format_RGB888))
if len(self.tags) > 0:
self.tagLock.acquire()
try:
painter = QtGui.QPainter()
painter.begin(image)
painter.setPen(QtGui.QColor(0,255,0))
painter.setBrush(QtGui.QColor(0,255,0))
for (x,y,d) in self.tags:
r = QtCore.QRectF((x*image.width())/1000-DETECT_RADIUS,(y*image.height())/1000-DETECT_RADIUS,DETECT_RADIUS*2,DETECT_RADIUS*2)
painter.drawEllipse(r)
painter.drawText((x*image.width())/1000+DETECT_RADIUS,(y*image.height())/1000-DETECT_RADIUS,str(d/100)[0:4]+'m')
painter.end()
finally:
self.tagLock.release()
finally:
self.imageLock.release()
# display the window.
self.resize(image.width(),image.height())
self.imageBox.setPixmap(image)
# Update the status bar to show the current drone status & battery level
self.statusBar().showMessage(self.statusMessage if self.connected else self.DisconnectedMessage)
def ReceiveImage(self,data):
# Indicate that new data has been received (thus we are connected)
self.communicationSinceTimer = True
# We have some issues with locking between the GUI update thread and the ROS messaging thread due to the size of the image, so we need to lock the resources
self.imageLock.acquire()
try:
self.image = data # Save the ros image for processing by the display thread
finally:
self.imageLock.release()
def ReceiveNavdata(self,navdata):
# Indicate that new data has been received (thus we are connected)
self.communicationSinceTimer = True
# Update the message to be displayed
msg = self.StatusMessages[navdata.state] if navdata.state in self.StatusMessages else self.UnknownMessage
self.statusMessage = '{} (Battery: {}%)'.format(msg,int(navdata.batteryPercent))
self.tagLock.acquire()
try:
if navdata.tags_count > 0:
self.tags = [(navdata.tags_xc[i],navdata.tags_yc[i],navdata.tags_distance[i]) for i in range(0,navdata.tags_count)]
else:
self.tags = []
finally:
self.tagLock.release()
from KalmanFilter import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt
import numpy.random as random
f = KalmanFilter(dim=4)
dt = 1
f.F = np.mat([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]])
f.H = np.mat([[1, 0, 0, 0], [0, 0, 1, 0]])
f.Q *= 4.
f.R = np.mat([[50, 0], [0, 50]])
f.x = np.mat([0, 0, 0, 0]).T
f.P *= 100.
xs = []
ys = []
count = 200
for i in range(count):
z = np.mat([[i + random.randn() * 1], [i + random.randn() * 1]])
f.predict()
f.update(z)
xs.append(f.x[0, 0])
ys.append(f.x[2, 0])
plt.plot(xs, ys)
plt.show()
class RobotControl(object):
"""
Class used to interface with the rover. Gets sensor measurements through ROS subscribers,
and transforms them into the 2D plane, and publishes velocity commands.
"""
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 dijkstras(occupancy_map, x_spacing, y_spacing, start, goal):
def process_measurements(self):
"""
Main loop of the robot - where all measurements, control, and estimation
are done. This function is called at 60Hz
"""
meas = self.robot_sim.get_measurements()
imu_meas = self.robot_sim.get_imu()
self.vw = self.diff_drive_controller.compute_vel(self.state, self.goal)
print "VW: ", self.vw
print "Running Controller."
if self.vw[2] == False:
self.robot_sim.command_velocity(self.vw[0], self.vw[1])
else:
self.robot_sim.command_velocity(0, 0)
est_x = self.kalman_filter.step_filter(self.vw, imu_meas, meas)
print "EST X: ", est_x, est_x[2][0]
if est_x[2][0] > 2.617991667:
est_x[2][0] = 2.617991667
if est_x[2][0] < 0.523598333:
est_x[2][0] = 0.523598333
self.state = est_x
print "Get GT Pose: ", self.robot_sim.get_gt_pose()
print "EKF Pose: ", est_x
self.robot_sim.get_gt_pose()
self.robot_sim.set_est_state(est_x)
if imu_meas != None:
self.kalman_filter.prediction(self.vw, imu_meas)
if meas != None and meas != []:
print("Measurements: ", meas)
if imu_meas != None:
# self.kalman_filter.prediction(self.vw, imu_meas)
self.kalman_filter.update(meas)
pos_x_check = ((self.goal[0] + self.state_tol) > est_x.item(0)) and \
((self.goal[0] - self.state_tol) < est_x.item(0))
pos_y_check = ((self.goal[1] + self.state_tol) > est_x.item(1)) and \
((self.goal[1] - self.state_tol) < est_x.item(1))
if pos_x_check and pos_y_check:
if self.path != []:
self.goal = self.path.pop()
# self.goal[0] += self.x_offset/2
# self.goal[1] += y_spacing
else:
self.goal = est_x
class SensorSimulator:
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 getSensorReadings(self, sensor_id, dataSize):
self.readings = getData(sensor_id, dataSize)
return self.readings
def RDR(self, sensorReading):
distance = sum = 0.0
transmitted_counter = 0
corr = 0.0
list_1 = []
est_list = []
s = 0
if len(self.cache) <= self.cacheLen:
self.cache.append(sensorReading)
SensorSimulator.findEnergyConsumption(self)
self.correcteddata.append(sensorReading)
self.totaltrans += 1
#print(sensorReading)
self.kf.predict()
self.kf.update([sensorReading])
return sensorReading
else:
if sensorReading == self.cache[self.cacheLen - 1]:
self.correcteddata.append(sensorReading)
return
else:
for i in range(0, self.cacheLen):
distance += pow(sensorReading - self.cache[i], 2)
sum += self.cache[i]
distance = math.sqrt(distance)
corr = 1 - distance / sum
est = self.kf.predict()
self.est_list.append(est[0])
estval = est[0][0]
self.kf.update(est[0][0])
#print(sensorReading - estval, ", ", corr)
if corr < self.theta:
sensorReading = est[0][0]
self.cache.pop(0)
self.cache.append(sensorReading)
self.counter += 1
SensorSimulator.findEnergyConsumption(self)
self.correcteddata.append(
sensorReading) ####################
self.totaltrans += 1
#print(sensorReading)
return sensorReading
else:
#print(sensorReading - estval)
if abs(sensorReading - estval) < self.emax:
self.correcteddata.append(sensorReading)
exit
else:
self.cache.pop(0)
self.cache.append(sensorReading)
self.counter += 1
SensorSimulator.findEnergyConsumption(self)
self.correcteddata.append(sensorReading)
self.totaltrans += 1
#print(sensorReading)
return sensorReading
def findVariance(self):
self.variance = np.var(self.readings)
return self.variance
def findEnergyEfficiency(self):
# print(self.totalTransferredData)
#print(3.6-self.batCap, 0)
# print("Energy Efficiency:", self.totalTransferredData/(3.6-self.batCap))
return 1 #self.totalTransferredData/(3.6-self.batCap)
def findEnergyConsumption(self):
#charge= self.Itx*(0.426+15)/32768
charge = 17.4 / 32768
Etx = self.V * charge
self.batCap = self.batCap - Etx
