Esempio n. 1
0
def initialize_landmark(ekf_state, tree):
    '''
    Initialize a newly observed landmark in the filter state, increasing its
    dimension by 2.

    Returns the new ekf_state.
    '''

    range = tree[0]
    angle = tree[1]

    xv = ekf_state['x'][0]
    yv = ekf_state['x'][1]
    phi = ekf_state['x'][2]

    xl = range * np.cos(slam_utils.clamp_angle(angle + phi)) + xv
    yl = range * np.sin(slam_utils.clamp_angle(angle + phi)) + yv

    ekf_state['x'] = np.append(ekf_state['x'], [xl, yl])

    m, n = ekf_state['P'].shape
    P_new = np.zeros((m + 2, n + 2))
    P_new[m, n] = 69.5
    P_new[m + 1, n + 1] = 69.5
    P_new[0:m, 0:n] = ekf_state['P']
    ekf_state['P'] = slam_utils.make_symmetric(P_new)

    ekf_state['num_landmarks'] += 1

    return ekf_state
Esempio n. 2
0
def laser_measurement_model(ekf_state, landmark_id):
    '''
    Returns the measurement model for a (range,bearing) sensor observing the
    mapped landmark with id 'landmark_id' along with its jacobian.

    Returns:
        h(x, l_id): the 2x1 predicted measurement vector [r_hat, theta_hat].

        dh/dX: For a measurement state with m mapped landmarks, i.e. a state vector of
                dimension 3 + 2*m, this should return the full 2 by 3+2m Jacobian
                matrix corresponding to a measurement of the landmark_id'th feature.
    '''

    ###
    # Implement the measurement model and its Jacobian you derived
    ###

    t_st = ekf_state['x'].copy()
    t_st[2] = slam_utils.clamp_angle(t_st[2])
    dim = t_st.shape[0]
    r_x,r_y,phi = t_st[0],t_st[1],t_st[2]
    m_x,m_y = t_st[3+2*landmark_id],t_st[4+2*landmark_id]
    del_x = m_x - r_x
    del_y = m_y - r_y
    q = (del_x)**2+(del_y)**2
    sqrt_q = np.sqrt(q)
    zhat = [[sqrt_q],[slam_utils.clamp_angle(np.arctan2(del_y,del_x)-phi)]]
    h = np.array([[-sqrt_q*del_x,-sqrt_q*del_y,0,sqrt_q*del_x,sqrt_q*del_y],[del_y,-del_x,-q,-del_y,del_x]])/q
    F_x = np.zeros((5,dim))
    F_x[:3,:3] = np.eye(3)
    F_x[3,3+2*landmark_id]=1
    F_x[4,4+2*landmark_id]=1
    H = np.matmul(h,F_x)

    return zhat, H
Esempio n. 3
0
def laser_update(trees, assoc, ekf_state, sigmas, params):
    '''
    Perform a measurement update of the EKF state given a set of tree measurements.

    trees is a list of measurements, where each measurement is a tuple (range, bearing, diameter).

    assoc is the data association for the given set of trees, i.e. trees[i] is an observation of the
    ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
    in the state for measurement i. If assoc[i] == -2, discard the measurement as 
    it is too ambiguous to use.

    The diameter component of the measurement can be discarded.

    Returns the ekf_state.
    '''

    Q = np.diag([sigmas['range']**2, sigmas['bearing']**2])

    for i, j in enumerate(assoc):
        if j == -2:
            continue
        elif j == -1:
            ekf_state = initialize_landmark(ekf_state, trees[i])
            n = ekf_state['num_landmarks']
            sigma = ekf_state['P']
            zhat, H = laser_measurement_model(ekf_state, n - 1)
            K = np.matmul(
                np.matmul(sigma, H.T),
                np.linalg.inv(np.matmul(np.matmul(H, sigma), H.T) + Q))
            z = np.zeros((2, 1))
            z[0, 0] = trees[i][0]
            z[1, 0] = trees[i][1]
            state_update = np.matmul(K, (z - zhat))
            ekf_state['x'] = np.reshape(
                (np.reshape(ekf_state['x'], (-1, 1)) + state_update), (-1, ))
            ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
            ekf_state['P'] = slam_utils.make_symmetric(
                np.matmul((np.eye(ekf_state['x'].shape[0]) - np.matmul(K, H)),
                          sigma))
        else:
            sigma = ekf_state['P']
            zhat, H = laser_measurement_model(ekf_state, j)
            K = np.matmul(
                np.matmul(sigma, H.T),
                np.linalg.inv(np.matmul(np.matmul(H, sigma), H.T) + Q))
            z = np.zeros((2, 1))
            z[0, 0] = trees[i][0]
            z[1, 0] = trees[i][1]
            state_update = np.matmul(K, (z - zhat))
            ekf_state['x'] = np.reshape(
                (np.reshape(ekf_state['x'], (-1, 1)) + state_update), (-1, ))
            ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
            ekf_state['P'] = slam_utils.make_symmetric(
                np.matmul((np.eye(ekf_state['x'].shape[0]) - np.matmul(K, H)),
                          sigma))

    return ekf_state
Esempio n. 4
0
def laser_update(trees, assoc, ekf_state, sigmas, params):
    '''
    Perform a measurement update of the EKF state given a set of tree measurements.

    trees is a list of measurements, where each measurement is a tuple (range, bearing, diameter).

    assoc is the data association for the given set of trees, i.e. trees[i] is an observation of the
    ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
    in the state for measurement i. If assoc[i] == -2, discard the measurement as 
    it is too ambiguous to use.

    The diameter component of the measurement can be discarded.

    Returns the ekf_state.
    '''

    ###
    # Implement the EKF update for a set of range, bearing measurements.
    ###

    R = np.diag([sigmas['range']**2, sigmas['bearing']**2])

    for i in range(0, len(trees)):
        if assoc[i] == -2:
            continue
        elif assoc[i] == -1:
            ekf_state = initialize_landmark(ekf_state, trees[i])
            P = ekf_state['P']
            z_hat, H = laser_measurement_model(ekf_state,
                                               ekf_state['num_landmarks'] - 1)
            r = np.array(np.array(trees[i][0:2]) - np.array(z_hat))
            S_inv = np.linalg.inv(
                np.matmul(np.matmul(H, P), np.transpose(H)) + R)
            K = np.matmul(np.matmul(P, np.transpose(H)), S_inv)
            ekf_state['x'] = ekf_state['x'] + np.matmul(K, r)
            ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
            temp1 = np.identity(P.shape[0]) - np.matmul(K, H)
            ekf_state['P'] = slam_utils.make_symmetric(np.matmul(temp1, P))
        else:
            P = ekf_state['P']
            z_hat, H = laser_measurement_model(ekf_state, assoc[i])
            r = np.array(np.array(trees[i][0:2]) - np.array(z_hat))
            S_inv = np.linalg.inv(
                np.matmul(np.matmul(H, P), np.transpose(H)) + R)
            K = np.matmul(np.matmul(P, np.transpose(H)), S_inv)
            ekf_state['x'] = ekf_state['x'] + np.matmul(K, r)
            ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
            temp2 = np.identity(P.shape[0]) - np.matmul(K, H)
            ekf_state['P'] = slam_utils.make_symmetric(np.matmul(temp2, P))

    return ekf_state
Esempio n. 5
0
def motion_model(u, dt, ekf_state, vehicle_params):
    '''
    Computes the discretized motion model for the given vehicle as well as its Jacobian

    Returns:
        f(x,u), a 3x1 vector corresponding to motion x_{t+1} - x_t given the odometry u.

        df/dX, the 3x3 Jacobian of f with respect to the vehicle state (x, y, phi)
    '''

    ###
    # Implement the vehicle model and its Jacobian you derived.
    ###
    Ve = u[0]
    alpha = slam_utils.clamp_angle(u[1])

    H = vehicle_params['H']
    L = vehicle_params['L']
    a = vehicle_params['a']
    b = vehicle_params['b']

    Vc = Ve / (1 - np.tan(alpha) * H / L)

    # phi = ekf_state['x'][2]
    phi = slam_utils.clamp_angle(ekf_state['x'][2])

    motion = np.empty([3], dtype=np.float64)

    motion[0] = dt * (Vc * np.cos(phi) - Vc / L * np.tan(alpha) *
                      (a * np.sin(phi) + b * np.cos(phi)))
    motion[1] = dt * (Vc * np.sin(phi) + Vc / L * np.tan(alpha) *
                      (a * np.cos(phi) - b * np.sin(phi)))
    motion[2] = dt * Vc / L * np.tan(alpha)

    G = np.empty([3, 3], dtype=np.float64)
    G[0, 0] = 1
    G[0, 1] = 0
    G[0, 2] = dt * (-Vc * np.sin(phi) - Vc / L * np.tan(alpha) *
                    (a * np.cos(phi) - b * np.sin(phi)))
    G[1, 0] = 0
    G[1, 1] = 1
    G[1, 2] = dt * (Vc * np.cos(phi) + Vc / L * np.tan(alpha) *
                    (-a * np.sin(phi) - b * np.cos(phi)))
    G[2, 0] = 0
    G[2, 1] = 0
    G[2, 2] = 1

    return motion, G
Esempio n. 6
0
def initialize_landmark(ekf_state, tree):
    '''turnin -c ese650 -p project2 slam.py
    Initialize a newly observed landmark in the filter state, increasing its
    dimension by 2.

    Returns the new ekf_state.
    '''

    ###
    # Implement this function.
    ###
    current_x = ekf_state['x'][0]
    current_y = ekf_state['x'][1]
    current_th = ekf_state['x'][2]

    measurements_r = tree[0]
    measurements_th = tree[1]

    ekf_state['x'] = np.hstack(
        (ekf_state['x'],
         current_x + measurements_r * np.cos(measurements_th + current_th),
         current_y + measurements_r * np.sin(measurements_th + current_th)))
    ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])

    ekf_state['num_landmarks'] = ekf_state['num_landmarks'] + 1

    newP = np.zeros((2 * ekf_state['num_landmarks'] + 3,
                     2 * ekf_state['num_landmarks'] + 3))
    newP[-1, -1], newP[-2, -2] = 1000, 1000
    newP[:-2, :-2] = ekf_state['P']
    ekf_state['P'] = newP

    return ekf_state
Esempio n. 7
0
def initialize_landmark(ekf_state, tree):
    '''
    Initialize a newly observed landmark in the filter state, increasing its
    dimension by 2.

    Returns the new ekf_state.
    '''

    ###
    # Implement this function.
    ###
    phi = ekf_state["x"][2]
    mu = ekf_state["x"][0:2]

    landmarks = np.reshape(mu, (2, 1)) + np.vstack(
        (tree[0] * np.cos(phi + tree[1]), tree[0] * np.sin(phi + tree[1])))
    ekf_state['x'] = np.append(ekf_state['x'], landmarks[0, 0])
    ekf_state['x'] = np.append(ekf_state['x'], landmarks[1, 0])
    P = ekf_state['P']
    # P = np.vstack((P,np.zeros((2,P.shape[0]))))
    # P = np.hstack((P, np.zeros((P.shape[0],2))))
    P_new = np.eye(P.shape[0] + 2) * 1000
    # P[-1,-1], P[-2,-2] = 1000,1000
    P_new[:-2, :-2] = P
    ekf_state['P'] = slam_utils.make_symmetric(P_new)
    ekf_state['num_landmarks'] += 1
    ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
    return ekf_state
def odom_predict(u, dt, ekf_state, vehicle_params, sigmas):
    '''
    Perform the propagation step of the EKF filter given an odometry measurement u 
    and time step dt where u = (ve, alpha) as shown in the vehicle/motion model.

    Returns the new ekf_state.
    '''

    ###
    # Implement the propagation
    ###

    motion, G = motion_model(u, dt, ekf_state, vehicle_params)
    R = np.diag([
        sigmas['xy'] * sigmas['xy'], sigmas['xy'] * sigmas['xy'],
        sigmas['phi'] * sigmas['phi']
    ])

    ekf_state['x'][0:3] = ekf_state['x'][0:3] + motion
    ekf_state['P'][:3, :3] = np.matmul(np.matmul(G, ekf_state['P'][:3, :3]),
                                       G.T) + R

    ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])

    ekf_state['P'] = slam_utils.make_symmetric(ekf_state['P'])

    return ekf_state
Esempio n. 9
0
def gps_update(gps, ekf_state, sigmas):
    '''
    Perform a measurement update of the EKF state given a GPS measurement (x,y).

    Returns the updated ekf_state.
    '''

    ###
    # Implement the GPS update.
    ###

    r = np.array(gps - ekf_state['x'][0:2])

    P = ekf_state['P']
    R = np.diag([sigmas['gps']**2, sigmas['gps']**2])
    H = np.zeros((2, ekf_state['x'].size))
    H[0, 0] = 1
    H[1, 1] = 1

    S_inv = np.linalg.inv(P[0:2, 0:2] + R)

    MD = np.matmul(np.matmul(np.transpose(r), S_inv), r)

    if MD < chi2.ppf(0.999, df=2):
        K = np.matmul(np.matmul(P, np.transpose(H)), S_inv)
        ekf_state['x'] = ekf_state['x'] + np.matmul(K, r)
        ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
        temp1 = np.identity(ekf_state['x'].size) - np.matmul(K, H)
        ekf_state['P'] = slam_utils.make_symmetric(np.matmul(temp1, P))
    # else:
    #     print("no gps")

    return ekf_state
Esempio n. 10
0
def motion_model(u, dt, ekf_state, vehicle_params):
    '''
    Computes the discretized motion model for the given vehicle as well as its Jacobian

    Returns:
        f(x,u), a 3x1 vector corresponding to motion x_{t+1} - x_t given the odometry u.

        df/dX, the 3x3 Jacobian of f with respect to the vehicle state (x, y, phi)
    '''

    v_e = u[0].copy()
    alpha = u[1].copy()
    H = vehicle_params['H']
    L = vehicle_params['L']
    a = vehicle_params['a']
    b = vehicle_params['b']
    v_c = (v_e)/(1-np.tan(alpha)*(H/L))
    t_st = ekf_state['x'].copy()
    phi = t_st[2]
    el1 = dt*(v_c*np.cos(phi)-(v_c/L)*np.tan(alpha)*(a*np.sin(phi)+b*np.cos(phi)))
    el2 = dt*(v_c*np.sin(phi)+(v_c/L)*np.tan(alpha)*(a*np.cos(phi)-b*np.sin(phi)))
    el3 = dt*(v_c/L)*np.tan(alpha)
    el31 = slam_utils.clamp_angle(el3)
    motion = np.array([[el1],[el2],[el31]])
    el13 = -dt*v_c*(np.sin(phi)+(1/L)*np.tan(alpha)*(a*np.cos(phi)-b*np.sin(phi)))
    el23 = dt*v_c*(np.cos(phi)-(1/L)*np.tan(alpha)*(a*np.sin(phi)+b*np.cos(phi)))
    G = np.array([[1,0,el13],[0,1,el23],[0,0,1]])

    return motion, G
Esempio n. 11
0
def odom_predict(u, dt, ekf_state, vehicle_params, sigmas):
    '''
    Perform the propagation step of the EKF filter given an odometry measurement u 
    and time step dt where u = (ve, alpha) as shown in the vehicle/motion model.

    Returns the new ekf_state.
    '''

    ###
    # Implement the propagation
    ###
    motion, G = motion_model(u, dt, ekf_state, vehicle_params)

    P = ekf_state['P']
    Q = np.diag([sigmas['xy']**2, sigmas['xy']**2, sigmas['phi']**2])
    upd_state = ekf_state['x'][0:3] + motion
    upd_covariance = np.matmul(np.matmul(G, P[0:3, 0:3]), np.transpose(G)) + Q

    ekf_state['x'][0:3] = upd_state
    ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])

    ekf_state['P'][0:3, 0:3] = upd_covariance
    ekf_state['P'] = slam_utils.make_symmetric(ekf_state['P'])

    return ekf_state
Esempio n. 12
0
def gps_update(gps, ekf_state, sigmas):
    '''
    Perform a measurement update of the EKF state given a GPS measurement (x,y).

    Returns the updated ekf_state.
    '''

    ##
    #Implement the GPS update.
    ##
    H = np.zeros((2, 3 + 2 * ekf_state['num_landmarks']))
    H[0, 0] = 1
    H[1, 1] = 1
    Q = np.eye(2, 2) * (sigmas['gps']**2)
    Sigma = ekf_state['P']
    r = gps - ekf_state['x'][:2]
    Sinv = np.linalg.inv(np.matmul(np.matmul(H, Sigma), H.T) + Q.T)
    d = np.matmul(np.matmul(r.T, Sinv), r)
    if d > chi2.ppf(0.999, df=2):
        return ekf_state
    K = np.matmul(np.matmul(Sigma, H.T), Sinv)
    ekf_state['x'] = ekf_state['x'] + np.matmul(K, r)
    ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
    Sigma = np.matmul(
        (np.eye(3 + 2 * ekf_state['num_landmarks'],
                3 + 2 * ekf_state['num_landmarks']) - np.matmul(K, H)), Sigma)
    ekf_state['P'] = slam_utils.make_symmetric(Sigma)
    # print(ekf_state)
    return ekf_state
Esempio n. 13
0
def initialize_landmark(ekf_state, tree):
    '''
    Initialize a newly observed landmark in the filter state, increasing its
    dimension by 2.

    Returns the new ekf_state.
    '''

    ###
    # Implement this function.
    ###
    xv, yv, phi = ekf_state['x'][0:3]
    P = ekf_state['P']
    ranges, bearings, dia = tree
    bearings = slam_utils.clamp_angle(bearings)

    xl = xv + ranges * np.cos(bearings + phi)
    yl = yv + ranges * np.sin(bearings + phi)

    upd_state = np.array(np.append(ekf_state['x'], np.array([xl, yl])))
    upd_covariance = np.zeros((upd_state.size, upd_state.size))
    upd_covariance[0:P.shape[0], 0:P.shape[1]] = P
    upd_covariance[upd_state.size - 1, upd_state.size - 1] = 100
    upd_covariance[upd_state.size - 2, upd_state.size - 2] = 100

    ekf_state['num_landmarks'] += 1
    ekf_state['x'] = upd_state
    ekf_state['P'] = slam_utils.make_symmetric(upd_covariance)

    return ekf_state
Esempio n. 14
0
def odom_predict(u, dt, ekf_state, vehicle_params, sigmas):
    '''
    Perform the propagation step of the EKF filter given an odometry measurement u
    and time step dt where u = (ve, alpha) as shown in the vehicle/motion model.

    Returns the new ekf_state.
    '''

    ###
    # Implement the propagation
    ###
    motion, G = motion_model(u, dt, ekf_state, vehicle_params)
    # motion += np.vstack([ekf_state['x'][0], ekf_state['x'][1], ekf_state['x'][2]])
    # G = G + np.eye(3)

    P = ekf_state['P'][:3, :3]
    sigma_xy = sigmas['xy']**2
    sigma_phi = sigmas['phi']**2

    Q = sigma_xy * np.eye(3)
    Q[2, 2] = sigma_phi

    ekf_state['x'][:3] = motion.reshape(3, )
    ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
    ekf_state['P'][:3, :3] = np.dot(np.dot(G, P), G.T) + Q
    ekf_state['P'] = fix_P(ekf_state['P'])
    return ekf_state
Esempio n. 15
0
def laser_measurement_model(ekf_state, landmark_id):
    '''
    Returns the measurement model for a (range,bearing) sensor observing the
    mapped landmark with id 'landmark_id' along with its jacobian.

    Returns:
        h(x, l_id): the 2x1 predicted measurement vector [r_hat, theta_hat].

        dh/dX: For a measurement state with m mapped landmarks, i.e. a state vector of
                dimension 3 + 2*m, this should return the full 2 by 3+2m Jacobian
                matrix corresponding to a measurement of the landmark_id'th feature.
    '''

    x, y, p = ekf_state['x'][:3]
    x_t, y_t = ekf_state['x'][3 + landmark_id * 2:3 + (landmark_id + 1) * 2]
    H = np.zeros((2, ekf_state['x'].shape[0]))
    zhat = np.array(
        [np.sqrt((x_t - x) ** 2 + (y_t - y) ** 2), slam_utils.clamp_angle(np.arctan2(y_t - y, x_t - x) - p)])
    H[:, :3] = np.array([[(x - x_t) / zhat[0], (y - y_t) / zhat[0], 0],
                         [-(y - y_t) / zhat[0] ** 2, (x - x_t) / zhat[0] ** 2, -1]])
    # middle_H = np.array([[-(2 * x - 2 * x_t) / (2 * ((x - x_t) ** 2 + (y - y_t) ** 2) ** (1 / 2)),
    #                       -(2 * y - 2 * y_t) / (2 * ((x - x_t) ** 2 + (y - y_t) ** 2) ** (1 / 2))]
    #                         , [(y - y_t) / ((x - x_t) ** 2 + (y - y_t) ** 2),
    #                            -(x - x_t) / ((x - x_t) ** 2 + (y - y_t) ** 2)]])
    H[:, 3 + landmark_id * 2:3 + (landmark_id + 1) * 2] = -H[:2, :2]
    return zhat, H
Esempio n. 16
0
def odom_predict(u, dt, ekf_state, vehicle_params, sigmas):
    '''
    Perform the propagation step of the EKF filter given an odometry measurement u 
    and time step dt where u = (ve, alpha) as shown in the vehicle/motion model.

    Returns the new ekf_state.
    '''
    # R - process noise
    R = np.zeros([3, 3])
    R[0, 0] = sigmas['xy'] * sigmas['xy']
    R[1, 1] = sigmas['xy'] * sigmas['xy']
    R[2, 2] = sigmas['phi'] * sigmas['phi']
    f, G = motion_model(u, dt, ekf_state, vehicle_params)

    F = np.zeros((3, ekf_state['x'].size))
    F[0:3, 0:3] = np.eye(3)

    G = G - np.eye(3)
    G = np.eye(ekf_state['x'].size) + np.matmul(np.matmul(F.T, G), (F))
    ekf_state['x'] = ekf_state['x'] + np.matmul(F.T, f)
    ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
    ekf_state['P'] = np.matmul(np.matmul(G, ekf_state['P']), G.T) + np.matmul(
        np.matmul(F.T, R), F)
    ekf_state['P'] = slam_utils.make_symmetric(ekf_state['P'])

    return ekf_state
Esempio n. 17
0
def gps_update(gps, ekf_state, sigmas):
    '''
    Performs a measurement update of the EKF state given a GPS measurement (x,y).

    Returns the updated ekf_state.
    '''

    ###
    x = ekf_state["x"].copy()
    P = ekf_state["P"].copy()

    H = np.zeros([2, len(x)])
    H[0, 0] = 1
    H[1, 1] = 1

    R_t = (sigmas['gps']**2) * np.eye(2)
    S = P[0:2, 0:2] + R_t

    update = gps.reshape(2, 1) - x[0:2].reshape(2, 1)

    inv_S = slam_utils.invert_2x2_matrix(S)
    d = np.dot(np.dot(np.transpose(update), inv_S), update)

    if d < 13.8:
        K = np.dot(P[:, 0:2], inv_S)

        ekf_state['x'] = x + np.dot(K, update).flatten()
        ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])

        ekf_state['P'] = slam_utils.make_symmetric(
            np.dot((np.eye(len(x)) - np.dot(K, H)), P))

    return ekf_state
Esempio n. 18
0
def gps_update(gps, ekf_state, sigmas):
    '''
    Perform a measurement update of the EKF state given a GPS measurement (x,y).

    Returns the updated ekf_state.
    '''

    ###
    # Implement the GPS update.
    ###

    P = ekf_state['P']
    residual = np.transpose([gps - ekf_state['x'][:2]])

    H_mat = np.matrix(np.zeros([2, P.shape[0]]))
    H_mat[0, 0], H_mat[1, 1] = 1, 1
    R_mat = np.diag([sigmas['gps']**2, sigmas['gps']**2])

    S_mat = H_mat * P * H_mat.T + R_mat
    d = (np.matrix(residual)).T * np.matrix(
        slam_utils.invert_2x2_matrix(np.array(S_mat))) * np.matrix(residual)

    if d <= chi2.ppf(0.999, 2):
        Kt = P * H_mat.T * np.matrix(
            slam_utils.invert_2x2_matrix(np.array(S_mat)))
        ekf_state['x'] = ekf_state['x'] + np.squeeze(
            np.array(Kt * np.matrix(residual)))
        ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
        ekf_state['P'] = slam_utils.make_symmetric(
            np.array((np.matrix(np.eye(P.shape[0])) - Kt * H_mat) * P))

    return ekf_state
Esempio n. 19
0
def gps_update(gps, ekf_state, sigmas):
    '''
    Perform a measurement update of the EKF state given a GPS measurement (x,y).

    Returns the updated ekf_state.
    '''

    ###
    # Implement the GPS update.
    ###
    P = ekf_state['P']
    dim  = P.shape[0]-2
    H = np.hstack((np.eye(2),np.zeros((2,dim))))
    r = np.transpose([gps - ekf_state['x'][:2]])
    Q = (sigmas['gps']**2)*(np.eye(2))
    S = np.matmul(np.matmul(H,P),H.T) + Q
    S_inv = slam_utils.invert_2x2_matrix(S)
    d = np.matmul(np.matmul(r.T,S_inv),r)
    if d <= chi2.ppf(0.999, 2):
        K = np.matmul(np.matmul(P,H.T),S_inv)
        ekf_state['x'] = ekf_state['x'] + np.squeeze(np.matmul(K,r))
        ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
        P_temp = np.matmul((np.eye(P.shape[0])- np.matmul(K,H)),P)
        ekf_state['P'] = slam_utils.make_symmetric(P_temp)

    return ekf_state
Esempio n. 20
0
def laser_update(trees, assoc, ekf_state, sigmas, params):
    '''
    Perform a measurement update of the EKF state given a set of tree measurements.

    trees is a list of measurements, where each measurement is a tuple (range, bearing, diameter).

    assoc is the data association for the given set of trees, i.e. trees[i] is an observation of the
    ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
    in the state for measurement i. If assoc[i] == -2, discard the measurement as 
    it is too ambiguous to use.

    The diameter component of the measurement can be discarded.

    Returns the ekf_state.
    '''

    ###
    # Implement the EKF update for a set of range, bearing measurements.
    ###

    Qt = np.diag([sigmas['range']**2, sigmas['bearing']**2])
    z = np.zeros((2, 1))

    for i in range(len(trees)):

        j = assoc[i]

        if j == -1:
            ekf_state = initialize_landmark(ekf_state, trees[i])
            j = np.int(len(ekf_state['x']) / 2) - 2

        elif j == -2:
            continue

        elif j < -2 or j >= ekf_state['num_landmarks']:
            raise ValueError('Problem in Data Association')

        zhat, H = laser_measurement_model(ekf_state, j)

        # Kalman Gain Initialization
        PHt = np.matmul(ekf_state['P'], np.transpose(H))
        HPHt_R_inv = np.linalg.inv(np.matmul(H, PHt) + Qt)
        Kt = np.matmul(PHt, HPHt_R_inv)

        # Lidar Landmark j measurement
        z[0] = trees[i][0]
        z[1] = trees[i][1]

        # Mean Update
        change_ut = np.matmul(Kt, z - zhat)
        ekf_state['x'] = ekf_state['x'] + np.squeeze(change_ut)
        ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])

        # Covariance Update
        ekf_state['P'] = np.matmul(
            (np.eye(2 * ekf_state['num_landmarks'] + 3) - np.matmul(Kt, H)),
            ekf_state['P'])

    return ekf_state
Esempio n. 21
0
def laser_update(trees, assoc, ekf_state, sigmas, params):
    '''
    Performs a measurement update of the EKF state given a set of tree measurements.

    trees is a list of measurements, where each measurement is a tuple (range, bearing, diameter).

    assoc is the data association for the given set of trees, i.e. trees[i] is an observation of the
    ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
    in the state for measurement i. If assoc[i] == -2, discard the measurement as 
    it is too ambiguous to use.

    The diameter component of the measurement can be discarded.

    Returns the ekf_state.
    '''

    ###
    assoc = np.asarray(assoc)
    trees = np.asarray(trees)
    n = ekf_state['num_landmarks']
    if assoc.shape[0] != 0:
        new_tree_index = assoc == -1
        new_trees = trees[new_tree_index, :]
        for i in range(new_trees.shape[0]):
            initialize_landmark(ekf_state, new_trees[i, :])

        assoc[new_tree_index] = np.linspace(n, n + new_trees.shape[0] - 1,
                                            new_trees.shape[0])
        Q = np.diag([sigmas['range']**2, sigmas['bearing']**2])
        exisiting_trees_index = assoc > -1

        exisiting_trees = trees[exisiting_trees_index, :]
        landmark_ids = assoc[exisiting_trees_index]

        for i in range(exisiting_trees.shape[0]):
            measure = exisiting_trees[i, 0:2].reshape(2, 1)

            zhat, H = laser_measurement_model(ekf_state, int(landmark_ids[i]))
            P = ekf_state['P'].copy()
            K = np.dot(
                np.dot(P, H.transpose()),
                slam_utils.invert_2x2_matrix(
                    np.dot(np.dot(H, P), H.transpose()) + Q))

            ekf_state['x'] = ekf_state['x'] + np.dot(K, (measure - zhat))[:, 0]
            ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])

            ekf_state['P'] = slam_utils.make_symmetric(
                np.dot((np.eye(P.shape[0]) - np.dot(K, H)), P))

    ###

    return ekf_state
Esempio n. 22
0
def laser_update(trees, assoc, ekf_state, sigmas, params):
    """
    The diameter component of the measurement can be discarded
    :param trees: a list of measurements, where each measurement is a tuple (range, bearing, diameter)
    :param assoc: the data association for the given set of trees, i.e. trees[i] is an observation of the
    ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
    in the state for measurement i. If assoc[i] == -2, discard the measurement as it is too ambiguous to use
    :param ekf_state:
    :param sigmas:
    :param params:
    :return:
    Returns the ekf_state
    """

    # measurement noise
    range_noise = sigmas['range']
    bearing_noise = sigmas['bearing']

    Q = np.diag((range_noise**2, bearing_noise**2))

    for i in range(len(trees)):
        # add state with new landmark
        if assoc[i] == -1:
            ekf_state = initialize_landmark(ekf_state, trees[i])
        # discard ambiguous measurement
        elif assoc[i] == -2:
            continue
        # update state with existing landmark
        else:
            landmark_id = assoc[i]
            z_t, H = laser_measurement_model(ekf_state, landmark_id)
            x = ekf_state['x']
            P = ekf_state['P']
            N = len(ekf_state['x'])

            # Kalman Gain update
            K_t = np.dot(P, H.T)
            K_t = np.dot(
                K_t,
                slam_utils.invert_2x2_matrix(np.dot(np.dot(H, P), H.T) + Q))

            z = np.asarray([trees[i][0], trees[i][1]]).reshape((2, 1))
            ekf_state['x'] = x + np.dot(K_t, (z - z_t)).reshape(-1)
            ekf_state['P'] = np.dot(np.identity(N) - np.dot(K_t, H), P)

        ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
        ekf_state['P'] = slam_utils.make_symmetric(ekf_state['P'] + 10e-6)

    return ekf_state
def laser_update(trees, assoc, ekf_state, sigmas, params):
    '''
    Perform a measurement update of the EKF state given a set of tree measurements.

    trees is a list of measurements, where each measurement is a tuple (range, bearing, diameter).

    assoc is the data association for the given set of trees, i.e. trees[i] is an observation of the
    ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
    in the state for measurement i. If assoc[i] == -2, discard the measurement as 
    it is too ambiguous to use.

    The diameter component of the measurement can be discarded.

    Returns the ekf_state.
    '''

    ###
    # Implement the EKF update for a set of range, bearing measurements.
    ###

    Q = np.eye(2, 2)
    Q[0][0] = sigmas['range']**2
    Q[1][1] = sigmas['bearing']**2

    for i in range(len(assoc)):
        j = assoc[i]
        if (j == -1):
            ekf_state = initialize_landmark(ekf_state, trees[i])
            j = ekf_state['num_landmarks'] - 1

        if (j != -2):

            z_hat, H = laser_measurement_model(ekf_state, j)

            K = np.matmul(
                np.matmul(ekf_state['P'], H.T),
                np.linalg.inv(
                    (np.matmul(np.matmul(H, ekf_state['P']), H.T) + Q.T)))
            z = trees[i][:2]
            ekf_state['x'] = ekf_state['x'] + np.matmul(
                K, (np.asarray(z) - z_hat))

            ekf_state['P'] = np.matmul(
                (np.eye(np.shape(ekf_state['P'])[0]) - np.matmul(K, H)),
                ekf_state['P'])

            ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
            ekf_state['P'] = slam_utils.make_symmetric(ekf_state['P'])
    return ekf_state
Esempio n. 24
0
def odom_predict(u, dt, ekf_state, vehicle_params, sigmas):
    '''
    Perform the propagation step of the EKF filter given an odometry measurement u
    and time step dt where u = (ve, alpha) as shown in the vehicle/motion model.

    Returns the new ekf_state.
    '''

    motion, G = motion_model(u, dt, ekf_state, vehicle_params)
    ekf_state['x'][:3] = motion
    ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
    ekf_state['P'][:3, :3] = np.matmul(np.matmul(G, ekf_state['P'][:3, :3]), G.T) + \
                             np.diag([sigmas['xy'] ** 2, sigmas['xy'] ** 2, sigmas['phi'] ** 2])  # + \
    # 10**-6*np.eye(3)
    ekf_state['P'] = fix_cov(ekf_state['P'])
    return ekf_state
Esempio n. 25
0
def laser_update(trees, assoc, ekf_state, sigmas, params):
    '''
    Perform a measurement update of the EKF state given a set of tree measurements.

    trees is a list of measurements, where each measurement is a tuple (range, bearing, diameter).

    assoc is the data association for the given set of trees, i.e. trees[i] is an observation of the
    ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
    in the state for measurement i. If assoc[i] == -2, discard the measurement as
    it is too ambiguous to use.

    The diameter component of the measurement can be discarded.

    Returns the ekf_state.
    '''

    ###
    # Implement the EKF update for a set of range, bearing measurements.
    ###
    Q_t = np.array([[sigmas['range']**2, 0], [0, sigmas['bearing']**2]])

    for i in range(len(trees)):
        j = assoc[i]
        if j == -1:
            ekf_state = initialize_landmark(ekf_state, trees[i])
            j = np.int(len(ekf_state['x'])/2) - 2
        elif j == -2:
            continue
        dim = ekf_state['x'].shape[0]
        z_hat, H = laser_measurement_model(ekf_state, j)

        S = np.matmul(H,np.matmul(ekf_state['P'],H.T)) + Q_t
        S_inv = np.linalg.inv(S)
        K = np.matmul(np.matmul(ekf_state['P'],H.T), S_inv)
        z = np.zeros((2, 1))
        z[0,0] = trees[i][0]
        z[1,0] = trees[i][1]

        inno = z - z_hat
        temp_st = ekf_state['x'] + np.squeeze(np.matmul(K, inno))
        temp_st[2] = slam_utils.clamp_angle(temp_st[2])
        ekf_state['x'] = temp_st
        temp_p = np.matmul((np.eye(dim) - np.matmul(K, H)), ekf_state['P'])
        temp_p = slam_utils.make_symmetric(temp_p)
        ekf_state['P'] = temp_p

    return ekf_state
Esempio n. 26
0
def laser_update(trees, assoc, ekf_state, sigmas, params):
    '''
    # Perform a measurement update of the EKF state given a set of tree measurements.

    # trees is a list of measurements, where each measurement is a tuple (range, bearing, diameter).

    # assoc is the data association for the given set of trees, i.e. trees[i] is an observation of the
    # ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
    # in the state for measurement i. If assoc[i] == -2, discard the measurement as 
    # it is too ambiguous to use.

    The diameter component of the measurement can be discarded.

    Returns the ekf_state.
    '''
    assoc = np.array(assoc)
    measurements = np.array(trees)[:, 0:2]

    good_assoc = assoc[assoc > -1]
    good_measurements = measurements[assoc > -1]
    Q = np.diag(
        np.array([
            sigmas['range'] * sigmas['range'],
            sigmas['bearing'] * sigmas['bearing']
        ]))
    Kg = np.zeros([ekf_state['x'].shape[0], 2 * good_assoc.size])
    residuals = np.zeros((2 * good_measurements.shape[0]))
    H = np.zeros([2 * good_assoc.size, ekf_state['x'].shape[0]])
    for i in range(good_assoc.size):
        zhat, H[i:i + 2, :] = laser_measurement_model(ekf_state, good_assoc[i])
        Kg[:, i:i+2] = np.matmul(ekf_state['P'] , np.matmul(H[i:i+2,:].T, \
            np.linalg.inv(np.matmul(H[i:i+2,:], np.matmul(ekf_state['P'], H[i:i+2,:].T))+ Q)))
        residuals[i:i + 2] = good_measurements[i] - zhat

    ekf_state['x'] = ekf_state['x'] + np.matmul(Kg, residuals)
    ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
    ekf_state['P'] = np.matmul(
        (np.eye(ekf_state['P'].shape[0]) - np.matmul(Kg, H)), ekf_state['P'])
    ekf_state['P'] = slam_utils.make_symmetric(ekf_state['P'])
    ekf_state['P'] = make_positive_definite(ekf_state['P'])

    new_measurements = measurements[assoc == -1]
    for i in range(new_measurements.shape[0]):
        ekf_state = initialize_landmark(ekf_state, new_measurements[i])

    return ekf_state
def motion_model(u, dt, ekf_state, vehicle_params):
    '''
    Computes the discretized motion model for the given vehicle as well as its Jacobian

    Returns:
        f(x,u), a 3x1 vector corresponding to motion x_{t+1} - x_t given the odometry u.

        df/dX, the 3x3 Jacobian of f with respect to the vehicle state (x, y, phi)
    '''

    ###
    # Implement the vehicle model and its Jacobian you derived.
    ###
    a = vehicle_params['a']
    b = vehicle_params['b']
    H = vehicle_params['H']
    L = vehicle_params['L']
    ve = u[0]
    alpha = u[1]
    vc = ve / (1 - np.tan(alpha) * H / L)
    motion = ekf_state['x']

    xv = ekf_state['x'][0] + dt * (
        vc * np.cos(ekf_state['x'][2]) - vc / L * np.tan(alpha) *
        (a * np.sin(ekf_state['x'][2]) + b * np.cos(ekf_state['x'][2])))
    yv = ekf_state['x'][1] + dt * (
        vc * np.sin(ekf_state['x'][2]) + vc / L * np.tan(alpha) *
        (a * np.cos(ekf_state['x'][2]) - b * np.sin(ekf_state['x'][2])))
    theta = slam_utils.clamp_angle(ekf_state['x'][2] +
                                   dt * vc / L * np.tan(alpha))
    motion[:3] = [xv, yv, theta]
    G = np.zeros([3, ekf_state['x'].shape[0]])
    xv_G = [
        1, 0,
        dt * (-vc * np.sin(ekf_state['x'][2]) - vc / L * np.tan(alpha) *
              (a * np.cos(ekf_state['x'][2]) - b * np.sin(ekf_state['x'][2])))
    ]
    yv_G = [
        0, 1,
        dt * (vc * np.cos(ekf_state['x'][2]) + vc / L * np.tan(alpha) *
              (-a * np.sin(ekf_state['x'][2]) - b * np.cos(ekf_state['x'][2])))
    ]
    theta_G = [0, 0, 1]
    G[:3, :3] = np.vstack([xv_G, yv_G, theta_G])

    return motion, G
Esempio n. 28
0
def motion_model(u, dt, ekf_state, vehicle_params):
    '''
    Computes the discretized motion model for the given vehicle as well as its Jacobian

    Returns:
        f(x,u), a 3x1 vector corresponding to motion x_{t+1} - x_t given the odometry u.

        df/dX, the 3x3 Jacobian of f with respect to the vehicle state (x, y, phi)
    '''

    ###
    # Implement the vehicle model and its Jacobian you derived.
    ###

    H = vehicle_params['H']
    L = vehicle_params['L']
    a = vehicle_params['a']
    b = vehicle_params['b']

    alpha = u[1]

    v_c = u[0] / (1 - np.tan(alpha) * (H / L))
    phi = ekf_state['x'][2]

    motion = np.array(
        [[
            dt * (v_c * np.cos(phi) - (v_c / L) * np.tan(alpha) *
                  (a * np.sin(phi) + b * np.cos(phi)))
        ],
         [
             dt * (v_c * np.sin(phi) + (v_c / L) * np.tan(alpha) *
                   (a * np.cos(phi) - b * np.sin(phi)))
         ], [slam_utils.clamp_angle(dt * (v_c / L) * np.tan(alpha))]])

    jacobian_process = np.array([[
        1, 0, -dt * v_c * (np.sin(phi) + (1 / L) * np.tan(alpha) *
                           (a * np.cos(phi) - b * np.sin(phi)))
    ],
                                 [
                                     0, 1, dt * v_c *
                                     (np.cos(phi) - (1 / L) * np.tan(alpha) *
                                      (a * np.sin(phi) + b * np.cos(phi)))
                                 ], [0, 0, 1]])

    return motion, jacobian_process
Esempio n. 29
0
def laser_measurement_model(ekf_state, landmark_id):
    """
    Returns the measurement model for a (range,bearing) sensor observing the
    mapped landmark with id 'landmark_id' along with its jacobian.
    :param ekf_state:
    :param landmark_id:
    :return:
    h(x, l_id): the 2x1 predicted measurement vector [r_hat, theta_hat].

    dh/dX: For a measurement state with m mapped landmarks, i.e. a state vector of
           dimension 3 + 2*m, this should return the full 2 by 3+2m Jacobian
           matrix corresponding to a measurement of the landmark_id'th feature.
    """

    # landmark ekf_state
    x_l = ekf_state['x'][3 + landmark_id * 2]
    y_l = ekf_state['x'][4 + landmark_id * 2]
    x = ekf_state['x'][0]
    y = ekf_state['x'][1]
    phi = ekf_state['x'][2]

    ### Observation Model ###
    z_t = np.zeros((2, 1))
    q = np.sqrt((x_l - x)**2 + (y_l - y)**2)

    z_t[0, :] = q
    z_t[1, :] = np.arctan2(y_l - y, x_l - x) - phi
    z_t[1, :] = slam_utils.clamp_angle(z_t[1, :])

    # Compute Jacobian for observation
    H = np.zeros((2, len(ekf_state['x'])))

    H[0, 0] = -(x_l - x) / q
    H[0, 1] = -(y_l - y) / q
    H[0, 2] = 0
    H[0, 3 + landmark_id * 2] = (x_l - x) / q
    H[0, 4 + landmark_id * 2] = (y_l - y) / q

    H[1, 0] = (y_l - y) / q**2
    H[1, 1] = -(x_l - x) / q**2
    H[1, 2] = -1
    H[1, 3 + landmark_id * 2] = -(y_l - y) / q**2
    H[1, 4 + landmark_id * 2] = (x_l - x) / q**2

    return z_t, H
Esempio n. 30
0
def motion_model(u, dt, ekf_state, vehicle_params):
    """
    Computes the discretized motion model for the given vehicle as well as its Jacobian
    :param u:
    :param dt:
    :param ekf_state:
    :param vehicle_params:
    :return:
    f(x,u), a 3x1 vector corresponding to motion x_{t+1} - x_t given the odometry u.
    df/dX, the 3x3 Jacobian of f with respect to the vehicle state (x, y, phi)
    """

    # vehicle parameters
    a = vehicle_params['a']
    b = vehicle_params['b']
    L = vehicle_params['L']
    H = vehicle_params['H']

    # transform between ve and vc
    ve = u[0]
    alpha = u[1]
    vc = ve / (1 - math.tan(alpha) * H / L)

    # parameters for motion model
    x, y, phi = ekf_state['x'][0], ekf_state['x'][1], ekf_state['x'][2]
    # make sure phi is between -pi and +pi
    phi = slam_utils.clamp_angle(phi)

    # create motion model
    motion = np.zeros((3, 1))

    motion[0, 0] = dt * (vc * math.cos(phi) - (vc / L) * math.tan(alpha) *
                         (a * math.sin(phi) + b * math.cos(phi)))
    motion[1, 0] = dt * (vc * math.sin(phi) + (vc / L) * math.tan(alpha) *
                         (a * math.cos(phi) - b * math.sin(phi)))
    motion[2, 0] = dt * (vc * math.tan(alpha)) / L

    # create prediction model
    G = np.identity(3)
    G[0, 2] = dt * (-vc * math.sin(phi) - vc / L * math.tan(alpha) *
                    (a * math.cos(phi) - b * math.sin(phi)))
    G[1, 2] = dt * (vc * math.cos(phi) + vc / L * math.tan(alpha) *
                    (-a * math.sin(phi) - b * math.cos(phi)))

    return motion, G