def clockLeastSquares_ranges(eph, pranges, itow, ref_pos, last_clock_error, weights=None):
'''estimate ECEF position of receiver via least squares fit to satellite positions and pseudo-ranges
The weights dictionary is optional. If supplied, it is the weighting from 0 to 1 for each satellite.
A weight of 1 means it has more influence on the solution
'''
import scipy
from scipy import optimize
data = [ref_pos]
for svid in pranges:
if svid in eph:
if weights is not None:
weight = weights[svid]
else:
weight = 1.0
tof = pranges[svid] / util.speedOfLight
transmitTime = itow - tof
satpos = satPosition.satPosition_raw(eph[svid], svid, transmitTime)
data.append((satpos, pranges[svid], weight))
if len(data) < 4:
return
p1, ier = optimize.leastsq(clockErrorFunction, [last_clock_error], args=(data))
if not ier in [1, 2, 3, 4]:
raise RuntimeError("Unable to find solution")
return p1[0]
def clockLeastSquares_ranges(eph, pranges, itow, ref_pos, last_clock_error, weights=None):
"""estimate ECEF position of receiver via least squares fit to satellite positions and pseudo-ranges
The weights dictionary is optional. If supplied, it is the weighting from 0 to 1 for each satellite.
A weight of 1 means it has more influence on the solution
"""
from scipy import optimize
data = [ref_pos]
for svid in pranges:
if svid in eph:
if weights is not None:
weight = weights[svid]
else:
weight = 1.0
tof = pranges[svid] / util.speedOfLight
transmitTime = itow - tof
satpos = satPosition.satPosition_raw(eph[svid], svid, transmitTime)
data.append((satpos, pranges[svid], weight))
if len(data) < 4:
return
p1, ier = optimize.leastsq(clockErrorFunction, [last_clock_error], args=(data))
if not ier in [1, 2, 3, 4]:
raise RuntimeError("Unable to find solution")
return p1[0]
def regen_v2_type1():
if ref_pos is None:
return
errset = {}
for svid in prs:
if svid not in eph:
#print("Don't have ephemeris for {}, only {}".format(svid, eph.keys()))
continue
toc = eph[svid].toc
tof = prs[svid] / util.speedOfLight
# assume the time_of_week is the exact receiver time of week that the message arrived.
# subtract the time of flight to get the satellite transmit time
transmitTime = itow - tof
T = util.correctWeeklyTime(transmitTime - toc)
satpos = satPosition.satPosition_raw(eph[svid], svid, transmitTime)
Trel = satpos.extra
satPosition.correctPosition_raw(satpos, tof)
geo = satpos.distance(util.PosVector(*ref_pos))
dTclck = eph[svid].af0 + eph[svid].af1 * T + eph[svid].af2 * T * T + Trel - eph[svid].Tgd
# Incoming PR is already corrected for receiver clock bias
prAdjusted = prs[svid] + dTclck * util.speedOfLight
errset[svid] = geo - prAdjusted
save_satlog(itow, errset)
iode = {}
for svid in eph:
iode[svid] = eph[svid].iode
msg = rtcm.RTCMType1_ext(errset, itow, week, iode)
if len(msg) > 0:
return msg
if msg.name() in [ 'AID_EPH', 'RXM_EPH' ]:
msg.unpack()
eph[msg.svid] = ephemeris.EphemerisData(msg)
elif msg.name() == 'RXM_RAW':
if ourpos is None:
continue
msg.unpack()
t = msg.iTOW * 0.001
for s in msg.recs:
svid = s.sv
if svid not in eph or svid > 32:
continue
satpos = satPosition.satPosition_raw(eph[svid], svid, t)
if satpos is None:
continue
az, el = satPosition.calculateAzimuthElevation_raw(satpos, ourpos)
if el > 5:
azs[svid].append(math.radians(az))
els[svid].append(math.cos(math.radians(el)))
elif msg.name() in ['NAV_SOL', 'NAV_POSECEF']:
msg.unpack()
ourpos = util.PosVector(msg.ecefX * 0.01, msg.ecefY * 0.01, msg.ecefZ * 0.01)
plt.figure()
plt.suptitle("Skyviews")
def regen_v2_type1():
if ref_pos is None:
return
errset = {}
pranges = {}
for svid in prs:
if svid not in eph:
#print("Don't have ephemeris for {}, only {}".format(svid, eph.keys()))
continue
toc = eph[svid].toc
tof = prs[svid] / util.speedOfLight
# assume the time_of_week is the exact receiver time of week that the message arrived.
# subtract the time of flight to get the satellite transmit time
transmitTime = itow - tof
T = util.correctWeeklyTime(transmitTime - toc)
satpos = satPosition.satPosition_raw(eph[svid], svid, transmitTime)
Trel = satpos.extra
satPosition.correctPosition_raw(satpos, tof)
geo = satpos.distance(util.PosVector(*ref_pos))
dTclck = eph[svid].af0 + eph[svid].af1 * T + eph[
svid].af2 * T * T + Trel - eph[svid].Tgd
# Incoming PR is already corrected for receiver clock bias
prAdjusted = prs[svid] + dTclck * util.speedOfLight
errset[svid] = geo - prAdjusted
pranges[svid] = prAdjusted
save_satlog(itow, errset)
if correct_rxclk:
rxerr = positionEstimate.clockLeastSquares_ranges(
eph, pranges, itow, ref_pos, 0)
if rxerr is None:
return
rxerr *= util.speedOfLight
for svid in errset:
errset[svid] += rxerr
pranges[svid] += rxerr
rxerr = positionEstimate.clockLeastSquares_ranges(
eph, pranges, itow, ref_pos, 0) * util.speedOfLight
print("Residual RX clock error {}".format(rxerr))
iode = {}
for svid in eph:
iode[svid] = eph[svid].iode
msg = rtcm.RTCMType1_ext(errset, itow, week, iode)
if len(msg) > 0:
return msg
