filt.py 文件源码

def _compute_output_errors(traj, x, P, output_stamps,
                           gyro_model, accel_model):
    T = _errors_transform_matrix(traj.loc[output_stamps])
    y = util.mv_prod(T, x[:, :N_BASE_STATES])
    Py = util.mm_prod(T, P[:, :N_BASE_STATES, :N_BASE_STATES])
    Py = util.mm_prod(Py, T, bt=True)
    sd_y = np.diagonal(Py, axis1=1, axis2=2) ** 0.5

    err = pd.DataFrame(index=output_stamps)
    err['lat'] = y[:, DRN]
    err['lon'] = y[:, DRE]
    err['VE'] = y[:, DVE]
    err['VN'] = y[:, DVN]
    err['h'] = np.rad2deg(y[:, DH])
    err['p'] = np.rad2deg(y[:, DP])
    err['r'] = np.rad2deg(y[:, DR])

    sd = pd.DataFrame(index=output_stamps)
    sd['lat'] = sd_y[:, DRN]
    sd['lon'] = sd_y[:, DRE]
    sd['VE'] = sd_y[:, DVE]
    sd['VN'] = sd_y[:, DVN]
    sd['h'] = np.rad2deg(sd_y[:, DH])
    sd['p'] = np.rad2deg(sd_y[:, DP])
    sd['r'] = np.rad2deg(sd_y[:, DR])

    gyro_err = pd.DataFrame(index=output_stamps)
    gyro_sd = pd.DataFrame(index=output_stamps)
    n = N_BASE_STATES
    for i, name in enumerate(gyro_model.states):
        gyro_err[name] = x[:, n + i]
        gyro_sd[name] = P[:, n + i, n + i] ** 0.5

    accel_err = pd.DataFrame(index=output_stamps)
    accel_sd = pd.DataFrame(index=output_stamps)
    ng = gyro_model.n_states
    for i, name in enumerate(accel_model.states):
        accel_err[name] = x[:, n + ng + i]
        accel_sd[name] = P[:, n + ng + i, n + ng + i] ** 0.5

    return err, sd, gyro_err, gyro_sd, accel_err, accel_sd