Ejemplo n.º 1
0
    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)
Ejemplo n.º 2
0
    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)
Ejemplo n.º 3
0
    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
Ejemplo n.º 4
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
Ejemplo n.º 5
0
    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
Ejemplo n.º 6
0
 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()
Ejemplo n.º 7
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)
Ejemplo n.º 8
0
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()
Ejemplo n.º 9
0
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()
Ejemplo n.º 10
0
    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
Ejemplo n.º 11
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")
Ejemplo n.º 12
0
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']}")
Ejemplo n.º 13
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)
Ejemplo n.º 14
0
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
Ejemplo n.º 15
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)
Ejemplo n.º 16
0
    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)
Ejemplo n.º 17
0
"""
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)
Ejemplo n.º 18
0
#!/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

Ejemplo n.º 19
0
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
Ejemplo n.º 20
0
 def __init__(self):
     self.lidar = HokuyoLX()
     self.kalman = KalmanFilter(adaptive=False, dt=0.025)
Ejemplo n.º 21
0
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
Ejemplo n.º 22
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)))
Ejemplo n.º 23
0
#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)
Ejemplo n.º 24
0
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:
Ejemplo n.º 25
0
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)
Ejemplo n.º 26
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)
Ejemplo n.º 27
0
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(
Ejemplo n.º 29
0
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
Ejemplo n.º 31
0
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")
Ejemplo n.º 32
0
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



Ejemplo n.º 33
0
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)
Ejemplo n.º 34
0
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)))
Ejemplo n.º 35
0
# [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()