kalman.py 文件源码

def precalc(C, R, x_bar_list, P_bar_list):
    assert C.ndim == 2
    assert R.ndim == 2

    nMeasurement, nStates = x_bar_list.shape
    nObservableState = C.shape[0]

    z_hat_list = C.dot(x_bar_list.T).T
    S_list = np.matmul(np.matmul(C, P_bar_list), C.T) + R
    S_inv_list = np.linalg.inv(S_list)
    K_list = np.matmul(np.matmul(P_bar_list, C.T), S_inv_list)
    P_hat_list = P_bar_list - np.matmul(K_list.dot(C), P_bar_list)

    assert z_hat_list.shape == (nMeasurement, nObservableState), "z_hat ERROR"
    assert S_list.shape == (nMeasurement, nObservableState, nObservableState), "S ERROR"
    assert S_inv_list.shape == S_list.shape, "S_inv ERROR"
    assert K_list.shape == (nMeasurement, nStates, nObservableState)
    assert P_hat_list.shape == P_bar_list.shape, "P_hat ERROR"

    return z_hat_list, S_list, S_inv_list, K_list, P_hat_list