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
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
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
