kalmanfilter.py

def kalman_predict(x_hat, u, P, wheel_slip_std):
    # non-linear state transition
    def f(p, u):
        x, y, θ = p
        dΦ_l, dΦ_r = u
        return v(
            x + (((WHEEL_R * dΦ_l) / 2) +
                 ((WHEEL_R * dΦ_r) / 2)) * DT * cos(θ),
            y + (((WHEEL_R * dΦ_l) / 2) +
                 ((WHEEL_R * dΦ_r) / 2)) * DT * sin(θ),
            θ + (((WHEEL_R * dΦ_r) / D) -
                 ((WHEEL_R * dΦ_l) / D)) * DT).T

    x, y, θ = x_hat
    dΦ_l, dΦ_r = u

    # Jacobian of f with respect to state
    F = v([1, 0, -((WHEEL_R * dΦ_l + WHEEL_R * dΦ_r) / 2) * DT * sin(θ)],
          [0, 1,  ((WHEEL_R * dΦ_l + WHEEL_R * dΦ_r) / 2) * DT * cos(θ)],
          [0, 0,  1])

    # Jacobian of f with respect to control
    W = v([(WHEEL_R * cos(θ)) / 2 * DT, (WHEEL_R * cos(θ)) / 2 * DT],
          [(WHEEL_R * sin(θ)) / 2 * DT, (WHEEL_R * sin(θ)) / 2 * DT],
          [-WHEEL_R / D * DT, WHEEL_R / D * DT])

    # Process noise covariance matrix
    Σ = v([wheel_slip_std ** 2, 0],
          [0, wheel_slip_std ** 2])

    # Process noise transformed to state space
    Q = W.dot(Σ).dot(W.T)

    new_x_hat = f(x_hat, u)
    new_P = F.dot(P).dot(F.T) + Q

    return new_x_hat, new_P

def kalman_correct(x_hat_est, P_est, b_pos, b_reading, beacon_std):
    h = dist(x_hat_est[:2], b_pos)

    # innovation
    y_twiddle = b_reading - h

    H = v((x_hat_est[0] - b_pos[0]) / h, (x_hat_est[1] - b_pos[1]) / h, 0)
    R = beacon_std ** 2

    # innovation covariance
    S = H.dot(P_est).dot(H.T) + R

    # Kalman gain
    K = P_est.dot(H.T) * (1 / S)

    new_x_hat = x_hat_est + K * y_twiddle
    new_P = (np.identity(3) - K.dot(H)).dot(P_est)

    # HACK
    if np.isinf(new_P).any():
        # sorry dr. schwesinger...
        # numpy was overflowing... and we suck...
        # ...
        new_P = np.identity(3)

    #print('P', P, 'h', h, 'H', H, 'R', R, 'y_tw', y_twiddle, 'S', S, 'K', K, 'nP', new_P, 'nX', new_x_hat)
    return new_x_hat, new_P

# ...

prev_pose = cur_pose
cur_pose, P = kalman_predict(prev_pose, prev_control, P, WHEEL_SLIP)
print(P)
for reading, b in zip(beacon_readings, BEACON_POSITIONS):
    if np.isnan(reading):
        continue
    cur_pose, P = kalman_correct(cur_pose, P, b, reading, BEACON_STD)