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