test_kf.py 文件源码

def plot_trajectory(state_true, state_estimated):
    plt.plot(state_true[:, 0], state_true[:, 2], color="red")
    plt.scatter(state_estimated[:, 0].tolist()[::10],
                state_estimated[:, 2].tolist()[::10],
                marker="o",
                color="blue")