def test_angle2dcm(self): """ Test forming transformation from navigation to body frame based on specified Euler angles. Data Source: Example generated using book GNSS Applications and Methods Chapter 7 library function: eul2Cbn.m (transpose of this used) """ # Define Euler angles yaw, pitch, roll = np.deg2rad([-83, 2.3, 13]) Rnav2body_expected = np.matrix([[0.121771, -0.991747, -0.040132], [0.968207, 0.109785, 0.224770], [-0.218509, -0.066226, 0.973585]]) Rnav2body_computed = navpy.angle2dcm(yaw, pitch, roll) np.testing.assert_almost_equal(Rnav2body_expected, Rnav2body_computed, decimal=6) # Test units feature yaw_deg, pitch_deg, roll_deg = np.rad2deg([yaw, pitch, roll]) Rnav2body_computed = navpy.angle2dcm(yaw_deg, pitch_deg, roll_deg, input_unit='deg') np.testing.assert_almost_equal(Rnav2body_expected, Rnav2body_computed, decimal=6)
def __init__(self, static_properties): self.static_properties = static_properties self.lat_ref = self.static_properties.initial_lat self.long_ref = self.static_properties.initial_long self.alt_ref = self.static_properties.initial_alt # Outputs of the Kalman filter # in NED, starting point is relative self.estimated_position = np.matrix(navpy.lla2ecef(self.lat_ref, self.long_ref, self.alt_ref)).T # prev gps variables self.prev_gps_position_ecef = np.matrix(navpy.lla2ecef(self.lat_ref, self.long_ref, self.alt_ref)).T self.estimated_velocity = self.ned_to_ecef( self.static_properties.initial_v_north, self.static_properties.initial_v_east, self.static_properties.initial_v_down, ) # initial velocity should be given in meters with respect to your NED # old_est_C_b_e in Loosely_coupled_INS_GNSS (line 166) self.estimated_attitude = navpy.angle2dcm( self.static_properties.initial_yaw, self.static_properties.initial_pitch, self.static_properties.initial_roll, )
def __init__(self, initial_roll, initial_pitch, initial_yaw, initial_lat, initial_long, initial_alt, initial_v_north, initial_v_east, initial_v_down, initial_attitude_unc, initial_velocity_unc, initial_position_unc, initial_accel_bias_unc, initial_gyro_bias_unc, gyro_noise_PSD, accel_noise_PSD, accel_bias_PSD, gyro_bias_PSD, pos_meas_SD, vel_meas_SD): # initialize properties (from in_profile) self.initial_lat = initial_lat # Column 2 self.initial_long = initial_long # Column 3 self.initial_alt = initial_alt # Column 4 self.initial_v_north = initial_v_north # Column 5 self.initial_v_east = initial_v_east # Column 6 self.initial_v_down = initial_v_down # Column 7 self.initial_roll = initial_roll # Column 8 self.initial_pitch = initial_pitch # Column 9 self.initial_yaw = initial_yaw # Column 10 # properties of LC_KF_config self.uncertainty_values = dict( attitude=initial_attitude_unc, # init_att_unc velocity=initial_velocity_unc, # init_vel_unc position=initial_position_unc, # init_pos_unc accel_bias=initial_accel_bias_unc, # init_b_a_unc gyro_bias=initial_gyro_bias_unc, # init_b_g_unc gyro_noise_PSD=gyro_noise_PSD, accel_noise_PSD=accel_noise_PSD, accel_bias_PSD=accel_bias_PSD, gyro_bias_PSD=gyro_bias_PSD, pos_meas_SD=pos_meas_SD, vel_meas_SD=vel_meas_SD, ) self.lat_ref = self.initial_lat self.long_ref = self.initial_long self.alt_ref = self.initial_alt # Outputs of the Kalman filter self.estimated_position = np.matrix( navpy.lla2ecef(self.lat_ref, self.long_ref, self.alt_ref)).T # prev gps variables self.prev_gps_position_ecef = np.matrix( navpy.lla2ecef(self.lat_ref, self.long_ref, self.alt_ref)).T self.estimated_velocity = self.ned_to_ecef( self.initial_v_north, self.initial_v_east, self.initial_v_down ) # initial velocity should be given in meters with respect to your NED # old_est_C_b_e in Loosely_coupled_INS_GNSS (line 166) self.estimated_attitude = np.matrix( navpy.angle2dcm( # Euler_to_CTM self.initial_yaw, self.initial_pitch, self.initial_roll, )) self.estimated_imu_biases = np.matrix(np.zeros((6, 1)))
def test_angle2dcm(self): """ Test forming transformation from navigation to body frame based on specified Euler angles. Data Source: Example 1 generated using book GNSS Applications and Methods Chapter 7 library function: eul2Cbn.m (transpose of this used). Example 2 found in Performance, Stability, Dynamics, and Control of Airplanes, Second Edition (p. 355). """ # Define Euler angles and expected DCMs checks = ( (np.deg2rad([-83, 2.3, 13]), 6, np.matrix([[ 0.121771, -0.991747, -0.040132], [ 0.968207, 0.109785, 0.224770], [-0.218509, -0.066226, 0.973585]])), (np.deg2rad([-10, 20, 30]), 4, np.matrix([[ 0.9254, 0.3188, 0.2049], [-0.1632, 0.8232, -0.5438], [-0.3420, 0.4698, 0.8138]]).T), ) for angles, decimal, Rnav2body_expected in checks: yaw, pitch, roll = angles Rnav2body_computed = navpy.angle2dcm(yaw, pitch, roll) np.testing.assert_almost_equal(Rnav2body_expected, Rnav2body_computed, decimal=decimal) # Test units feature yaw_deg, pitch_deg, roll_deg = np.rad2deg([yaw, pitch, roll]) Rnav2body_computed = navpy.angle2dcm(yaw_deg, pitch_deg, roll_deg, input_unit='deg') np.testing.assert_almost_equal(Rnav2body_expected, Rnav2body_computed, decimal=decimal) # Test with multiple inputs angles = np.column_stack([a for a, d, r in checks]) Rnav2body = navpy.angle2dcm(*angles) for (Rnav2body_expected, decimal), Rnav2body_computed in zip( [(r, d) for a, d, r in checks], Rnav2body): np.testing.assert_almost_equal(Rnav2body_expected, Rnav2body_computed, decimal=decimal)
def test_angle2dcm(self): """ Test forming transformation from navigation to body frame based on specified Euler angles. Data Source: Example generated using book GNSS Applications and Methods Chapter 7 library function: eul2Cbn.m (transpose of this used) """ # Define Euler angles yaw, pitch, roll = np.deg2rad([-83, 2.3, 13]) Rnav2body_expected = np.matrix([[ 0.121771, -0.991747, -0.040132], [ 0.968207, 0.109785, 0.224770], [-0.218509, -0.066226, 0.973585]]) Rnav2body_computed = navpy.angle2dcm(yaw, pitch, roll) np.testing.assert_almost_equal(Rnav2body_expected, Rnav2body_computed, decimal=6) # Test units feature yaw_deg, pitch_deg, roll_deg = np.rad2deg([yaw, pitch, roll]) Rnav2body_computed = navpy.angle2dcm(yaw_deg, pitch_deg, roll_deg, input_unit='deg') np.testing.assert_almost_equal(Rnav2body_expected, Rnav2body_computed, decimal=6)
def simulate_magnetometer(yaw, pitch, roll, input_units='rad'): """ Simulate magnetometer measurements, as measured by a 3-axis magnetomter. This is useful to demonstrate the ability of our calibration algorithm to recover a calibrated measurement in proximity to the truth. Parameters ---------- yaw, pitch, roll : Length N numpy arrays of truth/assumed Euler angles, assuming 3-2-1 rotation sequence, where N is the number of data points. input_units: units for input angles {'rad', 'deg'}, optional. Returns ------- hb: (N x 3) numpy array of truth magnetic field in the body-frame hm: (N x 3) numpy array of corrupted magnetometer measurements in body-frame, where N is the numberof data points. Notes ----- The following describes the steps for simulating the magentometer measurements: 1. Define magnetic fieled in navigation frame --> (h) 2. Load simulated true attitude (Euler Angles) profile 3. Map true magnetic field measurements to the body-frame using DCM --> (hb) 4. Load simulated sensor errors for magnetometer & form corrupting transformation (C, b). 5. Corrupt true body-frame measurements accoding to: hm = C * hb + b + n 6. Return true and corrupted body-axis magnetometer measurements. Todo ---- If position information is availble, we can use a magnetic field model to vary the true observed magnetic field. Date: September 25, 2013 Updated: Octtober 6, 2013 Author: Hamid Mokhtarzadeh """ print('Truth magnetic field assumed for Minneapolis, MN.') # TODO: Find way to define desired error terms - currently they are hard-coded! print('Warning: Assumed error terms are currently hard-coded in function!') # Apply necessary unit transformations. if input_units == 'rad': yaw_rad, pitch_rad, roll_rad = yaw, pitch, roll elif input_units == 'deg': yaw_rad, pitch_rad, roll_rad = np.radians([yaw, pitch, roll]) drl = len(yaw_rad) ########## # STEP 3 # ########## hb = np.nan * np.zeros((drl,3)) # Map true magnetic field measurements to the body-frame using DCM --> (hb) for k, [y, p, r] in enumerate(zip(yaw_rad, pitch_rad, roll_rad)): Rbody2nav = angle2dcm(y, p, r) hb[k,:] = np.dot(Rbody2nav, h) ########## # STEP 4 # ########## # Load simulated sensor errors for magnetometer & form # sigma_n - standard deviation of additive Guassian white-noise # b - 3x1 vector, null-shift or hard iron errors # Cs - 3x3 matrix, scale factor # Ce - 3x3 matrix, misalignment affects # Ca - 3x3 matrix, soft-iron and axes nonorthogonality # # C = Cs * Ce * Ca sigma_n = 0.0007 # Guass # TODO: decide whether to add noise term or not bx, by, bz = 0.2, 0.2, -0.1 # bias sx, sy, sz = 0.0, 0.0, 0.0 # scale factor (0: no scale factor error) ex, ey, ez = 0., 0., 0. # misalignment errors (0: no misalignment error) # Combined soft-iron and axes nonorthogonality effects (0: no error) axx, ayy, azz = 0., 0., 0. # We'll assume symmetric effects for now. axy = ayx = 0. axz = azx = 0. ayz = azy = 0. # Form error terms Cs = np.eye(3) + np.diag([sx, sy, sz]) Ce = np.array([[ 1., ez, -ey], [-ez, 1., ex], [ ey, -ex, 1.]]) Ca = np.array([[1.+axx, axy, axz], [ ayx, 1.+ayy, ayz], [ azx, azy, 1.+azz]]) b = np.array([bx, by, bz]) n = sigma_n * np.random.randn(drl, 3) C = Cs.dot(Ce).dot(Ca) ########## # STEP 5 # ########## # Corrupt true body-frame measurements according to: # hm = C * hb + b + n hm = C.dot(hb.T).T + b + n return hb, hm
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
def test_angle2dcm(self): """ Test forming transformation from navigation to body frame based on specified Euler angles. Data Source: Example 1 generated using book GNSS Applications and Methods Chapter 7 library function: eul2Cbn.m (transpose of this used). Example 2 found in Performance, Stability, Dynamics, and Control of Airplanes, Second Edition (p. 355). """ # Define Euler angles and expected DCMs checks = ( (np.deg2rad([45, 0, 0]), 15, 'ZYX', np.matrix([[ 0.707106781186548, 0.707106781186547, 0 ], [ -0.707106781186547, 0.707106781186548, 0], [ 0, 0, 1.000000000000000]])), (np.deg2rad([0, 45, 0]), 15, 'ZYX', np.matrix([[ 0.707106781186548, 0, -0.707106781186547 ], [ 0, 1.000000000000000, 0], [ 0.707106781186547, 0, 0.707106781186548]])), (np.deg2rad([0, 0, 45]), 15, 'ZYX', np.matrix([[ 1.000000000000000, 0, 0], [ 0, 0.707106781186548, 0.707106781186547], [ 0, -0.707106781186547, 0.707106781186548]])), (np.deg2rad([-83, 2.3, 13]), 6, 'ZYX', np.matrix([[ 0.121771, -0.991747, -0.040132], [ 0.968207, 0.109785, 0.224770], [-0.218509, -0.066226, 0.973585]])), (np.deg2rad([-10, 20, 30]), 4, 'ZYX', np.matrix([[ 0.9254, 0.3188, 0.2049], [-0.1632, 0.8232, -0.5438], [-0.3420, 0.4698, 0.8138]]).T), ) checks_sequences = ( (np.deg2rad([45, -5, 70]), 15, 'XYZ', np.matrix([[ 0.340718653421610, 0.643384864470459, 0.685541184306890], [-0.936116806662859, 0.299756531066926, 0.183932994229025], [-0.087155742747658, -0.704416026402759, 0.704416026402759]])), (np.deg2rad([45, -5, 70]), 15, 'ZYX', np.matrix([[0.704416026402759, 0.704416026402759, 0.087155742747658 ], [-0.299756531066926, 0.183932994229025, 0.936116806662859], [ 0.643384864470459, -0.685541184306890, 0.340718653421610]])), (np.deg2rad([45, -5, 70]), 15, 'ZYZ', np.matrix([[-0.423538554077505, 0.905387494699844, 0.029809019626209], [-0.903779304621979, -0.420089779326028, -0.081899608319089], [-0.061628416716219, -0.061628416716219, 0.996194698091746]])), (np.deg2rad([45, -5, 70]), 15, 'ZXY', np.matrix([[0.299756531066926, 0.183932994229025, -0.936116806662859], [-0.704416026402759, 0.704416026402759, -0.087155742747658], [0.643384864470459, 0.685541184306890, 0.340718653421610]])), (np.deg2rad([45, -5, 70]), 15, 'ZXZ', np.matrix([[-0.420089779326028, 0.903779304621979, -0.081899608319089], [-0.905387494699844, -0.423538554077505, -0.029809019626209], [-0.061628416716219, 0.061628416716219, 0.996194698091746]])), (np.deg2rad([45, -5, 70]), 15, 'YXZ', np.matrix([[0.183932994229025, 0.936116806662859, -0.299756531066926], [-0.685541184306890, 0.340718653421610, 0.643384864470459], [0.704416026402759, 0.087155742747658, 0.704416026402759]])), (np.deg2rad([45, -5, 70]), 15, 'YXY', np.matrix([[-0.420089779326028, -0.081899608319089, -0.903779304621979], [-0.061628416716219, 0.996194698091746, -0.061628416716219], [0.905387494699844, 0.029809019626209, -0.423538554077505]])), (np.deg2rad([45, -5, 70]), 15, 'YZX', np.matrix([[0.704416026402759, -0.087155742747658, -0.704416026402759], [0.685541184306890, 0.340718653421610, 0.643384864470459], [0.183932994229025, -0.936116806662859, 0.299756531066926]])), (np.deg2rad([45, -5, 70]), 15, 'YZY', np.matrix([[-0.423538554077505, -0.029809019626209, -0.905387494699844], [0.061628416716219, 0.996194698091746, -0.061628416716219], [0.903779304621979, -0.081899608319089, -0.420089779326028]])), (np.deg2rad([45, -5, 70]), 15, 'XYX', np.matrix([[0.996194698091746, -0.061628416716219, 0.061628416716219], [-0.081899608319089, -0.420089779326028, 0.903779304621979], [-0.029809019626209, -0.905387494699844, -0.423538554077505]])), (np.deg2rad([45, -5, 70]), 15, 'XZY', np.matrix([[0.340718653421610, 0.643384864470459, -0.685541184306890], [0.087155742747658, 0.704416026402759, 0.704416026402759], [0.936116806662859, -0.299756531066926, 0.183932994229025]])), (np.deg2rad([45, -5, 70]), 15, 'XZX', np.matrix([[0.996194698091746, -0.061628416716219, -0.061628416716219], [0.029809019626209, -0.423538554077505, 0.905387494699844], [-0.081899608319089, -0.903779304621979, -0.420089779326028]])), ) for angles, decimal, sequence, Rnav2body_expected in checks_sequences: yaw, pitch, roll = angles Rnav2body_computed = navpy.angle2dcm(yaw, pitch, roll, rotation_sequence=sequence) np.testing.assert_almost_equal(Rnav2body_expected, Rnav2body_computed, decimal=decimal) # Test units feature yaw_deg, pitch_deg, roll_deg = np.rad2deg([yaw, pitch, roll]) Rnav2body_computed = navpy.angle2dcm(yaw_deg, pitch_deg, roll_deg, input_unit='deg', rotation_sequence=sequence) np.testing.assert_almost_equal(Rnav2body_expected, Rnav2body_computed, decimal=decimal) # Test with multiple inputs angles = np.column_stack([a for a, d, s, r in checks]) sequences = np.column_stack([s for a, d, s, r in checks]) Rnav2body = navpy.angle2dcm(*angles) for (Rnav2body_expected, decimal), Rnav2body_computed in zip( [(r, d) for a, d, s, r in checks], Rnav2body): np.testing.assert_almost_equal(Rnav2body_expected, Rnav2body_computed, decimal=decimal)
if xmax > x.max(): xmax = x.max() print "flight range = %.3f - %.3f (%.3f)" % (xmin, xmax, xmax-xmin) trange = xmax - xmin sense_data = [] ideal_data = [] for i, x in enumerate( np.linspace(xmin, xmax, trange*args.resample_hz) ): alt = filter_alt(x) phi = filter_phi(x) the = filter_the(x) psi = filter_psi(x) #print phi, the, psi N2B = navpy.angle2dcm(psi, the, phi, input_unit='deg') mag_ideal = N2B.dot(mag_ned) norm = np.linalg.norm(mag_ideal) mag_ideal /= norm hx = imu_hx(x) hy = imu_hy(x) hz = imu_hz(x) if abs(hx) > 1000: print "oops:", hx, hy, hz mag_sense = np.array([hx, hy, hz]) # print mag_sense if args.flight: ideal_data.append( mag_ideal[:].tolist() ) sense_data.append( mag_sense[:].tolist() ) elif args.sentera and alt >= alt_cutoff: ideal_data.append( mag_ideal[:].tolist() )
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] 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) ecef = navpy.lla2ecef(flight_data.lat,flight_data.lon,flight_data.alt)
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
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, )
# TODO: Print header csv_writer = csv.writer(fobj) csv_writer.writerow(hdr_list) for k in range(len(t_store)): # Convert eps_NED to eps_YPR yaw_rad = nav_data_dict['psi_store'][k] pitch_rad = nav_data_dict['the_store'][k] roll_rad = nav_data_dict['phi_store'][k] # Note, as part of transformation we are # ignoring uncertinty in the mapping. epsNED_std_deg = [r2d(nav_data_dict['epsN_std'][k]), r2d(nav_data_dict['epsE_std'][k]), r2d(nav_data_dict['epsD_std'][k])] yaw_std_deg = epsNED_std_deg[2] pitch_std_deg = navpy.angle2dcm(yaw_rad, 0, 0, input_unit='rad').dot(epsNED_std_deg)[1] roll_std_deg = navpy.angle2dcm(yaw_rad, pitch_rad, 0, input_unit='rad').dot(epsNED_std_deg)[0] row = [int(t_store[k]*1e6), int(r2d(nav_data_dict['navlat_store'][k])*1e7), int(r2d(nav_data_dict['navlon_store'][k])*1e7), nav_data_dict['navalt_store'][k], int(roll_rad*1e4), int(pitch_rad*1e4), int(yaw_rad*1e4), nav_data_dict['NS_std'][k], nav_data_dict['WE_std'][k], nav_data_dict['alt_std'][k], yaw_std_deg, pitch_std_deg, roll_std_deg]
def WF2NED(wf_frame_Float32MultiArray, wf_heading = cf.WF_HEADING): dcm = navpy.angle2dcm(-wf_heading,0,0) newloc = Float32MultiArray() newloc.data = np.matmul(wf_frame_Float32MultiArray.data,dcm) return newloc