230 lines
7.3 KiB
Python
230 lines
7.3 KiB
Python
|
#!/usr/bin/env python3
|
||
|
|
import stdproc
|
||
|
|
import datetime
|
||
|
|
from .Orbit import StateVector
|
||
|
|
from isceobj.Util.geo.ellipsoid import Ellipsoid
|
||
|
|
from iscesys.StdOEL.StdOELPy import create_writer
|
||
|
|
from isceobj.Location.Peg import Peg
|
||
|
|
from iscesys.Component.Component import Component
|
||
|
|
from isceobj.Planet.Planet import Planet
|
||
|
|
from iscesys.DateTimeUtil.DateTimeUtil import DateTimeUtil as DTUtil
|
||
|
|
import numpy as np
|
||
|
|
|
||
|
|
|
||
|
|
NEW_POINTS = Component.Parameter('_newPoints',
|
||
|
|
public_name='NEW_POINTS',
|
||
|
|
default = 2,
|
||
|
|
type = int,
|
||
|
|
mandatory=False,
|
||
|
|
doc = 'Number of points to add the start and end of current orbit')
|
||
|
|
|
||
|
|
POLYNOMIAL_ORDER = Component.Parameter('_polyOrder',
|
||
|
|
public_name='POLYNOMIAL_ORDER',
|
||
|
|
default=2,
|
||
|
|
type=int,
|
||
|
|
mandatory=False,
|
||
|
|
doc='Order of the polynomial to use for SCH interpolation')
|
||
|
|
|
||
|
|
|
||
|
|
def diffOrbits(orig, new, skip=2):
|
||
|
|
'''
|
||
|
|
Compute statistics between old and new orbits.
|
||
|
|
Points in the middle were transformed from WGS84 -> SCH -> WGS84.
|
||
|
|
This reports the error in the transformation.
|
||
|
|
'''
|
||
|
|
|
||
|
|
oldnum = len(orig._stateVectors)
|
||
|
|
res = np.zeros((oldnum, 6))
|
||
|
|
|
||
|
|
for kk,sv in enumerate(orig):
|
||
|
|
newsv = new[kk+skip]
|
||
|
|
res[kk,0:3] = np.array(sv.getPosition()) - np.array(newsv.getPosition())
|
||
|
|
res[kk,3:6] = np.array(sv.getVelocity()) - np.array(newsv.getVelocity())
|
||
|
|
|
||
|
|
print('RMS error from interpolation: ')
|
||
|
|
print(np.sqrt(np.mean(res*res, axis=0)))
|
||
|
|
|
||
|
|
class OrbitExtender(Component):
|
||
|
|
'''
|
||
|
|
Code to extrapolate WGS84 orbits by a few points. Orbit is transformed in to a SCH coordinate system and transferred back to WGS84.'''
|
||
|
|
|
||
|
|
family = 'orbitextender'
|
||
|
|
logging_name='isceobj.orbitextender'
|
||
|
|
_planet = None
|
||
|
|
parameter_list = (NEW_POINTS,
|
||
|
|
POLYNOMIAL_ORDER)
|
||
|
|
|
||
|
|
|
||
|
|
def __init__(self, name='', num=None, order=None, planet=None):
|
||
|
|
super(OrbitExtender,self).__init__(family=self.__class__.family, name=name)
|
||
|
|
if num is not None:
|
||
|
|
self._newPoints = int(num)
|
||
|
|
|
||
|
|
if order is not None:
|
||
|
|
self._polyOrder = int(order)
|
||
|
|
|
||
|
|
if planet is not None:
|
||
|
|
self._planet = planet
|
||
|
|
else:
|
||
|
|
self._planet = Planet(pname='Earth')
|
||
|
|
|
||
|
|
def getPegAndHeading(self, orbit, midTime, delta=5000):
|
||
|
|
'''Compute the heading of the satellite and peg lat, lon'''
|
||
|
|
|
||
|
|
|
||
|
|
refElp = Ellipsoid(a=self._planet.ellipsoid.a, e2=self._planet.ellipsoid.e2, model='WGS84')
|
||
|
|
|
||
|
|
#Position just before mid Time
|
||
|
|
t1 = midTime - datetime.timedelta(microseconds=delta)
|
||
|
|
vec1 = orbit.interpolate(t1, method='hermite')
|
||
|
|
|
||
|
|
#Position just after midTime
|
||
|
|
t2 = midTime + datetime.timedelta(microseconds=delta)
|
||
|
|
vec2 = orbit.interpolate(t2, method='hermite')
|
||
|
|
|
||
|
|
pos = vec1.getPosition()
|
||
|
|
pt1 = refElp.ECEF(pos[0], pos[1], pos[2])
|
||
|
|
pos = vec2.getPosition()
|
||
|
|
pt2 = refElp.ECEF(pos[0], pos[1], pos[2])
|
||
|
|
|
||
|
|
llh1 = pt1.llh()
|
||
|
|
llh2 = pt2.llh()
|
||
|
|
|
||
|
|
#Heading
|
||
|
|
hdg = pt1.bearing(pt2)
|
||
|
|
|
||
|
|
#Average lat lon
|
||
|
|
peg = refElp.LLH(0.5*(llh1.lat + llh2.lat), 0.5*(llh1.lon + llh2.lon), 0.5*(llh1.hgt+llh2.hgt))
|
||
|
|
return peg, hdg
|
||
|
|
|
||
|
|
def getSCHOrbit(self, orbit, peg, hdg):
|
||
|
|
'''
|
||
|
|
Accepts a WGS-84 orbit and converts it to SCH.
|
||
|
|
'''
|
||
|
|
writer = create_writer("log","",True,filename='orbit_extender.log')
|
||
|
|
llh = [peg.lat, peg.lon, peg.hgt]
|
||
|
|
|
||
|
|
####Local radius
|
||
|
|
radius = self._planet.ellipsoid.radiusOfCurvature(llh, hdg=hdg)
|
||
|
|
|
||
|
|
#midPeg is a Location.Peg object
|
||
|
|
midPeg = Peg(latitude = peg.lat,
|
||
|
|
longitude = peg.lon,
|
||
|
|
heading = hdg,
|
||
|
|
radiusOfCurvature = radius)
|
||
|
|
|
||
|
|
|
||
|
|
orbSch = stdproc.createOrbit2sch(averageHeight = peg.hgt)
|
||
|
|
orbSch.setStdWriter(writer)
|
||
|
|
orbSch(planet=self._planet, orbit=orbit, peg=midPeg)
|
||
|
|
|
||
|
|
return orbSch.orbit
|
||
|
|
|
||
|
|
def extendSCHOrbit(self, orbit):
|
||
|
|
'''
|
||
|
|
Extends a given SCH orbit by _newPoints and using a
|
||
|
|
polynomial of order _polyOrder.
|
||
|
|
'''
|
||
|
|
|
||
|
|
lenv = len(orbit)
|
||
|
|
|
||
|
|
t = np.zeros(lenv)
|
||
|
|
pos = np.zeros((lenv,6))
|
||
|
|
|
||
|
|
t0 = orbit[0].getTime()
|
||
|
|
|
||
|
|
####Read in data in to numpy arrays
|
||
|
|
for kk,sv in enumerate(orbit):
|
||
|
|
t[kk] = float((sv.getTime()-t0).total_seconds())
|
||
|
|
pos[kk,0:3] = sv.getPosition()
|
||
|
|
pos[kk,3:6] = sv.getVelocity()
|
||
|
|
|
||
|
|
####Interpolation at top of the array
|
||
|
|
delta = t[1] - t[0]
|
||
|
|
ttop = delta*np.arange(-self._newPoints,0)
|
||
|
|
toppos = np.zeros((self._newPoints,6))
|
||
|
|
|
||
|
|
x = t[0:self._polyOrder+1]
|
||
|
|
y = pos[0:self._polyOrder+1,:]
|
||
|
|
|
||
|
|
###Simple polynomial interpolation for each coordinate
|
||
|
|
for kk in range(6):
|
||
|
|
toppoly = np.polyfit(x,y[:,kk],self._polyOrder)
|
||
|
|
toppos[:,kk] = np.polyval(toppoly, ttop)
|
||
|
|
|
||
|
|
for kk in range(self._newPoints):
|
||
|
|
sv = StateVector()
|
||
|
|
sv.setTime(t0 + datetime.timedelta(seconds=ttop[kk]))
|
||
|
|
sv.setPosition(list(toppos[kk,0:3]))
|
||
|
|
sv.setVelocity(list(toppos[kk,3:6]))
|
||
|
|
orbit._stateVectors.insert(kk,sv)
|
||
|
|
|
||
|
|
orbit._minTime = orbit[0].getTime()
|
||
|
|
|
||
|
|
|
||
|
|
###Interpolate at the bottom
|
||
|
|
delta = t[-1] - t[-2]
|
||
|
|
tbot = t[-1] + delta* np.arange(1, self._newPoints+1)
|
||
|
|
botpos = np.zeros((self._newPoints,6))
|
||
|
|
|
||
|
|
x = t[-self._polyOrder-1:]
|
||
|
|
y = pos[-self._polyOrder-1:,:]
|
||
|
|
for kk in range(6):
|
||
|
|
botpoly = np.polyfit(x,y[:,kk],self._polyOrder)
|
||
|
|
botpos[:,kk] = np.polyval(botpoly,tbot)
|
||
|
|
|
||
|
|
for kk in range(self._newPoints):
|
||
|
|
sv = StateVector()
|
||
|
|
sv.setTime(t0 + datetime.timedelta(seconds=tbot[kk]))
|
||
|
|
sv.setPosition(list(botpos[kk,0:3]))
|
||
|
|
sv.setVelocity(list(botpos[kk,3:6]))
|
||
|
|
orbit._stateVectors.append(sv)
|
||
|
|
|
||
|
|
orbit._maxTime = orbit[-1].getTime()
|
||
|
|
|
||
|
|
return
|
||
|
|
|
||
|
|
def getXYZOrbit(self, orbit, peg, hdg):
|
||
|
|
'''
|
||
|
|
Convert an input SCH orbit to XYZ coords.
|
||
|
|
'''
|
||
|
|
llh = [peg.lat, peg.lon, peg.hgt]
|
||
|
|
radius = self._planet.ellipsoid.radiusOfCurvature(llh, hdg=hdg)
|
||
|
|
|
||
|
|
midPeg = Peg(latitude=peg.lat,
|
||
|
|
longitude=peg.lon,
|
||
|
|
heading=hdg,
|
||
|
|
radiusOfCurvature=radius)
|
||
|
|
writer = create_writer("log","",True,filename='orbit_extender.log')
|
||
|
|
orbxyz = stdproc.createSch2orbit()
|
||
|
|
orbxyz.radiusOfCurvature = radius
|
||
|
|
orbxyz.setStdWriter(writer)
|
||
|
|
orbxyz(planet=self._planet, orbit=orbit, peg=midPeg)
|
||
|
|
return orbxyz.orbit
|
||
|
|
|
||
|
|
|
||
|
|
def extendOrbit(self, orbit):
|
||
|
|
'''
|
||
|
|
Input orbit must be WGS-84.
|
||
|
|
'''
|
||
|
|
|
||
|
|
deltaT = DTUtil.timeDeltaToSeconds(orbit.maxTime - orbit.minTime)/2.0
|
||
|
|
midTime = orbit.minTime + datetime.timedelta(microseconds=int(deltaT*1e6))
|
||
|
|
|
||
|
|
#pegCoord is an Util.geo coordinate object
|
||
|
|
pegCoord, hdg = self.getPegAndHeading(orbit, midTime)
|
||
|
|
|
||
|
|
####Sch orbit w.r.t mid point of orbit
|
||
|
|
schOrb = self.getSCHOrbit(orbit, pegCoord, hdg)
|
||
|
|
|
||
|
|
####Extend the SCH orbits
|
||
|
|
self.extendSCHOrbit(schOrb)
|
||
|
|
|
||
|
|
####Convert the SCH orbit back to WGS84 orbits
|
||
|
|
extOrb = self.getXYZOrbit(schOrb, pegCoord, hdg)
|
||
|
|
|
||
|
|
####Statistics on the transforms if needed
|
||
|
|
#diffOrbits(orbit, extOrb, skip=self._newPoints)
|
||
|
|
|
||
|
|
return extOrb
|