Exemplo n.º 1
0
def kf_predict(state, covariance, F, Q, B=None, u=None):
    pred_state = blas.dgemv(F, state)
    if (not B==None) and (not u==None):
        blas.dgemv(B, u, y=pred_state)
    # Repeat Q n times and return as the predicted covariance
    pred_cov = np.repeat(np.array([Q]), state.shape[0], 0)
    blas.dgemm(F, blas.dgemm(covariance, F, TRANSPOSE_B=True), C=pred_cov)
    return pred_state, pred_cov
Exemplo n.º 2
0
def kf_update_cov(covariance, H, R, INPLACE=True):
    kalman_info = lambda:0
    
    if INPLACE:
        upd_covariance = covariance
        covariance_copy = covariance.copy()
    else:
        upd_covariance = covariance.copy()
        covariance_copy = covariance
    
    # Store R
    #chol_S = np.repeat(R, covariance.shape[0], 0)
    # Compute PH^T
    p_ht = blas.dgemm(covariance, H, TRANSPOSE_B=True)
    # Compute HPH^T + R
    #blas.dgemm(H, p_ht, C=chol_S)
    hp_ht_pR = blas.dgemm(H, p_ht) + R
    # Compute the Cholesky decomposition
    chol_S = blas.dpotrf(hp_ht_pR, False)
    # Select the lower triangle (set the upper triangle to zero)
    blas.mktril(chol_S)
    # Compute the determinant
    diag_vec = np.array([np.diag(chol_S[i]) for i in range(chol_S.shape[0])])
    det_S = diag_vec.prod(1)**2
    # Compute the inverse of the square root
    inv_sqrt_S = blas.dtrtri(chol_S, 'l')
    # Compute the inverse using dsyrk
    inv_S = blas.dsyrk('l', inv_sqrt_S, TRANSPOSE_A=True)
    # Symmetrise the matrix since only the lower triangle is stored
    blas.symmetrise(inv_S, 'l')
    #blas.dpotri(op_S, True)
    # inv_S = op_S
    
    # Kalman gain
    kalman_gain = blas.dgemm(p_ht, inv_S)
    
    # Update the covariance
    k_h = blas.dgemm(kalman_gain, H)
    blas.dgemm(k_h, covariance_copy, alpha=-1.0, C=upd_covariance)
    
    kalman_info.S = hp_ht_pR
    kalman_info.inv_sqrt_S = inv_sqrt_S
    kalman_info.det_S = det_S
    kalman_info.kalman_gain = kalman_gain
    
    return upd_covariance, kalman_info
Exemplo n.º 3
0
def kf_predict_cov(covariance, F, Q):
    # Repeat Q n times and return as the predicted covariance
    #pred_cov = np.repeat(np.array([Q]), covariance.shape[0], 0)
    #blas.dgemm(F, blas.dgemm(covariance, F, TRANSPOSE_B=True), C=pred_cov)
    pred_cov = blas.dgemm(F, blas.dgemm(covariance, F, TRANSPOSE_B=True)) + Q
    return pred_cov