def get_GDOP(self,
X_ECEF=None,
rxTime_a=None,
prn_list=None,
verbose=False):
if prn_list is None:
prn_list = self.channels
if X_ECEF is None or rxTime_a is None:
rxTime_a, rxTime, X_ECEF, X_ECI, sats_ECI = naveng.calculate_nav_soln(
self, rxTime0=rxTime_a, rxPos0=X_ECEF)
else:
X_ECI = utils.ECEF_to_ECI(X_ECEF, t_gps=rxTime_a, t_c=rxTime_a)
sats_ECI, transmitTime = naveng.get_satellite_positions(
self, prn_list=prn_list, t_c=rxTime_a)
numChannels = len(prn_list)
los = sats_ECI[0:3, :] - np.tile(X_ECI[0:3], (1, numChannels))
los = (los / np.linalg.norm(los, axis=0)).T
G = np.hstack((-los, np.ones((numChannels, 1))))
G = np.matrix(G)
H = np.linalg.inv(G.T * G)
GDOP = np.sqrt(np.sum(np.diag(H)))
if verbose:
return GDOP, X_ECEF, rxTime_a, sats_ECI
return GDOP
def vt_time_update(self):
ds, fc, Tc, fcaid = self.rawfile.ds, F_CA, T_CA, ds * fc / F_L1
N = self.N
channel = self.channel
# UPDATE: fi, fc
# Satellites in ECI again for calculations
sats_ECI, transmitTime = naveng.get_satellite_positions(
self, prn_list=channelList, t_c=self.rxTime_a)
X_ECI = libgnss.utils.ECEF_to_ECI(self.X_ECEF,
t_gps=self.rxTime_a,
t_c=self.rxTime_a)
X_ECI[3, 0] = X_ECI[3, 0] / c
X_ECI[7, 0] = X_ECI[7, 0] / c
# bc = "back computing"
bc_doppler_all = np.zeros((numChannels))
for inr, nr in enumerate(channelList):
bc_los = (sats_ECI[0:3, inr] - X_ECI[0:3, 0]).T # Line of sight
bc_range = np.linalg.norm(bc_los, axis=1)[0]
# print(bc_range) Range
bc_los = bc_los / bc_range # Line of sight unit vector
bc_rangerate = X_ECI[4:7, 0] - sats_ECI[
4:7, inr] # Range rate vector (time derivative of bc_los)
bc_losrangerate = bc_los * bc_rangerate
bc_pseudorate = -bc_losrangerate[0, 0] + c * (X_ECI[7, 0] -
sats_ECI[7, inr])
bc_doppler = -L1 / c * bc_pseudorate
bc_doppler_all[inr] = bc_doppler
bc_fi = bc_doppler / ds
channel[nr]._fi = bc_fi
for inr, nr in enumerate(channelList):
bc_range = np.linalg.norm(sats_ECI[0:3, inr] - X_ECI[0:3, 0],
axis=0)[0]
bc_pseudorange = bc_range + c * (X_ECI[3, 0] - sats_ECI[3, inr])
bc_transmitTime = self.ekf.rxTime0 - bc_pseudorange / c
channel[nr]._fc = fc + 1.0/(N*0.001)*((bc_transmitTime-(transmitTime[inr]+sats_ECI[3,inr]))*fc)\
+ fcaid*bc_doppler_all[inr]
# PREDICT: new X, new Sigma
self.X_ECEF = self._predict_X()
Q = self.ekf._predict_Q()
Sigma = self.ekf._predict_Sigma()
self.ekf.rxTime0 = np.round(
(self.ekf.rxTime0 + self.N * Tc) * 1000.0) / 1000.0
self.ekf.rxTime_a = self.ekf.rxTime0 - self.X_ECEF[3, 0] / c
for i in range(N):
for channelNr in self.channelList:
channel[channelNr].update()
return self.X_ECEF, Q, Sigma, self.ekf.rxTime0, self.rxTime_a
def dp_measurement_update_channels(self):
mc = self._mcount
prn_list = sorted(self.channels.keys())
X_ECEF = self.ekf.X_ECEF
rxTime_a = self.rxTime_a
rxTime = self.rxTime
X_ECI = utils.ECEF_to_ECI(X_ECEF, t_gps=rxTime_a, t_c=rxTime_a)
sats_ECI, transmitTime = naveng.get_satellite_positions(self,
t_c=rxTime_a)
for prn_idx, prn in enumerate(prn_list):
bc_los = (sats_ECI[0:3,prn_idx]-X_ECI[0:3]).T/\
(np.linalg.norm((sats_ECI[0:3,prn_idx]-X_ECI[0:3]), axis=0)[0])
bc_rangerate = X_ECI[4:7] - sats_ECI[4:7, prn_idx]
bc_losrangerate = (bc_los * bc_rangerate)[0, 0]
bc_pseudorate = -bc_losrangerate + C * (X_ECI[7, 0] / C -
sats_ECI[7, prn_idx])
bc_doppler = -F_L1 / C * bc_pseudorate
bc_fi = bc_doppler / self.rawfile.ds
#assert np.abs(bc_fi - self.channels[prn].fi[mc]) < 10, \
#'prn: %d | bc_fi: %.3f, fi: %.3f'%(prn, bc_fi, self.channels[prn].fi[mc])
self.channels[prn].fi[mc] = bc_fi
bc_range = np.linalg.norm(sats_ECI[0:3, prn_idx] - X_ECI[0:3],
axis=0)[0]
bc_pseudorange = bc_range + C * (X_ECI[3, 0] / C -
sats_ECI[3, prn_idx])
bc_transmitTime = rxTime - bc_pseudorange / C
bc_codeFracDiff = bc_transmitTime \
- self.channels[prn].ephemerides.timestamp['TOW']\
- T_CA*(self.channels[prn].cp[mc]-self.channels[prn].ephemerides.timestamp['cp'])
bc_rc = bc_codeFracDiff * F_CA
#assert np.abs(bc_rc - self.channels[prn].rc[mc]) < 1, \
#'prn: %d | bc_rc: %.3f, rc: %.3f'%(prn, bc_rc, self.channels[prn].rc[mc])
bc_fc = F_CA + self.rawfile.fcaid * bc_fi + (
bc_rc - self.channels[prn].rc[mc]) / self.rawfile.T
#assert np.abs(bc_fc - self.channels[prn].fc[mc]) < 20, \
#'prn: %d | bc_fc: %.3f, fc: %.3f'%(prn, bc_fc, self.channels[prn].fc[mc])
self.channels[prn].fc[mc] = bc_fc
return
def vt_measurement_update(self, channelList=None):
ds = self.rawfile.ds
N = self.N
if channelList is None:
channelList = self.channels.keys()
numChannels = len(channelList)
numPrevSamples = self.numPrevSamples
mscount = self._mcount
ms_array = np.arange(mscount - numPrevSamples, mscount)
# CORRECTION: get error covariance W
epc_var = np.matrix(np.zeros(
(numChannels, numPrevSamples))) # error in phase
efi_var = np.matrix(np.zeros(
(numChannels, numPrevSamples))) # error in frequency
# Looping through all the channels to get the error in phase and error in frequency (doppler shift)
for idx, channelNr in enumerate(channelList):
epc_var[idx, :] = channel[channelNr].dpc[ms_array - 1]
efi_var[idx, :] = channel[channelNr].dfi[ms_array - 1]
# Converting to SI units
epc_var = epc_var * (-c / fc) # adds to rc [chip * m/s * 1/(chips/s)]
efi_var = efi_var * (-ds * c / L1
) # adds to fi [cycles/s * m/s * 1/(cycles/s)]
# Combining epc and efi into one vector and finding the variance (np.var)
e_var = np.concatenate((epc_var, efi_var))
e_var = np.var(e_var, axis=1)
W = np.matrix(np.diag(np.asarray(e_var)[:, 0]))
# CORRECTION: get discriminations e
epc = np.matrix(np.zeros((numChannels, 1)))
efi = np.matrix(np.zeros((numChannels, 1)))
# Actually finding the errors
for channelNr in self.channel.keys():
self.channel[channelNr].dpc[mscount-1], self.channel[channelNr].dfc[mscount-1] = \
dpc, dfc = self.channel[channelNr].cdiscriminator.update(self.iE[mc-1], self.qE[mc-1], self.iL[mc-1], self.qL[mc-1])
self.channel[channelNr].dpi[mscount-1], self.channel[channelNr].dfi[mscount-1] = \
dpi, dfi = self.channel[channelNr].idiscriminator.update(self.iP[mc-2],self.qP[mc-2],self.iP[mc-1],self.qP[mc-1]) # carrier and
# Input into error vector
for idx, channelNr in enumerate(channelList):
epc[idx] = self.channel[channelNr].dpc[mscount - 1]
efi[idx] = self.channel[channelNr].dfi[mscount - 1]
epc = epc * (-c / fc) # adds to rc
efi = efi * (-ds * c / L1) # adds to fi
e = np.concatenate((epc, efi))
# UPDATE: get geometry matrix H (or Jacobian) (ECI) - basically one-step Newton Raphson
sats_ECI, transmitTime = naveng.get_satellite_positions(
self, prn_list=channelList, t_c=self.rxTime_a)
X_ECI = libgnss.utils.ECEF_to_ECI(self.X_ECEF,
t_gps=self.rxTime_a,
t_c=self.rxTime_a)
H = np.matrix(np.zeros((2 * len(channelList), 8)))
# Populating the H matrix
for idx, channelNr in enumerate(channelList):
rxSat_range = np.linalg.norm(sats_ECI[0:3, idx] - X_ECI[0:3, 0],
axis=0)[0]
rxSat_los = ((sats_ECI[0:3, idx] - X_ECI[0:3, 0]) / rxSat_range).T
H[idx, 0:3] = -rxSat_los
H[idx, 3] = 1.0
H[idx + len(channelList), 4:7] = -rxSat_los
H[idx + len(channelList), 7] = 1.0
K = self.ekf._update_K(H, W)
dX = self.ekf._update_dX_ECI(e)
X_ECEF, X_ECI = self.ekf._correct_X_ECI(
) # Converts ECEF to ECI, updates X, then converts back to ECEF
Sigma = self.ekf._correct_Sigma(H)
self.X_ECEF = X_ECEF
return e, W, K, dX, X_ECEF, X_ECI, Sigma
def dp_measurement_estimation_unfolded(self, L_power=1, gXk_grid=None):
mc = self._mcount
prn_list = sorted(self.channels.keys())
X_ECEF = self.ekf.X_ECEF
rxTime_a = self.rxTime_a
rxTime = self.rxTime
X_ECI = utils.ECEF_to_ECI(X_ECEF, t_gps=rxTime_a, t_c=rxTime_a)
sats_ECI, transmitTime = naveng.get_satellite_positions(self,
t_c=rxTime_a)
if gXk_grid is None: #single receiver mode
gX_ECEF_fixed_vel, gX_ECI_fixed_vel, gX_ECEF_fixed_pos, gX_ECI_fixed_pos, bc_pos_corr, bc_vel_fft\
= self.navguess.get_nav_guesses(X_ECEF, rxTime_a)
else: # multi receiver mode
gX_ECEF_fixed_vel, gX_ECEF_fixed_pos = gXk_grid # unpacking
gX_ECI_fixed_vel = utils.ECEF_to_ECI(gX_ECEF_fixed_vel,
t_gps=self.rxTime_a,
t_c=self.rxTime_a)
gX_ECI_fixed_pos = utils.ECEF_to_ECI(gX_ECEF_fixed_pos,
t_gps=self.rxTime_a,
t_c=self.rxTime_a)
bc_pos_corr = np.zeros(np.size(gX_ECEF_fixed_vel, 1))
bc_vel_fft = np.zeros(np.size(gX_ECEF_fixed_pos, 1))
for prn_idx, prn in enumerate(prn_list):
bc_los = (sats_ECI[0:3,prn_idx]-X_ECI[0:3]).T/\
(np.linalg.norm((sats_ECI[0:3,prn_idx]-X_ECI[0:3]), axis=0)[0])
bc_rangerate = gX_ECI_fixed_pos[4:7, :] - sats_ECI[4:7, prn_idx]
bc_losrangerate = np.array(bc_los * bc_rangerate)[0, :]
bc_pseudorate = -bc_losrangerate + C * (
np.array(gX_ECI_fixed_pos)[7, :] / C - sats_ECI[7, prn_idx])
bc_doppler = -F_L1 / C * bc_pseudorate
bc_fi = bc_doppler / self.rawfile.ds
bc_fi0 = bc_fi - self.channels[prn].fi[mc]
bc_fi0_idx = (self.rawfile.carr_fftpts / self.rawfile.fs) * (
bc_fi0) + self.rawfile.carr_fftpts / 2.0
#bc_fi0_idx = (self.rawfile.S / self.rawfile.fs) * (bc_fi0) + self.rawfile.S / 2.0
#bc_fi0_idx = (131072 / self.rawfile.fs) * (bc_fi0) + 131072 / 2.0
bc_fi0_cidx = np.floor(bc_fi0_idx + 1).astype(np.int32)
bc_fi0_fidx = np.floor(bc_fi0_idx).astype(np.int32)
bc_fi0_fft = (self.channels[prn].carr_fft[bc_fi0_cidx]*(bc_fi0_idx-bc_fi0_fidx)\
+self.channels[prn].carr_fft[bc_fi0_fidx]*(bc_fi0_cidx-bc_fi0_idx))
bc_vel_fft = bc_vel_fft + np.abs(bc_fi0_fft)**L_power
bc_range = np.linalg.norm(sats_ECI[0:3, prn_idx] -
gX_ECI_fixed_vel[0:3, :],
axis=0)
bc_pseudorange = bc_range + C * (
np.array(gX_ECI_fixed_vel)[3, :] / C - sats_ECI[3, prn_idx])
bc_transmitTime = rxTime - bc_pseudorange / C
bc_codeFracDiff = bc_transmitTime \
- self.channels[prn].ephemerides.timestamp['TOW']\
- T_CA*(self.channels[prn].cp[mc]-self.channels[prn].ephemerides.timestamp['cp'])
bc_rc = bc_codeFracDiff * F_CA
bc_rc0 = bc_rc - self.channels[prn].rc[mc]
bc_rc0_idx = (self.rawfile.fs / self.channels[prn].fc[mc]) * (
-bc_rc0) + self.rawfile.S / 2.0
bc_rc0_cidx = np.floor(bc_rc0_idx + 1).astype(np.int32)
bc_rc0_fidx = np.floor(bc_rc0_idx).astype(np.int32)
bc_rc0_corr = (self.channels[prn].code_corr[bc_rc0_cidx]*(bc_rc0_idx-bc_rc0_fidx)\
+self.channels[prn].code_corr[bc_rc0_fidx]*(bc_rc0_cidx-bc_rc0_idx))
bc_pos_corr = bc_pos_corr + np.abs(bc_rc0_corr)**L_power
if gXk_grid is not None:
self.pos_corr = bc_pos_corr
self.vel_fft = bc_vel_fft
return
#mean_pos = np.sum(np.multiply(gX_ECEF_fixed_vel[0:4],np.tile(bc_pos_corr,(4,1))),axis=1)/np.sum(bc_pos_corr)
#mean_vel = np.sum(np.multiply(gX_ECEF_fixed_pos[4:8],np.tile(bc_vel_fft,(4,1))),axis=1)/np.sum(bc_vel_fft)
mean_pos = gX_ECEF_fixed_vel[0:4, np.argmax(bc_pos_corr)]
mean_vel = gX_ECEF_fixed_pos[4:8, np.argmax(bc_vel_fft)]
temp = np.vstack((mean_pos, mean_vel)) - X_ECEF
return np.vstack((mean_pos, mean_vel)) - X_ECEF