def compute_rover_gps(roverTruth, gps, latRef, lonRef, altRef, originEcef):
gps.lla = navpy.ned2lla(roverTruth.position, latRef, lonRef, altRef)
ecefPositionRelative = navpy.ned2ecef(roverTruth.position, latRef, lonRef,
altRef)
gps.positionEcef = ecefPositionRelative + originEcef
gps.velocityEcef = navpy.ned2ecef(roverTruth.velocity, latRef, lonRef,
altRef)
gps.fix = 3
def update_rover_virtual_PosVelEcef(self, dt):
#calculate virtual position in ecef frame with noise
self.rover_ned_noise = self.add_gps_noise(self.white_noise_3d,
self.rover_ned_noise,
self.sigma_rover_pos)
rover_ned_w_noise = self.rover_ned + self.rover_ned_noise
self.rover_virtual_lla = navpy.ned2lla(rover_ned_w_noise,
self.ref_lla[0],
self.ref_lla[1],
self.ref_lla[2])
self.rover_virtual_pos_ecef = navpy.lla2ecef(self.rover_virtual_lla[0],
self.rover_virtual_lla[1],
self.rover_virtual_lla[2])
#calculate virtual velocity in ecef frame with noise
#make sure we do not divide by zero
if dt != 0.0:
rover_vel = (self.rover_ned - self.rover_ned_prev) / dt
else:
rover_vel = np.zeros(3)
self.rover_vel_noise = self.add_noise_3d(np.zeros(3),
self.white_noise_3d)
self.rover_vel_lpf = self.lpf(self.rover_vel_noise,
self.rover_vel_noise_prev, self.Ts,
self.sigma_rover_vel)
rover_vel_w_noise = rover_vel + self.rover_vel_lpf
self.rover_virtual_vel_ecef = navpy.ned2ecef(
rover_vel_w_noise, self.ref_lla[0], self.ref_lla[1],
self.ref_lla[2]) #self.ned2ecef(rover_vel_w_noise, self.ref_lla)
#update histories
self.rover_ned_prev = self.rover_ned
self.rover_vel_prev = rover_vel
self.rover_vel_noise_prev = self.rover_vel_noise
def test_ned2ecef(self):
"""
Test conversion from NED to ECEF.
Data Source: derived from "test_ecef2ned" above.
"""
# A point near Los Angeles, CA, given from equation 2.12 [degrees]
lat = +(34. + 0. / 60 + 0.00174 / 3600) # North
lon = -(117. + 20. / 60 + 0.84965 / 3600) # West
alt = 251.702 # [meters]
# Define example ECEF vector and associated NED, given in Example 2.4
ecef = np.array([0.3808, 0.7364, -0.5592])
ned = np.array([0, 0, 1]) # local unit gravity
# Do conversion and check result
# Note: default assumption on units is deg and meters
ecef_computed = navpy.ned2ecef(ned, lat, lon, alt)
for e1, e2 in zip(ecef_computed, ecef):
self.assertAlmostEqual(e1, e2, places=3)
def add_gps_noise(gps, gpsHorizontalAccuracy, gpsVerticalAccuracy,
gpsSpeedAccuracy, latRef, lonRef, altRef, alpha, gpsNoise):
gpsEcefAccuracy = navpy.ned2ecef(
[gpsHorizontalAccuracy, gpsHorizontalAccuracy, gpsVerticalAccuracy],
latRef, lonRef, altRef)
whiteNoisePositionEcef = [0] * 3
whiteNoiseVelocityEcef = [0] * 3
for i in range(3):
whiteNoisePositionEcef[i] = np.random.normal(0.0, gpsEcefAccuracy[i])
whiteNoiseVelocityEcef[i] = np.random.normal(0.0, gpsSpeedAccuracy)
gpsNoise[i] = low_pass_filter(gpsNoise[i], whiteNoisePositionEcef[i],
alpha)
gpsNoise[i + 3] = low_pass_filter(gpsNoise[i + 3],
whiteNoiseVelocityEcef[i], alpha)
gps.positionEcef[0] = gps.positionEcef[0] + gpsNoise[0]
gps.positionEcef[1] = gps.positionEcef[1] + gpsNoise[1]
gps.positionEcef[2] = gps.positionEcef[2] + gpsNoise[2]
gps.velocityEcef[0] = gps.velocityEcef[0] + gpsNoise[3]
gps.velocityEcef[1] = gps.velocityEcef[1] + gpsNoise[4]
gps.velocityEcef[2] = gps.velocityEcef[2] + gpsNoise[5]
def test_ned2ecef(self):
"""
Test conversion from NED to ECEF.
Data Source: derived from "test_ecef2ned" above.
"""
# A point near Los Angeles, CA, given from equation 2.12 [degrees]
lat = +( 34. + 0./60 + 0.00174/3600) # North
lon = -(117. + 20./60 + 0.84965/3600) # West
alt = 251.702 # [meters]
# Define example ECEF vector and associated NED, given in Example 2.4
ecef = np.array([0.3808, 0.7364, -0.5592])
ned = np.array([0, 0, 1]) # local unit gravity
# Do conversion and check result
# Note: default assumption on units is deg and meters
ecef_computed = navpy.ned2ecef(ned, lat, lon, alt)
for e1, e2 in zip(ecef_computed, ecef):
self.assertAlmostEqual(e1, e2, places=3)
def igrffx(eci_vec, time):
#import igrf12
import numpy
import pyIGRF
import pymap3d
from pymap3d import ecef2eci
import navpy
from navpy import ned2ecef
import datetime
from datetime import datetime
import astropy
#eci_vec is a xyz vector in ECI km
#output B_ECI is a 3 item array in units of T
#get our lat long and alt from ECI
geod = pymap3d.eci2geodetic(1000 * eci_vec, time, useastropy=True)
latitude = geod[0][0] #degrees
longitude = geod[1][0] #degrees
altitude = geod[2] #meters
#call igrf to get b vector in NED
#mag = igrf12.igrf('2019-01-12', glat=latitude, glon=longitude, alt_km=altitude/1000)
b = pyIGRF.igrf_value(latitude, longitude, altitude / 1000, 2019)
#combine NED components back into an array
#NED = numpy.array([b_north,b_east,b_down])
NED = b[3:6]
#convert from NED to ECEF
ECEF = navpy.ned2ecef(NED, latitude, longitude, altitude)
#convert from ECEF to ECI
B_ECI = (pymap3d.ecef2eci(ECEF, time, useastropy=True)) * 10**(-9)
return B_ECI
def ned_to_ecef(self, north, east, down):
return np.matrix(navpy.ned2ecef(
(north, east, down),
self.lat_ref, self.long_ref, self.alt_ref)).T
def compute_base_gps(truth, gps, latRef, lonRef, altRef, originEcef):
Rb2i = R.from_euler('xyz', truth.orientation.squeeze())
trueVelocityNed = Rb2i.apply(truth.velocity.T).T
gps.velocityEcef = navpy.ned2ecef(trueVelocityNed, latRef, lonRef, altRef)
gps.fix = 3
def ned_to_ecef(self, north, east, down):
return np.matrix(navpy.ned2ecef((north, east, down), self.lat_ref, self.long_ref, self.alt_ref)).T
