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 get_gps_orientation(lat1, long1, alt1, lat2, long2, alt2):
pos1 = navpy.lla2ecef(lat1, long1, alt1)
pos2 = navpy.lla2ecef(lat2, long2, alt2)
delta_pos = pos2 - pos1
yaw = math.atan2(delta_pos[1], delta_pos[0])
pitch = math.atan2(delta_pos[2] * math.cos(yaw), delta_pos[0])
roll = math.atan2(math.cos(yaw), math.sin(yaw) * math.sin(pitch))
return yaw, pitch, roll
def test_lla2ecef_vector(self):
"""
Test conversion of multiple LLA to ECEF
Data Source: Pretoria and Sydney, Exercise 2.4 and 2.5 of
Aided Navigation: GPS with High Rate Sensors, Jay A. Farrel
2008
"""
ecef_pretoria = [5.057590377e6, 2.694861463e6, -2.794229000e6]#in meters
lat_pretoria = -(26. + 8./60 + 42.20/3600) # South
lon_pretoria = +(28. + 3./60 + 0.92/3600) # East
alt_pretoria = 1660.86 # [meters]
ecef_sydney = [-4.646678571e6, 2.549341033e6, -3.536478881e6] #in meters
lat_sydney = -( 33. + 53./60 + 28.15/3600) # South
lon_sydney = +(151. + 14./60 + 57.07/3600) # East
alt_sydney = 86.26 # [meters]
lat = np.array([lat_pretoria,lat_sydney])
lon = [lon_pretoria,lon_sydney]
alt = [alt_pretoria,alt_sydney]
ecef = np.vstack((ecef_pretoria,ecef_sydney))
# Do conversion and check result
ecef_computed = navpy.lla2ecef(lat,lon,alt)
# Note: see comment in test_lla2ecef_Ausralia() as to why
# only 2 digits of accuracy is being tested for the example.
np.testing.assert_almost_equal(ecef_computed,ecef,decimal=2)
def test_helpers():
lat1, long1, alt1 = 40.4404285, -79.9422232, 296.18
ecef_position = navpy.lla2ecef(lat1, long1, alt1)
x, y, z = ecef_position
almost_equal(x, 848993, epsilon=0.5)
almost_equal(y, -4786645, epsilon=0.5)
almost_equal(z, 4115520, epsilon=0.5)
test_coordinate_transforms(40.44057846069336, 79.94245147705078, 302.79998779296875)
def test_coordinate_transforms(lat1, long1, alt1):
ecef_position = navpy.lla2ecef(lat1, long1, alt1)
# print(ecef_position)
lat2, long2, alt2 = navpy.ecef2lla(ecef_position)
# print(lat2, long2, alt2)
almost_equal(lat1, lat2)
almost_equal(long1, long2)
almost_equal(alt1, alt2)
def test_lla2ecef(self):
"""
Test conversion of LLA to ECEF.
Data Source: Example 2.1 of Aided Navigation: GPS with High Rate
Sensors, Jay A. Farrel 2008
"""
# Near Los Angeles, CA
lat = +(34. + 0./60 + 0.00174/3600) # North
lon = -(117. + 20./60 + 0.84965/3600) # West
alt = 251.702 # [meters]
ecef = [-2430601.828, -4702442.703, 3546587.358] # [meters]
# Do conversion and check result
# Note: default units are degrees
ecef_computed = navpy.lla2ecef(lat, lon, alt)
for e1, e2 in zip(ecef_computed, ecef):
self.assertAlmostEqual(e1, e2, places=3)
def test_lla2ecef_SAfrica(self):
"""
Test conversion of LLA to ECEF.
Data Source: Exercise 2.4 and 2.5 of Aided Navigation: GPS with High
Rate Sensors, Jay A. Farrel 2008
Note: N, E (+) and S, W (-)
"""
# Pretoria S. Africa
lat = -(26. + 8./60 + 42.20/3600) # South
lon = +(28. + 3./60 + 0.92/3600) # East
alt = 1660.86 # [meters]
ecef = [5.057590377e6, 2.694861463e6, -2.794229000e6] # [meters]
# Do conversion and check result
# Note: default units are degrees
ecef_computed = navpy.lla2ecef(lat, lon, alt)
# Note: see comment in test_lla2ecef_Ausralia() as to why
# only 2 digits of accuracy is being tested for the example.
for e1, e2 in zip(ecef_computed, ecef):
self.assertAlmostEqual(e1, e2, places=2)
def test_lla2ecef_Ausralia(self):
"""
Test conversion of LLA to ECEF.
Data Source: Exercise 2.4 and 2.5 of Aided Navigation: GPS with High
Rate Sensors, Jay A. Farrel 2008
Note: N, E (+) and S, W (-)
"""
# Sydney Australia
lat = -( 33. + 53./60 + 28.15/3600) # South
lon = +(151. + 14./60 + 57.07/3600) # East
alt = 86.26 # [meters]
# Note: The book's example only seems reliable to 2 digits passed
# decimal. For example, ecef_x is recorded as xxx.571, but
# even using the books equation (2.7 and 2.9) you get xxx.572555
ecef = [-4.646678571e6, 2.549341033e6, -3.536478881e6] # [meters]
# Do conversion and check result
# Note: default units are deg
ecef_computed = navpy.lla2ecef(lat, lon, alt)
for e1, e2 in zip(ecef_computed, ecef):
self.assertAlmostEqual(e1, e2, places=2)
def run_filter(filter, imu_data, gps_data, filter_data, config=None):
data_dict = data_store.data_store()
errors = []
# Using while loop starting at k (set to kstart) and going to end
# of .mat file
gps_index = 0
filter_index = 0
new_gps = 0
if config and config['call_init']:
filter_init = False
else:
filter_init = True
if config and 'start_time' in config:
for k, imu_pt in enumerate(imu_data):
if imu_pt.time >= config['start_time']:
k_start = k
break
else:
k_start = 0
if config and 'end_time' in config:
for k, imu_pt in enumerate(imu_data):
if imu_pt.time >= config['end_time']:
k_end = k
break
else:
k_end = len(imu_data)
#print k_start, k_end
for k in range(k_start, k_end):
imupt = imu_data[k]
if gps_index < len(gps_data) - 1:
# walk the gps counter forward as needed
newData = 0
while gps_data[gps_index+1].time - gps_latency <= imupt.time:
gps_index += 1
newData = 1
gpspt = gps_data[gps_index]
gpspt.newData = newData
else:
# no more gps data, stay on the last record
gpspt = gps_data[gps_index]
gpspt.newData = 0
#print gpspt.time`
# walk the filter counter forward as needed
if len(filter_data):
if imupt.time > filter_data[filter_index].time:
filter_index += 1
if filter_index >= len(filter_data):
# no more filter data, stay on the last record
filter_index = len(filter_data)-1
filterpt = filter_data[filter_index]
else:
filterpt = None
#print "t(imu) = " + str(imupt.time) + " t(gps) = " + str(gpspt.time)
# If k is at the initialization time init_nav else get_nav
if not filter_init and gps_index > 0:
print "init:", imupt.time, gpspt.time
navpt = filter.init(imupt, gpspt, filterpt)
filter_init = True
elif filter_init:
navpt = filter.update(imupt, gpspt, filterpt)
# Store the desired results obtained from the compiled test
# navigation filter and the baseline filter
if filter_init:
data_dict.append(navpt)
if gpspt.newData:
# compute error metric with each new gps report
# full 3d distance error (in ecef)
p1 = navpy.lla2ecef(gpspt.lat, gpspt.lon, gpspt.alt,
latlon_unit='deg')
p2 = navpy.lla2ecef(navpt.lat, navpt.lon, navpt.alt,
latlon_unit='rad')
pe = np.linalg.norm(p1 - p2)
# weight horizontal error more highly than vertical error
ref = gps_data[0]
n1 = navpy.lla2ned(gpspt.lat, gpspt.lon, gpspt.alt,
ref.lat, ref.lon, 0.0,
latlon_unit='deg')
n2 = navpy.lla2ned(navpt.lat*r2d, navpt.lon*r2d, navpt.alt,
ref.lat, ref.lon, 0.0,
latlon_unit='deg')
dn = n2 - n1
ne = math.sqrt(dn[0]*dn[0] + dn[1]*dn[1] + dn[2]*dn[2]*0.5)
# it is always tempting to fit to the velocity vector
# (especially when seeing some of the weird velocity fits
# that the optimizer spews out), but it never helps
# ... seems to make the solution convergence much more
# shallow, ultimately never seems to produce a better fit
# than using position directly. Weird fits happen when
# the inertial errors just don't fit the gps errors.
# Fitting to velocity doesn't seem to improve that
# problem.
v1 = np.array( [gpspt.vn, gpspt.ve, gpspt.vd] )
v2 = np.array( [navpt.vn, navpt.ve, navpt.vd] )
ve = np.linalg.norm(v1 - v2)
errors.append(ne) # ned error weighted towards horizontal
# Increment time up one step for the next iteration of the
# while loop.
k += 1
# proper cleanup
filter.close()
return errors, data_dict
def test_angle_transform():
g = 9.80665
static_properties = StaticProperties(
initial_lat=40.44057846069336,
initial_long=79.94245147705078,
initial_alt=302.79998779296875,
initial_roll=0,
initial_pitch=0,
initial_yaw=0,
roll_error=math.radians(-0.05),
pitch_error=math.radians(0.04),
yaw_error=math.radians(1),
initial_v_north=0,
initial_v_east=0,
initial_v_down=0,
initial_attitude_unc=math.radians(1),
initial_velocity_unc=0.1,
initial_position_unc=10,
initial_accel_bias_unc=(g * 1e-3) ** 2,
initial_gyro_bias_unc=math.radians(1 / 3600),
initial_clock_offset_unc=10000,
initial_clock_drift_unc=100,
gyro_noise_PSD=math.radians(0.02 / 60) ** 2,
accel_noise_PSD=200 * (g * 1e-6) ** 2,
accel_bias_PSD=1.0e-7,
gyro_bias_PSD=2.0e-12,
pos_meas_SD=2.5,
vel_meas_SD=0.1,
)
frame_properties = CoordinateFrame(static_properties)
ins = INS(frame_properties)
gyro_measurement = np.matrix([0, 0, math.pi / 2]).T
accel_measurement = np.matrix([1, 0, 0]).T
dt = 0.001
earth_rotation_amount, earth_rotation_matrix = ins.get_earth_rotation_matrix(dt)
mag_angle_change, skew_gyro_body = ins.get_gyro_skew_matrix(dt, gyro_measurement)
estimated_attitude = ins.get_estimated_attitude(
mag_angle_change, skew_gyro_body, earth_rotation_matrix, frame_properties.estimated_attitude
)
average_attitude = ins.get_average_attitude(
mag_angle_change, estimated_attitude, skew_gyro_body, earth_rotation_amount
)
accel_meas_ecef = average_attitude * accel_measurement
print("accel_meas_ecef")
print(accel_meas_ecef, linalg.norm(accel_meas_ecef))
print("estimated_attitude")
print(estimated_attitude, linalg.norm(estimated_attitude))
print("average_attitude")
print(average_attitude, linalg.norm(average_attitude))
print("mag_angle_change")
print(mag_angle_change)
prev_est_p = frame_properties.estimated_position
prev_est_v = frame_properties.estimated_velocity
estimated_velocity = prev_est_v + dt * (
accel_meas_ecef - 2 * skew_symmetric(np.matrix([0, 0, earth_rotation_rate]).T) * prev_est_v
)
# update cartesian position
estimated_position = prev_est_p + (estimated_velocity + prev_est_v) * 0.5 * dt
delta_position = (
estimated_position
- np.matrix(
navpy.lla2ecef(
static_properties.initial_lat, static_properties.initial_long, static_properties.initial_alt
)
).T
)
print("estimated_position")
print(delta_position, linalg.norm(delta_position))
print("estimated_velocity")
print(estimated_velocity, linalg.norm(estimated_velocity))
# 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)
def lla_to_ecef(self, lat, long, alt):
return np.matrix(navpy.lla2ecef(lat, long, alt)).T
def __init__(self):
self.truthRos = Odometry()
self.imuRos = Imu()
self.relPosRos = RelPos()
self.baseGpsRos = PosVelEcef()
self.roverGpsRos = PosVelEcef()
self.rtkCompassRos = RelPos()
self.truth = TruthMsg()
self.roverTruth = TruthMsg()
self.imu = ImuMsg()
self.base2RoverRelPos = RelPosMsg()
self.baseGps = GpsMsg()
self.roverGps = GpsMsg()
self.rtkCompass = GpsCompassMsg
self.accelerometerAccuracyStdDev = 0.025
self.gyroAccuracyStdDev = 0.0023
self.gpsHorizontalAccuracyStdDev = 0.2
self.gpsVerticalAccuracyStdDev = 0.4
self.gpsSpeedAccuracyStdDev = 0.2
self.rtkHorizontalAccuracyStdDev = 0.02
self.rtkVerticalAccuracyStdDev = 0.04
self.rtkCompassAccuracyDegStdDev = 1.0 #depends on baseline. This also uses RTK
#These are to remember noise of previous update. They are used in the low pass filter
self.gpsNoise = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.rtkCompassNoise = 0.0
self.accelerometerBias = [0.3, -0.5, -0.2]
self.gyroBias = [0.01, 0.08, -0.02]
self.imuTs = 1.0 / 200.0
self.gpsTs = 1.0 / 5.0
self.firstCallback = True
self.firstTime = 0.0
self.latRef = 20.24
self.lonRef = -111.66
self.altRef = 1387.0
self.originEcef = navpy.lla2ecef(self.latRef, self.lonRef, self.altRef)
self.gravity = np.array([[0.0, 0.0, 9.81]]).T
self.lowPassFilterAlpha = 0.9
self.truth_pub_ = rospy.Publisher('base_truth',
Odometry,
queue_size=5,
latch=True)
self.imu_pub_ = rospy.Publisher('base_imu',
Imu,
queue_size=5,
latch=True)
self.relPos_pub_ = rospy.Publisher('base2Rover_relPos',
RelPos,
queue_size=5,
latch=True)
self.base_gps_pub_ = rospy.Publisher('base_gps',
PosVelEcef,
queue_size=5,
latch=True)
self.rover_gps_pub_ = rospy.Publisher('rover_gps',
PosVelEcef,
queue_size=5,
latch=True)
self.rtk_compass_pub_ = rospy.Publisher('base_rtk_compass',
RelPos,
queue_size=5,
latch=True)
self.truth_rate_timer_ = rospy.Timer(rospy.Duration(self.imuTs),
self.truthCallback)
self.gps_rate_timer_ = rospy.Timer(rospy.Duration(self.gpsTs),
self.gpsCallback)
while not rospy.is_shutdown():
rospy.spin()
def run_filter(filter, imu_data, gps_data, filter_data, config=None):
data_dict = data_store.data_store()
errors = []
# Using while loop starting at k (set to kstart) and going to end
# of .mat file
gps_index = 0
filter_index = 0
new_gps = 0
if config and config['call_init']:
filter_init = False
else:
filter_init = True
if config and 'start_time' in config:
for k, imu_pt in enumerate(imu_data):
if imu_pt.time >= config['start_time']:
k_start = k
break
else:
k_start = 0
if config and 'end_time' in config:
for k, imu_pt in enumerate(imu_data):
if imu_pt.time >= config['end_time']:
k_end = k
break
else:
k_end = len(imu_data)
#print k_start, k_end
for k in range(k_start, k_end):
imupt = imu_data[k]
if gps_index < len(gps_data) - 1:
# walk the gps counter forward as needed
newData = 0
while gps_data[gps_index+1].time - gps_latency <= imupt.time:
gps_index += 1
newData = 1
gpspt = gps_data[gps_index]
gpspt.newData = newData
else:
# no more gps data, stay on the last record
gpspt = gps_data[gps_index]
gpspt.newData = 0
#print gpspt.time`
# walk the filter counter forward as needed
if len(filter_data):
if imupt.time > filter_data[filter_index].time:
filter_index += 1
if filter_index >= len(filter_data):
# no more filter data, stay on the last record
filter_index = len(filter_data)-1
filterpt = filter_data[filter_index]
else:
filterpt = None
#print "t(imu) = " + str(imupt.time) + " t(gps) = " + str(gpspt.time)
# If k is at the initialization time init_nav else get_nav
if not filter_init and gps_index > 0:
print "init:", imupt.time, gpspt.time
navpt = filter.init(imupt, gpspt, filterpt)
filter_init = True
elif filter_init:
navpt = filter.update(imupt, gpspt, filterpt)
# Store the desired results obtained from the compiled test
# navigation filter and the baseline filter
if filter_init:
data_dict.append(navpt, imupt)
if gpspt.newData:
# compute error metric with each new gps report
# full 3d distance error (in ecef)
p1 = navpy.lla2ecef(gpspt.lat, gpspt.lon, gpspt.alt,
latlon_unit='deg')
p2 = navpy.lla2ecef(navpt.lat, navpt.lon, navpt.alt,
latlon_unit='rad')
pe = np.linalg.norm(p1 - p2)
# weight horizontal error more highly than vertical error
ref = gps_data[0]
n1 = navpy.lla2ned(gpspt.lat, gpspt.lon, gpspt.alt,
ref.lat, ref.lon, 0.0,
latlon_unit='deg')
n2 = navpy.lla2ned(navpt.lat*r2d, navpt.lon*r2d, navpt.alt,
ref.lat, ref.lon, 0.0,
latlon_unit='deg')
dn = n2 - n1
ne = math.sqrt(dn[0]*dn[0] + dn[1]*dn[1] + dn[2]*dn[2]*0.5)
# it is always tempting to fit to the velocity vector
# (especially when seeing some of the weird velocity fits
# that the optimizer spews out), but it never helps
# ... seems to make the solution convergence much more
# shallow, ultimately never seems to produce a better fit
# than using position directly. Weird fits happen when
# the inertial errors just don't fit the gps errors.
# Fitting to velocity doesn't seem to improve that
# problem.
v1 = np.array( [gpspt.vn, gpspt.ve, gpspt.vd] )
v2 = np.array( [navpt.vn, navpt.ve, navpt.vd] )
ve = np.linalg.norm(v1 - v2)
errors.append(ne) # ned error weighted towards horizontal
# Increment time up one step for the next iteration of the
# while loop.
k += 1
# proper cleanup
filter.close()
return errors, data_dict
def update(self, imu, gps, verbose=True):
# Check for validity of GNSS at this time
self.tcpu = imu.time
self.tow = gps.tow
# Test 1: navValid flag and the data is indeed new (new Time of Week)
# Execute Measurement Update if this is true and Test 2 passes
NEW_GNSS_FLAG = ((gps.valid == 0) &
(abs(self.tow - self.last_tow) > 1e-3))
# Test 2: Check if the delta time of the Time of Week and the
# CPU time are consistent
# If this fails, re-initialize the filter. There must have been a glitch
if NEW_GNSS_FLAG:
#print self.tcpu, self.last_tcpu, self.tcpu-self.last_tcpu
#print self.tow, self.last_tow, self.tow-self.last_tow
if abs((self.tcpu - self.last_tcpu) -
(self.tow - self.last_tow)) > 0.5:
self.TU_COUNT = 0
self.NAV_INIT = False
if verbose:
print("Time Sync Error -- Request reinitialization")
# Record the tow and tcpu at this new update
self.last_tow = self.tow
self.last_tcpu = self.tcpu
# Different subroutine executed in the filter
if not self.NAV_INIT:
if not self.IMU_CAL_INIT:
# SUBROUTINE: IMU CALIBRATION,
# This only happens first time round on the ground. Inflight
# reinitialization is not the same.
self.estAB[:] = [0, 0, 0]
if self.very_first_time:
self.very_first_time = False
self.estGB[:] = [imu.p, imu.q, imu.r]
self.phi = 0
self.theta = 0
else:
self.estGB[:] = self.last_estGB[:]*self.TU_COUNT \
+ [imu.p, imu.q, imu.r]
# Simple AHRS values
self.phi = self.phi*self.TU_COUNT \
+ np.arctan2(-imu.ay, -imu.az)
self.theta = self.theta*self.TU_COUNT \
+ np.arctan2(imu.ax, np.sqrt(imu.ay**2+imu.az**2))
self.phi /= (self.TU_COUNT + 1)
self.theta /= (self.TU_COUNT + 1)
self.estGB[:] /= (self.TU_COUNT + 1)
self.estATT[0], self.estATT[1:] = navpy.angle2quat(
0, self.theta, self.phi)
"""
print("t = %7.3f, Gyro Bias Value: [%6.2f, %6.2f, %6.2f] deg/sec" %\
(imu.time, np.rad2deg(self.estGB[0]), np.rad2deg(self.estGB[1]), np.rad2deg(self.estGB[2]) ))
print("t = %7.3f, phi = %6.2f, theta = %6.2f" % (imu.time,np.rad2deg(self.phi),np.rad2deg(self.theta)) )
"""
self.TU_COUNT += 1
if self.TU_COUNT >= 35:
self.TU_COUNT = 0
self.IMU_CAL_INIT = True
if verbose:
print("t = %7.3f, IMU Calibrated!" % (imu.time))
del (self.phi)
del (self.theta)
else:
if not NEW_GNSS_FLAG:
# SUBROUTINE 1: BACKUP NAVIGATION or IN-FLIGHT INITIALIZATION
# >>>> AHRS CODE GOES HERE
self.estATT[:] = self.last_estATT[:] # This should be some backup nav mode
# <<<<
# When still there is no GNSS signal, continue propagating bias
self.estAB[:] = self.last_estAB[:]
self.estGB[:] = self.last_estGB[:]
else:
# When there is GNSS fix available, initialize all the states
# and renew covariance
self.estPOS[:] = [gps.lat, gps.lon, gps.alt]
self.estVEL[:] = [gps.vn, gps.ve, gps.vd]
self.estATT[:] = self.last_estATT[:]
#self.estATT[0], self.estATT[1:] = navpy.angle2quat(flight_data.psi[i],flight_data.theta[i],flight_data.phi[i])
self.estAB[:] = self.last_estAB[:]
self.estGB[:] = self.last_estGB[:]
self.init_covariance()
#idx_init.append(i)
self.NAV_INIT = True
if verbose:
print("t = %7.3f, Filter (Re-)initialized" %
(imu.time))
elif self.NAV_INIT:
# SUBROUTINE 2: MAIN FILTER ALGORITHM, INS + GNSS
# ==== Time-Update ====
dt = imu.time - self.last_imu.time
q0, qvec = self.last_estATT[0], self.last_estATT[1:4]
C_B2N = navpy.quat2dcm(q0, qvec).T
# 0. Data Acquisition
f_b=np.array([0.5*(self.last_imu.ax+imu.ax)-self.last_estAB[0],\
0.5*(self.last_imu.ay+imu.ay)-self.last_estAB[1],\
0.5*(self.last_imu.az+imu.az)-self.last_estAB[2]])
om_ib=np.array([0.5*(self.last_imu.p+imu.p)-self.last_estGB[0],\
0.5*(self.last_imu.q+imu.q)-self.last_estGB[1],\
0.5*(self.last_imu.r+imu.r)-self.last_estGB[2]])
# 1. Attitude Update
# --> Need to compensate for navrate and earthrate
dqvec = 0.5 * om_ib * dt
self.estATT[0], self.estATT[1:4] = navpy.qmult(
q0, qvec, 1.0, dqvec)
self.estATT[0] /= np.sqrt(self.estATT[0]**2 \
+ la.norm(self.estATT[1:4])**2)
self.estATT[1:4] /= np.sqrt(self.estATT[0]**2 \
+ la.norm(self.estATT[1:4])**2)
# 2. Velocity Update
# --> Need to compensate for coriolis effect
g = np.array([0, 0, 9.81])
f0, fvec = navpy.qmult(q0, qvec, 0, f_b)
f_n0, f_nvec = navpy.qmult(f0, fvec, q0, -qvec)
self.estVEL[:] = self.last_estVEL[:] + (f_nvec + g) * dt
# 3. Position Update
dPOS = navpy.llarate(self.last_estVEL[0], self.last_estVEL[1],
self.last_estVEL[2], self.last_estPOS[0],
self.last_estPOS[2])
dPOS *= dt
self.estPOS[:] = self.last_estPOS[:] + dPOS
# 4. Biases are constant
self.estAB[:] = self.last_estAB[:]
self.estGB[:] = self.last_estGB[:]
# 5. Jacobian
pos2pos = np.zeros((3, 3))
pos2gs = np.eye(3)
pos2att = np.zeros((3, 3))
pos2acc = np.zeros((3, 3))
pos2gyr = np.zeros((3, 3))
gs2pos = np.zeros((3, 3))
gs2pos[2, 2] = -2 * 9.81 / wgs84.R0
gs2gs = np.zeros((3, 3))
gs2att = -2 * C_B2N.dot(navpy.skew(f_b))
gs2acc = -C_B2N
gs2gyr = np.zeros((3, 3))
att2pos = np.zeros((3, 3))
att2gs = np.zeros((3, 3))
att2att = -navpy.skew(om_ib)
att2acc = np.zeros((3, 3))
att2gyr = -0.5 * np.eye(3)
F = np.zeros((15, 15))
F[0:3, 0:3] = pos2pos
F[0:3, 3:6] = pos2gs
F[0:3, 6:9] = pos2att
F[0:3, 9:12] = pos2acc
F[0:3, 12:15] = pos2gyr
F[3:6, 0:3] = gs2pos
F[3:6, 3:6] = gs2gs
F[3:6, 6:9] = gs2att
F[3:6, 9:12] = gs2acc
F[3:6, 12:15] = gs2gyr
F[6:9, 0:3] = att2pos
F[6:9, 3:6] = att2gs
F[6:9, 6:9] = att2att
F[6:9, 9:12] = att2acc
F[6:9, 12:15] = att2gyr
F[9:12, 9:12] = -1.0 / self.tau_a * np.eye(3)
F[12:15, 12:15] = -1.0 / self.tau_g * np.eye(3)
PHI = np.eye(15) + F * dt
# 6. Process Noise
G = np.zeros((15, 12))
G[3:6, 0:3] = -C_B2N
G[6:9, 3:6] = att2gyr
G[9:12, 6:9] = np.eye(3)
G[12:15, 9:12] = np.eye(3)
Q = G.dot(self.Rw.dot(G.T)) * dt
# 7. Covariance Update
self.P = PHI.dot(self.P.dot(PHI.T)) + Q
self.TU_COUNT += 1
if self.TU_COUNT >= 500:
# Request reinitialization after 12 seconds of no GNSS updates
self.TU_COUNT = 0
self.NAV_INIT = False
if NEW_GNSS_FLAG:
# ==== Measurement-Update ====
# 0. Get measurement and make innovations
ecef_ref = navpy.lla2ecef(self.estPOS[0], self.estPOS[1], 0)
ins_ecef = navpy.lla2ecef(self.estPOS[0], self.estPOS[1],
self.estPOS[2])
gnss_ecef = navpy.lla2ecef(gps.lat, gps.lon, gps.alt)
ins_ned = navpy.ecef2ned(ins_ecef - ecef_ref, self.estPOS[0],
self.estPOS[1], self.estPOS[2])
gnss_ned = navpy.ecef2ned(gnss_ecef - ecef_ref, self.estPOS[0],
self.estPOS[1], self.estPOS[2])
dpos = gnss_ned - ins_ned
gnss_vel = np.array([gps.vn, gps.ve, gps.vd])
dvel = gnss_vel - self.estVEL[:]
dy = np.hstack((dpos, dvel))
self.stateInnov[:] = dy
# 1. Kalman Gain
K = self.P.dot(self.H.T)
K = K.dot(la.inv(self.H.dot(self.P.dot(self.H.T)) + self.R))
# 2. Covariance Update
ImKH = np.eye(15) - K.dot(self.H)
KRKt = K.dot(self.R.dot(K.T))
self.P = ImKH.dot(self.P.dot(ImKH.T)) + KRKt
# 3. State Update
dx = K.dot(dy)
Rew, Rns = navpy.earthrad(self.estPOS[0])
self.estPOS[2] -= dx[2]
self.estPOS[0] += np.rad2deg(dx[0] / (Rew + self.estPOS[2]))
self.estPOS[1] += np.rad2deg(
dx[1] / (Rns + self.estPOS[2]) /
np.cos(np.deg2rad(self.estPOS[0])))
self.estVEL[:] += dx[3:6]
self.estATT[0], self.estATT[1:4] = navpy.qmult(
self.estATT[0], self.estATT[1:4], 1, dx[6:9])
self.estATT[0] /= np.sqrt(self.estATT[0]**2 \
+ la.norm(self.estATT[1:4])**2)
self.estATT[1:4] /= np.sqrt(self.estATT[0]**2 \
+ la.norm(self.estATT[1:4])**2)
self.estAB[:] += dx[9:12]
self.estGB[:] += dx[12:15]
if verbose:
print("t = %7.3f, GNSS Update, self.TU_COUNT = %d" %\
(gps.time, self.TU_COUNT) )
self.TU_COUNT = 0
self.last_estPOS[:] = self.estPOS[:]
self.last_estVEL[:] = self.estVEL[:]
self.last_estATT[:] = self.estATT[:]
self.last_estAB[:] = self.estAB[:]
self.last_estGB[:] = self.estGB[:]
else:
# SUBROUTINE 3: e.g. BACKUP NAVIGATION MODE
pass
self.last_imu = imu
self.last_gps = gps
result = INSGPS(self.NAV_INIT, imu.time, self.estPOS, self.estVEL,
self.estATT, self.estAB, self.estGB, self.P,
self.stateInnov)
return result
print("Problem #1:")
print("-----------")
lat = 30
# deg
lon = 90
# deg
alt = 0
earth_gm = 3.986004419e14
# Part a
ecef_sphere = nav.lla2ecef(lat,
lon,
alt,
latlon_unit='deg',
alt_unit='m',
model='xxx')
# Part b
ecef_wgs84 = nav.lla2ecef(lat,
lon,
alt,
latlon_unit='deg',
alt_unit='m',
model='wgs84')
ecef_diff = ecef_wgs84 - ecef_sphere
file = open('Problem1ab.csv', 'w')
csvWriter = csv.writer(file, lineterminator='\n')
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)
gnss_ned = navpy.ecef2ned(ecef-ecef_ref,lat_ref,lon_ref,alt_ref)
gnss_ned[0:idx_init[0],:] = np.nan
ecef = navpy.lla2ecef(flight_data.navlat,flight_data.navlon,flight_data.navalt,latlon_unit='rad')
ref_ned = navpy.ecef2ned(ecef-ecef_ref,lat_ref,lon_ref,alt_ref)
ref_ned[0:idx_init[0],:] = np.nan
# ============================= INS PLOTS ======================================
nsig = 3
istart = idx_init[0]
# 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]
print "idx_init[0]: ", idx_init[0]
print "len(estPOS): ", len(estPOS)
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)
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,
)
def lla_to_ecef(self, lat, long, alt):
return np.matrix(navpy.lla2ecef(lat, long, alt)).T
def update(self, imu, gps, verbose=True):
# Check for validity of GNSS at this time
self.tcpu = imu.time
self.tow = gps.tow
# Test 1: navValid flag and the data is indeed new (new Time of Week)
# Execute Measurement Update if this is true and Test 2 passes
NEW_GNSS_FLAG = ((gps.valid==0) & (abs(self.tow-self.last_tow) > 1e-3))
# Test 2: Check if the delta time of the Time of Week and the
# CPU time are consistent
# If this fails, re-initialize the filter. There must have been a glitch
if NEW_GNSS_FLAG:
#print self.tcpu, self.last_tcpu, self.tcpu-self.last_tcpu
#print self.tow, self.last_tow, self.tow-self.last_tow
if abs((self.tcpu-self.last_tcpu) - (self.tow-self.last_tow)) > 0.5:
self.TU_COUNT = 0
self.NAV_INIT = False
if verbose:
print("Time Sync Error -- Request reinitialization")
# Record the tow and tcpu at this new update
self.last_tow = self.tow
self.last_tcpu = self.tcpu
# Different subroutine executed in the filter
if not self.NAV_INIT:
if not self.IMU_CAL_INIT:
# SUBROUTINE: IMU CALIBRATION,
# This only happens first time round on the ground. Inflight
# reinitialization is not the same.
self.estAB[:] = [0,0,0]
if self.very_first_time:
self.very_first_time = False
self.estGB[:] = [imu.p, imu.q, imu.r]
self.phi = 0
self.theta = 0
else:
self.estGB[:] = self.last_estGB[:]*self.TU_COUNT \
+ [imu.p, imu.q, imu.r]
# Simple AHRS values
self.phi = self.phi*self.TU_COUNT \
+ np.arctan2(-imu.ay, -imu.az)
self.theta = self.theta*self.TU_COUNT \
+ np.arctan2(imu.ax, np.sqrt(imu.ay**2+imu.az**2))
self.phi /= (self.TU_COUNT + 1)
self.theta /= (self.TU_COUNT + 1)
self.estGB[:] /= (self.TU_COUNT + 1)
self.estATT[0], self.estATT[1:] = navpy.angle2quat(0, self.theta, self.phi)
"""
print("t = %7.3f, Gyro Bias Value: [%6.2f, %6.2f, %6.2f] deg/sec" %\
(imu.time, np.rad2deg(self.estGB[0]), np.rad2deg(self.estGB[1]), np.rad2deg(self.estGB[2]) ))
print("t = %7.3f, phi = %6.2f, theta = %6.2f" % (imu.time,np.rad2deg(self.phi),np.rad2deg(self.theta)) )
"""
self.TU_COUNT += 1
if self.TU_COUNT >= 35:
self.TU_COUNT = 0
self.IMU_CAL_INIT = True
if verbose:
print("t = %7.3f, IMU Calibrated!" % (imu.time))
del(self.phi)
del(self.theta)
else:
if not NEW_GNSS_FLAG:
# SUBROUTINE 1: BACKUP NAVIGATION or IN-FLIGHT INITIALIZATION
# >>>> AHRS CODE GOES HERE
self.estATT[:] = self.last_estATT[:] # This should be some backup nav mode
# <<<<
# When still there is no GNSS signal, continue propagating bias
self.estAB[:] = self.last_estAB[:]
self.estGB[:] = self.last_estGB[:]
else:
# When there is GNSS fix available, initialize all the states
# and renew covariance
self.estPOS[:] = [gps.lat, gps.lon, gps.alt]
self.estVEL[:] = [gps.vn, gps.ve, gps.vd]
self.estATT[:] = self.last_estATT[:]
#self.estATT[0], self.estATT[1:] = navpy.angle2quat(flight_data.psi[i],flight_data.theta[i],flight_data.phi[i])
self.estAB[:] = self.last_estAB[:]
self.estGB[:] = self.last_estGB[:]
self.init_covariance()
#idx_init.append(i)
self.NAV_INIT = True
if verbose:
print("t = %7.3f, Filter (Re-)initialized" % (imu.time) )
elif self.NAV_INIT:
# SUBROUTINE 2: MAIN FILTER ALGORITHM, INS + GNSS
# ==== Time-Update ====
dt = imu.time - self.last_imu.time
q0, qvec = self.last_estATT[0], self.last_estATT[1:4]
C_B2N = navpy.quat2dcm(q0,qvec).T
# 0. Data Acquisition
f_b=np.array([0.5*(self.last_imu.ax+imu.ax)-self.last_estAB[0],\
0.5*(self.last_imu.ay+imu.ay)-self.last_estAB[1],\
0.5*(self.last_imu.az+imu.az)-self.last_estAB[2]])
om_ib=np.array([0.5*(self.last_imu.p+imu.p)-self.last_estGB[0],\
0.5*(self.last_imu.q+imu.q)-self.last_estGB[1],\
0.5*(self.last_imu.r+imu.r)-self.last_estGB[2]])
# 1. Attitude Update
# --> Need to compensate for navrate and earthrate
dqvec = 0.5*om_ib*dt
self.estATT[0], self.estATT[1:4] = navpy.qmult(q0,qvec,1.0,dqvec)
self.estATT[0] /= np.sqrt(self.estATT[0]**2 \
+ la.norm(self.estATT[1:4])**2)
self.estATT[1:4] /= np.sqrt(self.estATT[0]**2 \
+ la.norm(self.estATT[1:4])**2)
# 2. Velocity Update
# --> Need to compensate for coriolis effect
g = np.array([0,0,9.81])
f0, fvec = navpy.qmult(q0,qvec,0,f_b)
f_n0,f_nvec = navpy.qmult(f0,fvec,q0,-qvec)
self.estVEL[:] = self.last_estVEL[:] + (f_nvec+g)*dt
# 3. Position Update
dPOS = navpy.llarate(self.last_estVEL[0], self.last_estVEL[1],
self.last_estVEL[2], self.last_estPOS[0],
self.last_estPOS[2])
dPOS *= dt
self.estPOS[:] = self.last_estPOS[:] + dPOS
# 4. Biases are constant
self.estAB[:] = self.last_estAB[:]
self.estGB[:] = self.last_estGB[:]
# 5. Jacobian
pos2pos = np.zeros((3,3))
pos2gs = np.eye(3)
pos2att = np.zeros((3,3))
pos2acc = np.zeros((3,3))
pos2gyr = np.zeros((3,3))
gs2pos = np.zeros((3,3))
gs2pos[2,2] = -2*9.81/wgs84.R0
gs2gs = np.zeros((3,3))
gs2att = -2*C_B2N.dot(navpy.skew(f_b))
gs2acc = -C_B2N
gs2gyr = np.zeros((3,3))
att2pos = np.zeros((3,3))
att2gs = np.zeros((3,3))
att2att = -navpy.skew(om_ib)
att2acc = np.zeros((3,3))
att2gyr = -0.5*np.eye(3)
F = np.zeros((15,15))
F[0:3,0:3] = pos2pos
F[0:3,3:6] = pos2gs
F[0:3,6:9] = pos2att
F[0:3,9:12] = pos2acc
F[0:3,12:15] = pos2gyr
F[3:6,0:3] = gs2pos
F[3:6,3:6] = gs2gs
F[3:6,6:9] = gs2att
F[3:6,9:12] = gs2acc
F[3:6,12:15] = gs2gyr
F[6:9,0:3] = att2pos
F[6:9,3:6] = att2gs
F[6:9,6:9] = att2att
F[6:9,9:12] = att2acc
F[6:9,12:15] = att2gyr
F[9:12,9:12] = -1.0/self.tau_a*np.eye(3)
F[12:15,12:15] = -1.0/self.tau_g*np.eye(3)
PHI = np.eye(15) + F*dt
# 6. Process Noise
G = np.zeros((15,12))
G[3:6,0:3] = -C_B2N
G[6:9,3:6] = att2gyr
G[9:12,6:9] = np.eye(3)
G[12:15,9:12] = np.eye(3)
Q = G.dot(self.Rw.dot(G.T))*dt
# 7. Covariance Update
self.P = PHI.dot(self.P.dot(PHI.T)) + Q
self.TU_COUNT += 1
if self.TU_COUNT >= 500:
# Request reinitialization after 12 seconds of no GNSS updates
self.TU_COUNT = 0
self.NAV_INIT = False
if NEW_GNSS_FLAG:
# ==== Measurement-Update ====
# 0. Get measurement and make innovations
ecef_ref = navpy.lla2ecef(self.estPOS[0], self.estPOS[1], 0)
ins_ecef = navpy.lla2ecef(self.estPOS[0], self.estPOS[1],
self.estPOS[2])
gnss_ecef = navpy.lla2ecef(gps.lat, gps.lon, gps.alt)
ins_ned = navpy.ecef2ned(ins_ecef-ecef_ref, self.estPOS[0],
self.estPOS[1], self.estPOS[2])
gnss_ned = navpy.ecef2ned(gnss_ecef-ecef_ref, self.estPOS[0],
self.estPOS[1], self.estPOS[2])
dpos = gnss_ned - ins_ned
gnss_vel = np.array([gps.vn, gps.ve, gps.vd])
dvel = gnss_vel - self.estVEL[:]
dy = np.hstack((dpos, dvel))
self.stateInnov[:] = dy
# 1. Kalman Gain
K = self.P.dot(self.H.T)
K = K.dot( la.inv(self.H.dot(self.P.dot(self.H.T)) + self.R) )
# 2. Covariance Update
ImKH = np.eye(15) - K.dot(self.H)
KRKt = K.dot(self.R.dot(K.T))
self.P = ImKH.dot(self.P.dot(ImKH.T)) + KRKt
# 3. State Update
dx = K.dot(dy)
Rew, Rns = navpy.earthrad(self.estPOS[0])
self.estPOS[2] -= dx[2]
self.estPOS[0] += np.rad2deg(dx[0]/(Rew + self.estPOS[2]))
self.estPOS[1] += np.rad2deg(dx[1]/(Rns + self.estPOS[2])/np.cos(np.deg2rad(self.estPOS[0])))
self.estVEL[:] += dx[3:6]
self.estATT[0], self.estATT[1:4] = navpy.qmult(self.estATT[0], self.estATT[1:4], 1, dx[6:9])
self.estATT[0] /= np.sqrt(self.estATT[0]**2 \
+ la.norm(self.estATT[1:4])**2)
self.estATT[1:4] /= np.sqrt(self.estATT[0]**2 \
+ la.norm(self.estATT[1:4])**2)
self.estAB[:] += dx[9:12]
self.estGB[:] += dx[12:15]
if verbose:
print("t = %7.3f, GNSS Update, self.TU_COUNT = %d" %\
(gps.time, self.TU_COUNT) )
self.TU_COUNT = 0
self.last_estPOS[:] = self.estPOS[:]
self.last_estVEL[:] = self.estVEL[:]
self.last_estATT[:] = self.estATT[:]
self.last_estAB[:] = self.estAB[:]
self.last_estGB[:] = self.estGB[:]
else:
# SUBROUTINE 3: e.g. BACKUP NAVIGATION MODE
pass
self.last_imu = imu
self.last_gps = gps
result = INSGPS( self.NAV_INIT, imu.time, self.estPOS, self.estVEL,
self.estATT, self.estAB, self.estGB,
self.P, self.stateInnov )
return result
