├── README.md ├── acquisition.py ├── ephemeris.py ├── geoFunctions └── __init__.py ├── initialize.py ├── main.py ├── postNavigation.py └── tracking.py /README.md: -------------------------------------------------------------------------------- 1 | # Getting Started 2 | 3 | This is a GNSS Software Defined Radio (SDR) implemented in python, based on SoftGNSS 3.0 developed by Darius Plausinaitis and Dennis M. Akos in Matlab. 4 | 5 | # System requirements 6 | 7 | * python 2.7 8 | * matplotlib 9 | * scipy 10 | * numpy 11 | 12 | # Installation 13 | 14 | Coming soon! 15 | 16 | # Running the GNSS SDR 17 | 18 | 1. Examine "main.py" 19 | 2. Tweak parameters for the "settings" class if necessary 20 | 3. Specify the binary file to be processed and run "main.py" 21 | 4. Wait until it is finished 22 | 23 | 24 | # Resources 25 | * The official homepage of the textbook 26 | 27 | -------------------------------------------------------------------------------- /acquisition.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from initialize import Result 4 | 5 | 6 | class AcquisitionResult(Result): 7 | def __init__(self, settings): 8 | self._settings = settings 9 | self._results = None 10 | self._channels = None 11 | 12 | @property 13 | def peakMetric(self): 14 | assert isinstance(self._results, np.recarray) 15 | return self._results.peakMetric 16 | 17 | @property 18 | def carrFreq(self): 19 | assert isinstance(self._results, np.recarray) 20 | return self._results.carrFreq 21 | 22 | @property 23 | def codePhase(self): 24 | assert isinstance(self._results, np.recarray) 25 | return self._results.codePhase 26 | 27 | def acquire(self, longSignal): 28 | # ./acquisition.m 29 | # Function performs cold start acquisition on the collected "data". It 30 | # searches for GPS signals of all satellites, which are listed in field 31 | # "acqSatelliteList" in the settings structure. Function saves code phase 32 | # and frequency of the detected signals in the "acqResults" structure. 33 | 34 | # acqResults = acquisition(longSignal, settings) 35 | 36 | # Inputs: 37 | # longSignal - 11 ms of raw signal from the front-end 38 | # settings - Receiver settings. Provides information about 39 | # sampling and intermediate frequencies and other 40 | # parameters including the list of the satellites to 41 | # be acquired. 42 | # Outputs: 43 | # acqResults - Function saves code phases and frequencies of the 44 | # detected signals in the "acqResults" structure. The 45 | # field "carrFreq" is set to 0 if the signal is not 46 | # detected for the given PRN number. 47 | 48 | # Initialization ========================================================= 49 | settings = self._settings 50 | 51 | # Find number of samples per spreading code 52 | samplesPerCode = settings.samplesPerCode 53 | 54 | # Create two 1m sec vectors of data to correlate with and one with zero DC 55 | signal1 = longSignal[0:samplesPerCode] 56 | 57 | signal2 = longSignal[samplesPerCode:2 * samplesPerCode] 58 | 59 | signal0DC = longSignal - longSignal.mean() 60 | 61 | # Find sampling period 62 | ts = 1.0 / settings.samplingFreq 63 | 64 | # Find phase points of the local carrier wave 65 | phasePoints = np.arange(samplesPerCode) * 2 * np.pi * ts 66 | 67 | # Number of the frequency bins for the given acquisition band (500Hz steps) 68 | numberOfFrqBins = np.int(np.round(settings.acqSearchBand * 2) + 1) 69 | 70 | # Generate all C/A codes and sample them according to the sampling freq. 71 | caCodesTable = settings.makeCaTable() 72 | 73 | # --- Initialize arrays to speed up the code ------------------------------- 74 | # Search results of all frequency bins and code shifts (for one satellite) 75 | results = np.zeros((numberOfFrqBins, samplesPerCode)) 76 | 77 | # Carrier frequencies of the frequency bins 78 | frqBins = np.zeros(numberOfFrqBins) 79 | 80 | # --- Initialize acqResults ------------------------------------------------ 81 | # Carrier frequencies of detected signals 82 | carrFreq = np.zeros(32) 83 | 84 | # C/A code phases of detected signals 85 | codePhase_ = np.zeros(32) 86 | 87 | # Correlation peak ratios of the detected signals 88 | peakMetric = np.zeros(32) 89 | 90 | print '(' 91 | # Perform search for all listed PRN numbers ... 92 | for PRN in range(len(settings.acqSatelliteList)): 93 | # Correlate signals ====================================================== 94 | # --- Perform DFT of C/A code ------------------------------------------ 95 | caCodeFreqDom = np.fft.fft(caCodesTable[PRN, :]).conj() 96 | 97 | for frqBinIndex in range(numberOfFrqBins): 98 | # --- Generate carrier wave frequency grid (0.5kHz step) ----------- 99 | frqBins[frqBinIndex] = settings.IF - \ 100 | settings.acqSearchBand / 2 * 1000 + \ 101 | 500.0 * frqBinIndex 102 | 103 | sinCarr = np.sin(frqBins[frqBinIndex] * phasePoints) 104 | 105 | cosCarr = np.cos(frqBins[frqBinIndex] * phasePoints) 106 | 107 | I1 = sinCarr * signal1 108 | 109 | Q1 = cosCarr * signal1 110 | 111 | I2 = sinCarr * signal2 112 | 113 | Q2 = cosCarr * signal2 114 | 115 | IQfreqDom1 = np.fft.fft(I1 + 1j * Q1) 116 | 117 | IQfreqDom2 = np.fft.fft(I2 + 1j * Q2) 118 | 119 | # domain) 120 | convCodeIQ1 = IQfreqDom1 * caCodeFreqDom 121 | 122 | convCodeIQ2 = IQfreqDom2 * caCodeFreqDom 123 | 124 | acqRes1 = abs(np.fft.ifft(convCodeIQ1)) ** 2 125 | 126 | acqRes2 = abs(np.fft.ifft(convCodeIQ2)) ** 2 127 | 128 | # "blend" 1st and 2nd msec but will correct data bit issues 129 | if acqRes1.max() > acqRes2.max(): 130 | results[frqBinIndex, :] = acqRes1 131 | 132 | else: 133 | results[frqBinIndex, :] = acqRes2 134 | 135 | # Look for correlation peaks in the results ============================== 136 | # Find the highest peak and compare it to the second highest peak 137 | # The second peak is chosen not closer than 1 chip to the highest peak 138 | # --- Find the correlation peak and the carrier frequency -------------- 139 | peakSize = results.max(1).max() 140 | frequencyBinIndex = results.max(1).argmax() 141 | 142 | peakSize = results.max(0).max() 143 | codePhase = results.max(0).argmax() 144 | 145 | samplesPerCodeChip = long(round(settings.samplingFreq / settings.codeFreqBasis)) 146 | 147 | excludeRangeIndex1 = codePhase - samplesPerCodeChip 148 | 149 | excludeRangeIndex2 = codePhase + samplesPerCodeChip 150 | 151 | # boundaries 152 | if excludeRangeIndex1 <= 0: 153 | codePhaseRange = np.r_[excludeRangeIndex2:samplesPerCode + excludeRangeIndex1 + 1] 154 | 155 | elif excludeRangeIndex2 >= samplesPerCode - 1: 156 | codePhaseRange = np.r_[excludeRangeIndex2 - samplesPerCode:excludeRangeIndex1] 157 | 158 | else: 159 | codePhaseRange = np.r_[0:excludeRangeIndex1 + 1, excludeRangeIndex2:samplesPerCode] 160 | 161 | # --- Find the second highest correlation peak in the same freq. bin --- 162 | secondPeakSize = results[frequencyBinIndex, codePhaseRange].max() 163 | 164 | peakMetric[PRN] = peakSize / secondPeakSize 165 | 166 | if (peakSize / secondPeakSize) > settings.acqThreshold: 167 | # Fine resolution frequency search ======================================= 168 | # --- Indicate PRN number of the detected signal ------------------- 169 | print '%02d ' % (PRN + 1) 170 | caCode = settings.generateCAcode(PRN) 171 | 172 | codeValueIndex = np.floor(ts * np.arange(1, 10 * samplesPerCode + 1) / (1.0 / settings.codeFreqBasis)) 173 | 174 | longCaCode = caCode[np.longlong(codeValueIndex % 1023)] 175 | 176 | # (Using detected C/A code phase) 177 | xCarrier = signal0DC[codePhase:codePhase + 10 * samplesPerCode] * longCaCode 178 | 179 | fftNumPts = 8 * 2 ** (np.ceil(np.log2(len(xCarrier)))) 180 | 181 | # associated carrier frequency 182 | fftxc = np.abs(np.fft.fft(xCarrier, np.long(fftNumPts))) 183 | 184 | uniqFftPts = np.long(np.ceil((fftNumPts + 1) / 2.0)) 185 | 186 | fftMax = fftxc[4:uniqFftPts - 5].max() 187 | fftMaxIndex = fftxc[4:uniqFftPts - 5].argmax() 188 | 189 | fftFreqBins = np.arange(uniqFftPts) * settings.samplingFreq / fftNumPts 190 | 191 | carrFreq[PRN] = fftFreqBins[fftMaxIndex] 192 | 193 | codePhase_[PRN] = codePhase 194 | 195 | else: 196 | # --- No signal with this PRN -------------------------------------- 197 | print '. ' 198 | 199 | # === Acquisition is over ================================================== 200 | print ')\n' 201 | acqResults = np.core.records.fromarrays([carrFreq, codePhase_, peakMetric], 202 | names='carrFreq,codePhase,peakMetric') 203 | self._results = acqResults 204 | return 205 | 206 | def plot(self): 207 | assert isinstance(self._results, np.recarray) 208 | import matplotlib as mpl 209 | import matplotlib.pyplot as plt 210 | # from scipy.io.matlab import loadmat 211 | 212 | # %% configure matplotlib 213 | mpl.rcdefaults() 214 | # mpl.rcParams['font.sans-serif'] 215 | # mpl.rcParams['font.family'] = 'serif' 216 | mpl.rc('savefig', bbox='tight', transparent=False, format='png') 217 | mpl.rc('axes', grid=True, linewidth=1.5, axisbelow=True) 218 | mpl.rc('lines', linewidth=1.5, solid_joinstyle='bevel') 219 | mpl.rc('figure', figsize=[8, 6], autolayout=False, dpi=120) 220 | mpl.rc('text', usetex=True) 221 | mpl.rc('font', family='serif', serif='Computer Modern Roman', size=16) 222 | mpl.rc('mathtext', fontset='cm') 223 | 224 | # mpl.rc('font', size=16) 225 | # mpl.rc('text.latex', preamble=r'\usepackage{cmbright}') 226 | 227 | # ./plotAcquisition.m 228 | # Functions plots bar plot of acquisition results (acquisition metrics). No 229 | # bars are shown for the satellites not included in the acquisition list (in 230 | # structure SETTINGS). 231 | 232 | # plotAcquisition(acqResults) 233 | 234 | # Inputs: 235 | # acqResults - Acquisition results from function acquisition. 236 | 237 | # Plot all results ======================================================= 238 | f, hAxes = plt.subplots() 239 | 240 | plt.bar(range(1, 33), self.peakMetric) 241 | plt.title('Acquisition results') 242 | plt.xlabel('PRN number (no bar - SV is not in the acquisition list)') 243 | plt.ylabel('Acquisition Metric ($1^{st}$ to $2^{nd}$ Correlation Peaks Ratio') 244 | oldAxis = plt.axis() 245 | 246 | plt.axis([0, 33, 0, oldAxis[-1]]) 247 | plt.xticks(range(1, 33), size=12) 248 | # plt.minorticks_on() 249 | hAxes.xaxis.grid() 250 | # Mark acquired signals ================================================== 251 | 252 | acquiredSignals = self.peakMetric * (self.carrFreq > 0) 253 | 254 | plt.bar(range(1, 33), acquiredSignals, FaceColor=(0, 0.8, 0)) 255 | plt.legend(['Not acquired signals', 'Acquired signals']) 256 | plt.show() 257 | 258 | # preRun.m 259 | def preRun(self): 260 | assert isinstance(self._results, np.recarray) 261 | # Function initializes tracking channels from acquisition data. The acquired 262 | # signals are sorted according to the signal strength. This function can be 263 | # modified to use other satellite selection algorithms or to introduce 264 | # acquired signal properties offsets for testing purposes. 265 | 266 | # [channel] = preRun(acqResults, settings) 267 | 268 | # Inputs: 269 | # acqResults - results from acquisition. 270 | # settings - receiver settings 271 | 272 | # Outputs: 273 | # channel - structure contains information for each channel (like 274 | # properties of the tracked signal, channel status etc.). 275 | 276 | settings = self._settings 277 | # Initialize all channels ================================================ 278 | PRN = np.zeros(settings.numberOfChannels, dtype='int64') 279 | acquiredFreq = np.zeros(settings.numberOfChannels) 280 | codePhase = np.zeros(settings.numberOfChannels) 281 | status = ['-' for _ in range(settings.numberOfChannels)] 282 | 283 | # --- Copy initial data to all channels ------------------------------------ 284 | 285 | # Copy acquisition results =============================================== 286 | 287 | # --- Sort peaks to find strongest signals, keep the peak index information 288 | PRNindexes = sorted(enumerate(self.peakMetric), 289 | key=lambda x: x[-1], reverse=True) 290 | 291 | # --- Load information about each satellite -------------------------------- 292 | # Maximum number of initialized channels is number of detected signals, but 293 | # not more as the number of channels specified in the settings. 294 | for ii in range(min(settings.numberOfChannels, sum(self.carrFreq > 0))): 295 | PRN[ii] = PRNindexes[ii][0] + 1 296 | 297 | acquiredFreq[ii] = self.carrFreq[PRNindexes[ii][0]] 298 | 299 | codePhase[ii] = self.codePhase[PRNindexes[ii][0]] 300 | 301 | status[ii] = 'T' 302 | 303 | channel = np.core.records.fromarrays([PRN, acquiredFreq, codePhase, status], 304 | names='PRN,acquiredFreq,codePhase,status') 305 | self._channels = channel 306 | return 307 | 308 | def showChannelStatus(self): 309 | # Prints the status of all channels in a table. 310 | 311 | # showChannelStatus(channel, settings) 312 | 313 | # Inputs: 314 | # channel - data for each channel. It is used to initialize and 315 | # at the processing of the signal (tracking part). 316 | # settings - receiver settings 317 | 318 | channel = self._channels 319 | settings = self._settings 320 | assert isinstance(channel, np.recarray) 321 | print ('\n*=========*=====*===============*===========*=============*========*') 322 | print ('| Channel | PRN | Frequency | Doppler | Code Offset | Status |') 323 | print ('*=========*=====*===============*===========*=============*========*') 324 | for channelNr in range(settings.numberOfChannels): 325 | if channel[channelNr].status != '-': 326 | print '| %2d | %3d | %2.5e | %5.0f | %6d | %1s |' % ( 327 | channelNr, 328 | channel[channelNr].PRN, 329 | channel[channelNr].acquiredFreq, 330 | channel[channelNr].acquiredFreq - settings.IF, 331 | channel[channelNr].codePhase, 332 | channel[channelNr].status) 333 | else: 334 | print '| %2d | --- | ------------ | ----- | ------ | Off |' % channelNr 335 | 336 | print '*=========*=====*===============*===========*=============*========*\n' 337 | 338 | 339 | if __name__ == '__main__': 340 | pass 341 | -------------------------------------------------------------------------------- /ephemeris.py: -------------------------------------------------------------------------------- 1 | def bin2dec(binaryStr): 2 | assert isinstance(binaryStr, str) 3 | return int(binaryStr, 2) 4 | 5 | 6 | # twosComp2dec.m 7 | def twosComp2dec(binaryStr): 8 | # TWOSCOMP2DEC(binaryNumber) Converts a two's-complement binary number 9 | # BINNUMBER (in Matlab it is a string type), represented as a row vector of 10 | # zeros and ones, to an integer. 11 | 12 | # intNumber = twosComp2dec(binaryNumber) 13 | 14 | # --- Check if the input is string ----------------------------------------- 15 | if not isinstance(binaryStr, str): 16 | raise IOError('Input must be a string.') 17 | 18 | # --- Convert from binary form to a decimal number ------------------------- 19 | intNumber = int(binaryStr, 2) 20 | 21 | # --- If the number was negative, then correct the result ------------------ 22 | if binaryStr[0] == '1': 23 | intNumber -= 2 ** len(binaryStr) 24 | return intNumber 25 | 26 | 27 | # checkPhase.m 28 | 29 | 30 | def checkPhase(word, d30star): 31 | # Checks the parity of the supplied 30bit word. 32 | # The last parity bit of the previous word is used for the calculation. 33 | # A note on the procedure is supplied by the GPS standard positioning 34 | # service signal specification. 35 | 36 | # word = checkPhase(word, D30Star) 37 | 38 | # Inputs: 39 | # word - an array with 30 bit long word from the navigation 40 | # message (a character array, must contain only '0' or 41 | # '1'). 42 | # D30Star - the last bit of the previous word (char type). 43 | 44 | # Outputs: 45 | # word - word with corrected polarity of the data bits 46 | # (character array). 47 | 48 | word_new = [] 49 | if d30star == '1': 50 | # Data bits must be inverted 51 | for i in range(0, 24): 52 | if word[i] == '1': 53 | word[i] = '0' 54 | elif word[i] == '0': 55 | word[i] = '1' 56 | return word 57 | 58 | 59 | # ephemeris.m 60 | def ephemeris(bits, d30star): 61 | # Function decodes ephemerides and TOW from the given bit stream. The stream 62 | # (array) in the parameter BITS must contain 1500 bits. The first element in 63 | # the array must be the first bit of a subframe. The subframe ID of the 64 | # first subframe in the array is not important. 65 | 66 | # Function does not check parity! 67 | 68 | # [eph, TOW] = ephemeris(bits, D30Star) 69 | 70 | # Inputs: 71 | # bits - bits of the navigation messages (5 subframes). 72 | # Type is character array and it must contain only 73 | # characters '0' or '1'. 74 | # D30Star - The last bit of the previous nav-word. Refer to the 75 | # GPS interface control document ICD (IS-GPS-200D) for 76 | # more details on the parity checking. Parameter type is 77 | # char. It must contain only characters '0' or '1'. 78 | # Outputs: 79 | # TOW - Time Of Week (TOW) of the first sub-frame in the bit 80 | # stream (in seconds) 81 | # eph - SV ephemeris 82 | 83 | # Check if there is enough data ========================================== 84 | if len(bits) < 1500: 85 | raise TypeError('The parameter BITS must contain 1500 bits!') 86 | 87 | # Check if the parameters are strings ==================================== 88 | if any([not isinstance(x, str) for x in bits]): 89 | raise TypeError('The parameter BITS must be a character array!') 90 | 91 | if not isinstance(d30star, str): 92 | raise TypeError('The parameter D30Star must be a char!') 93 | 94 | # Pi used in the GPS coordinate system 95 | gpsPi = 3.1415926535898 96 | 97 | # Decode all 5 sub-frames ================================================ 98 | for i in range(5): 99 | # --- "Cut" one sub-frame's bits --------------------------------------- 100 | subframe = bits[300 * i:300 * (i + 1)] 101 | 102 | for j in range(10): 103 | subframe[30 * j: 30 * (j + 1)] = checkPhase(subframe[30 * j: 30 * (j + 1)], d30star) 104 | 105 | d30star = subframe[30 * (j + 1) - 1] 106 | 107 | # --- Decode the sub-frame id ------------------------------------------ 108 | # For more details on sub-frame contents please refer to GPS IS. 109 | subframe = ''.join(subframe) 110 | subframeID = bin2dec(subframe[49:52]) 111 | 112 | # The task is to select the necessary bits and convert them to decimal 113 | # numbers. For more details on sub-frame contents please refer to GPS 114 | # ICD (IS-GPS-200D). 115 | if 1 == subframeID: 116 | # It contains WN, SV clock corrections, health and accuracy 117 | weekNumber = bin2dec(subframe[60:70]) + 1024 118 | 119 | accuracy = bin2dec(subframe[72:76]) 120 | 121 | health = bin2dec(subframe[76:82]) 122 | 123 | T_GD = twosComp2dec(subframe[195:204]) * 2 ** (- 31) 124 | 125 | IODC = bin2dec(subframe[82:84] + subframe[196:204]) 126 | 127 | t_oc = bin2dec(subframe[218:234]) * 2 ** 4 128 | 129 | a_f2 = twosComp2dec(subframe[240:248]) * 2 ** (- 55) 130 | 131 | a_f1 = twosComp2dec(subframe[248:264]) * 2 ** (- 43) 132 | 133 | a_f0 = twosComp2dec(subframe[270:292]) * 2 ** (- 31) 134 | 135 | elif 2 == subframeID: 136 | # It contains first part of ephemeris parameters 137 | IODE_sf2 = bin2dec(subframe[60:68]) 138 | 139 | C_rs = twosComp2dec(subframe[68:84]) * 2 ** (- 5) 140 | 141 | deltan = twosComp2dec(subframe[90:106]) * 2 ** (- 43) * gpsPi 142 | 143 | M_0 = twosComp2dec(subframe[106:114] + subframe[120:144]) * 2 ** (- 31) * gpsPi 144 | 145 | C_uc = twosComp2dec(subframe[150:166]) * 2 ** (- 29) 146 | 147 | e = bin2dec(subframe[166:174] + subframe[180:204]) * 2 ** (- 33) 148 | 149 | C_us = twosComp2dec(subframe[210:226]) * 2 ** (- 29) 150 | 151 | sqrtA = bin2dec(subframe[226:234] + subframe[240:264]) * 2 ** (- 19) 152 | 153 | t_oe = bin2dec(subframe[270:286]) * 2 ** 4 154 | 155 | elif 3 == subframeID: 156 | # It contains second part of ephemeris parameters 157 | C_ic = twosComp2dec(subframe[60:76]) * 2 ** (- 29) 158 | 159 | omega_0 = twosComp2dec(subframe[76:84] + subframe[90:114]) * 2 ** (- 31) * gpsPi 160 | 161 | C_is = twosComp2dec(subframe[120:136]) * 2 ** (- 29) 162 | 163 | i_0 = twosComp2dec(subframe[136:144] + subframe[150:174]) * 2 ** (- 31) * gpsPi 164 | 165 | C_rc = twosComp2dec(subframe[180:196]) * 2 ** (- 5) 166 | 167 | omega = twosComp2dec(subframe[196:204] + subframe[210:234]) * 2 ** (- 31) * gpsPi 168 | 169 | omegaDot = twosComp2dec(subframe[240:264]) * 2 ** (- 43) * gpsPi 170 | 171 | IODE_sf3 = bin2dec(subframe[270:278]) 172 | 173 | iDot = twosComp2dec(subframe[278:292]) * 2 ** (- 43) * gpsPi 174 | 175 | elif 4 == subframeID: 176 | # Almanac, ionospheric model, UTC parameters. 177 | # SV health (PRN: 25-32). 178 | # Not decoded at the moment. 179 | pass 180 | elif 5 == subframeID: 181 | # SV almanac and health (PRN: 1-24). 182 | # Almanac reference week number and time. 183 | # Not decoded at the moment. 184 | pass 185 | 186 | # Compute the time of week (TOW) of the first sub-frames in the array ==== 187 | # Also correct the TOW. The transmitted TOW is actual TOW of the next 188 | # subframe and we need the TOW of the first subframe in this data block 189 | # (the variable subframe at this point contains bits of the last subframe). 190 | TOW = bin2dec(subframe[30:47]) * 6 - 30 191 | # Initialize fields for ephemeris 192 | eph = (weekNumber, accuracy, health, T_GD, IODC, t_oc, a_f2, a_f1, a_f0, 193 | IODE_sf2, C_rs, deltan, M_0, C_uc, e, C_us, sqrtA, t_oe, 194 | C_ic, omega_0, C_is, i_0, C_rc, omega, omegaDot, IODE_sf3, iDot) 195 | return eph, TOW 196 | -------------------------------------------------------------------------------- /geoFunctions/__init__.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | # cart2geo.m 5 | 6 | 7 | def cart2geo(X, Y, Z, i, *args, **kwargs): 8 | # CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical 9 | # coordinates (phi, lambda, h) on a selected reference ellipsoid. 10 | 11 | # [phi, lambda, h] = cart2geo(X, Y, Z, i); 12 | 13 | # Choices i of Reference Ellipsoid for Geographical Coordinates 14 | # 1. International Ellipsoid 1924 15 | # 2. International Ellipsoid 1967 16 | # 3. World Geodetic System 1972 17 | # 4. Geodetic Reference System 1980 18 | # 5. World Geodetic System 1984 19 | 20 | # Kai Borre 10-13-98 21 | # Copyright (c) by Kai Borre 22 | # Revision: 1.0 Date: 1998/10/23 23 | 24 | # ========================================================================== 25 | 26 | a = np.array([6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0]) 27 | 28 | f = np.array([1 / 297, 1 / 298.247, 1 / 298.26, 1 / 298.257222101, 1 / 298.257223563]) 29 | 30 | lambda_ = np.arctan2(Y, X) 31 | 32 | ex2 = (2 - f[i]) * f[i] / ((1 - f[i]) ** 2) 33 | 34 | c = a[i] * np.sqrt(1 + ex2) 35 | 36 | phi = np.arctan(Z / (np.sqrt(X ** 2 + Y ** 2) * (1 - (2 - f[i])) * f[i])) 37 | 38 | h = 0.1 39 | 40 | oldh = 0 41 | 42 | iterations = 0 43 | 44 | while abs(h - oldh) > 1e-12: 45 | 46 | oldh = h 47 | 48 | N = c / np.sqrt(1 + ex2 * np.cos(phi) ** 2) 49 | 50 | phi = np.arctan(Z / (np.sqrt(X ** 2 + Y ** 2) * (1 - (2 - f[i]) * f[i] * N / (N + h)))) 51 | 52 | h = np.sqrt(X ** 2 + Y ** 2) / np.cos(phi) - N 53 | 54 | iterations += 1 55 | 56 | if iterations > 100: 57 | print 'Failed to approximate h with desired precision. h-oldh: %e.' % (h - oldh) 58 | break 59 | 60 | phi *= (180 / np.pi) 61 | 62 | # b = zeros(1,3); 63 | # b(1,1) = fix(phi); 64 | # b(2,1) = fix(rem(phi,b(1,1))*60); 65 | # b(3,1) = (phi-b(1,1)-b(1,2)/60)*3600; 66 | 67 | lambda_ *= (180 / np.pi) 68 | 69 | # l = zeros(1,3); 70 | # l(1,1) = fix(lambda); 71 | # l(2,1) = fix(rem(lambda,l(1,1))*60); 72 | # l(3,1) = (lambda-l(1,1)-l(1,2)/60)*3600; 73 | 74 | # fprintf('\n phi =#3.0f #3.0f #8.5f',b(1),b(2),b(3)) 75 | # fprintf('\n lambda =#3.0f #3.0f #8.5f',l(1),l(2),l(3)) 76 | # fprintf('\n h =#14.3f\n',h) 77 | return phi, lambda_, h 78 | 79 | 80 | ############## end cart2geo.m ################### 81 | 82 | 83 | # clsin.m 84 | def clsin(ar, degree, argument, *args, **kwargs): 85 | # Clenshaw summation of sinus of argument. 86 | 87 | # result = clsin(ar, degree, argument); 88 | 89 | # Written by Kai Borre 90 | # December 20, 1995 91 | 92 | # See also WGS2UTM or CART2UTM 93 | 94 | # ========================================================================== 95 | 96 | cos_arg = 2 * np.cos(argument) 97 | 98 | hr1 = 0 99 | 100 | hr = 0 101 | 102 | # TODO fix index range of t 103 | for t in range(degree, 0, -1): 104 | hr2 = hr1 105 | 106 | hr1 = hr 107 | 108 | hr = ar[t - 1] + cos_arg * hr1 - hr2 109 | 110 | result = hr * np.sin(argument) 111 | return result 112 | 113 | 114 | ####################### end clsin.m ##################### 115 | 116 | 117 | # clksin.m 118 | def clksin(ar, degree, arg_real, arg_imag, *args, **kwargs): 119 | # Clenshaw summation of sinus with complex argument 120 | # [re, im] = clksin(ar, degree, arg_real, arg_imag); 121 | 122 | # Written by Kai Borre 123 | # December 20, 1995 124 | 125 | # See also WGS2UTM or CART2UTM 126 | 127 | # ========================================================================== 128 | 129 | sin_arg_r = np.sin(arg_real) 130 | 131 | cos_arg_r = np.cos(arg_real) 132 | 133 | sinh_arg_i = np.sinh(arg_imag) 134 | 135 | cosh_arg_i = np.cosh(arg_imag) 136 | 137 | r = 2 * cos_arg_r * cosh_arg_i 138 | 139 | i = - 2 * sin_arg_r * sinh_arg_i 140 | 141 | hr1 = 0 142 | 143 | hr = 0 144 | 145 | hi1 = 0 146 | 147 | hi = 0 148 | 149 | # TODO fix index range of t 150 | for t in range(degree, 0, - 1): 151 | hr2 = hr1 152 | 153 | hr1 = hr 154 | 155 | hi2 = hi1 156 | 157 | hi1 = hi 158 | 159 | z = ar[t - 1] + r * hr1 - i * hi - hr2 160 | 161 | hi = i * hr1 + r * hi1 - hi2 162 | 163 | hr = z 164 | 165 | r = sin_arg_r * cosh_arg_i 166 | 167 | i = cos_arg_r * sinh_arg_i 168 | 169 | re = r * hr - i * hi 170 | 171 | im = r * hi + i * hr 172 | return re, im 173 | 174 | 175 | # cart2utm.m 176 | def cart2utm(X, Y, Z, zone, *args, **kwargs): 177 | # CART2UTM Transformation of (X,Y,Z) to (N,E,U) in UTM, zone 'zone'. 178 | 179 | # [E, N, U] = cart2utm(X, Y, Z, zone); 180 | 181 | # Inputs: 182 | # X,Y,Z - Cartesian coordinates. Coordinates are referenced 183 | # with respect to the International Terrestrial Reference 184 | # Frame 1996 (ITRF96) 185 | # zone - UTM zone of the given position 186 | 187 | # Outputs: 188 | # E, N, U - UTM coordinates (Easting, Northing, Uping) 189 | 190 | # Kai Borre -11-1994 191 | # Copyright (c) by Kai Borre 192 | 193 | # This implementation is based upon 194 | # O. Andersson & K. Poder (1981) Koordinattransformationer 195 | # ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren 196 | # Vol. 30: 552--571 and Vol. 31: 76 197 | 198 | # An excellent, general reference (KW) is 199 | # R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der 200 | # h\"oheren Geod\"asie und Kartographie. 201 | # Erster Band, Springer Verlag 202 | 203 | # Explanation of variables used: 204 | # f flattening of ellipsoid 205 | # a semi major axis in m 206 | # m0 1 - scale at central meridian; for UTM 0.0004 207 | # Q_n normalized meridian quadrant 208 | # E0 Easting of central meridian 209 | # L0 Longitude of central meridian 210 | # bg constants for ellipsoidal geogr. to spherical geogr. 211 | # gb constants for spherical geogr. to ellipsoidal geogr. 212 | # gtu constants for ellipsoidal N, E to spherical N, E 213 | # utg constants for spherical N, E to ellipoidal N, E 214 | # tolutm tolerance for utm, 1.2E-10*meridian quadrant 215 | # tolgeo tolerance for geographical, 0.00040 second of arc 216 | 217 | # B, L refer to latitude and longitude. Southern latitude is negative 218 | # International ellipsoid of 1924, valid for ED50 219 | 220 | a = 6378388.0 221 | 222 | f = 1.0 / 297.0 223 | 224 | ex2 = (2 - f) * f / (1 - f) ** 2 225 | 226 | c = a * np.sqrt(1 + ex2) 227 | 228 | vec = np.array([X, Y, Z - 4.5]) 229 | 230 | alpha = 7.56e-07 231 | 232 | R = np.array([[1, - alpha, 0], 233 | [alpha, 1, 0], 234 | [0, 0, 1]]) 235 | 236 | trans = np.array([89.5, 93.8, 127.6]) 237 | 238 | scale = 0.9999988 239 | 240 | v = scale * R.dot(vec) + trans 241 | 242 | L = np.arctan2(v[1], v[0]) 243 | 244 | N1 = 6395000.0 245 | 246 | B = np.arctan2(v[2] / ((1 - f) ** 2 * N1), np.linalg.norm(v[0:2]) / N1) 247 | 248 | U = 0.1 249 | 250 | oldU = 0 251 | 252 | iterations = 0 253 | 254 | while abs(U - oldU) > 0.0001: 255 | 256 | oldU = U 257 | 258 | N1 = c / np.sqrt(1 + ex2 * (np.cos(B)) ** 2) 259 | 260 | B = np.arctan2(v[2] / ((1 - f) ** 2 * N1 + U), np.linalg.norm(v[0:2]) / (N1 + U)) 261 | 262 | U = np.linalg.norm(v[0:2]) / np.cos(B) - N1 263 | 264 | iterations += 1 265 | 266 | if iterations > 100: 267 | print 'Failed to approximate U with desired precision. U-oldU: %e.' % (U - oldU) 268 | break 269 | 270 | # Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21) 271 | m0 = 0.0004 272 | 273 | n = f / (2 - f) 274 | 275 | m = n ** 2 * (1.0 / 4.0 + n ** 2 / 64) 276 | 277 | w = (a * (-n - m0 + m * (1 - m0))) / (1 + n) 278 | 279 | Q_n = a + w 280 | 281 | # Easting and longitude of central meridian 282 | E0 = 500000.0 283 | 284 | L0 = (zone - 30) * 6 - 3 285 | 286 | # Check tolerance for reverse transformation 287 | tolutm = np.pi / 2 * 1.2e-10 * Q_n 288 | 289 | tolgeo = 4e-05 290 | 291 | # Coefficients of trigonometric series 292 | 293 | # ellipsoidal to spherical geographical, KW p. 186--187, (51)-(52) 294 | # bg[1] = n*(-2 + n*(2/3 + n*(4/3 + n*(-82/45)))); 295 | # bg[2] = n^2*(5/3 + n*(-16/15 + n*(-13/9))); 296 | # bg[3] = n^3*(-26/15 + n*34/21); 297 | # bg[4] = n^4*1237/630; 298 | 299 | # spherical to ellipsoidal geographical, KW p. 190--191, (61)-(62) 300 | # gb[1] = n*(2 + n*(-2/3 + n*(-2 + n*116/45))); 301 | # gb[2] = n^2*(7/3 + n*(-8/5 + n*(-227/45))); 302 | # gb[3] = n^3*(56/15 + n*(-136/35)); 303 | # gb[4] = n^4*4279/630; 304 | 305 | # spherical to ellipsoidal N, E, KW p. 196, (69) 306 | # gtu[1] = n*(1/2 + n*(-2/3 + n*(5/16 + n*41/180))); 307 | # gtu[2] = n^2*(13/48 + n*(-3/5 + n*557/1440)); 308 | # gtu[3] = n^3*(61/240 + n*(-103/140)); 309 | # gtu[4] = n^4*49561/161280; 310 | 311 | # ellipsoidal to spherical N, E, KW p. 194, (65) 312 | # utg[1] = n*(-1/2 + n*(2/3 + n*(-37/96 + n*1/360))); 313 | # utg[2] = n^2*(-1/48 + n*(-1/15 + n*437/1440)); 314 | # utg[3] = n^3*(-17/480 + n*37/840); 315 | # utg[4] = n^4*(-4397/161280); 316 | 317 | # With f = 1/297 we get 318 | 319 | bg = np.array([- 0.00337077907, 4.73444769e-06, -8.2991457e-09, 1.5878533e-11]) 320 | 321 | gb = np.array([0.00337077588, 6.6276908e-06, 1.78718601e-08, 5.49266312e-11]) 322 | 323 | gtu = np.array([0.000841275991, 7.67306686e-07, 1.2129123e-09, 2.48508228e-12]) 324 | 325 | utg = np.array([-0.000841276339, -5.95619298e-08, -1.69485209e-10, -2.20473896e-13]) 326 | 327 | # Ellipsoidal latitude, longitude to spherical latitude, longitude 328 | neg_geo = False 329 | 330 | if B < 0: 331 | neg_geo = True 332 | 333 | Bg_r = np.abs(B) 334 | 335 | res_clensin = clsin(bg, 4, 2 * Bg_r) 336 | 337 | Bg_r = Bg_r + res_clensin 338 | 339 | L0 = L0 * np.pi / 180 340 | 341 | Lg_r = L - L0 342 | 343 | # Spherical latitude, longitude to complementary spherical latitude 344 | # i.e. spherical N, E 345 | cos_BN = np.cos(Bg_r) 346 | 347 | Np = np.arctan2(np.sin(Bg_r), np.cos(Lg_r) * cos_BN) 348 | 349 | Ep = np.arctanh(np.sin(Lg_r) * cos_BN) 350 | 351 | # Spherical normalized N, E to ellipsoidal N, E 352 | Np *= 2 353 | 354 | Ep *= 2 355 | 356 | dN, dE = clksin(gtu, 4, Np, Ep) 357 | 358 | Np /= 2 359 | 360 | Ep /= 2 361 | 362 | Np += dN 363 | 364 | Ep += dE 365 | 366 | N = Q_n * Np 367 | 368 | E = Q_n * Ep + E0 369 | 370 | if neg_geo: 371 | N = -N + 20000000 372 | return E, N, U 373 | 374 | 375 | #################### end cart2utm.m #################### 376 | 377 | 378 | # deg2dms.m 379 | def deg2dms(deg, *args, **kwargs): 380 | # DEG2DMS Conversion of degrees to degrees, minutes, and seconds. 381 | # The output format (dms format) is: (degrees*100 + minutes + seconds/100) 382 | 383 | # Written by Kai Borre 384 | # February 7, 2001 385 | # Updated by Darius Plausinaitis 386 | 387 | # Save the sign for later processing 388 | neg_arg = False 389 | 390 | if deg < 0: 391 | # Only positive numbers should be used while spliting into deg/min/sec 392 | deg = -deg 393 | 394 | neg_arg = True 395 | 396 | # Split degrees minutes and seconds 397 | int_deg = np.floor(deg) 398 | 399 | decimal = deg - int_deg 400 | 401 | min_part = decimal * 60 402 | 403 | min_ = np.floor(min_part) 404 | 405 | sec_part = min_part - np.floor(min_part) 406 | 407 | sec = sec_part * 60 408 | 409 | # Check for overflow 410 | if sec == 60.0: 411 | min_ = min_ + 1 412 | 413 | sec = 0.0 414 | 415 | if min_ == 60.0: 416 | int_deg = int_deg + 1 417 | 418 | min_ = 0.0 419 | 420 | # Construct the output 421 | dmsOutput = int_deg * 100 + min_ + sec / 100 422 | 423 | # Correct the sign 424 | if neg_arg: 425 | dmsOutput = -dmsOutput 426 | return dmsOutput 427 | 428 | 429 | ################### end deg2dms.m ################ 430 | 431 | 432 | # dms2mat.m 433 | def dms2mat(dmsInput, n, *args, **kwargs): 434 | # DMS2MAT Splits a real a = dd*100 + mm + s/100 into[dd mm s.ssss] 435 | # where n specifies the power of 10, to which the resulting seconds 436 | # of the output should be rounded. E.g.: if a result is 23.823476 437 | # seconds, and n = -3, then the output will be 23.823. 438 | 439 | # Written by Kai Borre 440 | # January 7, 2007 441 | # Updated by Darius Plausinaitis 442 | 443 | neg_arg = False 444 | 445 | if dmsInput < 0: 446 | # Only positive numbers should be used while spliting into deg/min/sec 447 | dmsInput = -dmsInput 448 | 449 | neg_arg = True 450 | 451 | # Split degrees minutes and seconds 452 | int_deg = np.floor(dmsInput / 100) 453 | 454 | mm = np.floor(dmsInput - 100 * int_deg) 455 | 456 | # we assume n<7; hence #2.10f is sufficient to hold ssdec 457 | ssdec = '%2.10f' % (dmsInput - 100 * int_deg - mm) * 100 458 | 459 | # Check for overflow 460 | if ssdec == 60.0: 461 | mm = mm + 1 462 | 463 | ssdec = 0.0 464 | 465 | if mm == 60.0: 466 | int_deg = int_deg + 1 467 | 468 | mm = 0.0 469 | 470 | # Corect the sign 471 | if neg_arg: 472 | int_deg = -int_deg 473 | 474 | # Compose the output 475 | matOutput = [] 476 | matOutput[0] = int_deg 477 | 478 | matOutput[1] = mm 479 | 480 | matOutput[2] = float(ssdec[0:- n + 3]) 481 | 482 | return matOutput 483 | 484 | 485 | ################### end dms2mat.m ################ 486 | 487 | 488 | # e_r_corr.m 489 | 490 | 491 | def e_r_corr(traveltime, X_sat, *args, **kwargs): 492 | # E_R_CORR Returns rotated satellite ECEF coordinates due to Earth 493 | # rotation during signal travel time 494 | 495 | # X_sat_rot = e_r_corr(traveltime, X_sat); 496 | 497 | # Inputs: 498 | # travelTime - signal travel time 499 | # X_sat - satellite's ECEF coordinates 500 | 501 | # Outputs: 502 | # X_sat_rot - rotated satellite's coordinates (ECEF) 503 | 504 | # Written by Kai Borre 505 | # Copyright (c) by Kai Borre 506 | 507 | # ========================================================================== 508 | 509 | Omegae_dot = 7.292115147e-05 510 | 511 | # --- Find rotation angle -------------------------------------------------- 512 | omegatau = Omegae_dot * traveltime 513 | 514 | # --- Make a rotation matrix ----------------------------------------------- 515 | R3 = np.array([[np.cos(omegatau), np.sin(omegatau), 0.0], 516 | [-np.sin(omegatau), np.cos(omegatau), 0.0], 517 | [0.0, 0.0, 1.0]]) 518 | 519 | # --- Do the rotation ------------------------------------------------------ 520 | X_sat_rot = R3.dot(X_sat) 521 | return X_sat_rot 522 | 523 | 524 | ######## end e_r_corr.m #################### 525 | 526 | # findUtmZone.m 527 | 528 | 529 | def findUtmZone(latitude, longitude, *args, **kwargs): 530 | # Function finds the UTM zone number for given longitude and latitude. 531 | # The longitude value must be between -180 (180 degree West) and 180 (180 532 | # degree East) degree. The latitude must be within -80 (80 degree South) and 533 | # 84 (84 degree North). 534 | 535 | # utmZone = findUtmZone(latitude, longitude); 536 | 537 | # Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not 538 | # 15 deg 30 min). 539 | 540 | # Check value bounds ===================================================== 541 | 542 | if longitude > 180 or longitude < - 180: 543 | raise IOError('Longitude value exceeds limits (-180:180).') 544 | 545 | if latitude > 84 or latitude < - 80: 546 | raise IOError('Latitude value exceeds limits (-80:84).') 547 | 548 | # Find zone ============================================================== 549 | 550 | # Start at 180 deg west = -180 deg 551 | 552 | utmZone = np.fix((180 + longitude) / 6) + 1 553 | 554 | # Correct zone numbers for particular areas ============================== 555 | 556 | if latitude > 72: 557 | # Corrections for zones 31 33 35 37 558 | if 0 <= longitude < 9: 559 | utmZone = 31 560 | 561 | elif 9 <= longitude < 21: 562 | utmZone = 33 563 | 564 | elif 21 <= longitude < 33: 565 | utmZone = 35 566 | 567 | elif 33 <= longitude < 42: 568 | utmZone = 37 569 | 570 | elif 56 <= latitude < 64: 571 | # Correction for zone 32 572 | if 3 <= longitude < 12: 573 | utmZone = 32 574 | return utmZone 575 | 576 | 577 | # geo2cart.m 578 | def geo2cart(phi, lambda_, h, i=4, *args, **kwargs): 579 | # GEO2CART Conversion of geographical coordinates (phi, lambda, h) to 580 | # Cartesian coordinates (X, Y, Z). 581 | 582 | # [X, Y, Z] = geo2cart(phi, lambda, h, i); 583 | 584 | # Format for phi and lambda: [degrees minutes seconds]. 585 | # h, X, Y, and Z are in meters. 586 | 587 | # Choices i of Reference Ellipsoid 588 | # 1. International Ellipsoid 1924 589 | # 2. International Ellipsoid 1967 590 | # 3. World Geodetic System 1972 591 | # 4. Geodetic Reference System 1980 592 | # 5. World Geodetic System 1984 593 | 594 | # Inputs: 595 | # phi - geocentric latitude (format [degrees minutes seconds]) 596 | # lambda - geocentric longitude (format [degrees minutes seconds]) 597 | # h - height 598 | # i - reference ellipsoid type 599 | 600 | # Outputs: 601 | # X, Y, Z - Cartesian coordinates (meters) 602 | 603 | # Kai Borre 10-13-98 604 | # Copyright (c) by Kai Borre 605 | 606 | # ========================================================================== 607 | 608 | b = phi[0] + phi[1] / 60. + phi[2] / 3600. 609 | 610 | b = b * np.pi / 180. 611 | 612 | l = lambda_[1] + lambda_[2] / 60. + lambda_[3] / 3600. 613 | 614 | l = l * np.pi / 180. 615 | 616 | a = [6378388, 6378160, 6378135, 6378137, 6378137] 617 | 618 | f = [1 / 297, 1 / 298.247, 1 / 298.26, 1 / 298.257222101, 1 / 298.257223563] 619 | 620 | ex2 = (2. - f[i]) * f[i] / (1. - f[i]) ** 2 621 | 622 | c = a[i] * np.sqrt(1. + ex2) 623 | 624 | N = c / np.sqrt(1. + ex2 * np.cos(b) ** 2) 625 | 626 | X = (N + h) * np.cos(b) * np.cos(l) 627 | 628 | Y = (N + h) * np.cos(b) * np.sin(l) 629 | 630 | Z = ((1. - f[i]) ** 2 * N + h) * np.sin(b) 631 | 632 | return X, Y, Z 633 | 634 | 635 | # leastSquarePos.m 636 | def leastSquarePos(satpos_, obs, settings, *args, **kwargs): 637 | # Function calculates the Least Square Solution. 638 | 639 | # [pos, el, az, dop] = leastSquarePos(satpos, obs, settings); 640 | 641 | # Inputs: 642 | # satpos - Satellites positions (in ECEF system: [X; Y; Z;] - 643 | # one column per satellite) 644 | # obs - Observations - the pseudorange measurements to each 645 | # satellite: 646 | # (e.g. [20000000 21000000 .... .... .... .... ....]) 647 | # settings - receiver settings 648 | 649 | # Outputs: 650 | # pos - receiver position and receiver clock error 651 | # (in ECEF system: [X, Y, Z, dt]) 652 | # el - Satellites elevation angles (degrees) 653 | # az - Satellites azimuth angles (degrees) 654 | # dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP]) 655 | 656 | # === Initialization ======================================================= 657 | nmbOfIterations = 7 658 | 659 | dtr = np.pi / 180 660 | 661 | pos = np.zeros(4) 662 | 663 | X = satpos_.copy() 664 | 665 | nmbOfSatellites = satpos_.shape[1] 666 | 667 | A = np.zeros((nmbOfSatellites, 4)) 668 | 669 | omc = np.zeros(nmbOfSatellites) 670 | 671 | az = np.zeros(nmbOfSatellites) 672 | 673 | el = np.zeros(nmbOfSatellites) 674 | 675 | dop = np.zeros(5) 676 | # === Iteratively find receiver position =================================== 677 | for iter_ in range(nmbOfIterations): 678 | for i in range(nmbOfSatellites): 679 | if iter_ == 0: 680 | # --- Initialize variables at the first iteration -------------- 681 | Rot_X = X[:, i].copy() 682 | 683 | trop = 2 684 | 685 | else: 686 | # --- Update equations ----------------------------------------- 687 | rho2 = (X[0, i] - pos[0]) ** 2 + (X[1, i] - pos[1]) ** 2 + (X[2, i] - pos[2]) ** 2 688 | 689 | traveltime = np.sqrt(rho2) / settings.c 690 | 691 | Rot_X = e_r_corr(traveltime, X[:, i]) 692 | 693 | az[i], el[i], dist = topocent(pos[0:3], Rot_X - pos[0:3]) 694 | 695 | if settings.useTropCorr: 696 | # --- Calculate tropospheric correction -------------------- 697 | trop = tropo(np.sin(el[i] * dtr), 0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0) 698 | 699 | else: 700 | # Do not calculate or apply the tropospheric corrections 701 | trop = 0 702 | 703 | # --- Apply the corrections ---------------------------------------- 704 | omc[i] = obs[i] - np.linalg.norm(Rot_X - pos[0:3]) - pos[3] - trop 705 | 706 | A[i, :] = np.array([-(Rot_X[0] - pos[0]) / obs[i], 707 | -(Rot_X[1] - pos[1]) / obs[i], 708 | -(Rot_X[2] - pos[2]) / obs[i], 709 | 1]) 710 | 711 | # These lines allow the code to exit gracefully in case of any errors 712 | if np.linalg.matrix_rank(A) != 4: 713 | pos = np.zeros((4, 1)) 714 | 715 | return pos, el, az, dop 716 | # --- Find position update --------------------------------------------- 717 | x = np.linalg.lstsq(A, omc, rcond=None)[0] 718 | 719 | pos = pos + x.flatten() 720 | 721 | pos = pos.T 722 | 723 | # === Calculate Dilution Of Precision ====================================== 724 | # --- Initialize output ------------------------------------------------ 725 | # dop = np.zeros((1, 5)) 726 | 727 | Q = np.linalg.inv(A.T.dot(A)) 728 | 729 | dop[0] = np.sqrt(np.trace(Q)) 730 | 731 | dop[1] = np.sqrt(Q[0, 0] + Q[1, 1] + Q[2, 2]) 732 | 733 | dop[2] = np.sqrt(Q[0, 0] + Q[1, 1]) 734 | 735 | dop[3] = np.sqrt(Q[2, 2]) 736 | 737 | dop[4] = np.sqrt(Q[3, 3]) 738 | 739 | return pos, el, az, dop 740 | 741 | 742 | # check_t.m 743 | 744 | 745 | def check_t(time, *args, **kwargs): 746 | # CHECK_T accounting for beginning or end of week crossover. 747 | 748 | # corrTime = check_t(time); 749 | 750 | # Inputs: 751 | # time - time in seconds 752 | 753 | # Outputs: 754 | # corrTime - corrected time (seconds) 755 | 756 | # Kai Borre 04-01-96 757 | # Copyright (c) by Kai Borre 758 | 759 | # ========================================================================== 760 | 761 | half_week = 302400.0 762 | 763 | corrTime = time 764 | 765 | if time > half_week: 766 | corrTime = time - 2 * half_week 767 | 768 | elif time < - half_week: 769 | corrTime = time + 2 * half_week 770 | return corrTime 771 | 772 | 773 | ####### end check_t.m ################# 774 | 775 | 776 | # satpos.m 777 | 778 | 779 | def satpos(transmitTime, prnList, eph, settings, *args, **kwargs): 780 | # SATPOS Computation of satellite coordinates X,Y,Z at TRANSMITTIME for 781 | # given ephemeris EPH. Coordinates are computed for each satellite in the 782 | # list PRNLIST. 783 | # [satPositions, satClkCorr] = satpos(transmitTime, prnList, eph, settings); 784 | 785 | # Inputs: 786 | # transmitTime - transmission time 787 | # prnList - list of PRN-s to be processed 788 | # eph - ephemerides of satellites 789 | # settings - receiver settings 790 | 791 | # Outputs: 792 | # satPositions - position of satellites (in ECEF system [X; Y; Z;]) 793 | # satClkCorr - correction of satellite clocks 794 | 795 | # Initialize constants =================================================== 796 | numOfSatellites = prnList.size 797 | 798 | # GPS constatns 799 | 800 | gpsPi = 3.14159265359 801 | 802 | # system 803 | 804 | # --- Constants for satellite position calculation ------------------------- 805 | Omegae_dot = 7.2921151467e-05 806 | 807 | GM = 3.986005e+14 808 | 809 | # the mass of the Earth, [m^3/s^2] 810 | F = - 4.442807633e-10 811 | 812 | # Initialize results ===================================================== 813 | satClkCorr = np.zeros(numOfSatellites) 814 | 815 | satPositions = np.zeros((3, numOfSatellites)) 816 | 817 | # Process each satellite ================================================= 818 | 819 | for satNr in range(numOfSatellites): 820 | prn = prnList[satNr] - 1 821 | 822 | # Find initial satellite clock correction -------------------------------- 823 | # --- Find time difference --------------------------------------------- 824 | dt = check_t(transmitTime - eph[prn].t_oc) 825 | 826 | satClkCorr[satNr] = (eph[prn].a_f2 * dt + eph[prn].a_f1) * dt + eph[prn].a_f0 - eph[prn].T_GD 827 | 828 | time = transmitTime - satClkCorr[satNr] 829 | 830 | # Find satellite's position ---------------------------------------------- 831 | # Restore semi-major axis 832 | a = eph[prn].sqrtA * eph[prn].sqrtA 833 | 834 | tk = check_t(time - eph[prn].t_oe) 835 | 836 | n0 = np.sqrt(GM / a ** 3) 837 | 838 | n = n0 + eph[prn].deltan 839 | 840 | M = eph[prn].M_0 + n * tk 841 | 842 | M = np.remainder(M + 2 * gpsPi, 2 * gpsPi) 843 | 844 | E = M 845 | 846 | for ii in range(10): 847 | E_old = E 848 | 849 | E = M + eph[prn].e * np.sin(E) 850 | 851 | dE = np.remainder(E - E_old, 2 * gpsPi) 852 | 853 | if abs(dE) < 1e-12: 854 | # Necessary precision is reached, exit from the loop 855 | break 856 | # Reduce eccentric anomaly to between 0 and 360 deg 857 | E = np.remainder(E + 2 * gpsPi, 2 * gpsPi) 858 | 859 | dtr = F * eph[prn].e * eph[prn].sqrtA * np.sin(E) 860 | 861 | nu = np.arctan2(np.sqrt(1 - eph[prn].e ** 2) * np.sin(E), np.cos(E) - eph[prn].e) 862 | 863 | phi = nu + eph[prn].omega 864 | 865 | phi = np.remainder(phi, 2 * gpsPi) 866 | 867 | u = phi + eph[prn].C_uc * np.cos(2 * phi) + eph[prn].C_us * np.sin(2 * phi) 868 | 869 | r = a * (1 - eph[prn].e * np.cos(E)) + eph[prn].C_rc * np.cos(2 * phi) + eph[prn].C_rs * np.sin(2 * phi) 870 | 871 | i = eph[prn].i_0 + eph[prn].iDot * tk + eph[prn].C_ic * np.cos(2 * phi) + eph[prn].C_is * np.sin(2 * phi) 872 | 873 | Omega = eph[prn].omega_0 + (eph[prn].omegaDot - Omegae_dot) * tk - Omegae_dot * eph[prn].t_oe 874 | 875 | Omega = np.remainder(Omega + 2 * gpsPi, 2 * gpsPi) 876 | 877 | satPositions[0, satNr] = np.cos(u) * r * np.cos(Omega) - np.sin(u) * r * np.cos(i) * np.sin(Omega) 878 | 879 | satPositions[1, satNr] = np.cos(u) * r * np.sin(Omega) + np.sin(u) * r * np.cos(i) * np.cos(Omega) 880 | 881 | satPositions[2, satNr] = np.sin(u) * r * np.sin(i) 882 | 883 | # Include relativistic correction in clock correction -------------------- 884 | satClkCorr[satNr] = (eph[prn].a_f2 * dt + eph[prn].a_f1) * dt + eph[prn].a_f0 - eph[prn].T_GD + dtr 885 | return satPositions, satClkCorr 886 | 887 | 888 | # topocent.m 889 | 890 | 891 | # togeod.m 892 | def togeod(a, finv, X, Y, Z, *args, **kwargs): 893 | # TOGEOD Subroutine to calculate geodetic coordinates latitude, longitude, 894 | # height given Cartesian coordinates X,Y,Z, and reference ellipsoid 895 | # values semi-major axis (a) and the inverse of flattening (finv). 896 | 897 | # [dphi, dlambda, h] = togeod(a, finv, X, Y, Z); 898 | 899 | # The units of linear parameters X,Y,Z,a must all agree (m,km,mi,ft,..etc) 900 | # The output units of angular quantities will be in decimal degrees 901 | # (15.5 degrees not 15 deg 30 min). The output units of h will be the 902 | # same as the units of X,Y,Z,a. 903 | 904 | # Inputs: 905 | # a - semi-major axis of the reference ellipsoid 906 | # finv - inverse of flattening of the reference ellipsoid 907 | # X,Y,Z - Cartesian coordinates 908 | 909 | # Outputs: 910 | # dphi - latitude 911 | # dlambda - longitude 912 | # h - height above reference ellipsoid 913 | 914 | # Copyright (C) 1987 C. Goad, Columbus, Ohio 915 | # Reprinted with permission of author, 1996 916 | # Fortran code translated into MATLAB 917 | # Kai Borre 03-30-96 918 | 919 | # ========================================================================== 920 | 921 | h = 0.0 922 | 923 | tolsq = 1e-10 924 | 925 | maxit = 10 926 | 927 | # compute radians-to-degree factor 928 | rtd = 180 / np.pi 929 | 930 | # compute square of eccentricity 931 | if finv < 1e-20: 932 | esq = 0.0 933 | 934 | else: 935 | esq = (2 - 1 / finv) / finv 936 | 937 | oneesq = 1 - esq 938 | 939 | # first guess 940 | # P is distance from spin axis 941 | P = np.sqrt(X ** 2 + Y ** 2) 942 | 943 | # direct calculation of longitude 944 | 945 | if P > 1e-20: 946 | dlambda = np.arctan2(Y, X) * rtd 947 | 948 | else: 949 | dlambda = 0.0 950 | 951 | if dlambda < 0: 952 | dlambda = dlambda + 360 953 | 954 | # r is distance from origin (0,0,0) 955 | r = np.sqrt(P ** 2 + Z ** 2) 956 | 957 | if r > 1e-20: 958 | sinphi = Z / r 959 | 960 | else: 961 | sinphi = 0.0 962 | 963 | dphi = np.arcsin(sinphi) 964 | 965 | # initial value of height = distance from origin minus 966 | # approximate distance from origin to surface of ellipsoid 967 | if r < 1e-20: 968 | h = 0.0 969 | 970 | return dphi, dlambda, h 971 | 972 | h = r - a * (1 - sinphi * sinphi / finv) 973 | 974 | # iterate 975 | for i in range(maxit): 976 | sinphi = np.sin(dphi) 977 | 978 | cosphi = np.cos(dphi) 979 | 980 | N_phi = a / np.sqrt(1 - esq * sinphi * sinphi) 981 | 982 | dP = P - (N_phi + h) * cosphi 983 | 984 | dZ = Z - (N_phi * oneesq + h) * sinphi 985 | 986 | h = h + sinphi * dZ + cosphi * dP 987 | 988 | dphi = dphi + (cosphi * dZ - sinphi * dP) / (N_phi + h) 989 | 990 | if (dP * dP + dZ * dZ) < tolsq: 991 | break 992 | # Not Converged--Warn user 993 | if i == maxit - 1: 994 | print ' Problem in TOGEOD, did not converge in %2.0f iterations' % i 995 | 996 | dphi *= rtd 997 | return dphi, dlambda, h 998 | 999 | 1000 | ######## end togeod.m ###################### 1001 | 1002 | 1003 | def topocent(X, dx, *args, **kwargs): 1004 | # TOPOCENT Transformation of vector dx into topocentric coordinate 1005 | # system with origin at X. 1006 | # Both parameters are 3 by 1 vectors. 1007 | 1008 | # [Az, El, D] = topocent(X, dx); 1009 | 1010 | # Inputs: 1011 | # X - vector origin corrdinates (in ECEF system [X; Y; Z;]) 1012 | # dx - vector ([dX; dY; dZ;]). 1013 | 1014 | # Outputs: 1015 | # D - vector length. Units like units of the input 1016 | # Az - azimuth from north positive clockwise, degrees 1017 | # El - elevation angle, degrees 1018 | 1019 | # Kai Borre 11-24-96 1020 | # Copyright (c) by Kai Borre 1021 | 1022 | # ========================================================================== 1023 | 1024 | dtr = np.pi / 180 1025 | 1026 | phi, lambda_, h = togeod(6378137, 298.257223563, X[0], X[1], X[2]) 1027 | 1028 | cl = np.cos(lambda_ * dtr) 1029 | 1030 | sl = np.sin(lambda_ * dtr) 1031 | 1032 | cb = np.cos(phi * dtr) 1033 | 1034 | sb = np.sin(phi * dtr) 1035 | 1036 | F = np.array([[- sl, -sb * cl, cb * cl], [cl, -sb * sl, cb * sl], [0.0, cb, sb]]) 1037 | 1038 | local_vector = F.T.dot(dx) 1039 | 1040 | E = local_vector[0] 1041 | 1042 | N = local_vector[1] 1043 | 1044 | U = local_vector[2] 1045 | 1046 | hor_dis = np.sqrt(E ** 2 + N ** 2) 1047 | 1048 | if hor_dis < 1e-20: 1049 | Az = 0.0 1050 | 1051 | El = 90.0 1052 | 1053 | else: 1054 | Az = np.arctan2(E, N) / dtr 1055 | 1056 | El = np.arctan2(U, hor_dis) / dtr 1057 | 1058 | if Az < 0: 1059 | Az = Az + 360 1060 | 1061 | D = np.sqrt(dx[0] ** 2 + dx[1] ** 2 + dx[2] ** 2) 1062 | return Az, El, D 1063 | 1064 | 1065 | ######### end topocent.m ######### 1066 | 1067 | 1068 | # tropo.m 1069 | 1070 | 1071 | def tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum): 1072 | # TROPO Calculation of tropospheric correction. 1073 | # The range correction ddr in m is to be subtracted from 1074 | # pseudo-ranges and carrier phases 1075 | 1076 | # ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum); 1077 | 1078 | # Inputs: 1079 | # sinel - sin of elevation angle of satellite 1080 | # hsta - height of station in km 1081 | # p - atmospheric pressure in mb at height hp 1082 | # tkel - surface temperature in degrees Kelvin at height htkel 1083 | # hum - humidity in # at height hhum 1084 | # hp - height of pressure measurement in km 1085 | # htkel - height of temperature measurement in km 1086 | # hhum - height of humidity measurement in km 1087 | 1088 | # Outputs: 1089 | # ddr - range correction (meters) 1090 | 1091 | # Reference 1092 | # Goad, C.C. & Goodman, L. (1974) A Modified Tropospheric 1093 | # Refraction Correction Model. Paper presented at the 1094 | # American Geophysical Union Annual Fall Meeting, San 1095 | # Francisco, December 12-17 1096 | 1097 | # A Matlab reimplementation of a C code from driver. 1098 | # Kai Borre 06-28-95 1099 | 1100 | # ========================================================================== 1101 | 1102 | a_e = 6378.137 1103 | 1104 | b0 = 7.839257e-05 1105 | 1106 | tlapse = -6.5 1107 | 1108 | tkhum = tkel + tlapse * (hhum - htkel) 1109 | 1110 | atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15) 1111 | 1112 | e0 = 0.0611 * hum * 10 ** atkel 1113 | 1114 | tksea = tkel - tlapse * htkel 1115 | 1116 | em = -978.77 / (2870400.0 * tlapse * 1e-05) 1117 | 1118 | tkelh = tksea + tlapse * hhum 1119 | 1120 | e0sea = e0 * (tksea / tkelh) ** (4 * em) 1121 | 1122 | tkelp = tksea + tlapse * hp 1123 | 1124 | psea = p * (tksea / tkelp) ** em 1125 | 1126 | if sinel < 0: 1127 | sinel = 0 1128 | 1129 | tropo_ = 0.0 1130 | 1131 | done = False 1132 | 1133 | refsea = 7.7624e-05 / tksea 1134 | 1135 | htop = 1.1385e-05 / refsea 1136 | 1137 | refsea = refsea * psea 1138 | 1139 | ref = refsea * ((htop - hsta) / htop) ** 4 1140 | 1141 | while 1: 1142 | rtop = (a_e + htop) ** 2 - (a_e + hsta) ** 2 * (1 - sinel ** 2) 1143 | 1144 | if rtop < 0: 1145 | rtop = 0 1146 | 1147 | rtop = np.sqrt(rtop) - (a_e + hsta) * sinel 1148 | 1149 | a = -sinel / (htop - hsta) 1150 | 1151 | b = -b0 * (1 - sinel ** 2) / (htop - hsta) 1152 | 1153 | rn = np.zeros(8) 1154 | 1155 | for i in range(8): 1156 | rn[i] = rtop ** (i + 2) 1157 | 1158 | alpha = np.array([2 * a, 2 * a ** 2 + 4 * b / 3, a * (a ** 2 + 3 * b), 1159 | a ** 4 / 5 + 2.4 * a ** 2 * b + 1.2 * b ** 2, 1160 | 2 * a * b * (a ** 2 + 3 * b) / 3, 1161 | b ** 2 * (6 * a ** 2 + 4 * b) * 0.1428571, 0, 0]) 1162 | 1163 | if b ** 2 > 1e-35: 1164 | alpha[6] = a * b ** 3 / 2 1165 | 1166 | alpha[7] = b ** 4 / 9 1167 | 1168 | dr = rtop 1169 | 1170 | dr = dr + alpha.dot(rn) 1171 | 1172 | tropo_ += dr * ref * 1000 1173 | 1174 | if done: 1175 | ddr = tropo_ 1176 | 1177 | break 1178 | done = True 1179 | 1180 | refsea = (0.3719 / tksea - 1.292e-05) / tksea 1181 | 1182 | htop = 1.1385e-05 * (1255.0 / tksea + 0.05) / refsea 1183 | 1184 | ref = refsea * e0sea * ((htop - hsta) / htop) ** 4 1185 | return ddr 1186 | 1187 | 1188 | ######### end tropo.m ################### 1189 | 1190 | 1191 | if __name__ == '__main__': 1192 | # print "This program is being run by itself" 1193 | pass 1194 | else: 1195 | print 'Importing functions from ./geoFunctions/' 1196 | -------------------------------------------------------------------------------- /initialize.py: -------------------------------------------------------------------------------- 1 | # ./initSettings.m 2 | 3 | # Functions initializes and saves settings. Settings can be edited inside of 4 | # the function, updated from the command line or updated using a dedicated 5 | # GUI - "setSettings". 6 | 7 | # All settings are described inside function code. 8 | 9 | # settings = initSettings() 10 | 11 | # Inputs: none 12 | 13 | # Outputs: 14 | # settings - Receiver settings (a structure). 15 | import datetime 16 | 17 | import numpy as np 18 | 19 | 20 | class Result(object): 21 | def __init__(self, settings): 22 | self._settings = settings 23 | self._results = None 24 | self._channels = None 25 | 26 | @property 27 | def settings(self): 28 | return self._settings 29 | 30 | @property 31 | def channels(self): 32 | assert isinstance(self._channels, np.recarray) 33 | return self._channels 34 | 35 | @property 36 | def results(self): 37 | assert isinstance(self._results, np.recarray) 38 | return self._results 39 | 40 | @results.setter 41 | def results(self, records): 42 | assert isinstance(records, np.recarray) 43 | self._results = records 44 | 45 | def plot(self): 46 | pass 47 | 48 | 49 | class TruePosition(object): 50 | def __init__(self): 51 | self._E = None 52 | self._N = None 53 | self._U = None 54 | 55 | @property 56 | def E(self): 57 | return self._E 58 | 59 | @E.setter 60 | def E(self, e): 61 | self._E = e 62 | 63 | @property 64 | def N(self): 65 | return self._N 66 | 67 | @N.setter 68 | def N(self, n): 69 | self._N = n 70 | 71 | @property 72 | def U(self): 73 | return self._U 74 | 75 | @U.setter 76 | def U(self, u): 77 | self._U = u 78 | 79 | 80 | class Settings(object): 81 | def __init__(self): 82 | # Processing settings ==================================================== 83 | # Number of milliseconds to be processed used 36000 + any transients (see 84 | # below - in Nav parameters) to ensure nav subframes are provided 85 | self.msToProcess = 37000.0 86 | 87 | # Number of channels to be used for signal processing 88 | self.numberOfChannels = 8 89 | 90 | # Move the starting point of processing. Can be used to start the signal 91 | # processing at any point in the data record (e.g. for long records). fseek 92 | # function is used to move the file read point, therefore advance is byte 93 | # based only. 94 | self.skipNumberOfBytes = 0 95 | 96 | # Raw signal file name and other parameter =============================== 97 | # This is a "default" name of the data file (signal record) to be used in 98 | # the post-processing mode 99 | self.fileName = '/Users/yangsu/Downloads/GNSS_signal_records/GPSdata-DiscreteComponents-fs38_192-if9_55.bin' 100 | 101 | # Data type used to store one sample 102 | self.dataType = 'int8' 103 | 104 | # Intermediate, sampling and code frequencies 105 | self.IF = 9548000.0 106 | 107 | self.samplingFreq = 38192000.0 108 | 109 | self.codeFreqBasis = 1023000.0 110 | 111 | # Define number of chips in a code period 112 | self.codeLength = 1023 113 | 114 | # Acquisition settings =================================================== 115 | # Skips acquisition in the script postProcessing.m if set to 1 116 | self.skipAcquisition = False 117 | 118 | # List of satellites to look for. Some satellites can be excluded to speed 119 | # up acquisition 120 | self.acqSatelliteList = range(1, 33) 121 | 122 | # Band around IF to search for satellite signal. Depends on max Doppler 123 | self.acqSearchBand = 14.0 124 | 125 | # Threshold for the signal presence decision rule 126 | self.acqThreshold = 2.5 127 | 128 | # Tracking loops settings ================================================ 129 | # Code tracking loop parameters 130 | self.dllDampingRatio = 0.7 131 | 132 | self.dllNoiseBandwidth = 2.0 133 | 134 | self.dllCorrelatorSpacing = 0.5 135 | 136 | # Carrier tracking loop parameters 137 | self.pllDampingRatio = 0.7 138 | 139 | self.pllNoiseBandwidth = 25.0 140 | 141 | # Navigation solution settings =========================================== 142 | 143 | # Period for calculating pseudoranges and position 144 | self.navSolPeriod = 500.0 145 | 146 | # Elevation mask to exclude signals from satellites at low elevation 147 | self.elevationMask = 10.0 148 | 149 | # Enable/dissable use of tropospheric correction 150 | self.useTropCorr = True 151 | 152 | # 1 - On 153 | 154 | # True position of the antenna in UTM system (if known). Otherwise enter 155 | # all NaN's and mean position will be used as a reference . 156 | self.truePosition = TruePosition() 157 | # self.truePosition.E = np.nan 158 | 159 | # self.truePosition.N = np.nan 160 | 161 | # self.truePosition.U = np.nan 162 | 163 | # Plot settings ========================================================== 164 | # Enable/disable plotting of the tracking results for each channel 165 | self.plotTracking = True 166 | 167 | # 1 - On 168 | 169 | # Constants ============================================================== 170 | 171 | self._c = 299792458.0 172 | 173 | self._startOffset = 68.802 174 | 175 | @property 176 | def c(self): 177 | return self._c 178 | 179 | @property 180 | def startOffset(self): 181 | return self._startOffset 182 | 183 | @property 184 | def samplesPerCode(self): 185 | return np.long(np.round(self.samplingFreq / (self.codeFreqBasis / self.codeLength))) 186 | 187 | # makeCaTable.m 188 | def makeCaTable(self): 189 | # Function generates CA codes for all 32 satellites based on the settings 190 | # provided in the structure "settings". The codes are digitized at the 191 | # sampling frequency specified in the settings structure. 192 | # One row in the "caCodesTable" is one C/A code. The row number is the PRN 193 | # number of the C/A code. 194 | 195 | # caCodesTable = makeCaTable(settings) 196 | 197 | # Inputs: 198 | # settings - receiver settings 199 | # Outputs: 200 | # caCodesTable - an array of arrays (matrix) containing C/A codes 201 | # for all satellite PRN-s 202 | 203 | # --- Find number of samples per spreading code ---------------------------- 204 | samplesPerCode = self.samplesPerCode 205 | 206 | # --- Prepare the output matrix to speed up function ----------------------- 207 | caCodesTable = np.zeros((32, samplesPerCode)) 208 | 209 | # --- Find time constants -------------------------------------------------- 210 | ts = 1.0 / self.samplingFreq 211 | 212 | tc = 1.0 / self.codeFreqBasis 213 | 214 | # === For all satellite PRN-s ... 215 | for PRN in range(32): 216 | # --- Generate CA code for given PRN ----------------------------------- 217 | caCode = self.generateCAcode(PRN) 218 | 219 | # --- Make index array to read C/A code values ------------------------- 220 | # The length of the index array depends on the sampling frequency - 221 | # number of samples per millisecond (because one C/A code period is one 222 | # millisecond). 223 | codeValueIndex = np.ceil(ts * np.arange(1, samplesPerCode + 1) / tc) - 1 224 | codeValueIndex = np.longlong(codeValueIndex) 225 | 226 | codeValueIndex[-1] = 1022 227 | 228 | # The "upsampled" code is made by selecting values form the CA code 229 | # chip array (caCode) for the time instances of each sample. 230 | caCodesTable[PRN] = caCode[codeValueIndex] 231 | return caCodesTable 232 | 233 | # generateCAcode.m 234 | def generateCAcode(self, prn): 235 | # generateCAcode.m generates one of the 32 GPS satellite C/A codes. 236 | 237 | # CAcode = generateCAcode(PRN) 238 | 239 | # Inputs: 240 | # PRN - PRN number of the sequence. 241 | 242 | # Outputs: 243 | # CAcode - a vector containing the desired C/A code sequence 244 | # (chips). 245 | 246 | # --- Make the code shift array. The shift depends on the PRN number ------- 247 | # The g2s vector holds the appropriate shift of the g2 code to generate 248 | # the C/A code (ex. for SV#19 - use a G2 shift of g2s(19) = 471) 249 | 250 | assert prn in range(0, 32) 251 | g2s = [5, 6, 7, 8, 17, 18, 139, 140, 141, 251, 252 | 252, 254, 255, 256, 257, 258, 469, 470, 471, 472, 253 | 473, 474, 509, 512, 513, 514, 515, 516, 859, 860, 254 | 861, 862, 255 | 145, 175, 52, 21, 237, 235, 886, 657, 634, 762, 355, 1012, 176, 603, 130, 359, 595, 68, 386] 256 | 257 | # --- Pick right shift for the given PRN number ---------------------------- 258 | g2shift = g2s[prn] 259 | 260 | # --- Generate G1 code ----------------------------------------------------- 261 | 262 | # --- Initialize g1 output to speed up the function --- 263 | g1 = np.zeros(1023) 264 | 265 | # --- Load shift register --- 266 | reg = -1 * np.ones(10) 267 | 268 | # --- Generate all G1 signal chips based on the G1 feedback polynomial ----- 269 | for i in range(1023): 270 | g1[i] = reg[-1] 271 | 272 | saveBit = reg[2] * reg[9] 273 | 274 | reg[1:] = reg[:-1] 275 | 276 | reg[0] = saveBit 277 | 278 | # --- Generate G2 code ----------------------------------------------------- 279 | 280 | # --- Initialize g2 output to speed up the function --- 281 | g2 = np.zeros(1023) 282 | 283 | # --- Load shift register --- 284 | reg = -1 * np.ones(10) 285 | 286 | # --- Generate all G2 signal chips based on the G2 feedback polynomial ----- 287 | for i in range(1023): 288 | g2[i] = reg[-1] 289 | 290 | saveBit = reg[1] * reg[2] * reg[5] * reg[7] * reg[8] * reg[9] 291 | 292 | reg[1:] = reg[:-1] 293 | 294 | reg[0] = saveBit 295 | 296 | # --- Shift G2 code -------------------------------------------------------- 297 | # The idea: g2 = concatenate[ g2_right_part, g2_left_part ]; 298 | g2 = np.r_[g2[1023 - g2shift:], g2[:1023 - g2shift]] 299 | 300 | # --- Form single sample C/A code by multiplying G1 and G2 ----------------- 301 | CAcode = -g1 * g2 302 | return CAcode 303 | 304 | @staticmethod 305 | # calcLoopCoef.m 306 | def calcLoopCoef(LBW, zeta, k): 307 | # Function finds loop coefficients. The coefficients are used then in PLL-s 308 | # and DLL-s. 309 | 310 | # [tau1, tau2] = calcLoopCoef(LBW, zeta, k) 311 | 312 | # Inputs: 313 | # LBW - Loop noise bandwidth 314 | # zeta - Damping ratio 315 | # k - Loop gain 316 | 317 | # Outputs: 318 | # tau1, tau2 - Loop filter coefficients 319 | 320 | # Solve natural frequency 321 | Wn = LBW * 8.0 * zeta / (4.0 * zeta ** 2 + 1) 322 | 323 | # solve for t1 & t2 324 | tau1 = k / (Wn * Wn) 325 | 326 | tau2 = 2.0 * zeta / Wn 327 | 328 | return tau1, tau2 329 | 330 | def probeData(self, fileNameStr=None): 331 | 332 | import matplotlib.pyplot as plt 333 | from scipy.signal import welch 334 | from scipy.signal.windows.windows import hamming 335 | 336 | # Function plots raw data information: time domain plot, a frequency domain 337 | # plot and a histogram. 338 | 339 | # The function can be called in two ways: 340 | # probeData(settings) 341 | # or 342 | # probeData(fileName, settings) 343 | 344 | # Inputs: 345 | # fileName - name of the data file. File name is read from 346 | # settings if parameter fileName is not provided. 347 | 348 | # settings - receiver settings. Type of data file, sampling 349 | # frequency and the default filename are specified 350 | # here. 351 | 352 | # Check the number of arguments ========================================== 353 | if fileNameStr is None: 354 | fileNameStr = self.fileName 355 | if not isinstance(fileNameStr, str): 356 | raise TypeError('File name must be a string') 357 | settings = self 358 | # Generate plot of raw data ============================================== 359 | 360 | try: 361 | with open(fileNameStr, 'rb') as fid: 362 | # Move the starting point of processing. Can be used to start the 363 | # signal processing at any point in the data record (e.g. for long 364 | # records). 365 | fid.seek(settings.skipNumberOfBytes, 0) 366 | samplesPerCode = settings.samplesPerCode 367 | 368 | try: 369 | data = np.fromfile(fid, 370 | settings.dataType, 371 | 10 * samplesPerCode) 372 | 373 | except IOError: 374 | # The file is too short 375 | print 'Could not read enough data from the data file.' 376 | # --- Initialization --------------------------------------------------- 377 | plt.figure(100) 378 | plt.clf() 379 | timeScale = np.arange(0, 0.005, 1 / settings.samplingFreq) 380 | 381 | plt.subplot(2, 2, 1) 382 | plt.plot(1000 * timeScale[1:samplesPerCode / 50], 383 | data[1:samplesPerCode / 50]) 384 | plt.axis('tight') 385 | plt.grid() 386 | plt.title('Time domain plot') 387 | plt.xlabel('Time (ms)') 388 | plt.ylabel('Amplitude') 389 | plt.subplot(2, 2, 2) 390 | f, Pxx = welch(data - np.mean(data), 391 | settings.samplingFreq / 1000000.0, 392 | hamming(16384, False), 393 | 16384, 394 | 1024, 395 | 16384) 396 | plt.semilogy(f, Pxx) 397 | plt.axis('tight') 398 | plt.grid() 399 | plt.title('Frequency domain plot') 400 | plt.xlabel('Frequency (MHz)') 401 | plt.ylabel('Magnitude') 402 | plt.show() 403 | plt.subplot(2, 2, 3.5) 404 | plt.hist(data, np.arange(- 128, 128)) 405 | dmax = np.max(np.abs(data)) + 1 406 | 407 | plt.axis('tight') 408 | adata = plt.axis() 409 | 410 | plt.axis([-dmax, dmax, adata[2], adata[3]]) 411 | plt.grid('on') 412 | plt.title('Histogram') 413 | plt.xlabel('Bin') 414 | plt.ylabel('Number in bin') 415 | # === Error while opening the data file ================================ 416 | except IOError as e: 417 | print 'Unable to read file "%s": %s' % (fileNameStr, e) 418 | 419 | # ./postProcessing.m 420 | 421 | # Script postProcessing.m processes the raw signal from the specified data 422 | # file (in settings) operating on blocks of 37 seconds of data. 423 | 424 | # First it runs acquisition code identifying the satellites in the file, 425 | # then the code and carrier for each of the satellites are tracked, storing 426 | # the 1m sec accumulations. After processing all satellites in the 37 sec 427 | # data block, then postNavigation is called. It calculates pseudoranges 428 | # and attempts a position solutions. At the end plots are made for that 429 | # block of data. 430 | 431 | # THE SCRIPT "RECIPE" 432 | 433 | # The purpose of this script is to combine all parts of the software 434 | # receiver. 435 | 436 | # 1.1) Open the data file for the processing and seek to desired point. 437 | 438 | # 2.1) Acquire satellites 439 | 440 | # 3.1) Initialize channels (preRun.m). 441 | # 3.2) Pass the channel structure and the file identifier to the tracking 442 | # function. It will read and process the data. The tracking results are 443 | # stored in the trackResults structure. The results can be accessed this 444 | # way (the results are stored each millisecond): 445 | # trackResults(channelNumber).XXX(fromMillisecond : toMillisecond), where 446 | # XXX is a field name of the result (e.g. I_P, codePhase etc.) 447 | 448 | # 4) Pass tracking results to the navigation solution function. It will 449 | # decode navigation messages, find satellite positions, measure 450 | # pseudoranges and find receiver position. 451 | 452 | # 5) Plot the results. 453 | 454 | def postProcessing(self, fileNameStr=None): 455 | # Initialization ========================================================= 456 | import acquisition 457 | import postNavigation 458 | import tracking 459 | print 'Starting processing...' 460 | settings = self 461 | if not fileNameStr: 462 | fileNameStr = settings.fileName 463 | if not isinstance(fileNameStr, str): 464 | raise TypeError('File name must be a string') 465 | try: 466 | with open(fileNameStr, 'rb') as fid: 467 | 468 | # If success, then process the data 469 | # Move the starting point of processing. Can be used to start the 470 | # signal processing at any point in the data record (e.g. good for long 471 | # records or for signal processing in blocks). 472 | fid.seek(settings.skipNumberOfBytes, 0) 473 | # Acquisition ============================================================ 474 | # Do acquisition if it is not disabled in settings or if the variable 475 | # acqResults does not exist. 476 | if not settings.skipAcquisition: # or 'acqResults' not in globals(): 477 | # Find number of samples per spreading code 478 | samplesPerCode = settings.samplesPerCode 479 | 480 | # frequency estimation 481 | data = np.fromfile(fid, settings.dataType, 11 * samplesPerCode) 482 | 483 | print ' Acquiring satellites...' 484 | acqResults = acquisition.AcquisitionResult(settings) 485 | acqResults.acquire(data) 486 | acqResults.plot() 487 | # Initialize channels and prepare for the run ============================ 488 | # Start further processing only if a GNSS signal was acquired (the 489 | # field FREQUENCY will be set to 0 for all not acquired signals) 490 | if np.any(acqResults.carrFreq): 491 | acqResults.preRun() 492 | acqResults.showChannelStatus() 493 | else: 494 | # No satellites to track, exit 495 | print 'No GNSS signals detected, signal processing finished.' 496 | trackResults = None 497 | 498 | # Track the signal ======================================================= 499 | startTime = datetime.datetime.now() 500 | 501 | print ' Tracking started at %s' % startTime.strftime('%X') 502 | trackResults = tracking.TrackingResult(acqResults) 503 | try: 504 | trackResults.results = np.load('trackingResults_python.npy') 505 | except IOError: 506 | trackResults.track(fid) 507 | np.save('trackingResults_python', trackResults.results) 508 | 509 | print ' Tracking is over (elapsed time %s s)' % (datetime.datetime.now() - startTime).total_seconds() 510 | # Auto save the acquisition & tracking results to save time. 511 | print ' Saving Acquisition & Tracking results to storage' 512 | # Calculate navigation solutions ========================================= 513 | print ' Calculating navigation solutions...' 514 | navResults = postNavigation.NavigationResult(trackResults) 515 | navResults.postNavigate() 516 | 517 | print ' Processing is complete for this data block' 518 | # Plot all results =================================================== 519 | print ' Plotting results...' 520 | # TODO turn off tracking plots for now 521 | if not settings.plotTracking: 522 | trackResults.plot() 523 | navResults.plot() 524 | print 'Post processing of the signal is over.' 525 | except IOError as e: 526 | # Error while opening the data file. 527 | print 'Unable to read file "%s": %s.' % (settings.fileName, e) 528 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | import initialize 2 | 3 | # ./init.m 4 | 5 | # -------------------------------------------------------------------------- 6 | # SoftGNSS v3.0 7 | # 8 | # Copyright (C) Darius Plausinaitis and Dennis M. Akos 9 | # Written by Darius Plausinaitis and Dennis M. Akos 10 | # -------------------------------------------------------------------------- 11 | # This program is free software; you can redistribute it and/or 12 | # modify it under the terms of the GNU General Public License 13 | # as published by the Free Software Foundation; either version 2 14 | # of the License, or (at your option) any later version. 15 | 16 | # This program is distributed in the hope that it will be useful, 17 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | # GNU General Public License for more details. 20 | 21 | # You should have received a copy of the GNU General Public License 22 | # along with this program; if not, write to the Free Software 23 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, 24 | # USA. 25 | # -------------------------------------------------------------------------- 26 | 27 | # Script initializes settings and environment of the software receiver. 28 | # Then the processing is started. 29 | 30 | # -------------------------------------------------------------------------- 31 | 32 | 33 | # Clean up the environment first ========================================= 34 | # clear 35 | # close_('all') 36 | # clc 37 | # format('compact') 38 | # format('long','g') 39 | # --- Include folders with functions --------------------------------------- 40 | # addpath('include') 41 | # addpath('geoFunctions') 42 | # Print startup ========================================================== 43 | print '\n', \ 44 | 'Welcome to: softGNSS\n\n', \ 45 | 'An open source GNSS SDR software project initiated by:\n\n', \ 46 | ' Danish GPS Center/Aalborg University\n\n', \ 47 | 'The code was improved by GNSS Laboratory/University of Colorado.\n\n', \ 48 | 'The software receiver softGNSS comes with ABSOLUTELY NO WARRANTY;\n', \ 49 | 'for details please read license details in the file license.txt. This\n', \ 50 | 'is free software, and you are welcome to redistribute it under\n', \ 51 | 'the terms described in the license.\n\n', \ 52 | ' -------------------------------\n\n' 53 | # Initialize settings class========================================= 54 | settings = initialize.Settings() 55 | 56 | # Generate plot of raw data and ask if ready to start processing ========= 57 | try: 58 | print 'Probing data "%s"...' % settings.fileName 59 | settings.probeData() 60 | settings.probeData('/Users/yangsu/Downloads/GNSS_signal_records/GPS_and_GIOVE_A-NN-fs16_3676-if4_1304.bin') 61 | finally: 62 | pass 63 | 64 | print ' Raw IF data plotted ' 65 | print ' (run setSettings or change settings in "initialize.py" to reconfigure)' 66 | print ' ' 67 | gnssStart = True 68 | # gnssStart = int(raw_input('Enter "1" to initiate GNSS processing or "0" to exit : ').strip()) 69 | 70 | if gnssStart: 71 | print ' ' 72 | settings.postProcessing() 73 | -------------------------------------------------------------------------------- /postNavigation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | import ephemeris 4 | from geoFunctions import satpos, leastSquarePos, cart2geo, findUtmZone, cart2utm 5 | from initialize import Result 6 | 7 | 8 | class NavigationResult(Result): 9 | def __init__(self, trackResult): 10 | self._results = trackResult.results 11 | self._channels = trackResult.channels 12 | self._settings = trackResult.settings 13 | self._solutions = None 14 | self._eph = None 15 | 16 | @property 17 | def solutions(self): 18 | assert isinstance(self._solutions, np.recarray) 19 | return self._solutions 20 | 21 | @property 22 | def ephemeris(self): 23 | assert isinstance(self._solutions, np.recarray) 24 | return self._eph 25 | 26 | # ./calculatePseudoranges.m 27 | def calculatePseudoranges(self, msOfTheSignal, channelList): 28 | trackResults = self._results 29 | settings = self._settings 30 | # calculatePseudoranges finds relative pseudoranges for all satellites 31 | # listed in CHANNELLIST at the specified millisecond of the processed 32 | # signal. The pseudoranges contain unknown receiver clock offset. It can be 33 | # found by the least squares position search procedure. 34 | 35 | # [pseudoranges] = calculatePseudoranges(trackResults, msOfTheSignal, ... 36 | # channelList, settings) 37 | 38 | # Inputs: 39 | # trackResults - output from the tracking function 40 | # msOfTheSignal - pseudorange measurement point (millisecond) in 41 | # the trackResults structure 42 | # channelList - list of channels to be processed 43 | # settings - receiver settings 44 | 45 | # Outputs: 46 | # pseudoranges - relative pseudoranges to the satellites. 47 | 48 | # --- Set initial travel time to infinity ---------------------------------- 49 | # Later in the code a shortest pseudorange will be selected. Therefore 50 | # pseudoranges from non-tracking channels must be the longest - e.g. 51 | # infinite. 52 | travelTime = np.Inf * np.ones(settings.numberOfChannels) 53 | 54 | # Find number of samples per spreading code 55 | samplesPerCode = settings.samplesPerCode 56 | 57 | # --- For all channels in the list ... 58 | for channelNr in channelList: 59 | # --- Compute the travel times ----------------------------------------- 60 | travelTime[channelNr] = trackResults[channelNr].absoluteSample[ 61 | np.int(msOfTheSignal[channelNr])] / samplesPerCode 62 | 63 | # --- Truncate the travelTime and compute pseudoranges --------------------- 64 | minimum = np.floor(travelTime.min()) 65 | 66 | travelTime = travelTime - minimum + settings.startOffset 67 | 68 | # --- Convert travel time to a distance ------------------------------------ 69 | # The speed of light must be converted from meters per second to meters 70 | # per millisecond. 71 | pseudoranges = travelTime * settings.c / 1000 72 | return pseudoranges 73 | 74 | # ./postNavigation.m 75 | def postNavigate(self): 76 | trackResults = self._results 77 | settings = self._settings 78 | # Function calculates navigation solutions for the receiver (pseudoranges, 79 | # positions). At the end it converts coordinates from the WGS84 system to 80 | # the UTM, geocentric or any additional coordinate system. 81 | 82 | # [navSolutions, eph] = postNavigation(trackResults, settings) 83 | 84 | # Inputs: 85 | # trackResults - results from the tracking function (structure 86 | # array). 87 | # settings - receiver settings. 88 | # Outputs: 89 | # navSolutions - contains measured pseudoranges, receiver 90 | # clock error, receiver coordinates in several 91 | # coordinate systems (at least ECEF and UTM). 92 | # eph - received ephemerides of all SV (structure array). 93 | 94 | # Check is there enough data to obtain any navigation solution =========== 95 | # It is necessary to have at least three subframes (number 1, 2 and 3) to 96 | # find satellite coordinates. Then receiver position can be found too. 97 | # The function requires all 5 subframes, because the tracking starts at 98 | # arbitrary point. Therefore the first received subframes can be any three 99 | # from the 5. 100 | # One subframe length is 6 seconds, therefore we need at least 30 sec long 101 | # record (5 * 6 = 30 sec = 30000ms). We add extra seconds for the cases, 102 | # when tracking has started in a middle of a subframe. 103 | 104 | if settings.msToProcess < 36000 or sum(trackResults.status != '-') < 4: 105 | # Show the error message and exit 106 | print 'Record is to short or too few satellites tracked. Exiting!' 107 | navSolutions = None 108 | self._solutions = navSolutions 109 | eph = None 110 | self._eph = eph 111 | return 112 | 113 | # Find preamble start positions ========================================== 114 | 115 | subFrameStart, activeChnList = self.findPreambles() 116 | 117 | # Decode ephemerides ===================================================== 118 | field_str = 'weekNumber,accuracy,health,T_GD,IODC,t_oc,a_f2,a_f1,a_f0,' 119 | field_str += 'IODE_sf2,C_rs,deltan,M_0,C_uc,e,C_us,sqrtA,t_oe,' 120 | field_str += 'C_ic,omega_0,C_is,i_0,C_rc,omega,omegaDot,IODE_sf3,iDot' 121 | eph = np.recarray((32,), formats=['O'] * 27, names=field_str) 122 | for channelNr in activeChnList: 123 | # === Convert tracking output to navigation bits ======================= 124 | # --- Copy 5 sub-frames long record from tracking output --------------- 125 | navBitsSamples = trackResults[channelNr].I_P[subFrameStart[channelNr] - 20: 126 | subFrameStart[channelNr] + 1500 * 20].copy() 127 | 128 | navBitsSamples = navBitsSamples.reshape(20, -1, order='F') 129 | 130 | navBits = navBitsSamples.sum(0) 131 | 132 | # The expression (navBits > 0) returns an array with elements set to 1 133 | # if the condition is met and set to 0 if it is not met. 134 | navBits = (navBits > 0) * 1 135 | 136 | # The function ephemeris expects input in binary form. In Matlab it is 137 | # a string array containing only "0" and "1" characters. 138 | navBitsBin = map(str, navBits) 139 | 140 | eph[trackResults[channelNr].PRN - 1], TOW = ephemeris.ephemeris(navBitsBin[1:], navBitsBin[0]) 141 | 142 | if eph[trackResults[channelNr].PRN - 1].IODC is None or \ 143 | eph[trackResults[channelNr].PRN - 1].IODE_sf2 is None or \ 144 | eph[trackResults[channelNr].PRN - 1].IODE_sf3 is None: 145 | # --- Exclude channel from the list (from further processing) ------ 146 | activeChnList = np.setdiff1d(activeChnList, channelNr) 147 | 148 | # Check if the number of satellites is still above 3 ===================== 149 | if activeChnList.size == 0 or activeChnList.size < 4: 150 | # Show error message and exit 151 | print 'Too few satellites with ephemeris data for position calculations. Exiting!' 152 | navSolutions = None 153 | self._solutions = navSolutions 154 | eph = None 155 | self._eph = eph 156 | return 157 | 158 | # Initialization ========================================================= 159 | 160 | # Set the satellite elevations array to INF to include all satellites for 161 | # the first calculation of receiver position. There is no reference point 162 | # to find the elevation angle as there is no receiver position estimate at 163 | # this point. 164 | satElev = np.Inf * np.ones(settings.numberOfChannels) 165 | 166 | # Save the active channel list. The list contains satellites that are 167 | # tracked and have the required ephemeris data. In the next step the list 168 | # will depend on each satellite's elevation angle, which will change over 169 | # time. 170 | readyChnList = activeChnList.copy() 171 | 172 | transmitTime = TOW 173 | 174 | ########################################################################### 175 | # Do the satellite and receiver position calculations # 176 | ########################################################################### 177 | # Initialization of current measurement ================================== 178 | channel = np.rec.array([(np.zeros((settings.numberOfChannels, 64)), 179 | np.nan * np.ones((settings.numberOfChannels, 64)), 180 | np.nan * np.ones((settings.numberOfChannels, 64)), 181 | np.nan * np.ones((settings.numberOfChannels, 64)), 182 | np.nan * np.ones((settings.numberOfChannels, 64)) 183 | )], formats=['O'] * 5, names='PRN,el,az,rawP,correctedP') 184 | navSolutions = np.rec.array([(channel, 185 | np.zeros((5, 64)), 186 | np.nan * np.ones(64), 187 | np.nan * np.ones(64), 188 | np.nan * np.ones(64), 189 | np.nan * np.ones(64), 190 | np.nan * np.ones(64), 191 | np.nan * np.ones(64), 192 | np.nan * np.ones(64), 193 | 0, 194 | np.nan * np.ones(64), 195 | np.nan * np.ones(64), 196 | np.nan * np.ones(64) 197 | )], formats=['O'] * 13, 198 | names='channel,DOP,X,Y,Z,dt,latitude,longitude,height,utmZone,E,N,U') 199 | for currMeasNr in range(np.int(np.fix(settings.msToProcess - subFrameStart.max()) / settings.navSolPeriod)): 200 | # Exclude satellites, that are below elevation mask 201 | activeChnList = np.intersect1d((satElev >= settings.elevationMask).nonzero()[0], readyChnList) 202 | 203 | channel[0].PRN[activeChnList, currMeasNr] = trackResults[activeChnList].PRN 204 | 205 | # do to elevation mask will not "jump" to position (0,0) in the sky 206 | # plot. 207 | # channel[0].el[:, currMeasNr] = np.nan * np.ones(settings.numberOfChannels) 208 | 209 | # channel[0].az[:, currMeasNr] = np.nan * np.ones(settings.numberOfChannels) 210 | 211 | # Find pseudoranges ====================================================== 212 | channel[0].rawP[:, currMeasNr] = self.calculatePseudoranges( 213 | subFrameStart + settings.navSolPeriod * currMeasNr, 214 | activeChnList) 215 | 216 | # Find satellites positions and clocks corrections ======================= 217 | satPositions, satClkCorr = satpos(transmitTime, trackResults[activeChnList].PRN, eph, settings) 218 | 219 | # Find receiver position ================================================= 220 | # 3D receiver position can be found only if signals from more than 3 221 | # satellites are available 222 | if activeChnList.size > 3: 223 | # === Calculate receiver position ================================== 224 | (xyzdt, 225 | channel[0].el[activeChnList, currMeasNr], 226 | channel[0].az[activeChnList, currMeasNr], 227 | navSolutions[0].DOP[:, currMeasNr]) = leastSquarePos(satPositions, 228 | channel[0].rawP[ 229 | activeChnList, currMeasNr] + 230 | satClkCorr * settings.c, 231 | settings) 232 | 233 | navSolutions[0].X[currMeasNr] = xyzdt[0] 234 | 235 | navSolutions[0].Y[currMeasNr] = xyzdt[1] 236 | 237 | navSolutions[0].Z[currMeasNr] = xyzdt[2] 238 | 239 | navSolutions[0].dt[currMeasNr] = xyzdt[3] 240 | 241 | satElev = channel[0].el[:, currMeasNr] 242 | 243 | channel[0].correctedP[activeChnList, currMeasNr] = channel[0].rawP[activeChnList, currMeasNr] + \ 244 | satClkCorr * settings.c + \ 245 | navSolutions[0].dt[currMeasNr] 246 | 247 | # Coordinate conversion ================================================== 248 | # === Convert to geodetic coordinates ============================== 249 | (navSolutions[0].latitude[currMeasNr], 250 | navSolutions[0].longitude[currMeasNr], 251 | navSolutions[0].height[currMeasNr]) = cart2geo(navSolutions[0].X[currMeasNr], 252 | navSolutions[0].Y[currMeasNr], 253 | navSolutions[0].Z[currMeasNr], 254 | 4) 255 | 256 | navSolutions[0].utmZone = findUtmZone(navSolutions[0].latitude[currMeasNr], 257 | navSolutions[0].longitude[currMeasNr]) 258 | 259 | (navSolutions[0].E[currMeasNr], 260 | navSolutions[0].N[currMeasNr], 261 | navSolutions[0].U[currMeasNr]) = cart2utm(xyzdt[0], xyzdt[1], xyzdt[2], 262 | navSolutions[0].utmZone) 263 | 264 | else: 265 | # --- There are not enough satellites to find 3D position ---------- 266 | print ' Measurement No. %d' % currMeasNr + ': Not enough information for position solution.' 267 | # excluded automatically in all plots. For DOP it is easier to use 268 | # zeros. NaN values might need to be excluded from results in some 269 | # of further processing to obtain correct results. 270 | navSolutions[0].X[currMeasNr] = np.nan 271 | 272 | navSolutions[0].Y[currMeasNr] = np.nan 273 | 274 | navSolutions[0].Z[currMeasNr] = np.nan 275 | 276 | navSolutions[0].dt[currMeasNr] = np.nan 277 | 278 | navSolutions[0].DOP[:, currMeasNr] = np.zeros(5) 279 | 280 | navSolutions[0].latitude[currMeasNr] = np.nan 281 | 282 | navSolutions[0].longitude[currMeasNr] = np.nan 283 | 284 | navSolutions[0].height[currMeasNr] = np.nan 285 | 286 | navSolutions[0].E[currMeasNr] = np.nan 287 | 288 | navSolutions[0].N[currMeasNr] = np.nan 289 | 290 | navSolutions[0].U[currMeasNr] = np.nan 291 | 292 | channel[0].az[activeChnList, currMeasNr] = np.nan * np.ones(activeChnList.shape) 293 | 294 | channel[0].el[activeChnList, currMeasNr] = np.nan * np.ones(activeChnList.shape) 295 | 296 | # satellites are excluded do to elevation mask. Therefore raising 297 | # satellites will be not included even if they will be above 298 | # elevation mask at some point. This would be a good place to 299 | # update positions of the excluded satellites. 300 | # === Update the transmit time ("measurement time") ==================== 301 | transmitTime += settings.navSolPeriod / 1000 302 | 303 | self._solutions = navSolutions 304 | self._eph = eph 305 | return 306 | 307 | def plot(self): 308 | settings = self._settings 309 | navSolutions = self._solutions 310 | assert isinstance(navSolutions, np.recarray) 311 | 312 | import matplotlib as mpl 313 | import matplotlib.gridspec as gs 314 | import matplotlib.pyplot as plt 315 | from mpl_toolkits.mplot3d import axes3d 316 | 317 | import initialize 318 | 319 | # %% configure matplotlib 320 | mpl.rcdefaults() 321 | # mpl.rcParams['font.sans-serif'] 322 | # mpl.rcParams['font.family'] = 'serif' 323 | mpl.rc('savefig', bbox='tight', transparent=False, format='png') 324 | mpl.rc('axes', grid=True, linewidth=1.5, axisbelow=True) 325 | mpl.rc('lines', linewidth=1.5, solid_joinstyle='bevel') 326 | mpl.rc('figure', figsize=[8, 6], autolayout=False, dpi=120) 327 | mpl.rc('text', usetex=True) 328 | mpl.rc('font', family='serif', serif='Computer Modern Roman', size=10) 329 | mpl.rc('mathtext', fontset='cm') 330 | 331 | # mpl.rc('font', size=16) 332 | # mpl.rc('text.latex', preamble=r'\usepackage{cmbright}') 333 | 334 | # ./plotNavigation.m 335 | 336 | # Functions plots variations of coordinates over time and a 3D position 337 | # plot. It plots receiver coordinates in UTM system or coordinate offsets if 338 | # the true UTM receiver coordinates are provided. 339 | 340 | # plotNavigation(navSolutions, settings) 341 | 342 | # Inputs: 343 | # navSolutions - Results from navigation solution function. It 344 | # contains measured pseudoranges and receiver 345 | # coordinates. 346 | # settings - Receiver settings. The true receiver coordinates 347 | # are contained in this structure. 348 | 349 | # Plot results in the necessary data exists ============================== 350 | if navSolutions is not None: 351 | refCoord = initialize.TruePosition() 352 | # If reference position is not provided, then set reference position 353 | # to the average postion 354 | if settings.truePosition.E is None or settings.truePosition.N is None or settings.truePosition.U is None: 355 | # === Compute mean values ========================================== 356 | # Remove NaN-s or the output of the function MEAN will be NaN. 357 | refCoord.E = np.nanmean(navSolutions[0].E) 358 | 359 | refCoord.N = np.nanmean(navSolutions[0].N) 360 | 361 | refCoord.U = np.nanmean(navSolutions[0].U) 362 | 363 | meanLongitude = np.nanmean(navSolutions[0].longitude) 364 | 365 | meanLatitude = np.nanmean(navSolutions[0].latitude) 366 | 367 | refPointLgText = 'Mean Position' + '\\newline Lat: %.5f $^\circ$' % meanLatitude + \ 368 | '\\newline Lng: %.5f $^\circ$' % meanLongitude + \ 369 | '\\newline Hgt: %+6.1f' % np.nanmean(navSolutions[0].height) 370 | 371 | else: 372 | refPointLgText = 'Reference Position' 373 | 374 | refCoord.E = settings.truePosition.E 375 | 376 | refCoord.N = settings.truePosition.N 377 | 378 | refCoord.U = settings.truePosition.U 379 | 380 | figureNumber = 300 381 | 382 | # figure windows, when many figures are closed and reopened. Figures 383 | # drawn or opened by the user, will not be "overwritten" by this 384 | # function if the auto numbering is not used. 385 | # === Select (or create) and clear the figure ========================== 386 | f = plt.figure(figureNumber) 387 | f.clf() 388 | f.set_label('Navigation solutions') 389 | spec = gs.GridSpec(2, 2) 390 | h11 = plt.subplot(spec[0:2]) 391 | 392 | # the axes3d module is needed for the following line 393 | dummy = axes3d.Axes3D 394 | h31 = plt.subplot(spec[2], projection='3d') 395 | 396 | h32 = plt.subplot(spec[3], projection='polar') 397 | 398 | # Plot all figures ======================================================= 399 | # --- Coordinate differences in UTM system ----------------------------- 400 | h11.plot(navSolutions[0].E - refCoord.E, '-', 401 | navSolutions[0].N - refCoord.N, '-', 402 | navSolutions[0].U - refCoord.U, '-') 403 | h11.legend(['E', 'N', 'U']) 404 | h11.set(title='Coordinates variations in UTM system', 405 | xlabel='Measurement period: %i ms' % settings.navSolPeriod, 406 | ylabel='Variations (m)') 407 | h11.grid() 408 | h11.axis('tight') 409 | h31.plot((navSolutions[0].E - refCoord.E).T, 410 | (navSolutions[0].N - refCoord.N).T, 411 | (navSolutions[0].U - refCoord.U).T, '+') 412 | h31.hold(True) 413 | h31.plot([0], [0], [0], 'r+', lw=1.5, ms=10) 414 | h31.hold(False) 415 | # h31.viewLim(0,90) 416 | h31.axis('equal') 417 | h31.grid(which='minor') 418 | h31.legend(['Measurements', refPointLgText]) 419 | h31.set(title='Positions in UTM system (3D plot)', 420 | xlabel='East (m)', 421 | ylabel='North (m)', 422 | zlabel='Upping (m)') 423 | h32.plot(np.deg2rad(navSolutions[0].channel[0].az.T), 424 | 90 - navSolutions[0].channel[0].el.T) 425 | [h32.text(x, y, s) for x, y, s in zip(np.deg2rad(navSolutions[0].channel[0].az[:, 0]), 426 | 90 - navSolutions[0].channel[0].el[:, 0], 427 | navSolutions[0].channel[0].PRN[:, 0])] 428 | h32.set_theta_direction(-1) 429 | h32.set_theta_zero_location('N') 430 | h32.set_xlim([0, 2 * np.pi]) 431 | h32.set_xticks(np.linspace(0, 2 * np.pi, 12, endpoint=False)) 432 | h32.set_rlabel_position(0) 433 | h32.set_ylim([0, 90]) 434 | h32.set_yticks([0, 15, 30, 45, 60, 75]) 435 | h32.set_yticklabels([90, 75, 60, 45, 30, 15]) 436 | h32.set_title('Sky plot (mean PDOP: %f )' % np.mean(navSolutions[0].DOP[1, :])) 437 | f.show() 438 | else: 439 | print 'plotNavigation: No navigation data to plot.' 440 | 441 | @staticmethod 442 | # navPartyChk.m 443 | def navPartyChk(ndat): 444 | # This function is called to compute and status the parity bits on GPS word. 445 | # Based on the flowchart in Figure 2-10 in the 2nd Edition of the GPS-SPS 446 | # Signal Spec. 447 | 448 | # status = navPartyChk(ndat) 449 | 450 | # Inputs: 451 | # ndat - an array (1x32) of 32 bits represent a GPS navigation 452 | # word which is 30 bits plus two previous bits used in 453 | # the parity calculation (-2 -1 0 1 2 ... 28 29) 454 | 455 | # Outputs: 456 | # status - the test value which equals EITHER +1 or -1 if parity 457 | # PASSED or 0 if parity fails. The +1 means bits #1-24 458 | # of the current word have the correct polarity, while -1 459 | # means the bits #1-24 of the current word must be 460 | # inverted. 461 | 462 | # In order to accomplish the exclusive or operation using multiplication 463 | # this program represents a '0' with a '-1' and a '1' with a '1' so that 464 | # the exclusive or table holds true for common data operations 465 | 466 | # a b xor a b product 467 | # -------------- ----------------- 468 | # 0 0 1 -1 -1 1 469 | # 0 1 0 -1 1 -1 470 | # 1 0 0 1 -1 -1 471 | # 1 1 1 1 1 1 472 | 473 | # --- Check if the data bits must be inverted ------------------------------ 474 | if ndat[1] != 1: 475 | ndat[2:26] *= (-1) 476 | 477 | # --- Calculate 6 parity bits ---------------------------------------------- 478 | # The elements of the ndat array correspond to the bits showed in the table 479 | # 20-XIV (ICD-200C document) in the following way: 480 | # The first element in the ndat is the D29* bit and the second - D30*. 481 | # The elements 3 - 26 are bits d1-d24 in the table. 482 | # The elements 27 - 32 in the ndat array are the received bits D25-D30. 483 | # The array "parity" contains the computed D25-D30 (parity) bits. 484 | parity = np.zeros(6) 485 | parity[0] = ndat[0] * ndat[2] * ndat[3] * ndat[4] * ndat[6] * \ 486 | ndat[7] * ndat[11] * ndat[12] * ndat[13] * ndat[14] * \ 487 | ndat[15] * ndat[18] * ndat[19] * ndat[21] * ndat[24] 488 | 489 | parity[1] = ndat[1] * ndat[3] * ndat[4] * ndat[5] * ndat[7] * \ 490 | ndat[8] * ndat[12] * ndat[13] * ndat[14] * ndat[15] * \ 491 | ndat[16] * ndat[19] * ndat[20] * ndat[22] * ndat[25] 492 | 493 | parity[2] = ndat[0] * ndat[2] * ndat[4] * ndat[5] * ndat[6] * \ 494 | ndat[8] * ndat[9] * ndat[13] * ndat[14] * ndat[15] * \ 495 | ndat[16] * ndat[17] * ndat[20] * ndat[21] * ndat[23] 496 | 497 | parity[3] = ndat[1] * ndat[3] * ndat[5] * ndat[6] * ndat[7] * \ 498 | ndat[9] * ndat[10] * ndat[14] * ndat[15] * ndat[16] * \ 499 | ndat[17] * ndat[18] * ndat[21] * ndat[22] * ndat[24] 500 | 501 | parity[4] = ndat[1] * ndat[2] * ndat[4] * ndat[6] * ndat[7] * \ 502 | ndat[8] * ndat[10] * ndat[11] * ndat[15] * ndat[16] * \ 503 | ndat[17] * ndat[18] * ndat[19] * ndat[22] * ndat[23] * \ 504 | ndat[25] 505 | 506 | parity[5] = ndat[0] * ndat[4] * ndat[6] * ndat[7] * ndat[9] * \ 507 | ndat[10] * ndat[11] * ndat[12] * ndat[14] * ndat[16] * \ 508 | ndat[20] * ndat[23] * ndat[24] * ndat[25] 509 | 510 | # --- Compare if the received parity is equal the calculated parity -------- 511 | if (parity == ndat[26:]).sum() == 6: 512 | # Parity is OK. Function output is -1 or 1 depending if the data bits 513 | # must be inverted or not. The "ndat[2]" is D30* bit - the last bit of 514 | # previous subframe. 515 | status = -1 * ndat[1] 516 | 517 | else: 518 | # Parity failure 519 | status = 0 520 | 521 | return status 522 | 523 | # ./findPreambles.m 524 | def findPreambles(self): 525 | assert isinstance(self._results, np.recarray) 526 | trackResults = self._results 527 | settings = self._settings 528 | # findPreambles finds the first preamble occurrence in the bit stream of 529 | # each channel. The preamble is verified by check of the spacing between 530 | # preambles (6sec) and parity checking of the first two words in a 531 | # subframe. At the same time function returns list of channels, that are in 532 | # tracking state and with valid preambles in the nav data stream. 533 | 534 | # [firstSubFrame, activeChnList] = findPreambles(trackResults, settings) 535 | 536 | # Inputs: 537 | # trackResults - output from the tracking function 538 | # settings - Receiver settings. 539 | 540 | # Outputs: 541 | # firstSubframe - the array contains positions of the first 542 | # preamble in each channel. The position is ms count 543 | # since start of tracking. Corresponding value will 544 | # be set to 0 if no valid preambles were detected in 545 | # the channel. 546 | # activeChnList - list of channels containing valid preambles 547 | 548 | # Preamble search can be delayed to a later point in the tracking results 549 | # to avoid noise due to tracking loop transients 550 | searchStartOffset = 0 551 | 552 | # --- Initialize the firstSubFrame array ----------------------------------- 553 | firstSubFrame = np.zeros(settings.numberOfChannels, dtype=int) 554 | 555 | # --- Generate the preamble pattern ---------------------------------------- 556 | preamble_bits = np.r_[1, - 1, - 1, - 1, 1, - 1, 1, 1] 557 | 558 | # "Upsample" the preamble - make 20 vales per one bit. The preamble must be 559 | # found with precision of a sample. 560 | preamble_ms = np.kron(preamble_bits, np.ones(20)) 561 | 562 | # --- Make a list of channels excluding not tracking channels -------------- 563 | activeChnList = (trackResults.status != '-').nonzero()[0] 564 | 565 | # === For all tracking channels ... 566 | for channelNr in range(len(activeChnList)): 567 | # Correlate tracking output with preamble ================================ 568 | # Read output from tracking. It contains the navigation bits. The start 569 | # of record is skipped here to avoid tracking loop transients. 570 | bits = trackResults[channelNr].I_P[searchStartOffset:].copy() 571 | 572 | bits[bits > 0] = 1 573 | 574 | bits[bits <= 0] = - 1 575 | 576 | # have to zero pad the preamble so that they are the same length 577 | tlmXcorrResult = np.correlate(bits, 578 | np.pad(preamble_ms, (0, bits.size - preamble_ms.size), 'constant'), 579 | mode='full') 580 | 581 | # Find all starting points off all preamble like patterns ================ 582 | # clear('index') 583 | # clear('index2') 584 | xcorrLength = (len(tlmXcorrResult) + 1) / 2 585 | 586 | index = (np.abs(tlmXcorrResult[xcorrLength - 1:xcorrLength * 2]) > 153).nonzero()[0] + searchStartOffset 587 | 588 | # Analyze detected preamble like patterns ================================ 589 | for i in range(len(index)): 590 | # --- Find distances in time between this occurrence and the rest of 591 | # preambles like patterns. If the distance is 6000 milliseconds (one 592 | # subframe), the do further verifications by validating the parities 593 | # of two GPS words 594 | index2 = index - index[i] 595 | 596 | if (index2 == 6000).any(): 597 | # === Re-read bit vales for preamble verification ============== 598 | # Preamble occurrence is verified by checking the parity of 599 | # the first two words in the subframe. Now it is assumed that 600 | # bit boundaries a known. Therefore the bit values over 20ms are 601 | # combined to increase receiver performance for noisy signals. 602 | # in Total 62 bits mast be read : 603 | # 2 bits from previous subframe are needed for parity checking; 604 | # 60 bits for the first two 30bit words (TLM and HOW words). 605 | # The index is pointing at the start of TLM word. 606 | bits = trackResults[channelNr].I_P[index[i] - 40:index[i] + 20 * 60].copy() 607 | 608 | bits = bits.reshape(20, -1, order='F') 609 | 610 | bits = bits.sum(0) 611 | 612 | bits[bits > 0] = 1 613 | 614 | bits[bits <= 0] = - 1 615 | 616 | if self.navPartyChk(bits[:32]) != 0 and self.navPartyChk(bits[30:62]) != 0: 617 | # Parity was OK. Record the preamble start position. Skip 618 | # the rest of preamble pattern checking for this channel 619 | # and process next channel. 620 | firstSubFrame[channelNr] = index[i] 621 | 622 | break 623 | # Exclude channel from the active channel list if no valid preamble was 624 | # detected 625 | if firstSubFrame[channelNr] == 0: 626 | # Exclude channel from further processing. It does not contain any 627 | # valid preamble and therefore nothing more can be done for it. 628 | activeChnList = np.setdiff1d(activeChnList, channelNr) 629 | 630 | print 'Could not find valid preambles in channel %2d !' % channelNr 631 | return firstSubFrame, activeChnList 632 | 633 | 634 | if __name__ == '__main__': 635 | pass 636 | -------------------------------------------------------------------------------- /tracking.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from initialize import Result 4 | 5 | 6 | class TrackingResult(Result): 7 | def __init__(self, acqResult): 8 | self._results = None 9 | self._channels = acqResult.channels 10 | self._settings = acqResult.settings 11 | 12 | # ./tracking.m 13 | def track(self, fid): 14 | channel = self._channels 15 | settings = self._settings 16 | # Performs code and carrier tracking for all channels. 17 | 18 | # [trackResults, channel] = tracking(fid, channel, settings) 19 | 20 | # Inputs: 21 | # fid - file identifier of the signal record. 22 | # channel - PRN, carrier frequencies and code phases of all 23 | # satellites to be tracked (prepared by preRum.m from 24 | # acquisition results). 25 | # settings - receiver settings. 26 | # Outputs: 27 | # trackResults - tracking results (structure array). Contains 28 | # in-phase prompt outputs and absolute starting 29 | # positions of spreading codes, together with other 30 | # observation data from the tracking loops. All are 31 | # saved every millisecond. 32 | 33 | # Initialize tracking variables ========================================== 34 | 35 | codePeriods = settings.msToProcess 36 | 37 | # --- DLL variables -------------------------------------------------------- 38 | # Define early-late offset (in chips) 39 | earlyLateSpc = settings.dllCorrelatorSpacing 40 | 41 | # Summation interval 42 | PDIcode = 0.001 43 | 44 | # Calculate filter coefficient values 45 | tau1code, tau2code = settings.calcLoopCoef(settings.dllNoiseBandwidth, settings.dllDampingRatio, 1.0) 46 | 47 | # --- PLL variables -------------------------------------------------------- 48 | # Summation interval 49 | PDIcarr = 0.001 50 | 51 | # Calculate filter coefficient values 52 | tau1carr, tau2carr = settings.calcLoopCoef(settings.pllNoiseBandwidth, settings.pllDampingRatio, 0.25) 53 | 54 | # hwb=waitbar(0,'Tracking...') 55 | 56 | # Initialize a temporary list of records 57 | rec = [] 58 | # Start processing channels ============================================== 59 | for channelNr in range(settings.numberOfChannels): 60 | msToProcess = np.long(settings.msToProcess) 61 | # Initialize fields for record(structured) array of tracked results 62 | status = '-' 63 | 64 | # The absolute sample in the record of the C/A code start: 65 | absoluteSample = np.zeros(msToProcess) 66 | 67 | # Freq of the C/A code: 68 | codeFreq_ = np.Inf * np.ones(msToProcess) 69 | 70 | # Frequency of the tracked carrier wave: 71 | carrFreq_ = np.Inf * np.ones(msToProcess) 72 | 73 | # Outputs from the correlators (In-phase): 74 | I_P_ = np.zeros(msToProcess) 75 | 76 | I_E_ = np.zeros(msToProcess) 77 | 78 | I_L_ = np.zeros(msToProcess) 79 | 80 | # Outputs from the correlators (Quadrature-phase): 81 | Q_E_ = np.zeros(msToProcess) 82 | 83 | Q_P_ = np.zeros(msToProcess) 84 | 85 | Q_L_ = np.zeros(msToProcess) 86 | 87 | # Loop discriminators 88 | dllDiscr = np.Inf * np.ones(msToProcess) 89 | 90 | dllDiscrFilt = np.Inf * np.ones(msToProcess) 91 | 92 | pllDiscr = np.Inf * np.ones(msToProcess) 93 | 94 | pllDiscrFilt = np.Inf * np.ones(msToProcess) 95 | 96 | PRN = 0 97 | 98 | # Only process if PRN is non zero (acquisition was successful) 99 | if channel[channelNr].PRN != 0: 100 | # Save additional information - each channel's tracked PRN 101 | PRN = channel[channelNr].PRN 102 | 103 | # signal processing at any point in the data record (e.g. for long 104 | # records). In addition skip through that data file to start at the 105 | # appropriate sample (corresponding to code phase). Assumes sample 106 | # type is schar (or 1 byte per sample) 107 | fid.seek(settings.skipNumberOfBytes + channel[channelNr].codePhase, 0) 108 | # Here PRN is the actual satellite ID instead of the 0-based index 109 | caCode = settings.generateCAcode(channel[channelNr].PRN - 1) 110 | 111 | caCode = np.r_[caCode[-1], caCode, caCode[0]] 112 | 113 | # define initial code frequency basis of NCO 114 | codeFreq = settings.codeFreqBasis 115 | 116 | remCodePhase = 0.0 117 | 118 | carrFreq = channel[channelNr].acquiredFreq 119 | 120 | carrFreqBasis = channel[channelNr].acquiredFreq 121 | 122 | remCarrPhase = 0.0 123 | 124 | oldCodeNco = 0.0 125 | 126 | oldCodeError = 0.0 127 | 128 | oldCarrNco = 0.0 129 | 130 | oldCarrError = 0.0 131 | 132 | for loopCnt in range(np.long(codePeriods)): 133 | # GUI update ------------------------------------------------------------- 134 | # The GUI is updated every 50ms. This way Matlab GUI is still 135 | # responsive enough. At the same time Matlab is not occupied 136 | # all the time with GUI task. 137 | if loopCnt % 50 == 0: 138 | try: 139 | print 'Tracking: Ch %d' % (channelNr + 1) + ' of %d' % settings.numberOfChannels + \ 140 | '; PRN#%02d' % channel[channelNr].PRN + \ 141 | '; Completed %d' % loopCnt + ' of %d' % codePeriods + ' msec' 142 | finally: 143 | pass 144 | # Read next block of data ------------------------------------------------ 145 | # Find the size of a "block" or code period in whole samples 146 | # Update the phasestep based on code freq (variable) and 147 | # sampling frequency (fixed) 148 | codePhaseStep = codeFreq / settings.samplingFreq 149 | 150 | blksize = np.ceil((settings.codeLength - remCodePhase) / codePhaseStep) 151 | blksize = np.long(blksize) 152 | 153 | # interaction 154 | rawSignal = np.fromfile(fid, settings.dataType, blksize) 155 | samplesRead = len(rawSignal) 156 | 157 | # If did not read in enough samples, then could be out of 158 | # data - better exit 159 | if samplesRead != blksize: 160 | print 'Not able to read the specified number of samples for tracking, exiting!' 161 | fid.close() 162 | trackResults = None 163 | return trackResults 164 | # Set up all the code phase tracking information ------------------------- 165 | # Define index into early code vector 166 | tcode = np.linspace(remCodePhase - earlyLateSpc, 167 | blksize * codePhaseStep + remCodePhase - earlyLateSpc, 168 | blksize, endpoint=False) 169 | 170 | tcode2 = np.ceil(tcode) 171 | 172 | earlyCode = caCode[np.longlong(tcode2)] 173 | 174 | tcode = np.linspace(remCodePhase + earlyLateSpc, 175 | blksize * codePhaseStep + remCodePhase + earlyLateSpc, 176 | blksize, endpoint=False) 177 | 178 | tcode2 = np.ceil(tcode) 179 | 180 | lateCode = caCode[np.longlong(tcode2)] 181 | 182 | tcode = np.linspace(remCodePhase, 183 | blksize * codePhaseStep + remCodePhase, 184 | blksize, endpoint=False) 185 | 186 | tcode2 = np.ceil(tcode) 187 | 188 | promptCode = caCode[np.longlong(tcode2)] 189 | 190 | remCodePhase = tcode[blksize - 1] + codePhaseStep - 1023.0 191 | 192 | # Generate the carrier frequency to mix the signal to baseband ----------- 193 | time = np.arange(0, blksize + 1) / settings.samplingFreq 194 | 195 | trigarg = carrFreq * 2.0 * np.pi * time + remCarrPhase 196 | 197 | remCarrPhase = trigarg[blksize] % (2 * np.pi) 198 | 199 | carrCos = np.cos(trigarg[0:blksize]) 200 | 201 | carrSin = np.sin(trigarg[0:blksize]) 202 | 203 | # Generate the six standard accumulated values --------------------------- 204 | # First mix to baseband 205 | qBasebandSignal = carrCos * rawSignal 206 | 207 | iBasebandSignal = carrSin * rawSignal 208 | 209 | I_E = (earlyCode * iBasebandSignal).sum() 210 | 211 | Q_E = (earlyCode * qBasebandSignal).sum() 212 | 213 | I_P = (promptCode * iBasebandSignal).sum() 214 | 215 | Q_P = (promptCode * qBasebandSignal).sum() 216 | 217 | I_L = (lateCode * iBasebandSignal).sum() 218 | 219 | Q_L = (lateCode * qBasebandSignal).sum() 220 | 221 | # Find PLL error and update carrier NCO ---------------------------------- 222 | # Implement carrier loop discriminator (phase detector) 223 | carrError = np.arctan(Q_P / I_P) / 2.0 / np.pi 224 | 225 | carrNco = oldCarrNco + \ 226 | tau2carr / tau1carr * (carrError - oldCarrError) + \ 227 | carrError * (PDIcarr / tau1carr) 228 | 229 | oldCarrNco = carrNco 230 | 231 | oldCarrError = carrError 232 | 233 | carrFreq = carrFreqBasis + carrNco 234 | 235 | carrFreq_[loopCnt] = carrFreq 236 | 237 | # Find DLL error and update code NCO ------------------------------------- 238 | codeError = (np.sqrt(I_E * I_E + Q_E * Q_E) - np.sqrt(I_L * I_L + Q_L * Q_L)) / ( 239 | np.sqrt(I_E * I_E + Q_E * Q_E) + np.sqrt(I_L * I_L + Q_L * Q_L)) 240 | 241 | codeNco = oldCodeNco + \ 242 | tau2code / tau1code * (codeError - oldCodeError) + \ 243 | codeError * (PDIcode / tau1code) 244 | 245 | oldCodeNco = codeNco 246 | 247 | oldCodeError = codeError 248 | 249 | codeFreq = settings.codeFreqBasis - codeNco 250 | 251 | codeFreq_[loopCnt] = codeFreq 252 | 253 | # Record various measures to show in postprocessing ---------------------- 254 | # Record sample number (based on 8bit samples) 255 | absoluteSample[loopCnt] = fid.tell() 256 | 257 | dllDiscr[loopCnt] = codeError 258 | 259 | dllDiscrFilt[loopCnt] = codeNco 260 | 261 | pllDiscr[loopCnt] = carrError 262 | 263 | pllDiscrFilt[loopCnt] = carrNco 264 | 265 | I_E_[loopCnt] = I_E 266 | 267 | I_P_[loopCnt] = I_P 268 | 269 | I_L_[loopCnt] = I_L 270 | 271 | Q_E_[loopCnt] = Q_E 272 | 273 | Q_P_[loopCnt] = Q_P 274 | 275 | Q_L_[loopCnt] = Q_L 276 | 277 | # If we got so far, this means that the tracking was successful 278 | # Now we only copy status, but it can be update by a lock detector 279 | # if implemented 280 | status = channel[channelNr].status 281 | rec.append((status, absoluteSample, codeFreq_, carrFreq_, 282 | I_P_, I_E_, I_L_, Q_E_, Q_P_, Q_L_, 283 | dllDiscr, dllDiscrFilt, pllDiscr, pllDiscrFilt, PRN)) 284 | 285 | trackResults = np.rec.fromrecords(rec, 286 | dtype=[('status', 'S1'), ('absoluteSample', 'object'), ('codeFreq', 'object'), 287 | ('carrFreq', 'object'), ('I_P', 'object'), ('I_E', 'object'), 288 | ('I_L', 'object'), 289 | ('Q_E', 'object'), ('Q_P', 'object'), ('Q_L', 'object'), 290 | ('dllDiscr', 'object'), 291 | ('dllDiscrFilt', 'object'), ('pllDiscr', 'object'), 292 | ('pllDiscrFilt', 'object'), 293 | ('PRN', 'int64')]) 294 | self._results = trackResults 295 | return 296 | 297 | def plot(self): 298 | import matplotlib as mpl 299 | 300 | # %% configure matplotlib 301 | mpl.rcdefaults() 302 | # mpl.rcParams['font.sans-serif'] 303 | # mpl.rcParams['font.family'] = 'serif' 304 | mpl.rc('savefig', bbox='tight', transparent=False, format='png') 305 | mpl.rc('axes', grid=True, linewidth=1.5, axisbelow=True) 306 | mpl.rc('lines', linewidth=1.5, solid_joinstyle='bevel') 307 | mpl.rc('figure', figsize=[8, 6], dpi=120) 308 | mpl.rc('text', usetex=True) 309 | mpl.rc('font', family='serif', serif='Computer Modern Roman', size=8) 310 | mpl.rc('mathtext', fontset='cm') 311 | 312 | # mpl.rc('font', size=16) 313 | # mpl.rc('text.latex', preamble=r'\usepackage{cmbright}') 314 | 315 | # ./plotTracking.m 316 | 317 | trackResults = self._results 318 | settings = self._settings 319 | channelList = range(settings.numberOfChannels) 320 | 321 | import matplotlib as mpl 322 | import matplotlib.gridspec as gs 323 | import matplotlib.pyplot as plt 324 | 325 | # %% configure matplotlib 326 | mpl.rcdefaults() 327 | # mpl.rcParams['font.sans-serif'] 328 | # mpl.rcParams['font.family'] = 'serif' 329 | mpl.rc('savefig', bbox='tight', transparent=False, format='png') 330 | mpl.rc('axes', grid=True, linewidth=1.5, axisbelow=True) 331 | mpl.rc('lines', linewidth=1.5, solid_joinstyle='bevel') 332 | mpl.rc('figure', figsize=[8, 6], dpi=120) 333 | mpl.rc('text', usetex=True) 334 | mpl.rc('font', family='serif', serif='Computer Modern Roman', size=8) 335 | mpl.rc('mathtext', fontset='cm') 336 | 337 | # mpl.rc('font', size=16) 338 | # mpl.rc('text.latex', preamble=r'\usepackage{cmbright}') 339 | 340 | # ./plotTracking.m 341 | 342 | # This function plots the tracking results for the given channel list. 343 | 344 | # plotTracking(channelList, trackResults, settings) 345 | 346 | # Inputs: 347 | # channelList - list of channels to be plotted. 348 | # trackResults - tracking results from the tracking function. 349 | # settings - receiver settings. 350 | 351 | # Protection - if the list contains incorrect channel numbers 352 | channelList = np.intersect1d(channelList, range(settings.numberOfChannels)) 353 | 354 | # === For all listed channels ============================================== 355 | for channelNr in channelList: 356 | # Select (or create) and clear the figure ================================ 357 | # The number 200 is added just for more convenient handling of the open 358 | # figure windows, when many figures are closed and reopened. 359 | # Figures drawn or opened by the user, will not be "overwritten" by 360 | # this function. 361 | f = plt.figure(channelNr + 200) 362 | f.set_label('Channel ' + str(channelNr) + 363 | ' (PRN ' + str(trackResults[channelNr].PRN) + ') results') 364 | # Draw axes ============================================================== 365 | # Row 1 366 | spec = gs.GridSpec(3, 3) 367 | h11 = plt.subplot(spec[0, 0]) 368 | 369 | h12 = plt.subplot(spec[0, 1:]) 370 | 371 | h21 = plt.subplot(spec[1, 0]) 372 | 373 | h22 = plt.subplot(spec[1, 1:]) 374 | 375 | h31 = plt.subplot(spec[2, 0]) 376 | 377 | h32 = plt.subplot(spec[2, 1]) 378 | 379 | h33 = plt.subplot(spec[2, 2]) 380 | 381 | # Plot all figures ======================================================= 382 | timeAxisInSeconds = np.arange(settings.msToProcess) / 1000.0 383 | 384 | h11.plot(trackResults[channelNr].I_P, trackResults[channelNr].Q_P, '.') 385 | h11.grid() 386 | h11.axis('equal') 387 | h11.set(title='Discrete-Time Scatter Plot', xlabel='I prompt', ylabel='Q prompt') 388 | h12.plot(timeAxisInSeconds, trackResults[channelNr].I_P) 389 | h12.grid() 390 | h12.set(title='Bits of the navigation message', xlabel='Time (s)') 391 | h12.axis('tight') 392 | h21.plot(timeAxisInSeconds, trackResults[channelNr].pllDiscr, 'r') 393 | h21.grid() 394 | h21.axis('tight') 395 | h21.set(xlabel='Time (s)', ylabel='Amplitude', title='Raw PLL discriminator') 396 | h22.plot(timeAxisInSeconds, 397 | np.sqrt(trackResults[channelNr].I_E ** 2 + trackResults[channelNr].Q_E ** 2).T, 398 | timeAxisInSeconds, 399 | np.sqrt(trackResults[channelNr].I_P ** 2 + trackResults[channelNr].Q_P ** 2).T, 400 | timeAxisInSeconds, 401 | np.sqrt(trackResults[channelNr].I_L ** 2 + trackResults[channelNr].Q_L ** 2).T, '-*') 402 | h22.grid() 403 | h22.set(title='Correlation results', xlabel='Time (s)') 404 | h22.axis('tight') 405 | h22.legend(['$\sqrt{I_{E}^2 + Q_{E}^2}$', '$\sqrt{I_{P}^2 + Q_{P}^2}$', 406 | '$\sqrt{I_{L}^2 + Q_{L}^2}$']) 407 | 408 | h31.plot(timeAxisInSeconds, trackResults[channelNr].pllDiscrFilt, 'b') 409 | h31.grid() 410 | h31.axis('tight') 411 | h31.set(xlabel='Time (s)', 412 | ylabel='Amplitude', 413 | title='Filtered PLL discriminator') 414 | h32.plot(timeAxisInSeconds, trackResults[channelNr].dllDiscr, 'r') 415 | h32.grid() 416 | h32.axis('tight') 417 | h32.set(xlabel='Time (s)', 418 | ylabel='Amplitude', 419 | title='Raw DLL discriminator') 420 | h33.plot(timeAxisInSeconds, trackResults[channelNr].dllDiscrFilt, 'b') 421 | h33.grid() 422 | h33.axis('tight') 423 | h33.set(xlabel='Time (s)', 424 | ylabel='Amplitude', 425 | title='Filtered DLL discriminator') 426 | f.show() 427 | --------------------------------------------------------------------------------