コード例 #1
0
    elif not est.valid:
        nav_init = False
    estPOS[i, :] = est.estPOS[:]
    estVEL[i, :] = est.estVEL[:]
    estATT[i, :] = est.estATT[:]
    estAB[i, :] = est.estAB[:]
    estGB[i, :] = est.estGB[:]
    Pp[i, :] = np.diag(est.P[0:3, 0:3])
    Pvel[i, :] = np.diag(est.P[3:6, 3:6])
    Patt[i, :] = np.diag(est.P[6:9, 6:9])
    Pab[i, :] = np.diag(est.P[9:12, 9:12])
    Pgb[i, :] = np.diag(est.P[12:15, 12:15])
    stateInnov[i, :] = est.stateInnov[:]

psi, theta, phi = navpy.quat2angle(estATT[:, 0],
                                   estATT[:, 1:4],
                                   output_unit='deg')

# Calculate Attitude Error
delta_att = np.nan * np.zeros((drl, 3))
for i in range(0, drl):
    C1 = navpy.angle2dcm(flight_data.psi[i], flight_data.theta[i],
                         flight_data.phi[i]).T
    C2 = navpy.angle2dcm(psi[i], theta[i], phi[i], input_unit='deg').T
    dC = C2.dot(C1.T) - np.eye(3)
    # dC contains delta angle. To match delta quaternion, divide by 2.
    delta_att[i, :] = [-dC[1, 2] / 2.0, dC[0, 2] / 2.0, -dC[0, 1] / 2.0]

lat_ref = estPOS[idx_init[0], 0]
lon_ref = estPOS[idx_init[0], 1]
alt_ref = estPOS[idx_init[0], 2]
コード例 #2
0
def run_filter(imu_data, gps_data):
    drl = len(imu_data)

    # result structures
    estPOS = np.nan * np.ones((drl, 3))
    estVEL = np.nan * np.ones((drl, 3))
    estATT = np.nan * np.ones((drl, 4))
    estAB = np.nan * np.ones((drl, 4))
    estGB = np.nan * np.ones((drl, 4))

    Pp = np.nan * np.ones((drl, 3))
    Pvel = np.nan * np.ones((drl, 3))
    Patt = np.nan * np.ones((drl, 3))
    Pab = np.nan * np.ones((drl, 3))
    Pgb = np.nan * np.ones((drl, 3))

    stateInnov = np.nan * np.ones((drl, 6))

    plot_time = [0.0] * drl

    # Variable Initializer
    idx_init = []

    # Main Loop
    filter = EKF.Filter()

    gps_index = 1
    nav_init = False
    for i, imu in enumerate(imu_data):
        # walk the gps counter forward as needed
        if imu.time >= gps_data[gps_index].time:
            gps_index += 1
        if gps_index >= len(gps_data):
            # no more gps data, stick on the last record
            gps_index = len(gps_data) - 1
        gps = gps_data[gps_index - 1]
        # print "t(imu) = " + str(imu.time) + " t(gps) = " + str(gps.time)

        # update the filter
        plot_time[i] = imu.time
        est = filter.update(imu, gps, verbose=False)

        # save the results for plotting
        if not nav_init and est.valid:
            nav_init = True
            idx_init.append(i)
        elif not est.valid:
            nav_init = False
        estPOS[i, :] = est.estPOS[:]
        estVEL[i, :] = est.estVEL[:]
        estATT[i, :] = est.estATT[:]
        estAB[i, 0:3] = est.estAB[:]
        estAB[i, 3] = imu.temp
        estGB[i, 0:3] = est.estGB[:]
        estGB[i, 3] = imu.temp
        Pp[i, :] = np.diag(est.P[0:3, 0:3])
        Pvel[i, :] = np.diag(est.P[3:6, 3:6])
        Patt[i, :] = np.diag(est.P[6:9, 6:9])
        Pab[i, :] = np.diag(est.P[9:12, 9:12])
        Pgb[i, :] = np.diag(est.P[12:15, 12:15])
        stateInnov[i, :] = est.stateInnov[:]

    psi, theta, phi = navpy.quat2angle(estATT[:, 0],
                                       estATT[:, 1:4],
                                       output_unit='deg')

    # Calculate Attitude Error
    delta_att = np.nan * np.zeros((drl, 3))
    for i in range(0, drl):
        # when processing real data, we have no truth reference
        #C1 = navpy.angle2dcm(flight_data.psi[i],flight_data.theta[i],flight_data.phi[i]).T
        C1 = navpy.angle2dcm(psi[i], theta[i], phi[i], input_unit='deg').T
        C2 = navpy.angle2dcm(psi[i], theta[i], phi[i], input_unit='deg').T
        dC = C2.dot(C1.T) - np.eye(3)
        # dC contains delta angle. To match delta quaternion, divide by 2.
        delta_att[i, :] = [-dC[1, 2] / 2.0, dC[0, 2] / 2.0, -dC[0, 1] / 2.0]

    lat_ref = estPOS[idx_init[0], 0]
    lon_ref = estPOS[idx_init[0], 1]
    alt_ref = estPOS[idx_init[0], 2]

    ecef_ref = navpy.lla2ecef(lat_ref, lon_ref, alt_ref)

    ecef = navpy.lla2ecef(estPOS[:, 0], estPOS[:, 1], estPOS[:, 2])
    filter_ned = navpy.ecef2ned(ecef - ecef_ref, lat_ref, lon_ref, alt_ref)

    # when processing real data, we have no truth reference
    #ecef = navpy.lla2ecef(flight_data.lat,flight_data.lon,flight_data.alt)
    #ecef = navpy.lla2ecef(estPOS[:,0],estPOS[:,1],estPOS[:,2])
    #gnss_ned = navpy.ecef2ned(ecef-ecef_ref,lat_ref,lon_ref,alt_ref)
    #gnss_ned[0:idx_init[0],:] = np.nan

    rawgps = np.nan * np.ones((len(gps_data), 3))
    for i, gps in enumerate(gps_data):
        rawgps[i, :] = [gps.lat, gps.lon, gps.alt]
    ecef = navpy.lla2ecef(rawgps[:, 0],
                          rawgps[:, 1],
                          rawgps[:, 2],
                          latlon_unit='deg')
    ref_ned = navpy.ecef2ned(ecef - ecef_ref, lat_ref, lon_ref, alt_ref)
    ref_ned[0:idx_init[0], :] = np.nan

    return estPOS, estVEL, estATT, estAB, estGB, Pp, Pvel, Patt, Pab, Pgb, stateInnov, plot_time, idx_init, filter_ned, ref_ned, psi, theta, phi, delta_att
コード例 #3
0
ファイル: demo_avior.py プロジェクト: AuraUAS/aura-core
        nav_init = False
    estPOS[i,:] = est.estPOS[:]
    estVEL[i,:] = est.estVEL[:]
    estATT[i,:] = est.estATT[:]
    estAB[i,0:3] = est.estAB[:]
    estAB[i,3] = imu.temp
    estGB[i,0:3] = est.estGB[:]
    estGB[i,3] = imu.temp
    Pp[i,:] = np.diag(est.P[0:3,0:3])
    Pvel[i,:] = np.diag(est.P[3:6,3:6])
    Patt[i,:] = np.diag(est.P[6:9,6:9])
    Pab[i,:] = np.diag(est.P[9:12,9:12])
    Pgb[i,:] = np.diag(est.P[12:15,12:15])
    stateInnov[i,:] = est.stateInnov[:]

psi, theta, phi = navpy.quat2angle(estATT[:,0],estATT[:,1:4],output_unit='deg')

# Calculate Attitude Error
delta_att = np.nan*np.zeros((drl,3))
for i in range(0,drl):
    # when processing real data, we have no truth reference
    #C1 = navpy.angle2dcm(flight_data.psi[i],flight_data.theta[i],flight_data.phi[i]).T
    C1 = navpy.angle2dcm(psi[i],theta[i],phi[i],input_unit='deg').T
    C2 = navpy.angle2dcm(psi[i],theta[i],phi[i],input_unit='deg').T
    dC = C2.dot(C1.T)-np.eye(3)
    # dC contains delta angle. To match delta quaternion, divide by 2.
    delta_att[i,:] = [-dC[1,2]/2.0, dC[0,2]/2.0, -dC[0,1]/2.0]

lat_ref = estPOS[idx_init[0],0]
lon_ref = estPOS[idx_init[0],1]
alt_ref = estPOS[idx_init[0],2]
コード例 #4
0
def run_filter(imu_data, gps_data):
    drl = len(imu_data)

    # result structures
    estPOS = np.nan * np.ones((drl, 3))
    estVEL = np.nan * np.ones((drl, 3))
    estATT = np.nan * np.ones((drl, 4))
    estAB = np.nan * np.ones((drl, 4))
    estGB = np.nan * np.ones((drl, 4))

    Pp = np.nan * np.ones((drl, 3))
    Pvel = np.nan * np.ones((drl, 3))
    Patt = np.nan * np.ones((drl, 3))
    Pab = np.nan * np.ones((drl, 3))
    Pgb = np.nan * np.ones((drl, 3))

    stateInnov = np.nan * np.ones((drl, 6))

    plot_time = [0.0] * drl

    # Variable Initializer
    idx_init = []

    # Main Loop
    filter = EKF.Filter()

    gps_index = 1
    nav_init = False
    for i, imu in enumerate(imu_data):
        # walk the gps counter forward as needed
        if imu.time >= gps_data[gps_index].time:
            gps_index += 1
        if gps_index >= len(gps_data):
            # no more gps data, stick on the last record
            gps_index = len(gps_data) - 1
        gps = gps_data[gps_index - 1]
        # print "t(imu) = " + str(imu.time) + " t(gps) = " + str(gps.time)

        # update the filter
        plot_time[i] = imu.time
        est = filter.update(imu, gps, verbose=False)

        # save the results for plotting
        if not nav_init and est.valid:
            nav_init = True
            idx_init.append(i)
        elif not est.valid:
            nav_init = False
        estPOS[i, :] = est.estPOS[:]
        estVEL[i, :] = est.estVEL[:]
        estATT[i, :] = est.estATT[:]
        estAB[i, 0:3] = est.estAB[:]
        estAB[i, 3] = imu.temp
        estGB[i, 0:3] = est.estGB[:]
        estGB[i, 3] = imu.temp
        Pp[i, :] = np.diag(est.P[0:3, 0:3])
        Pvel[i, :] = np.diag(est.P[3:6, 3:6])
        Patt[i, :] = np.diag(est.P[6:9, 6:9])
        Pab[i, :] = np.diag(est.P[9:12, 9:12])
        Pgb[i, :] = np.diag(est.P[12:15, 12:15])
        stateInnov[i, :] = est.stateInnov[:]

    psi, theta, phi = navpy.quat2angle(estATT[:, 0], estATT[:, 1:4], output_unit="deg")

    # Calculate Attitude Error
    delta_att = np.nan * np.zeros((drl, 3))
    for i in range(0, drl):
        # when processing real data, we have no truth reference
        # C1 = navpy.angle2dcm(flight_data.psi[i],flight_data.theta[i],flight_data.phi[i]).T
        C1 = navpy.angle2dcm(psi[i], theta[i], phi[i], input_unit="deg").T
        C2 = navpy.angle2dcm(psi[i], theta[i], phi[i], input_unit="deg").T
        dC = C2.dot(C1.T) - np.eye(3)
        # dC contains delta angle. To match delta quaternion, divide by 2.
        delta_att[i, :] = [-dC[1, 2] / 2.0, dC[0, 2] / 2.0, -dC[0, 1] / 2.0]

    lat_ref = estPOS[idx_init[0], 0]
    lon_ref = estPOS[idx_init[0], 1]
    alt_ref = estPOS[idx_init[0], 2]

    ecef_ref = navpy.lla2ecef(lat_ref, lon_ref, alt_ref)

    ecef = navpy.lla2ecef(estPOS[:, 0], estPOS[:, 1], estPOS[:, 2])
    filter_ned = navpy.ecef2ned(ecef - ecef_ref, lat_ref, lon_ref, alt_ref)

    # when processing real data, we have no truth reference
    # ecef = navpy.lla2ecef(flight_data.lat,flight_data.lon,flight_data.alt)
    # ecef = navpy.lla2ecef(estPOS[:,0],estPOS[:,1],estPOS[:,2])
    # gnss_ned = navpy.ecef2ned(ecef-ecef_ref,lat_ref,lon_ref,alt_ref)
    # gnss_ned[0:idx_init[0],:] = np.nan

    rawgps = np.nan * np.ones((len(gps_data), 3))
    for i, gps in enumerate(gps_data):
        rawgps[i, :] = [gps.lat, gps.lon, gps.alt]
    ecef = navpy.lla2ecef(rawgps[:, 0], rawgps[:, 1], rawgps[:, 2], latlon_unit="deg")
    ref_ned = navpy.ecef2ned(ecef - ecef_ref, lat_ref, lon_ref, alt_ref)
    ref_ned[0 : idx_init[0], :] = np.nan

    return (
        estPOS,
        estVEL,
        estATT,
        estAB,
        estGB,
        Pp,
        Pvel,
        Patt,
        Pab,
        Pgb,
        stateInnov,
        plot_time,
        idx_init,
        filter_ned,
        ref_ned,
        psi,
        theta,
        phi,
        delta_att,
    )