test_kf.py 文件源码

def test_propagate_information_filter():
    np.set_printoptions(precision=2, linewidth=132)

    M_matrix = np.eye(7)
    sigma = np.array([0.12, 0.7, 0.0959, 0.15, 1.5, 0.2, 0.5])
    x_analysis = np.array([0.17, 1.0, 0.1, 0.7, 2.0, 0.18, np.exp(-0.5*1.5)])
    Pd = np.diag(sigma**2).astype(np.float32)
    Pd[5, 2] = 0.8862*0.0959*0.2
    Pd[2, 5] = 0.8862*0.0959*0.2
    Pi = np.linalg.inv(Pd)
    Q_matrix = np.eye(7)*0.1

    x_forecast, P_forecast, P_forecast_inverse = propagate_information_filter(
        x_analysis, None, Pi, M_matrix, Q_matrix)
    assert np.allclose(
        np.array(P_forecast_inverse.todense()).squeeze().diagonal(),
        np.array([8.74, 1.69, 9.81, 8.16, 0.43, 9.21, 2.86]), atol=0.01)
    # In reality, the matrix ought to be
    # [[ 8.74  0.    0.    0.    0.    0.    0.  ]
    # [ 0.    1.69  0.    0.    0.    0.    0.  ]
    # [ 0.    0.    9.33  0.    0.   -1.13  0.  ]
    # [ 0.    0.    0.    8.16  0.    0.    0.  ]
    # [ 0.    0.    0.    0.    0.43  0.    0.  ]
    # [ 0.    0.   -1.13  0.    0.    7.28  0.  ]
    # [ 0.    0.    0.    0.    0.    0.    2.86]]