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
