import numpy as np from initialize import Result class TrackingResult(Result): def __init__(self, acqResult): self._results = None self._channels = acqResult.channels self._settings = acqResult.settings # ./tracking.m def track(self, fid): channel = self._channels settings = self._settings # Performs code and carrier tracking for all channels. # [trackResults, channel] = tracking(fid, channel, settings) # Inputs: # fid - file identifier of the signal record. # channel - PRN, carrier frequencies and code phases of all # satellites to be tracked (prepared by preRum.m from # acquisition results). # settings - receiver settings. # Outputs: # trackResults - tracking results (structure array). Contains # in-phase prompt outputs and absolute starting # positions of spreading codes, together with other # observation data from the tracking loops. All are # saved every millisecond. # Initialize tracking variables ========================================== codePeriods = settings.msToProcess # --- DLL variables -------------------------------------------------------- # Define early-late offset (in chips) earlyLateSpc = settings.dllCorrelatorSpacing # Summation interval PDIcode = 0.001 # Calculate filter coefficient values tau1code, tau2code = settings.calcLoopCoef(settings.dllNoiseBandwidth, settings.dllDampingRatio, 1.0) # --- PLL variables -------------------------------------------------------- # Summation interval PDIcarr = 0.001 # Calculate filter coefficient values tau1carr, tau2carr = settings.calcLoopCoef(settings.pllNoiseBandwidth, settings.pllDampingRatio, 0.25) # hwb=waitbar(0,'Tracking...') # Initialize a temporary list of records rec = [] # Start processing channels ============================================== for channelNr in range(settings.numberOfChannels): msToProcess = np.long(settings.msToProcess) # Initialize fields for record(structured) array of tracked results status = '-' # The absolute sample in the record of the C/A code start: absoluteSample = np.zeros(msToProcess) # Freq of the C/A code: codeFreq_ = np.inf * np.ones(msToProcess) # Frequency of the tracked carrier wave: carrFreq_ = np.inf * np.ones(msToProcess) # Outputs from the correlators (In-phase): I_P_ = np.zeros(msToProcess) I_E_ = np.zeros(msToProcess) I_L_ = np.zeros(msToProcess) # Outputs from the correlators (Quadrature-phase): Q_E_ = np.zeros(msToProcess) Q_P_ = np.zeros(msToProcess) Q_L_ = np.zeros(msToProcess) # Loop discriminators dllDiscr = np.inf * np.ones(msToProcess) dllDiscrFilt = np.inf * np.ones(msToProcess) pllDiscr = np.inf * np.ones(msToProcess) pllDiscrFilt = np.inf * np.ones(msToProcess) PRN = 0 # Only process if PRN is non zero (acquisition was successful) if channel[channelNr].PRN != 0: # Save additional information - each channel's tracked PRN PRN = channel[channelNr].PRN # signal processing at any point in the data record (e.g. for long # records). In addition skip through that data file to start at the # appropriate sample (corresponding to code phase). Assumes sample # type is schar (or 1 byte per sample) # fid.seek(settings.skipNumberOfBytes + channel[channelNr].codePhase, 0) fid.seek(int(settings.skipNumberOfBytes + channel[channelNr].codePhase), 0) # Here PRN is the actual satellite ID instead of the 0-based index caCode = settings.generateCAcode(channel[channelNr].PRN - 1) caCode = np.r_[caCode[-1], caCode, caCode[0]] # define initial code frequency basis of NCO codeFreq = settings.codeFreqBasis remCodePhase = 0.0 carrFreq = channel[channelNr].acquiredFreq carrFreqBasis = channel[channelNr].acquiredFreq remCarrPhase = 0.0 oldCodeNco = 0.0 oldCodeError = 0.0 oldCarrNco = 0.0 oldCarrError = 0.0 for loopCnt in range(np.long(codePeriods)): # GUI update ------------------------------------------------------------- # The GUI is updated every 50ms. This way Matlab GUI is still # responsive enough. At the same time Matlab is not occupied # all the time with GUI task. if loopCnt % 50 == 0: try: print ('Tracking: Ch %d' % (channelNr + 1) + ' of %d' % settings.numberOfChannels + \ '; PRN#%02d' % channel[channelNr].PRN + \ '; Completed %d' % loopCnt + ' of %d' % codePeriods + ' msec') finally: pass # Read next block of data ------------------------------------------------ # Find the size of a "block" or code period in whole samples # Update the phasestep based on code freq (variable) and # sampling frequency (fixed) codePhaseStep = codeFreq / settings.samplingFreq blksize = np.ceil((settings.codeLength - remCodePhase) / codePhaseStep) blksize = np.long(blksize) # interaction rawSignal = np.fromfile(fid, settings.dataType, blksize) samplesRead = len(rawSignal) # If did not read in enough samples, then could be out of # data - better exit if samplesRead != blksize: print ('Not able to read the specified number of samples for tracking, exiting!') fid.close() trackResults = None return trackResults # Set up all the code phase tracking information ------------------------- # Define index into early code vector tcode = np.linspace(remCodePhase - earlyLateSpc, blksize * codePhaseStep + remCodePhase - earlyLateSpc, blksize, endpoint=False) tcode2 = np.ceil(tcode) earlyCode = caCode[np.longlong(tcode2)] tcode = np.linspace(remCodePhase + earlyLateSpc, blksize * codePhaseStep + remCodePhase + earlyLateSpc, blksize, endpoint=False) tcode2 = np.ceil(tcode) lateCode = caCode[np.longlong(tcode2)] tcode = np.linspace(remCodePhase, blksize * codePhaseStep + remCodePhase, blksize, endpoint=False) tcode2 = np.ceil(tcode) promptCode = caCode[np.longlong(tcode2)] remCodePhase = tcode[blksize - 1] + codePhaseStep - 1023.0 # Generate the carrier frequency to mix the signal to baseband ----------- time = np.arange(0, blksize + 1) / settings.samplingFreq trigarg = carrFreq * 2.0 * np.pi * time + remCarrPhase remCarrPhase = trigarg[blksize] % (2 * np.pi) carrCos = np.cos(trigarg[0:blksize]) carrSin = np.sin(trigarg[0:blksize]) # Generate the six standard accumulated values --------------------------- # First mix to baseband qBasebandSignal = carrCos * rawSignal iBasebandSignal = carrSin * rawSignal I_E = (earlyCode * iBasebandSignal).sum() Q_E = (earlyCode * qBasebandSignal).sum() I_P = (promptCode * iBasebandSignal).sum() Q_P = (promptCode * qBasebandSignal).sum() I_L = (lateCode * iBasebandSignal).sum() Q_L = (lateCode * qBasebandSignal).sum() # Find PLL error and update carrier NCO ---------------------------------- # Implement carrier loop discriminator (phase detector) carrError = np.arctan(Q_P / I_P) / 2.0 / np.pi carrNco = oldCarrNco + \ tau2carr / tau1carr * (carrError - oldCarrError) + \ carrError * (PDIcarr / tau1carr) oldCarrNco = carrNco oldCarrError = carrError carrFreq = carrFreqBasis + carrNco carrFreq_[loopCnt] = carrFreq # Find DLL error and update code NCO ------------------------------------- codeError = (np.sqrt(I_E * I_E + Q_E * Q_E) - np.sqrt(I_L * I_L + Q_L * Q_L)) / ( np.sqrt(I_E * I_E + Q_E * Q_E) + np.sqrt(I_L * I_L + Q_L * Q_L)) codeNco = oldCodeNco + \ tau2code / tau1code * (codeError - oldCodeError) + \ codeError * (PDIcode / tau1code) oldCodeNco = codeNco oldCodeError = codeError codeFreq = settings.codeFreqBasis - codeNco codeFreq_[loopCnt] = codeFreq # Record various measures to show in postprocessing ---------------------- # Record sample number (based on 8bit samples) absoluteSample[loopCnt] = fid.tell() dllDiscr[loopCnt] = codeError dllDiscrFilt[loopCnt] = codeNco pllDiscr[loopCnt] = carrError pllDiscrFilt[loopCnt] = carrNco I_E_[loopCnt] = I_E I_P_[loopCnt] = I_P I_L_[loopCnt] = I_L Q_E_[loopCnt] = Q_E Q_P_[loopCnt] = Q_P Q_L_[loopCnt] = Q_L # If we got so far, this means that the tracking was successful # Now we only copy status, but it can be update by a lock detector # if implemented status = channel[channelNr].status rec.append((status, absoluteSample, codeFreq_, carrFreq_, I_P_, I_E_, I_L_, Q_E_, Q_P_, Q_L_, dllDiscr, dllDiscrFilt, pllDiscr, pllDiscrFilt, PRN)) trackResults = np.rec.fromrecords(rec, dtype=[('status', 'S1'), ('absoluteSample', 'object'), ('codeFreq', 'object'), ('carrFreq', 'object'), ('I_P', 'object'), ('I_E', 'object'), ('I_L', 'object'), ('Q_E', 'object'), ('Q_P', 'object'), ('Q_L', 'object'), ('dllDiscr', 'object'), ('dllDiscrFilt', 'object'), ('pllDiscr', 'object'), ('pllDiscrFilt', 'object'), ('PRN', 'int64')]) self._results = trackResults return def plot(self): import matplotlib as mpl import matplotlib.gridspec as gs import matplotlib.pyplot as plt import numpy as np # %% configure matplotlib (no LaTeX required) mpl.rcdefaults() mpl.rc('savefig', bbox='tight', transparent=False, format='png') mpl.rc('axes', grid=True, linewidth=1.5, axisbelow=True) mpl.rc('lines', linewidth=1.5, solid_joinstyle='bevel') mpl.rc('figure', figsize=[8, 6], dpi=120) mpl.rc('font', family='serif', size=8) mpl.rc('mathtext', fontset='cm') # dùng mathtext thay cho LaTeX # ./plotTracking.m (Python version) trackResults = self._results settings = self._settings channelList = range(settings.numberOfChannels) # Protection - if the list contains incorrect channel numbers channelList = np.intersect1d(channelList, range(settings.numberOfChannels)) # === For all listed channels ============================================== for channelNr in channelList: # Tạo figure cho mỗi channel f = plt.figure(channelNr + 200) f.set_label('Channel ' + str(channelNr) + ' (PRN ' + str(trackResults[channelNr].PRN) + ') results') # Bố cục 3x3 spec = gs.GridSpec(3, 3) h11 = plt.subplot(spec[0, 0]) h12 = plt.subplot(spec[0, 1:]) h21 = plt.subplot(spec[1, 0]) h22 = plt.subplot(spec[1, 1:]) h31 = plt.subplot(spec[2, 0]) h32 = plt.subplot(spec[2, 1]) h33 = plt.subplot(spec[2, 2]) # Trục thời gian (s) timeAxisInSeconds = np.arange(settings.msToProcess) / 1000.0 # --- Scatter plot (I_P vs Q_P) h11.plot(trackResults[channelNr].I_P, trackResults[channelNr].Q_P, '.') h11.grid() h11.axis('equal') h11.set(title='Discrete-Time Scatter Plot', xlabel='I prompt', ylabel='Q prompt') # --- Bits of navigation message (I_P theo thời gian) h12.plot(timeAxisInSeconds, trackResults[channelNr].I_P) h12.grid() h12.set(title='Bits of the navigation message', xlabel='Time (s)') h12.axis('tight') # --- Raw PLL discriminator h21.plot(timeAxisInSeconds, trackResults[channelNr].pllDiscr, 'r') h21.grid() h21.axis('tight') h21.set(xlabel='Time (s)', ylabel='Amplitude', title='Raw PLL discriminator') # --- Correlation results (Early, Prompt, Late) h22.plot(timeAxisInSeconds, np.sqrt(trackResults[channelNr].I_E ** 2 + trackResults[channelNr].Q_E ** 2).T, timeAxisInSeconds, np.sqrt(trackResults[channelNr].I_P ** 2 + trackResults[channelNr].Q_P ** 2).T, timeAxisInSeconds, np.sqrt(trackResults[channelNr].I_L ** 2 + trackResults[channelNr].Q_L ** 2).T, '-*') h22.grid() h22.set(title='Correlation results', xlabel='Time (s)') h22.axis('tight') h22.legend([r'$\sqrt{I_{E}^2 + Q_{E}^2}$', r'$\sqrt{I_{P}^2 + Q_{P}^2}$', r'$\sqrt{I_{L}^2 + Q_{L}^2}$']) # --- Filtered PLL discriminator h31.plot(timeAxisInSeconds, trackResults[channelNr].pllDiscrFilt, 'b') h31.grid() h31.axis('tight') h31.set(xlabel='Time (s)', ylabel='Amplitude', title='Filtered PLL discriminator') # --- Raw DLL discriminator h32.plot(timeAxisInSeconds, trackResults[channelNr].dllDiscr, 'r') h32.grid() h32.axis('tight') h32.set(xlabel='Time (s)', ylabel='Amplitude', title='Raw DLL discriminator') # --- Filtered DLL discriminator h33.plot(timeAxisInSeconds, trackResults[channelNr].dllDiscrFilt, 'b') h33.grid() h33.axis('tight') h33.set(xlabel='Time (s)', ylabel='Amplitude', title='Filtered DLL discriminator') # Show figure plt.show() # f.show()