예제 #1
0
    def kalman_filter(data,
                      col_num,
                      threshold,
                      Q: float,
                      EGM: bool,
                      EGM_window_width=800,
                      verbose=True):
        '''
		----------------
		DESCRIPTION
		----------------
		Simple implementation of a Kalman filter based on:
		http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
		'''
        Measurement = effectiv_trans.gradient_filter(data=data,
                                                     col_num=col_num,
                                                     threshold=threshold,
                                                     verbose=verbose)

        P = np.diag(np.array(1.0).reshape(-1))
        F = np.matrix(1.0)
        H = F
        R = np.matrix(0.1**2)
        Q = Q
        G = np.matrix(1.0)
        Q = G * (G.T) * Q
        Z = np.matrix(Measurement[0])
        X = Z
        kf = KalmanFilter(X, P, F, Q, Z, H, R)
        X_ = [X[0, 0]]

        for i in tqdm(range(1, len(Measurement)),
                      desc="Kalman filter...",
                      ascii=False,
                      ncols=75):
            # Predict
            (X, P) = kf.predict(X, P, w=0)
            # Update
            (X, P) = kf.update(X, P, Z)

            Z = np.matrix(Measurement[i])

            X_.append(X[0, 0])

        if EGM:

            EGM = [X_[0]]

            n = EGM_window_width

            for i in tqdm(range(1,
                                len(Measurement) - n + 1),
                          desc="EGM filter...",
                          ascii=False,
                          ncols=80):

                EGM.append(np.mean(X_[i:i + n]))

            EGM.extend(X_[len(Measurement) - n + 1:])

            return EGM

        else:

            return X_
class FusionEKF:
    """
  A class that gets sensor measurements from class DataPoint 
  and predicts the next state of the system using an extended Kalman filter algorithm

  The state variables we are considering in this system are the position and velocity
  in x and y cartesian coordinates, in essence there are 4 variables we are tracking.
  
  In particular, an instance of this class gets measurements from both lidar and radar sensors
  lidar sensors measure positions in cartesian coordinates (2 values)
  radar sensors measure position and velocity in polar coordinates (3 values)

  lidar sensor are linear and radar sensors are non-linear, so we use the jacobian algorithm
  to compute the state transition matrix H unlike a simple kalman filter.
  """
    def __init__(self, d):
        self.initialized = False
        self.timestamp = 0
        self.n = d['number_of_states']
        self.P = d['initial_process_matrix']
        self.F = d['inital_state_transition_matrix']
        self.Q = d['initial_noise_matrix']
        self.radar_R = d['radar_covariance_matrix']
        self.lidar_R = d['lidar_covariance_matrix']
        self.lidar_H = d['lidar_transition_matrix']
        self.a = (d['acceleration_noise_x'], d['acceleration_noise_y'])
        self.kalmanFilter = KalmanFilter(self.n)

    def updateQ(self, dt):

        dt2 = dt * dt
        dt3 = dt * dt2
        dt4 = dt * dt3

        x, y = self.a

        r11 = dt4 * x / 4
        r13 = dt3 * x / 2
        r22 = dt4 * y / 4
        r24 = dt3 * y / 2
        r31 = dt3 * x / 2
        r33 = dt2 * x
        r42 = dt3 * y / 2
        r44 = dt2 * y

        Q = np.matrix([[r11, 0, r13, 0], [0, r22, 0, r24], [r31, 0, r33, 0],
                       [0, r42, 0, r44]])

        self.kalmanFilter.setQ(Q)

    def update(self, data):

        dt = time_difference(self.timestamp, data.get_timestamp())
        self.timestamp = data.get_timestamp()

        self.kalmanFilter.updateF(dt)
        self.updateQ(dt)
        self.kalmanFilter.predict()

        z = np.matrix(data.get_raw()).T
        x = self.kalmanFilter.getx()

        if data.get_name() == 'radar':

            px, py, vx, vy = x[0, 0], x[1, 0], x[2, 0], x[3, 0]
            rho, phi, drho = cartesian_to_polar(px, py, vx, vy)
            H = calculate_jacobian(px, py, vx, vy)
            Hx = (np.matrix([[rho, phi, drho]])).T
            R = self.radar_R

        elif data.get_name() == 'lidar':

            H = self.lidar_H
            Hx = self.lidar_H * x
            R = self.lidar_R

        self.kalmanFilter.update(z, H, Hx, R)

    def start(self, data):

        self.timestamp = data.get_timestamp()
        x = np.matrix([data.get()]).T
        self.kalmanFilter.start(x, self.P, self.F, self.Q)
        self.initialized = True

    def process(self, data):

        if self.initialized:
            self.update(data)
        else:
            self.start(data)

    def get(self):
        return self.kalmanFilter.getx()