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 _sigma_cov_(sigma_x, x_hat, wt_cv, proc_noise):
residuals = sigma_x - x_hat
sigma_cov = np.array([ (blas.dsyr('l', residuals, wt_cv)).sum(axis=0) ])
blas.symmetrise(sigma_cov, 'l')
sigma_cov = sigma_cov[0] + proc_noise
