def e_step(self):
# initialize parameters
transition_matrix = self.stacked_var_param
observation_matrix = self.stacked_loading
transition_covariance = self.stacked_factor_res_cov
observation_covariance = self.obs_res_cov
# e_step
kf = KalmanFilter(transition_matrices=transition_matrix,
observation_matrices=observation_matrix,
observation_covariance=observation_covariance,
transition_covariance=transition_covariance)
#change input format
observations = self.observations.T
# estimate state with filtering and smoothing
smoothed_state_mean, smoothed_state_covariances, smoothing_gain = kf.smooth(
observations)
# estimated mean and covariances [n_factor,n_time_step], [n_time_step,n_factor,n_factor]
self.stacked_factor = smoothed_state_mean.T
self.smoothed_state_cov = smoothed_state_covariances
#estimated P_{t,t-1}|T
self.pairwise_cov = self.pairwise_covariance(
smoothed_state_covariances, smoothing_gain)
def __init__(self, config, video_helper): # fps: frame per second
self.num_misses = 0
self.max_misses = config.MAX_NUM_MISSING_PERMISSION
self.has_match = False
# flags: self.delete.......
self.kalman = KalmanFilter(video_helper)
def __init__(self, detection, trackID):
super(Tracks, self).__init__()
self.KF = KalmanFilter()
self.KF.predict()
self.KF.correct(np.matrix(detection).reshape(3, 1))
self.trace = deque()
self.prediction = detection.reshape(1,3)
self.trackID = trackID
self.skipped = 0
def kinematic_kf(dim, order, dt=1.):
""" Returns a KalmanFilter using newtonian kinematics for an arbitrary
number of dimensions and order. So, for example, a constant velocity
filter in 3D space would be created with
kinematic_kf(3, 1)
which will set the state `x` to be interpreted as
[x, x', y, y', z, z'].T
As another example, a 2D constant jerk is created with
kinematic_kf(2, 3)
Assumes that the measurement z is position in each dimension. If this is not
true you will have to alter the H matrix by hand.
P, Q, R are all set to the Identity matrix.
H is assigned assuming the measurement is position, one per dimension `dim`.
Parameters
----------
dim : int
number of dimensions
order : int, >= 1
order of the filter. 2 would be a const acceleration model.
dim_z : int, default 1
size of z vector *per* dimension `dim`. Normally should be 1
dt : float, default 1.0
Time step. Used to create the state transition matrix
"""
kf = KalmanFilter(dim*order, dim)
F = kinematic_state_transition(order, dt)
diag = [F] * dim
kf.F = sp.linalg.block_diag(*diag)
kf.H = np.zeros((dim, dim*order))
for i in range(dim):
kf.H[i, i * order] = 1.
return kf
def run(self, img):
"""
Parameters:
img : array(shape=(n, 2))
points in the form of (x, y)
Returns:
labels : array(shape=(n, 1))
labels of every points, returned from DBSCAN.
"""
labels = extract_patterns(img)
n = np.max(labels)
x_m = [] # measurements of x
y_m = []
for i in range(n):
cluster = img[labels == i]
x_m.append(np.average(cluster[:, 0]))
y_m.append(np.average(cluster[:, 1]))
if self.tracks == []: # initialize kalman filters
for i in range(n):
x0 = np.array([x_m[i], 0]).reshape(2, 1)
y0 = np.array([y_m[i], 0]).reshape(2, 1)
kf_x = KalmanFilter(self.F, self.H, self.Q, self.R,
P=self.P.copy(), x0=x0)
kf_y = KalmanFilter(self.F, self.H, self.Q, self.R,
P=self.P.copy(), x0=y0)
track = Track(kf_x, kf_y)
self.tracks.append(track)
else:
for track in self.tracks:
track.predict()
row_ind, col_ind = self.associate_track(x_m, y_m)
for i, j in zip(row_ind, col_ind):
self.tracks[i].correct(x_m[j], y_m[j])
self.maintain_track(row_ind, col_ind)
# add(initialize) the tracks if any observations(measurements) are
# not assigned with a tracks.
for i in set(range(len(x_m))).difference(col_ind):
x0 = np.array([x_m[i], 0]).reshape(2, 1)
y0 = np.array([y_m[i], 0]).reshape(2, 1)
kf_x = KalmanFilter(self.F, self.H, self.Q, self.R,
P=self.P.copy(), x0=x0)
kf_y = KalmanFilter(self.F, self.H, self.Q, self.R,
P=self.P.copy(), x0=y0)
track = Track(kf_x, kf_y)
self.tracks.append(track)
return labels
def __init__(self):
# each history entry is numpy array [frame_id, bbox_x1, bbox_y1, bbox_x2, bbox_y2, fvec_1...128]
self.history = np.array([])
self.num_misses = 0 # num of missed assignments
self.max_misses = const.MAX_NUM_MISSES_TRACK
self.has_match = False
self.delete_me = False # set for manual deletion
self.kalman = KalmanFilter()
color = tuple(np.random.random_integers(0, 255, size=3))
self.drawing_color = color
self.predicted_next_bb = None
self.LENGTH_ESTABLISHED = 1 # number of sequential detections before we consider it a track
self.uid = uuid.uuid4()
def __init__(self, bounds=(-10.0, 10.0, -10.0, 10.0), resolution=0.2,
prior=0.5, unknown=50.0, decay=0.00001, num_angles=1):
""" bounds: the extends of the map, in m
resolution: the size of each grid cell, in m """
assert(bounds[1] > bounds[0])
assert(bounds[3] > bounds[2])
assert(resolution > 0)
numx = int(math.ceil((bounds[1] - bounds[0])/resolution))
numy = int(math.ceil((bounds[3] - bounds[2])/resolution))
tot_prod = int(numx*numy*num_angles)
self.shape = (numy, numx, num_angles)
self.bounds = bounds
self.resolution = resolution
big_covar = unknown #1000.0
small_covar = decay #10^-5
p_occ = logodds(prior)
# C, R matrix will be changed for every measurement, these
# are just place holder values
A = (sp.eye(tot_prod, tot_prod)).tocsr()
B = (sp.eye(tot_prod, 1, k=2)).tocsr() #k-diag is ones, but outside, so this is zero matrix
C = (sp.eye(1,tot_prod,k=-1)).tocsr()
x0 = p_occ*numpy.ones((tot_prod,1))
P0 = (big_covar*sp.eye(tot_prod,tot_prod)).tocsr()
Q = (small_covar*sp.eye(tot_prod,tot_prod)).tocsr()
R = (sp.eye(tot_prod, tot_prod)).tocsr()
self.kf = KalmanFilter(A=A, B=B, C=C, x0=x0, P0=P0, Q=Q, R=R)
def Demo(data):
try:
# check if the file exists in the directory named as demo.json
if os.path.exists("demo.json"):
# if yes open the file and read the data
with open('demo.json') as file:
data = json.load(file)
else:
# store the data into a file so that we don't have to request for the data again
with open('demo.json', 'w') as file:
json.dump(data, file, indent=4)
except:
sys.exit("There was an Error loading the data")
# temp list to store the data
m = []
for i in data['Time Series (5min)'].values():
t = np.array([float(i['4. close'])], dtype='float64')
m.append(t[0])
# as the data is store in desending order we will reverse it
measurements = m[::-1]
# initialise the kalman filter
kalman = KalmanFilter(closePrice=measurements[0], dt=300)
# to store the predictions
predictions = []
# to predict the next state store it and update the filter
for value in measurements:
kalman.update(value)
predictions.append(kalman.predict()[0][0])
# calculate the r2 score
coefficient_of_dermination = r2_score(measurements, predictions)
print(f'R2 score of the filter {coefficient_of_dermination}')
plt.figure(figsize=(10, 5))
plt.plot(measurements, label='measurements', marker='*', color="red")
plt.plot(predictions, label='prediction', marker=".", color="blue")
plt.suptitle("Kalman filter predictions")
plt.title(
"Intially the filter prediction has noise so prediction can divert")
plt.legend()
plt.show()
class Instance(object):
def __init__(self, config, video_helper): # fps: frame per second
self.num_misses = 0
self.max_misses = config.MAX_NUM_MISSING_PERMISSION
self.has_match = False
# flags: self.delete.......
self.kalman = KalmanFilter(video_helper)
# self.history
def add_to_track(self, tag, bbox):
corrected_bbox = self.kalman.correct(bbox)
# self.history.append(corrected_bbox)
def get_predicted_bbox(self):
# get a prediction
return self.kalman.get_predicted_bbx()
def __init__(self, config, video_helper, frame):
#each history entry is an array with [frame_id, tag, bbx_left, bbx_right, bbx_up, bbx_down]
self.history = []
self.history_size = config.HISTORY_SIZE = 10
self.face_id = 'None' # 人脸识别
self.his_face_id = 'None' # 记录上一次预测的人脸ID
self.emotion = 'None' # 表情识别
self.his_emotion = 'None' # 记录上一次表情
self.detector = config.detector
self.num_misses = 0 # num of missed assignments
self.max_misses = config.MAX_NUM_MISSING_PERMISSION
self.has_match = False
self.delete_duplicate = False
self.delete_still = False
self.delete_singular = False
self.num_of_still = 0 # num of detector num
self.kcf = KcfFilter(video_helper,
frame) # video_helper here can provide us the fps
self.kalman = KalmanFilter(video_helper)
# this color is for bbx (color assigned to this instance itself）
color = np.random.randint(0, 255, (1, 3))[0]
self.color = [int(color[0]),
int(color[1]),
int(color[2])] # color needs to be a tuple
# this color is for central point
# because we need to fade the color
self.center_color = []
self.center_color.append([int(color[0]), int(color[1]), int(color[2])])
self.predicted_next_bbx = None
self.num_established = 1 # num of sequential detections before we consider it a track
self.COLOR_FADING_PARAM = config.COLOR_FADING_PARAM
# we still need to change it later
self.speed = 0
self.direction = 0 # degree of velocity direction with [1, 0]
self.still_history = 0
def init():
print("Initializing WiFi AP...")
ap = network.WLAN(network.AP_IF)
ap.active(True)
print("WiFi AP initialized")
print(peltier_exact.scan())
print("Setting resolutions...")
peltier_exact.set_resolution(12)
"""peltier_exact.set_resolution(12)
incubator_fast.set_resolution(11)
incubator_exact.set_resolution(12)"""
print("Resolutions set")
print("Initializing temperature sensors...")
peltier_exact.begin_reading()
"""peltier_exact.init_read()
incubator_fast.init_read()
incubator_exact.init_read()"""
print("Temperature sensors initialized")
print("Waiting for first readings...")
utime.sleep_ms(750)
print("Wait over.")
peltier_exact_z = peltier_exact.begin_reading()
print("Initializing Kalman models...")
peltier_model = KalmanFilter(peltier_exact_z, 1)
incubator_model = KalmanFilter(peltier_exact_z, 1)
print("Initializing control loop timers...")
fast_timer.init(period=380,
mode=machine.Timer.PERIODIC,
callback=fast_timer_IRQ)
exact_timer.init(period=755,
mode=machine.Timer.PERIODIC,
callback=exact_timer_IRQ)
kalman_timer.init(period=500,
mode=machine.Timer.PERIODIC,
callback=kalman_timer_IRQ)
print("Control loop timers initialized")
def main():
# keep the track the current day transactions
transactions = LoadTransactions()
# initiate kalman filter for all STOCKS and store the previous closing prices
stock_data = dict()
# For first 10 iterations we won't predict and let the kalman filter to reduce the noise
# But we will update the kalman filter
iterations = 0
for stock in STOCKS:
stock_price = array([GetLiveStockData(stock)], dtype='float64')
# initial Close price = 0
stock_data[stock] = KalmanFilter(closePrice=stock_price[0], dt=300)
while (True):
for stock in STOCKS:
# get the live price of the stock
price = array([GetLiveStockData(stock)], dtype="float64")
price = price[0]
if transactions['onhold'].get(stock) != None:
# update the kalman filter to the current state
stock_data[stock].update(price)
# predict the next state
prediction = stock_data[stock].predict()[0][0]
# the bought at price for the stock
boughtAt = transactions['onhold'][stock]["bought_at"]
if ((prediction < price) and
(boughtAt < prediction)) or ((boughtAt > prediction) and
(prediction < price)):
transactions = sell(transactions, stock, price)
WriteTransactions(transactions)
else:
# update the kalman filter to current state
stock_data[stock].update(price)
# make the predictions using the kalman filter
predicted_price = stock_data[stock].predict()[0][0]
# for first 10 iterations we will reduce the noise in kalman filterr for better predictions
if (predicted_price - price) > 0 and iterations > 10:
# sold_at -1 means it is not sold yet or we don't know the selling price
transactions = buy(transactions, stock, {
"bought_at": price,
"sold_at": -1
})
# we will wait for 5 minutes
sleep(300)
iterations += 1
print(f"Amount {Transaction.amount} at {GetDateTime()['time']}")
class Tracker(object):
num_objects = 0
def __init__(self, init_loc):
self.X = np.zeros((dim_x,1))
self.miss_frame_count = 0
self.object_id = num_objects
num_objects += 1
self.X[:4,1] = init_loc
A = 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]])
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 = KalmanFilter(dim_x=7,dim_z=4,A=A,H=H)
def predict_kalman_seq(seq, feat):
# load the model
with gzip.open(os.path.join('data', 'models', 'ETR_1_6_3_300.pklz'), 'rb') as fd:
rf = pickle.load(fd)
X = np.array(feat)
y_pred = rf.predict(X)
# for i, tree_ in enumerate(rf.estimators_):
# with open('tree_' + str(i) + '.dot', 'w') as dotfile:
# tree.export_graphviz(tree_, dotfile)
x = np.array([[0.], [0.]]) # initial state (location and velocity)
P = np.array([[1000., 0.], [0., 1000.]]) # initial uncertainty
kf = KalmanFilter()
x_out = [x]
xabs = 0
for measurement in y_pred:
xabs = xabs + measurement
x, P = kf.step(x, P, xabs)
x_out.append(x)
return x_out
def __init__(self,
bounds=(-10.0, 10.0, -10.0, 10.0),
resolution=0.2,
prior=0.5,
unknown=50.0,
decay=0.00001,
num_angles=1):
""" bounds: the extends of the map, in m
resolution: the size of each grid cell, in m """
assert (bounds[1] > bounds[0])
assert (bounds[3] > bounds[2])
assert (resolution > 0)
numx = int(math.ceil((bounds[1] - bounds[0]) / resolution))
numy = int(math.ceil((bounds[3] - bounds[2]) / resolution))
tot_prod = int(numx * numy * num_angles)
self.shape = (numy, numx, num_angles)
self.bounds = bounds
self.resolution = resolution
big_covar = unknown #1000.0
small_covar = decay #10^-5
p_occ = logodds(prior)
# C, R matrix will be changed for every measurement, these
# are just place holder values
A = (sp.eye(tot_prod, tot_prod)).tocsr()
B = (sp.eye(
tot_prod, 1,
k=2)).tocsr() #k-diag is ones, but outside, so this is zero matrix
C = (sp.eye(1, tot_prod, k=-1)).tocsr()
x0 = p_occ * numpy.ones((tot_prod, 1))
P0 = (big_covar * sp.eye(tot_prod, tot_prod)).tocsr()
Q = (small_covar * sp.eye(tot_prod, tot_prod)).tocsr()
R = (sp.eye(tot_prod, tot_prod)).tocsr()
self.kf = KalmanFilter(A=A, B=B, C=C, x0=x0, P0=P0, Q=Q, R=R)
def initialize_kalman(self,
x=0,
vx=0,
y=0,
vy=0,
z=0,
vz=0,
p=10,
q=1,
r=1):
F = lambda dt: np.matrix([
[1, dt, 0, 0, 0, 0], # x' = x + vx*dt
[0, 1, 0, 0, 0, 0], # vx' = vx
[0, 0, 1, dt, 0, 0], # y' = y + vy*dt
[0, 0, 0, 1, 0, 0], # vy' = vy
[0, 0, 0, 0, 1, dt], # z' = z + vz*dt
[0, 0, 0, 0, 0, 1], # vz' = vz
])
# Control model
B = 0
# Measurement model
H = np.matrix([
[1, 0, 0, 0, 0, 0], # x
[0, 0, 1, 0, 0, 0], # y
[0, 0, 0, 0, 1, 0], # z
])
# Initial state vector
x0 = np.matrix([x, vx, y, vy, z, vz]).T
# Initial state covariance
P0 = np.matrix(np.eye(6)) * p
# Process error covariance (continuous white noise acceleration model)
Q = lambda dt: np.matrix([
[dt**3 / 3.0, dt**2 / 2.0, 0, 0, 0, 0],
[dt**2 / 2.0, dt, 0, 0, 0, 0],
[0, 0, dt**3 / 3.0, dt**2 / 2.0, 0, 0],
[0, 0, dt**2 / 2.0, dt, 0, 0],
[0, 0, 0, 0, dt**3 / 3.0, dt**2 / 2.0],
[0, 0, 0, 0, dt**2 / 2.0, dt],
]) * q
# Measurement error covariance
R = np.matrix(np.eye(3)) * r
self.tracker = KalmanFilter(F, B, H, x0, P0, Q, R)
"""
Uses kalman filter to predict subsequent detection bounding boxes
"""
import cv2
from ball_detector import BallDetector
from kalman import KalmanFilter
detector = BallDetector()
# detector = CarDetector()
kf = KalmanFilter()
video_filename = '/home/bob/datasets/video/TrackBalls/2017-05-12-112517.webm'
record_flag = False
if record_flag:
# declare video writer obj
fourcc = cv2.VideoWriter_fourcc(*'XVID')
out1 = cv2.VideoWriter('/home/bob/datasets/video/TrackBalls/Tracker_balls/1.avi', fourcc, 20.0, (1920, 1080))
vc = cv2.VideoCapture()
vc.open(video_filename)
scale_factor = 0.75 # reduce frame by this size
tracked_bbox = [] # a list of past bboxes
max_num_missed_det = 5
num_missed_det = 0
while True:
_, img = vc.read()
if img is None:
break
img = cv2.resize(img, (int(img.shape[1]*scale_factor), int(img.shape[0]*scale_factor)))
# detect ball in frame (just one for now)
#!/usr/bin/env python
from robot import Robot
from kalman import KalmanFilter
if __name__ == '__main__':
robot = Robot()
filter = KalmanFilter(10.0, 10.0, 10.0, 10.0, 10.0, 10.0)
for i in xrange(1000):
robot.move(1.0)
filter.update(1.0, robot.sensor())
# Print the robot's actual position vs the filter estimate
print robot.x, filter.mean
class Track(object):
def __init__(self):
# each history entry is numpy array [frame_id, bbox_x1, bbox_y1, bbox_x2, bbox_y2, fvec_1...128]
self.history = np.array([])
self.num_misses = 0 # num of missed assignments
self.max_misses = const.MAX_NUM_MISSES_TRACK
self.has_match = False
self.delete_me = False # set for manual deletion
self.kalman = KalmanFilter()
color = tuple(np.random.random_integers(0, 255, size=3))
self.drawing_color = color
self.predicted_next_bb = None
self.LENGTH_ESTABLISHED = 1 # number of sequential detections before we consider it a track
self.uid = uuid.uuid4()
def get_length(self):
return self.history.shape[0]
def is_singular(self):
return True if self.history.shape[
0] == self.LENGTH_ESTABLISHED else False
def is_established(self):
return True if self.history.shape[
0] > self.LENGTH_ESTABLISHED else False
def is_empty(self):
return True if self.history.shape[0] == 0 else False
def is_dead(self):
return True if (self.num_misses >= self.max_misses
or self.delete_me is True) else False
def propagate_track(self, frame_id):
# propagate track as if there was a detection, perhaps object is temporarily occluded or not detected
# use predicted bb as measurement
det = Detection.det_from_numpy_array(
self.history[self.history.shape[0] - 1])
det.bbox = self.get_predicted_next_bb()
# TODO: adjust bbox to have same width height but only propegate x,y centroid position
# pred_c_x, pred_c_y = util.centroid_from_bb(self.get_predicted_next_bb())
# wid, ht = util.wid_ht_from_bb(det.bbox)
# det.bbox = np.array([pred_c_x - wid / 2,
# pred_c_y - ht / 2,
# pred_c_x + wid / 2,
# pred_c_y + ht / 2], dtype=np.int32)
det.frame_id = frame_id
self.add_to_track(det)
def get_predicted_next_bb(self):
# # get a prediction using the latest history as a measurement
# measurement = np.array([self.get_latest_bb()], dtype=np.float32).T
return self.kalman.get_predicted_bb()
def add_to_track(self, det):
# use detection measurement to predict and correct the kalman filter
corrected_bb = self.kalman.correct(det.bbox)
# use corrected bbox
det.bbox = corrected_bb
# increment detections number of matches (could be assigned to several tracks, need to keep track of this)
det.num_matches += 1
new_history = det.as_numpy_array()
# print new_history
if self.history.size > 0:
self.history = np.vstack((self.history, new_history))
else:
self.history = new_history
def get_latest_fvec(self):
# get feature vector from the latest detection in the track (already computed during detection phase)
return self.history[self.history.shape[0] - 1][5:]
def get_latest_bb(self):
return self.history[self.history.shape[0] - 1][1:5]
def draw_history(self, img, draw_at_bottom=False):
if self.history.shape[0] > 1:
bb_latest = self.get_latest_bb()
cv2.rectangle(img, (int(bb_latest[0]), int(bb_latest[1])),
(int(bb_latest[2]), int(bb_latest[3])),
self.drawing_color, 2)
# iterate through detection history
prev_bb = self.history[0][1:5]
for det in self.history:
bb = det[1:5]
# cv2.rectangle(img, tuple(bb[:2]),tuple(bb[2:]),(255,0,0),2)
centroid = (int(bb[0] + (bb[2] - bb[0]) / 2),
int(bb[1] + (bb[3] - bb[1]) / 2))
bottom = (int(bb[0] + (bb[2] - bb[0]) / 2), int(bb[3]))
bottom_prev = (int(prev_bb[0] + (prev_bb[2] - prev_bb[0]) / 2),
int(prev_bb[3]))
# cv2.circle(img, centroid, 5, self.drawing_color, 2)
# cv2.circle(img, bottom, 3, self.drawing_color, 2)
cv2.line(img, bottom_prev, bottom, self.drawing_color, 4)
prev_bb = bb
def __init__(self):
self.lidar = HokuyoLX()
self.kalman = KalmanFilter(adaptive=False, dt=0.025)
import utime
#import picoweb
import machine
import network
from temperature import DS18B20x
from kalman import KalmanFilter
from spark import Spark
ESSID = "Incubator-1"
#peltier_fast = DS18B20x(14, )
peltier_exact = DS18B20x(2, b'(D\x12-\t\x00\x00+')
#incubator_fast = DS18B20x(16, 11)
#incubator_exact = DS18B20x(17, 12)
peltier_model = KalmanFilter(28, 400)
incubator_model = KalmanFilter(28, 400)
spark_controller = Spark(2)
fast_timer = machine.Timer(0)
exact_timer = machine.Timer(1)
kalman_timer = machine.Timer(2)
def disable_debug():
esp.osdebug(None)
def fast_timer_IRQ(timer):
pass
def main():
## set root directory
data_root = "../data/global_flow"
save_root = "results/global_flow"
# make directory
if not os.path.exists(save_root):
os.mkdir(save_root)
## set data
Tf = 1000 # length of time-series
N = 30 # widht, height of images
dtype = "float32"
obs = xp.asarray(np.load(os.path.join(data_root, "obs.npy")), dtype=dtype)
true_xp = xp.asarray(np.load(os.path.join(data_root, "true.npy")),
dtype=dtype)
## set data for kalman filter
bg_value = 20
skip = 1 # downsampling
Nf = int(N / skip) # Number of lines after skip
obs = obs[:Tf, ::skip, ::skip].reshape(Tf, -1)
true_xp = true_xp[:Tf, ::skip, ::skip].reshape(Tf, -1) + bg_value
boundary = xp.zeros((Nf, Nf), dtype=bool)
boundary[0] = True
boundary[-1] = True
boundary[:, 0] = True
boundary[:, -1] = True
boundary = boundary.reshape(-1)
## set parameters
d = 1 # number of adjacency element
sigma_initial = 0 # standard deviation of normal distribution for random making
update_interval = 10 # update interval for LSLOCK
llock_update_interval = 50 # update interval for LLOCK
estimation_mode = "forward"
eta = 0.8 # learning rate
cutoff = 1.0 # cutoff distance for update of transition matrix
sigma = 0.2 # standard deviation of gaussian noise
Q = sigma**2 * xp.eye(Nf * Nf)
R = sigma**2 * xp.eye(Nf * Nf) # Nf x nlines
## record list
mse_record = xp.zeros((2, 4, Tf))
mae_record = xp.zeros((2, 4, Tf))
time_record = xp.zeros((2, 3))
all_start_time = time.time()
### Execute
F_initial = xp.eye(Nf * Nf) # identity
A = xp.asarray(parametric_matrix_cyclic_2d(Nf, d), dtype="int32")
## Kalman Filter
filtered_value = xp.zeros((Tf, Nf * Nf))
kf = KalmanFilter(transition_matrix=F_initial,
transition_covariance=Q,
observation_covariance=R,
initial_mean=obs[0],
dtype=dtype)
for t in range(Tf):
filtered_value[t] = kf.forward_update(t, obs[t], return_on=True)
xp.save(os.path.join(save_root, "kf_states.npy"), filtered_value)
## LLOCK
llock_save_dir = os.path.join(save_root, "llock")
if not os.path.exists(llock_save_dir):
os.mkdir(llock_save_dir)
print("LLOCK : d={}, update_interval={}, eta={}, cutoff={}".format(
d, llock_update_interval, eta, cutoff))
llock = LocalLOCK(observation=obs,
transition_matrix=F_initial,
transition_covariance=Q,
observation_covariance=R,
initial_mean=obs[0],
adjacency_matrix=A,
dtype=dtype,
estimation_length=llock_update_interval,
estimation_interval=llock_update_interval,
eta=eta,
cutoff=cutoff,
save_dir=llock_save_dir,
estimation_mode=estimation_mode,
use_gpu=False)
start_time = time.time()
llock.forward()
time_record[0, 0] = time.time() - start_time
time_record[0, 1] = llock.times[3]
time_record[0, 2] = llock.times[3] / llock.times[4]
print("LLOCK times : {}".format(time.time() - start_time))
## LSLOCK
lslock_save_dir = os.path.join(save_root, "lslock")
if not os.path.exists(lslock_save_dir):
os.mkdir(lslock_save_dir)
print("LSLOCK : d={}, update_interval={}, eta={}, cutoff={}".format(
d, update_interval, eta, cutoff))
lslock = LSLOCK(observation=obs,
transition_matrix=F_initial,
transition_covariance=Q,
observation_covariance=R,
initial_mean=obs[0],
parameter_matrix=A,
dtype=dtype,
estimation_length=update_interval,
estimation_interval=update_interval,
eta=eta,
cutoff=cutoff,
save_dir=lslock_save_dir,
estimation_mode=estimation_mode,
method="gridwise",
use_gpu=False)
start_time = time.time()
lslock.forward()
time_record[1, 0] = time.time() - start_time
time_record[1, 1] = lslock.times[3]
time_record[1, 2] = lslock.times[3] / lslock.times[4]
print("LSLOCK times : {}".format(time.time() - start_time))
# record error infromation
area_list = [xp.ones((Nf * Nf), dtype=bool), ~boundary]
for r, area in enumerate(area_list):
for t in range(Tf):
mse_record[r, 0, t] = mean_squared_error(
lslock.get_filtered_value()[t][area], true_xp[t][area])
mae_record[r, 0, t] = mean_absolute_error(
lslock.get_filtered_value()[t][area], true_xp[t][area])
mse_record[r, 1, t] = mean_squared_error(
llock.get_filtered_value()[t][area], true_xp[t][area])
mae_record[r, 1, t] = mean_absolute_error(
llock.get_filtered_value()[t][area], true_xp[t][area])
mse_record[r, 2, t] = mean_squared_error(filtered_value[t][area],
true_xp[t][area])
mae_record[r, 2, t] = mean_absolute_error(filtered_value[t][area],
true_xp[t][area])
mse_record[r, 3, t] = mean_squared_error(obs[t][area],
true_xp[t][area])
mae_record[r, 3, t] = mean_absolute_error(obs[t][area],
true_xp[t][area])
## save error-record
if True:
xp.save(os.path.join(save_root, "time_record.npy"), time_record)
xp.save(os.path.join(save_root, "mse_record.npy"), mse_record)
xp.save(os.path.join(save_root, "mae_record.npy"), mae_record)
# mse_record = np.load(os.path.join(save_root, "mse_record.npy"))
fig, ax = plt.subplots(1, 1, figsize=(8, 5))
for i, label in enumerate(["LSLOCK", "LLOCK", "KF", "observation"]):
ax.plot(np.sqrt(mse_record[0, i]), label=label, lw=2)
ax.set_xlabel("Timestep", fontsize=12)
ax.set_ylabel("RMSE", fontsize=12)
ax.legend(fontsize=15)
fig.savefig(os.path.join(save_root, "rmse.png"), bbox_to_inches="tight")
## short-term prediction
color_list = ["r", "g", "b", "m", "y"]
threshold = 200
pred_state = xp.zeros((Tf, Nf * Nf))
llock_pred_state = xp.zeros((Tf, Nf * Nf))
pred_mse = mse_record[0].copy()
s = threshold // update_interval
F = np.load(
os.path.join(lslock_save_dir, "transition_matrix_{:03}.npy".format(s)))
state = np.load(os.path.join(lslock_save_dir, "states.npy"))[threshold]
pred_state[threshold] = state.reshape(-1)
for t in range(threshold, Tf - 1):
pred_state[t + 1] = F @ pred_state[t]
s = threshold // llock_update_interval
F = np.load(
os.path.join(llock_save_dir, "transition_matrix_{:02}.npy".format(s)))
llock_state = np.load(os.path.join(llock_save_dir,
"states.npy"))[threshold]
llock_pred_state[threshold] = llock_state.reshape(-1)
for t in range(threshold, Tf - 1):
llock_pred_state[t + 1] = F @ llock_pred_state[t]
kf_state = np.load(os.path.join(save_root, "kf_states.npy"))[threshold]
for t in range(threshold, Tf):
pred_mse[0, t] = mean_squared_error(pred_state[t], true_xp[t])
pred_mse[1, t] = mean_squared_error(llock_pred_state[t], true_xp[t])
pred_mse[2, t] = mean_squared_error(kf_state.reshape(-1), true_xp[t])
pred_mse[3, t] = mean_squared_error(obs[threshold], true_xp[t])
convlstm_mse = np.load(
os.path.join(save_root, "convlstm",
"convlstm_mse.npy")) # epoch//save_epoch x 10
fig, ax = plt.subplots(1, 1, figsize=(8, 5))
low = threshold - 4
up = threshold + 6
lw = 2
ax.axvline(threshold, c="k", lw=lw, ls=":")
for i, label in enumerate(["LSLOCK", "LLOCK", "KF", "observation"]):
ax.plot(range(low, up),
np.sqrt(pred_mse[i, low:up]),
lw=lw,
ls="--",
c=color_list[i])
ax.plot(range(low, threshold + 1),
np.sqrt(mse_record[0, i, low:threshold + 1]),
label=label,
lw=lw,
c=color_list[i])
ax.plot(range(threshold, up),
np.sqrt(convlstm_mse[400 // 50, :len(range(up - threshold))]),
label="ConvLSTM",
lw=lw,
c=color_list[4])
ax.set_xlabel("Timestep", fontsize=12)
ax.set_ylabel("RMSE", fontsize=12)
ax.legend(bbox_to_anchor=(1.05, 1.0), loc="upper left", fontsize=15)
fig.savefig(os.path.join(save_root, "prediction.png"), bbox_inches="tight")
all_execute_time = int(time.time() - all_start_time)
print("all time (sec): {} sec".format(all_execute_time))
print("all time (min): {} min".format(int(all_execute_time // 60)))
print("all time (hour): {} hour + {} min".format(
int(all_execute_time // 3600), int((all_execute_time // 60) % 60)))
#from data import measurements
#from data import true_velocity
from data_test_input import measurements
from data_test_input import true_velocity
from kalman import KalmanFilter
from plot import plot_graphs
dt = 0.1
A = np.array([[1, dt], [0, 1]])
# if we measure the velocity instead of position,
# we have to modify the measurement matrix(C) from [1, 0] to [0, 1]
C = np.array([[0, 1]])
R = np.array([[1, 0], [0, 3]])
Q = np.array([10])
KF = KalmanFilter(A, C, R, Q)
filtered_positions = []
filtered_velocity = []
for m in measurements:
x = KF.filter(m, dt)
filtered_positions.append(x[0])
filtered_velocity.append(x[1])
plot_graphs(measurements, filtered_positions, true_velocity, filtered_velocity)
from data_logging.data_logger import DataLogger from detection.car_detector import CarDetector from detection.detection import Detection from kalman import KalmanFilter from multiple_object_tracker import MultipleObjectTracker # log the data to a csv file filename = 'bb_logs.csv' headers = ['time', 'id', 'x1', 'y1', 'x2', 'y2'] logger = DataLogger(filename=filename, headers=headers) tracker = MultipleObjectTracker() # detector = BallDetector() detector = CarDetector() kf = KalmanFilter() # video_filename = '/home/bob/datasets/video/TrackBalls/2017-05-12-112517.webm' video_filename = '/home/bob/datasets/video/MOVI0003.avi' save_video_filename = '/home/bob/datasets/video/MOVI0003_tracking.avi' record_flag = False vc = cv2.VideoCapture() vc.open(video_filename) scale_factor = 0.75 # reduce frame by this size tracked_bbox = [] # a list of past bboxes max_num_missed_det = 5 num_missed_det = 0 current_frame_position = 0 if record_flag:
from kalman import KalmanFilter
if __name__ == "__main__":
"""
Demo implementation of a Kalman Filter
For two hidden state variables x (position) and xv (velocity) with a constant acceleration
"""
# Initial estimates of position and velocity and (co)-variances
stateNaut = OrderedDict([("position (m)", 4000), ("velocity (m/s)", 280)])
processCovarianceNaut = np.diag([400.0, 25.0])
# Create the Kalman Filter with the initial state
# The filter implements the physics internally
KF = KalmanFilter(stateNaut, processCovarianceNaut)
# Add a list of observations to the filter
observations = np.array([[4260, 282], [4550, 285], [4860, 286],
[5110, 290]])
# And a covariance on the observations
observationCovariance = np.diag([625, 36])
for observation in observations:
KF.addObservation(observation, observationCovariance)
print(KF.currentState)
# Add observations as points
plt.plot(observations, marker="X", label="_nolegend_", linewidth=0)
def __init__(self, ip="192.168.1.10", port=10940):
self.lidar = HokuyoLX(addr=(ip, port))
self.kalman = KalmanFilter(adaptive=False, dt=0.025)
class OccupancyGrid(object):
""" Occupancy grid type map """
def __init__(self, bounds=(-10.0, 10.0, -10.0, 10.0), resolution=0.2,
prior=0.5, unknown=50.0, decay=0.00001, num_angles=1):
""" bounds: the extends of the map, in m
resolution: the size of each grid cell, in m """
assert(bounds[1] > bounds[0])
assert(bounds[3] > bounds[2])
assert(resolution > 0)
numx = int(math.ceil((bounds[1] - bounds[0])/resolution))
numy = int(math.ceil((bounds[3] - bounds[2])/resolution))
tot_prod = int(numx*numy*num_angles)
self.shape = (numy, numx, num_angles)
self.bounds = bounds
self.resolution = resolution
big_covar = unknown #1000.0
small_covar = decay #10^-5
p_occ = logodds(prior)
# C, R matrix will be changed for every measurement, these
# are just place holder values
A = (sp.eye(tot_prod, tot_prod)).tocsr()
B = (sp.eye(tot_prod, 1, k=2)).tocsr() #k-diag is ones, but outside, so this is zero matrix
C = (sp.eye(1,tot_prod,k=-1)).tocsr()
x0 = p_occ*numpy.ones((tot_prod,1))
P0 = (big_covar*sp.eye(tot_prod,tot_prod)).tocsr()
Q = (small_covar*sp.eye(tot_prod,tot_prod)).tocsr()
R = (sp.eye(tot_prod, tot_prod)).tocsr()
self.kf = KalmanFilter(A=A, B=B, C=C, x0=x0, P0=P0, Q=Q, R=R)
def __call__(self, sonar_measurement):
C = self.calc_C(sonar_measurement.indices)
# Convert probabilities to log odds format first
prob = logodds(sonar_measurement.prob)
prob = prob[:, numpy.newaxis]
n = sonar_measurement.var.shape[0]
R = sp.spdiags(sonar_measurement.var, 0, n, n).tocsr()
return self.kf.meas_update(prob, C, R)
def time_update(self):
self.kf.time_update()
def calc_C(self, indices):
nbr_y = numpy.size(indices,0)
# C = numpy.zeros((nbr_y, numpy.prod(self.shape)))
# for i in range(nbr_y):
# # Convert to linear indexing
# ind = indices[i, 1]*self.shape[0]+indices[i, 0]
# C_old[i,ind] = 1
y = range(nbr_y)
x = indices[:, 2]*self.shape[1]*self.shape[0] + indices[:, 1]*self.shape[0]+indices[:, 0]
C_data = ((1,)*nbr_y, (y, x))
C_shape = (nbr_y, numpy.prod(self.shape))
C = sp.coo_matrix(C_data, shape=C_shape)
return C.tocsr()
def get_map(self, axis=None):
# Return the mean of the estimates for each angle,
# Needs to convert to array first, matrix types seem
# to loose the single dimension during reshape
tmp = numpy.reshape(numpy.asarray(self.kf.x_new),
self.shape, order='F')
#return inv_logodds(numpy.mean(tmp,2))
if (axis != None):
return inv_logodds(tmp[:,:,axis])
return inv_logodds(numpy.max(tmp,2))
def get_variance(self, axis=None):
tmp = self.kf.P.diagonal()
tmp = numpy.reshape(tmp, self.shape, order='F')
if (axis != None):
return tmp[:,:,axis]
return numpy.mean(tmp,2)
#return numpy.min(tmp,2)
def find_visible_cells(self, cone):
""" Finds all cells centers of all grids within the cone specified
state - (x,y,dir)
prec - FOV width, summetric around dir
dist - max distance to search """
def course_crop(dim, bounds, cone):
""" Rectangular bounds on which indices need to be evaluated """
(xmin, xmax, ymin, ymax) = cone.get_bounding_rect()
xleft = math.floor((dim[1]-1)*
(xmin-bounds[0])/(bounds[1]-bounds[0]))
xright = math.ceil((dim[1]-1)*
(xmax-bounds[0])/(bounds[1]-bounds[0]))
xleft = max((xleft, 1))
xright = min((xright, dim[1]-1))
yupp = math.floor((dim[0]-1)*
(ymin-bounds[2])/(bounds[3]-bounds[2]))
ylow = math.ceil((dim[0]-1)*
(ymax-bounds[2])/(bounds[3]-bounds[2]))
yupp = max((yupp, 1))
ylow = min((ylow, dim[0]-1))
return (int(xleft), int(xright), int(yupp), int(ylow))
(xle, xri, yup, ylo) = course_crop(self.shape, self.bounds, cone)
# Evaluate each cell center for the FOV
x_ind = numpy.arange(xle, xri+1, dtype=numpy.int)
y_ind = numpy.arange(yup, ylo+1, dtype=numpy.int)
x, y = numpy.meshgrid(x_ind*self.resolution+self.bounds[0],
y_ind*self.resolution+self.bounds[2])
x_ind, y_ind = numpy.meshgrid(x_ind, y_ind)
x = x.reshape((-1, 1))
y = y.reshape((-1, 1))
x_ind = x_ind.reshape((-1, 1))
y_ind = y_ind.reshape((-1, 1))
if (x.shape[0] == 0):
return None
# cells will contains (a,r,y_ind,x_ind) for all visible cells
cells = cone(y[:, 0], x[:, 0], numpy.hstack([y_ind, x_ind]))
if (cells == None):
return None
cells.sort(order='r', axis=0)
return cells
#!/usr/bin/env python import os, sys import rosbag import matplotlib.pyplot as plt from kalman import SimpleKalmanFilter, KalmanFilter import numpy as np mainDir = os.path.split(os.path.split(sys.path[0])[0])[0] bagDir = mainDir + '/bag/' filename = sys.argv[1] bag = rosbag.Bag(bagDir + filename) tMin = np.inf dist = np.array([2.0, 2.0, 2.0, 2.0]) # initial condition kf_no_imu = KalmanFilter(X=np.concatenate((dist, np.array([0.0, 0.0, 0.0]))).T) kf = KalmanFilter(X=np.concatenate((dist, np.array([0.0, 0.0, 0.0]))).T) t_old = None x_data = [] xi_data = [] p_data = [] pi_data = [] k_data = [] ki_data = [] t_data = [] vel_data = [] dist_data = [] vel_data = [] dt_data = [] for topic, msg, t_nano in bag.read_messages(
def exercise_4():
colors = ['b', 'g', 'm', 'c', 'y']
car = target
time = np.linspace(0, car.location[0] / car.v, 900)
trajectory = np.array([car.position(t) for t in time])
# 4.2 Measurements transformation
fig = plt.figure()
for i, radar in enumerate(radars, start=1):
measurements = [radar.measure(car, t) for t in time]
trans_measures = np.array([
Radar.cartesian_measurement(m) + radar.location[:2]
for m in measurements
])
plt.plot(trans_measures[:, 0],
trans_measures[:, 1],
c=colors[(i % 5) - 1],
label='Radar %s Measurements' % i)
plt.plot(trajectory[:, 0],
trajectory[:, 1],
c='r',
label='Real trajectory')
plt.title('Trajectory and Radars Measurements')
plt.legend()
plt.xlabel('X-axis (m)')
plt.ylabel('Y-axis (m)')
plt.show()
# 4.3 Kalman Filter
time_limit = car.location[0] / car.v
kalman = KalmanFilter(radars, car, delta_t=2)
kalman.filter(time_limit)
fig = plt.figure()
plt.plot(trajectory[:, 0],
trajectory[:, 1],
c='r',
label='Target Trajectory')
plt.plot(kalman.track[:, 0], kalman.track[:, 1], c='g', label='Track')
plt.xlabel('X-axis (m)')
plt.ylabel('Y-axis (m)')
plt.title('Real Trajectory vs Track using Kalman Filter')
plt.legend(loc='upper right')
plt.xlim(-1000, 12000)
plt.ylim(-2000, 2000)
# plt.show()
kalman.d_retro(time_limit)
#
fig = plt.figure()
plt.plot(trajectory[:, 0],
trajectory[:, 1],
c='r',
label='Target Trajectory')
plt.plot(kalman.no_filtered_track[:, 0],
kalman.no_filtered_track[:, 1],
c='y',
label='Predicted Track')
plt.plot(kalman.track[:, 0],
kalman.track[:, 1],
c='g',
label='Filtered Track')
plt.plot(kalman.retro_track[:, 0],
kalman.retro_track[:, 1],
c='b',
label='Track with retrodiction')
plt.xlabel('X-axis (m)')
plt.ylabel('Y-axis (m)')
plt.title(
'Real Trajectory vs Track using Kalman Filter vs Track using retrodiction'
)
plt.legend(loc='upper right')
plt.xlim(-1000, 12000)
plt.ylim(-2000, 2000)
plt.show()
# error calculation:
err = np.sum(
np.sqrt(
np.sum((trajectory[:, 0] - kalman.retro_track[:, 0])**2 +
(trajectory[:, 1] - kalman.retro_track[:, 1])**2)))
print('Error of the track when applying retrodiction: ')
print(err)
# error calculation:
err = np.sum(
np.sqrt(
np.sum((trajectory[:, 0] - kalman.track[:, 0])**2 +
(trajectory[:, 1] - kalman.track[:, 1])**2)))
print('Error of the track without retrodiction: ')
print(err)
def test():
m = lambda x: np.array(x)
a = m([[1, 0, 0.1, 0], [0, 1, 0, 0.1], [0, 0, 1, 0], [0, 0, 0, 1]])
u = m([[0.005, 0, 1, 0], [0, 0.005, 0, 1]]).T
q = 0.1 * np.eye(4)
# q[2,2] = 0.001
# q[3,3] = 0.001
h = 1. * np.eye(4)
r = 1. * np.eye(4)
p_init = 1. * np.eye(4)
x_init = m([[0], [0], [1], [1]])
num_particles = 100
particles_init = np.random.uniform(-1, 1, size=(4, num_particles))
particles_init[0, :] = 0
particles_init[1, :] = 0
running_x = x_init
def system(running_x, actuation):
sample = np.random.multivariate_normal(np.zeros(4), q, 1).T
running_x = a.dot(running_x) + u.dot(actuation) + sample
return running_x
def proposal(running_x, actuation):
sample = np.random.multivariate_normal(np.zeros(4), q, num_particles).T
running_x = a.dot(running_x) + u.dot(actuation) + sample
return running_x
def sensor(position):
sample = np.random.multivariate_normal(np.zeros(4), r, 1).T
measure = h.dot(position) + sample
return measure
def sensor_likelihood(position, measurement, normalization=10.):
likelihoods = []
all_dist = (position - measurement.reshape(-1, 1))
weights = 1. / ((normalization * all_dist)**2).mean(0)
# for i in range(position.shape[1]):
# l = normal_density.pdf(measurement.reshape(-1), position[:, i], normalization)
# likelihoods.append(l)
return weights
f = ParticleFilter(num_particles, particles_init, sensor_likelihood,
proposal)
all_x = []
all_y = []
all_sensor_x = []
all_sensor_y = []
all_particle_x = []
all_particle_y = []
noiseless_x = []
noiseless_y = []
naive_pos = x_init
particles_x = []
particles_y = []
all_kalman_x = []
all_kalman_y = []
kalman = KalmanFilter(a, u, q, h, r)
kalman.initialize(x_init, p_init)
errors_particle = []
errors_kalman = []
for _ in range(1000):
print(_)
actuation = m([[1], [0]])
running_x = system(running_x, actuation)
measurement = sensor(running_x)
pos, _ = f.filter(actuation, measurement)
# particles_x.append(f.particles[0].copy())
# particles_y.append(f.particles[1].copy())
all_x.append(running_x[0])
all_y.append(running_x[1])
all_sensor_x.append(measurement[0])
all_sensor_y.append(measurement[1])
all_particle_x.append(pos[0])
all_particle_y.append(pos[1])
naive_pos = a.dot(naive_pos) + u.dot(m([[1], [0]]))
noiseless_x.append(naive_pos[0])
noiseless_y.append(naive_pos[1])
kalman.time_update(actuation)
pos_kalman, p = kalman.measure_update(measurement)
all_kalman_x.append(pos_kalman[0])
all_kalman_y.append(pos_kalman[1])
errors_kalman.append(((running_x - pos_kalman)**2).mean())
errors_particle.append(((running_x - pos.reshape(4, 1))**2).mean())
plt.scatter(all_x, all_y)
plt.scatter(all_particle_x, all_particle_y, alpha=0.8)
plt.scatter(all_sensor_x, all_sensor_y, alpha=0.5)
plt.scatter(all_kalman_x, all_kalman_y, alpha=0.8)
# plt.scatter(noiseless_x, noiseless_y, 0.2)
plt.scatter(m(particles_x).reshape(-1),
m(particles_y).reshape(-1),
0.5,
alpha=0.3)
plt.show()
plt.plot(errors_kalman)
plt.plot(errors_particle)
plt.show()
print(np.array(errors_kalman).mean())
print(np.array(errors_particle).mean())
return True
import matplotlib.patches as mpatches
num = 100
x_real = [0]*num
x_sensor = [0]*num
x_filter = [0]*num
x_filter2 = [0]*num
x_filter3 = [0]*num
x_filter4 = [0]*num
if __name__ == '__main__':
# Question 1 -------------------------------------------------------
robot = Robot(0.0,0.0,0.0)
filter = KalmanFilter(1.0, 1.0, 1.0, \
1.0, 1.0,\
0.0,10.0)
for i in xrange(num):
speed = 1.0 - (float(i)/float(num))
robot.move(speed)
x_sensor[i] = robot.sensor()
filter.update(speed, x_sensor[i])
x_real[i] = robot.x
x_filter[i] = filter.mean
plt.figure(1,figsize=(12, 8))
plt.title('Perfect movement and perfect sensing (var_move=0, var_sensor=0)')
plt.plot(x_real,label = 'Actual position',lw = 2)
plt.plot(x_sensor, 'r--',label = 'Observations')
plt.plot(x_filter,label = 'Filter estimate')
plt.xlabel("time")
from kalman import KalmanFilter
import math
process_noise = 0.05 # Kalman parameter {R}
measurement_noise = 3 # Kalman parameter {Q}
use_kalman_filter = 0
kf = KalmanFilter(R = 0.01, Q =3)
data = [42.1, 22.2, 34, 44, 56, 44.1, 31.5]
data = map(kf.filter, data)
print data
detector = Detector()
#Image Receiver
net_recvd_length = 0
recvd_image = b''
#Test Controller
direction = -1
cnt = 0
packed_data = b''
nb_byte_to_receive = int(sz_image / 8)
# init kalman filter
kalman = KalmanFilter()
err_in_row = 0
width_bbox = 60
height_bbox = 60
while True:
#######################
# Receive data
#######################
while net_recvd_length < sz_image:
reply = s.recv(nb_byte_to_receive)
recvd_image += reply
net_recvd_length += len(reply)
def main():
## set root directory
data_root = "../data/object_moving"
save_root = "results/object_moving"
# make directory
if not os.path.exists(save_root):
os.mkdir(save_root)
## set seed
seed = 121
xp.random.seed(seed)
print("Set seed number {}".format(seed))
## set data
Tf = 100 # length of time-series
N = 25 # widht, height of images
dtype = "float32"
obs = xp.asarray(np.load(os.path.join(data_root, "obs.npy")), dtype=dtype)
true_xp = xp.asarray(np.load(os.path.join(data_root, "true.npy")), dtype=dtype)
## set data for kalman filter
skip = 1 # downsampling
bg_value = 20 # background value
Nf = int(N/skip) # Number of lines after skip
obs = obs[:Tf, ::skip, ::skip].reshape(Tf, -1)
true_xp = true_xp[:Tf, ::skip, ::skip].reshape(Tf, -1) + bg_value
## set parameters
d = 1 # number of adjacency element
advance = True
sigma_initial = 0 # standard deviation of normal distribution for random making
update_interval = 1 # update interval
eta = 1.0 # learning rate
cutoff = 1.0 # cutoff distance for update of transition matrix
sigma = 0.2 # standard deviation of gaussian noise
Q = sigma**2 * xp.eye(Nf*Nf)
R = sigma**2 * xp.eye(Nf*Nf) # Nf x nlines
## record list
mse_record = xp.zeros((3, Tf))
mae_record = xp.zeros((3, Tf))
time_record = xp.zeros(3)
all_start_time = time.time()
### Execute
F_initial = xp.eye(Nf*Nf) # identity
A = xp.asarray(parametric_matrix_2d(Nf, Nf, d), dtype="int32")
## Kalman Filter
filtered_value = xp.zeros((Tf, Nf*Nf))
kf = KalmanFilter(transition_matrix = F_initial,
transition_covariance = Q, observation_covariance = R,
initial_mean = obs[0], dtype = dtype)
for t in range(Tf):
filtered_value[t] = kf.forward_update(t, obs[t], return_on=True)
xp.save(os.path.join(save_root, "kf_states.npy"), filtered_value)
## LLOCK
save_dir = os.path.join(save_root, "llock")
if not os.path.exists(save_dir):
os.mkdir(save_dir)
print("SLOCK : d={}, update_interval={}, eta={}, cutoff={}".format(
d, update_interval, eta, cutoff))
slock = SpatiallyUniformLOCK(observation = obs,
transition_matrix = F_initial,
transition_covariance = Q,
observation_covariance = R,
initial_mean = obs[0],
localization_matrix = A,
dtype = dtype,
update_interval = update_interval,
eta = eta,
cutoff = cutoff,
save_dir = save_dir,
advance_mode = advance,
use_gpu = False)
start_time = time.time()
slock.forward()
time_record[0] = time.time() - start_time
time_record[1] = slock.times[3]
time_record[2] = slock.times[3] / slock.times[4]
print("SLOCK times : {}".format(time.time() - start_time))
# record error infromation
for t in range(Tf):
mse_record[0,t] = mean_squared_error(
slock.get_filtered_value()[t],
true_xp[t])
mae_record[0,t] = mean_absolute_error(
slock.get_filtered_value()[t],
true_xp[t])
mse_record[1,t] = mean_squared_error(
filtered_value[t],
true_xp[t])
mae_record[1,t] = mean_absolute_error(
filtered_value[t],
true_xp[t])
mse_record[2,t] = mean_squared_error(
obs[t],
true_xp[t])
mae_record[2,t] = mean_absolute_error(
obs[t],
true_xp[t])
## save error-record
if True:
xp.save(os.path.join(save_root, "time_record.npy"), time_record)
xp.save(os.path.join(save_root, "mse_record.npy"), mse_record)
xp.save(os.path.join(save_root, "mae_record.npy"), mae_record)
# mse_record = np.load(os.path.join(save_root, "mse_record.npy"))
fig, ax = plt.subplots(1,1,figsize=(8,5))
for i, label in enumerate(["SLOCK", "KF", "observation"]):
ax.plot(np.sqrt(mse_record[i]), label=label, lw=2)
ax.set_xlabel("Timestep", fontsize=12)
ax.set_ylabel("RMSE", fontsize=12)
ax.legend(fontsize=15)
fig.savefig(os.path.join(save_root, "rmse.png"), bbox_to_inches="tight")
## substantial mse
def translation_matrix4(W=10, H=10, direction="right", cyclic=False):
F = xp.zeros((W*H, W*H))
if direction=="right":
F_block = xp.diag(xp.ones(W-1), -1)
if cyclic:
F_block[0, -1] = 1
for i in range(H):
F[i*W:(i+1)*W, i*W:(i+1)*W] = F_block
elif direction=="left":
F_block = xp.diag(xp.ones(W-1), 1)
if cyclic:
F_block[-1, 0] = 1
for i in range(H):
F[i*W:(i+1)*W, i*W:(i+1)*W] = F_block
elif direction=="up":
F_block = xp.eye(W)
for i in range(H-1):
F[i*W:(i+1)*W, (i+1)*W:(i+2)*W] = F_block
if cyclic:
F[(H-1)*W:H*W, 0:W] = F_block
elif direction=="down":
F_block = xp.eye(W)
for i in range(H-1):
F[(i+1)*W:(i+2)*W, i*W:(i+1)*W] = F_block
if cyclic:
F[0:W, (H-1)*W:H*W] = F_block
return F
def translation_matrix8(W=10, H=10, direction="right", cyclic=False):
if direction in ["right", "left", "up", "down"]:
F = translation_matrix4(W, H, direction, cyclic)
elif direction in ["up-right", "up-left", "down-right", "down-left"]:
direction1, direction2 = direction.split("-")
F = translation_matrix4(W, H, direction1, cyclic) @ translation_matrix4(W, H, direction2, cyclic)
return F
direction_count = 0
directions = ["right", "up", "left", "down", "right", "up", "up-right","left",
"down-right","up","down-left","up","down-right"]
direction_changes = [5,10,20,30,35,40,45,55,65,75,85,95,1000]
Ftrue = translation_matrix8(Nf, Nf, directions[0])
mean_error = np.zeros((2, Tf//update_interval-1))
for t in range(Tf//update_interval-1):
fvalue = np.load(os.path.join(save_dir, "transition_matrix_{:03}.npy".format(t)))
if t+1>=direction_changes[direction_count]:
direction_count += 1
Ftrue = translation_matrix8(Nf, Nf, directions[direction_count], True)
mean_error[0,t] = np.sqrt(np.power(np.absolute(fvalue - Ftrue)[A.astype(bool) & ~Ftrue.astype(bool)], 2).mean())
mean_error[1,t] = np.sqrt(np.power(np.absolute(fvalue - Ftrue)[A.astype(bool) & Ftrue.astype(bool)], 2).mean())
fig, ax = plt.subplots(1,1,figsize=(12,5))
lw = 2
for i in range(2):
ax.plot(update_interval * np.array(range(Tf//update_interval-1)), mean_error[i],
label="true={}".format(i), lw=lw)
for mc in direction_changes[:-1]:
ax.axvline(mc, ls="--", color="navy", lw=1)
ax.set_xlabel("Timestep", fontsize=15)
ax.set_ylabel("SRMSE", fontsize=15)
ax.set_yscale("log")
ax.legend(fontsize=12)
ax.tick_params(labelsize=12)
directions_for_print = ["right", " up", " left", " down", "right", " up", "up-right"," left",
" down-right"," up"," down-left"," up"," down-right"]
fig.text(0.09, 0, "Direction: ", fontsize=10)
for kk, direction, mc in zip(range(len(directions)), directions_for_print, [0] + direction_changes[:-1]):
fig.text(0.16 + mc*0.0071, 0, direction, fontsize=10)
fig.savefig(os.path.join(save_root, "srmse.png"), bbox_inches="tight")
all_execute_time = int(time.time() - all_start_time)
print("all time (sec): {} sec".format(all_execute_time))
print("all time (min): {} min".format(int(all_execute_time//60)))
print("all time (hour): {} hour + {} min".format(int(all_execute_time//3600), int((all_execute_time//60)%60)))
# [227, 255, 207, 255, 230, 255, 97, 1, 119, 253, 178, 2, 255, 255, 1, 0, 3, 0, 232, 3, 111, 4]
print("fully calibrated!")
carX = 0
carvX = 0
carY = 0
carvY = 0
caraX = 0
caraY = 0
carAngle = 0
carkvX = 0
carkX = 0
kalmanFilter = KalmanFilter(0.001, 0.5)
input("are you ready?")
WAIT = 0.01
calibrate(4)
while True:
heading, roll, pitch = bno.read_euler()
x, y, z = bno.read_linear_acceleration()
#print("\nheading: "+str(heading)+", roll: "+str(roll)+", pitch: "+str(pitch))
#print("X: "+str(x/100)+", Y: "+str(y/100)+", Z: "+str(z/100)+"")
kalmanFilter.input_latest_noisy_measurement(x)
kX = kalmanFilter.get_latest_estimated_measurement()
