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 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():
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 __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
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 __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 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)
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()
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__(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 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)
"""
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)
def __init__(self):
self.lidar = HokuyoLX()
self.kalman = KalmanFilter(adaptive=False, dt=0.025)
def __init__(self, master):
"""
Canvas frame for clicking to set true postion.
Noise measurements are made from these points, and the kalman filter
estimates the position based on measurements and the underlying model.
These points are visualized on a canvas.
---
Attributes:
initalized with config.py
"""
# settings
master.title(config.master_title)
master_width = config.master_width
master_heigth = config.master_heigth
master.geometry(f"{master_width}x{master_heigth}")
self.Show_True_Values = config.show_true_values
self.Show_Measured_Values = config.show_measured_values
self.Show_Est_Values = config.show_est_values
self.sigma = config.sigma
self.mu = config.mu
self.canvas_oval_delta = config.canvas_oval_delta
stored_values = config.n_stored_values
# frames
canvas_width = config.canvas_width
canvas_height = config.canvas_height
main_frame_heigth = config.main_frame_heigth
main_frame_width = config.main_frame_width
canvas_frame = tk.Frame(master, height= canvas_height)
main_frame = tk.Frame(master, width= canvas_width, height= main_frame_heigth)
# layout
master.grid_rowconfigure(0, weight=1)
master.grid_columnconfigure(0, weight=1)
# main frames
canvas_frame.grid(row=0, column= 0, sticky= "NESW", padx= 5, pady= 5)
main_frame.grid(row=1, column= 0, sticky= "NESW", padx= 5, pady= 5)
# canvas
self.canvas_bg_color = config.canvas_bg_color
self.canvas_true_val_color = config.canvas_true_val_color
self.canvas_measurement_color = config.canvas_measurement_color
self.canvas_estimate_color = config.canvas_estimate_color
self.w = tk.Canvas(canvas_frame,
bg= self.canvas_bg_color)
self.w.pack(fill="both", expand=True)
self.w.bind("<B1-Motion>", self.update_canvas) #drag
self.w.bind("<Button-1>", self.update_canvas) #click
# display frames
padx = config.display_padx
pady = config.display_pady
display_frame = tk.Frame(main_frame, width=main_frame_width, height= main_frame_heigth, padx= padx, pady= pady)
display_frame.grid(row=0, column=0, sticky= "NW")
P_frame = tk.Frame(main_frame, width=main_frame_width, height= main_frame_heigth, padx= padx, pady= pady)
P_frame.grid(row=0, column=1, sticky= "NW")
K_frame = tk.Frame(main_frame, width=main_frame_width, height= main_frame_heigth, padx= padx, pady= pady)
K_frame.grid(row=0, column=2, sticky= "NW")
# display frame labels
self.display_var = tk.StringVar()
display_label = tk.Label(display_frame, textvariable= self.display_var)
display_label.grid(row= 0, column= 0, sticky = "NW")
self.P_var = tk.StringVar()
display_label = tk.Label(P_frame, textvariable= self.P_var)
display_label.grid(row=0, column= 0, sticky = "NW")
self.K_var = tk.StringVar()
display_label = tk.Label(K_frame, textvariable= self.K_var)
display_label.grid(row=0, column= 0, sticky = "NW")
# values
self.true_val_idx = np.ones((2, stored_values))*-1
self.meas_val_idx = np.ones((2, stored_values))*-1
self.est_val_idx = np.ones((2, stored_values))*-1
# kalman filter
# only position measurement
X0 = [int(canvas_width/2), 0, int(canvas_height/2), 0]
self.kf = KalmanFilter(X0= X0, nz= 2)
#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)
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 __init__(self, ip="192.168.1.10", port=10940):
self.lidar = HokuyoLX(addr=(ip, port))
self.kalman = KalmanFilter(adaptive=False, dt=0.025)
# [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()
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
track_fails = 0
prev_el = el
orientations = []
ellipses = []
frame_num = 0
#ellipses = pickle.load(open("ellipses.pkl", "rb"))
#centers = [el[0] for el in ellipses]
#o_centers = np.array(centers)
#for i in range(1, len(centers)-1):
# avg = (o_centers[i]-1 + o_centers[i] + o_centers[i+1]) / 3
# centers[i] = (int(avg[0]), int(avg[1]))
proc_noise_cov = 5e-2
meas_noise_cov = 5e-1
kalman_filter = KalmanFilter(proc_noise_cov, meas_noise_cov, el[0])
estimated = (0, 0)
while frame_good:
"""
center = centers.pop(0)
frame_good, image = read_scaled_img_with_border(capture, FRAME_BORDER_SIZE, FRAME_SCALE)
cv2.circle(image, center, 3, (0,0,255), -1)
cv2.imshow("Iris Tracker", image)
#cv2.waitKey(int(1000./fps))
cv2.waitKey()
"""
track_success, el = update_ellipse(el, image)
if el[1][0] * el[1][1] > 1.5 * prev_el[1][0] * prev_el[1][1] or max(
el[1]) / min(el[1]) > 3:
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/global_flow"
save_root = "results/global_flow"
# 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 = 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
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
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
advance = True
sigma_initial = 0 # standard deviation of normal distribution for random making
update_interval = 50 # update interval
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, 3, Tf))
mae_record = xp.zeros((2, 3, Tf))
time_record = xp.zeros(3)
all_start_time = time.time()
### Execute
F_initial = xp.eye(Nf * Nf) # identity
A = xp.asarray(adjacency_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
save_dir = os.path.join(save_root, "llock")
if not os.path.exists(save_dir):
os.mkdir(save_dir)
print("LLOCK : d={}, update_interval={}, eta={}, cutoff={}".format(
d, 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,
update_interval=update_interval,
eta=eta,
cutoff=cutoff,
save_dir=save_dir,
advance_mode=advance,
use_gpu=False)
start_time = time.time()
llock.forward()
time_record[0] = time.time() - start_time
time_record[1] = llock.times[3]
time_record[2] = llock.times[3] / llock.times[4]
print("LLOCK 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(
llock.get_filtered_value()[t][area], true_xp[t][area])
mae_record[r, 0, t] = mean_absolute_error(
llock.get_filtered_value()[t][area], true_xp[t][area])
mse_record[r, 1, t] = mean_squared_error(filtered_value[t][area],
true_xp[t][area])
mae_record[r, 1, t] = mean_absolute_error(filtered_value[t][area],
true_xp[t][area])
mse_record[r, 2, t] = mean_squared_error(obs[t][area],
true_xp[t][area])
mae_record[r, 2, 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(["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"]
threshold = 200
pred_state = xp.zeros((Tf, Nf * Nf))
pred_mse = mse_record[0].copy()
s = threshold // update_interval
F = np.load(os.path.join(save_dir,
"transition_matrix_{:02}.npy".format(s)))
state = np.load(os.path.join(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]
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(kf_state.reshape(-1), true_xp[t])
pred_mse[2, t] = mean_squared_error(obs[threshold], true_xp[t])
fig, ax = plt.subplots(1, 1, figsize=(8, 5))
low = threshold - 4
up = threshold + 6
lw = 2
for i, label in enumerate(["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.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")
## substantial mse
def translation_matrix4(W=10, H=10, direction=0, cyclic=False):
xp = np
F = xp.zeros((W * H, W * H))
if direction == 0: #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 == 1: #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 == 2: #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 == 3: # 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
change_interval = 250 // update_interval
directions = [0, 2, 1, 3]
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_{:02}.npy".format(t)))
Ftrue = translation_matrix4(Nf, Nf, directions[t // change_interval],
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=(8, 5))
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)
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)
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)))
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
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 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)))
#!/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(
