kalman_filter.py 文件源码

def sqrt_kf_tu(x_hat_posterior,
               P_sqrt_posterior,
               F_i,
               Q_sqrt_i,
               z_i=None):
    """
    Square root Kalman filter time update. Given the following:
    - *x_hat_posterior*: posterior state estimate (N)
    - *P_sqrt_posterior*: posterior error covariance square root (NxN)
    - *F_i*: time update operator (NxN)
    - *Q_sqrt_i*: time update noise covariance square root (NxN)
    - *z_i*: optional) systematic time update input (N)

    Return the tuple containing the one time step prediction of the
    state and the square root of the error covariance.
    """
    N, _ = F_i.shape
    x_hat_prior = NP.matmul(F_i, x_hat_posterior)
    if z_i is not None:
        x_hat_prior += z_i
    A_T = NP.block([NP.matmul(F_i, P_sqrt_posterior), Q_sqrt_i])
    R_T = NP.linalg.qr(A_T.T, mode='r')
    P_sqrt_prior = R_T.T[:, :N]
    return x_hat_prior, P_sqrt_prior