commit fee1cd14c8682cb0c591172d60ebdd26041d91e2 Author: ngocmx Date: Wed Oct 22 16:08:12 2025 +0700 first commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..7247465 --- /dev/null +++ b/.gitignore @@ -0,0 +1,102 @@ +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +*.egg-info/ +.installed.cfg +*.egg + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# Jupyter Notebook checkpoints +.ipynb_checkpoints + +# pyenv +.python-version + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype +.pytype/ + +# Cython debug symbols +cython_debug/ + +# VSCode / IDE +.vscode/ +.idea/ + +# Environment +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Logs +*.log + +# Local configs +*.local +*.cfg +*.ini + +# Ignore Mac/Windows/Linux junk +.DS_Store +Thumbs.db +desktop.ini + +# Temporary +tmp/ +temp/ +*.tmp + +# Ignore notebooks outputs +*.ipynb_checkpoints/ + + +NPU_FFT/ + +*.bin \ No newline at end of file diff --git a/Dialnet-AcquisitionAndProcessingOfGPSSignalsBasedOnSoftwar-9963696.pdf b/Dialnet-AcquisitionAndProcessingOfGPSSignalsBasedOnSoftwar-9963696.pdf new file mode 100644 index 0000000..cf6ba03 --- /dev/null +++ b/Dialnet-AcquisitionAndProcessingOfGPSSignalsBasedOnSoftwar-9963696.pdf @@ -0,0 +1,660 @@ + Original Article + + Revista elektron, Vol. 8, No. 2, pp. 43-53 (2024) + + https://doi.org/10.37537/rev.elektron.8.2.195.2024 + +Acquisition and Processing of GPS signals based on + Software Defined Radio + + Adquisición y Procesamiento de señales GPS basadas en Radio Definida por Software + + Castillo Delacroix L. #1, Fagre M. #*2, Vaquila I. *α3, Cabrera M. A. #4 + + #Laboratorio de Telecomunicaciones, Facultad de Ciencias Exactas e Ingeniería, Universidad Nacional de Tucumán + Av. Independencia 1800, Tucumán (4000), Argentina + 1 lecastillodelacroix@herrera.unt.edu.ar + 2 mfagre@herrera.unt.edu.ar + 4 mcabrera@herrera.unt.edu.ar + + * Consejo Nacional de Investigaciones Científicas y Técnicas (CONICET) + Ciudad Autónoma de Buenos Aires (C1425FQB), Argentina + 3 ivaquila@unrn.edu.ar + α INVAP S. E. + Rio Negro(R8400), Argentina + + Recibido: 17/09/24; Aceptado: 02/12/24 + +Abstract- This paper presents the development of a software tool I. INTRODUCTION +that implements an acquisition block for GPS signals using SDR +technology, specifically devices like the HackRF One and RTL- In the last decades, location services through Global +SDR. The tool, implemented in Python, successfully detects Navigation Satellite System (GNSS), such as the Global +satellites and estimates their parameters, offering customizable Positioning System (GPS), have become an essential feature +algorithm settings and the capability to visualize the satellite of mobile devices. GPS receivers require a high level of +search space in three dimensions. Comparative tests with the integration, low cost, and reduced power consumption [1,2]. +GNSS-SDR software tool demonstrated excellent performance, To achieve these requirements, they are implemented in +providing a solid foundation for further development toward a Application Specific Integrated Circuits (ASIC) [3,4]. This +complete SDR-based GNSS receiver. This work highlights the solution is suitable for applications that only need to know +potential of SDR platforms for research and development, the position of the device but limits the user’s ability to +emphasizing their flexibility, potential for future upgrades, and interact with the architecture, algorithm, or parameters of the +cost-effectiveness in advancing future GNSS technologies. receiver [5,6]. Due to these limitations, the approach of + GNSS Receivers implemented in Software Defined Radio +Keywords: GNSS, GPS, Receiver, SDR. (SDR) emerged, where all signal processing is done in the + software domain. This approach allows modifying the +Resumen- Este trabajo presenta el desarrollo de una herramienta algorithms or parameters in real-time or in post-processing, +de software que se implementa mediante un bloque de simply by modifying the receiver software, thus granting a +adquisición para señales GPS utilizando tecnología de SDR, high degree of flexibility to the design. The current GNSS +específicamente dirigida a dispositivos como HackRF One y scenario involves multi-constellation systems, which pose +RTL-SDR. La herramienta implementada en Python detecta the challenge of designing receivers capable of processing +satélites y estima sus parámetros con muy buen desempeño, signals of different characteristics, mitigating interference, +ofreciendo configuraciones de algoritmo personalizables y posee and providing high-precision positioning, such as Precise +la capacidad de visualizar el espacio de búsqueda de satélites en Point Positioning (PPP). Most current civilian receivers can +tres dimensiones. Las pruebas comparativas con la herramienta only decode GPS-L1 C/A signals, so research laboratories +de software GNSS-SDR demostraron un alto rendimiento, are working on receivers implemented in software [7,8,9]. +proporcionando una base sólida para expandir el desarrollo Development and research on SDR-based GNSS receivers +hacia un receptor GNSS completo basado en tecnología SDR. En are expanding due to the flexibility and upgradeability they +esta propuesta se destaca el potencial de las plataformas SDR offer, which is crucial for advanced applications and future +para la investigación y el desarrollo, enfatizando su flexibilidad, navigation technologies. A notable example is the open- +capacidad de actualización y rentabilidad para avanzar en source GNSS-SDR project of the Center Tecnològic de +futuras tecnologías GNSS. Telecommunications de Catalunya. This program, + +Palabras Claves: GNSS, GPS, Receptor, SDR. + +ISSN 2525-0159 43 + Revista elektron, Vol. 8, No. 2, pp. 43-53 (2024) + +developed in C++ and combined with GNU-Radio, runs on block can be modified once implemented in hardware, +a general-purpose computer and allows selecting between whereas SDR implementations allow for the modification of +different algorithms and accessing intermediate signals, any parameter or block in the receiver chain simply by +which is useful to validate the development of the adjusting the software. This flexibility provides a significant +acquisition stage [10,11,12]. advantage in SDR-based implementations, making them an +This paper presents the development of a Python-based invaluable prototyping tool for DSP engineers, who can +software tool for GPS signal acquisition based on SDR rapidly test and iterate different architectures or algorithms. +devices. The acquisition stage, critical for detecting visible Figure 1. shows the GPS receiver block diagram based on +satellites and estimating synchronization parameters like SDR. The antenna used in this work was a GPS commercial +Doppler shift and code delay, is implemented using patch antenna, designed for a frequency of 1575.42 MHz, +advanced algorithms and optimized for parallel processing. corresponding to the L1 band. The RF Front-End block was +Recent research has highlighted significant advancements in implemented using an SDR device which will amplify, filter +signal acquisition and tracking for GPS-based Software- a digitize the analog signal received by the antenna. To +Defined Radio (SDR) receivers, demonstrating the choose the SDR hardware, we analyzed two options +feasibility of using SDR platforms for efficient acquisition available in our laboratory. It’s important to mention that +and tracking stages and the flexibility of SDR in optimizing both devices are widely used due to their low cost and good +signal algorithms for enhanced performance under varying performance. The commercial HackRF One [24] is a +environmental conditions [13,14,15]. This work contributes versatile SDR platform, designed for both transmission and +to the field of SDR-based GNSS receivers by providing a reception of radio signals across a wide frequency range +customizable and user-friendly platform for GPS signal from 1 MHz to 6 GHz. This open-source hardware device is +acquisition. The tool was fully implemented in Python, due ideal for testing, developing, and experimenting with a +to the existence of libraries related to digital signal broad spectrum of modern and future radio technologies. +processing and the extensive documentation available [16]. Whether used as a USB peripheral or configured for +Besides, it supports multiple SDR formats and incorporates standalone operation, the HackRF One offers flexibility and +visualization features to enhance usability. Comparative adaptability for a variety of applications, making it a +tests with the GNSS-SDR software validate its accuracy and valuable tool for hobbyists, researchers, and professionals in +performance, demonstrating its potential as a foundation for the field of wireless communications. The RTL-SDR [25] is +a complete SDR-based GNSS receiver capable of multi- a cost-effective SDR receiver, widely popular among +frequency, multi-constellation processing [17,18,19, hobbyists and professionals for its versatility and ease of +20,21,22,23]. use. Originating from repurposed USB TV tuner dongles, + the RTL-SDR can receive frequencies from approximately + II. SYSTEM ARCHITECTURE 500 kHz to 1.75 GHz. It supports a wide range of + applications, including radio astronomy, weather satellite +Although this development follows the SDR paradigm, the image reception, ADS-B aircraft tracking, and general radio +proposed diagram shown in Figure 1 is applicable to both scanning. Its affordability, coupled with an extensive array +Application Specific Integrated Circuit (ASIC) and SDR of compatible software, makes the RTL-SDR an excellent +implementations. The key difference lies in the approach: in entry point into the world of SDR for beginners and a +an ASIC implementation, all these blocks are hardwired valuable tool for experienced users. In Table 1, there are the +using transistors integrated into a single silicon chip, making main characteristics of both devices. +them fixed and unalterable. In contrast, the SDR approach Tests were conducted with both devices to evaluate their +employs a reconfigurable radio frequency interface (RF reception capabilities. During the tests with the HackRF, it +Front-End), where the SDR device is responsible for was impossible to achieve a lock on any satellite. After +converting the analog RF signal from the antenna into digital several tests, it was concluded that it needs an external clock +samples. These samples are then processed by a general- signal because its internal oscillator is not precise enough to +purpose processor, a Digital Signal Processor (DSP), or even +a Field Programmable Gate Array (FPGA). With ASICs, no + +Fig. 1. GPS receiver block diagram implemented by SDR. + +ISSN 2525-0159 44 http://elektron.fi.uba.ar + Revista elektron, Vol. 8, No. 2, pp. 43-53 (2024) + +process GPS signals, which require 1 ppm (part per million) Fig. 2. Power spectral density diagram for the L1 band.[8] +or less. Various software configurations were tried, even +reducing the frequency and time resolution requirements to The C/A code is a spread sequence that belongs to the family +attempt to achieve a lock, but it was unsuccessful. of Gold Codes [26]. These sequences are commonly known +In the tests with the RTL-SDR dongle, excellent results were as pseudo-random codes or Pseudo-Random Noise (PRN) +achieved for the acquisition, lock, and decoding of the because of their apparently random nature. Pseudo-random +navigation message, successfully obtaining the position codes are utilized because they do not repeat within their +solution. sequence, providing highly advantageous correlation +Based on Table 1, it is observed that both devices have a properties for signal decoding. In the GPS system the C/A +suitable frequency range to capture the L1 carrier and both codes are binary and deterministic pseudo-random +have an 8-bit quadrature ADC. Regarding the sampling sequences with noise-like properties. Each code consists of +frequency, the RTL-SDR is very close to the Nyquist 1023 chips, with a chip analogous to a bit, however, a chip +sampling theorem limit, but it satisfies it, unlike the HackRF, does not carry useful information. These sequences consist +which far exceeds it. Both have a bias-t, but only the dongle of 512 ones and 511 zeros and repeat every 1 ms, +has a <1 ppm TCXO. Although the HackRF is a transceiver corresponding to a frequency of 1.023 MHz. +and the RTL-SDR is only a receiver, the application to be Each SV is assigned a unique PRN code for a specific +developed only involves reception, so this characteristic period, which may be updated over time [9]. This code is +does not affect it, but the enormous price difference between crucial because all satellites transmit simultaneously on the +the two does, precisely due to that functionality. For all the same carrier frequency using a Code Division Multiple +reasons mentioned above and based on the tests conducted, Access (CDMA) scheme. In practice, this is achieved +without making any modifications to the devices, where through a combination of CDMA and Direct Sequence +only the RTL-SDR achieved successful reception, we Spread Spectrum (DSSS) technology, resulting in what is +believe that the latter best suits our needs. known as Direct Sequence CDMA (DS-CDMA). This + encoding technique involves performing a logical Exclusive + TABLE I. OR (XOR) operation between the PRN code and the data to +Comparison of technical characteristics of the SDR devices analyzed be transmitted. Consequently, the signal is transmitted with + a bandwidth much broader than that required by the data, + Freq. Sampling reducing the power spectral density. The resulting signal + Range Rate ADC External Price exhibits a noise-like spectrum, making it undetectable and +SDR [MHz] [MHz] [bits] TCXO Clock [US$] undecodable without the correct PRN code. From all this, + the transmitted signal for a k satellite is given by: +HackRF 1-6000 20 8 No Yes 250 + () = √2(() ⊕ ()) cos(21) + +One √21(() ⊕ ()) sin(21) + +√22(() ⊕ ()) sin(22) (1) +RTL- 8 Yes No 25 + 24-1766 3.2 where, , 1 and 2 are the signal power of C/A and P + (L1 and L2) codes, and are the C/A and P (Y) codes +SDRv3 assigned to the k satellite, is the navigation data, and 1 + and 2 are the carrier frequencies L1 and L2. Since in the + III. METHODOLOGY + +A. Signal characteristics + +The Digital Signal Processing (DSP) block shown in Figure +1 consists of three main stages: acquisition, tracking and +demodulation. In this work we will focus on the acquisition +stage. +Each satellite, referred to as a Satellite Vehicle (SV), +currently has two unique spreading codes or sequences for +signal transmission. The first one is the unencrypted Coarse +Acquisition (C/A) code, assigned to civilian use, while the +second is an encrypted code, known as the P(Y) code, +designated exclusively for military applications and +restricted from civilian access. The C/A code is modulated +only on the L1 carrier frequency, whereas the P(Y) code is +modulated on both the L1 and L2 carrier frequencies. In this +study, our focus is on the C/A code, and its spectral +characteristics are illustrated in the power spectral density +diagram of the L1 band, as shown in Figure 2. + +ISSN 2525-0159 45 http://elektron.fi.uba.ar + Revista elektron, Vol. 8, No. 2, pp. 43-53 (2024) + +present work we will only focus on the C/A code, which is represents a unique position within the search space, known +modulated in the L1 band, the parameters related to the P as a cell, as shown in Figure 3. To perform the search +and L2 codes are canceled. Then, our signal is simplified as process, a test statistic must be evaluated in each cell. The +follows: result of this function is then compared to a predefined + threshold. This comparison determines whether the satellite +() = √2(() ⊕ ()) cos(21) (2) is present and identifies its synchronization parameters. + +When propagating from the SV to the receiver in Earth this Fig. 3. Algorithm Search Space +signal is affected for different phenomena., mainly by the +deviation in the L1 carrier frequency due to the Doppler A key aspect of any acquisition algorithm is defining the +effect and the propagation delay in the C/A code. The dimensions of the search space, the frequency and code +Doppler effect is the result of the frequency shift caused by search ranges, and the resolution between two consecutive +the relative movement of the transmitter (located on the bins. For the code delay axis, the range is determined by the +satellite) with respect to the receiver on the ground, even length of the C/A code, specified by the GPS L1 C/A signal +though the latter is static. The Doppler shift affects both the specification. This code has a length of 1023 chips, starting +process acquisition and GPS signal tracking. For a stationary from position 0. The resolution between two consecutive +receiver, the maximum Doppler shift for the L1 frequency is bins must be at least one chip, resulting in 1023 code bins. +usually considered approximately ±5 KHz, and for a moving However, it is possible to perform a higher-resolution search +one ±10 KHz. This effect, and the propagation delay on the using smaller separations between bins. For the frequency +C/A code in its path from the transmitter to the receiver, are axis, the range is typically ± 5 KHz for a static receiver and +two key parameters for the accurate decoding and ±10 KHz for a moving receiver. The resolution is commonly +demodulation of the signal at the receiver. The estimation of given by frequency bins equal to 1/T, where T is the coherent +these parameters is the main function of the acquisition integration time. This implies that as the integration periods +stage, which is the focus of our study. So, we already know extend, the width of the frequency bins decreases, enhancing +the main characteristics of the transmitted signal, then we the frequency resolution. The improvements in the +can proceed to design the acquisition stage. resolution of each axis depend exclusively on the algorithm + used. There are different algorithms to perform the search, + B. Acquisition algorithms such as Serial Search (SS), Parallel Frequency Space Search + (PFSS), and Parallel Code Phase Search (PCPS) acquisition, +The acquisition stage aims to detect the satellites visible to the latter being the option chosen in this work. The PCPS +the receiver and obtain a first estimation of Doppler algorithm, whose block diagram is shown in Figure 4, +deviation and code delay. We can express the received signal performs the search by going through all possible frequency +as a combination of the n visible satellite signals: values and parallelizing the search through the code + parameter, which has a significantly higher number of steps. + () = 1() + 2() + ⋯ + () (3) Instead of multiplying the input signal by the local PRN for + all possible code delays, as SS and PFSS algorithms do, it +Suppose we need to acquire the i satellite. The received uses the circular cross-correlation technique based on the +signal s should be multiplied by a locally generated C/A Fourier Transform. This technique relies on the property that +code corresponding to the satellite being tracked. This the Discrete Fourier Transform (DFT) of circular cross- +ensures that only if the phase of the local code matches the +phase of the received signal, meaning both codes are +perfectly time-aligned, the signals from other satellites, are +eliminated due to the correlation properties of the C/A codes. +Similarly, the carrier frequency must be filtered, as it is +affected by the Doppler shift. Therefore, knowing its exact +value is necessary for proper filtering. The acquisition +process can be viewed as a search for these two parameters +in a two-dimensional space or grid, called the Search Grid. +The axes of this grid are the Doppler Frequency and the +Code Delay. Since these parameters are continuous, we must +set a resolution for discretizing the space. The smallest +search resolution or step is called a bin, which results in a +frequency bin and a code bin. A combination of specific bins + +ISSN 2525-0159 46 http://elektron.fi.uba.ar + Revista elektron, Vol. 8, No. 2, pp. 43-53 (2024) + +correlation is equivalent to the multiplication of the DFT of frequency fs or the integration time T, as a correlation value +the code and the DFT of the input signal, taking the complex is obtained for each sampling instant. For example, using a +conjugate of one of them. Performing the inverse transform sample rate of 2 MHz and an integration time of 1 ms yields +of this multiplication yields the circular cross-correlation in 2000 samples, representing 2000 bins of code instead of +the time domain. The squared magnitude of this result 1023. That is 2000/1023=1.96 samples for each bin, +represents the test statistic and is compared with a resulting in a resolution of 1/1.96=0.51 between bins. In +predefined threshold to determine if the desired satellite has summary, the accuracy of the parameters estimated with this +been acquired. If there is a peak in the test statistic, its index algorithm is ±1/2 bin in frequency, and in the code +over the code dimension indicates the delay of the PRN code dimension, it depends on the sampling frequency. +of the input signal for the analyzed frequency. A good way to compare the algorithms mentioned above is + by evaluating the number of combinations each requires and + the complexity of their implementation, as shown in Table + 2. + + TABLE II. + Results of the comparison between SS, PFSS, and PCPS Acquisition + + algorithms. + + Algorithm Combinations Complexity + + Serial Search Acquisition 41943 Low + + Parallel Frequency Space 1023 Medium + Search Acquisition 41 High + Parallel Code Phase Search + Acquisition + +Fig. 4. Block Diagram of Parallel Code Phase Search acquisition As observed, the Serial Search algorithm performs the worst +algorithm due to the large number of combinations needed compared + to the other two algorithms. The performance of both +This algorithm sweeps only over the frequency dimension, parallelized algorithms strongly depends on the +that is, across all possible frequencies, which results in the implementation of the DFT. There are many +number of combinations given by: implementations for the DFT, the Fast Fourier Transform + (FFT) stands out as the fastest and most widely used. +. + 1 = (4) As previously explained, the acquisition process involves + searching in a two-dimensional grid to determine if the + . target satellite is present in any cell and, if so, obtaining its + coordinates, which correspond to the synchronization +The algorithm allows for defining the resolution and bin size parameters Doppler shift and code phase. This requires +in the frequency dimension. However, this choice is not evaluating the test statistic in each cell (SS), column (PFSS), +entirely independent, as it depends on the length of the data or row (PCPS) processed, depending on the algorithm, and +on the DFT, which means the number of samples analyzed, comparing it with a pre-established threshold to decide if the +and consequently the integration period. The size of a satellite is present. In all three algorithms, the test statistics +frequency bin is commonly defined as follows: are implemented using the mathematical function "squared + modulus." There are different ways to find the maximum +∆ = = = 1 (5) value of the test statistic across the entire search space, + . which will be compared to the threshold. In the chosen + implementation PCPS, the value corresponding to the +which results in a resolution (the maximum frequency maximum of the first row (or the first frequency) processed +separation between two consecutive bins) of 1/2T [12]. For is initially stored along with its position on the grid. If the +example, for integration periods of 1 ms, frequency bins of statistic for the new analyzed frequency exceeds the stored +1 kHz are obtained, resulting in a maximum resolution of value, the new value is saved; otherwise, the process moves +500 Hz. For integration periods of 2 ms, frequency bins of to the next frequency. This process continues until all +500 Hz are obtained, resulting in a maximum resolution of frequencies are examined, resulting in the identification of +250 Hz. To get better resolution, a longer integration period the maximum test statistic across the entire search space. +must be used to increase the number of DFT samples in Calculating the decision threshold is critical for any +equation 5. Additionally, the algorithm indirectly improves acquisition algorithm. If the threshold value is too low, it +the resolution in the code dimension by increasing the +number of samples, either by increasing the sampling + +ISSN 2525-0159 47 http://elektron.fi.uba.ar + Revista elektron, Vol. 8, No. 2, pp. 43-53 (2024) + +leads to more false positives; if too high, it may miss Fig. 5. Structure of an integration window with multiple dwells +detecting satellites. Advanced techniques like the Constant +False Alarm Rate (CFAR) [27,28] can calculate adaptive Fig. 6. Block Diagram of the initial version of the implemented algorithm +thresholds, but they add complexity and are beyond the +scope of this work. Therefore, a fixed threshold of 0.019 is C. Implementation and Optimization +implemented, derived from averaging numerous tests under +various propagation conditions, which can also be modified To implement the proposed algorithm, it is necessary to +by the user to adapt to different conditions and locations. locally generate the PRN code of the satellite to be searched. +Techniques for determining satellite presence can be Additionally, a small adjustment to the number of samples +classified based on the number of times or windows a cell is for each chip is required. The signal samples entering the +analyzed during a non-coherent integration period: acquisition stage have been sampled by the ADC at a + specific frequency, in our case 2 MS/s. Therefore: + • Single-dwell: Analysis is performed only once per + cell to decide, using a single coherent integration ℎ [] = [] · ℎ [] (6) + window. + + • Multiple-dwell: Analysis is repeated on the same + cell at regular intervals, with averaging performed ℎ = 1⁄ℎ (7) + to decide, using two or more coherent integration + windows that form a non-coherent integration + window. The number of coherent integrations and + the required positive acquisitions (max dwells) + must be defined for acquisition over the non- + coherent integration window. + +A related parameter is the "dwell time," which is the time +needed to verify the satellite presence. For single-dwell, it +equals the coherent integration period; for multiple-dwell, it +is the product of max dwell and the coherent integration +time. Figure 5 illustrates a multiple-dwell technique with +three coherent integration windows. If max dwells are set to +two, at least two of the three integrations must show positive +acquisition for cell acquisition. The developed tool allows +users to choose between single-dwell or multiple-dwell +decisions, configuring the number of dwells and max dwells +accordingly. The navigation data is transmitted at 50 bps, +resulting in a 20 ms period where the bit is set to 1 or -1, +after which a transition may occur. If a bit of transition +occurs during the acquisition process, it can cause errors. To +ensure optimal algorithm performance, no bit transitions +should occur in the analyzed data sequence. While longer +data sequences increase the likelihood of successful satellite +acquisition, they also demand more processing time and +capacity. Additionally, the frequency resolution, which is +inversely proportional to the integration time, improves with +longer integration periods, enhancing the number of DFT +samples. For all these reasons, the length of the data to be +analyzed should be chosen carefully. This length should not +be less than 1 ms, which is the duration of a complete C/A +code. Using a shorter length would result in correlations +with incomplete codes, affecting the performance of the +algorithm. Therefore, the length should be an integer +multiple of 1 ms. A recommended duration is 2 ms, as 1 ms +does not provide good frequency resolution. However, in our +implementation, this parameter can be configured by the +user for each execution of the program. + +ISSN 2525-0159 48 http://elektron.fi.uba.ar + Revista elektron, Vol. 8, No. 2, pp. 43-53 (2024) + +Considering = 2[] and ℎ = 0.977 [] then implement this is through a classic for loop, which iterates +from equation 6 the number of samples per chip will be through all the satellites and applies the proposed algorithm +ℎ = 2. It follows that for the 1023 C/A code chips, our to each one. The main disadvantage of using a for loop is +block must generate 1023 · 2 = 2046 samples for each that it was designed to search for a single satellite at a time, +C/A code period, as shown below: and in each iteration, the process of reading the input + samples is repeated, even though the samples are always the + = [(1) (1)(2)(2) . . . (1023) (1023)] (8) same. Although it is possible to modify the reading process + so that it is done only once and then the 32 satellites are +The initial version of our algorithm shown in Figure 6 was searched through the loop, it was already observed in the +designed to search for only one satellite to validate the first implementation that the processing took a considerable +studied methods. The software tool was developed entirely amount of time. Furthermore, as is characteristic of the for +in Python. After the first test, we observed a bottleneck in loop, it is a blocking execution which means that we must +data processing, which significantly increased the wait for the search for one satellite to finish before starting +processing time. To address this issue, we decided to modify the next one, despite there being no limitations between +the data reading process by implementing generating individual searches for each satellite. For these reasons, and +functions. These functions generate each data item only with a future implementation that can process samples in +when it needs to be processed, like lists functions, but real-time in mind, we decided to investigate different +without keeping the data in memory. The data doesn't exist parallelization methods at the processor level that allow for +until requested, eliminating the need to load the entire list searching more than one satellite at the same time. This +before using it. Generating functions are especially useful approach would also take advantage of the multicore +when dealing with large datasets, where lists can consume architectures of current processors to improve performance. + Currently, it is common practice to parallelize code by + isolating a specific function that can be executed multiple + times and running it on different processors to enable + processors on a machine, by running independent parallel + + TABLE III. + Comparison between different methods of Multiprocessing module in + + Python + + Methodology Multiple ConcurrenceBlocker Sorted + + Arguments Results + + Pool.map No Yes Yes Yes + + Pool.map_async No Yes No Yes + + Pool.apply Yes No Yes No + + Pool.apply_sync Yes Yes No No + + Pool.starmap Yes Yes Yes Yes + + Pool.starmap_async Yes Yes No No + +Fig. 7. Block Diagram of part of the modified version of the algorithm processes using child processes instead of threads. The + maximum number of processes that can run at the same time +significant memory, or when data is received in real-time, is limited by the number of host processors. The +such as an SDR device, making it impractical to wait for all multiprocessing.Pool() class provides the apply(), map(), +data to arrive before processing. Furthermore, a new and starmap() methods to execute the passed function in +acquisition strategy was adopted. To detect a satellite that is parallel. Choosing the most suitable one for our +visible at any given second, and assuming it remains visible implementation must consider the following parameters: +for at least 0.5 to 1 second, it is sufficient to search for it multiple arguments, concurrency, blocking and order of +once, twice, or multiple times per second, depending on the results. Table 3 presents a summary of the methods and +number of integrations selected. It is not necessary to search parameters mentioned, with the aim of being able to +for the satellite in every sample throughout all seconds of compare them. All three methods offer synchronous and +the file. Once the performance issues with the algorithm asynchronous variants. Due to the design of our algorithm, +were resolved, we began the scaling process to search for all it requires a method that can handle multiple arguments and, +32 satellites, our main aim. The simplest and fastest way to for simplicity, able to return ordered results. According to + +ISSN 2525-0159 49 http://elektron.fi.uba.ar + Revista elektron, Vol. 8, No. 2, pp. 43-53 (2024) + +Table 3, the only method that satisfies these criteria is recording include satellites 2, 12, 24, 25, and 29. These +Pool.starmap. To implement this, we first initialize the n results agree with those obtained by our tool and GNSS- +processors using the Pool class, then pass the functions we SDR tool. Although the application lists additional satellites, +want to execute in parallel to starmap. In our case, we set their acquisition is significantly influenced by factors such +the number of processors to the total number of system as signal-to-noise ratio, line of sight, and mask angle. +processors minus one, to avoid overloading the operating +system and to prevent potential issues. This is a common Fig. 8. List of Satellites acquired by GNSS Status mobile application while +practice that ensures efficient use of resources when running recording data samples using RTL-SDR. +algorithms on platforms with many cores. Then, the +structure of our initial algorithm is modified as shown in TABLE IV. +Figure 7. Comparison of the results obtained processing the GNSS-SDR input +Another modification, as shown in Figure 7, was also made format files with GNSS-SDR software tool, and the tool developed in this +regarding the way of generating the PRN codes belonging to +each satellite. Previously, a single code belonging to the work. +searched satellite had to be generated at the time of +execution. But when searching for the 32 satellites, instead Software Tool +of generating the codes one by one each time, it is +convenient to pre-allocate or previously generate a list with Case Satellite GNSS-SDR This work +the codes of the 32 satellites, removing this function of the +interior of the diagram proposed in Figure 6. Doppler Delay Doppler Delay + + IV. RESULTS 2 - - 0 657 + +To validate our implementation, we compared the obtained 1 12 1000 1274 1000 797 +results with the generated by the GNSS-SDR software tool. +To achieve this, the developed tool allows to record samples, 29 - - 2000 782 +and read raw sample files, whether they were recorded using +this tool or with the GNSS-SDR software. Due to the 2 -500 1620 -500 657 +existence of two different types of sample files, our reading +function fits to both formats (packaging type, number of 12 1000 876 1000 796 +bytes per sample, sign and amplitude correction factors). +Furthermore, it has been developed so that, if we need to add 2 24 -1500 1194 -1500 437 +another type of reading format from a different SDR device, +it is relatively easy to extend the code if the characteristics 25 2500 1900 2500 651 +of the samples are known. +We chose three cases to compare these tools using GNSS- 29 2500 1020 2500 781 +SDR format files, modifying two parameters, the coherent +integration time and the Doppler resolution. The cases 2 - - -250 657 +analyzed are using an Integration time of 1, 2 and 4 [ms] and +a Doppler resolution of 1000, 500, 250 [Hz], corresponding 12 1000 649 1000 793 +to Case 1, 2 and 3 respectively. The results obtained from +the comparison are summarized in Table 4. 3 24 -1500 2000 -1500 439 +As an additional verification method to identify the satellites +present (excluding their synchronization parameters) 25 2750 1462 2750 645 +detected by the developed tool, the GNSS Status application +for Android devices was used while recording the signal file. 29 2250 385 2250 775 +This application displays real-time information on the +GNSS satellites present at the location of the device, After searching for all the satellites with the tool, it is known +including signal-to-noise ratio, elevation and azimuth which of them are present, so that individual searches can be +angles, position, and accuracy, among other parameters. As carried out for them and the graphical representation of the +shown in Figure 8, the satellites detected during the file search space can be observed. For example, Figure 9 shows + the plots obtained by our tool and by GNSSSDR, using an + +ISSN 2525-0159 50 http://elektron.fi.uba.ar + Revista elektron, Vol. 8, No. 2, pp. 43-53 (2024) + +Fig. 9. Three-dimensional plots of the search space for our tool (left column) and GNSS-SDR tool (right column) for Satellite 29 (Top row), Satellite 25 (Middle +row) and Satellite 1 (Bottom row). + +integration Time = 2 [ms] and Doppler Resolution = 500 As shown in Table 4, each execution of the developed tool +[Hz], for the individual searches of satellites 29 and 25 both consistently produces the same code delay values for the +present and satellite 1, absent. On the top and middle rows same input file. In contrast, the GNSS-SDR software tool +of Figure 9, corresponding to satellites 29 and 25 yields varying delay values between executions, which do +respectively, we can see that there is a maximum peak on the not match those produced by our tool. This variability is +correlation matrix represented in the search space with attributed to the internal workings of GNSS-SDR, which is +coordinates in the Doppler frequency and delay axes that designed to operate in real-time across various SDR devices. +coincide with the values of these parameters previously It internally relies on the GNU Radio Scheduler, where each +obtained by the tool during the search for all satellites. In processing block runs as quickly as possible. This causes the +other words, the acquisition of these satellites has been exact execution order to fluctuate based on the machine's +achieved with those synchronization parameters. Also, we current load, making the process non-deterministic. +can see on the bottom row of Figure 9 that the same does not However, this randomness in execution does not +apply to satellite 1, which is not found as it is absent. significantly affect the final position calculation, resulting + +ISSN 2525-0159 51 http://elektron.fi.uba.ar + Revista elektron, Vol. 8, No. 2, pp. 43-53 (2024) + +only in minor variations in the decimal points of longitude, (GLONASS, Galileo, Beidou). Other planned advancements +latitude, and altitude. If the hardware environment is known, include integrating adaptive thresholding techniques, such +real-time requirements are absent, or a fixed and known as Constant False Alarm Rate (CFAR), for enhanced +amount of data is read from a file (buffer size), the sample detection threshold calculation. These developments aim to +management can be more controlled, allowing for orderly further expand the capabilities and applications of the tool +and efficient processing. This ensures a deterministic in GNSS research. +workflow, as demonstrated by the developed tool, where +the same input consistently produces the same output. REFERENCES +Additionally, the Doppler frequency results generated by the +tool are identical to those obtained by GNSS-SDR in every [1] Groves, P. D., Principles of GNSS, Inertial, and Multisensor Integrated +execution. + Navigation Systems, Artech House, 2021. + V. CONCLUSIONS + [2] Dabove, P., & Manzino, A. M., GNSS positioning using smartphones: +In this work, a software tool was developed to implement +the acquisition stage of GPS signals using SDR technology. challenges and opportunities, Electronics, 9(2), 258, 2020. +The tool successfully detects satellite signals and their +associated parameters, allowing users to configure various [3] Misra, P., & Enge, P., Global Positioning System: Signals, +algorithm settings quickly and intuitively. Besides, it +provides a graphical representation of the satellite search Measurements, and Performance (2nd ed.). Ganga-Jamuna Press, 2018. +space in three dimensions and enables results to be saved for +further analysis. The tool was fully implemented in Python, [4] Kaplan, E. D., & Hegarty, C. J., Understanding GPS/GNSS: Principles +a widely used programming language in the scientific +community, which simplifies maintenance and updates. Its and Applications, Artech House, 2017. +compatibility ensures that it can run seamlessly on any +computer with Python installed. The Python code used in [5] Angrisano, A., Gaglione, S., & Gioia, C., Performance assessment of +this study is available in the GitHub repository “PyGNSS- +SDR” [29]. The tool supports input data files in multiple assisted GNSS for smartphones in hybrid positioning mode, Sensors, 13(9), +formats, including those generated by RTL-SDR and GNSS- +SDR devices, providing flexibility for different research 11485-11505, 2013. +scenarios. Validation tests demonstrated excellent +agreement between the results of the developed tool and [6] Rao, Y. S., Wang, R., & Zhang, X., Advances in GNSS-R Technologies +those obtained using GNSS-SDR software, confirming its +accuracy and reliability. and Applications: A Survey, Remote Sensing, 12(8), 1335, 2020. +While the tool currently represents only the acquisition +block of a GNSS receiver, it establishes a robust foundation [7] Petovello, M. G., & Lachapelle, G., Software-defined GNSS receivers: +for the development of a complete receiver. The envisioned +system is intended for applications such as supporting Architecture, design, and future trends, IEEE Transactions on Aerospace +Ground-Based Augmentation Systems (GBAS), conducting +ionospheric scintillation studies, and providing GPS signal and Electronic Systems, 59(1), 123-138, 2023. +integrity reports. The use of SDR technology offers a highly +flexible and cost-effective approach to GNSS receiver [8] Borre, K., & Akos, D. M., Flexible GNSS receivers: The potential of +development, particularly in the current landscape of multi- +constellation systems (GPS, GLONASS, Galileo, Beidou) SDR, Navigation: Journal of the Institute of Navigation, 69(4), 321-335, +and multi-frequency bands. SDR platforms democratize +access to GNSS research, enabling researchers and 2022. +developers to engage in this field without the prohibitive +costs associated with traditional hardware-based [9] Gamba, F., Tiberius, C., & Teunissen, P. J. G., Challenges and +approaches. The tool developed in this project introduces a +novel testing framework for GNSS research and advancements in multi-constellation GNSS positioning: Focusing on +development in our country. Future work will focus on +implementing the tracking and demodulation stages, Precise Point Positioning (PPP), GPS Solutions, 28(1), 23-38, 2024. +extending support to additional frequency bands (L2 and +L5), and enabling compatibility with other constellations [10] Fernandez-Prades, C., & Seco-Granados, G., GNSS-SDR: Enhancing + + research capabilities in GNSS signal processing with open-source tools, + + Proceedings of the International Technical Meeting of the Satellite Division + + of The Institute of Navigation (ION GNSS+ 2021), 123-134, 2021. + + [11] Closas, P., Seco-Granados, G., & Fernandez-Prades, C., Open-source + + software-defined GNSS receivers: A versatile tool for signal processing and + + algorithm validation, Navigation: Journal of the Institute of Navigation, + + 69(4), 407-419, 2022. + + [12] Fernandez-Prades, C., Closas, P., & Seco-Granados, G., GNSS-SDR: + + An open-source tool for research and experimentation in GNSS signal + + processing, IEEE Transactions on Aerospace and Electronic Systems, + + 59(2), 789-798, 2023. + + [13]Söderholm, S., Bhuiyan, M.Z.H., Thombre, S. et al. A multi-GNSS + + software-defined receiver: design, implementation, and performance + + benefits. Ann. Telecommun. 71, 399–410, 2016. + + https://doi.org/10.1007/s12243-016-0518-7. + + [14] Zhao, J., Chang, J., Yin, R., & Wang, C., Acquisition and tracking + + loops based on software defined radio, Symposium on ICT and Energy + + Efficiency and Workshop on Information Theory and Security (CIICT + + 2012), pp. 136-141, 2012. doi: 10.1049/cp.2012.1878. + + [15] Htay, H., Lwin, Z., & Hla, T., Implementation of Signal Acquisition + + and Tracking for GPS-Based Software Defined Radio + + Receiver. International Journal of Geoinformatics, 19(2), 55–64, 2023. + + https://doi.org/10.52939/ijg.v19i2.2567 + + [16] Liu, Y., Li, J., & Wang, S., Implementation of GNSS signal processing + + using Python libraries, Digital Signal Processing, 120, 103-112, 2022. + + [17] Akos, D. M., Normark, P. L., Enge, P., Hansson, A. & Rosenlind, A., + + Real-Time GPS Software Radio Receiver, Proceedings of the 2001 National + + Technical Meeting of The Institute of Navigation, 2001. + + [18] Humphreys, T., Psiaki, M., & Kintner, P., GNSS Receiver + + Implementation on a DSP: Status Challenges and Prospects, Proceedings + + of the 19th International Technical Meeting of the Satellite Division of the + + Institute of Navigation ION GNSS, vol. 4, 2006. + +ISSN 2525-0159 52 http://elektron.fi.uba.ar + Revista elektron, Vol. 8, No. 2, pp. 43-53 (2024) + +[19] Borre, K., Akos, D. M., Bertelsen N., Rinder P., & Jensen S. H., A +software-defined GPS and Galileo receiver: a single-frequency approach, +Applied and Numerical Harmonic Analysis, Birkhäuser Boston, MA, 2007. +https://doi.org/10.1007/978-0-8176-4540-3 +[20] Schmidt, E., Akopian, D., & Pack, D. J., Development of a Real-Time +Software-Defined GPS Receiver in a LabVIEW-Based Instrumentation +Environment. IEEE Transactions on Instrumentation and Measurement, +Vol. 67(9), 2082-2096, 2018. +[21] Capuano, P., Lo Presti, L., & Lohan, E. S., Real-time GNSS signal +processing using software-defined radios: Challenges and solutions, +Sensors, 22(7), 2448, 2022. +[22] Rao, B. R., & Sathyanarayana, K., Implementation of GNSS SDR +receivers: Techniques and applications, IEEE Access, 11, 567-580, 2023. +[23] Konovaltsev, A., & Hein, G. W., Advances in GNSS software-defined +radios: From theory to practice, Journal of Satellite Communications and +Navigation, 10(2), 104-116, 2023. +[24] Great Scott Gadgets, HackRF One [Software Defined Radio]. +https://greatscottgadgets.com/hackrf, 2014. +[25] Osmocom, RTL-SDR [Software Defined Radio]. +https://osmocom.org/projects/rtl-sdr, 2012 +[26] Gold, R., Optimal binary sequences for spread spectrum multiplexing, +IEEE Transactions on Information Theory, 13(4), 619-621. +doi:10.1109/TIT.1967.1054010, 1967. +[27] Rohling, H., Radar CFAR thresholding in clutter and multiple target +situations. IEEE Transactions on Aerospace and Electronic Systems, AES- +19(4), 608-621. doi:10.1109/TAES.1983.309362, 1983. +[28] Gao, Z., Liu, F., Wen, Y., & Wang, X., An overview on target detection +techniques in CFAR processing for non-Gaussian interference environment. +IEEE Access, 6, 5630-5645. doi:10.1P109/ACCESS.2017.2778080, 2018. +[29] Castillo Delacroix L., Fagre M., Vaquila I., Cabrera M. A., PyGNSS- +SDR (versión 1.0) [repository], GitHub, 2024, +https://github.com/ltcfacet/PyGNSS-SDR + +ISSN 2525-0159 53 http://elektron.fi.uba.ar + diff --git a/GNSS_SDR_IQ/GNSS_SDR_IQ.m b/GNSS_SDR_IQ/GNSS_SDR_IQ.m new file mode 100644 index 0000000..b2f3777 --- /dev/null +++ b/GNSS_SDR_IQ/GNSS_SDR_IQ.m @@ -0,0 +1,96 @@ + +%--- Include folders with functions --------------------------------------- +addpath include % The software receiver functions +addpath geoFunctions % Position calculation related functions +%% Clean up the environment first ========================================= +%%clc; close all; % +format ('compact'); +format ('long', 'g');clc + +%--- Include folders with functions --------------------------------------- +addpath include +fprintf('-------------------------------\n\n'); +%% Initialize the setting +global settings; +settings = initSettings_IQ(); +disp(' '); +fprintf('Probing data (%s)...\n', settings.fileName) +[fid, message] = fopen(settings.fileName, 'rb'); + +%% Acquisition ============================================================ +fseek(fid, settings.skipNumberOfBytes, 'bof'); +tmp = fread(fid, 2*11*settings.samplesPerCode, settings.dataType)'; +%tmp=tmp-127.5; +data=tmp(1:2:end)+1i*tmp(2:2:end); +hist(tmp(1:2:end),20); grid on; +xlabel('Bin'); +ylabel('Number in Bin'); + +%--- Do the acquisition ------------------------------------------- +disp ('Acquiring satellites...'); +acqResults = acquisitionIQ(data, settings); +%acqResults = acquisitionIQ_weak(data, settings); +plotAcquisition(acqResults); +% acqResults.carrFreq(1)=-1e3; +% acqResults.codePhase(1)=1; +% acqResults.peakMetric(1)=2; + +% plotAcquisition(acqResults); + +settings.numberOfChannels = min([settings.numberOfChannels, sum(acqResults.peakMetric > settings.acqThreshold)]); +if (any(acqResults.peakMetric > settings.acqThreshold)) + channel = preRun(acqResults, settings); +% channel(1).PRN=1; +% channel(1).acquiredFreq=acqResults.carrFreq(1); +% channel(1).codePhase=acqResults.codePhase(1); +% channel(1).status='T'; +% settings.numberOfChannels=1; + showChannelStatus(channel, settings); +else + % No satellites to track, exit + disp('No GNSS signals detected, signal processing finished.'); + trackResults = []; + fclose(fid); + return; +end +%% Track the signal ======================================================= +startTime = now; +disp ([' Tracking started at ', datestr(startTime)]); +for cidx=1:size(channel,2) + if channel(cidx).acquiredFreq >settings.samplingFreq/2; + channel(cidx).acquiredFreq=channel(cidx).acquiredFreq-settings.samplingFreq; + end; +end; +showChannelStatus(channel, settings); +%channel.codePhase=3351 ; +[trackResults, channel] = tracking_V0_IQ(fid, channel, settings); + +% Close the data file +if fid>0 + fclose(fid); +end; + disp([' Tracking is over (elapsed time ', ... + datestr(now - startTime, 13), ')']) + + % Auto save the acquisition & tracking results to a file to allow + % running the positioning solution afterwards. + disp(' Saving Acq & Tracking results to file "trackingResults.mat"') + save('trackingResults', ... + 'trackResults', 'settings', 'acqResults', 'channel'); + +%% Calculate navigation solutions ========================================= + disp(' Calculating navigation solutions...'); + navSolutions = postNavigation0(trackResults, settings); + %navSolutions = postNavigationSnapshot(trackResults, settings); + + disp(' Processing is complete for this data block'); + +%% Plot all results =================================================== + disp (' Ploting results...'); + if settings.plotTracking + plotTracking(1:settings.numberOfChannels, trackResults, settings); + end + + plotNavigation(navSolutions, settings); + + disp('Post processing of the signal is over.'); diff --git a/GNSS_SDR_IQ/Snapshot.m b/GNSS_SDR_IQ/Snapshot.m new file mode 100644 index 0000000..b59e4f8 --- /dev/null +++ b/GNSS_SDR_IQ/Snapshot.m @@ -0,0 +1,52 @@ +%--- Include folders with functions --------------------------------------- +addpath include % The software receiver functions +addpath geoFunctions % Position calculation related functions +%% Clean up the environment first ========================================= +%%clc; close all; % +format ('compact'); +format ('long', 'g'); +load trackingResults +% plotTracking(1:settings.numberOfChannels, trackResults, settings) +sat2Remove=[]; +for sat=sat2Remove + trackResults(find([trackResults.PRN]==sat))=[]; + channel(find([trackResults.PRN]==sat))=[]; + +end +settings.numberOfChannels=settings.numberOfChannels-length(sat2Remove); +settings.navSolPeriod=100; +navSolutions = postNavigationSnapshot(trackResults, settings); +plotNavigation(navSolutions,settings); + +idx=3; +P=sqrt(trackResults(idx).I_P.^2+trackResults(idx).Q_P.^2); +E=sqrt(trackResults(idx).I_E.^2+trackResults(idx).Q_E.^2); +L=sqrt(trackResults(idx).I_L.^2+trackResults(idx).Q_L.^2); + +figure;hold on; +tt=(1:length(E))/1000; +h=plot(tt,E,'-*r'); +plot(tt,P,'-*b'); +plot(tt,L,'-*y'); +xlabel('seconds'); +grid on; +hLegend= legend('$\sqrt{I_{E}^2 + Q_{E}^2}$', ... + '$\sqrt{I_{P}^2 + Q_{P}^2}$', ... + '$\sqrt{I_{L}^2 + Q_{L}^2}$'); +set(hLegend, 'Interpreter', 'Latex'); + + refCoord.E = mean(navSolutions.E(~isnan(navSolutions.E))); + refCoord.N = mean(navSolutions.N(~isnan(navSolutions.N))); + refCoord.U = mean(navSolutions.U(~isnan(navSolutions.U))); +figure; + plot( [(navSolutions.E - refCoord.E)', ... + (navSolutions.N - refCoord.N)',... + (navSolutions.U - refCoord.U)']); + + title ( 'Coordinates variations in UTM system'); + legend('E', 'N', 'U'); + xlabel( ['Measurement period: ', ... + num2str(settings.navSolPeriod), 'ms']); + ylabel('Variations (m)'); + grid ; + axis ([0 240 -200 200]); \ No newline at end of file diff --git a/GNSS_SDR_IQ/acquisition.m b/GNSS_SDR_IQ/acquisition.m new file mode 100644 index 0000000..fe4c386 --- /dev/null +++ b/GNSS_SDR_IQ/acquisition.m @@ -0,0 +1,219 @@ +function acqResults = acquisition(longSignal, settings) +%Function performs cold start acquisition on the collected "data". It +%searches for GPS signals of all satellites, which are listed in field +%"acqSatelliteList" in the settings structure. Function saves code phase +%and frequency of the detected signals in the "acqResults" structure. +% +%acqResults = acquisition(longSignal, settings) +% +% Inputs: +% longSignal - 11 ms of raw signal from the front-end +% settings - Receiver settings. Provides information about +% sampling and intermediate frequencies and other +% parameters including the list of the satellites to +% be acquired. +% Outputs: +% acqResults - Function saves code phases and frequencies of the +% detected signals in the "acqResults" structure. The +% field "carrFreq" is set to 0 if the signal is not +% detected for the given PRN number. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis and Dennis M. Akos +% Written by Darius Plausinaitis and Dennis M. Akos +% Based on Peter Rinder and Nicolaj Bertelsen +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: acquisition.m,v 1.1.2.12 2006/08/14 12:08:03 dpl Exp $ + +%% Initialization ========================================================= + +% Find number of samples per spreading code +samplesPerCode = round(settings.samplingFreq / ... + (settings.codeFreqBasis / settings.codeLength)); + +% Create two 1msec vectors of data to correlate with and one with zero DC +signal1 = longSignal(1 : samplesPerCode); +signal2 = longSignal(samplesPerCode+1 : 2*samplesPerCode); + +signal0DC = longSignal - mean(longSignal); + +% Find sampling period +ts = 1 / settings.samplingFreq; + +% Find phase points of the local carrier wave +phasePoints = (0 : (samplesPerCode-1)) * 2 * pi * ts; + +% Number of the frequency bins for the given acquisition band (500Hz steps) +numberOfFrqBins = round(settings.acqSearchBand * 2) + 1; + +% Generate all C/A codes and sample them according to the sampling freq. +caCodesTable = makeCaTable(settings); + + +%--- Initialize arrays to speed up the code ------------------------------- +% Search results of all frequency bins and code shifts (for one satellite) +results = zeros(numberOfFrqBins, samplesPerCode); + +% Carrier frequencies of the frequency bins +frqBins = zeros(1, numberOfFrqBins); + + +%--- Initialize acqResults ------------------------------------------------ +% Carrier frequencies of detected signals +acqResults.carrFreq = zeros(1, 32); +% C/A code phases of detected signals +acqResults.codePhase = zeros(1, 32); +% Correlation peak ratios of the detected signals +acqResults.peakMetric = zeros(1, 32); + +fprintf('('); + +% Perform search for all listed PRN numbers ... +for PRN = settings.acqSatelliteList + +%% Correlate signals ====================================================== + %--- Perform DFT of C/A code ------------------------------------------ + caCodeFreqDom = conj(fft(caCodesTable(PRN, :))); + + %--- Make the correlation for whole frequency band (for all freq. bins) + for frqBinIndex = 1:numberOfFrqBins + + %--- Generate carrier wave frequency grid (0.5kHz step) ----------- + frqBins(frqBinIndex) = settings.IF - ... + (settings.acqSearchBand/2) * 1000 + ... + 0.5e3 * (frqBinIndex - 1); + + %--- Generate local sine and cosine ------------------------------- + sinCarr = sin(frqBins(frqBinIndex) * phasePoints); + cosCarr = cos(frqBins(frqBinIndex) * phasePoints); + + %--- "Remove carrier" from the signal ----------------------------- + I1 = sinCarr .* signal1; + Q1 = cosCarr .* signal1; + I2 = sinCarr .* signal2; + Q2 = cosCarr .* signal2; + + %--- Convert the baseband signal to frequency domain -------------- + IQfreqDom1 = fft(I1 + j*Q1); + IQfreqDom2 = fft(I2 + j*Q2); + + %--- Multiplication in the frequency domain (correlation in time + %domain) + convCodeIQ1 = IQfreqDom1 .* caCodeFreqDom; + convCodeIQ2 = IQfreqDom2 .* caCodeFreqDom; + + %--- Perform inverse DFT and store correlation results ------------ + acqRes1 = abs(ifft(convCodeIQ1)) .^ 2; + acqRes2 = abs(ifft(convCodeIQ2)) .^ 2; + + %--- Check which msec had the greater power and save that, will + %"blend" 1st and 2nd msec but will correct data bit issues + if (max(acqRes1) > max(acqRes2)) + results(frqBinIndex, :) = acqRes1; + else + results(frqBinIndex, :) = acqRes2; + end + + end % frqBinIndex = 1:numberOfFrqBins + +%% Look for correlation peaks in the results ============================== + % Find the highest peak and compare it to the second highest peak + % The second peak is chosen not closer than 1 chip to the highest peak + + %--- Find the correlation peak and the carrier frequency -------------- + [peakSize frequencyBinIndex] = max(max(results, [], 2)); + + %--- Find code phase of the same correlation peak --------------------- + [peakSize codePhase] = max(max(results)); + + %--- Find 1 chip wide C/A code phase exclude range around the peak ---- + samplesPerCodeChip = round(settings.samplingFreq / settings.codeFreqBasis); + excludeRangeIndex1 = codePhase - samplesPerCodeChip; + excludeRangeIndex2 = codePhase + samplesPerCodeChip; + + %--- Correct C/A code phase exclude range if the range includes array + %boundaries + if excludeRangeIndex1 < 1 + codePhaseRange = excludeRangeIndex2 : ... + (samplesPerCode + excludeRangeIndex1); + + elseif excludeRangeIndex2 >= samplesPerCode + codePhaseRange = (excludeRangeIndex2 - samplesPerCode) : ... + excludeRangeIndex1; + else + codePhaseRange = [1:excludeRangeIndex1, ... + excludeRangeIndex2 : samplesPerCode]; + end + + %--- Find the second highest correlation peak in the same freq. bin --- + secondPeakSize = max(results(frequencyBinIndex, codePhaseRange)); + + %--- Store result ----------------------------------------------------- + acqResults.peakMetric(PRN) = peakSize/secondPeakSize; + + % If the result is above threshold, then there is a signal ... + if (peakSize/secondPeakSize) > settings.acqThreshold + +%% Fine resolution frequency search ======================================= + + %--- Indicate PRN number of the detected signal ------------------- + fprintf('%02d ', PRN); + + %--- Generate 10msec long C/A codes sequence for given PRN -------- + caCode = generateCAcode(PRN); + + codeValueIndex = floor((ts * (1:10*samplesPerCode)) / ... + (1/settings.codeFreqBasis)); + + longCaCode = caCode((rem(codeValueIndex, 1023) + 1)); + + %--- Remove C/A code modulation from the original signal ---------- + % (Using detected C/A code phase) + xCarrier = ... + signal0DC(codePhase:(codePhase + 10*samplesPerCode-1)) ... + .* longCaCode; + + %--- Find the next highest power of two and increase by 8x -------- + fftNumPts = 8*(2^(nextpow2(length(xCarrier)))); + + %--- Compute the magnitude of the FFT, find maximum and the + %associated carrier frequency + fftxc = abs(fft(xCarrier, fftNumPts)); + + uniqFftPts = ceil((fftNumPts + 1) / 2); + [fftMax, fftMaxIndex] = max(fftxc(5 : uniqFftPts-5)); + fftFreqBins = (0 : uniqFftPts-1) * settings.samplingFreq/fftNumPts; + + + %--- Save properties of the detected satellite signal ------------- + acqResults.carrFreq(PRN) = fftFreqBins(fftMaxIndex); + acqResults.codePhase(PRN) = codePhase; + + else + %--- No signal with this PRN -------------------------------------- + fprintf('. '); + end % if (peakSize/secondPeakSize) > settings.acqThreshold + +end % for PRN = satelliteList + +%=== Acquisition is over ================================================== +fprintf(')\n'); diff --git a/GNSS_SDR_IQ/acquisitionIQ.m b/GNSS_SDR_IQ/acquisitionIQ.m new file mode 100644 index 0000000..8e136c2 --- /dev/null +++ b/GNSS_SDR_IQ/acquisitionIQ.m @@ -0,0 +1,268 @@ +function acqResults = acquisition(longSignal, settings) +%Function performs cold start acquisition on the collected "data". It +%searches for GPS signals of all satellites, which are listed in field +%"acqSatelliteList" in the settings structure. Function saves code phase +%and frequency of the detected signals in the "acqResults" structure. +% +%acqResults = acquisition(longSignal, settings) +% +% Inputs: +% longSignal - 11 ms of raw signal from the front-end +% settings - Receiver settings. Provides information about +% sampling and intermediate frequencies and other +% parameters including the list of the satellites to +% be acquired. +% Outputs: +% acqResults - Function saves code phases and frequencies of the +% detected signals in the "acqResults" structure. The +% field "carrFreq" is set to 0 if the signal is not +% detected for the given PRN number. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis and Dennis M. Akos +% Written by Darius Plausinaitis and Dennis M. Akos +% Based on Peter Rinder and Nicolaj Bertelsen +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: acquisition.m,v 1.1.2.12 2006/08/14 12:08:03 dpl Exp $ + +%% Initialization ========================================================= + +% Find number of samples per spreading code (số mẫu trong mỗi mã trải phổ) +samplesPerCode = round(settings.samplingFreq / ... + (settings.codeFreqBasis / settings.codeLength)); + +% Create two 1msec vectors of data to correlate with and one with zero DC +% (tạo ra 2 vector 1ms để tương quan và 1 vector có DC = 0) +signal1 = longSignal(1 : samplesPerCode); +signal2 = longSignal(samplesPerCode+1 : 2*samplesPerCode); +% Tạo ra 2 signal để + +signal0DC = longSignal - mean(longSignal); + +% Find sampling period: khoảng thời gian lấy mẫu +ts = 1 / settings.samplingFreq; + +% Find phase points of the local carrier wave: điểm pha của sóng mang cục +% bộ +phasePoints = (0 : (samplesPerCode-1)) * 2 * pi * ts; + +% Number of the frequency bins for the given acquisition band (500Hz steps) +% số lượng ngăn tần số cho băng tần acquisition +numberOfFrqBins = round(settings.acqSearchBand * 2) + 1; + +% Generate all C/A codes and sample them according to the sampling freq. +% Tạo các mã C/A và lấy mẫu theo tần số lấy mẫu +caCodesTable = makeCaTable(settings); + + +%--- Initialize arrays to speed up the code ------------------------------- +% Khởi tạo mảng tăng tốc mã +% Search results of all frequency bins and code shifts (for one satellite) +% Kết quả tìm kiếm của tất cả ngăn tần số và dịch chuyển mã +results = zeros(numberOfFrqBins, samplesPerCode); + +% Carrier frequencies of the frequency bins +% Tần số sóng mang của ngăn tần số +frqBins = zeros(1, numberOfFrqBins); + + +%--- Initialize acqResults ------------------------------------------------ +% Khởi tạo acqResults +% Carrier frequencies of detected signals: tần số sóng mang của tín hiệu +% được phát hiện +acqResults.carrFreq = zeros(1, 32); +% C/A code phases of detected signals: pha mã C/A của tín hiệu được phát +% hiện +acqResults.codePhase = zeros(1, 32); +% Correlation peak ratios of the detected signals: tỉ lệ cực đại tương quan +% của các tín hiệu được phát hiện +acqResults.peakMetric = zeros(1, 32); + +fprintf('('); + +% Perform search for all listed PRN numbers ... : thực hiện tìm kiếm các +% PRN được liệt kê +for PRN = settings.acqSatelliteList + +%% Correlate signals ====================================================== + %--- Perform DFT of C/A code ------------------------------------------ + caCodeFreqDom = conj(fft(caCodesTable(PRN, :))); + + %--- Make the correlation for whole frequency band (for all freq. bins) + % Tạo mối tương quan cho toàn bộ dải tần + for frqBinIndex = 1:numberOfFrqBins + + %--- Generate carrier wave frequency grid (0.5kHz step) ----------- + % Tạo lưới tần số sóng mang (0.5 kHz) + frqBins(frqBinIndex) = settings.IF - ... + (settings.acqSearchBand/2) * 1000 + ... + 0.5e3 * (frqBinIndex - 1); + + %--- c ------------------------------- + % Tạo sin và cos cục bộ + sinCarr = sin(frqBins(frqBinIndex) * phasePoints); + cosCarr = cos(frqBins(frqBinIndex) * phasePoints); + + %--- "Remove carrier" from the signal ----------------------------- + % Loại bỏ sóng mang + IQ1 = (sinCarr+1i*cosCarr).* signal1; + I1 = real(IQ1); + Q1 = imag(IQ1); + + IQ2 = (sinCarr+1i*cosCarr).* signal2; + I2 = real(IQ2); + Q2 = imag(IQ2); + + %--- Convert the baseband signal to frequency domain -------------- + % Chuyển đổi tín hiệu băng cơ sở sang miền tần số + IQfreqDom1 = fft(I1 + 1i*Q1); + IQfreqDom2 = fft(I2 + 1i*Q2); + + %--- Multiplication in the frequency domain (correlation in time + %domain) + % Phép nhân trong miền tần số (tương quan trong miền thời gian) + convCodeIQ1 = IQfreqDom1 .* caCodeFreqDom; + convCodeIQ2 = IQfreqDom2 .* caCodeFreqDom; + + %--- Perform inverse DFT and store correlation results ------------ + % Thực hiện DFT nghịch đảo và lưu kết quả tương quan + acqRes1 = abs(ifft(convCodeIQ1)) .^ 2; + acqRes2 = abs(ifft(convCodeIQ2)) .^ 2; + + %--- Check which msec had the greater power and save that, will + %"blend" 1st and 2nd msec but will correct data bit issues + % Kiểm tra ms nào mạnh hơn và lưu lại, trộn ms thứ 1 và 2 nhưng sẽ + % khắc phục vấn đề về bit dữ liệu + if (max(acqRes1) > max(acqRes2)) + results(frqBinIndex, :) = acqRes1; %Lưu acqRes1 vào hàng thứ frqBinIndex + else + results(frqBinIndex, :) = acqRes2; + end + + end % frqBinIndex = 1:numberOfFrqBins + +%% Look for correlation peaks in the results ============================== + % Find the highest peak and compare it to the second highest peak + % Tìm đỉnh cao nhất và so sánh với đỉnh cao thứ 2 + % The second peak is chosen not closer than 1 chip to the highest peak + % Đỉnh thứ 2 được chọn không gần hơn 1 chip so với đỉnh cao nhất + + %--- Find the correlation peak and the carrier frequency -------------- + % Tìm đỉnh tương quan và tần số sóng mang + [peakSize frequencyBinIndex] = max(max(results, [], 2)); %Giá trị lớn nhất trên mỗi hàng + + %--- Find code phase of the same correlation peak --------------------- + % Tìm pha mã của cùng 1 đỉnh tương quan + [peakSize codePhase] = max(max(results)); + + %--- Find 1 chip wide C/A code phase exclude range around the peak ---- + % Tìm 1 chip mã C/A loại trừ khoảng xung quanh đỉnh + samplesPerCodeChip = round(settings.samplingFreq / settings.codeFreqBasis); + excludeRangeIndex1 = codePhase - samplesPerCodeChip; + excludeRangeIndex2 = codePhase + samplesPerCodeChip; + + %--- Correct C/A code phase exclude range if the range includes array + %boundaries + % Đúng vị trí mã C/A và loại bỏ khoảng nếu khoảng đó bao gồm các biên + % của mảng + if excludeRangeIndex1 < 2 + codePhaseRange = excludeRangeIndex2 : ... + samplesPerCode;%(samplesPerCode + excludeRangeIndex1); + + elseif excludeRangeIndex2 >= samplesPerCode + codePhaseRange = (excludeRangeIndex2 - samplesPerCode) : ... + excludeRangeIndex1; + else + codePhaseRange = [1:excludeRangeIndex1, ... + excludeRangeIndex2 : samplesPerCode]; + end +try + %--- Find the second highest correlation peak in the same freq. bin --- + % Tìm đỉnh tương quan cao thứ 2 trong cùng freq. bin + secondPeakSize = max(results(frequencyBinIndex, codePhaseRange)); +catch exception + msgbox(exception.message); +end; + %--- Store result ----------------------------------------------------- + acqResults.peakMetric(PRN) = peakSize/secondPeakSize; + + % If the result is above threshold, then there is a signal ... + % Nếu kết quả vượt ngưỡng thì có tín hiệu + if (peakSize/secondPeakSize) > settings.acqThreshold + + freq_Range=1:numberOfFrqBins; + xR=settings.IF - (settings.acqSearchBand/2) * 1000 + 0.5e3 * (freq_Range - 1); + yR=[0:samplesPerCode-1]; + figure; + surf(results);shading interp, title("PRN "+PRN); + +%% Fine resolution frequency search ======================================= + + %--- Indicate PRN number of the detected signal ------------------- + % Cho biết số PRN của tín hiệu được phát hiện + fprintf('%02d ', PRN); + + %--- Generate 10msec long C/A codes sequence for given PRN -------- + % Tạo chuỗi mã C/A dài 10 ms cho PRN đã cho + caCode = generateCAcode(PRN); + + codeValueIndex = floor((ts * (1:10*samplesPerCode)) / ... + (1/settings.codeFreqBasis)); + + longCaCode = caCode((rem(codeValueIndex, 1023) + 1)); + + %--- Remove C/A code modulation from the original signal ---------- + % Loại bỏ điều chế mã C/A khỏi tín hiệu gốc + % (Using detected C/A code phase) (Sử dụng pha mã C/A được phát + % hiện) + xCarrier = ... + signal0DC(codePhase:(codePhase + 10*samplesPerCode-1)) ... + .* longCaCode; + + %--- Find the next highest power of two and increase by 8x -------- + fftNumPts = 8*(2^(nextpow2(length(xCarrier)))); + + %--- Compute the magnitude of the FFT, find maximum and the + %associated carrier frequency + % Tính toán độ lớn của FFT, tìm tần số sóng mang tối đa và liên + % quan + fftxc = abs(fft(xCarrier, fftNumPts)); + + uniqFftPts = ceil((fftNumPts + 1) / 2); + [fftMax, fftMaxIndex] = max(fftxc); + fftFreqBins = (0 : fftNumPts) * settings.samplingFreq/fftNumPts; + + + %--- Save properties of the detected satellite signal ------------- + % Lưu thuộc tính tín hiệu vệ tinh được phát hiện + acqResults.carrFreq(PRN) = fftFreqBins(fftMaxIndex); + acqResults.codePhase(PRN) = codePhase; + + else + %--- No signal with this PRN -------------------------------------- + fprintf('. '); + end % if (peakSize/secondPeakSize) > settings.acqThreshold + +end % for PRN = satelliteList + +%=== Acquisition is over ================================================== +fprintf(')\n'); \ No newline at end of file diff --git a/GNSS_SDR_IQ/acquisitionIQ_weak.m b/GNSS_SDR_IQ/acquisitionIQ_weak.m new file mode 100644 index 0000000..73afa21 --- /dev/null +++ b/GNSS_SDR_IQ/acquisitionIQ_weak.m @@ -0,0 +1,227 @@ +function acqResults = acquisitionIQ_weak(longSignal, settings) +%Function performs cold start acquisition on the collected "data". It +%searches for GPS signals of all satellites, which are listed in field +%"acqSatelliteList" in the settings structure. Function saves code phase +%and frequency of the detected signals in the "acqResults" structure. +% +%acqResults = acquisition(longSignal, settings) +% +% Inputs: +% longSignal - 11 ms of raw signal from the front-end +% settings - Receiver settings. Provides information about +% sampling and intermediate frequencies and other +% parameters including the list of the satellites to +% be acquired. +% Outputs: +% acqResults - Function saves code phases and frequencies of the +% detected signals in the "acqResults" structure. The +% field "carrFreq" is set to 0 if the signal is not +% detected for the given PRN number. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis and Dennis M. Akos +% Written by Darius Plausinaitis and Dennis M. Akos +% Based on Peter Rinder and Nicolaj Bertelsen +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: acquisition.m,v 1.1.2.12 2006/08/14 12:08:03 dpl Exp $ + +%% Initialization ========================================================= + +% Find number of samples per spreading code +samplesPerCode = round(settings.samplingFreq / ... + (settings.codeFreqBasis / settings.codeLength)); + +% Create two 1msec vectors of data to correlate with and one with zero DC +signal0DC = longSignal - mean(longSignal); + +% Find sampling period +ts = 1 / settings.samplingFreq; + +% Find phase points of the local carrier wave +phasePoints = (0 : (samplesPerCode-1)) * 2 * pi * ts; + +% Number of the frequency bins for the given acquisition band (500Hz steps) +DopplerStepSize=100; +numberOfFrqBins = round(settings.acqSearchBand *1000/DopplerStepSize) + 1; + +% Generate all C/A codes and sample them according to the sampling freq. +caCodesTable = makeCaTable(settings); + + +%--- Initialize arrays to speed up the code ------------------------------- +% Search results of all frequency bins and code shifts (for one satellite) +results = zeros(numberOfFrqBins, samplesPerCode); +tmpResults = zeros(numberOfFrqBins, samplesPerCode); +% Carrier frequencies of the frequency bins +frqBins = zeros(1, numberOfFrqBins); + + +%--- Initialize acqResults ------------------------------------------------ +% Carrier frequencies of detected signals +acqResults.carrFreq = zeros(1, 32); +% C/A code phases of detected signals +acqResults.codePhase = zeros(1, 32); +% Correlation peak ratios of the detected signals +acqResults.peakMetric = zeros(1, 32); + +fprintf('('); + +% Perform search for all listed PRN numbers ... +for PRN = settings.acqSatelliteList + results = zeros(numberOfFrqBins, samplesPerCode); + for msIdx=0:9 + signal = longSignal(msIdx*samplesPerCode+1 : (msIdx+1)*samplesPerCode); + %% Correlate signals ====================================================== + %--- Perform DFT of C/A code ------------------------------------------ + caCodeFreqDom = conj(fft(caCodesTable(PRN, :))); + + %--- Make the correlation for whole frequency band (for all freq. bins) + for frqBinIndex = 1:numberOfFrqBins + + %--- Generate carrier wave frequency grid (0.5kHz step) ----------- + frqBins(frqBinIndex) = settings.IF - ... + (settings.acqSearchBand/2) * 1000 + ... + DopplerStepSize * (frqBinIndex - 1); + + %--- Generate local sine and cosine ------------------------------- + sinCarr = sin(frqBins(frqBinIndex) * phasePoints); + cosCarr = cos(frqBins(frqBinIndex) * phasePoints); + + %--- "Remove carrier" from the signal ----------------------------- + IQ = (sinCarr+1i*cosCarr).* signal; + I = real(IQ); + Q = imag(IQ); + + %--- Convert the baseband signal to frequency domain -------------- + IQfreqDom = fft(I + 1i*Q); + + + %--- Multiplication in the frequency domain (correlation in time + %domain) + convCodeIQ = IQfreqDom .* caCodeFreqDom; + + + %--- Perform inverse DFT and store correlation results ------------ + acqRes = abs(ifft(convCodeIQ)) .^ 2; + + %--- Check which msec had the greater power and save that, will + %"blend" 1st and 2nd msec but will correct data bit issues + + tmpResults(frqBinIndex, :) = acqRes; + end % frqBinIndex = 1:numberOfFrqBins + results=results+tmpResults; + end; + +%% Look for correlation peaks in the results ============================== + % Find the highest peak and compare it to the second highest peak + % The second peak is chosen not closer than 1 chip to the highest peak + if settings.plotAcquisition + surf(results); + end; + %--- Find the correlation peak and the carrier frequency -------------- + [peakSize frequencyBinIndex] = max(max(results, [], 2)); + + %--- Find code phase of the same correlation peak --------------------- + [peakSize codePhase] = max(max(results)); + + %--- Find 1 chip wide C/A code phase exclude range around the peak ---- + samplesPerCodeChip = round(settings.samplingFreq / settings.codeFreqBasis); + excludeRangeIndex1 = codePhase - samplesPerCodeChip; + excludeRangeIndex2 = codePhase + samplesPerCodeChip; + + %--- Correct C/A code phase exclude range if the range includes array + %boundaries + if excludeRangeIndex1 < 2 + codePhaseRange = excludeRangeIndex2 : ... + samplesPerCode;%(samplesPerCode + excludeRangeIndex1); + + elseif excludeRangeIndex2 >= samplesPerCode + codePhaseRange = (excludeRangeIndex2 - samplesPerCode) : ... + excludeRangeIndex1; + else + codePhaseRange = [1:excludeRangeIndex1, ... + excludeRangeIndex2 : samplesPerCode]; + end +try + %--- Find the second highest correlation peak in the same freq. bin --- + secondPeakSize = max(results(frequencyBinIndex, codePhaseRange)); +catch exception + msgbox(exception.message); +end; + %--- Store result ----------------------------------------------------- + acqResults.peakMetric(PRN) = peakSize/secondPeakSize; + + % If the result is above threshold, then there is a signal ... + if (peakSize/secondPeakSize) > settings.acqThreshold + figure; + surf(results); +%% Fine resolution frequency search ======================================= + + %--- Indicate PRN number of the detected signal ------------------- + fprintf('%02d ', PRN); + + %--- Generate 10msec long C/A codes sequence for given PRN -------- + caCode = generateCAcode(PRN); + + codeValueIndex = floor((ts * (1:10*samplesPerCode)) / ... + (1/settings.codeFreqBasis)); + + longCaCode = caCode((rem(codeValueIndex, 1023) + 1)); + + %--- Remove C/A code modulation from the original signal ---------- + % (Using detected C/A code phase) + xCarrier = ... + signal0DC(codePhase:(codePhase + 10*samplesPerCode-1)) ... + .* longCaCode; + + %--- Find the next highest power of two and increase by 8x -------- + fftNumPts = 8*(2^(nextpow2(length(xCarrier)))); + + %--- Compute the magnitude of the FFT, find maximum and the + %associated carrier frequency + fftxc = abs(fft(xCarrier, fftNumPts)); + + uniqFftPts = ceil((fftNumPts + 1) / 2); + [fftMax, fftMaxIndex] = max(fftxc); + fftFreqBins = (0 : fftNumPts) * settings.samplingFreq/fftNumPts; + + + %--- Save properties of the detected satellite signal ------------- + acqResults.carrFreq(PRN) = fftFreqBins(fftMaxIndex); + + acqResults.codePhase(PRN) = codePhase; + +% THUAN + acqResults.carrFreq(PRN) =settings.IF- ... + (settings.acqSearchBand/2) * 1000 + ... + DopplerStepSize * (frequencyBinIndex - 1); + + + else + %--- No signal with this PRN -------------------------------------- + fprintf('. '); + end % if (peakSize/secondPeakSize) > settings.acqThreshold + +end % for PRN = satelliteList + +%=== Acquisition is over ================================================== +fprintf(')\n'); \ No newline at end of file diff --git a/GNSS_SDR_IQ/alm2eph.m b/GNSS_SDR_IQ/alm2eph.m new file mode 100644 index 0000000..e7fbbc5 --- /dev/null +++ b/GNSS_SDR_IQ/alm2eph.m @@ -0,0 +1,31 @@ +function [eph]=alm2eph(ID,Health,Eccentricity,TimeOfApp,Inclination,RateOfRightAscen,sqrtA,RightAscen,ArgOfPerigee,MeanAnom,Af0,Af1,week) +if nargin<13 + error('Not enough input'); +end; +eph.IODE_sf2=0; +eph.IODE_sf3=0; +eph.iDot=0; +eph.C_rs=0; +eph.deltan=0; +eph.M_0=MeanAnom; +eph.e=Eccentricity; +eph.sqrtA=sqrtA; +eph.omega_0=RightAscen; +eph.i_0=Inclination; +eph.omega=ArgOfPerigee; +eph.omegaDot=RateOfRightAscen; +eph.weekNumber=1636; +eph.accuracy=1; +eph.T_GD=0; +eph.IODC=512; +eph.t_oc=475200; +eph.a_f2=0; +eph.a_f1=Af1; +eph.a_f0=Af0; +eph.C_uc=0; +eph.C_us=0; +eph.C_ic=0; +eph.C_is=0; +eph.C_rc=0; +eph.C_rs=0; +eph.t_oe=eph.t_oc; \ No newline at end of file diff --git a/GNSS_SDR_IQ/calculatePseudoranges.m b/GNSS_SDR_IQ/calculatePseudoranges.m new file mode 100644 index 0000000..1bcc855 --- /dev/null +++ b/GNSS_SDR_IQ/calculatePseudoranges.m @@ -0,0 +1,77 @@ +function [pseudoranges] = calculatePseudoranges(trackResults, ... + msOfTheSignal, ... + channelList, settings) +%calculatePseudoranges finds relative pseudoranges for all satellites +%listed in CHANNELLIST at the specified millisecond of the processed +%signal. The pseudoranges contain unknown receiver clock offset. It can be +%found by the least squares position search procedure. +% +%[pseudoranges] = calculatePseudoranges(trackResults, msOfTheSignal, ... +% channelList, settings) +% +% Inputs: +% trackResults - output from the tracking function +% msOfTheSignal - pseudorange measurement point (millisecond) in +% the trackResults structure +% channelList - list of channels to be processed +% settings - receiver settings +% +% Outputs: +% pseudoranges - relative pseudoranges to the satellites. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +% Based on Peter Rinder and Nicolaj Bertelsen +%-------------------------------------------------------------------------- +% +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% CVS record: +% $Id: calculatePseudoranges.m,v 1.1.2.18 2006/08/09 17:20:11 dpl Exp $ + +%--- Set initial travel time to infinity ---------------------------------- +% Later in the code a shortest pseudorange will be selected. Therefore +% pseudoranges from non-tracking channels must be the longest - e.g. +% infinite. +travelTime = inf(1, settings.numberOfChannels); + +% Find number of samples per spreading code +samplesPerCode = round(settings.samplingFreq / ... + (settings.codeFreqBasis / settings.codeLength)); + +%--- For all channels in the list ... +if(size(channelList,1)>1) + channelList=channelList'; +end; +for channelNr = channelList + + %--- Compute the travel times ----------------------------------------- + travelTime(channelNr) = ... + trackResults(channelNr).absoluteSample(msOfTheSignal(channelNr)) / samplesPerCode; +end + +%--- Truncate the travelTime and compute pseudoranges --------------------- +minimum = floor(min(travelTime)); +travelTime = travelTime - minimum + settings.startOffset; + +%--- Convert travel time to a distance ------------------------------------ +% The speed of light must be converted from meters per second to meters +% per millisecond. +pseudoranges = travelTime * (settings.c / 1000); diff --git a/GNSS_SDR_IQ/ecef2llh.m b/GNSS_SDR_IQ/ecef2llh.m new file mode 100644 index 0000000..36ef769 --- /dev/null +++ b/GNSS_SDR_IQ/ecef2llh.m @@ -0,0 +1,50 @@ +% +% llh = ecef2llh (ecef) +% +% ecef2llh: converts ECEF to Lat, Lon, Height in WGS-84 +% +% Input: ecef [3xN] = x,y,z (user units) +% Output: llh [3xN] = lat,long,height location (rad,rad,user units) +% +% EE6900 - Flight Management Systems +% +% Maarten Uijt de Haag +% +% Based on: OU-ECE-AEC Oct. 1988 FvG +% +function llh = ecef2llh (ecef) + + A = 6378137.0; + E = 8.1819190842622e-02; + ESQ = E*E; + + llh = 0.0*ecef; + + X = ecef(1,:); + Y = ecef(2,:); + Z = ecef(3,:); + + RSQ = (X.*X) + (Y.*Y); + H = ESQ .* Z; + + %cnt = 0; + %done = 0; + %while cnt < 10 & done == 0, + for cnt = 1:10, + %cnt = cnt + 1; + ZP = Z + H; + R = sqrt(RSQ + (ZP .* ZP)); + SP = ZP ./ R; + GSQ = 1.0 - (ESQ .* SP .* SP); + EN = A ./ sqrt(GSQ); + P = EN .* ESQ .* SP; + %if abs(H-P) < 5.0e-04, done = 1; end; + H = P; + end; + P = atan2( ZP, sqrt(RSQ) ); + H = R - EN; + llh(1,:) = P; + llh(2,:) = atan2(Y,X); + llh(3,:) = H; + +return diff --git a/GNSS_SDR_IQ/fig3-3.eps b/GNSS_SDR_IQ/fig3-3.eps new file mode 100644 index 0000000..5cfdcf9 --- /dev/null +++ b/GNSS_SDR_IQ/fig3-3.eps @@ -0,0 +1,730 @@ +%!PS-Adobe-2.0 EPSF-1.2 +%%Creator: MATLAB, The Mathworks, Inc. Version 7.8.0.347 (R2009a). Operating System: Microsoft Windows Vista. +%%Title: .\fig3-3.eps +%%CreationDate: 10/05/2010 11:56:28 +%%DocumentNeededFonts: Helvetica +%%DocumentProcessColors: Cyan Magenta Yellow Black +%%Pages: 1 +%%BoundingBox: 26 184 545 591 +%%EndComments + +%%BeginProlog +% MathWorks dictionary +/MathWorks 160 dict begin +% definition operators +/bdef {bind def} bind def +/ldef {load def} bind def +/xdef {exch def} bdef +/xstore {exch store} bdef +% operator abbreviations +/c /clip ldef +/cc /concat ldef +/cp /closepath ldef +/gr /grestore ldef +/gs /gsave ldef +/mt /moveto ldef +/np /newpath ldef +/cm /currentmatrix ldef +/sm /setmatrix ldef +/rm /rmoveto ldef +/rl /rlineto ldef +/s {show newpath} bdef +/sc {setcmykcolor} bdef +/sr /setrgbcolor ldef +/sg /setgray ldef +/w /setlinewidth ldef +/j /setlinejoin ldef +/cap /setlinecap ldef +/rc {rectclip} bdef +/rf {rectfill} bdef +% page state control +/pgsv () def +/bpage {/pgsv save def} bdef +/epage {pgsv restore} bdef +/bplot /gsave ldef +/eplot {stroke grestore} bdef +% orientation switch +/portraitMode 0 def /landscapeMode 1 def /rotateMode 2 def +% coordinate system mappings +/dpi2point 0 def +% font control +/FontSize 0 def +/FMS {/FontSize xstore findfont [FontSize 0 0 FontSize neg 0 0] + makefont setfont} bdef +/ISOLatin1Encoding where {pop /WindowsLatin1Encoding 256 array bdef +ISOLatin1Encoding WindowsLatin1Encoding copy pop +/.notdef/.notdef/quotesinglbase/florin/quotedblbase/ellipsis/dagger +/daggerdbl/circumflex/perthousand/Scaron/guilsinglleft/OE/.notdef/.notdef +/.notdef/.notdef/quoteleft/quoteright/quotedblleft/quotedblright/bullet +/endash/emdash/tilde/trademark/scaron/guilsinglright/oe/.notdef/.notdef +/Ydieresis WindowsLatin1Encoding 128 32 getinterval astore pop} +{/WindowsLatin1Encoding StandardEncoding bdef} ifelse +/reencode {exch dup where {pop load} {pop StandardEncoding} ifelse + exch dup 3 1 roll findfont dup length dict begin + { 1 index /FID ne {def}{pop pop} ifelse } forall + /Encoding exch def currentdict end definefont pop} bdef +/isroman {findfont /CharStrings get /Agrave known} bdef +/FMSR {3 1 roll 1 index dup isroman {reencode} {pop pop} ifelse + exch FMS} bdef +/csm {1 dpi2point div -1 dpi2point div scale neg translate + dup landscapeMode eq {pop -90 rotate} + {rotateMode eq {90 rotate} if} ifelse} bdef +% line types: solid, dotted, dashed, dotdash +/SO { [] 0 setdash } bdef +/DO { [.5 dpi2point mul 4 dpi2point mul] 0 setdash } bdef +/DA { [6 dpi2point mul] 0 setdash } bdef +/DD { [.5 dpi2point mul 4 dpi2point mul 6 dpi2point mul 4 + dpi2point mul] 0 setdash } bdef +% macros for lines and objects +/L {lineto stroke} bdef +/MP {3 1 roll moveto 1 sub {rlineto} repeat} bdef +/AP {{rlineto} repeat} bdef +/PDlw -1 def +/W {/PDlw currentlinewidth def setlinewidth} def +/PP {closepath eofill} bdef +/DP {closepath stroke} bdef +/MR {4 -2 roll moveto dup 0 exch rlineto exch 0 rlineto + neg 0 exch rlineto closepath} bdef +/FR {MR stroke} bdef +/PR {MR fill} bdef +/L1i {{currentfile picstr readhexstring pop} image} bdef +/tMatrix matrix def +/MakeOval {newpath tMatrix currentmatrix pop translate scale +0 0 1 0 360 arc tMatrix setmatrix} bdef +/FO {MakeOval stroke} bdef +/PO {MakeOval fill} bdef +/PD {currentlinewidth 2 div 0 360 arc fill + PDlw -1 eq not {PDlw w /PDlw -1 def} if} def +/FA {newpath tMatrix currentmatrix pop translate scale + 0 0 1 5 -2 roll arc tMatrix setmatrix stroke} bdef +/PA {newpath tMatrix currentmatrix pop translate 0 0 moveto scale + 0 0 1 5 -2 roll arc closepath tMatrix setmatrix fill} bdef +/FAn {newpath tMatrix currentmatrix pop translate scale + 0 0 1 5 -2 roll arcn tMatrix setmatrix stroke} bdef +/PAn {newpath tMatrix currentmatrix pop translate 0 0 moveto scale + 0 0 1 5 -2 roll arcn closepath tMatrix setmatrix fill} bdef +/vradius 0 def /hradius 0 def /lry 0 def +/lrx 0 def /uly 0 def /ulx 0 def /rad 0 def +/MRR {/vradius xdef /hradius xdef /lry xdef /lrx xdef /uly xdef + /ulx xdef newpath tMatrix currentmatrix pop ulx hradius add uly + vradius add translate hradius vradius scale 0 0 1 180 270 arc + tMatrix setmatrix lrx hradius sub uly vradius add translate + hradius vradius scale 0 0 1 270 360 arc tMatrix setmatrix + lrx hradius sub lry vradius sub translate hradius vradius scale + 0 0 1 0 90 arc tMatrix setmatrix ulx hradius add lry vradius sub + translate hradius vradius scale 0 0 1 90 180 arc tMatrix setmatrix + closepath} bdef +/FRR {MRR stroke } bdef +/PRR {MRR fill } bdef +/MlrRR {/lry xdef /lrx xdef /uly xdef /ulx xdef /rad lry uly sub 2 div def + newpath tMatrix currentmatrix pop ulx rad add uly rad add translate + rad rad scale 0 0 1 90 270 arc tMatrix setmatrix lrx rad sub lry rad + sub translate rad rad scale 0 0 1 270 90 arc tMatrix setmatrix + closepath} bdef +/FlrRR {MlrRR stroke } bdef +/PlrRR {MlrRR fill } bdef +/MtbRR {/lry xdef /lrx xdef /uly xdef /ulx xdef /rad lrx ulx sub 2 div def + newpath tMatrix currentmatrix pop ulx rad add uly rad add translate + rad rad scale 0 0 1 180 360 arc tMatrix setmatrix lrx rad sub lry rad + sub translate rad rad scale 0 0 1 0 180 arc tMatrix setmatrix + closepath} bdef +/FtbRR {MtbRR stroke } bdef +/PtbRR {MtbRR fill } bdef +/stri 6 array def /dtri 6 array def +/smat 6 array def /dmat 6 array def +/tmat1 6 array def /tmat2 6 array def /dif 3 array def +/asub {/ind2 exch def /ind1 exch def dup dup + ind1 get exch ind2 get sub exch } bdef +/tri_to_matrix { + 2 0 asub 3 1 asub 4 0 asub 5 1 asub + dup 0 get exch 1 get 7 -1 roll astore } bdef +/compute_transform { + dmat dtri tri_to_matrix tmat1 invertmatrix + smat stri tri_to_matrix tmat2 concatmatrix } bdef +/ds {stri astore pop} bdef +/dt {dtri astore pop} bdef +/db {2 copy /cols xdef /rows xdef mul dup string + currentfile exch readhexstring pop + /bmap xdef pop pop} bdef +/it {gs np dtri aload pop moveto lineto lineto cp c + cols rows 8 compute_transform + {bmap} image gr}bdef +/il {newpath moveto lineto stroke}bdef +currentdict end def +%%EndProlog + +%%BeginSetup +MathWorks begin + +0 cap + +end +%%EndSetup + +%%Page: 1 1 +%%BeginPageSetup +%%PageBoundingBox: 26 184 545 591 +MathWorks begin +bpage +%%EndPageSetup + +%%BeginObject: obj1 +bplot + +/dpi2point 12 def +portraitMode 0204 7344 csm + + 109 247 6236 4889 MR c np +85 dict begin %Colortable dictionary +/c0 { 0.000000 0.000000 0.000000 sr} bdef +/c1 { 1.000000 1.000000 1.000000 sr} bdef +/c2 { 0.900000 0.000000 0.000000 sr} bdef +/c3 { 0.000000 0.820000 0.000000 sr} bdef +/c4 { 0.000000 0.000000 0.800000 sr} bdef +/c5 { 0.910000 0.820000 0.320000 sr} bdef +/c6 { 1.000000 0.260000 0.820000 sr} bdef +/c7 { 0.000000 0.820000 0.820000 sr} bdef +c0 +1 j +1 sg + 0 0 6913 5185 PR +6 w +0 4225 5356 0 0 -4225 899 4614 4 MP +PP +-5356 0 0 4225 5356 0 0 -4225 899 4614 5 MP stroke +4 w +DO +SO +6 w +0 sg + 899 4614 mt 6255 4614 L + 899 4614 mt 899 389 L +1345 4614 mt 1345 4560 L +%%IncludeResource: font Helvetica +/Helvetica /WindowsLatin1Encoding 216 FMSR + +1159 4849 mt +(-1) s +2461 4614 mt 2461 4560 L +2185 4849 mt +(-0.5) s +3577 4614 mt 3577 4560 L +3517 4849 mt +(0) s +4692 4614 mt 4692 4560 L +4542 4849 mt +(0.5) s +5808 4614 mt 5808 4560 L +5748 4849 mt +(1) s + 899 4402 mt 952 4402 L + 438 4482 mt +(-0.8) s + 899 3980 mt 952 3980 L + 438 4060 mt +(-0.6) s + 899 3557 mt 952 3557 L + 438 3637 mt +(-0.4) s + 899 3135 mt 952 3135 L + 438 3215 mt +(-0.2) s + 899 2712 mt 952 2712 L + 744 2792 mt +(0) s + 899 2290 mt 952 2290 L + 564 2370 mt +(0.2) s + 899 1867 mt 952 1867 L + 564 1947 mt +(0.4) s + 899 1445 mt 952 1445 L + 564 1525 mt +(0.6) s + 899 1022 mt 952 1022 L + 564 1102 mt +(0.8) s + 899 600 mt 952 600 L + 744 680 mt +(1) s +gs 899 389 5357 4226 MR c np +12 w +4 -4 5 -5 4 -4 5 -4 4 -4 5 -5 4 -4 5 -4 +4 -4 5 -5 4 -4 4 -4 5 -4 4 -4 5 -5 4 -4 +5 -4 4 -4 5 -5 4 -4 5 -4 4 -4 5 -5 4 -4 +5 -4 4 -4 4 -4 5 -5 4 -4 5 -4 4 -4 5 -5 +4 -4 5 -4 4 -4 5 -5 4 -4 5 -4 4 -4 5 -4 +4 -5 4 -4 5 -4 4 -4 5 -5 4 -4 5 -4 4 -4 +5 -4 4 -5 5 -4 4 -4 5 -4 4 -5 5 -4 4 -4 +4 -4 5 -5 4 -4 5 -4 4 -4 5 -4 4 -5 5 -4 +4 -4 5 -4 4 -5 5 -4 4 -4 5 -4 4 -5 5 -4 +4 -4 4 -4 5 -4 4 -5 5 -4 4 -4 5 -4 4 -5 +5 -4 4 -4 5 -4 4 -5 5 -4 4 -4 5 -4 4 -4 +4 -5 5 -4 4 -4 5 -4 4 -5 5 -4 4 -4 5 -4 +4 -4 5 -5 4 -4 5366 3131 100 MP stroke +5 -4 4 -4 5 -5 4 -4 4 -4 5 -4 4 -5 5 -4 +4 -4 5 -4 4 -4 5 -5 4 -4 5 -4 4 -4 5 -5 +4 -4 5 -4 4 -4 5 -5 4 -4 4 -4 5 -4 4 -4 +5 -5 4 -4 5 -4 4 -4 5 -5 4 -4 5 -4 4 -4 +5 -5 4 -4 5 -4 4 -4 4 -4 5 -5 4 -4 5 -4 +4 -4 5 -5 4 -4 5 -4 4 -4 5 -4 4 -5 5 -4 +4 -4 5 -4 4 -5 4 -4 5 -4 4 -4 5 -5 4 -4 +5 -4 4 -4 5 -4 4 -5 5 -4 4 -4 5 -4 4 -5 +5 -4 4 -4 4 -4 5 -5 4 -4 5 -4 4 -4 5 -4 +4 -5 5 -4 4 -4 5 -4 4 -5 5 -4 4 -4 5 -4 +4 -5 5 -4 4 -4 4 -4 5 -4 4 -5 5 -4 4 -4 +5 -4 4 -5 5 -4 4 -4 5 -4 4 -4 5 -5 4 -4 +5 -4 4 -4 4 -5 4924 3550 100 MP stroke +5 -4 4 -4 5 -4 4 -5 5 -4 4 -4 5 -4 4 -4 +5 -5 4 -4 5 -4 4 -4 5 -5 4 -4 4 -4 5 -4 +4 -5 5 -4 4 -4 5 -4 4 -4 5 -5 4 -4 5 -4 +4 -4 5 -5 4 -4 5 -4 4 -4 4 -5 5 -4 4 -4 +5 -4 4 -4 5 -5 4 -4 5 -4 4 -4 5 -5 4 -4 +5 -4 4 -4 5 -4 4 -5 5 -4 4 -4 4 -4 5 -5 +4 -4 5 -4 4 -4 5 0 4 12 5 13 4 13 5 12 +4 13 5 13 4 12 5 13 4 13 4 13 5 12 4 13 +5 13 4 12 5 13 4 13 5 12 4 13 5 13 4 12 +5 13 4 13 5 12 4 13 4 13 5 13 4 12 5 13 +4 13 5 12 4 13 5 13 4 12 5 13 4 13 5 12 +4 13 5 13 4 12 4 13 5 13 4 13 5 12 4 13 +5 13 4 12 5 13 4481 3169 100 MP stroke +4 13 5 12 4 13 5 13 4 12 5 13 4 13 5 12 +4 13 4 13 5 13 4 12 5 13 4 13 5 12 4 13 +5 13 4 12 5 13 4 13 5 12 4 13 5 13 4 12 +4 13 5 13 4 13 5 12 4 13 5 13 4 12 5 13 +4 13 5 12 4 13 5 13 4 12 5 13 4 13 4 13 +5 12 4 13 5 13 4 12 5 13 4 13 5 12 4 13 +5 13 4 12 5 13 4 13 5 12 4 13 4 13 5 13 +4 12 5 13 4 13 5 12 4 13 5 13 4 12 5 13 +4 13 5 12 4 13 5 13 4 12 5 13 4 13 4 13 +5 12 4 13 5 13 4 12 5 13 4 13 5 12 4 13 +5 13 4 12 5 13 4 13 5 12 4 13 4 13 5 13 +4 12 5 13 4 13 5 12 4 13 5 13 4 12 5 13 +4 13 5 12 4 13 4039 1913 100 MP stroke +5 13 4 12 4 13 5 13 4 13 5 12 4 13 5 13 +4 12 5 13 4 13 5 12 4 13 5 13 4 12 5 13 +4 13 5 12 4 13 4 13 5 13 4 12 5 13 4 13 +5 12 4 13 5 13 4 12 5 13 4 13 5 12 4 13 +5 13 4 12 4 13 5 13 4 13 5 12 4 13 5 13 +4 12 5 13 4 13 5 12 4 13 5 13 4 12 5 13 +4 13 4 12 5 13 4 13 5 13 4 12 5 13 4 13 +5 12 4 13 5 13 4 12 5 13 4 13 5 12 4 13 +4 13 5 12 4 13 5 13 4 13 5 12 4 13 5 13 +4 12 5 13 4 13 5 12 4 13 5 13 4 12 5 13 +4 13 4 12 5 13 4 13 5 13 4 12 5 13 4 13 +5 12 4 13 5 13 4 12 5 13 4 13 5 12 4 13 +4 13 5 12 4 13 3597 657 100 MP stroke +5 13 4 13 5 12 4 13 5 0 4 -13 5 -12 4 -13 +5 -13 4 -13 5 -12 4 -13 4 -13 5 -12 4 -13 5 -13 +4 -12 5 -13 4 -13 5 -12 4 -13 5 -13 4 -12 5 -13 +4 -13 5 -13 4 -12 4 -13 5 -13 4 -12 5 -13 4 -13 +5 -12 4 -13 5 -13 4 -12 5 -13 4 -13 5 -12 4 -13 +5 -13 4 -13 5 -12 4 -13 4 -13 5 -12 4 -13 5 -13 +4 -12 5 -13 4 -13 5 -12 4 -13 5 -13 4 -12 5 -13 +4 -13 5 -13 4 -12 4 -13 5 -13 4 -12 5 -13 4 -13 +5 -12 4 -13 5 -13 4 -12 5 -13 4 -13 5 -12 4 -13 +5 -13 4 -13 4 -12 5 -13 4 -13 5 -12 4 -13 5 -13 +4 -12 5 -13 4 -13 5 -12 4 -13 5 -13 4 -12 5 -13 +4 -13 4 -13 5 -12 4 -13 5 -13 4 -12 5 -13 4 -13 +5 -12 4 -13 5 -13 3154 1799 100 MP stroke +4 -12 5 -13 4 -13 5 -12 4 -13 5 -13 4 -13 4 -12 +5 -13 4 -13 5 -12 4 -13 5 -13 4 -12 5 -13 4 -13 +5 -12 4 -13 5 -13 4 -12 5 -13 4 -13 4 -13 5 -12 +4 -13 5 -13 4 -12 5 -13 4 -13 5 -12 4 -13 5 -13 +4 -12 5 -13 4 -13 5 -12 4 -13 4 -13 5 -13 4 -12 +5 -13 4 -13 5 -12 4 -13 5 -13 4 -12 5 -13 4 -13 +5 -12 4 -13 5 -13 4 -12 5 -13 4 -13 4 -13 5 -12 +4 -13 5 -13 4 -12 5 -13 4 -13 5 -12 4 -13 5 -13 +4 -12 5 -13 4 -13 5 -12 4 -13 4 -13 5 -13 4 -12 +5 -13 4 -13 5 -12 4 -13 5 -13 4 -12 5 -13 4 -13 +5 -12 4 -13 5 -13 4 -13 4 -12 5 -13 4 -13 5 -12 +4 -13 5 -13 4 -12 5 -13 4 -13 5 -12 4 -13 5 -13 +4 -12 5 -13 4 -13 2712 3055 100 MP stroke +4 -13 5 -12 4 -13 5 -13 4 -12 5 -13 4 -13 5 -12 +4 -13 5 -13 4 -12 5 -13 4 -13 5 -12 4 -13 5 -13 +4 -13 4 -12 5 -13 4 -13 5 -12 4 -13 5 -13 4 -12 +5 -13 4 -13 5 -12 4 -13 5 -13 4 -12 5 -13 4 -13 +4 -13 5 -12 4 -13 5 -13 4 -12 5 -13 4 -13 5 -12 +4 -13 5 -13 4 -12 5 -13 4 -13 5 -12 4 -13 4 -13 +5 -13 4 -12 5 -13 4 -13 5 -12 4 -13 5 -13 4 -12 +5 0 4 4 5 4 4 4 5 5 4 4 4 4 5 4 +4 5 5 4 4 4 5 4 4 4 5 5 4 4 5 4 +4 4 5 5 4 4 5 4 4 4 5 4 4 5 4 4 +5 4 4 4 5 5 4 4 5 4 4 4 5 5 4 4 +5 4 4 4 5 4 4 5 5 4 4 4 4 4 5 5 +4 4 5 4 4 4 2270 3588 100 MP stroke +5 5 4 4 5 4 4 4 5 4 4 5 5 4 4 4 +5 4 4 5 4 4 5 4 4 4 5 5 4 4 5 4 +4 4 5 4 4 5 5 4 4 4 5 4 4 5 5 4 +4 4 4 4 5 4 4 5 5 4 4 4 5 4 4 5 +5 4 4 4 5 4 4 5 5 4 4 4 5 4 4 4 +5 5 4 4 4 4 5 4 4 5 5 4 4 4 5 4 +4 5 5 4 4 4 5 4 4 4 5 5 4 4 5 4 +4 4 4 5 5 4 4 4 5 4 4 5 5 4 4 4 +5 4 4 4 5 5 4 4 5 4 4 4 5 5 4 4 +4 4 5 4 4 4 5 5 4 4 5 4 4 4 5 5 +4 4 5 4 4 4 5 5 4 4 5 4 4 4 4 4 +5 5 4 4 5 4 4 4 5 5 4 4 5 4 4 4 +5 5 4 4 5 4 1827 3169 100 MP stroke +4 4 5 4 4 5 5 4 4 4 4 4 5 5 4 4 +5 4 4 4 5 5 4 4 5 4 4 4 5 4 4 5 +5 4 4 4 5 4 4 5 4 4 5 4 4 4 5 4 +4 5 5 4 4 4 5 4 4 5 5 4 4 4 5 4 +4 5 5 4 4 4 4 4 5 4 4 5 5 4 4 4 +5 4 4 5 5 4 4 4 5 4 4 5 5 4 4 4 +5 4 4 4 5 5 4 4 4 4 5 4 4 5 5 4 +4 4 5 4 4 5 5 4 4 4 5 4 4 4 5 5 +4 4 5 4 4 4 4 5 5 4 4 4 5 4 4 4 +5 5 4 4 5 4 4 4 5 5 4 4 5 4 4 4 +5 5 4 4 4 4 5 4 4 4 5 5 4 4 5 4 +4 4 5 5 4 4 5 4 4 4 5 5 4 4 5 4 +4 4 4 4 5 5 1385 2750 100 MP stroke +4 4 5 4 4 4 5 5 4 4 5 4 4 4 5 5 +4 4 1345 2712 10 MP stroke +DA +4 -4 5 -5 4 -4 5 -4 4 -4 5 -5 4 -4 5 -4 +4 -4 5 -5 4 -4 4 -4 5 -4 4 -4 5 -5 4 -4 +5 -4 4 -4 5 -5 4 -4 5 -4 4 -4 5 -5 4 -4 +5 -4 4 -4 4 -4 5 -5 4 -4 5 -4 4 -4 5 -5 +4 -4 5 -4 4 -4 5 -5 4 -4 5 -4 4 -4 5 -4 +4 -5 4 -4 5 -4 4 -4 5 -5 4 -4 5 -4 4 -4 +5 -4 4 -5 5 -4 4 -4 5 -4 4 -5 5 -4 4 -4 +4 -4 5 -5 4 -4 5 -4 4 -4 5 -4 4 -5 5 -4 +4 -4 5 -4 4 -5 5 -4 4 -4 5 -4 4 -5 5 -4 +4 -4 4 -4 5 -4 4 -5 5 -4 4 -4 5 -4 4 -5 +5 -4 4 -4 5 -4 4 -5 5 -4 4 -4 5 -4 4 -4 +4 -5 5 -4 4 -4 5 -4 4 -5 5 -4 4 -4 5 -4 +4 -4 5 -5 4 -4 5366 3131 100 MP stroke +5 -4 4 -4 5 -5 4 -4 4 -4 5 -4 4 -5 5 -4 +4 -4 5 -4 4 -4 5 -5 4 -4 5 -4 4 -4 5 -5 +4 -4 5 -4 4 -4 5 -5 4 -4 4 -4 5 -4 4 -4 +5 -5 4 -2 5 13 4 13 5 12 4 13 5 13 4 12 +5 13 4 13 5 12 4 13 4 13 5 12 4 13 5 13 +4 13 5 12 4 13 5 13 4 12 5 13 4 13 5 12 +4 13 5 13 4 12 4 13 5 13 4 12 5 13 4 13 +5 13 4 12 5 13 4 13 5 12 4 13 5 13 4 12 +5 13 4 13 4 12 5 13 4 13 5 12 4 13 5 13 +4 13 5 12 4 13 5 13 4 12 5 13 4 13 5 12 +4 13 5 13 4 12 4 13 5 13 4 12 5 13 4 13 +5 13 4 12 5 13 4 13 5 12 4 13 5 13 4 12 +5 13 4 13 4 12 4924 2313 100 MP stroke +5 13 4 13 5 12 4 13 5 13 4 13 5 12 4 13 +5 13 4 12 5 13 4 13 5 12 4 13 4 13 5 12 +4 13 5 13 4 12 5 13 4 13 5 13 4 12 5 13 +4 13 5 12 4 13 5 13 4 12 4 13 5 13 4 12 +5 13 4 13 5 12 4 13 5 13 4 13 5 12 4 13 +5 13 4 12 5 13 4 13 5 12 4 13 4 13 5 12 +4 13 5 13 4 12 5 5 4 -21 5 -22 4 -21 5 -21 +4 -21 5 -21 4 -21 5 -21 4 -22 4 -21 5 -21 4 -21 +5 -21 4 -21 5 -21 4 -22 5 -21 4 -21 5 -21 4 -21 +5 -21 4 -22 5 -21 4 -21 4 -21 5 -21 4 -21 5 -21 +4 -22 5 -21 4 -21 5 -21 4 -21 5 -21 4 -21 5 -22 +4 -21 5 -21 4 -21 4 -21 5 -21 4 -21 5 -22 4 -21 +5 -21 4 -21 5 -21 4481 2655 100 MP stroke +4 -21 5 -21 4 -22 5 -21 4 -21 5 -21 4 -21 5 -21 +4 -21 4 -22 5 -21 4 -21 5 -21 4 -21 5 -21 4 -21 +5 -22 4 -21 5 -21 4 -21 5 -21 4 -21 5 -22 4 -21 +4 -21 5 -21 4 -21 5 -21 4 -21 5 -22 4 -21 5 -21 +4 -21 5 -21 4 -21 5 -21 4 -22 5 -21 4 -21 4 -21 +5 -21 4 -21 5 -21 4 -22 5 -21 4 -21 5 -21 4 -21 +5 -21 4 -21 5 -22 4 -21 5 -21 4 -21 4 -21 5 -21 +4 -21 5 -22 4 -21 5 -21 4 -21 5 -21 4 -21 5 -22 +4 -21 5 -21 4 -21 5 -21 4 -21 5 -21 4 -22 4 -21 +5 -21 4 -21 5 -21 4 -21 5 -21 4 -3 5 30 4 30 +5 29 4 30 5 29 4 30 5 30 4 29 4 30 5 30 +4 29 5 30 4 29 5 30 4 30 5 29 4 30 5 29 +4 30 5 30 4 29 4039 3664 100 MP stroke +5 30 4 29 4 30 5 30 4 29 5 30 4 29 5 30 +4 30 5 29 4 30 5 29 4 30 5 30 4 29 5 30 +4 29 5 30 4 30 4 29 5 30 4 29 5 30 4 30 +5 29 4 30 5 29 4 30 5 30 4 29 5 30 4 30 +5 29 4 30 4 29 5 30 4 30 5 29 4 30 5 29 +4 30 5 30 4 29 5 30 4 29 5 30 4 30 5 29 +4 30 4 29 5 30 4 30 5 29 4 30 5 29 4 30 +5 30 4 29 5 30 4 29 5 30 4 30 5 29 4 30 +4 29 5 30 4 30 5 29 4 30 5 29 4 30 5 30 +4 29 5 30 4 30 5 29 4 30 5 29 4 30 5 30 +4 29 4 30 5 29 4 30 5 30 4 29 5 30 4 29 +5 30 4 30 5 29 4 30 5 29 4 30 5 30 4 29 +4 30 5 29 4 30 3597 733 100 MP stroke +5 30 4 29 5 30 4 29 5 0 4 -29 5 -30 4 -29 +5 -30 4 -30 5 -29 4 -30 4 -29 5 -30 4 -30 5 -29 +4 -30 5 -29 4 -30 5 -30 4 -29 5 -30 4 -29 5 -30 +4 -30 5 -29 4 -30 4 -29 5 -30 4 -30 5 -29 4 -30 +5 -29 4 -30 5 -30 4 -29 5 -30 4 -30 5 -29 4 -30 +5 -29 4 -30 5 -30 4 -29 4 -30 5 -29 4 -30 5 -30 +4 -29 5 -30 4 -29 5 -30 4 -30 5 -29 4 -30 5 -29 +4 -30 5 -30 4 -29 4 -30 5 -29 4 -30 5 -30 4 -29 +5 -30 4 -29 5 -30 4 -30 5 -29 4 -30 5 -29 4 -30 +5 -30 4 -29 4 -30 5 -29 4 -30 5 -30 4 -29 5 -30 +4 -30 5 -29 4 -30 5 -29 4 -30 5 -30 4 -29 5 -30 +4 -29 4 -30 5 -30 4 -29 5 -30 4 -29 5 -30 4 -30 +5 -29 4 -30 5 -29 3154 3397 100 MP stroke +4 -30 5 -30 4 -29 5 -30 4 -29 5 -30 4 -30 4 -29 +5 -30 4 -29 5 -30 4 -30 5 -29 4 -30 5 -29 4 -30 +5 -30 4 -29 5 -30 4 -29 5 -30 4 -30 4 -29 5 -30 +4 -30 5 -29 4 -30 5 -29 4 -30 5 -30 4 3 5 21 +4 21 5 21 4 21 5 21 4 21 4 22 5 21 4 21 +5 21 4 21 5 21 4 21 5 22 4 21 5 21 4 21 +5 21 4 21 5 22 4 21 5 21 4 21 4 21 5 21 +4 21 5 22 4 21 5 21 4 21 5 21 4 21 5 21 +4 22 5 21 4 21 5 21 4 21 4 21 5 21 4 22 +5 21 4 21 5 21 4 21 5 21 4 21 5 22 4 21 +5 21 4 21 5 21 4 21 4 21 5 22 4 21 5 21 +4 21 5 21 4 21 5 22 4 21 5 21 4 21 5 21 +4 21 5 21 4 22 2712 2845 100 MP stroke +4 21 5 21 4 21 5 21 4 21 5 21 4 22 5 21 +4 21 5 21 4 21 5 21 4 21 5 22 4 21 5 21 +4 21 4 21 5 21 4 21 5 22 4 21 5 21 4 21 +5 21 4 21 5 21 4 22 5 21 4 21 5 21 4 21 +4 21 5 21 4 22 5 21 4 21 5 21 4 21 5 21 +4 22 5 21 4 21 5 21 4 21 5 21 4 21 4 22 +5 21 4 21 5 21 4 21 5 21 4 21 5 22 4 21 +5 -5 4 -12 5 -13 4 -13 5 -12 4 -13 4 -13 5 -12 +4 -13 5 -13 4 -12 5 -13 4 -13 5 -12 4 -13 5 -13 +4 -13 5 -12 4 -13 5 -13 4 -12 5 -13 4 -13 4 -12 +5 -13 4 -13 5 -12 4 -13 5 -13 4 -12 5 -13 4 -13 +5 -13 4 -12 5 -13 4 -13 5 -12 4 -13 4 -13 5 -12 +4 -13 5 -13 4 -12 2270 2198 100 MP stroke +5 -13 4 -13 5 -12 4 -13 5 -13 4 -13 5 -12 4 -13 +5 -13 4 -12 4 -13 5 -13 4 -12 5 -13 4 -13 5 -12 +4 -13 5 -13 4 -12 5 -13 4 -13 5 -13 4 -12 5 -13 +4 -13 4 -12 5 -13 4 -13 5 -12 4 -13 5 -13 4 -12 +5 -13 4 -13 5 -12 4 -13 5 -13 4 -13 5 -12 4 -13 +5 -13 4 -12 4 -13 5 -13 4 -12 5 -13 4 -13 5 -12 +4 -13 5 -13 4 -12 5 -13 4 -13 5 -13 4 -12 5 -13 +4 -13 4 -12 5 -13 4 -13 5 -12 4 -13 5 -13 4 -12 +5 -13 4 -13 5 -12 4 -13 5 -13 4 -13 5 -12 4 -13 +4 -13 5 -12 4 -13 5 -13 4 -12 5 -13 4 -13 5 -12 +4 -13 5 -13 4 2 5 5 4 4 5 4 4 4 4 4 +5 5 4 4 5 4 4 4 5 5 4 4 5 4 4 4 +5 5 4 4 5 4 1827 3169 100 MP stroke +4 4 5 4 4 5 5 4 4 4 4 4 5 5 4 4 +5 4 4 4 5 5 4 4 5 4 4 4 5 4 4 5 +5 4 4 4 5 4 4 5 4 4 5 4 4 4 5 4 +4 5 5 4 4 4 5 4 4 5 5 4 4 4 5 4 +4 5 5 4 4 4 4 4 5 4 4 5 5 4 4 4 +5 4 4 5 5 4 4 4 5 4 4 5 5 4 4 4 +5 4 4 4 5 5 4 4 4 4 5 4 4 5 5 4 +4 4 5 4 4 5 5 4 4 4 5 4 4 4 5 5 +4 4 5 4 4 4 4 5 5 4 4 4 5 4 4 4 +5 5 4 4 5 4 4 4 5 5 4 4 5 4 4 4 +5 5 4 4 4 4 5 4 4 4 5 5 4 4 5 4 +4 4 5 5 4 4 5 4 4 4 5 5 4 4 5 4 +4 4 4 4 5 5 1385 2750 100 MP stroke +4 4 5 4 4 4 5 5 4 4 5 4 4 4 5 5 +4 4 1345 2712 10 MP stroke +DD +4 -4 5 -5 4 -4 5 -4 4 -4 5 -5 4 -4 5 -4 +4 -4 5 -5 4 -4 4 -4 5 -4 4 -4 5 -5 4 -4 +5 -4 4 -4 5 -5 4 -4 5 -4 4 -4 5 -5 4 -4 +5 -4 4 -4 4 -4 5 -5 4 -4 5 -4 4 -4 5 -5 +4 -4 5 -4 4 -4 5 -5 4 -4 5 -4 4 -4 5 -4 +4 -5 4 -4 5 -4 4 -4 5 -5 4 -4 5 -4 4 -4 +5 -4 4 -5 5 -4 4 -4 5 -4 4 -5 5 -4 4 -4 +4 -4 5 -5 4 -4 5 -4 4 -4 5 -4 4 5 5 13 +4 12 5 13 4 13 5 12 4 13 5 13 4 12 5 13 +4 13 4 12 5 13 4 13 5 12 4 13 5 13 4 13 +5 12 4 13 5 13 4 12 5 13 4 13 5 12 4 13 +4 13 5 12 4 13 5 13 4 12 5 13 4 13 5 13 +4 12 5 13 4 13 5366 2512 100 MP stroke +5 12 4 13 5 13 4 12 4 13 5 13 4 12 5 13 +4 13 5 12 4 13 5 13 4 13 5 12 4 13 5 13 +4 12 5 13 4 13 5 12 4 13 4 13 5 12 4 13 +5 13 4 8 5 -21 4 -21 5 -21 4 -21 5 -21 4 -22 +5 -21 4 -21 5 -21 4 -21 4 -21 5 -22 4 -21 5 -21 +4 -21 5 -21 4 -21 5 -21 4 -22 5 -21 4 -21 5 -21 +4 -21 5 -21 4 -21 4 -22 5 -21 4 -21 5 -21 4 -21 +5 -21 4 -21 5 -22 4 -21 5 -21 4 -21 5 -21 4 -21 +5 -21 4 -22 4 -21 5 -21 4 -21 5 -21 4 -21 5 -21 +4 -22 5 -21 4 -21 5 -21 4 -21 5 -21 4 -22 5 -21 +4 -21 5 -21 4 -21 4 -21 5 -21 4 -22 5 -21 4 -21 +5 14 4 30 5 29 4 30 5 29 4 30 5 30 4 29 +5 30 4 29 4 30 4924 3188 100 MP stroke +5 30 4 29 5 30 4 29 5 30 4 30 5 29 4 30 +5 29 4 30 5 30 4 29 5 30 4 29 4 30 5 30 +4 29 5 30 4 29 5 30 4 30 5 29 4 30 5 29 +4 30 5 30 4 29 5 30 4 29 4 30 5 30 4 29 +5 30 4 30 5 29 4 30 5 29 4 30 5 30 4 29 +5 30 4 29 5 30 4 30 5 29 4 30 4 29 5 30 +4 30 5 29 4 30 5 12 4 -38 5 -38 4 -38 5 -38 +4 -38 5 -38 4 -38 5 -38 4 -38 4 -38 5 -38 4 -38 +5 -38 4 -38 5 -38 4 -39 5 -38 4 -38 5 -38 4 -38 +5 -38 4 -38 5 -38 4 -38 4 -38 5 -38 4 -38 5 -38 +4 -38 5 -38 4 -38 5 -39 4 -38 5 -38 4 -38 5 -38 +4 -38 5 -38 4 -38 4 -38 5 -38 4 -38 5 -38 4 -38 +5 -38 4 -38 5 -38 4481 3454 100 MP stroke +4 -39 5 -38 4 -38 5 -38 4 -38 5 -38 4 -38 5 -38 +4 -38 4 -38 5 -38 4 -38 5 -38 4 -38 5 -38 4 30 +5 47 4 46 5 47 4 46 5 47 4 46 5 47 4 46 +4 47 5 46 4 47 5 46 4 47 5 47 4 46 5 47 +4 46 5 47 4 46 5 47 4 46 5 47 4 46 4 47 +5 46 4 47 5 46 4 47 5 46 4 47 5 46 4 47 +5 46 4 47 5 46 4 47 5 47 4 46 4 47 5 46 +4 47 5 46 4 47 5 46 4 47 5 46 4 47 5 46 +4 47 5 46 4 47 5 46 4 47 5 46 4 47 4 46 +5 47 4 46 5 47 4 47 5 46 4 9 5 -55 4 -55 +5 -55 4 -55 5 -55 4 -55 5 -55 4 -55 4 -55 5 -55 +4 -55 5 -55 4 -55 5 -55 4 -55 5 -55 4 -55 5 -55 +4 -55 5 -55 4 -55 4039 2303 100 MP stroke +5 -55 4 -55 4 -55 5 -55 4 -55 5 -55 4 -55 5 -55 +4 -55 5 -55 4 -55 5 -55 4 -55 5 -55 4 -55 5 -55 +4 -55 5 -55 4 -55 4 -55 5 -55 4 -55 5 -55 4 -55 +5 -55 4 -55 5 -55 4 -55 5 -54 4 -55 5 -55 4 -55 +5 -55 4 -55 4 -55 5 -55 4 -55 5 -55 4 -55 5 -55 +4 -55 5 56 4 63 5 64 4 63 5 64 4 63 5 63 +4 64 4 63 5 64 4 63 5 64 4 63 5 63 4 64 +5 63 4 64 5 63 4 64 5 63 4 64 5 63 4 63 +4 64 5 63 4 64 5 63 4 64 5 63 4 64 5 63 +4 63 5 64 4 63 5 64 4 63 5 64 4 63 5 63 +4 64 4 63 5 64 4 63 5 64 4 63 5 64 4 63 +5 63 4 64 5 63 4 64 5 63 4 64 5 63 4 63 +4 64 5 63 4 64 3597 885 100 MP stroke +5 63 4 64 5 63 4 64 5 0 4 -64 5 -63 4 -64 +5 -63 4 -64 5 -63 4 -64 4 -63 5 -63 4 -64 5 -63 +4 -64 5 -63 4 -64 5 -63 4 -63 5 -64 4 -63 5 -64 +4 -63 5 -64 4 -63 4 -64 5 -63 4 -63 5 -64 4 -63 +5 -64 4 -63 5 -64 4 -63 5 -63 4 -64 5 -63 4 -64 +5 -63 4 -64 5 -63 4 -64 4 -63 5 -63 4 -64 5 -63 +4 -64 5 -63 4 -64 5 -63 4 -64 5 -63 4 -63 5 -64 +4 -63 5 -64 4 -63 4 -64 5 -63 4 -63 5 -64 4 -63 +5 -64 4 -63 5 -56 4 55 5 55 4 55 5 55 4 55 +5 55 4 55 4 55 5 55 4 55 5 55 4 55 5 54 +4 55 5 55 4 55 5 55 4 55 5 55 4 55 5 55 +4 55 4 55 5 55 4 55 5 55 4 55 5 55 4 55 +5 55 4 55 5 55 3154 2798 100 MP stroke +4 55 5 55 4 55 5 55 4 55 5 55 4 55 4 55 +5 55 4 55 5 55 4 55 5 55 4 55 5 55 4 55 +5 55 4 55 5 55 4 55 5 55 4 55 4 55 5 55 +4 55 5 55 4 55 5 55 4 55 5 55 4 -9 5 -46 +4 -47 5 -47 4 -46 5 -47 4 -46 4 -47 5 -46 4 -47 +5 -46 4 -47 5 -46 4 -47 5 -46 4 -47 5 -46 4 -47 +5 -46 4 -47 5 -46 4 -47 5 -46 4 -47 4 -46 5 -47 +4 -47 5 -46 4 -47 5 -46 4 -47 5 -46 4 -47 5 -46 +4 -47 5 -46 4 -47 5 -46 4 -47 4 -46 5 -47 4 -46 +5 -47 4 -46 5 -47 4 -46 5 -47 4 -46 5 -47 4 -47 +5 -46 4 -47 5 -46 4 -47 4 -46 5 -47 4 -46 5 -47 +4 -46 5 -47 4 -46 5 -47 4 -30 5 38 4 38 5 38 +4 38 5 38 4 38 2712 3797 100 MP stroke +4 38 5 38 4 38 5 38 4 38 5 38 4 38 5 38 +4 39 5 38 4 38 5 38 4 38 5 38 4 38 5 38 +4 38 4 38 5 38 4 38 5 38 4 38 5 38 4 38 +5 39 4 38 5 38 4 38 5 38 4 38 5 38 4 38 +4 38 5 38 4 38 5 38 4 38 5 38 4 38 5 38 +4 39 5 38 4 38 5 38 4 38 5 38 4 38 4 38 +5 38 4 38 5 38 4 38 5 38 4 38 5 38 4 38 +5 -12 4 -30 5 -29 4 -30 5 -30 4 -29 4 -30 5 -29 +4 -30 5 -30 4 -29 5 -30 4 -29 5 -30 4 -30 5 -29 +4 -30 5 -29 4 -30 5 -30 4 -29 5 -30 4 -30 4 -29 +5 -30 4 -29 5 -30 4 -30 5 -29 4 -30 5 -29 4 -30 +5 -30 4 -29 5 -30 4 -29 5 -30 4 -30 4 -29 5 -30 +4 -29 5 -30 4 -30 2270 2922 100 MP stroke +5 -29 4 -30 5 -29 4 -30 5 -30 4 -29 5 -30 4 -29 +5 -30 4 -30 4 -29 5 -30 4 -29 5 -30 4 -30 5 -29 +4 -30 5 -29 4 -30 5 -14 4 21 5 21 4 22 5 21 +4 21 4 21 5 21 4 21 5 21 4 22 5 21 4 21 +5 21 4 21 5 21 4 22 5 21 4 21 5 21 4 21 +5 21 4 21 4 22 5 21 4 21 5 21 4 21 5 21 +4 21 5 22 4 21 5 21 4 21 5 21 4 21 5 21 +4 22 4 21 5 21 4 21 5 21 4 21 5 21 4 22 +5 21 4 21 5 21 4 21 5 21 4 21 5 22 4 21 +4 21 5 21 4 21 5 21 4 22 5 21 4 21 5 21 +4 21 5 21 4 -8 5 -13 4 -13 5 -12 4 -13 4 -13 +5 -12 4 -13 5 -13 4 -12 5 -13 4 -13 5 -12 4 -13 +5 -13 4 -13 5 -12 1827 2398 100 MP stroke +4 -13 5 -13 4 -12 5 -13 4 -13 4 -12 5 -13 4 -13 +5 -12 4 -13 5 -13 4 -12 5 -13 4 -13 5 -13 4 -12 +5 -13 4 -13 5 -12 4 -13 4 -13 5 -12 4 -13 5 -13 +4 -12 5 -13 4 -13 5 -12 4 -13 5 -13 4 -13 5 -12 +4 -13 5 -13 4 -12 4 -13 5 -13 4 -12 5 -13 4 -13 +5 -12 4 -13 5 -13 4 -12 5 -13 4 -5 5 4 4 4 +5 4 4 4 5 5 4 4 4 4 5 4 4 5 5 4 +4 4 5 4 4 5 5 4 4 4 5 4 4 4 5 5 +4 4 5 4 4 4 4 5 5 4 4 4 5 4 4 4 +5 5 4 4 5 4 4 4 5 5 4 4 5 4 4 4 +5 5 4 4 4 4 5 4 4 4 5 5 4 4 5 4 +4 4 5 5 4 4 5 4 4 4 5 5 4 4 5 4 +4 4 4 4 5 5 1385 2750 100 MP stroke +4 4 5 4 4 4 5 5 4 4 5 4 4 4 5 5 +4 4 1345 2712 10 MP stroke +gr + +12 w +DD +2985 5063 mt +(Delay [chip]) s + 335 3030 mt -90 rotate +(Correlation) s +90 rotate +%%IncludeResource: font Helvetica +/Helvetica /WindowsLatin1Encoding 120 FMSR + + 882 4657 mt +( ) s +6239 431 mt +( ) s +SO +6 w +%%IncludeResource: font Helvetica-Oblique +/Helvetica-Oblique /WindowsLatin1Encoding 216 FMSR + +5664 675 mt +(p) s +%%IncludeResource: font Helvetica +/Helvetica /WindowsLatin1Encoding 216 FMSR + +5784 675 mt +( = 1) s +gs 5203 449 993 814 MR c np +12 w +355 0 5273 597 2 MP stroke +gr + +12 w +%%IncludeResource: font Helvetica-Oblique +/Helvetica-Oblique /WindowsLatin1Encoding 216 FMSR + +5664 933 mt +(p) s +%%IncludeResource: font Helvetica +/Helvetica /WindowsLatin1Encoding 216 FMSR + +5784 933 mt +( = 2) s +gs 5203 449 993 814 MR c np +DA +355 0 5273 855 2 MP stroke +SO +gr + +%%IncludeResource: font Helvetica-Oblique +/Helvetica-Oblique /WindowsLatin1Encoding 216 FMSR + +5664 1191 mt +(p) s +%%IncludeResource: font Helvetica +/Helvetica /WindowsLatin1Encoding 216 FMSR + +5784 1191 mt +( = 4) s +gs 5203 449 993 814 MR c np +DD +355 0 5273 1113 2 MP stroke +SO +6 w +gr + +6 w + +end %%Color Dict + +eplot +%%EndObject + +epage +end + +showpage + +%%Trailer +%%EOF diff --git a/GNSS_SDR_IQ/findPreambles.m b/GNSS_SDR_IQ/findPreambles.m new file mode 100644 index 0000000..390ff6e --- /dev/null +++ b/GNSS_SDR_IQ/findPreambles.m @@ -0,0 +1,150 @@ +function [firstSubFrame, activeChnList] = findPreambles(trackResults, ... + settings) +% findPreambles finds the first preamble occurrence in the bit stream of +% each channel. The preamble is verified by check of the spacing between +% preambles (6sec) and parity checking of the first two words in a +% subframe. At the same time function returns list of channels, that are in +% tracking state and with valid preambles in the nav data stream. +% +%[firstSubFrame, activeChnList] = findPreambles(trackResults, settings) +% +% Inputs: +% trackResults - output from the tracking function +% settings - Receiver settings. +% +% Outputs: +% firstSubframe - the array contains positions of the first +% preamble in each channel. The position is ms count +% since start of tracking. Corresponding value will +% be set to 0 if no valid preambles were detected in +% the channel. +% activeChnList - list of channels containing valid preambles + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen +% Written by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen +%-------------------------------------------------------------------------- +% +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% CVS record: +% $Id: findPreambles.m,v 1.1.2.10 2006/08/14 11:38:22 dpl Exp $ + +% Preamble search can be delayed to a later point in the tracking results +% to avoid noise due to tracking loop transients +searchStartOffset = 0; + +%--- Initialize the firstSubFrame array ----------------------------------- +firstSubFrame = zeros(1, settings.numberOfChannels); + +%--- Generate the preamble pattern ---------------------------------------- +preamble_bits = [1 -1 -1 -1 1 -1 1 1]; + +% "Upsample" the preamble - make 20 vales per one bit. The preamble must be +% found with precision of a sample. +preamble_ms = kron(preamble_bits, ones(1, 20)); + +%--- Make a list of channels excluding not tracking channels -------------- +activeChnList = find([trackResults.status] ~= '-'); + +%=== For all tracking channels ... +for channelNr = activeChnList + +%% Correlate tracking output with preamble ================================ + % Read output from tracking. It contains the navigation bits. The start + % of record is skiped here to avoid tracking loop transients. + bits = trackResults(channelNr).I_P(1 + searchStartOffset : end); + + % Now threshold the output and convert it to -1 and +1 + bits(bits > 0) = 1; + bits(bits <= 0) = -1; + + % Correlate tracking output with the preamble + tlmXcorrResult = xcorr(bits, preamble_ms); + +%% Find all starting points off all preamble like patterns ================ + clear index + clear index2 + + xcorrLength = (length(tlmXcorrResult) + 1) /2; + + %--- Find at what index/ms the preambles start ------------------------ + index = find(... + abs(tlmXcorrResult(xcorrLength : xcorrLength * 2 - 1)) > 153)' + ... + searchStartOffset; + +%% Analyze detected preamble like patterns ================================ + for i = 1:size(index) % For each occurrence + + %--- Find distances in time between this occurrence and the rest of + %preambles like patterns. If the distance is 6000 milliseconds (one + %subframe), the do further verifications by validating the parities + %of two GPS words + + index2 = index - index(i); + + if (~isempty(find(index2 == 6000))) + + %=== Re-read bit vales for preamble verification ============== + % Preamble occurrence is verified by checking the parity of + % the first two words in the subframe. Now it is assumed that + % bit boundaries a known. Therefore the bit values over 20ms are + % combined to increase receiver performance for noisy signals. + % in Total 62 bits mast be read : + % 2 bits from previous subframe are needed for parity checking; + % 60 bits for the first two 30bit words (TLM and HOW words). + % The index is pointing at the start of TLM word. + bits = trackResults(channelNr).I_P(index(i)-40 : ... + index(i) + 20 * 60 -1)'; + + %--- Combine the 20 values of each bit ------------------------ + bits = reshape(bits, 20, (size(bits, 1) / 20)); + bits = sum(bits); + + % Now threshold and make it -1 and +1 + bits(bits > 0) = 1; + bits(bits <= 0) = -1; + + %--- Check the parity of the TLM and HOW words ---------------- + if (navPartyChk(bits(1:32)) ~= 0) && ... + (navPartyChk(bits(31:62)) ~= 0) + % Parity was OK. Record the preamble start position. Skip + % the rest of preamble pattern checking for this channel + % and process next channel. + + firstSubFrame(channelNr) = index(i); + break; + end % if parity is OK ... + + end % if (~isempty(find(index2 == 6000))) + end % for i = 1:size(index) + + % Exclude channel from the active channel list if no valid preamble was + % detected + if firstSubFrame(channelNr) == 0 + + % Exclude channel from further processing. It does not contain any + % valid preamble and therefore nothing more can be done for it. + activeChnList = setdiff(activeChnList, channelNr); + + disp(['Could not find valid preambles in channel ', ... + num2str(channelNr),'!']); + end + +end % for channelNr = activeChnList diff --git a/GNSS_SDR_IQ/geoFunctions/cart2geo.m b/GNSS_SDR_IQ/geoFunctions/cart2geo.m new file mode 100644 index 0000000..826fc6c --- /dev/null +++ b/GNSS_SDR_IQ/geoFunctions/cart2geo.m @@ -0,0 +1,60 @@ +function [phi, lambda, h] = cart2geo(X, Y, Z, i) +%CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical +%coordinates (phi, lambda, h) on a selected reference ellipsoid. +% +%[phi, lambda, h] = cart2geo(X, Y, Z, i); +% +% Choices i of Reference Ellipsoid for Geographical Coordinates +% 1. International Ellipsoid 1924 +% 2. International Ellipsoid 1967 +% 3. World Geodetic System 1972 +% 4. Geodetic Reference System 1980 +% 5. World Geodetic System 1984 + +%Kai Borre 10-13-98 +%Copyright (c) by Kai Borre +%Revision: 1.0 Date: 1998/10/23 +% +% CVS record: +% $Id: cart2geo.m,v 1.1.2.3 2007/01/29 15:22:49 dpl Exp $ +%========================================================================== + +a = [6378388 6378160 6378135 6378137 6378137]; +f = [1/297 1/298.247 1/298.26 1/298.257222101 1/298.257223563]; + +lambda = atan2(Y,X); +ex2 = (2-f(i))*f(i)/((1-f(i))^2); +c = a(i)*sqrt(1+ex2); +phi = atan(Z/((sqrt(X^2+Y^2)*(1-(2-f(i)))*f(i)))); + +h = 0.1; oldh = 0; +iterations = 0; +while abs(h-oldh) > 1.e-12 + oldh = h; + N = c/sqrt(1+ex2*cos(phi)^2); + phi = atan(Z/((sqrt(X^2+Y^2)*(1-(2-f(i))*f(i)*N/(N+h))))); + h = sqrt(X^2+Y^2)/cos(phi)-N; + + iterations = iterations + 1; + if iterations > 100 + fprintf('Failed to approximate h with desired precision. h-oldh: %e.\n', h-oldh); + break; + end +end + +phi = phi*180/pi; +% b = zeros(1,3); +% b(1,1) = fix(phi); +% b(2,1) = fix(rem(phi,b(1,1))*60); +% b(3,1) = (phi-b(1,1)-b(1,2)/60)*3600; + +lambda = lambda*180/pi; +% l = zeros(1,3); +% l(1,1) = fix(lambda); +% l(2,1) = fix(rem(lambda,l(1,1))*60); +% l(3,1) = (lambda-l(1,1)-l(1,2)/60)*3600; + +%fprintf('\n phi =%3.0f %3.0f %8.5f',b(1),b(2),b(3)) +%fprintf('\n lambda =%3.0f %3.0f %8.5f',l(1),l(2),l(3)) +%fprintf('\n h =%14.3f\n',h) +%%%%%%%%%%%%%% end cart2geo.m %%%%%%%%%%%%%%%%%%% diff --git a/GNSS_SDR_IQ/geoFunctions/cart2utm.m b/GNSS_SDR_IQ/geoFunctions/cart2utm.m new file mode 100644 index 0000000..7b12864 --- /dev/null +++ b/GNSS_SDR_IQ/geoFunctions/cart2utm.m @@ -0,0 +1,176 @@ +function [E, N, U] = cart2utm(X, Y, Z, zone) +%CART2UTM Transformation of (X,Y,Z) to (N,E,U) in UTM, zone 'zone'. +% +%[E, N, U] = cart2utm(X, Y, Z, zone); +% +% Inputs: +% X,Y,Z - Cartesian coordinates. Coordinates are referenced +% with respect to the International Terrestrial Reference +% Frame 1996 (ITRF96) +% zone - UTM zone of the given position +% +% Outputs: +% E, N, U - UTM coordinates (Easting, Northing, Uping) + +%Kai Borre -11-1994 +%Copyright (c) by Kai Borre +% +% CVS record: +% $Id: cart2utm.m,v 1.1.1.1.2.6 2007/01/30 09:45:12 dpl Exp $ + +%This implementation is based upon +%O. Andersson & K. Poder (1981) Koordinattransformationer +% ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren +% Vol. 30: 552--571 and Vol. 31: 76 +% +%An excellent, general reference (KW) is +%R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der +% h\"oheren Geod\"asie und Kartographie. +% Erster Band, Springer Verlag + +% Explanation of variables used: +% f flattening of ellipsoid +% a semi major axis in m +% m0 1 - scale at central meridian; for UTM 0.0004 +% Q_n normalized meridian quadrant +% E0 Easting of central meridian +% L0 Longitude of central meridian +% bg constants for ellipsoidal geogr. to spherical geogr. +% gb constants for spherical geogr. to ellipsoidal geogr. +% gtu constants for ellipsoidal N, E to spherical N, E +% utg constants for spherical N, E to ellipoidal N, E +% tolutm tolerance for utm, 1.2E-10*meridian quadrant +% tolgeo tolerance for geographical, 0.00040 second of arc + +% B, L refer to latitude and longitude. Southern latitude is negative +% International ellipsoid of 1924, valid for ED50 + +a = 6378388; +f = 1/297; +ex2 = (2-f)*f / ((1-f)^2); +c = a * sqrt(1+ex2); +vec = [X; Y; Z-4.5]; +alpha = .756e-6; +R = [ 1 -alpha 0; + alpha 1 0; + 0 0 1]; +trans = [89.5; 93.8; 127.6]; +scale = 0.9999988; +v = scale*R*vec + trans; % coordinate vector in ED50 +L = atan2(v(2), v(1)); +N1 = 6395000; % preliminary value +B = atan2(v(3)/((1-f)^2*N1), norm(v(1:2))/N1); % preliminary value +U = 0.1; oldU = 0; + +iterations = 0; +while abs(U-oldU) > 1.e-4 + oldU = U; + N1 = c/sqrt(1+ex2*(cos(B))^2); + B = atan2(v(3)/((1-f)^2*N1+U), norm(v(1:2))/(N1+U) ); + U = norm(v(1:2))/cos(B)-N1; + + iterations = iterations + 1; + if iterations > 100 + fprintf('Failed to approximate U with desired precision. U-oldU: %e.\n', U-oldU); + break; + end +end + +%Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21) +m0 = 0.0004; +n = f / (2-f); +m = n^2 * (1/4 + n*n/64); +w = (a*(-n-m0+m*(1-m0))) / (1+n); +Q_n = a + w; + +%Easting and longitude of central meridian +E0 = 500000; +L0 = (zone-30)*6 - 3; + +%Check tolerance for reverse transformation +tolutm = pi/2 * 1.2e-10 * Q_n; +tolgeo = 0.000040; + +%Coefficients of trigonometric series + +%ellipsoidal to spherical geographical, KW p. 186--187, (51)-(52) +% bg[1] = n*(-2 + n*(2/3 + n*(4/3 + n*(-82/45)))); +% bg[2] = n^2*(5/3 + n*(-16/15 + n*(-13/9))); +% bg[3] = n^3*(-26/15 + n*34/21); +% bg[4] = n^4*1237/630; + +%spherical to ellipsoidal geographical, KW p. 190--191, (61)-(62) +% gb[1] = n*(2 + n*(-2/3 + n*(-2 + n*116/45))); +% gb[2] = n^2*(7/3 + n*(-8/5 + n*(-227/45))); +% gb[3] = n^3*(56/15 + n*(-136/35)); +% gb[4] = n^4*4279/630; + +%spherical to ellipsoidal N, E, KW p. 196, (69) +% gtu[1] = n*(1/2 + n*(-2/3 + n*(5/16 + n*41/180))); +% gtu[2] = n^2*(13/48 + n*(-3/5 + n*557/1440)); +% gtu[3] = n^3*(61/240 + n*(-103/140)); +% gtu[4] = n^4*49561/161280; + +%ellipsoidal to spherical N, E, KW p. 194, (65) +% utg[1] = n*(-1/2 + n*(2/3 + n*(-37/96 + n*1/360))); +% utg[2] = n^2*(-1/48 + n*(-1/15 + n*437/1440)); +% utg[3] = n^3*(-17/480 + n*37/840); +% utg[4] = n^4*(-4397/161280); + +%With f = 1/297 we get + +bg = [-3.37077907e-3; + 4.73444769e-6; + -8.29914570e-9; + 1.58785330e-11]; + +gb = [ 3.37077588e-3; + 6.62769080e-6; + 1.78718601e-8; + 5.49266312e-11]; + +gtu = [ 8.41275991e-4; + 7.67306686e-7; + 1.21291230e-9; + 2.48508228e-12]; + +utg = [-8.41276339e-4; + -5.95619298e-8; + -1.69485209e-10; + -2.20473896e-13]; + +%Ellipsoidal latitude, longitude to spherical latitude, longitude +neg_geo = 'FALSE'; + +if B < 0 + neg_geo = 'TRUE '; +end + +Bg_r = abs(B); +[res_clensin] = clsin(bg, 4, 2*Bg_r); +Bg_r = Bg_r + res_clensin; +L0 = L0*pi / 180; +Lg_r = L - L0; + +%Spherical latitude, longitude to complementary spherical latitude +% i.e. spherical N, E +cos_BN = cos(Bg_r); +Np = atan2(sin(Bg_r), cos(Lg_r)*cos_BN); +Ep = atanh(sin(Lg_r) * cos_BN); + +%Spherical normalized N, E to ellipsoidal N, E +Np = 2 * Np; +Ep = 2 * Ep; +[dN, dE] = clksin(gtu, 4, Np, Ep); +Np = Np/2; +Ep = Ep/2; +Np = Np + dN; +Ep = Ep + dE; +N = Q_n * Np; +E = Q_n*Ep + E0; + +if neg_geo == 'TRUE ' + N = -N + 20000000; +end; + +%%%%%%%%%%%%%%%%%%%% end cart2utm.m %%%%%%%%%%%%%%%%%%%% \ No newline at end of file diff --git a/GNSS_SDR_IQ/geoFunctions/check_t.m b/GNSS_SDR_IQ/geoFunctions/check_t.m new file mode 100644 index 0000000..2b94d84 --- /dev/null +++ b/GNSS_SDR_IQ/geoFunctions/check_t.m @@ -0,0 +1,28 @@ +function corrTime = check_t(time) +%CHECK_T accounting for beginning or end of week crossover. +% +%corrTime = check_t(time); +% +% Inputs: +% time - time in seconds +% +% Outputs: +% corrTime - corrected time (seconds) + +%Kai Borre 04-01-96 +%Copyright (c) by Kai Borre +% +% CVS record: +% $Id: check_t.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $ +%========================================================================== + +half_week = 302400; % seconds + +corrTime = time; + +if time > half_week + corrTime = time - 2*half_week; +elseif time < -half_week + corrTime = time + 2*half_week; +end +%%%%%%% end check_t.m %%%%%%%%%%%%%%%%% \ No newline at end of file diff --git a/GNSS_SDR_IQ/geoFunctions/clksin.m b/GNSS_SDR_IQ/geoFunctions/clksin.m new file mode 100644 index 0000000..40b98f1 --- /dev/null +++ b/GNSS_SDR_IQ/geoFunctions/clksin.m @@ -0,0 +1,38 @@ +function [re, im] = clksin(ar, degree, arg_real, arg_imag) +%Clenshaw summation of sinus with complex argument +%[re, im] = clksin(ar, degree, arg_real, arg_imag); + +% Written by Kai Borre +% December 20, 1995 +% +% See also WGS2UTM or CART2UTM +% +% CVS record: +% $Id: clksin.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $ +%========================================================================== + +sin_arg_r = sin(arg_real); +cos_arg_r = cos(arg_real); +sinh_arg_i = sinh(arg_imag); +cosh_arg_i = cosh(arg_imag); + +r = 2 * cos_arg_r * cosh_arg_i; +i =-2 * sin_arg_r * sinh_arg_i; + +hr1 = 0; hr = 0; hi1 = 0; hi = 0; + +for t = degree : -1 : 1 + hr2 = hr1; + hr1 = hr; + hi2 = hi1; + hi1 = hi; + z = ar(t) + r*hr1 - i*hi - hr2; + hi = i*hr1 + r*hi1 - hi2; + hr = z; +end + +r = sin_arg_r * cosh_arg_i; +i = cos_arg_r * sinh_arg_i; + +re = r*hr - i*hi; +im = r*hi + i*hr; diff --git a/GNSS_SDR_IQ/geoFunctions/clsin.m b/GNSS_SDR_IQ/geoFunctions/clsin.m new file mode 100644 index 0000000..549208d --- /dev/null +++ b/GNSS_SDR_IQ/geoFunctions/clsin.m @@ -0,0 +1,26 @@ +function result = clsin(ar, degree, argument) +%Clenshaw summation of sinus of argument. +% +%result = clsin(ar, degree, argument); + +% Written by Kai Borre +% December 20, 1995 +% +% See also WGS2UTM or CART2UTM +% +% CVS record: +% $Id: clsin.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $ +%========================================================================== + +cos_arg = 2 * cos(argument); +hr1 = 0; +hr = 0; + +for t = degree : -1 : 1 + hr2 = hr1; + hr1 = hr; + hr = ar(t) + cos_arg*hr1 - hr2; +end + +result = hr * sin(argument); +%%%%%%%%%%%%%%%%%%%%%%% end clsin.m %%%%%%%%%%%%%%%%%%%%% \ No newline at end of file diff --git a/GNSS_SDR_IQ/geoFunctions/deg2dms.m b/GNSS_SDR_IQ/geoFunctions/deg2dms.m new file mode 100644 index 0000000..d119329 --- /dev/null +++ b/GNSS_SDR_IQ/geoFunctions/deg2dms.m @@ -0,0 +1,43 @@ +function dmsOutput = deg2dms(deg) +%DEG2DMS Conversion of degrees to degrees, minutes, and seconds. +%The output format (dms format) is: (degrees*100 + minutes + seconds/100) + +% Written by Kai Borre +% February 7, 2001 +% Updated by Darius Plausinaitis + +%%% Save the sign for later processing +neg_arg = false; +if deg < 0 + % Only positive numbers should be used while spliting into deg/min/sec + deg = -deg; + neg_arg = true; +end + +%%% Split degrees minutes and seconds +int_deg = floor(deg); +decimal = deg - int_deg; +min_part = decimal*60; +min = floor(min_part); +sec_part = min_part - floor(min_part); +sec = sec_part*60; + +%%% Check for overflow +if sec == 60 + min = min + 1; + sec = 0; +end +if min == 60 + int_deg = int_deg + 1; + min = 0; +end + +%%% Construct the output +dmsOutput = int_deg * 100 + min + sec/100; + +%%% Correct the sign +if neg_arg == true + dmsOutput = -dmsOutput; +end + +%%%%%%%%%%%%%%%%%%% end deg2dms.m %%%%%%%%%%%%%%%% \ No newline at end of file diff --git a/GNSS_SDR_IQ/geoFunctions/dms2mat.m b/GNSS_SDR_IQ/geoFunctions/dms2mat.m new file mode 100644 index 0000000..6427d1b --- /dev/null +++ b/GNSS_SDR_IQ/geoFunctions/dms2mat.m @@ -0,0 +1,43 @@ +function matOutput = dms2mat(dmsInput,n) +%DMS2MAT Splits a real a = dd*100 + mm + s/100 into[dd mm s.ssss] +% where n specifies the power of 10, to which the resulting seconds +% of the output should be rounded. E.g.: if a result is 23.823476 +% seconds, and n = -3, then the output will be 23.823. + +% Written by Kai Borre +% January 7, 2007 +% Updated by Darius Plausinaitis + +neg_arg = false; +if dmsInput < 0 + % Only positive numbers should be used while spliting into deg/min/sec + dmsInput = -dmsInput; + neg_arg = true; +end + +%%% Split degrees minutes and seconds +int_deg = floor(dmsInput/100); +mm = floor(dmsInput - 100*int_deg); +%we assume n<7; hence %2.10f is sufficient to hold ssdec +ssdec = sprintf('%2.10f', (dmsInput-100*int_deg-mm)*100); + +%%% Check for overflow +if ssdec == 60 + mm = mm+1; + ssdec = 0; +end +if mm == 60 + int_deg = int_deg+1; + mm = 0; +end + +%%% Corect the sign +if neg_arg == true + int_deg = -int_deg; +end + +%%% Compose the output +matOutput(1) = int_deg; +matOutput(2) = mm; +matOutput(3) = str2double(ssdec(1:-n+3)); +%%%%%%%%%%%%%%%%%%% end dms2mat.m %%%%%%%%%%%%%%%% \ No newline at end of file diff --git a/GNSS_SDR_IQ/geoFunctions/e_r_corr.m b/GNSS_SDR_IQ/geoFunctions/e_r_corr.m new file mode 100644 index 0000000..8244b02 --- /dev/null +++ b/GNSS_SDR_IQ/geoFunctions/e_r_corr.m @@ -0,0 +1,34 @@ +function X_sat_rot = e_r_corr(traveltime, X_sat) +%E_R_CORR Returns rotated satellite ECEF coordinates due to Earth +%rotation during signal travel time +% +%X_sat_rot = e_r_corr(traveltime, X_sat); +% +% Inputs: +% travelTime - signal travel time +% X_sat - satellite's ECEF coordinates +% +% Outputs: +% X_sat_rot - rotated satellite's coordinates (ECEF) + +%Written by Kai Borre +%Copyright (c) by Kai Borre +% +% CVS record: +% $Id: e_r_corr.m,v 1.1.1.1.2.6 2006/08/22 13:45:59 dpl Exp $ +%========================================================================== + +Omegae_dot = 7.292115147e-5; % rad/sec + +%--- Find rotation angle -------------------------------------------------- +omegatau = Omegae_dot * traveltime; + +%--- Make a rotation matrix ----------------------------------------------- +R3 = [ cos(omegatau) sin(omegatau) 0; + -sin(omegatau) cos(omegatau) 0; + 0 0 1]; + +%--- Do the rotation ------------------------------------------------------ +X_sat_rot = R3 * X_sat; + +%%%%%%%% end e_r_corr.m %%%%%%%%%%%%%%%%%%%% \ No newline at end of file diff --git a/GNSS_SDR_IQ/geoFunctions/findUtmZone.m b/GNSS_SDR_IQ/geoFunctions/findUtmZone.m new file mode 100644 index 0000000..bdb071e --- /dev/null +++ b/GNSS_SDR_IQ/geoFunctions/findUtmZone.m @@ -0,0 +1,72 @@ +function utmZone = findUtmZone(latitude, longitude) +%Function finds the UTM zone number for given longitude and latitude. +%The longitude value must be between -180 (180 degree West) and 180 (180 +%degree East) degree. The latitude must be within -80 (80 degree South) and +%84 (84 degree North). +% +%utmZone = findUtmZone(latitude, longitude); +% +%Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not +%15 deg 30 min). + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%========================================================================== + +%CVS record: +%$Id: findUtmZone.m,v 1.1.2.2 2006/08/22 13:45:59 dpl Exp $ + +%% Check value bounds ===================================================== + +if ((longitude > 180) || (longitude < -180)) + error('Longitude value exceeds limits (-180:180).'); +end + +if ((latitude > 84) || (latitude < -80)) + error('Latitude value exceeds limits (-80:84).'); +end + +%% Find zone ============================================================== + +% Start at 180 deg west = -180 deg + +utmZone = fix((180 + longitude)/ 6) + 1; + +%% Correct zone numbers for particular areas ============================== + +if (latitude > 72) + % Corrections for zones 31 33 35 37 + if ((longitude >= 0) && (longitude < 9)) + utmZone = 31; + elseif ((longitude >= 9) && (longitude < 21)) + utmZone = 33; + elseif ((longitude >= 21) && (longitude < 33)) + utmZone = 35; + elseif ((longitude >= 33) && (longitude < 42)) + utmZone = 37; + end + +elseif ((latitude >= 56) && (latitude < 64)) + % Correction for zone 32 + if ((longitude >= 3) && (longitude < 12)) + utmZone = 32; + end +end \ No newline at end of file diff --git a/GNSS_SDR_IQ/geoFunctions/geo2cart.m b/GNSS_SDR_IQ/geoFunctions/geo2cart.m new file mode 100644 index 0000000..c6bcf13 --- /dev/null +++ b/GNSS_SDR_IQ/geoFunctions/geo2cart.m @@ -0,0 +1,48 @@ +function [X, Y, Z] = geo2cart(phi, lambda, h, i) +%GEO2CART Conversion of geographical coordinates (phi, lambda, h) to +%Cartesian coordinates (X, Y, Z). +% +%[X, Y, Z] = geo2cart(phi, lambda, h, i); +% +%Format for phi and lambda: [degrees minutes seconds]. +%h, X, Y, and Z are in meters. +% +%Choices i of Reference Ellipsoid +% 1. International Ellipsoid 1924 +% 2. International Ellipsoid 1967 +% 3. World Geodetic System 1972 +% 4. Geodetic Reference System 1980 +% 5. World Geodetic System 1984 +% +% Inputs: +% phi - geocentric latitude (format [degrees minutes seconds]) +% lambda - geocentric longitude (format [degrees minutes seconds]) +% h - height +% i - reference ellipsoid type +% +% Outputs: +% X, Y, Z - Cartesian coordinates (meters) + +%Kai Borre 10-13-98 +%Copyright (c) by Kai Borre +% +% CVS record: +% $Id: geo2cart.m,v 1.1.2.7 2006/08/22 13:45:59 dpl Exp $ +%========================================================================== + +b = phi(1) + phi(2)/60 + phi(3)/3600; +b = b*pi / 180; +l = lambda(1) + lambda(2)/60 + lambda(3)/3600; +l = l*pi / 180; + +a = [6378388 6378160 6378135 6378137 6378137]; +f = [1/297 1/298.247 1/298.26 1/298.257222101 1/298.257223563]; + +ex2 = (2-f(i))*f(i) / ((1-f(i))^2); +c = a(i) * sqrt(1+ex2); +N = c / sqrt(1 + ex2*cos(b)^2); + +X = (N+h) * cos(b) * cos(l); +Y = (N+h) * cos(b) * sin(l); +Z = ((1-f(i))^2*N + h) * sin(b); +%%%%%%%%%%%%%% end geo2cart.m %%%%%%%%%%%%%%%%%%%%%%%% diff --git a/GNSS_SDR_IQ/geoFunctions/leastSquarePos.m b/GNSS_SDR_IQ/geoFunctions/leastSquarePos.m new file mode 100644 index 0000000..c68e669 --- /dev/null +++ b/GNSS_SDR_IQ/geoFunctions/leastSquarePos.m @@ -0,0 +1,114 @@ +function [pos, el, az, dop] = leastSquarePos(satpos, obs, settings) +%Function calculates the Least Square Solution. +% +%[pos, el, az, dop] = leastSquarePos(satpos, obs, settings); +% +% Inputs: +% satpos - Satellites positions (in ECEF system: [X; Y; Z;] - +% one column per satellite) +% obs - Observations - the pseudorange measurements to each +% satellite: +% (e.g. [20000000 21000000 .... .... .... .... ....]) +% settings - receiver settings +% +% Outputs: +% pos - receiver position and receiver clock error +% (in ECEF system: [X, Y, Z, dt]) +% el - Satellites elevation angles (degrees) +% az - Satellites azimuth angles (degrees) +% dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP]) + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +%-------------------------------------------------------------------------- +%Based on Kai Borre +%Copyright (c) by Kai Borre +%Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen +% +% CVS record: +% $Id: leastSquarePos.m,v 1.1.2.12 2006/08/22 13:45:59 dpl Exp $ +%========================================================================== + +%=== Initialization ======================================================= +nmbOfIterations = 7; + +dtr = pi/180; +pos = zeros(4, 1); +X = satpos; +nmbOfSatellites = size(satpos, 2); + +A = zeros(nmbOfSatellites, 4); +omc = zeros(nmbOfSatellites, 1); +az = zeros(1, nmbOfSatellites); +el = az; + +%=== Iteratively find receiver position =================================== +for iter = 1:nmbOfIterations + + for i = 1:nmbOfSatellites + if iter == 1 + %--- Initialize variables at the first iteration -------------- + Rot_X = X(:, i); + trop = 2; + else + %--- Update equations ----------------------------------------- + rho2 = (X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ... + (X(3, i) - pos(3))^2; + traveltime = sqrt(rho2) / settings.c ; + + %--- Correct satellite position (do to earth rotation) -------- + Rot_X = e_r_corr(traveltime, X(:, i)); + + %--- Find the elevation angel of the satellite ---------------- + [az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :)); + + if (settings.useTropCorr == 1) + %--- Calculate tropospheric correction -------------------- + trop = tropo(sin(el(i) * dtr), ... + 0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); + else + % Do not calculate or apply the tropospheric corrections + trop = 0; + end + end % if iter == 1 ... ... else + + %--- Apply the corrections ---------------------------------------- + omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop); + + %--- Construct the A matrix --------------------------------------- + A(i, :) = [ (-(Rot_X(1) - pos(1))) / obs(i) ... + (-(Rot_X(2) - pos(2))) / obs(i) ... + (-(Rot_X(3) - pos(3))) / obs(i) ... + 1 ]; + end % for i = 1:nmbOfSatellites + + % These lines allow the code to exit gracefully in case of any errors + if rank(A) ~= 4 + pos = zeros(1, 4); + return + end + + %--- Find position update --------------------------------------------- + x = A \ omc; + + %--- Apply position update -------------------------------------------- + pos = pos + x; + +end % for iter = 1:nmbOfIterations + +pos = pos'; + +%=== Calculate Dilution Of Precision ====================================== +if nargout == 4 + %--- Initialize output ------------------------------------------------ + dop = zeros(1, 5); + + %--- Calculate DOP ---------------------------------------------------- + Q = inv(A'*A); + + dop(1) = sqrt(trace(Q)); % GDOP + dop(2) = sqrt(Q(1,1) + Q(2,2) + Q(3,3)); % PDOP + dop(3) = sqrt(Q(1,1) + Q(2,2)); % HDOP + dop(4) = sqrt(Q(3,3)); % VDOP + dop(5) = sqrt(Q(4,4)); % TDOP +end diff --git a/GNSS_SDR_IQ/geoFunctions/satpos.m b/GNSS_SDR_IQ/geoFunctions/satpos.m new file mode 100644 index 0000000..c473767 --- /dev/null +++ b/GNSS_SDR_IQ/geoFunctions/satpos.m @@ -0,0 +1,141 @@ +function [satPositions, satClkCorr] = satpos(transmitTime, prnList, ... + eph, settings) +%SATPOS Computation of satellite coordinates X,Y,Z at TRANSMITTIME for +%given ephemeris EPH. Coordinates are computed for each satellite in the +%list PRNLIST. +%[satPositions, satClkCorr] = satpos(transmitTime, prnList, eph, settings); +% +% Inputs: +% transmitTime - transmission time +% prnList - list of PRN-s to be processed +% eph - ephemerides of satellites +% settings - receiver settings +% +% Outputs: +% satPositions - position of satellites (in ECEF system [X; Y; Z;]) +% satClkCorr - correction of satellite clocks + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +%-------------------------------------------------------------------------- +%Based on Kai Borre 04-09-96 +%Copyright (c) by Kai Borre +%Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen +% +% CVS record: +% $Id: satpos.m,v 1.1.2.17 2007/01/30 09:45:12 dpl Exp $ + +%% Initialize constants =================================================== +numOfSatellites = size(prnList, 2); + +% GPS constatns + +gpsPi = 3.1415926535898; % Pi used in the GPS coordinate + % system + +%--- Constants for satellite position calculation ------------------------- +Omegae_dot = 7.2921151467e-5; % Earth rotation rate, [rad/s] +GM = 3.986005e14; % Universal gravitational constant times + % the mass of the Earth, [m^3/s^2] +F = -4.442807633e-10; % Constant, [sec/(meter)^(1/2)] + +%% Initialize results ===================================================== +satClkCorr = zeros(1, numOfSatellites); +satPositions = zeros(3, numOfSatellites); + +%% Process each satellite ================================================= + +for satNr = 1 : numOfSatellites + + prn = prnList(satNr); + +%% Find initial satellite clock correction -------------------------------- + + %--- Find time difference --------------------------------------------- + dt = check_t(transmitTime - eph(prn).t_oc); + + %--- Calculate clock correction --------------------------------------- + satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ... + eph(prn).a_f0 - ... + eph(prn).T_GD; + + time = transmitTime - satClkCorr(satNr); + +%% Find satellite's position ---------------------------------------------- + + %Restore semi-major axis + a = eph(prn).sqrtA * eph(prn).sqrtA; + + %Time correction + tk = check_t(time - eph(prn).t_oe); + + %Initial mean motion + n0 = sqrt(GM / a^3); + %Mean motion + n = n0 + eph(prn).deltan; + + %Mean anomaly + M = eph(prn).M_0 + n * tk; + %Reduce mean anomaly to between 0 and 360 deg + M = rem(M + 2*gpsPi, 2*gpsPi); + + %Initial guess of eccentric anomaly + E = M; + + %--- Iteratively compute eccentric anomaly ---------------------------- + for ii = 1:10 + E_old = E; + E = M + eph(prn).e * sin(E); + dE = rem(E - E_old, 2*gpsPi); + + if abs(dE) < 1.e-12 + % Necessary precision is reached, exit from the loop + break; + end + end + + %Reduce eccentric anomaly to between 0 and 360 deg + E = rem(E + 2*gpsPi, 2*gpsPi); + + %Compute relativistic correction term + dtr = F * eph(prn).e * eph(prn).sqrtA * sin(E); + + %Calculate the true anomaly + nu = atan2(sqrt(1 - eph(prn).e^2) * sin(E), cos(E)-eph(prn).e); + + %Compute angle phi + phi = nu + eph(prn).omega; + %Reduce phi to between 0 and 360 deg + phi = rem(phi, 2*gpsPi); + + %Correct argument of latitude + u = phi + ... + eph(prn).C_uc * cos(2*phi) + ... + eph(prn).C_us * sin(2*phi); + %Correct radius + r = a * (1 - eph(prn).e*cos(E)) + ... + eph(prn).C_rc * cos(2*phi) + ... + eph(prn).C_rs * sin(2*phi); + %Correct inclination + i = eph(prn).i_0 + eph(prn).iDot * tk + ... + eph(prn).C_ic * cos(2*phi) + ... + eph(prn).C_is * sin(2*phi); + + %Compute the angle between the ascending node and the Greenwich meridian + Omega = eph(prn).omega_0 + (eph(prn).omegaDot - Omegae_dot)*tk - ... + Omegae_dot * eph(prn).t_oe; + %Reduce to between 0 and 360 deg + Omega = rem(Omega + 2*gpsPi, 2*gpsPi); + + %--- Compute satellite coordinates ------------------------------------ + satPositions(1, satNr) = cos(u)*r * cos(Omega) - sin(u)*r * cos(i)*sin(Omega); + satPositions(2, satNr) = cos(u)*r * sin(Omega) + sin(u)*r * cos(i)*cos(Omega); + satPositions(3, satNr) = sin(u)*r * sin(i); + + +%% Include relativistic correction in clock correction -------------------- + satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ... + eph(prn).a_f0 - ... + eph(prn).T_GD + dtr; + +end % for satNr = 1 : numOfSatellites diff --git a/GNSS_SDR_IQ/geoFunctions/togeod.m b/GNSS_SDR_IQ/geoFunctions/togeod.m new file mode 100644 index 0000000..9b94580 --- /dev/null +++ b/GNSS_SDR_IQ/geoFunctions/togeod.m @@ -0,0 +1,112 @@ +function [dphi, dlambda, h] = togeod(a, finv, X, Y, Z) +%TOGEOD Subroutine to calculate geodetic coordinates latitude, longitude, +% height given Cartesian coordinates X,Y,Z, and reference ellipsoid +% values semi-major axis (a) and the inverse of flattening (finv). +% +%[dphi, dlambda, h] = togeod(a, finv, X, Y, Z); +% +% The units of linear parameters X,Y,Z,a must all agree (m,km,mi,ft,..etc) +% The output units of angular quantities will be in decimal degrees +% (15.5 degrees not 15 deg 30 min). The output units of h will be the +% same as the units of X,Y,Z,a. +% +% Inputs: +% a - semi-major axis of the reference ellipsoid +% finv - inverse of flattening of the reference ellipsoid +% X,Y,Z - Cartesian coordinates +% +% Outputs: +% dphi - latitude +% dlambda - longitude +% h - height above reference ellipsoid + +% Copyright (C) 1987 C. Goad, Columbus, Ohio +% Reprinted with permission of author, 1996 +% Fortran code translated into MATLAB +% Kai Borre 03-30-96 +% +% CVS record: +% $Id: togeod.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $ +%========================================================================== + +h = 0; +tolsq = 1.e-10; +maxit = 10; + +% compute radians-to-degree factor +rtd = 180/pi; + +% compute square of eccentricity +if finv < 1.e-20 + esq = 0; +else + esq = (2 - 1/finv) / finv; +end + +oneesq = 1 - esq; + +% first guess +% P is distance from spin axis +P = sqrt(X^2+Y^2); +% direct calculation of longitude + +if P > 1.e-20 + dlambda = atan2(Y,X) * rtd; +else + dlambda = 0; +end + +if (dlambda < 0) + dlambda = dlambda + 360; +end + +% r is distance from origin (0,0,0) +r = sqrt(P^2 + Z^2); + +if r > 1.e-20 + sinphi = Z/r; +else + sinphi = 0; +end + +dphi = asin(sinphi); + +% initial value of height = distance from origin minus +% approximate distance from origin to surface of ellipsoid +if r < 1.e-20 + h = 0; + return +end + +h = r - a*(1-sinphi*sinphi/finv); + +% iterate +for i = 1:maxit + sinphi = sin(dphi); + cosphi = cos(dphi); + + % compute radius of curvature in prime vertical direction + N_phi = a/sqrt(1-esq*sinphi*sinphi); + + % compute residuals in P and Z + dP = P - (N_phi + h) * cosphi; + dZ = Z - (N_phi*oneesq + h) * sinphi; + + % update height and latitude + h = h + (sinphi*dZ + cosphi*dP); + dphi = dphi + (cosphi*dZ - sinphi*dP)/(N_phi + h); + + % test for convergence + if (dP*dP + dZ*dZ < tolsq) + break; + end + + % Not Converged--Warn user + if i == maxit + fprintf([' Problem in TOGEOD, did not converge in %2.0f',... + ' iterations\n'], i); + end +end % for i = 1:maxit + +dphi = dphi * rtd; +%%%%%%%% end togeod.m %%%%%%%%%%%%%%%%%%%%%% diff --git a/GNSS_SDR_IQ/geoFunctions/topocent.m b/GNSS_SDR_IQ/geoFunctions/topocent.m new file mode 100644 index 0000000..343f2c8 --- /dev/null +++ b/GNSS_SDR_IQ/geoFunctions/topocent.m @@ -0,0 +1,57 @@ +function [Az, El, D] = topocent(X, dx) +%TOPOCENT Transformation of vector dx into topocentric coordinate +% system with origin at X. +% Both parameters are 3 by 1 vectors. +% +%[Az, El, D] = topocent(X, dx); +% +% Inputs: +% X - vector origin corrdinates (in ECEF system [X; Y; Z;]) +% dx - vector ([dX; dY; dZ;]). +% +% Outputs: +% D - vector length. Units like units of the input +% Az - azimuth from north positive clockwise, degrees +% El - elevation angle, degrees + +%Kai Borre 11-24-96 +%Copyright (c) by Kai Borre +% +% CVS record: +% $Id: topocent.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $ +%========================================================================== + +dtr = pi/180; + +[phi, lambda, h] = togeod(6378137, 298.257223563, X(1), X(2), X(3)); + +cl = cos(lambda * dtr); +sl = sin(lambda * dtr); +cb = cos(phi * dtr); +sb = sin(phi * dtr); + +F = [-sl -sb*cl cb*cl; + cl -sb*sl cb*sl; + 0 cb sb]; + +local_vector = F' * dx; +E = local_vector(1); +N = local_vector(2); +U = local_vector(3); + +hor_dis = sqrt(E^2 + N^2); + +if hor_dis < 1.e-20 + Az = 0; + El = 90; +else + Az = atan2(E, N)/dtr; + El = atan2(U, hor_dis)/dtr; +end + +if Az < 0 + Az = Az + 360; +end + +D = sqrt(dx(1)^2 + dx(2)^2 + dx(3)^2); +%%%%%%%%% end topocent.m %%%%%%%%% \ No newline at end of file diff --git a/GNSS_SDR_IQ/geoFunctions/tropo.m b/GNSS_SDR_IQ/geoFunctions/tropo.m new file mode 100644 index 0000000..46d89be --- /dev/null +++ b/GNSS_SDR_IQ/geoFunctions/tropo.m @@ -0,0 +1,98 @@ +function ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum) +%TROPO Calculation of tropospheric correction. +% The range correction ddr in m is to be subtracted from +% pseudo-ranges and carrier phases +% +%ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum); +% +% Inputs: +% sinel - sin of elevation angle of satellite +% hsta - height of station in km +% p - atmospheric pressure in mb at height hp +% tkel - surface temperature in degrees Kelvin at height htkel +% hum - humidity in % at height hhum +% hp - height of pressure measurement in km +% htkel - height of temperature measurement in km +% hhum - height of humidity measurement in km +% +% Outputs: +% ddr - range correction (meters) +% +% Reference +% Goad, C.C. & Goodman, L. (1974) A Modified Tropospheric +% Refraction Correction Model. Paper presented at the +% American Geophysical Union Annual Fall Meeting, San +% Francisco, December 12-17 + +% A Matlab reimplementation of a C code from driver. +% Kai Borre 06-28-95 +% +% CVS record: +% $Id: tropo.m,v 1.1.1.1.2.4 2006/08/22 13:46:00 dpl Exp $ +%========================================================================== + +a_e = 6378.137; % semi-major axis of earth ellipsoid +b0 = 7.839257e-5; +tlapse = -6.5; +tkhum = tkel + tlapse*(hhum-htkel); +atkel = 7.5*(tkhum-273.15) / (237.3+tkhum-273.15); +e0 = 0.0611 * hum * 10^atkel; +tksea = tkel - tlapse*htkel; +em = -978.77 / (2.8704e6*tlapse*1.0e-5); +tkelh = tksea + tlapse*hhum; +e0sea = e0 * (tksea/tkelh)^(4*em); +tkelp = tksea + tlapse*hp; +psea = p * (tksea/tkelp)^em; + +if sinel < 0 + sinel = 0; +end + +tropo = 0; +done = 'FALSE'; +refsea = 77.624e-6 / tksea; +htop = 1.1385e-5 / refsea; +refsea = refsea * psea; +ref = refsea * ((htop-hsta)/htop)^4; + +while 1 + rtop = (a_e+htop)^2 - (a_e+hsta)^2*(1-sinel^2); + + % check to see if geometry is crazy + if rtop < 0 + rtop = 0; + end + + rtop = sqrt(rtop) - (a_e+hsta)*sinel; + a = -sinel/(htop-hsta); + b = -b0*(1-sinel^2) / (htop-hsta); + rn = zeros(8,1); + + for i = 1:8 + rn(i) = rtop^(i+1); + end + + alpha = [2*a, 2*a^2+4*b/3, a*(a^2+3*b),... + a^4/5+2.4*a^2*b+1.2*b^2, 2*a*b*(a^2+3*b)/3,... + b^2*(6*a^2+4*b)*1.428571e-1, 0, 0]; + + if b^2 > 1.0e-35 + alpha(7) = a*b^3/2; + alpha(8) = b^4/9; + end + + dr = rtop; + dr = dr + alpha*rn; + tropo = tropo + dr*ref*1000; + + if done == 'TRUE ' + ddr = tropo; + break; + end + + done = 'TRUE '; + refsea = (371900.0e-6/tksea-12.92e-6)/tksea; + htop = 1.1385e-5 * (1255/tksea+0.05)/refsea; + ref = refsea * e0sea * ((htop-hsta)/htop)^4; +end; +%%%%%%%%% end tropo.m %%%%%%%%%%%%%%%%%%% diff --git a/GNSS_SDR_IQ/include/calcLoopCoef.m b/GNSS_SDR_IQ/include/calcLoopCoef.m new file mode 100644 index 0000000..f3e82ba --- /dev/null +++ b/GNSS_SDR_IQ/include/calcLoopCoef.m @@ -0,0 +1,45 @@ +function [tau1, tau2] = calcLoopCoef(LBW, zeta, k) +%Function finds loop coefficients. The coefficients are used then in PLL-s +%and DLL-s. +% +%[tau1, tau2] = calcLoopCoef(LBW, zeta, k) +% +% Inputs: +% LBW - Loop noise bandwidth +% zeta - Damping ratio +% k - Loop gain +% +% Outputs: +% tau1, tau2 - Loop filter coefficients + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis and Dennis M. Akos +% Written by Darius Plausinaitis and Dennis M. Akos +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: calcLoopCoef.m,v 1.1.2.2 2006/08/14 11:38:22 dpl Exp $ + +% Solve natural frequency +Wn = LBW*8*zeta / (4*zeta.^2 + 1); + +% solve for t1 & t2 +tau1 = k / (Wn * Wn); +tau2 = 2.0 * zeta / Wn; diff --git a/GNSS_SDR_IQ/include/checkPhase.m b/GNSS_SDR_IQ/include/checkPhase.m new file mode 100644 index 0000000..5bb3a0a --- /dev/null +++ b/GNSS_SDR_IQ/include/checkPhase.m @@ -0,0 +1,46 @@ +function word = checkPhase(word, D30Star) +%Checks the parity of the supplied 30bit word. +%The last parity bit of the previous word is used for the calculation. +%A note on the procedure is supplied by the GPS standard positioning +%service signal specification. +% +%word = checkPhase(word, D30Star) +% +% Inputs: +% word - an array with 30 bit long word from the navigation +% message (a character array, must contain only '0' or +% '1'). +% D30Star - the last bit of the previous word (char type). +% +% Outputs: +% word - word with corrected polarity of the data bits +% (character array). + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Written by Darius Plausinaitis and Dennis M. Akos +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% CVS record: +% $Id: checkPhase.m,v 1.1.2.4 2006/08/14 11:38:22 dpl Exp $ + +if D30Star == '1' + % Data bits must be inverted + word(1:24) = invert(word(1:24)); +end diff --git a/GNSS_SDR_IQ/include/ephemeris.m b/GNSS_SDR_IQ/include/ephemeris.m new file mode 100644 index 0000000..45cc4e6 --- /dev/null +++ b/GNSS_SDR_IQ/include/ephemeris.m @@ -0,0 +1,159 @@ +function [eph, TOW] = ephemeris(bits, D30Star) +%Function decodes ephemerides and TOW from the given bit stream. The stream +%(array) in the parameter BITS must contain 1500 bits. The first element in +%the array must be the first bit of a subframe. The subframe ID of the +%first subframe in the array is not important. +% +%Function does not check parity! +% +%[eph, TOW] = ephemeris(bits, D30Star) +% +% Inputs: +% bits - bits of the navigation messages (5 subframes). +% Type is character array and it must contain only +% characters '0' or '1'. +% D30Star - The last bit of the previous nav-word. Refer to the +% GPS interface control document ICD (IS-GPS-200D) for +% more details on the parity checking. Parameter type is +% char. It must contain only characters '0' or '1'. +% Outputs: +% TOW - Time Of Week (TOW) of the first sub-frame in the bit +% stream (in seconds) +% eph - SV ephemeris + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis and Kristin Larson +% Written by Darius Plausinaitis and Kristin Larson +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: ephemeris.m,v 1.1.2.7 2006/08/14 11:38:22 dpl Exp $ + + +%% Check if there is enough data ========================================== +if length(bits) < 1500 + error('The parameter BITS must contain 1500 bits!'); +end + +%% Check if the parameters are strings ==================================== +if ~ischar(bits) + error('The parameter BITS must be a character array!'); +end + +if ~ischar(D30Star) + error('The parameter D30Star must be a char!'); +end + +% Pi used in the GPS coordinate system +gpsPi = 3.1415926535898; +eph=struct('weekNumber',[],'accuracy',[],'health',[],'T_GD',[],'IODC',[],'t_oc',[],'a_f2',[],'a_f1',[],'a_f0',[],... + 'IODE_sf2',[],'C_rs',[],'deltan',[],'M_0',[],'C_uc',[],'e',[],'C_us',[],'sqrtA',[],'t_oe',[],... + 'C_ic',[],'omega_0',[],'C_is',[],'i_0',[],'C_rc',[],'omega',[],'omegaDot',[],'IODE_sf3',[],'iDot',[]); +%% Decode all 5 sub-frames ================================================ +for i = 1:5 + + %--- "Cut" one sub-frame's bits --------------------------------------- + subframe = bits(300*(i-1)+1 : 300*i); + + %--- Correct polarity of the data bits in all 10 words ---------------- + for j = 1:10 + [subframe(30*(j-1)+1 : 30*j)] = ... + checkPhase(subframe(30*(j-1)+1 : 30*j), D30Star); + + D30Star = subframe(30*j); + end + + %--- Decode the sub-frame id ------------------------------------------ + % For more details on sub-frame contents please refer to GPS IS. + subframeID = bin2dec(subframe(50:52)); + + %--- Decode sub-frame based on the sub-frames id ---------------------- + % The task is to select the necessary bits and convert them to decimal + % numbers. For more details on sub-frame contents please refer to GPS + % ICD (IS-GPS-200D). + switch subframeID + case 1 %--- It is subframe 1 ------------------------------------- + % It contains WN, SV clock corrections, health and accuracy + eph.weekNumber = bin2dec(subframe(61:70)) + 1024; + eph.accuracy = bin2dec(subframe(73:76)); + eph.health = bin2dec(subframe(77:82)); + eph.T_GD = twosComp2dec(subframe(197:204)) * 2^(-31); + eph.IODC = bin2dec([subframe(83:84) subframe(211:218)]); + eph.t_oc = bin2dec(subframe(219:234)) * 2^4; + eph.a_f2 = twosComp2dec(subframe(241:248)) * 2^(-55); + eph.a_f1 = twosComp2dec(subframe(249:264)) * 2^(-43); + eph.a_f0 = twosComp2dec(subframe(271:292)) * 2^(-31); + + case 2 %--- It is subframe 2 ------------------------------------- + % It contains first part of ephemeris parameters + eph.IODE_sf2 = bin2dec(subframe(61:68)); + eph.C_rs = twosComp2dec(subframe(69: 84)) * 2^(-5); + eph.deltan = ... + twosComp2dec(subframe(91:106)) * 2^(-43) * gpsPi; + eph.M_0 = ... + twosComp2dec([subframe(107:114) subframe(121:144)]) ... + * 2^(-31) * gpsPi; + eph.C_uc = twosComp2dec(subframe(151:166)) * 2^(-29); + eph.e = ... + bin2dec([subframe(167:174) subframe(181:204)]) ... + * 2^(-33); + eph.C_us = twosComp2dec(subframe(211:226)) * 2^(-29); + eph.sqrtA = ... + bin2dec([subframe(227:234) subframe(241:264)]) ... + * 2^(-19); + eph.t_oe = bin2dec(subframe(271:286)) * 2^4; + + case 3 %--- It is subframe 3 ------------------------------------- + % It contains second part of ephemeris parameters + eph.C_ic = twosComp2dec(subframe(61:76)) * 2^(-29); + eph.omega_0 = ... + twosComp2dec([subframe(77:84) subframe(91:114)]) ... + * 2^(-31) * gpsPi; + eph.C_is = twosComp2dec(subframe(121:136)) * 2^(-29); + eph.i_0 = ... + twosComp2dec([subframe(137:144) subframe(151:174)]) ... + * 2^(-31) * gpsPi; + eph.C_rc = twosComp2dec(subframe(181:196)) * 2^(-5); + eph.omega = ... + twosComp2dec([subframe(197:204) subframe(211:234)]) ... + * 2^(-31) * gpsPi; + eph.omegaDot = twosComp2dec(subframe(241:264)) * 2^(-43) * gpsPi; + eph.IODE_sf3 = bin2dec(subframe(271:278)); + eph.iDot = twosComp2dec(subframe(279:292)) * 2^(-43) * gpsPi; + + case 4 %--- It is subframe 4 ------------------------------------- + % Almanac, ionospheric model, UTC parameters. + % SV health (PRN: 25-32). + % Not decoded at the moment. + + case 5 %--- It is subframe 5 ------------------------------------- + % SV almanac and health (PRN: 1-24). + % Almanac reference week number and time. + % Not decoded at the moment. + + end % switch subframeID ... + +end % for all 5 sub-frames ... + +%% Compute the time of week (TOW) of the first sub-frames in the array ==== +% Also correct the TOW. The transmitted TOW is actual TOW of the next +% subframe and we need the TOW of the first subframe in this data block +% (the variable subframe at this point contains bits of the last subframe). +TOW = bin2dec(subframe(31:47)) * 6 - 30; diff --git a/GNSS_SDR_IQ/include/generateCAcode.m b/GNSS_SDR_IQ/include/generateCAcode.m new file mode 100644 index 0000000..86f79f1 --- /dev/null +++ b/GNSS_SDR_IQ/include/generateCAcode.m @@ -0,0 +1,90 @@ +function CAcode = generateCAcode(PRN) +% generateCAcode.m generates one of the 32 GPS satellite C/A codes. +% +% CAcode = generateCAcode(PRN) +% +% Inputs: +% PRN - PRN number of the sequence. +% +% Outputs: +% CAcode - a vector containing the desired C/A code sequence +% (chips). + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +% Based on Dennis M. Akos, Peter Rinder and Nicolaj Bertelsen +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: generateCAcode.m,v 1.1.2.5 2006/08/14 11:38:22 dpl Exp $ + +%--- Make the code shift array. The shift depends on the PRN number ------- +% The g2s vector holds the appropriate shift of the g2 code to generate +% the C/A code (ex. for SV#19 - use a G2 shift of g2s(19) = 471) +g2s = [ 5, 6, 7, 8, 17, 18, 139, 140, 141, 251, ... + 252, 254, 255, 256, 257, 258, 469, 470, 471, 472, ... + 473, 474, 509, 512, 513, 514, 515, 516, 859, 860, ... + 861, 862 ... end of shifts for GPS satellites + ... Shifts for the ground GPS transmitter are not included + ... Shifts for EGNOS and WAAS satellites (true_PRN = PRN + 87) + 145, 175, 52, 21, 237, 235, 886, 657, ... + 634, 762, 355, 1012, 176, 603, 130, 359, 595, 68, ... + 386]; + +%--- Pick right shift for the given PRN number ---------------------------- +g2shift = g2s(PRN); + +%--- Generate G1 code ----------------------------------------------------- + +%--- Initialize g1 output to speed up the function --- +g1 = zeros(1, 1023); +%--- Load shift register --- +reg = -1*ones(1, 10); + +%--- Generate all G1 signal chips based on the G1 feedback polynomial ----- +for i=1:1023 + g1(i) = reg(10); + saveBit = reg(3)*reg(10); + reg(2:10) = reg(1:9); + reg(1) = saveBit; +end + +%--- Generate G2 code ----------------------------------------------------- + +%--- Initialize g2 output to speed up the function --- +g2 = zeros(1, 1023); +%--- Load shift register --- +reg = -1*ones(1, 10); + +%--- Generate all G2 signal chips based on the G2 feedback polynomial ----- +for i=1:1023 + g2(i) = reg(10); + saveBit = reg(2)*reg(3)*reg(6)*reg(8)*reg(9)*reg(10); + reg(2:10) = reg(1:9); + reg(1) = saveBit; +end + +%--- Shift G2 code -------------------------------------------------------- +%The idea: g2 = concatenate[ g2_right_part, g2_left_part ]; +g2 = [g2(1023-g2shift+1 : 1023), g2(1 : 1023-g2shift)]; + +%--- Form single sample C/A code by multiplying G1 and G2 ----------------- +CAcode = -(g1 .* g2); diff --git a/GNSS_SDR_IQ/include/invert.m b/GNSS_SDR_IQ/include/invert.m new file mode 100644 index 0000000..41e50c8 --- /dev/null +++ b/GNSS_SDR_IQ/include/invert.m @@ -0,0 +1,35 @@ +function result = invert(data) +% Inverts the binary input-string so that 0 becomes 1 and 1 becomes 0. +% +%result = invert(data) + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Written by Darius Plausinaitis, Kristin Larson and Dennis M. Akos +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% CVS record: +% $Id: invert.m,v 1.1.2.4 2006/08/14 11:38:22 dpl Exp $ + +dataLength = length(data); +temp(1:dataLength) = '1'; + +invertMask = bin2dec(char(temp)); + +result = dec2bin(bitxor(bin2dec(data), invertMask), dataLength); \ No newline at end of file diff --git a/GNSS_SDR_IQ/include/makeCaTable.m b/GNSS_SDR_IQ/include/makeCaTable.m new file mode 100644 index 0000000..3377427 --- /dev/null +++ b/GNSS_SDR_IQ/include/makeCaTable.m @@ -0,0 +1,74 @@ +function caCodesTable = makeCaTable(settings) +%Function generates CA codes for all 32 satellites based on the settings +%provided in the structure "settings". The codes are digitized at the +%sampling frequency specified in the settings structure. +%One row in the "caCodesTable" is one C/A code. The row number is the PRN +%number of the C/A code. +% +%caCodesTable = makeCaTable(settings) +% +% Inputs: +% settings - receiver settings +% Outputs: +% caCodesTable - an array of arrays (matrix) containing C/A codes +% for all satellite PRN-s + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +% Based on Peter Rinder and Nicolaj Bertelsen +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: makeCaTable.m,v 1.1.2.6 2006/08/14 11:38:22 dpl Exp $ + +%--- Find number of samples per spreading code ---------------------------- +samplesPerCode = round(settings.samplingFreq / ... + (settings.codeFreqBasis / settings.codeLength)); + +%--- Prepare the output matrix to speed up function ----------------------- +caCodesTable = zeros(32, samplesPerCode); + +%--- Find time constants -------------------------------------------------- +ts = 1/settings.samplingFreq; % Sampling period in sec +tc = 1/settings.codeFreqBasis; % C/A chip period in sec + +%=== For all satellite PRN-s ... +for PRN = 1:32 + %--- Generate CA code for given PRN ----------------------------------- + caCode = generateCAcode(PRN); + + %=== Digitizing ======================================================= + + %--- Make index array to read C/A code values ------------------------- + % The length of the index array depends on the sampling frequency - + % number of samples per millisecond (because one C/A code period is one + % millisecond). + codeValueIndex = ceil((ts * (1:samplesPerCode)) / tc); + + %--- Correct the last index (due to number rounding issues) ----------- + codeValueIndex(end) = 1023; + + %--- Make the digitized version of the C/A code ----------------------- + % The "upsampled" code is made by selecting values form the CA code + % chip array (caCode) for the time instances of each sample. + caCodesTable(PRN, :) = caCode(codeValueIndex); + +end % for PRN = 1:32 diff --git a/GNSS_SDR_IQ/include/navPartyChk.m b/GNSS_SDR_IQ/include/navPartyChk.m new file mode 100644 index 0000000..59071fd --- /dev/null +++ b/GNSS_SDR_IQ/include/navPartyChk.m @@ -0,0 +1,103 @@ +function status = navPartyChk(ndat) +% This function is called to compute and status the parity bits on GPS word. +% Based on the flowchart in Figure 2-10 in the 2nd Edition of the GPS-SPS +% Signal Spec. +% +%status = navPartyChk(ndat) +% +% Inputs: +% ndat - an array (1x32) of 32 bits represent a GPS navigation +% word which is 30 bits plus two previous bits used in +% the parity calculation (-2 -1 0 1 2 ... 28 29) +% +% Outputs: +% status - the test value which equals EITHER +1 or -1 if parity +% PASSED or 0 if parity fails. The +1 means bits #1-24 +% of the current word have the correct polarity, while -1 +% means the bits #1-24 of the current word must be +% inverted. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Written by Darius Plausinaitis, Kristin Larson +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% CVS record: +% $Id: navPartyChk.m,v 1.1.2.5 2006/08/14 11:38:22 dpl Exp $ + +% In order to accomplish the exclusive or operation using multiplication +% this program represents a '0' with a '-1' and a '1' with a '1' so that +% the exclusive or table holds true for common data operations +% +% a b xor a b product +% -------------- ----------------- +% 0 0 1 -1 -1 1 +% 0 1 0 -1 1 -1 +% 1 0 0 1 -1 -1 +% 1 1 1 1 1 1 + +%--- Check if the data bits must be inverted ------------------------------ +if (ndat(2) ~= 1) + ndat(3:26)= -1 .* ndat(3:26); % Also could just negate +end + +%--- Calculate 6 parity bits ---------------------------------------------- +% The elements of the ndat array correspond to the bits showed in the table +% 20-XIV (ICD-200C document) in the following way: +% The first element in the ndat is the D29* bit and the second - D30*. +% The elements 3 - 26 are bits d1-d24 in the table. +% The elements 27 - 32 in the ndat array are the received bits D25-D30. +% The array "parity" contains the computed D25-D30 (parity) bits. + +parity(1) = ndat(1) * ndat(3) * ndat(4) * ndat(5) * ndat(7) * ... + ndat(8) * ndat(12) * ndat(13) * ndat(14) * ndat(15) * ... + ndat(16) * ndat(19) * ndat(20) * ndat(22) * ndat(25); + +parity(2) = ndat(2) * ndat(4) * ndat(5) * ndat(6) * ndat(8) * ... + ndat(9) * ndat(13) * ndat(14) * ndat(15) * ndat(16) * ... + ndat(17) * ndat(20) * ndat(21) * ndat(23) * ndat(26); + +parity(3) = ndat(1) * ndat(3) * ndat(5) * ndat(6) * ndat(7) * ... + ndat(9) * ndat(10) * ndat(14) * ndat(15) * ndat(16) * ... + ndat(17) * ndat(18) * ndat(21) * ndat(22) * ndat(24); + +parity(4) = ndat(2) * ndat(4) * ndat(6) * ndat(7) * ndat(8) * ... + ndat(10) * ndat(11) * ndat(15) * ndat(16) * ndat(17) * ... + ndat(18) * ndat(19) * ndat(22) * ndat(23) * ndat(25); + +parity(5) = ndat(2) * ndat(3) * ndat(5) * ndat(7) * ndat(8) * ... + ndat(9) * ndat(11) * ndat(12) * ndat(16) * ndat(17) * ... + ndat(18) * ndat(19) * ndat(20) * ndat(23) * ndat(24) * ... + ndat(26); + +parity(6) = ndat(1) * ndat(5) * ndat(7) * ndat(8) * ndat(10) * ... + ndat(11) * ndat(12) * ndat(13) * ndat(15) * ndat(17) * ... + ndat(21) * ndat(24) * ndat(25) * ndat(26); + +%--- Compare if the received parity is equal the calculated parity -------- +if ((sum(parity == ndat(27:32))) == 6) + + % Parity is OK. Function output is -1 or 1 depending if the data bits + % must be inverted or not. The "ndat(2)" is D30* bit - the last bit of + % previous subframe. + status = -1 * ndat(2); +else + % Parity failure + status = 0; +end diff --git a/GNSS_SDR_IQ/include/preRun.m b/GNSS_SDR_IQ/include/preRun.m new file mode 100644 index 0000000..1f09762 --- /dev/null +++ b/GNSS_SDR_IQ/include/preRun.m @@ -0,0 +1,73 @@ +function [channel] = preRun(acqResults, settings) +%Function initializes tracking channels from acquisition data. The acquired +%signals are sorted according to the signal strength. This function can be +%modified to use other satellite selection algorithms or to introduce +%acquired signal properties offsets for testing purposes. +% +%[channel] = preRun(acqResults, settings) +% +% Inputs: +% acqResults - results from acquisition. +% settings - receiver settings +% +% Outputs: +% channel - structure contains information for each channel (like +% properties of the tracked signal, channel status etc.). + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +% Based on Peter Rinder and Nicolaj Bertelsen +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: preRun.m,v 1.8.2.20 2006/08/14 11:38:22 dpl Exp $ + +%% Initialize all channels ================================================ +channel = []; % Clear, create the structure + +channel.PRN = 0; % PRN number of the tracked satellite +channel.acquiredFreq = 0; % Used as the center frequency of the NCO +channel.codePhase = 0; % Position of the C/A start + +channel.status = '-'; % Mode/status of the tracking channel + % "-" - "off" - no signal to track + % "T" - Tracking state + +%--- Copy initial data to all channels ------------------------------------ +channel = repmat(channel, 1, settings.numberOfChannels); + +%% Copy acquisition results =============================================== + +%--- Sort peaks to find strongest signals, keep the peak index information +[junk, PRNindexes] = sort(acqResults.peakMetric, 2, 'descend'); + +%--- Load information about each satellite -------------------------------- +% Maximum number of initialized channels is number of detected signals, but +% not more as the number of channels specified in the settings. +%for ii = 1:min([settings.numberOfChannels, sum(acqResults.carrFreq > 0)]) +for ii = 1:settings.numberOfChannels + channel(ii).PRN = PRNindexes(ii); + channel(ii).acquiredFreq = acqResults.carrFreq(PRNindexes(ii)); + channel(ii).codePhase = acqResults.codePhase(PRNindexes(ii)); + + % Set tracking into mode (there can be more modes if needed e.g. pull-in) + channel(ii).status = 'T'; +end diff --git a/GNSS_SDR_IQ/include/showChannelStatus.m b/GNSS_SDR_IQ/include/showChannelStatus.m new file mode 100644 index 0000000..9640e74 --- /dev/null +++ b/GNSS_SDR_IQ/include/showChannelStatus.m @@ -0,0 +1,56 @@ +function showChannelStatus(channel, settings) +%Prints the status of all channels in a table. +% +%showChannelStatus(channel, settings) +% +% Inputs: +% channel - data for each channel. It is used to initialize and +% at the processing of the signal (tracking part). +% settings - receiver settings + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Peter Rinder and Nicolaj Bertelsen +% Written by Peter Rinder Nicolaj Bertelsen and Darius Plausinaitis +% Based on Peter Rinder and Nicolaj Bertelsen +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: showChannelStatus.m,v 1.4.2.8 2006/08/14 11:38:22 dpl Exp $ + +fprintf('\n*=========*=====*===============*===========*=============*========*\n'); +fprintf( '| Channel | PRN | Frequency | Doppler | Code Offset | Status |\n'); +fprintf( '*=========*=====*===============*===========*=============*========*\n'); + +for channelNr = 1 : settings.numberOfChannels + if (channel(channelNr).status ~= '-') + fprintf('| %2d | %3d | %2.5e | %5.0f | %6d | %1s |\n', ... + channelNr, ... + channel(channelNr).PRN, ... + channel(channelNr).acquiredFreq, ... + channel(channelNr).acquiredFreq - settings.IF, ... + channel(channelNr).codePhase, ... + channel(channelNr).status); + else + fprintf('| %2d | --- | ------------ | ----- | ------ | Off |\n', ... + channelNr); + end +end + +fprintf('*=========*=====*===============*===========*=============*========*\n\n'); diff --git a/GNSS_SDR_IQ/include/skyPlot.m b/GNSS_SDR_IQ/include/skyPlot.m new file mode 100644 index 0000000..d27d035 --- /dev/null +++ b/GNSS_SDR_IQ/include/skyPlot.m @@ -0,0 +1,177 @@ +function hpol = skyPlot(varargin) +%Function plots "sky view" from the receiver perspective. +% +%h = skyPlot(AZ, EL, PRN, line_style) +% +% Inputs: +% AZ - contains satellite azimuth angles. It is a 2D +% matrix. One line contains data of one satellite. +% The columns are the calculated azimuth values. +% EL - contains satellite elevation angles. It is a 2D +% matrix. One line contains data of one satellite. +% The columns are the calculated elevations. +% PRN - a row vector containing PRN numbers of the +% satellites. +% line_style - line style of the plot. The same style will be +% used to plot all satellite positions (including +% color). +% Outputs: +% h - handle to the plot + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis and Kristin Larson +% Written by Darius Plausinaitis and Kristin Larson +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: skyPlot.m,v 1.1.2.5 2006/08/18 11:41:57 dpl Exp $ + +%% Check arguments and sort them ========================================== +[hAxis, args, nargs] = axescheck(varargin{:}); + +if nargs < 3 || nargs > 4 + error('Requires 3 or 4 data arguments.') +elseif nargs == 3 + [az, el, prn] = deal(args{1:3}); + line_style = 'auto'; +else + [az, el, prn, line_style] = deal(args{1:4}); +end + +if ischar(az) || ischar(el) || ischar(prn) + error('AZ and EL must be numeric.'); +end + +if ~isequal(size(az), size(el)) + error('AZ and EL must be same size.'); +end + +%% Prepare axis =========================================================== +hAxis = newplot(hAxis); + +%--- Get x-axis text color so grid is in same color ----------------------- +tc = get(hAxis, 'xcolor'); + +hold(hAxis, 'on'); + +%--- Plot white background ------------------------------------------------ +rectangle('position', [-90, -90, 180, 180], ... + 'Curvature', [1 1], ... + 'facecolor', 'white', ... + 'edgecolor', tc); + +%% Plot spokes ============================================================ + +%--- Find spoke angles ---------------------------------------------------- +% Only 6 lines are needed to divide circle into 12 parts +th = (1:6) * 2*pi / 12; + +%--- Convert spoke end point coordinate to Cartesian system --------------- +cst = cos(th); snt = sin(th); +cs = [cst; -cst]; +sn = [snt; -snt]; + +%--- Plot the spoke lines ------------------------------------------------- +line(90*sn, 90*cs, 'linestyle', ':', 'color', tc, 'linewidth', 0.5, ... + 'handlevisibility', 'off'); + +%% Annotate spokes in degrees ============================================= +rt = 1.1 * 90; + +for i = 1:max(size(th)) + + %--- Write text in the first half of the plot ------------------------- + text(rt*snt(i), rt*cst(i), int2str(i*30), ... + 'horizontalalignment', 'center', 'handlevisibility', 'off'); + + if i == max(size(th)) + loc = int2str(0); + else + loc = int2str(180 + i*30); + end + + %--- Write text in the opposite half of the plot ---------------------- + text(-rt*snt(i), -rt*cst(i), loc, ... + 'handlevisibility', 'off', 'horizontalalignment', 'center'); +end + +%% Plot elevation grid ==================================================== + +%--- Define a "unit" radius circle ---------------------------------------- +th = 0 : pi/50 : 2*pi; +xunit = cos(th); +yunit = sin(th); + +%--- Plot elevation grid lines and tick text ------------------------------ +for elevation = 0 : 15 : 90 + elevationSpherical = 90*cos((pi/180) * elevation); + + line(yunit * elevationSpherical, xunit * elevationSpherical, ... + 'lineStyle', ':', 'color', tc, 'linewidth', 0.5, ... + 'handlevisibility', 'off'); + + text(0, elevationSpherical, num2str(elevation), ... + 'BackgroundColor', 'white', 'horizontalalignment','center', ... + 'handlevisibility', 'off'); +end + +%--- Set view to 2-D ------------------------------------------------------ +view(0, 90); + +%--- Set axis limits ------------------------------------------------------ +%save some space for the title +axis([-95 95 -90 101]); + +%% Transform elevation angle to a distance to the center of the plot ------ +elSpherical = 90*cos(el * pi/180); + +%--- Transform data to Cartesian coordinates ------------------------------ +yy = elSpherical .* cos(az * pi/180); +xx = elSpherical .* sin(az * pi/180); + +%% Plot data on top of the grid =========================================== + +if strcmp(line_style, 'auto') + %--- Plot with "default" line style ----------------------------------- + hpol = plot(hAxis, xx', yy', '.-'); +else + %--- Plot with user specified line style ------------------------------ + % The same line style and color will be used for all satellites + hpol = plot(hAxis, xx', yy', line_style); +end + +%--- Mark the last position of the satellite ------------------------------ +plot(hAxis, xx(:,end)', yy(:,end)', 'o', 'MarkerSize', 7); + +%--- Place satellite PRN numbers at the latest position ------------------- +for i = 1:length(prn) + if(prn(i) ~= 0) + % The empthy space is used to place the text a side of the last + % point. This solution results in constant offset even if a zoom + % is used. + text(xx(i, end), yy(i, end), [' ', int2str(prn(i))], 'color', 'b'); + end +end + +%--- Make sure both axis have the same data aspect ratio ------------------ +axis(hAxis, 'equal'); + +%--- Switch off the standard Cartesian axis ------------------------------- +axis(hAxis, 'off'); diff --git a/GNSS_SDR_IQ/include/trackingResults.mat b/GNSS_SDR_IQ/include/trackingResults.mat new file mode 100644 index 0000000..90e108e Binary files /dev/null and b/GNSS_SDR_IQ/include/trackingResults.mat differ diff --git a/GNSS_SDR_IQ/include/twosComp2dec.m b/GNSS_SDR_IQ/include/twosComp2dec.m new file mode 100644 index 0000000..edc1779 --- /dev/null +++ b/GNSS_SDR_IQ/include/twosComp2dec.m @@ -0,0 +1,44 @@ +function intNumber = twosComp2dec(binaryNumber) +% TWOSCOMP2DEC(binaryNumber) Converts a two's-complement binary number +% BINNUMBER (in Matlab it is a string type), represented as a row vector of +% zeros and ones, to an integer. +% +%intNumber = twosComp2dec(binaryNumber) + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% CVS record: +% $Id: twosComp2dec.m,v 1.1.2.4 2006/08/14 11:38:22 dpl Exp $ + +%--- Check if the input is string ----------------------------------------- +if ~isstr(binaryNumber) + error('Input must be a string.') +end + +%--- Convert from binary form to a decimal number ------------------------- +intNumber = bin2dec(binaryNumber); + +%--- If the number was negative, then correct the result ------------------ +if binaryNumber(1) == '1' + intNumber = intNumber - 2^size(binaryNumber, 2); +end diff --git a/GNSS_SDR_IQ/init.m b/GNSS_SDR_IQ/init.m new file mode 100644 index 0000000..3b86d9b --- /dev/null +++ b/GNSS_SDR_IQ/init.m @@ -0,0 +1,77 @@ +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis and Dennis M. Akos +% Written by Darius Plausinaitis and Dennis M. Akos +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- +% +%Script initializes settings and environment of the software receiver. +%Then the processing is started. + +%-------------------------------------------------------------------------- +% CVS record: +% $Id: init.m,v 1.14.2.21 2006/08/22 13:46:00 dpl Exp $ + +%% Clean up the environment first ========================================= +clear; close all; clc; + +format ('compact'); +format ('long', 'g'); + +%--- Include folders with functions --------------------------------------- +addpath include % The software receiver functions +addpath geoFunctions % Position calculation related functions + +%% Print startup ========================================================== +fprintf(['\n',... + 'Welcome to: han trong thanh\n\n', ... + 'An open source GNSS SDR software project initiated by:\n\n', ... + ' Danish GPS Center/Aalborg University\n\n', ... + 'The code was improved by GNSS Laboratory/University of Colorado.\n\n',... + 'The software receiver softGNSS comes with ABSOLUTELY NO WARRANTY;\n',... + 'for details please read license details in the file license.txt. This\n',... + 'is free software, and you are welcome to redistribute it under\n',... + 'the terms described in the license.\n\n']); +fprintf(' -------------------------------\n\n'); + +%% Initialize constants, settings ========================================= +settings = initSettings(); + +%% Generate plot of raw data and ask if ready to start processing ========= +try + fprintf('Probing data (%s)...\n', settings.fileName) + probeData(settings); +catch + % There was an error, print it and exit + errStruct = lasterror; + disp(errStruct.message); + disp(' (run setSettings or change settings in "initSettings.m" to reconfigure)') + return; +end + +disp(' Raw IF data plotted ') +disp(' (run setSettings or change settings in "initSettings.m" to reconfigure)') +disp(' '); +gnssStart = 1;%input('Enter "1" to initiate GNSS processing or "0" to exit : '); + +if (gnssStart == 1) + disp(' '); + %start things rolling... + postProcessing +end + diff --git a/GNSS_SDR_IQ/initSettings.m b/GNSS_SDR_IQ/initSettings.m new file mode 100644 index 0000000..60824e7 --- /dev/null +++ b/GNSS_SDR_IQ/initSettings.m @@ -0,0 +1,125 @@ +function settings = initSettings() +%Functions initializes and saves settings. Settings can be edited inside of +%the function, updated from the command line or updated using a dedicated +%GUI - "setSettings". +% +%All settings are described inside function code. +% +%settings = initSettings() +% +% Inputs: none +% +% Outputs: +% settings - Receiver settings (a structure). + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% CVS record: +% $Id: initSettings.m,v 1.9.2.32 2007/01/29 10:22:23 dpl Exp $ + +%% Processing settings ==================================================== +% Number of milliseconds to be processed used 36000 + any transients (see +% below - in Nav parameters) to ensure nav subframes are provided +settings.msToProcess =50000; %[ms] + +% Number of channels to be used for signal processing +settings.numberOfChannels = 6; + +% Move the starting point of processing. Can be used to start the signal +% processing at any point in the data record (e.g. for long records). fseek +% function is used to move the file read point, therefore advance is byte +% based only. +settings.skipNumberOfBytes = 0; + +%% Raw signal file name and other parameter =============================== +% This is a "default" name of the data file (signal record) to be used in +% the post-processing mode +settings.fileName = ... + 'E:\data\data_ant_1.bin'; + % 'D:\Study\SDR\GPS_SDR\akos\GNSS_signal_records\GPS_and_GIOVE_A-NN-fs16_3676-if4_1304.bin'; + + % '..\GNSS_signal_records\GPSdata-DiscreteComponents-fs38_192-if9_55. + % bin'; +% Data type used to store one sample +settings.dataType = 'short'; +settings.dataTypeSize = 2; + +% Intermediate, sampling and code frequencies +settings.IF = ... + 2e6; + %4.1304e6;%9.548e6; %[Hz] +settings.samplingFreq = ... + 5e6;%5.71385e6;%16.3676e6; + %16.3676e6;%38.192e6; %[Hz] +settings.codeFreqBasis = 1.023e6; %[Hz] + +% Define number of chips in a code period +settings.codeLength = 1023; + +%% Acquisition settings =================================================== +% Skips acquisition in the script postProcessing.m if set to 1 +settings.skipAcquisition = 1000; +% List of satellites to look for. Some satellites can be excluded to speed +% up acquisition +settings.acqSatelliteList = [1:32]; %[PRN numbers] +% Band around IF to search for satellite signal. Depends on max Doppler +settings.acqSearchBand = 14; %[kHz] +% Threshold for the signal presence decision rule +settings.acqThreshold = 2.0; + +%% Tracking loops settings ================================================ +% Code tracking loop parameters +settings.dllDampingRatio = 0.7; +settings.dllNoiseBandwidth = 2; %[Hz] +settings.dllCorrelatorSpacing = 0.5; %[chips] + +% Carrier tracking loop parameters +settings.pllDampingRatio = 0.7; +settings.pllNoiseBandwidth = 25; %[Hz] + +%% Navigation solution settings =========================================== + +% Period for calculating pseudoranges and position +settings.navSolPeriod = 500; %[ms] + +% Elevation mask to exclude signals from satellites at low elevation +settings.elevationMask = 0; %[degrees 0 - 90] +% Enable/dissable use of tropospheric correction +settings.useTropCorr = 1; % 0 - Off + % 1 - On + +% True position of the antenna in UTM system (if known). Otherwise enter +% all NaN's and mean position will be used as a reference . +settings.truePosition.E = nan; +settings.truePosition.N = nan; +settings.truePosition.U = nan; + +%% Plot settings ========================================================== +% Enable/disable plotting of the tracking results for each channel +settings.plotTracking = 1; % 0 - Off + % 1 - On + +%% Constants ============================================================== + +settings.c = 299792458; % The speed of light, [m/s] +settings.startOffset = 68.802; %[ms] Initial sign. travel time diff --git a/GNSS_SDR_IQ/initSettings_IQ.m b/GNSS_SDR_IQ/initSettings_IQ.m new file mode 100644 index 0000000..3ada4cd --- /dev/null +++ b/GNSS_SDR_IQ/initSettings_IQ.m @@ -0,0 +1,110 @@ +function settings = initSettings_IQ() +%Functions initializes and saves settings. Settings can be edited inside of +%the function, updated from the command line or updated using a dedicated +%GUI - "setSettings". +% +%All settings are described inside function code.r +% +%settings = initSettings() +% +% Inputs: none +% +% Outputs: +% settings - Receiver settings (a structure). + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- +%% Optional Parameters +% Number of milliseconds to be processed used 36000 + any transients (see +% below - in Nav parameters) to ensure nav subframes are provided +settings.msToProcess = 5*1e3; %[ms] +% Move the starting point of processing. +settings.skipNumberOfBytes = 0*2.046e9; %0*2.046e6; %0*2.046e6;%1.5*4.092e6*2; % settings.samplingFreq;%13169313;%settings.samplingFreq/5*4;%+16e3*50;%16e3*100.; +% The notch bandwidth of filter +settings.Brej = 5e3; +settings.fileName = 'D:\Long\NAVIS\Data\2023-12-20.bin'; +%settings.fileName = 'E:\TEXBAT\cleanDynamic_spoofing.bin'; +%settings.fileName ='K:\DATA\Projects\SDR\gnss-sdr-sim\vs-gps-sdr-sim\vs-gps-sdr-sim\x64\Debug\gpssim.bin';'D:\test_4093.bin';'D:\RTL2832U_Data\test_1.bin';'D:\test.bin';['C:\rtldata\test_0.bin'];%['C:\Users\thuan\Downloads\sdrsharp-x86\SDRSharp_20151223_155844Z_1575420000Hz_IQ.wav'];% +% settings.fileName = ['D:\IF_DATA\RTL2832\140320_1243_1.dat']; +settings.relativeFreq = 0; +%% Independent parameters +% Number of channels to be used for signal processing +settings.numberOfChannels = 5; +% Intermediate, sampling and code frequencies +settings.IF = 0e3; %[Hz] tần số trung gian +settings.samplingFreq = 2e6; %2.5e7; %2e6; 2.6e6; 8184000; [Hz] tần số lấy mẫu +settings.codeFreqBasis = 1.023e6; %[Hz] tần số mã +% Define number of chips in a code period +settings.codeLength = 1023; % số lượng chip trong 1 chu kỳ mã +settings.samplesPDI = settings.samplingFreq*10e-3; +% Number of samples per spreading code +settings.samplesPerCode = round(settings.samplingFreq / ... + (settings.codeFreqBasis / settings.codeLength)); +settings.dataType = 'int16'; +if strcmp(settings.dataType,'int8') + settings.dataTypeSize = 1; +else + settings.dataTypeSize = 2; +end; +%% Acquisition settings =================================================== +% Skips acquisition in the script postProcessing.m if set to 1 +settings.skipAcquisition = 2e6; +% List of satellites to look for. Some satellites can be excluded to speed +% up acquisition +settings.acqSatelliteList = 1:32; %[PRN numbers] +% Band around IF to search for satellite signal. Depends on max Doppler +settings.acqSearchBand = 10; %[kHz] +% Threshold for the signal presence decision rule +settings.acqThreshold = 2.4; + +%% Tracking loops settings ================================================ +% Code tracking loop parameters +settings.dllDampingRatio = 0.7; +settings.dllNoiseBandwidth = 2; %[Hz] 2 +settings.dllCorrelatorSpacing = 0.5; %[chips] +% Carrier tracking loop parameters +settings.pllDampingRatio = 0.7; +settings.pllNoiseBandwidth = 25; %[Hz] 25 + +%% Navigation solution settings =========================================== +% Period for calculating pseudoranges and position +settings.navSolPeriod = 100; %[ms] +% Elevation mask to exclude signals from satellites at low elevation +settings.elevationMask = 10; %[degrees 0 - 90] +% Enable/disable use of tropospheric correction +settings.useTropCorr = 1; % 0 - Off + % 1 - On +% True position of the antenna in UTM system (if known). Otherwise enter +% all NaN's and mean position will be used as a reference . +settings.truePosition.E = nan; +settings.truePosition.N = nan; +settings.truePosition.U = nan; + +%% Plot settings ========================================================== +% Enable/disable plotting of the tracking results for each channel +settings.plotTracking = 1; % 0 - Off + % 1 - On +settings.plotAcquisition = 1; + +%% Constants ============================================================== +settings.c = 299792458; % The speed of light, [m/s] +settings.startOffset = 68.802; %[ms] Initial sign. travel time diff --git a/GNSS_SDR_IQ/leastSquarePos_Snapshot.asv b/GNSS_SDR_IQ/leastSquarePos_Snapshot.asv new file mode 100644 index 0000000..4515e3c --- /dev/null +++ b/GNSS_SDR_IQ/leastSquarePos_Snapshot.asv @@ -0,0 +1,140 @@ +function [pos, el, az, dop] = leastSquarePos_Snapshot(satpos, obs_fract1ms,obs_1ms,Doppler,satClkCorr, settings) +%Function calculates the Least Square Solution. +% +%[pos, el, az, dop] = leastSquarePos(satpos, obs, settings); +% +% Inputs: +% satpos - Satellites positions (in ECEF system: [X; Y; Z;] - +% one column per satellite) +% obs - Observations - the pseudorange measurements to each +% satellite: +% (e.g. [20000000 21000000 .... .... .... .... ....]) +% settings - receiver settings +% +% Outputs: +% pos - receiver position and receiver clock error +% (in ECEF system: [X, Y, Z, dt]) +% el - Satellites elevation angles (degrees) +% az - Satellites azimuth angles (degrees) +% dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP]) + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +%-------------------------------------------------------------------------- +%Based on Kai Borre +%Copyright (c) by Kai Borre +%Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen +% +% CVS record: +% $Id: leastSquarePos.m,v 1.1.2.12 2006/08/22 13:45:59 dpl Exp $ +%========================================================================== + +%=== Initialization ======================================================= +%% THUAN +obs=obs_fract1ms;%+obs_1ms*(settings.c*0.001) + satClkCorr * settings.c; +[tmp idx_ref]=min(obs_1ms); +obs_ref1ms=68; +%% +nmbOfIterations = 7; + +dtr = pi/180; +pos = zeros(5, 1); +% pos(1)=-1.626058869465007e+006; +% pos(2)=5.730480840292945e+006; +% pos(3)=2.272159906059972e+006; +pos(1)= -1541.802e3; +pos(2)= 5754.083e3; +pos(3)= 2271.395e3; +[pos(1) pos(2) pos(3)]=llh2xyz(21,105,0); + + +X = satpos; +nmbOfSatellites = size(satpos, 2); + +A = zeros(nmbOfSatellites, 5); +omc = zeros(nmbOfSatellites, 1); +az = zeros(1, nmbOfSatellites); +el = az; + +%=== Iteratively find receiver position =================================== +for iter = 1:nmbOfIterations + for i=1:nmbOfSatellites + rr(i) = sqrt((X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ... + (X(3, i) - pos(3))^2); + end; + N_1ms=round((rr-rr(idx_ref)+(obs_fract1ms(idx_ref)-obs_fract1ms)+satClkCorr-satClkCorr(idx_ref))/(settings.c*0.001)+obs_ref1ms); + obs=obs_fract1ms+N_1ms*(settings.c*0.001) + satClkCorr * settings.c; + yy=obs-rr; + yy(yy>149000)=yy(yy>149000)-settings.c*0.001; + yy(yy<-149000)=yy(yy<-149000)+settings.c*0.001; + for i = 1:nmbOfSatellites + if iter == 1 + %--- Initialize variables at the first iteration -------------- + Rot_X = X(:, i); + trop = 2; + else + %--- Update equations ----------------------------------------- + rho2 = (X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ... + (X(3, i) - pos(3))^2; + traveltime = sqrt(rho2) / settings.c ; + + %--- Correct satellite position (do to earth rotation) -------- + Rot_X = e_r_corr(traveltime, X(:, i)); + + %--- Find the elevation angel of the satellite ---------------- + [az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :)); + + if (settings.useTropCorr == 1) + %--- Calculate tropospheric correction -------------------- + trop = tropo(sin(el(i) * dtr), ... + 0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); + else + % Do not calculate or apply the tropospheric corrections + trop = 0; + end + end % if iter == 1 ... ... else + + %% THUAN + + %% + %--- Apply the corrections ---------------------------------------- + + omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop); + + %--- Construct the A matrix --------------------------------------- + A(i, :) = [ (-(Rot_X(1) - pos(1))) / obs(i) ... + (-(Rot_X(2) - pos(2))) / obs(i) ... + (-(Rot_X(3) - pos(3))) / obs(i) ... + 1 Doppler(i)*0.001*settings.c/(1.57542e9) ]; + end % for i = 1:nmbOfSatellites + + % These lines allow the code to exit gracefully in case of any errors + if rank(A) ~= 5 + pos = zeros(1, 4); + return + end + + %--- Find position update --------------------------------------------- + x = A \ omc; + + %--- Apply position update -------------------------------------------- + pos = pos + x; + +end % for iter = 1:nmbOfIterations + +pos = pos'; + +%=== Calculate Dilution Of Precision ====================================== +if nargout == 4 + %--- Initialize output ------------------------------------------------ + dop = zeros(1, 5); + + %--- Calculate DOP ---------------------------------------------------- + Q = inv(A'*A); + + dop(1) = sqrt(trace(Q)); % GDOP + dop(2) = sqrt(Q(1,1) + Q(2,2) + Q(3,3)); % PDOP + dop(3) = sqrt(Q(1,1) + Q(2,2)); % HDOP + dop(4) = sqrt(Q(3,3)); % VDOP + dop(5) = sqrt(Q(4,4)); % TDOP +end diff --git a/GNSS_SDR_IQ/leastSquarePos_Snapshot.m b/GNSS_SDR_IQ/leastSquarePos_Snapshot.m new file mode 100644 index 0000000..226fa52 --- /dev/null +++ b/GNSS_SDR_IQ/leastSquarePos_Snapshot.m @@ -0,0 +1,145 @@ +function [pos, el, az, dop] = leastSquarePos_Snapshot(satpos, obs_fract1ms,obs_1ms,Doppler,satClkCorr, settings) +%Function calculates the Least Square Solution. +% +%[pos, el, az, dop] = leastSquarePos(satpos, obs, settings); +% +% Inputs: +% satpos - Satellites positions (in ECEF system: [X; Y; Z;] - +% one column per satellite) +% obs - Observations - the pseudorange measurements to each +% satellite: +% (e.g. [20000000 21000000 .... .... .... .... ....]) +% settings - receiver settings +% +% Outputs: +% pos - receiver position and receiver clock error +% (in ECEF system: [X, Y, Z, dt]) +% el - Satellites elevation angles (degrees) +% az - Satellites azimuth angles (degrees) +% dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP]) + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +%-------------------------------------------------------------------------- +%Based on Kai Borre +%Copyright (c) by Kai Borre +%Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen +% +% CVS record: +% $Id: leastSquarePos.m,v 1.1.2.12 2006/08/22 13:45:59 dpl Exp $ +%========================================================================== + +%=== Initialization ======================================================= +%% THUAN +obs=obs_fract1ms;%+obs_1ms*(settings.c*0.001) + satClkCorr * settings.c; +[tmp idx_ref]=min(obs_1ms); +obs_ref1ms=68; +%% +nmbOfIterations = 7; + +dtr = pi/180; +pos = zeros(5, 1); +% pos(1)=-1.626058869465007e+006; +% pos(2)=5.730480840292945e+006; +% pos(3)=2.272159906059972e+006; +[pos(1) pos(2) pos(3)]=llh2xyz(21,105,0); + + +X = satpos; +nmbOfSatellites = size(satpos, 2); + +A = zeros(nmbOfSatellites, 5); +omc = zeros(nmbOfSatellites, 1); +az = zeros(1, nmbOfSatellites); +el = az; + +%=== Iteratively find receiver position =================================== +for iter = 1:nmbOfIterations + for i=1:nmbOfSatellites + rr(i) = sqrt((X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ... + (X(3, i) - pos(3))^2); + end; + N_1ms=round((rr-rr(idx_ref)+(obs_fract1ms(idx_ref)-obs_fract1ms)+satClkCorr-satClkCorr(idx_ref))/(settings.c*0.001)+obs_ref1ms); + obs=obs_fract1ms+N_1ms*(settings.c*0.001) + satClkCorr * settings.c; + yy=obs-rr; + %[tmp idx_ref]=min(yy); + yyy=yy-yy(idx_ref); + for i=1:nmbOfSatellites + if(yyy(i)>149000) + obs(i)=obs(i)-settings.c*0.001; + end; + if(yyy(i)<-149000) + obs(i)=obs(i)+settings.c*0.001; + end; + end; + for i = 1:nmbOfSatellites + if iter == 1 + %--- Initialize variables at the first iteration -------------- + Rot_X = X(:, i); + trop = 2; + else + %--- Update equations ----------------------------------------- + rho2 = (X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ... + (X(3, i) - pos(3))^2; + traveltime = sqrt(rho2) / settings.c ; + + %--- Correct satellite position (do to earth rotation) -------- + Rot_X = e_r_corr(traveltime, X(:, i)); + + %--- Find the elevation angel of the satellite ---------------- + [az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :)); + + if (settings.useTropCorr == 1) + %--- Calculate tropospheric correction -------------------- + trop = tropo(sin(el(i) * dtr), ... + 0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); + else + % Do not calculate or apply the tropospheric corrections + trop = 0; + end + end % if iter == 1 ... ... else + + %% THUAN + + %% + %--- Apply the corrections ---------------------------------------- + + omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop); + + %--- Construct the A matrix --------------------------------------- + A(i, :) = [ (-(Rot_X(1) - pos(1))) / obs(i) ... + (-(Rot_X(2) - pos(2))) / obs(i) ... + (-(Rot_X(3) - pos(3))) / obs(i) ... + 1 Doppler(i)*settings.c/(1.57542e9) ]; + end % for i = 1:nmbOfSatellites + + % These lines allow the code to exit gracefully in case of any errors + if rank(A) ~= 5 + pos = zeros(1, 4); + return + end + + %--- Find position update --------------------------------------------- + x = A \ omc; + + %--- Apply position update -------------------------------------------- + pos = pos + x; + +end % for iter = 1:nmbOfIterations + +pos = pos'; + +%=== Calculate Dilution Of Precision ====================================== +if nargout == 4 + %--- Initialize output ------------------------------------------------ + dop = zeros(1, 5); + + %--- Calculate DOP ---------------------------------------------------- + Q = inv(A'*A); + + dop(1) = sqrt(trace(Q)); % GDOP + dop(2) = sqrt(Q(1,1) + Q(2,2) + Q(3,3)); % PDOP + dop(3) = sqrt(Q(1,1) + Q(2,2)); % HDOP + dop(4) = sqrt(Q(3,3)); % VDOP + dop(5) = sqrt(Q(4,4)); % TDOP +end diff --git a/GNSS_SDR_IQ/leastSquarePos_Snapshot_0.m b/GNSS_SDR_IQ/leastSquarePos_Snapshot_0.m new file mode 100644 index 0000000..9af67fb --- /dev/null +++ b/GNSS_SDR_IQ/leastSquarePos_Snapshot_0.m @@ -0,0 +1,147 @@ +function [pos, el, az, dop] = leastSquarePos_Snapshot(satpos, obs_fract1ms,obs_1ms,Doppler,satClkCorr, settings) +%Function calculates the Least Square Solution. +% +%[pos, el, az, dop] = leastSquarePos(satpos, obs, settings); +% +% Inputs: +% satpos - Satellites positions (in ECEF system: [X; Y; Z;] - +% one column per satellite) +% obs - Observations - the pseudorange measurements to each +% satellite: +% (e.g. [20000000 21000000 .... .... .... .... ....]) +% settings - receiver settings +% +% Outputs: +% pos - receiver position and receiver clock error +% (in ECEF system: [X, Y, Z, dt]) +% el - Satellites elevation angles (degrees) +% az - Satellites azimuth angles (degrees) +% dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP]) + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +%-------------------------------------------------------------------------- +%Based on Kai Borre +%Copyright (c) by Kai Borre +%Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen +% +% CVS record: +% $Id: leastSquarePos.m,v 1.1.2.12 2006/08/22 13:45:59 dpl Exp $ +%========================================================================== + +%=== Initialization ======================================================= +%% THUAN +obs=obs_fract1ms;%+obs_1ms*(settings.c*0.001) + satClkCorr * settings.c; +[tmp idx_ref]=min(obs_1ms); +obs_ref1ms=68; +%% +nmbOfIterations = 7; + +dtr = pi/180; +pos = zeros(4, 1); +% pos(1)=-1.626058869465007e+006; +% pos(2)=5.730480840292945e+006; +% pos(3)=2.272159906059972e+006; +pos(1)= -1541.802e3; +pos(2)= 5754.083e3; +pos(3)= 2271.395e3; +[pos(1) pos(2) pos(3)]=llh2xyz(21,105,0); + + +X = satpos; +nmbOfSatellites = size(satpos, 2); + +A = zeros(nmbOfSatellites, 4); +omc = zeros(nmbOfSatellites, 1); +az = zeros(1, nmbOfSatellites); +el = az; + +%=== Iteratively find receiver position =================================== +for iter = 1:nmbOfIterations + for i=1:nmbOfSatellites + rr(i) = sqrt((X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ... + (X(3, i) - pos(3))^2); + end; + N_1ms=round((rr-rr(idx_ref)+(obs_fract1ms(idx_ref)-obs_fract1ms)+satClkCorr-satClkCorr(idx_ref))/(settings.c*0.001)+obs_ref1ms); + obs=obs_fract1ms+N_1ms*(settings.c*0.001) + satClkCorr * settings.c; + yy=obs-rr; + yyy=yy-yy(idx_ref); + for i=1:nmbOfSatellites + if(yyy(i)>149000) + obs(i)=obs(i)-settings.c*0.001; + end; + if(yyy(i)<-149000) + obs(i)=obs(i)+settings.c*0.001; + end; + end; + for i = 1:nmbOfSatellites + if iter == 1 + %--- Initialize variables at the first iteration -------------- + Rot_X = X(:, i); + trop = 2; + else + %--- Update equations ----------------------------------------- + rho2 = (X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ... + (X(3, i) - pos(3))^2; + traveltime = sqrt(rho2) / settings.c ; + + %--- Correct satellite position (do to earth rotation) -------- + Rot_X = e_r_corr(traveltime, X(:, i)); + + %--- Find the elevation angel of the satellite ---------------- + [az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :)); + + if (settings.useTropCorr == 1) + %--- Calculate tropospheric correction -------------------- + trop = tropo(sin(el(i) * dtr), ... + 0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); + else + % Do not calculate or apply the tropospheric corrections + trop = 0; + end + end % if iter == 1 ... ... else + + %% THUAN + + %% + %--- Apply the corrections ---------------------------------------- + + omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop); + + %--- Construct the A matrix --------------------------------------- + A(i, :) = [ (-(Rot_X(1) - pos(1))) / obs(i) ... + (-(Rot_X(2) - pos(2))) / obs(i) ... + (-(Rot_X(3) - pos(3))) / obs(i) ... + 1 ]; + end % for i = 1:nmbOfSatellites + + % These lines allow the code to exit gracefully in case of any errors + if rank(A) ~= 4 + pos = zeros(1, 4); + return + end + + %--- Find position update --------------------------------------------- + x = A \ omc; + + %--- Apply position update -------------------------------------------- + pos = pos + x; + +end % for iter = 1:nmbOfIterations + +pos = pos'; + +%=== Calculate Dilution Of Precision ====================================== +if nargout == 4 + %--- Initialize output ------------------------------------------------ + dop = zeros(1, 5); + + %--- Calculate DOP ---------------------------------------------------- + Q = inv(A'*A); + + dop(1) = sqrt(trace(Q)); % GDOP + dop(2) = sqrt(Q(1,1) + Q(2,2) + Q(3,3)); % PDOP + dop(3) = sqrt(Q(1,1) + Q(2,2)); % HDOP + dop(4) = sqrt(Q(3,3)); % VDOP + dop(5) = sqrt(Q(4,4)); % TDOP +end diff --git a/GNSS_SDR_IQ/license.txt b/GNSS_SDR_IQ/license.txt new file mode 100644 index 0000000..08ddefd --- /dev/null +++ b/GNSS_SDR_IQ/license.txt @@ -0,0 +1,340 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Lesser General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) year name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + , 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. + diff --git a/GNSS_SDR_IQ/llh2xyz.m b/GNSS_SDR_IQ/llh2xyz.m new file mode 100644 index 0000000..937a20c --- /dev/null +++ b/GNSS_SDR_IQ/llh2xyz.m @@ -0,0 +1,24 @@ +% llh2xyz.m +% +% DESCRIPTION: +% Convert lat, long, height in WGS84 to ECEF X,Y,Z +% lat and long given in decimal degrees. +% altitude should be given in meters +% +% PFMS Project, 2009 +% Nicholas Rutherford + +% NOTES: +% None. + +function [X,Y,Z] = llh2xyz(lat,long, h) + lat = lat/180*pi; %converting to radians + long = long/180*pi; %converting to radians + a = 6378137.0; % earth semimajor axis in meters + f = 1/298.257223563; % reciprocal flattening + e2 = 2*f -f^2; % eccentricity squared + + chi = sqrt(1-e2*(sin(lat)).^2); + X = (a./chi +h).*cos(lat).*cos(long); + Y = (a./chi +h).*cos(lat).*sin(long); + Z = (a*(1-e2)./chi + h).*sin(lat); \ No newline at end of file diff --git a/GNSS_SDR_IQ/matlab.mat b/GNSS_SDR_IQ/matlab.mat new file mode 100644 index 0000000..248d0d5 Binary files /dev/null and b/GNSS_SDR_IQ/matlab.mat differ diff --git a/GNSS_SDR_IQ/plotAcquisition.m b/GNSS_SDR_IQ/plotAcquisition.m new file mode 100644 index 0000000..fd3e6ef --- /dev/null +++ b/GNSS_SDR_IQ/plotAcquisition.m @@ -0,0 +1,60 @@ +function plotAcquisition(acqResults) +%Functions plots bar plot of acquisition results (acquisition metrics). No +%bars are shown for the satellites not included in the acquisition list (in +%structure SETTINGS). +% +%plotAcquisition(acqResults) +% +% Inputs: +% acqResults - Acquisition results from function acquisition. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% CVS record: +% $Id: plotAcquisition.m,v 1.1.2.4 2006/08/09 17:20:11 dpl Exp $ + +%% Plot all results ======================================================= +figure(101); + +hAxes = newplot(); + +bar(hAxes, acqResults.peakMetric); + +title (hAxes, 'Acquisition results'); +xlabel(hAxes, 'PRN number (no bar - SV is not in the acquisition list)'); +ylabel(hAxes, 'Acquisition Metric'); + +oldAxis = axis(hAxes); +axis (hAxes, [0, 33, 0, oldAxis(4)]); +set (hAxes, 'XMinorTick', 'on'); +set (hAxes, 'YGrid', 'on'); + +%% Mark acquired signals ================================================== + +acquiredSignals = acqResults.peakMetric .* (acqResults.carrFreq > 0); + +hold(hAxes, 'on'); +bar (hAxes, acquiredSignals, 'FaceColor', [0 0.8 0]); +hold(hAxes, 'off'); + +legend(hAxes, 'Not acquired signals', 'Acquired signals'); diff --git a/GNSS_SDR_IQ/plotNavigation.m b/GNSS_SDR_IQ/plotNavigation.m new file mode 100644 index 0000000..2e66d5a --- /dev/null +++ b/GNSS_SDR_IQ/plotNavigation.m @@ -0,0 +1,139 @@ +function plotNavigation(navSolutions, settings) +%Functions plots variations of coordinates over time and a 3D position +%plot. It plots receiver coordinates in UTM system or coordinate offsets if +%the true UTM receiver coordinates are provided. +% +%plotNavigation(navSolutions, settings) +% +% Inputs: +% navSolutions - Results from navigation solution function. It +% contains measured pseudoranges and receiver +% coordinates. +% settings - Receiver settings. The true receiver coordinates +% are contained in this structure. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% CVS record: +% $Id: plotNavigation.m,v 1.1.2.25 2006/08/09 17:20:11 dpl Exp $ + +%% Plot results in the necessary data exists ============================== +if (~isempty(navSolutions)) + + %% If reference position is not provided, then set reference position + %% to the average postion + if isnan(settings.truePosition.E) || isnan(settings.truePosition.N) ... + || isnan(settings.truePosition.U) + + %=== Compute mean values ========================================== + % Remove NaN-s or the output of the function MEAN will be NaN. + refCoord.E = mean(navSolutions.E(~isnan(navSolutions.E))); + refCoord.N = mean(navSolutions.N(~isnan(navSolutions.N))); + refCoord.U = mean(navSolutions.U(~isnan(navSolutions.U))); + + %Also convert geodetic coordinates to deg:min:sec vector format + meanLongitude = dms2mat(deg2dms(... + mean(navSolutions.longitude(~isnan(navSolutions.longitude)))), -5); + meanLatitude = dms2mat(deg2dms(... + mean(navSolutions.latitude(~isnan(navSolutions.latitude)))), -5); + + refPointLgText = ['Mean Position\newline Lat: ', ... + num2str(meanLatitude(1)), '{\circ}', ... + num2str(meanLatitude(2)), '{\prime}', ... + num2str(meanLatitude(3)), '{\prime}{\prime}', ... + '\newline Lng: ', ... + num2str(meanLongitude(1)), '{\circ}', ... + num2str(meanLongitude(2)), '{\prime}', ... + num2str(meanLongitude(3)), '{\prime}{\prime}', ... + '\newline Hgt: ', ... + num2str(mean(navSolutions.height(~isnan(navSolutions.height))), '%+6.1f')]; + else + refPointLgText = 'Reference Position'; + refCoord.E = settings.truePosition.E; + refCoord.N = settings.truePosition.N; + refCoord.U = settings.truePosition.U; + end + + figureNumber = 300; + % The 300 is chosen for more convenient handling of the open + % figure windows, when many figures are closed and reopened. Figures + % drawn or opened by the user, will not be "overwritten" by this + % function if the auto numbering is not used. + + %=== Select (or create) and clear the figure ========================== + figure(figureNumber); + clf (figureNumber); + set (figureNumber, 'Name', 'Navigation solutions'); + + %--- Draw axes -------------------------------------------------------- + handles(1, 1) = subplot(4, 2, 1 : 4); + handles(3, 1) = subplot(4, 2, [5, 7]); + handles(3, 2) = subplot(4, 2, [6, 8]); + +%% Plot all figures ======================================================= + + %--- Coordinate differences in UTM system ----------------------------- + plot(handles(1, 1), [(navSolutions.E - refCoord.E)', ... + (navSolutions.N - refCoord.N)',... + (navSolutions.U - refCoord.U)']); + + title (handles(1, 1), 'Coordinates variations in UTM system'); + legend(handles(1, 1), 'E', 'N', 'U'); + xlabel(handles(1, 1), ['Measurement period: ', ... + num2str(settings.navSolPeriod), 'ms']); + ylabel(handles(1, 1), 'Variations (m)'); + grid (handles(1, 1)); + axis (handles(1, 1), 'tight'); + + %--- Position plot in UTM system -------------------------------------- + plot3 (handles(3, 1), navSolutions.E - refCoord.E, ... + navSolutions.N - refCoord.N, ... + navSolutions.U - refCoord.U, '+'); + hold (handles(3, 1), 'on'); + %Plot the reference point + plot3 (handles(3, 1), 0, 0, 0, 'r+', 'LineWidth', 1.5, 'MarkerSize', 10); + hold (handles(3, 1), 'off'); + + view (handles(3, 1), 0, 90); + axis (handles(3, 1), 'equal'); + grid (handles(3, 1), 'minor'); + + legend(handles(3, 1), 'Measurements', refPointLgText); + + title (handles(3, 1), 'Positions in UTM system (3D plot)'); + xlabel(handles(3, 1), 'East (m)'); + ylabel(handles(3, 1), 'North (m)'); + zlabel(handles(3, 1), 'Upping (m)'); + + %--- Satellite sky plot ----------------------------------------------- + skyPlot(handles(3, 2), ... + navSolutions.channel.az, ... + navSolutions.channel.el, ... + navSolutions.channel.PRN(:, 1)); + + title (handles(3, 2), ['Sky plot (mean PDOP: ', ... + num2str(mean(navSolutions.DOP(2,:))), ')']); + +else + disp('plotNavigation: No navigation data to plot.'); +end % if (~isempty(navSolutions)) diff --git a/GNSS_SDR_IQ/plotTracking.m b/GNSS_SDR_IQ/plotTracking.m new file mode 100644 index 0000000..45da240 --- /dev/null +++ b/GNSS_SDR_IQ/plotTracking.m @@ -0,0 +1,153 @@ +function plotTracking(channelList, trackResults, settings) +%This function plots the tracking results for the given channel list. +% +%plotTracking(channelList, trackResults, settings) +% +% Inputs: +% channelList - list of channels to be plotted. +% trackResults - tracking results from the tracking function. +% settings - receiver settings. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: plotTracking.m,v 1.5.2.23 2006/08/14 14:45:14 dpl Exp $ + +% Protection - if the list contains incorrect channel numbers +channelList = intersect(channelList, 1:settings.numberOfChannels); + +%=== For all listed channels ============================================== +for channelNr = channelList + +%% Select (or create) and clear the figure ================================ + % The number 200 is added just for more convenient handling of the open + % figure windows, when many figures are closed and reopened. + % Figures drawn or opened by the user, will not be "overwritten" by + % this function. + + figure(channelNr +200); + clf(channelNr +200); + set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ... + ' (PRN ', ... + num2str(trackResults(channelNr).PRN), ... + ') results']); + +%% Draw axes ============================================================== + % Row 1 + handles(1, 1) = subplot(3, 3, 1); + handles(1, 2) = subplot(3, 3, [2 3]); + % Row 2 + handles(2, 1) = subplot(3, 3, 4); + handles(2, 2) = subplot(3, 3, [5 6]); + % Row 3 + handles(3, 1) = subplot(3, 3, 7); + handles(3, 2) = subplot(3, 3, 8); + handles(3, 3) = subplot(3, 3, 9); + +%% Plot all figures ======================================================= + + timeAxisInSeconds = (1:settings.msToProcess)/1000; + + %----- Discrete-Time Scatter Plot --------------------------------- + plot(handles(1, 1), trackResults(channelNr).I_P,... + trackResults(channelNr).Q_P, ... + '.'); + + grid (handles(1, 1)); + axis (handles(1, 1), 'equal'); + title (handles(1, 1), 'Discrete-Time Scatter Plot'); + xlabel(handles(1, 1), 'I prompt'); + ylabel(handles(1, 1), 'Q prompt'); + + %----- Nav bits --------------------------------------------------- + plot (handles(1, 2), timeAxisInSeconds, ... + trackResults(channelNr).I_P); + + grid (handles(1, 2)); + title (handles(1, 2), 'Bits of the navigation message'); + xlabel(handles(1, 2), 'Time (s)'); + axis (handles(1, 2), 'tight'); + + %----- PLL discriminator unfiltered-------------------------------- + plot (handles(2, 1), timeAxisInSeconds, ... + trackResults(channelNr).pllDiscr, 'r'); + + grid (handles(2, 1)); + axis (handles(2, 1), 'tight'); + xlabel(handles(2, 1), 'Time (s)'); + ylabel(handles(2, 1), 'Amplitude'); + title (handles(2, 1), 'Raw PLL discriminator'); + + %----- Correlation ------------------------------------------------ + plot(handles(2, 2), timeAxisInSeconds, ... + [sqrt(trackResults(channelNr).I_E.^2 + ... + trackResults(channelNr).Q_E.^2)', ... + sqrt(trackResults(channelNr).I_P.^2 + ... + trackResults(channelNr).Q_P.^2)', ... + sqrt(trackResults(channelNr).I_L.^2 + ... + trackResults(channelNr).Q_L.^2)'], ... + '-*'); + + grid (handles(2, 2)); + title (handles(2, 2), 'Correlation results'); + xlabel(handles(2, 2), 'Time (s)'); + axis (handles(2, 2), 'tight'); + + hLegend = legend(handles(2, 2), '$\sqrt{I_{E}^2 + Q_{E}^2}$', ... + '$\sqrt{I_{P}^2 + Q_{P}^2}$', ... + '$\sqrt{I_{L}^2 + Q_{L}^2}$'); + + %set interpreter from tex to latex. This will draw \sqrt correctly + set(hLegend, 'Interpreter', 'Latex'); + + %----- PLL discriminator filtered---------------------------------- + plot (handles(3, 1), timeAxisInSeconds, ... + trackResults(channelNr).pllDiscrFilt, 'b'); + + grid (handles(3, 1)); + axis (handles(3, 1), 'tight'); + xlabel(handles(3, 1), 'Time (s)'); + ylabel(handles(3, 1), 'Amplitude'); + title (handles(3, 1), 'Filtered PLL discriminator'); + + %----- DLL discriminator unfiltered-------------------------------- + plot (handles(3, 2), timeAxisInSeconds, ... + trackResults(channelNr).dllDiscr, 'r'); + + grid (handles(3, 2)); + axis (handles(3, 2), 'tight'); + xlabel(handles(3, 2), 'Time (s)'); + ylabel(handles(3, 2), 'Amplitude'); + title (handles(3, 2), 'Raw DLL discriminator'); + + %----- DLL discriminator filtered---------------------------------- + plot (handles(3, 3), timeAxisInSeconds, ... + trackResults(channelNr).dllDiscrFilt, 'b'); + + grid (handles(3, 3)); + axis (handles(3, 3), 'tight'); + xlabel(handles(3, 3), 'Time (s)'); + ylabel(handles(3, 3), 'Amplitude'); + title (handles(3, 3), 'Filtered DLL discriminator'); + +end % for channelNr = channelList diff --git a/GNSS_SDR_IQ/postNavigation.m b/GNSS_SDR_IQ/postNavigation.m new file mode 100644 index 0000000..011f4ce --- /dev/null +++ b/GNSS_SDR_IQ/postNavigation.m @@ -0,0 +1,254 @@ +function [navSolutions, eph] = postNavigation(trackResults, settings) +%Function calculates navigation solutions for the receiver (pseudoranges, +%positions). At the end it converts coordinates from the WGS84 system to +%the UTM, geocentric or any additional coordinate system. +% +%[navSolutions, eph] = postNavigation(trackResults, settings) +% +% Inputs: +% trackResults - results from the tracking function (structure +% array). +% settings - receiver settings. +% Outputs: +% navSolutions - contains measured pseudoranges, receiver +% clock error, receiver coordinates in several +% coordinate systems (at least ECEF and UTM). +% eph - received ephemerides of all SV (structure array). + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis with help from Kristin Larson +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: postNavigation.m,v 1.1.2.22 2006/08/09 17:20:11 dpl Exp $ + +%% Check is there enough data to obtain any navigation solution =========== +% It is necessary to have at least three subframes (number 1, 2 and 3) to +% find satellite coordinates. Then receiver position can be found too. +% The function requires all 5 subframes, because the tracking starts at +% arbitrary point. Therefore the first received subframes can be any three +% from the 5. +% One subframe length is 6 seconds, therefore we need at least 30 sec long +% record (5 * 6 = 30 sec = 30000ms). We add extra seconds for the cases, +% when tracking has started in a middle of a subframe. + +if (settings.msToProcess < 36000) || (sum([trackResults.status] ~= '-') < 4) + % Show the error message and exit + disp('Record is to short or too few satellites tracked. Exiting!'); + navSolutions = []; + eph = []; + return +end + +%% Find preamble start positions ========================================== + +[subFrameStart, activeChnList] = findPreambles(trackResults, settings); + +%% Decode ephemerides ===================================================== + +for channelNr = activeChnList + + %=== Convert tracking output to navigation bits ======================= + + %--- Copy 5 sub-frames long record from tracking output --------------- + navBitsSamples = trackResults(channelNr).I_P(subFrameStart(channelNr) - 20 : ... + subFrameStart(channelNr) + (1500 * 20) -1)'; + + %--- Group every 20 vales of bits into columns ------------------------ + navBitsSamples = reshape(navBitsSamples, ... + 20, (size(navBitsSamples, 1) / 20)); + + %--- Sum all samples in the bits to get the best estimate ------------- + navBits = sum(navBitsSamples); + + %--- Now threshold and make 1 and 0 ----------------------------------- + % The expression (navBits > 0) returns an array with elements set to 1 + % if the condition is met and set to 0 if it is not met. + navBits = (navBits > 0); + + %--- Convert from decimal to binary ----------------------------------- + % The function ephemeris expects input in binary form. In Matlab it is + % a string array containing only "0" and "1" characters. + navBitsBin = dec2bin(navBits); + + %=== Decode ephemerides and TOW of the first sub-frame ================ + [eph(trackResults(channelNr).PRN), TOW] = ... + ephemeris(navBitsBin(2:1501)', navBitsBin(1)); + + %--- Exclude satellite if it does not have the necessary nav data ----- + if (isempty(eph(trackResults(channelNr).PRN).IODC) || ... + isempty(eph(trackResults(channelNr).PRN).IODE_sf2) || ... + isempty(eph(trackResults(channelNr).PRN).IODE_sf3)) + + %--- Exclude channel from the list (from further processing) ------ + activeChnList = setdiff(activeChnList, channelNr); + end +end + +%% Check if the number of satellites is still above 3 ===================== +if (isempty(activeChnList) || (size(activeChnList, 2) < 4)) + % Show error message and exit + disp('Too few satellites with ephemeris data for postion calculations. Exiting!'); + navSolutions = []; + eph = []; + return +end + +%% Initialization ========================================================= + +% Set the satellite elevations array to INF to include all satellites for +% the first calculation of receiver position. There is no reference point +% to find the elevation angle as there is no receiver position estimate at +% this point. +satElev = inf(1, settings.numberOfChannels); + +% Save the active channel list. The list contains satellites that are +% tracked and have the required ephemeris data. In the next step the list +% will depend on each satellite's elevation angle, which will change over +% time. +readyChnList = activeChnList; + +transmitTime = TOW; + +%########################################################################## +%# Do the satellite and receiver position calculations # +%########################################################################## + +%% Initialization of current measurement ================================== +for currMeasNr = 1:fix((settings.msToProcess - max(subFrameStart)) / ... + settings.navSolPeriod) + + % Exclude satellites, that are belove elevation mask + activeChnList = intersect(find(satElev >= settings.elevationMask), ... + readyChnList); + + % Save list of satellites used for position calculation + navSolutions.channel.PRN(activeChnList, currMeasNr) = ... + [trackResults(activeChnList).PRN]; + + % These two lines help the skyPlot function. The satellites excluded + % do to elevation mask will not "jump" to possition (0,0) in the sky + % plot. + navSolutions.channel.el(:, currMeasNr) = ... + NaN(settings.numberOfChannels, 1); + navSolutions.channel.az(:, currMeasNr) = ... + NaN(settings.numberOfChannels, 1); + +%% Find pseudoranges ====================================================== + navSolutions.channel.rawP(:, currMeasNr) = calculatePseudoranges(... + trackResults, ... + subFrameStart + settings.navSolPeriod * (currMeasNr-1), ... + activeChnList, settings); + +%% Find satellites positions and clocks corrections ======================= + [satPositions, satClkCorr] = satpos(transmitTime, ... + [trackResults(activeChnList).PRN], ... + eph, settings); + +%% Find receiver position ================================================= + + % 3D receiver position can be found only if signals from more than 3 + % satellites are available + if size(activeChnList, 2) > 3 + + %=== Calculate receiver position ================================== + [xyzdt, ... + navSolutions.channel.el(activeChnList, currMeasNr), ... + navSolutions.channel.az(activeChnList, currMeasNr), ... + navSolutions.DOP(:, currMeasNr)] = ... + leastSquarePos(satPositions, ... + navSolutions.channel.rawP(activeChnList, currMeasNr)' + satClkCorr * settings.c, ... + settings); + + %--- Save results ------------------------------------------------- + navSolutions.X(currMeasNr) = xyzdt(1); + navSolutions.Y(currMeasNr) = xyzdt(2); + navSolutions.Z(currMeasNr) = xyzdt(3); + navSolutions.dt(currMeasNr) = xyzdt(4); + + % Update the satellites elevations vector + satElev = navSolutions.channel.el(:, currMeasNr); + + %=== Correct pseudorange measurements for clocks errors =========== + navSolutions.channel.correctedP(activeChnList, currMeasNr) = ... + navSolutions.channel.rawP(activeChnList, currMeasNr) + ... + satClkCorr' * settings.c + navSolutions.dt(currMeasNr); + +%% Coordinate conversion ================================================== + + %=== Convert to geodetic coordinates ============================== + [navSolutions.latitude(currMeasNr), ... + navSolutions.longitude(currMeasNr), ... + navSolutions.height(currMeasNr)] = cart2geo(... + navSolutions.X(currMeasNr), ... + navSolutions.Y(currMeasNr), ... + navSolutions.Z(currMeasNr), ... + 5); + + %=== Convert to UTM coordinate system ============================= + navSolutions.utmZone = findUtmZone(navSolutions.latitude(currMeasNr), ... + navSolutions.longitude(currMeasNr)); + + [navSolutions.E(currMeasNr), ... + navSolutions.N(currMeasNr), ... + navSolutions.U(currMeasNr)] = cart2utm(xyzdt(1), xyzdt(2), ... + xyzdt(3), ... + navSolutions.utmZone); + + else % if size(activeChnList, 2) > 3 + %--- There are not enough satellites to find 3D position ---------- + disp([' Measurement No. ', num2str(currMeasNr), ... + ': Not enough information for position solution.']); + + %--- Set the missing solutions to NaN. These results will be + %excluded automatically in all plots. For DOP it is easier to use + %zeros. NaN values might need to be excluded from results in some + %of further processing to obtain correct results. + navSolutions.X(currMeasNr) = NaN; + navSolutions.Y(currMeasNr) = NaN; + navSolutions.Z(currMeasNr) = NaN; + navSolutions.dt(currMeasNr) = NaN; + navSolutions.DOP(:, currMeasNr) = zeros(5, 1); + navSolutions.latitude(currMeasNr) = NaN; + navSolutions.longitude(currMeasNr) = NaN; + navSolutions.height(currMeasNr) = NaN; + navSolutions.E(currMeasNr) = NaN; + navSolutions.N(currMeasNr) = NaN; + navSolutions.U(currMeasNr) = NaN; + + navSolutions.channel.az(activeChnList, currMeasNr) = ... + NaN(1, length(activeChnList)); + navSolutions.channel.el(activeChnList, currMeasNr) = ... + NaN(1, length(activeChnList)); + + % TODO: Know issue. Satellite positions are not updated if the + % satellites are excluded do to elevation mask. Therefore rasing + % satellites will be not included even if they will be above + % elevation mask at some point. This would be a good place to + % update positions of the excluded satellites. + + end % if size(activeChnList, 2) > 3 + + %=== Update the transmit time ("measurement time") ==================== + transmitTime = transmitTime + settings.navSolPeriod / 1000; + +end %for currMeasNr... +x=0; \ No newline at end of file diff --git a/GNSS_SDR_IQ/postNavigation0.m b/GNSS_SDR_IQ/postNavigation0.m new file mode 100644 index 0000000..337c3bd --- /dev/null +++ b/GNSS_SDR_IQ/postNavigation0.m @@ -0,0 +1,273 @@ +function [navSolutions, eph,TOW] = postNavigation0(trackResults, settings) +%Function calculates navigation solutions for the receiver (pseudoranges, +%positions). At the end it converts coordinates from the WGS84 system to +%the UTM, geocentric or any additional coordinate system. +% +%[navSolutions, eph] = postNavigation(trackResults, settings) +% +% Inputs: +% trackResults - results from the tracking function (structure +% array). +% settings - receiver settings. +% Outputs: +% navSolutions - contains measured pseudoranges, receiver +% clock error, receiver coordinates in several +% coordinate systems (at least ECEF and UTM). +% eph - received ephemerides of all SV (structure array). + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis with help from Kristin Larson +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: postNavigation.m,v 1.1.2.22 2006/08/09 17:20:11 dpl Exp $ + +%% Check is there enough data to obtain any navigation solution =========== +% It is necessary to have at least three subframes (number 1, 2 and 3) to +% find satellite coordinates. Then receiver position can be found too. +% The function requires all 5 subframes, because the tracking starts at +% arbitrary point. Therefore the first received subframes can be any three +% from the 5. +% One subframe length is 6 seconds, therefore we need at least 30 sec long +% record (5 * 6 = 30 sec = 30000ms). We add extra seconds for the cases, +% when tracking has started in a middle of a subframe. + +if (settings.msToProcess < 36000) || (sum([trackResults.status] ~= '-') < 4) + % Show the error message and exit + disp('Record is to short or too few satellites tracked. Exiting!'); + navSolutions = []; + eph = []; + return +end + +%% Find preamble start positions ========================================== + +[subFrameStart, activeChnList] = findPreambles(trackResults, settings); + +%% Decode ephemerides ===================================================== + +for channelNr = activeChnList + + %=== Convert tracking output to navigation bits ======================= + + %--- Copy 5 sub-frames long record from tracking output --------------- + navBitsSamples = trackResults(channelNr).I_P(subFrameStart(channelNr) - 20 : ... + subFrameStart(channelNr) + (1500 * 20) -1)'; + + %--- Group every 20 vales of bits into columns ------------------------ + navBitsSamples = reshape(navBitsSamples, ... + 20, (size(navBitsSamples, 1) / 20)); + + %--- Sum all samples in the bits to get the best estimate ------------- + navBits = sum(navBitsSamples); + + %--- Now threshold and make 1 and 0 ----------------------------------- + % The expression (navBits > 0) returns an array with elements set to 1 + % if the condition is met and set to 0 if it is not met. + navBits = (navBits > 0); + + %--- Convert from decimal to binary ----------------------------------- + % The function ephemeris expects input in binary form. In Matlab it is + % a string array containing only "0" and "1" characters. + navBitsBin = dec2bin(navBits); + + %=== Decode ephemerides and TOW of the first sub-frame ================ + [eph(trackResults(channelNr).PRN), TOW] = ... + ephemeris(navBitsBin(2:1501)', navBitsBin(1)); + + %--- Exclude satellite if it does not have the necessary nav data ----- + if (isempty(eph(trackResults(channelNr).PRN).IODC) || ... + isempty(eph(trackResults(channelNr).PRN).IODE_sf2) || ... + isempty(eph(trackResults(channelNr).PRN).IODE_sf3)) + + %--- Exclude channel from the list (from further processing) ------ + activeChnList = setdiff(activeChnList, channelNr); + end +end + +%% Check if the number of satellites is still above 3 ===================== +if (isempty(activeChnList) || (size(activeChnList, 2) < 4)) + % Show error message and exit + disp('Too few satellites with ephemeris data for postion calculations. Exiting!'); + navSolutions = []; + eph = []; + return +end + +%% Initialization ========================================================= + +% Set the satellite elevations array to INF to include all satellites for +% the first calculation of receiver position. There is no reference point +% to find the elevation angle as there is no receiver position estimate at +% this point. +satElev = inf(1, settings.numberOfChannels); + +% Save the active channel list. The list contains satellites that are +% tracked and have the required ephemeris data. In the next step the list +% will depend on each satellite's elevation angle, which will change over +% time. +readyChnList = activeChnList; + +transmitTime = TOW; + +%########################################################################## +%# Do the satellite and receiver position calculations # +%########################################################################## +prevClkDrift=0; +bias=0; +%Start Clock in samples +%Find the seconds nearest the startOfFrame and converting it to samples +%tmp=ceil(trackResults(1).absoluteSample(subFrameStart(1))/settings.samplingFreq); +%startClock_PVT_GPS=tmp*settings.samplingFreq; +%% Initialization of current measurement ================================== +for currMeasNr = 1:fix((settings.msToProcess - max(subFrameStart)) / ... + settings.navSolPeriod) + + % Exclude satellites, that are belove elevation mask + activeChnList = intersect(find(satElev >= settings.elevationMask), ... + readyChnList); + + % Save list of satellites used for position calculation + navSolutions.channel.PRN(activeChnList, currMeasNr) = ... + [trackResults(activeChnList).PRN]; + + % These two lines help the skyPlot fun ction. The satellites excluded + % do to elevation mask will not "jump" to possition (0,0) in the sky + % plot. + navSolutions.channel.el(:, currMeasNr) = ... + NaN(settings.numberOfChannels, 1); + navSolutions.channel.az(:, currMeasNr) = ... + NaN(settings.numberOfChannels, 1); + + + %% THUAN + if currMeasNr>1 + settings.startOffset=settings.startOffset-1000*(navSolutions.dt(currMeasNr-1))/settings.c; + end; + + navSolutions.channel.rawP(:, currMeasNr) = calculatePseudoranges(... + trackResults, ... + subFrameStart+settings.navSolPeriod*(currMeasNr-1)-1, ... + activeChnList, settings); + + xx=subFrameStart+settings.navSolPeriod*(currMeasNr-1); + for channelNr = activeChnList + yy(channelNr)=(1023-trackResults(channelNr).remCodePhase(xx(channelNr)-1))/trackResults(channelNr).codeFreq(xx(channelNr)-1)*settings.c; + end; + + + navSolutions.channel.rawP(:, currMeasNr)=navSolutions.channel.rawP(:, currMeasNr)+yy'; + %% END THUAN + +%% Find satellites positions and clocks corrections ======================= + [satPositions, satClkCorr] = satpos(transmitTime, ... + [trackResults(activeChnList).PRN], ... + eph, settings); + +%% Find receiver position ================================================= + + % 3D receiver position can be found only if signals from more than 3 + % satellites are available + if size(activeChnList, 2) > 3 + + %=== Calculate receiver position ================================== + [xyzdt, ... + navSolutions.channel.el(activeChnList, currMeasNr), ... + navSolutions.channel.az(activeChnList, currMeasNr), ... + navSolutions.DOP(:, currMeasNr)] = ... + leastSquarePos(satPositions, ... + navSolutions.channel.rawP(activeChnList, currMeasNr)' + satClkCorr * settings.c, ... + settings); + + %--- Save results ------------------------------------------------- + navSolutions.X(currMeasNr) = xyzdt(1); + navSolutions.Y(currMeasNr) = xyzdt(2); + navSolutions.Z(currMeasNr) = xyzdt(3); + navSolutions.dt(currMeasNr) = xyzdt(4); + + % Update the satellites elevations vector + satElev = navSolutions.channel.el(:, currMeasNr); + + %=== Correct pseudorange measurements for clocks errors =========== + navSolutions.channel.correctedP(activeChnList, currMeasNr) = ... + navSolutions.channel.rawP(activeChnList, currMeasNr) + ... + satClkCorr' * settings.c + navSolutions.dt(currMeasNr); + +%% Coordinate conversion ================================================== + + %=== Convert to geodetic coordinates ============================== + [navSolutions.latitude(currMeasNr), ... + navSolutions.longitude(currMeasNr), ... + navSolutions.height(currMeasNr)] = cart2geo(... + navSolutions.X(currMeasNr), ... + navSolutions.Y(currMeasNr), ... + navSolutions.Z(currMeasNr), ... + 5); + + %=== Convert to UTM coordinate system ============================= + navSolutions.utmZone = findUtmZone(navSolutions.latitude(currMeasNr), ... + navSolutions.longitude(currMeasNr)); + + [navSolutions.E(currMeasNr), ... + navSolutions.N(currMeasNr), ... + navSolutions.U(currMeasNr)] = cart2utm(xyzdt(1), xyzdt(2), ... + xyzdt(3), ... + navSolutions.utmZone); + + else % if size(activeChnList, 2) > 3 + %--- There are not enough satellites to find 3D position ---------- + disp([' Measurement No. ', num2str(currMeasNr), ... + ': Not enough information for position solution.']); + + %--- Set the missing solutions to NaN. These results will be + %excluded automatically in all plots. For DOP it is easier to use + %zeros. NaN values might need to be excluded from results in some + %of further processing to obtain correct results. + navSolutions.X(currMeasNr) = NaN; + navSolutions.Y(currMeasNr) = NaN; + navSolutions.Z(currMeasNr) = NaN; + navSolutions.dt(currMeasNr) = NaN; + navSolutions.DOP(:, currMeasNr) = zeros(5, 1); + navSolutions.latitude(currMeasNr) = NaN; + navSolutions.longitude(currMeasNr) = NaN; + navSolutions.height(currMeasNr) = NaN; + navSolutions.E(currMeasNr) = NaN; + navSolutions.N(currMeasNr) = NaN; + navSolutions.U(currMeasNr) = NaN; + + navSolutions.channel.az(activeChnList, currMeasNr) = ... + NaN(1, length(activeChnList)); + navSolutions.channel.el(activeChnList, currMeasNr) = ... + NaN(1, length(activeChnList)); + + % TODO: Know issue. Satellite positions are not updated if the + % satellites are excluded do to elevation mask. Therefore rasing + % satellites will be not included even if they will be above + % elevation mask at some point. This would be a good place to + % update positions of the excluded satellites. + + end % if size(activeChnList, 2) > 3 + + %=== Update the transmit time ("measurement time") ==================== + transmitTime = transmitTime + settings.navSolPeriod / 1000; + +end %for currMeasNr... +x=0; \ No newline at end of file diff --git a/GNSS_SDR_IQ/postNavigationSnapshot.m b/GNSS_SDR_IQ/postNavigationSnapshot.m new file mode 100644 index 0000000..919a293 --- /dev/null +++ b/GNSS_SDR_IQ/postNavigationSnapshot.m @@ -0,0 +1,282 @@ +function [navSolutions, eph,TOW] = postNavigationSnapshot(trackResults, settings) +%Function calculates navigation solutions for the receiver (pseudoranges, +%positions). At the end it converts coordinates from the WGS84 system to +%the UTM, geocentric or any additional coordinate system. +% +%[navSolutions, eph] = postNavigation(trackResults, settings) +% +% Inputs: +% trackResults - results from the tracking function (structure +% array). +% settings - receiver settings. +% Outputs: +% navSolutions - contains measured pseudoranges, receiver +% clock error, receiver coordinates in several +% coordinate systems (at least ECEF and UTM). +% eph - received ephemerides of all SV (structure array). + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis with help from Kristin Larson +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: postNavigation.m,v 1.1.2.22 2006/08/09 17:20:11 dpl Exp $ + +%% Check is there enough data to obtain any navigation solution =========== +% It is necessary to have at least three subframes (number 1, 2 and 3) to +% find satellite coordinates. Then receiver position can be found too. +% The function requires all 5 subframes, because the tracking starts at +% arbitrary point. Therefore the first received subframes can be any three +% from the 5. +% One subframe length is 6 seconds, therefore we need at least 30 sec long +% record (5 * 6 = 30 sec = 30000ms). We add extra seconds for the cases, +% when tracking has started in a middle of a subframe. + +if (settings.msToProcess < 36000) || (sum([trackResults.status] ~= '-') < 4) + % Show the error message and exit + disp('Record is to short or too few satellites tracked. Exiting!'); + navSolutions = []; + eph = []; + return +end + +%% Find preamble start positions ========================================== + +[subFrameStart, activeChnList] = findPreambles(trackResults, settings); +%% Decode ephemerides ===================================================== + +for channelNr = activeChnList + + %=== Convert tracking output to navigation bits ======================= + + %--- Copy 5 sub-frames long record from tracking output --------------- + navBitsSamples = trackResults(channelNr).I_P(subFrameStart(channelNr) - 20 : ... + subFrameStart(channelNr) + (1500 * 20) -1)'; + + %--- Group every 20 vales of bits into columns ------------------------ + navBitsSamples = reshape(navBitsSamples, ... + 20, (size(navBitsSamples, 1) / 20)); + + %--- Sum all samples in the bits to get the best estimate ------------- + navBits = sum(navBitsSamples); + + %--- Now threshold and make 1 and 0 ----------------------------------- + % The expression (navBits > 0) returns an array with elements set to 1 + % if the condition is met and set to 0 if it is not met. + navBits = (navBits > 0); + + %--- Convert from decimal to binary ----------------------------------- + % The function ephemeris expects input in binary form. In Matlab it is + % a string array containing only "0" and "1" characters. + navBitsBin = dec2bin(navBits); + + %=== Decode ephemerides and TOW of the first sub-frame ================ + [eph(trackResults(channelNr).PRN), TOW] = ... + ephemeris(navBitsBin(2:1501)', navBitsBin(1)); + + %--- Exclude satellite if it does not have the necessary nav data ----- + if (isempty(eph(trackResults(channelNr).PRN).IODC) || ... + isempty(eph(trackResults(channelNr).PRN).IODE_sf2) || ... + isempty(eph(trackResults(channelNr).PRN).IODE_sf3)) + + %--- Exclude channel from the list (from further processing) ------ + activeChnList = setdiff(activeChnList, channelNr); + end +end + +%% Check if the number of satellites is still above 3 ===================== +if (isempty(activeChnList) || (size(activeChnList, 2) < 4)) + % Show error message and exit + disp('Too few satellites with ephemeris data for postion calculations. Exiting!'); + navSolutions = []; + eph = []; + return +end + +%% Initialization ========================================================= + +% Set the satellite elevations array to INF to include all satellites for +% the first calculation of receiver position. There is no reference point +% to find the elevation angle as there is no receiver position estimate at +% this point. +satElev = inf(1, settings.numberOfChannels); + +% Save the active channel list. The list contains satellites that are +% tracked and have the required ephemeris data. In the next step the list +% will depend on each satellite's elevation angle, which will change over +% time. +readyChnList = activeChnList; + +transmitTime = TOW; + +%########################################################################## +%# Do the satellite and receiver position calculations # +%########################################################################## +prevClkDrift=0; +bias=0; +%Start Clock in samples +%Find the seconds nearest the startOfFrame and converting it to samples +%tmp=ceil(trackResults(1).absoluteSample(subFrameStart(1))/settings.samplingFreq); +%startClock_PVT_GPS=tmp*settings.samplingFreq; +%% Initialization of current measurement ================================== +for currMeasNr = 1:240 + + % Exclude satellites, that are belove elevation mask + activeChnList = intersect(find(satElev >= settings.elevationMask), ... + readyChnList); + + % Save list of satellites used for position calculation + navSolutions.channel.PRN(activeChnList, currMeasNr) = ... + [trackResults(activeChnList).PRN]; + + % These two lines help the skyPlot function. The satellites excluded + % do to elevation mask will not "jump" to possition (0,0) in the sky + % plot. + navSolutions.channel.el(:, currMeasNr) = ... + NaN(settings.numberOfChannels, 1); + navSolutions.channel.az(:, currMeasNr) = ... + NaN(settings.numberOfChannels, 1); + + + %% THUAN + if currMeasNr>1 + settings.startOffset=settings.startOffset-1000*(navSolutions.dt(currMeasNr-1))/settings.c; + end; + + navSolutions.channel.rawP(:, currMeasNr) = calculatePseudoranges(... + trackResults, ... + subFrameStart+settings.navSolPeriod*(currMeasNr-1), ... + activeChnList, settings); +% navSolutions.channel.rawP(:, currMeasNr) = calculatePseudoranges(... +% trackResults, ... +% subFrameStart+settings.navSolPeriod*(currMeasNr-1)-1, ... +% activeChnList, settings); +% + xx=subFrameStart+settings.navSolPeriod*(currMeasNr-1); + yy=inf(1,settings.numberOfChannels); + if size(activeChnList,1)>1 + activeChnList=activeChnList'; + end; + for channelNr = activeChnList + yy(channelNr)=(1023-trackResults(channelNr).remCodePhase(xx(channelNr)-1))/trackResults(channelNr).codeFreq(xx(channelNr)-1)*settings.c; + Doppler(channelNr)=trackResults(channelNr).carrFreq(xx(channelNr)); + end; +% navSolutions.channel.rawP(:, currMeasNr)=navSolutions.channel.rawP(:, currMeasNr)+yy'; + + % navSolutions.channel.carrFreq(:,currMeasNr)=trackResults(channelNr + %% END THUAN + +%% Find satellites positions and clocks corrections ======================= + [satPositions, satClkCorr] = satpos(transmitTime+0, ... + [trackResults(activeChnList).PRN], ... + eph, settings); + +%% Find receiver position ================================================= + + % 3D receiver position can be found only if signals from more than 3 + % satellites are available + if size(activeChnList, 2) > 3 + + %=== Calculate receiver position ================================== + ps_fract1ms=rem(navSolutions.channel.rawP(activeChnList, currMeasNr)',settings.c*0.001); + ps_1ms=floor(navSolutions.channel.rawP(activeChnList, currMeasNr)'/(settings.c*0.001)); + [xyzdt, ... + navSolutions.channel.el(activeChnList, currMeasNr), ... + navSolutions.channel.az(activeChnList, currMeasNr), ... + navSolutions.DOP(:, currMeasNr)] = ... + leastSquarePos_Snapshot(satPositions, ... + ps_fract1ms,ps_1ms,Doppler, satClkCorr, ... + settings); + + %--- Save results ------------------------------------------------- + navSolutions.X(currMeasNr) = xyzdt(1); + navSolutions.Y(currMeasNr) = xyzdt(2); + navSolutions.Z(currMeasNr) = xyzdt(3); + navSolutions.dt(currMeasNr) = xyzdt(4); + + % Update the satellites elevations vector + satElev = navSolutions.channel.el(:, currMeasNr); + + %=== Correct pseudorange measurements for clocks errors =========== + navSolutions.channel.correctedP(activeChnList, currMeasNr) = ... + navSolutions.channel.rawP(activeChnList, currMeasNr) + ... + satClkCorr' * settings.c + navSolutions.dt(currMeasNr); + +%% Coordinate conversion ================================================== + + %=== Convert to geodetic coordinates ============================== + [navSolutions.latitude(currMeasNr), ... + navSolutions.longitude(currMeasNr), ... + navSolutions.height(currMeasNr)] = cart2geo(... + navSolutions.X(currMeasNr), ... + navSolutions.Y(currMeasNr), ... + navSolutions.Z(currMeasNr), ... + 5); + + %=== Convert to UTM coordinate system ============================= + navSolutions.utmZone = findUtmZone(navSolutions.latitude(currMeasNr), ... + navSolutions.longitude(currMeasNr)); + + [navSolutions.E(currMeasNr), ... + navSolutions.N(currMeasNr), ... + navSolutions.U(currMeasNr)] = cart2utm(xyzdt(1), xyzdt(2), ... + xyzdt(3), ... + navSolutions.utmZone); + + else % if size(activeChnList, 2) > 3 + %--- There are not enough satellites to find 3D position ---------- + disp([' Measurement No. ', num2str(currMeasNr), ... + ': Not enough information for position solution.']); + + %--- Set the missing solutions to NaN. These results will be + %excluded automatically in all plots. For DOP it is easier to use + %zeros. NaN values might need to be excluded from results in some + %of further processing to obtain correct results. + navSolutions.X(currMeasNr) = NaN; + navSolutions.Y(currMeasNr) = NaN; + navSolutions.Z(currMeasNr) = NaN; + navSolutions.dt(currMeasNr) = NaN; + navSolutions.DOP(:, currMeasNr) = zeros(5, 1); + navSolutions.latitude(currMeasNr) = NaN; + navSolutions.longitude(currMeasNr) = NaN; + navSolutions.height(currMeasNr) = NaN; + navSolutions.E(currMeasNr) = NaN; + navSolutions.N(currMeasNr) = NaN; + navSolutions.U(currMeasNr) = NaN; + + navSolutions.channel.az(activeChnList, currMeasNr) = ... + NaN(1, length(activeChnList)); + navSolutions.channel.el(activeChnList, currMeasNr) = ... + NaN(1, length(activeChnList)); + + % TODO: Know issue. Satellite positions are not updated if the + % satellites are excluded do to elevation mask. Therefore rasing + % satellites will be not included even if they will be above + % elevation mask at some point. This would be a good place to + % update positions of the excluded satellites. + + end % if size(activeChnList, 2) > 3 + + %=== Update the transmit time ("measurement time") ==================== + transmitTime = transmitTime+ settings.navSolPeriod / 1000; + +end %for currMeasNr... +x=0; \ No newline at end of file diff --git a/GNSS_SDR_IQ/postProcessing.m b/GNSS_SDR_IQ/postProcessing.m new file mode 100644 index 0000000..4dbdbb9 --- /dev/null +++ b/GNSS_SDR_IQ/postProcessing.m @@ -0,0 +1,144 @@ +%-------------------------------------------------------------------------- +% Script postProcessing.m processes the raw signal from the specified data +% file (in settings) operating on blocks of 37 seconds of data. +% +% First it runs acquisition code identifying the satellites in the file, +% then the code and carrier for each of the satellites are tracked, storing +% the 1msec accumulations. After processing all satellites in the 37 sec +% data block, then postNavigation is called. It calculates pseudoranges +% and attempts a position solutions. At the end plots are made for that +% block of data. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis, Dennis M. Akos +% Some ideas by Dennis M. Akos +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% THE SCRIPT "RECIPE" +% +% The purpose of this script is to combine all parts of the software +% receiver. +% +% 1.1) Open the data file for the processing and seek to desired point. +% +% 2.1) Acquire satellites +% +% 3.1) Initialize channels (preRun.m). +% 3.2) Pass the channel structure and the file identifier to the tracking +% function. It will read and process the data. The tracking results are +% stored in the trackResults structure. The results can be accessed this +% way (the results are stored each millisecond): +% trackResults(channelNumber).XXX(fromMillisecond : toMillisecond), where +% XXX is a field name of the result (e.g. I_P, codePhase etc.) +% +% 4) Pass tracking results to the navigation solution function. It will +% decode navigation messages, find satellite positions, measure +% pseudoranges and find receiver position. +% +% 5) Plot the results. + +%% Initialization ========================================================= +disp ('Starting processing...'); + +[fid, message] = fopen(settings.fileName, 'rb'); + +%If success, then process the data +if (fid > 0) + + % Move the starting point of processing. Can be used to start the + % signal processing at any point in the data record (e.g. good for long + % records or for signal processing in blocks). + fseek(fid, settings.skipNumberOfBytes, 'bof'); + +%% Acquisition ============================================================ + + % Do acquisition if it is not disabled in settings or if the variable + % acqResults does not exist. + if ((settings.skipAcquisition == 0) || ~exist('acqResults', 'var')) + + % Find number of samples per spreading code + samplesPerCode = round(settings.samplingFreq / ... + (settings.codeFreqBasis / settings.codeLength)); + + % Read data for acquisition. 11ms of signal are needed for the fine + % frequency estimation + data = fread(fid, 11*samplesPerCode, settings.dataType)'; + + %--- Do the acquisition ------------------------------------------- + disp (' Acquiring satellites...'); + acqResults = acquisition(data, settings); + + plotAcquisition(acqResults); + end + +%% Initialize channels and prepare for the run ============================ + + % Start further processing only if a GNSS signal was acquired (the + % field FREQUENCY will be set to 0 for all not acquired signals) + if (any(acqResults.carrFreq)) + channel = preRun(acqResults, settings); + showChannelStatus(channel, settings); + else + % No satellites to track, exit + disp('No GNSS signals detected, signal processing finished.'); + trackResults = []; + return; + end + +%% Track the signal ======================================================= + startTime = now; + disp ([' Tracking started at ', datestr(startTime)]); + + % Process all channels for given data block + [trackResults, channel] = tracking(fid, channel, settings); + + % Close the data file + fclose(fid); + + disp([' Tracking is over (elapsed time ', ... + datestr(now - startTime, 13), ')']) + + % Auto save the acquisition & tracking results to a file to allow + % running the positioning solution afterwards. + disp(' Saving Acq & Tracking results to file "trackingResults.mat"') + save('trackingResults', ... + 'trackResults', 'settings', 'acqResults', 'channel'); + +%% Calculate navigation solutions ========================================= + disp(' Calculating navigation solutions...'); + navSolutions = postNavigation(trackResults, settings); + + disp(' Processing is complete for this data block'); + +%% Plot all results =================================================== + disp (' Ploting results...'); + if settings.plotTracking + plotTracking(1:settings.numberOfChannels, trackResults, settings); + end + + plotNavigation0(navSolutions, settings); + + disp('Post processing of the signal is over.'); + +else + % Error while opening the data file. + error('Unable to read file %s: %s.', settings.fileName, message); +end % if (fid > 0) diff --git a/GNSS_SDR_IQ/probeData.m b/GNSS_SDR_IQ/probeData.m new file mode 100644 index 0000000..2971846 --- /dev/null +++ b/GNSS_SDR_IQ/probeData.m @@ -0,0 +1,118 @@ +function probeData(varargin) +%Function plots raw data information: time domain plot, a frequency domain +%plot and a histogram. +% +%The function can be called in two ways: +% probeData(settings) +% or +% probeData(fileName, settings) +% +% Inputs: +% fileName - name of the data file. File name is read from +% settings if parameter fileName is not provided. +% +% settings - receiver settings. Type of data file, sampling +% frequency and the default filename are specified +% here. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Dennis M. Akos +% Written by Darius Plausinaitis and Dennis M. Akos +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% CVS record: +% $Id: probeData.m,v 1.1.2.7 2006/08/22 13:46:00 dpl Exp $ + +%% Check the number of arguments ========================================== +if (nargin == 1) + settings = deal(varargin{1}); + fileNameStr = settings.fileName; +elseif (nargin == 2) + [fileNameStr, settings] = deal(varargin{1:2}); + if ~ischar(fileNameStr) + error('File name must be a string'); + end +else + error('Incorect number of arguments'); +end + +%% Generate plot of raw data ============================================== +[fid, message] = fopen(fileNameStr, 'rb'); + +if (fid > 0) + % Move the starting point of processing. Can be used to start the + % signal processing at any point in the data record (e.g. for long + % records). + fseek(fid, settings.skipNumberOfBytes, 'bof'); + + % Find number of samples per spreading code + samplesPerCode = round(settings.samplingFreq / ... + (settings.codeFreqBasis / settings.codeLength)); + + % Read 10ms of signal + [data, count] = fread(fid, [1, 10*samplesPerCode], settings.dataType); + + fclose(fid); + + if (count < 10*samplesPerCode) + % The file is to short + error('Could not read enough data from the data file.'); + end + + %--- Initialization --------------------------------------------------- + figure(100); + clf(100); + + timeScale = 0 : 1/settings.samplingFreq : 5e-3; + + %--- Time domain plot ------------------------------------------------- + subplot(2, 2, 1); + plot(1000 * timeScale(1:round(samplesPerCode/50)), ... + data(1:round(samplesPerCode/50))); + + axis tight; + grid on; + title ('Time domain plot'); + xlabel('Time (ms)'); ylabel('Amplitude'); + + %--- Frequency domain plot -------------------------------------------- + subplot(2,2,2); + pwelch(data-mean(data), 16384, 1024, 2048, settings.samplingFreq/1e6) + + axis tight; + grid on; + title ('Frequency domain plot'); + xlabel('Frequency (MHz)'); ylabel('Magnitude'); + + %--- Histogram -------------------------------------------------------- + subplot(2, 2, 3.5); + hist(data, -128:128) + + dmax = max(abs(data)) + 1; + axis tight; + adata = axis; + axis([-dmax dmax adata(3) adata(4)]); + grid on; + title ('Histogram'); + xlabel('Bin'); ylabel('Number in bin'); +else + %=== Error while opening the data file ================================ + error('Unable to read file %s: %s.', fileNameStr, message); +end % if (fid > 0) diff --git a/GNSS_SDR_IQ/setSettings.fig b/GNSS_SDR_IQ/setSettings.fig new file mode 100644 index 0000000..de7b922 Binary files /dev/null and b/GNSS_SDR_IQ/setSettings.fig differ diff --git a/GNSS_SDR_IQ/setSettings.m b/GNSS_SDR_IQ/setSettings.m new file mode 100644 index 0000000..888792f --- /dev/null +++ b/GNSS_SDR_IQ/setSettings.m @@ -0,0 +1,384 @@ +function varargout = setSettings(varargin) +% SETSETTINGS M-file for setSettings.fig +% SETSETTINGS, by itself, creates a new SETSETTINGS or raises the existing +% singleton*. +% +% H = SETSETTINGS returns the handle to a new SETSETTINGS or the handle to +% the existing singleton*. +% +% SETSETTINGS('CALLBACK',hObject,eventData,handles,...) calls the local +% function named CALLBACK in SETSETTINGS.M with the given input arguments. +% +% SETSETTINGS('Property','Value',...) creates a new SETSETTINGS or raises the +% existing singleton*. Starting from the left, property value pairs are +% applied to the GUI before setSettings_OpeningFunction gets called. An +% unrecognized property name or invalid value makes property application +% stop. All inputs are passed to setSettings_OpeningFcn via varargin. +% +% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one +% instance to run (singleton)". +% +% See also: GUIDE, GUIDATA, GUIHANDLES + +% Edit the above text to modify the response to help setSettings + +% Last Modified by GUIDE v2.5 02-Aug-2006 10:33:09 + +% Begin initialization code - DO NOT EDIT +gui_Singleton = 1; +gui_State = struct('gui_Name', mfilename, ... + 'gui_Singleton', gui_Singleton, ... + 'gui_OpeningFcn', @setSettings_OpeningFcn, ... + 'gui_OutputFcn', @setSettings_OutputFcn, ... + 'gui_LayoutFcn', [] , ... + 'gui_Callback', []); +if nargin && ischar(varargin{1}) + gui_State.gui_Callback = str2func(varargin{1}); +end + +if nargout + [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); +else + gui_mainfcn(gui_State, varargin{:}); +end +% End initialization code - DO NOT EDIT + + +% --- Executes just before setSettings is made visible. +function setSettings_OpeningFcn(hObject, eventdata, handles, varargin) +% This function has no output args, see OutputFcn. +% hObject handle to figure +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) +% varargin command line arguments to setSettings (see VARARGIN) + +%--- Try to read data from the variable "settings" ------------------------ +% (variable "settings" is in the base Matlab workspace) +try + handles.settings = evalin('base', 'settings'); +catch + %--- Creat a new settings structure in case of an error --------------- + handles.settings = initSettings(); +end + +%--- Assign it to the GUI data structure ---------------------------------- +loadSettings(handles); + +% Choose default command line output for setSettings +handles.output = hObject; + +% Update handles structure +guidata(hObject, handles); + +% UIWAIT makes setSettings wait for user response (see UIRESUME) +% uiwait(handles.figure1); + + +% --- Outputs from this function are returned to the command line. +function varargout = setSettings_OutputFcn(hObject, eventdata, handles) +% varargout cell array for returning output args (see VARARGOUT); +% hObject handle to figure +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Get default command line output from handles structure +varargout{1} = handles.output; + +%### Callback functions ################################################### + +% --- Executes during object creation, after setting all properties. +function edit_CreateFcn(hObject, eventdata, handles) +% hObject handle to edit (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles empty - handles not created until after all CreateFcns called + +% Hint: edit controls usually have a white background on Windows. +% See ISPC and COMPUTER. +if ispc && isequal(get(hObject,'BackgroundColor'), ... + get(0,'defaultUicontrolBackgroundColor')) + set(hObject,'BackgroundColor','white'); +end + + +function edit_Callback(hObject, eventdata, handles) +% hObject handle to edit (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Hints: get(hObject,'String') returns contents of edit as text +% str2double(get(hObject,'String')) returns contents of edit as a double + +%--- Enable the apply button on any input event --------------------------- +set(handles.pushbuttonApply, 'Enable', 'on'); + + +% --- Executes on button press in any checkbox. +function checkbox_Callback(hObject, eventdata, handles) +% hObject handle to PRN1checkbox (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) +% Hint: get(hObject,'Value') returns toggle state of PRN1checkbox + +%--- Enable the apply button on any input event --------------------------- +set(handles.pushbuttonApply, 'Enable', 'on'); + +% --- Executes on button press in applybutton. +function pushbuttonApply_Callback(hObject, eventdata, handles) +% hObject handle to applybutton (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +%--- Try to read values from the GUI input fields ------------------------- +[settings, error] = saveSettings(handles); + +%--- If no errors, then ... +if error == 0 + %--- Save the updated settings in the main workspace ------------------ + assignin('base', 'settings', settings); + + % Turn off the apply button + set(hObject, 'Enable', 'off'); +end + +% --- Executes on button press in resetbutton. +function pushbuttonReset_Callback(hObject, eventdata, handles) +% hObject handle to resetbutton (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +%--- Try to read data from the variable "settings" ------------------------ +% (variable "settings" is in the base Matlab workspace) +try + handles.settings = evalin('base', 'settings'); +catch + %--- Create a new settings structure in case of an error -------------- + handles.settings = initSettings(); +end + +%--- Assign it to the GUI data structure ---------------------------------- +loadSettings(handles); + +%--- Save changes in the GUI data structure ------------------------------- +guidata(hObject, handles); + +%--- Turn off the apply button -------------------------------------------- +set(handles.pushbuttonApply, 'Enable', 'off'); + +%@@@ Function reads values from the GUI and updates the settings structure +function [settings, error] = saveSettings(handles) +settings = handles.settings; +error = 0; % no error + +try + %Please read the Matlab help for mo details on TRY, CATCH and ERROR + %commands. + + %--- Signal properties related fields --------------------------------- + settings.fileName = get(handles.editFileName, 'String'); + settings.numberOfChannels = edit2double(handles.editNumberOfChannels); + settings.msToProcess = edit2double(handles.editMsToProcess); + settings.skipNumberOfBytes = edit2double(handles.editSkipNumberOfBytes); + settings.IF = edit2double(handles.editIF); + settings.samplingFreq = edit2double(handles.editSamplingFreq); + settings.dataType = get(handles.editDataType, 'String'); + + %--- Satellite PRN numbers -------------------------------------------- + for PRN = 1:32 + %If checkbox is checked + if getCheckbox(getfield(handles, ['checkboxPRN', num2str(PRN)])) == 1 + + % Include satellite in the list + settings.acqSatelliteList = ... + union(settings.acqSatelliteList, PRN); + else + % Exclude satellite from the list + settings.acqSatelliteList = ... + setdiff(settings.acqSatelliteList, PRN); + end + end + + %--- Acquisition parameters ------------------------------------------- + settings.acqSearchBand = edit2double(handles.editAcqSearchBand); + settings.acqThreshold = edit2double(handles.editAcqThreshold); + settings.skipAcquisition = getCheckbox(handles.checkboxSkipAcquisition); + + %--- Tracking --------------------------------------------------------- + settings.dllCorrelatorSpacing = edit2double(handles.editDllCorrelatorSpacing); + settings.dllDampingRatio = edit2double(handles.editDllDampingRatio); + settings.dllNoiseBandwidth = edit2double(handles.editDllNoiseBandwidth); + settings.pllDampingRatio = edit2double(handles.editPllDampingRatio); + settings.pllNoiseBandwidth = edit2double(handles.editPllNoiseBandwidth); + + %--- Nav solutions ---------------------------------------------------- + settings.elevationMask = edit2double(handles.editElevationMask); + settings.navSolPeriod = edit2double(handles.editNavSolPeriod); + settings.useTropCorr = getCheckbox(handles.checkboxUseTropCorr); + settings.truePosition.E = edit2double(handles.editUtmE); + settings.truePosition.N = edit2double(handles.editUtmN); + settings.truePosition.U = edit2double(handles.editUtmU); + + %--- Plotting --------------------------------------------------------- + settings.plotTracking = getCheckbox(handles.checkboxPlotTracking); + +catch + %Please read the Matlab help for mo details on TRY, CATCH and ERROR + %commands. + + %--- Read error information ------------------------------------------- + e = lasterror; + + %If this error caused by bad input + if strcmp(e.identifier, 'setSettings:badInput') + % then do not save settings, return an error indication + error = 1; + else + % Not our error, this error must be handled/reported in the system + rethrow(e); + end +end + +%@@@ Function loads the settings into the GUI @@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +function loadSettings(handles) + +%--- Signal properties related fields ------------------------------------- +set(handles.editFileName, 'String', handles.settings.fileName); +set(handles.editNumberOfChannels, 'String', num2str(handles.settings.numberOfChannels)); +set(handles.editSkipNumberOfBytes, 'String', num2str(handles.settings.skipNumberOfBytes)); +set(handles.editMsToProcess, 'String', num2str(handles.settings.msToProcess)); +set(handles.editIF, 'String', num2str(handles.settings.IF)); +set(handles.editSamplingFreq, 'String', num2str(handles.settings.samplingFreq )); +set(handles.editDataType, 'String', handles.settings.dataType); + +%--- Satellite PRN numbers ------------------------------------------------ +for PRN = 1:32 + % If the PRN number is in the list + if ismember(PRN, handles.settings.acqSatelliteList) + % then set the checkbox to "checked" state + setCheckbox(getfield(handles, ['checkboxPRN', num2str(PRN)]), 1); + else + % set the checkbox to "unchecked" state + setCheckbox(getfield(handles, ['checkboxPRN', num2str(PRN)]), 0); + end +end + +%--- Acquisition parameters ------------------------------------------- +set(handles.editAcqSearchBand, 'String', num2str(handles.settings.acqSearchBand)); +set(handles.editAcqThreshold, 'String', num2str(handles.settings.acqThreshold)); +setCheckbox(handles.checkboxSkipAcquisition, handles.settings.skipAcquisition); + +%--- Tracking --------------------------------------------------------- +set(handles.editDllCorrelatorSpacing, 'String', num2str(handles.settings.dllCorrelatorSpacing)); +set(handles.editDllDampingRatio, 'String', num2str(handles.settings.dllDampingRatio)); +set(handles.editDllNoiseBandwidth, 'String', num2str(handles.settings.dllNoiseBandwidth)); +set(handles.editPllDampingRatio, 'String', num2str(handles.settings.pllDampingRatio)); +set(handles.editPllNoiseBandwidth, 'String', num2str(handles.settings.pllNoiseBandwidth)); + +%--- Nav solutions -------------------------------------------------------- +set(handles.editElevationMask, 'String', num2str(handles.settings.elevationMask)); +set(handles.editNavSolPeriod, 'String', num2str(handles.settings.navSolPeriod)); +setCheckbox(handles.checkboxUseTropCorr, handles.settings.useTropCorr); +set(handles.editUtmE, 'String', num2str(handles.settings.truePosition.E)); +set(handles.editUtmN, 'String', num2str(handles.settings.truePosition.N)); +set(handles.editUtmU, 'String', num2str(handles.settings.truePosition.U)); + +%--- Plotting ------------------------------------------------------------- +setCheckbox(handles.checkboxPlotTracking, handles.settings.plotTracking); + +%@@@ Function reads current state of a checkbox "in the Matlab way" @@@@@@@ +function value = getCheckbox(handle) + +if (get(handle, 'Value') == get(handle,'Max')) + % then checkbox is checked + value = 1; +else + % checkbox is not checked + value = 0; +end + +%@@@ Function sets checkbox state @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +% Setting the "Value" variable to some number not equal to "Min" or "Max" +% will cause Matlab runtime error. +function setCheckbox(handle, value) + +if (value == 1) + % "check" the checkbox + set(handle, 'Value', get(handle,'Max')); +else + % "uncheck" the checkbox + set(handle, 'Value', get(handle,'Min')); +end + +%@@@ Function checks if the edit field contains a numeric value. If yes, +%then it converts string type value to double. @@@@@@@@@@@@@@@@@@@@@@@@@@@@ +function value = edit2double(handle) + +%--- Try to convert string in the entry field to double ------------------- +value = str2double(get(handle, 'String')); + +% If it is not a number, then handle the incorect input ------------------- +if isnan(value) && ~strcmpi(get(handle, 'String'), 'NaN') + %--- Make the message text --- + text = ['Bad input in the field "', get(handle, 'UserData'),... + '". You must enter a numeric value.']; + + % Show the error message in a message box + errordlg(text, 'Bad Input', 'modal'); + + %--- Stop code execution here and "Throw an error". The error will be + % "cached" by the "CATCH" statement. The code execution resumes from + % at the "CATCH" statement. Please read the Matlab help for mo details + % on TRY, CATCH and ERROR commands. + error('setSettings:badInput', text); +end + + +% --- Executes on button press in pushbuttonSelectDataFile. +function pushbuttonSelectDataFile_Callback(hObject, eventdata, handles) +% hObject handle to pushbuttonSelectDataFile (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +[fileName, pathName] = uigetfile('*.bin', ... + 'Select data file...', ... + get(handles.editFileName, 'String')); + +if (~isequal(fileName, 0) && ~isequal(pathName, 0)) + set(handles.editFileName, 'String', fullfile(pathName, fileName)); + set(handles.pushbuttonApply, 'Enable', 'on'); +end + +% --- Executes on button press in pushbuttonProbeData. +function pushbuttonProbeData_Callback(hObject, eventdata, handles) +% hObject handle to pushbuttonProbeData (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +[settings, error] = saveSettings(handles); + +%--- If no errors, then ... +if error == 0 + try + probeData(settings); + catch + errStruct = lasterror; + msgbox(errStruct.message, 'Error', 'error'); + end +end + + +% --- Executes on button press in pushbuttonDefault. +function pushbuttonDefault_Callback(hObject, eventdata, handles) +% hObject handle to pushbuttonDefault (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +handles.settings = initSettings(); + +% Assign it to the GUI data structure +loadSettings(handles); + +% Update handles structure +guidata(hObject, handles); + +% Turn on the apply button +set(handles.pushbuttonApply, 'Enable', 'on'); diff --git a/GNSS_SDR_IQ/test/InitCodePhase.mat b/GNSS_SDR_IQ/test/InitCodePhase.mat new file mode 100644 index 0000000..c0d89d5 Binary files /dev/null and b/GNSS_SDR_IQ/test/InitCodePhase.mat differ diff --git a/GNSS_SDR_IQ/test/NAV/navascii13 b/GNSS_SDR_IQ/test/NAV/navascii13 new file mode 100644 index 0000000..f01da50 --- /dev/null +++ b/GNSS_SDR_IQ/test/NAV/navascii13 @@ -0,0 +1 @@ +0010001011000010110000010011010101000111111101110100100111010001010100001011011001011000011111110000000010010010100110101011111101001110100000000010111010100001000011011110001001000000101010001101001000011011000111001011011010110001001000010010110110100100100001001101101000001011000000000000110000110010001011000010110000010011010101000111111101101101100111000010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001011111000110101101011101010010000100000111111111000000000001100100101000010010011100110000000001010010001011000010110000010011010101000111111101100101010001100001010010000001000110100110001000101110101011110001010001001110010111110110111111101000100000000011011110010000001010101101101011111001001001111001001000001000000011111010000111100000001100010111000101110101000010001010110111100111111010000010001011000010110000010011010101000111111101011101000010010011111111111010110101111111101011101110011011111101010011110100000000000101111101011100011111111001110101111011101110011111011000010111101010101100100110011111111001011001011111111011111111101001100000110001010110101101000001001011101101110010001011000010110000010011010101000111111101010100110110000001111001111101001101001011111101000101000011100101100010011011000110110110001100000111110010111011101000101100110001010101100001000111010000011000110000111001001001100000011011101111111101010010000110010010111001001110101111100110100011000010001011000010110000010011010101000111111101001100100001000001010101101000111001100111101000001111111110010010001101011100000010110011001111111100101101011110111100101011011100101000000001101011100000111110010101011000011000000111101011010011001100000100110011111010001011011010111111111111101110000010001011000010110000010011010101000111111101000101101110110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001011111000110101101011101010010000100000111111111000000000001100100101000010010011100110000000001010010001011000010110000010011010101000111111100111101011111100001010010000001000110100110001000101110101011110001010001001110010111110110111111101000100000000011011110010000001010101101101011111001001001111001001000001000000011111010000111100000001100010111000101110101000010001010110111100111111010000010001011000010110000010011010101000111111100110101000111000011111111111010110101111111101011101110011011111101010011110100000000000101111101011100011111111001110101111011101110011111011000010111101010101100100110011111111001011001011111111011111111101001100000110001010110101101000001001011101101110010001011000010110000010011010101000111111100101100111100110001111100001110101000101011010111100100010011100001110101101011011111111000100101110111111010101001101000100110000001100011101010111010010011111110100011100101110110100000011101110010100111100101101111110001001110011101110001010000101011100010001011000010110000010011010101000111111100100100100100010001010110001100110011111100001111110000000010110001101110010100000010110010001111111101110010100001000011001100101011100000101100011111111111000011101010101100111101011000101111101101111001011101000001110111101111101010111111111111110111110010001011000010110000010011010101000111111100011101100000110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001011111000110101101011101010010000100000111111111000000000001100100101000010010011100110000000001010010001011000010110000010011010101000111111100010101010110010001010010000001000110100110001000101110101011110001010001001110010111110110111111101000100000000011011110010000001010101101101011111001001001111001001000001000000011111010000111100000001100010111000101110101000010001010110111100111111010000010001011000010110000010011010101000111111100001101000001010011111111111010110101111111101011101110011011111101010011110100000000000101111101011100011111111001110101111011101110011111011000010111101010101100100110011111111001011001011111111011111111101001100000110001010110101101000001001011101101110010001011000010110000010011010101000111111100000100110101000001111101101010101110000100110000011100000101000010010001000010010110011100001001100111111010010111101000110000101010101110010110110111110110000101000101101011100100001111100010101011000110001100010011111001000001101000000101100010010111100010001011000010110000010011010101000111111011111100100000010001010111010000110100110100011000001111000010010010111011010011111101001110010000000001011010100001000011011000101111111001010111011111110000110101011101110110110000100110101010000111001011110011010000100000110000001110111111111110101101000010001011000010110000010011010101000111111011110101101111100010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001011111000110101101011101010010000100000111111111000000000001100100101000010010011100110000000001010010001011000010110000010011010101000111111011101101010110100001010010000001000110100110001000101110101011110001010001001110010111110110111111101000100000000011011110010000001010101101101011111001001001111001001000001000000011111010000111100000001100010111000101110101000010001010110111100111111010000010001011000010110000010011010101000111111011100101001110000011111111111010110101111111101011101110011011111101010011110100000000000101111101011100011111111001110101111011101110011111011000010111101010101100100110011111111001011001011111111011111111101001100000110001010110101101000001001011101101110010001011000010110000010011010101000111111011011100110001000001111110010011100010100010000110010100110110111001101100111000001011001100101101011100001001001010110010011000100101110010010110010101001000111010000011010100111001010000011110110100000100110100011001010110001010101010101010101010100011000010001011000010110000010011010101000111111011010100101001100001011000000001100010111101010000001111000010110001011010000100000010110010011111111101001010100001000011010010111100101010000001010011110111101000101111110111101110011001110101110000010101001111100000110010111011111111000000000001101000000010001011000010110000010011010101000111111011001101101001110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001011111000110101101011101010010000100000111111111000000000001100100101000010010011100110000000001010010001011000010110000010011010101000111111011000101011111010001010010000001000110100110001000101110101011110001010001001110010111110110111111101000100000000011011110010000001010101101101011111001001001111001001000001000000011111010000111100000001100010111000101110101000010001010110111100111111010000010001011000010110000010011010101000111111010111101000000000011111111111010110101111111101011101110011011111101010011110100000000000101111101011100011111111001110101111011101110011111011000010111101010101100100110011111111001011001011111111011111111101001100000110001010110101101000001001011101101110010001011000010110000010011010101000111111010110100110100010001111111101110011001100110111010101001101010011001100101000010011010100110011010100111010101010110011001100110011010011010011011101110011010100110000010101001101010010000000011011000000011111100000000000001001111111111111111111111111010110010001011000010110000010011010101000111111010101100100011010001110011000011111100001010011111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111101010101010101010101011010010001011000010110000010011010101000111111010100101101100100010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001011111000110101101011101010010000100000111111111000000000001100100101000010010011100110000000001010010001011000010110000010011010101000111111010011101010001010001010010000001000110100110001000101110101011110001010001001110010111110110111111101000100000000011011110010000001010101101101011111001001001111001001000001000000011111010000111100000001100010111000101110101000010001010110111100111111010000010001011000010110000010011010101000111111010010101001001110011111111111010110101111111101011101110011011111101010011110100000000000101111101011100011111111001110101111011101110011111011000010111101010101100100110011111111001011001011111111011111111101001100000110001010110101101000001001011101101110010001011000010110000010011010101000111111010001100110010000001111001111101001101001011111101000101000011100101100010011011000110110110001100000111110010111011101000101100110001010101100001000111010000011000110000111001001001100000011011101111111101010010000110010010111000100010001110001110110010110010001011000010110000010011010101000111111010000100101010100001000001000011010110000100110000001111000010111101001011111011111101010011000000000010010101011110111100101101001001000000000001101001000100011001100111110000011100000001001101110000000111010010101110001101101000000000000000000011010100010010001011000010110000010011010101000111111001111101101011000010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001011111000110101101011101010010000100000111111111000000000001100100101000010010011100110000000001010010001011000010110000010011010101000111111001110101011101100001010010000001000110100110001000101110101011110001010001001110010111110110111111101000100000000011011110010000001010101101101011111001001001111001001000001000000011111010000111100000001100010111000101110101000010001010110111100111111010000010001011000010110000010011010101000111111001101101001010100011111111111010110101111111101011101110011011111101010011110100000000000101111101011100011111111001110101111011101110011111011000010111101010101100100110011111111001011001011111111011111111101001100000110001010110101101000001001011101101110010001011000010110000010011010101000111111001100100111110110001011001000100100101111101100000001111000100110010001100100100000010101010011111111100000010100001000011000100100110100010101011111111101100100100100111101001011001011100011000101011000011000101001101000001111000001001000000000011100010100010001011000010110000010011010101000111111001011100101101000001000010011000110010000110011111110000000000100010110100110100000010110001011111111101000101011110111100111000110110101111111111001010100010000101001101101100001011100000100100001111010100111001100101011101111000110111000000000000011000000010001011000010110000010011010101000111111001010101100010110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001011111000110101101011101010010000100000111111111000000000001100100101000010010011100110000000001010010001011000010110000010011010101000111111001001101011011110001010010000001000110100110001000101110101011110001010001001110010111110110111111101000100000000011011110010000001010101101101011111001001001111001001000001000000011111010000111100000001100010111000101110101000010001010110111100111111010000010001011000010110000010011010101000111111001000101000011010011111111111010110101111111101011101110011011111101010011110100000000000101111101011100011111111001110101111011101110011111011000010111101010101100100110011111111001011001011111111011111111101001100000110001010110101101000001001011101101110010001011000010110000010011010101000111111000111100110000110001011010101011011111110001101000001111000110000101011001001100000010101101001111111101110010100001000011001110100000010001011010101101001010011110111111001110001110101010000011010000110000101100101110001100110100001010111111110110111000110010001011000010110000010011010101000111111000110100101000010001000011100000110000111000110111110000000001011101100011001011111101001100000000000000110010100001000011001111001100100011010001001110111101111010101000110011110110000011001111101111001110001110100001001101000111101000111111111101111010000010001011000010110000010011010101000111111000101101101000000010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001011111000110101101011101010010000100000111111111000000000001100100101000010010011100110000000001010010001011000010110000010011010101000111111000100101011110100001010010000001000110100110001000101110101011110001010001001110010111110110111111101000100000000011011110010000001010101101101011111001001001111001001000001000000011111010000111100000001100010111000101110101000010001010110111100111111010000010001011000010110000010011010101000111111000011101001101010011111111111010110101111111101011101110011011111101010011110100000000000101111101011100011111111001110101111011101110011111011000010111101010101100100110011111111001011001011111111011111111101001100000110001010110101101000001001011101101110010001011000010110000010011010101000111111000010100111001000001000000101010101010101010110010101010101010101010101011110010101010101010101010101011110010101010101010101010101011110010101010101010101010101011110010101010101010101010101011110010101010101010101010101011110010101010101010101010101011110010001011000010110000010011010101000111111000001100101110000001000100010101001100011001110000001111111111010011000100100100000010110001101111111100000010100001000011011000111000111111111110011111011010111010011111011000110010001011110101010101010000110001110001101010100111101100111111111000011100110010001011000010110000010011010101000111111000000101100001110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001011111000110101101011101010010000100000111111111000000000001100100101000010010011100110000000001010010001011000010110000010011010101000111110111111101010011110001010010000001000110100110001000101110101011110001010001001110010111110110111111101000100000000011011110010000001010101101101011111001001001111001001000001000000011111010000111100000001100010111000101110101000010001010110111100111111010000010001011000010110000010011010101000111110111110101001011010011111111111010110101111111101011101110011011111101010011110100000000000101111101011100011111111001110101111011101110011111011000010111101010101100100110011111111001011001011111111011111111101001100000110001010110101101000001001011101101110010001011000010110000010011010101000111110111101100110000100001011100100110001100111001111000001111000110011101011110100011111101011000000000000011111010100001000011001100101001000101010001011110101001110111110010110111100010100111001111100101111101110010111101110011111000011111000000000010010010100010001011000010110000010011010101000111110111100100101000000001000101000110010010111111011111110000111110111011001110000100000010101101011111111111101101011110111100111101101111001000101100001101010011100100001111110011001001000001010011101001111111111010000000101111110100110010111111111111001100000010001011000010110000010011010101000111110111011101101100100010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001011111000110101101011101010010000100000111111111000000000001100100101000010010011100110000000001010010001011000010110000010011010101000111110111010101011010000001010010000001000110100110001000101110101011110001010001001110010111110110111111101000100000000011011110010000001010101101101011111001001001111001001000001000000011111010000111100000001100010111000101110101000010001010110111100111111010000010001011000010110000010011010101000111110111001101001101000011111111111010110101111111101011101110011011111101010011110100000000000101111101011100011111111001110101111011101110011111011000010111101010101100100110011111111001011001011111111011111111101001100000110001010110101101000001001011101101110010001011000010110000010011010101000111110111000100111001010001111001111101001101001011111101000101000011100101100010011011000110110110001100000111110010111011101000101100110001010101100001000111010000011000110000111001001001100000011011101111111101010010000110010010111011101101010000000101000010010010001011000010110000010011010101000111110110111100100110000001000110001111111111000010001111110000000000010111011111000100000010110010011111111101001010100001000011001111111111110011010100011100100100100010010011110000101010000110000001100110000001100111101011001111010000101011111111111000011011010010001011000010110000010011010101000111110110110101101001110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001011111000110101101011101010010000100000111111111000000000001100100101000010010011100110000000001010010001011000010110000010011010101000111110110101101010000110001010010000001000110100110001000101110101011110001010001001110010111110110111111101000100000000011011110010000001010101101101011111001001001111001001000001000000011111010000111100000001100010111000101110101000010001010110111100111111010000010001011000010110000010011010101000111110110100101001000010011111111111010110101111111101011101110011011111101010011110100000000000101111101011100011111111001110101111011101110011111011000010111101010101100100110011111111001011001011111111011111111101001100000110001010110101101000001001011101101110010001011000010110000 \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/NAV/navascii16 b/GNSS_SDR_IQ/test/NAV/navascii16 new file mode 100644 index 0000000..64c2872 --- /dev/null +++ b/GNSS_SDR_IQ/test/NAV/navascii16 @@ -0,0 +1 @@ +1101110100111101001111101100101010111000000010001011011000101110101011110100100110100111100000001111111101101101011001010100000010110001011111111101000101011110111100100001110110111111010101110010110111100100111000110100100101001110110111101101001001011011011110110010010111110100111111111111001111001101110100111101001111101100101010111000000010010010011000111101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011110101111100001010011100010101101111001101000000000111111111111111101110011011110111001100011110100111101110100111101001111101100101010111000000010011010101110011110101100000000011101000000001111010110101000111011011000100100101010000101001000100111111011111110101110100000001100100010010100111110110000010101100000010101111100111010000111011000001100110010011100100010010010001010110111100111111111001101110100111101001111101100101010111000000010100010111101101111111111111111110100110001101000110011000000111101000110001111111111110001101101011100101000010100001100110010011010110111101000001011011111111011101010001111000110010110110110001011111111101010111011101111011001010011000011100110110110111101110100111101001111101100101010111000000010101011001001111110000110000010110010110100000010111010111100011010011101100100111001001001110011111000001101000100010111010011001110101010011110111000101111100111001111000110110110011111100100010000000010101101111001101101000110110001010000011001011100111101110100111101001111101100101010111000000010110011011110111110101010010111000110011000010111110000000001101101110010100011111101001100110000000011010010100001000011010100100011010111111110010100011111000001101010100111100111111000010100101100110011111011001100000101110100100101000000000000010001111101110100111101001111101100101010111000000010111010010001001101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011110101111100001010011100010101101111001101000000000111111111111111101110011011110111001100011110100111101110100111101001111101100101010111000000011000010100000011110101100000000011101000000001111010110101000111011011000100100101010000101001000100111111011111110101110100000001100100010010100111110110000010101100000010101111100111010000111011000001100110010011100100010010010001010110111100111111111001101110100111101001111101100101010111000000011001010111000111111111111111111110100110001101000110011000000111101000110001111111111110001101101011100101000010100001100110010011010110111101000001011011111111011101010001111000110010110110110001011111111101010111011101111011001010011000011100110110110111101110100111101001111101100101010111000000011010011000011001110000011110001010111010100101000011011101100011110001010010100100000000111011010001000000101010110010111011001111110011100010101000101101100000001011100011010001001011111100010001101011000011010010000001110110001100010001110101111010100011101110100111101001111101100101010111000000011011011011011101110101001110011001100000011110000001111111101001110010001101011111101001101110000000010001101011110111100110011010100011111010011100000000000111100010101010011000010100111010000010010000110100010111110001000010000010101000000000000001000001101110100111101001111101100101010111000000011100010011111001101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011110101111100001010011100010101101111001101000000000111111111111111101110011011110111001100011110100111101110100111101001111101100101010111000000011101010101001101110101100000000011101000000001111010110101000111011011000100100101010000101001000100111111011111110101110100000001100100010010100111110110000010101100000010101111100111010000111011000001100110010011100100010010010001010110111100111111111001101110100111101001111101100101010111000000011110010111110101111111111111111110100110001101000110011000000111101000110001111111111110001101101011100101000010100001100110010011010110111101000001011011111111011101010001111000110010110110110001011111111101010111011101111011001010011000011100110110110111101110100111101001111101100101010111000000011111011001010111110000010010101010001111011001111100011111010111101101110111101101001100011110110011000000101101000010111001111010101010001101001001000001001111010111010010100011011110000011101010100111001110011101100000110111110010111111010011101101000011101110100111101001111101100101010111000000100000011011111101110101000101111001011001011100111110000111101101101000100101100000010110001101111111110100101011110111100100111010000000110101000100000001111001010100010001001001111011001010101111000110100001100101111011111001111110001000000000001010010111101110100111101001111101100101010111000000100001010010000011101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011110101111100001010011100010101101111001101000000000111111111111111101110011011110111001100011110100111101110100111101001111101100101010111000000100010010101001011110101100000000011101000000001111010110101000111011011000100100101010000101001000100111111011111110101110100000001100100010010100111110110000010101100000010101111100111010000111011000001100110010011100100010010010001010110111100111111111001101110100111101001111101100101010111000000100011010110001111111111111111111110100110001101000110011000000111101000110001111111111110001101101011100101000010100001100110010011010110111101000001011011111111011101010001111000110010110110110001011111111101010111011101111011001010011000011100110110110111101110100111101001111101100101010111000000100100011001110111110000001101100011101011101111001101011001001000110010011000111110100110011010010100011110110110101001101100111011010001101101001101010110111000101111100101011000110101111100001001011111011001011100110101001110101010101010101010101011100111101110100111101001111101100101010111000000100101011010110011110100111111110011101000010101111110000111101001110100101111011111101001101100000000010110101011110111100101101000011010101111110101100001000010111010000001000010001100110001010001111101010110000011111001101000100000000111111111110010111111101110100111101001111101100101010111000000100110010010110001101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011110101111100001010011100010101101111001101000000000111111111111111101110011011110111001100011110100111101110100111101001111101100101010111000000100111010100000101110101100000000011101000000001111010110101000111011011000100100101010000101001000100111111011111110101110100000001100100010010100111110110000010101100000010101111100111010000111011000001100110010011100100010010010001010110111100111111111001101110100111101001111101100101010111000000101000010111111111111111111111111110100110001101000110011000000111101000110001111111111110001101101011100101000010100001100110010011010110111101000001011011111111011101010001111000110010110110110001011111111101010111011101111011001010011000011100110110110111101110100111101001111101100101010111000000101001011001011101110000000010001100110011001000101010110010101100110011010111101100101011001100101011000101010101001100110011001100101100101100100010001100101011001111101010110010101101111111100100111111100000011111111111110110000000000000000000000000101001101110100111101001111101100101010111000000101010011011100101110001100111100000011110101100000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000010101010101010101010100101101110100111101001111101100101010111000000101011010010011011101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011110101111100001010011100010101101111001101000000000111111111111111101110011011110111001100011110100111101110100111101001111101100101010111000000101100010101110101110101100000000011101000000001111010110101000111011011000100100101010000101001000100111111011111110101110100000001100100010010100111110110000010101100000010101111100111010000111011000001100110010011100100010010010001010110111100111111111001101110100111101001111101100101010111000000101101010110110001111111111111111110100110001101000110011000000111101000110001111111111110001101101011100101000010100001100110010011010110111101000001011011111111011101010001111000110010110110110001011111111101010111011101111011001010011000011100110110110111101110100111101001111101100101010111000000101110011001101111110000110000010110010110100000010111010111100011010011101100100111001001001110011111000001101000100010111010011001110101010011110111000101111100111001111000110110110011111100100010000000010101101111001101101000111011101110001110001001101001101110100111101001111101100101010111000000101111011010101011110111110111100101001111011001111110000111101000010110100000100000010101100111111111101101010100001000011010010110110111111111110010110111011100110011000001111100011111110110010001111111000101101010001110010010111111111111111111100101011101101110100111101001111101100101010111000000110000010010100111101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011110101111100001010011100010101101111001101000000000111111111111111101110011011110111001100011110100111101110100111101001111101100101010111000000110001010100010011110101100000000011101000000001111010110101000111011011000100100101010000101001000100111111011111110101110100000001100100010010100111110110000010101100000010101111100111010000111011000001100110010011100100010010010001010110111100111111111001101110100111101001111101100101010111000000110010010110101011111111111111111110100110001101000110011000000111101000110001111111111110001101101011100101000010100001100110010011010110111101000001011011111111011101010001111000110010110110110001011111111101010111011101111011001010011000011100110110110111101110100111101001111101100101010111000000110011011000001001110100110111011011010000010011111110000111011001101110011011011111101010101100000000011111101011110111100111011011001011101010100000000010011011011011000010110100110100011100111010100111100111010110010111110000111110110111111111100011101011101110100111101001111101100101010111000000110100011010010111110111101100111001101111001100000001111111111011101001011001011111101001110100000000010111010100001000011000111001001010000000000110101011101111010110010010011110100011111011011110000101011000110011010100010000111001000111111111111100111111101110100111101001111101100101010111000000110101010011101001101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011110101111100001010011100010101101111001101000000000111111111111111101110011011110111001100011110100111101110100111101001111101100101010111000000110110010100100001110101100000000011101000000001111010110101000111011011000100100101010000101001000100111111011111110101110100000001100100010010100111110110000010101100000010101111100111010000111011000001100110010011100100010010010001010110111100111111111001101110100111101001111101100101010111000000110111010111100101111111111111111110100110001101000110011000000111101000110001111111111110001101101011100101000010100001100110010011010110111101000001011011111111011101010001111000110010110110110001011111111101010111011101111011001010011000011100110110110111101110100111101001111101100101010111000000111000011001111001110100101010100100000001110010111110000111001111010100110110011111101010010110000000010001101011110111100110001011111101110100101010010110101100001000000110001110001010101111100101111001111010011010001110011001011110101000000001001000111001101110100111101001111101100101010111000000111001011010111101110111100011111001111000111001000001111111110100010011100110100000010110011111111111111001101011110111100110000110011011100101110110001000010000101010111001100001001111100110000010000110001110001011110110010111000010111000000000010000101111101110100111101001111101100101010111000000111010010010111111101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011110101111100001010011100010101101111001101000000000111111111111111101110011011110111001100011110100111101110100111101001111101100101010111000000111011010100001011110101100000000011101000000001111010110101000111011011000100100101010000101001000100111111011111110101110100000001100100010010100111110110000010101100000010101111100111010000111011000001100110010011100100010010010001010110111100111111111001101110100111101001111101100101010111000000111100010110010101111111111111111110100110001101000110011000000111101000110001111111111110001101101011100101000010100001100110010011010110111101000001011011111111011101010001111000110010110110110001011111111101010111011101111011001010011000011100110110110111101110100111101001111101100101010111000000111101011000110111110111111010101010101010101001101010101010101010101010100001101010101010101010101010100001101010101010101010101010100001101010101010101010101010100001101010101010101010101010100001101010101010101010101010100001101010101010101010101010100001101110100111101001111101100101010111000000111110011010001111110111011101010110011100110001111110000000000101100111011011011111101001110010000000011111101011110111100100111000111000000000001100000100101000101100000100111001101110100001010101010101111001110001110010101011000010011000000000111100011001101110100111101001111101100101010111000000111111010011110001101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011110101111100001010011100010101101111001101000000000111111111111111101110011011110111001100011110100111101110100111101001111101100101010111000001000000010101100001110101100000000011101000000001111010110101000111011011000100100101010000101001000100111111011111110101110100000001100100010010100111110110000010101100000010101111100111010000111011000001100110010011100100010010010001010110111100111111111001101110100111101001111101100101010111000001000001010110100101111111111111111110100110001101000110011000000111101000110001111111111110001101101011100101000010100001100110010011010110111101000001011011111111011101010001111000110010110110110001011111111101010111011101111011001010011000011100110110110111101110100111101001111101100101010111000001000010011001111011110100011011001110011000110000111110000111001100010100001011100000010100111111111111100000101011110111100110011010110111010101110100001010110001000001101001000011101011000110000011010000010001101000010001100000111100000111111111101101101011101110100111101001111101100101010111000001000011011010111111110111010111001101101000000100000001111000001000100110001111011111101010010100000000000010010100001000011000010010000110111010011110010101100011011110000001100110110111110101100010110000000000101111111010000001011001101000000000000110011111101110100111101001111101100101010111000001000100010010011011101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011110101111100001010011100010101101111001101000000000111111111111111101110011011110111001100011110100111101110100111101001111101100101010111000001000101010100101111110101100000000011101000000001111010110101000111011011000100100101010000101001000100111111011111110101110100000001100100010010100111110110000010101100000010101111100111010000111011000001100110010011100100010010010001010110111100111111111001101110100111101001111101100101010111000001000110010110010111111111111111111110100110001101000110011000000111101000110001111111111110001101101011100101000010100001100110010011010110111101000001011011111111011101010001111000110010110110110001011111111101010111011101111011001010011000011100110110110111101110100111101001111101100101010111000001000111011000110101110000110000010110010110100000010111010111100011010011101100100111001001001110011111000001101000100010111010011001110101010011110111000101111100111001111000110110110011111100100010000000010101101111001101101000100010010101111111010111101101101110100111101001111101100101010111000001001000011011001111110111001110000000000111101110000001111111111101000100000111011111101001101100000000010110101011110111100110000000000001100101011100011011011011101101100001111010101111001111110011001111110011000010100110000101111010100000000000111100100101101110100111101001111101100101010111000001001001010010110001101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011110101111100001010011100010101101111001101000000000111111111111111101110011011110111001100011110100111101110100111101001111101100101010111000001001010010101111001110101100000000011101000000001111010110101000111011011000100100101010000101001000100111111011111110101110100000001100100010010100111110110000010101100000010101111100111010000111011000001100110010011100100010010010001010110111100111111111001101110100111101001111101100101010111000001001011010110111101111111111111111110100110001101000110011000000111101000110001111111111110001101101011100101000010100001100110010011010110111101000001011011111111011101010001111000110010110110110001011111111101010111011101111011001010011000011100110110110111101110100111101001111 \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/NAV/navascii19 b/GNSS_SDR_IQ/test/NAV/navascii19 new file mode 100644 index 0000000..0fbf102 --- /dev/null +++ b/GNSS_SDR_IQ/test/NAV/navascii19 @@ -0,0 +1 @@ +0010001011000010110000010011010101000111111101110100100111010001010100001011011001010111000111011011000010010010101011011011111101001101010000000011110010100001000011011101100110110000101010001011010000001100010000110100100110001001101010101101001010010011001100011111010000001011000000000000111000000010001011000010110000010011010101000111111101101101100111000010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001111110011111100011011101010010000101111000000000111111111110010001000011010001101110110010010110110010001011000010110000010011010101000111111101100101010001100000011100111111100110011011000000110101110101010000001100011101011000101001001001010011001011111110110000010000010011001101101010100111000011010000010000001011100101001010000101010111110010101011010001111100101010001010110111100111110010110010001011000010110000010011010101000111111101011101000010010000000000001111001101111101100101011111111010010101100100111111111111111011101101100010110111001110001011010000010011111000100010001010110000101111010110011111010110101000011001110100000000010110111110001000101000011100000010010001100010010010001011000010110000010011010101000111111101010100110110000001111001111101001101001011111101000101000011100101100010011011000110110110001100000111110010111011101000101100110001010101100001000111010000011000110000111001001001100000011011101111111101010010000110010010111001001110101111100110100011000010001011000010110000010011010101000111111101001100100001000001010101101000111010100001110000100100111110010010000101100011111101001100110000000011010010100001000011010100010111101111111110010110010111111001000010100111101000101010100110001011001011001110100111100011010100100101000000000000010100110010001011000010110000010011010101000111111101000101101110110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001111110011111100011011101010010000101111000000000111111111110010001000011010001101110110010010110110010001011000010110000010011010101000111111100111101011111100000011100111111100110011011000000110101110101010000001100011101011000101001001001010011001011111110110000010000010011001101101010100111000011010000010000001011100101001010000101010111110010101011010001111100101010001010110111100111110010110010001011000010110000010011010101000111111100110101000111000000000000001111001101111101100101011111111010010101100100111111111111111011101101100010110111001110001011010000010011111000100010001010110000101111010110011111010110101000011001110100000000010110111110001000101000011100000010010001100010010010001011000010110000010011010101000111111100101100111100110001111100001110101000101011010111100100010011100001110101101011011111111000100101110111111010101001101000100110000001100011101010111010010011111110100011100101110110100000011101110010100111100101101111110001001110011101110001010000101011100010001011000010110000010011010101000111111100100100100100010001010110001100110100011000000111011011000010110001110110000100000010110011001111111110001010100001000011001100001000011111010011100001111001110001011010101100111100011110010000010101111010010000010011100110111000010101000000000000000101110010001011000010110000010011010101000111111100011101100000110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001111110011111100011011101010010000101111000000000111111111110010001000011010001101110110010010110110010001011000010110000010011010101000111111100010101010110010000011100111111100110011011000000110101110101010000001100011101011000101001001001010011001011111110110000010000010011001101101010100111000011010000010000001011100101001010000101010111110010101011010001111100101010001010110111100111110010110010001011000010110000010011010101000111111100001101000001010000000000001111001101111101100101011111111010010101100100111111111111111011101101100010110111001110001011010000010011111000100010001010110000101111010110011111010110101000011001110100000000010110111110001000101000011100000010010001100010010010001011000010110000010011010101000111111100000100110101000001111101101010101110000100110000011100000101000010010001000010010110011100001001100111111010010111101000110000101010101110010110110111110110000101000101101011100100001111100010101011000110001100010011111001000001101000000101100010010111100010001011000010110000010011010101000111111011111100100000010001010111010000110101011000101000100100000010010010111011011100000010101110111111111100111010100001000011011000111111010110101000100010000110000011101010001001001111111100000010111000110011010110001010111110101111110001000000000001101100010010001011000010110000010011010101000111111011110101101111100010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001111110011111100011011101010010000101111000000000111111111110010001000011010001101110110010010110110010001011000010110000010011010101000111111011101101010110100000011100111111100110011011000000110101110101010000001100011101011000101001001001010011001011111110110000010000010011001101101010100111000011010000010000001011100101001010000101010111110010101011010001111100101010001010110111100111110010110010001011000010110000010011010101000111111011100101001110000000000000001111001101111101100101011111111010010101100100111111111111111011101101100010110111001110001011010000010011111000100010001010110000101111010110011111010110101000011001110100000000010110111110001000101000011100000010010001100010010010001011000010110000010011010101000111111011011100110001000001111110010011100010100010000110010100110110111001101100111000001011001100101101011100001001001010110010011000100101110010010110010101001000111010000011010100111001010000011110110100000100110100011001010110001010101010101010101010100011000010001011000010110000010011010101000111111011010100101001100001011000000001100011100101011000100100000010110001001101001100000010110010111111111110010010100001000011010010111011000010000001010010000000011100010000001000011011100000000010000111101011101111100100111001000011111111000000000001100000110010001011000010110000010011010101000111111011001101101001110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001111110011111100011011101010010000101111000000000111111111110010001000011010001101110110010010110110010001011000010110000010011010101000111111011000101011111010000011100111111100110011011000000110101110101010000001100011101011000101001001001010011001011111110110000010000010011001101101010100111000011010000010000001011100101001010000101010111110010101011010001111100101010001010110111100111110010110010001011000010110000010011010101000111111010111101000000000000000000001111001101111101100101011111111010010101100100111111111111111011101101100010110111001110001011010000010011111000100010001010110000101111010110011111010110101000011001110100000000010110111110001000101000011100000010010001100010010010001011000010110000010011010101000111111010110100110100010001111111101110011001100110111010101001101010011001100101000010011010100110011010100111010101010110011001100110011010011010011011101110011010100110000010101001101010010000000011011000000011111100000000000001001111111111111111111111111010110010001011000010110000010011010101000111111010101100100011010001110011001001001100001011101000000000000000000000000010100111111111111111111111111101011000000000000000000000000010100111111111111111111111111101011000000000000000000000000010100111111111111111111111111101011000010101010101010101011001010010001011000010110000010011010101000111111010100101101100100010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001111110011111100011011101010010000101111000000000111111111110010001000011010001101110110010010110110010001011000010110000010011010101000111111010011101010001010000011100111111100110011011000000110101110101010000001100011101011000101001001001010011001011111110110000010000010011001101101010100111000011010000010000001011100101001010000101010111110010101011010001111100101010001010110111100111110010110010001011000010110000010011010101000111111010010101001001110000000000001111001101111101100101011111111010010101100100111111111111111011101101100010110111001110001011010000010011111000100010001010110000101111010110011111010110101000011001110100000000010110111110001000101000011100000010010001100010010010001011000010110000010011010101000111111010001100110010000001111001111101001101001011111101000101000011100101100010011011000110110110001100000111110010111011101000101100110001010101100001000111010000011000110000111001001001100000011011101111111101010010000110010010111000100010001110001110110010110010001011000010110000010011010101000111111010000100101010100001000001000011010110000100110000100100000010111101000100101100000010101101001111111101110010100001000011010010101110100000000001100111010000100111101000001111100000000111000111010111111001011110100101011011011000000000000000000011010100010010001011000010110000010011010101000111111001111101101011000010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001111110011111100011011101010010000101111000000000111111111110010001000011010001101110110010010110110010001011000010110000010011010101000111111001110101011101100000011100111111100110011011000000110101110101010000001100011101011000101001001001010011001011111110110000010000010011001101101010100111000011010000010000001011100101001010000101010111110010101011010001111100101010001010110111100111110010110010001011000010110000010011010101000111111001101101001010100000000000001111001101111101100101011111111010010101100100111111111111111011101101100010110111001110001011010000010011111000100010001010110000101111010110011111010110101000011001110100000000010110111110001000101000011100000010010001100010010010001011000010110000010011010101000111111001100100111110110001011001000100100110101110011000100100000100110010010001111011111101010011100000000011101010100001000011000100010101110010101011111101111001110110111111101001010110011100110001101011000010001111001100011001110111110110111111111100001101110010001011000010110000010011010101000111111001011100101101000001000010011000110010110111101000100100111111011101000011110100000010110001101111111100000010100001000011000111000100101111111111001100011001110100100101101100001001111110000100001000101010010001011100100110010111001000111111111111100010110010001011000010110000010011010101000111111001010101100010110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001111110011111100011011101010010000101111000000000111111111110010001000011010001101110110010010110110010001011000010110000010011010101000111111001001101011011110000011100111111100110011011000000110101110101010000001100011101011000101001001001010011001011111110110000010000010011001101101010100111000011010000010000001011100101001010000101010111110010101011010001111100101010001010110111100111110010110010001011000010110000010011010101000111111001000101000011010000000000001111001101111101100101011111111010010101100100111111111111111011101101100010110111001110001011010000010011111000100010001010110000101111010110011111010110101000011001110100000000010110111110001000101000011100000010010001100010010010001011000010110000010011010101000111111000111100110000110001011010101011011111110001101000100100000110000101011001000011111101010101100000000001011010100001000011001110111000001110100101010100101001000001100111001110001110001100110011010000101111110111101111011011101100001010111111110110100101100010001011000010110000010011010101000111111000110100101000010001000011100000110000111101111000100100111110100010101000000011111101001101110000000000101010100001000011001111110000101011010001001101000100011000110111001100001001100101110000000000110000111100011111100110000000010111000000000010011101100010001011000010110000010011010101000111111000101101101000000010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001111110011111100011011101010010000101111000000000111111111110010001000011010001101110110010010110110010001011000010110000010011010101000111111000100101011110100000011100111111100110011011000000110101110101010000001100011101011000101001001001010011001011111110110000010000010011001101101010100111000011010000010000001011100101001010000101010111110010101011010001111100101010001010110111100111110010110010001011000010110000010011010101000111111000011101001101010000000000001111001101111101100101011111111010010101100100111111111111111011101101100010110111001110001011010000010011111000100010001010110000101111010110011111010110101000011001110100000000010110111110001000101000011100000010010001100010010010001011000010110000010011010101000111111000010100111001000001000000101010101010101010110010101010101010101010101011110010101010101010101010101011110010101010101010101010101011110010101010101010101010101011110010101010101010101010101011110010101010101010101010101011110010101010101010101010101011110010001011000010110000010011010101000111111000001100101110000001000100010101001100000110000111011011000000101100111001110011111101001110000000000001100010100001000011011000110111100111111110100001010010110001110111011000110001000101111000010010101110010111000100010110000111101100111111111000001001010010001011000010110000010011010101000111111000000101100001110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001111110011111100011011101010010000101111000000000111111111110010001000011010001101110110010010110110010001011000010110000010011010101000111110111111101010011110000011100111111100110011011000000110101110101010000001100011101011000101001001001010011001011111110110000010000010011001101101010100111000011010000010000001011100101001010000101010111110010101011010001111100101010001010110111100111110010110010001011000010110000010011010101000111110111110101001011010000000000001111001101111101100101011111111010010101100100111111111111111011101101100010110111001110001011010000010011111000100010001010110000101111010110011111010110101000011001110100000000010110111110001000101000011100000010010001100010010010001011000010110000010011010101000111110111101100110000100001011100100110001100110011101111011011111001100010100011110100000010101001011111111100001101011110111100110011011110001010101110011111100101000001001010110111100011000011000110011010000001011000011000010011111111100000111111111101011011000010001011000010110000010011010101000111110111100100101000000001000101000110010010110101001000100100000001000100100110110011111101010001110000000010000010100001000011000001111000101111010011110100100001010101111000001100110111000011011110011001111111000111010001011000001011001101000000000000110110110010001011000010110000010011010101000111110111011101101100100010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001111110011111100011011101010010000101111000000000111111111110010001000011010001101110110010010110110010001011000010110000010011010101000111110111010101011010000000011100111111100110011011000000110101110101010000001100011101011000101001001001010011001011111110110000010000010011001101101010100111000011010000010000001011100101001010000101010111110010101011010001111100101010001010110111100111110010110010001011000010110000010011010101000111110111001101001101000000000000001111001101111101100101011111111010010101100100111111111111111011101101100010110111001110001011010000010011111011011101001010110000101111010110011111010110101000011001110100000000010110111110001000101000011100000010010001100010010010001011000010110000010011010101000111110111000100111001010001111001111101001101001011111101000101000011100101100010011011000110110110001100000111110010111011101000101100110001010101100001000111010000011000110000111001001001100000011011101111111101010010000110010010111011101101010000000101000010010010001011000010110000010011010101000111110110111100100110000001000110001111111111100110011000100100111111101000101101001011111101001111100000000001000010100001000011010000011110110011010100011010101100010001101011110000101010111110011111101001111101100010101101001001011111010100000000000111110100000010001011000010110000010011010101000111110110110101101001110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100001111110011111100011011101010010000101111000000000111111111110010001000011010001101110110010010110110010001011000010110000010011010101000111110110101101010000110000011100111111100110011011000000110101110101010000001100011101011000101001001001010011001011111110110000010000010011001101101010100111000011010000010000001011100101001010000101010111110010101011010001111100101010001010110111100111110010110010001011000010110000010011010101000111110110100101001000010000000000001111001101111101100101011111111010010101100100111111111111111011101101100010110111001110001011010000010011111000100010001010110000101111010110011111010110101000011001110100000000010110111110001000101000011100000010010001100010010010001011000010110000 \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/NAV/navascii23 b/GNSS_SDR_IQ/test/NAV/navascii23 new file mode 100644 index 0000000..6d67344 --- /dev/null +++ b/GNSS_SDR_IQ/test/NAV/navascii23 @@ -0,0 +1 @@ +0010001011000010110000010011010101000111111101110100100111010001010100001011011001010111000111011011000010010010101011011011111101001101010000000011110010100001000011011101100110110000101010001011010000001100010000110100100110001001101010101101001010010011001100011111010000001011000000000000111000000010001011000010110000010011010101000111111101101101100111000010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100010101000101000010100100010101101111010011111111111000000000001111100110111110001101110100011111000000010001011000010110000010011010101000111111101100101010001100000010100000001011111011100011111001010010011110000111001111000101010100101001011110111101111111011000100001111101100010000110011101101110100001000011000000111101011011010000101111000001101111100100001001101010010001010110111100111111010000010001011000010110000010011010101000111111101011101000010010000000000001110000101110011011110010000100001111100110111100000000000001010110010011010101011111001010010100101111001010111011001111100110111011010000000111010111000000011110111110011111111101000100110011100100111101011000000111010001101000010001011000010110000010011010101000111111101010100110110000001111001111101001101001011111101000101000011100101100010011011000110110110001100000111110010111011101000101100110001010101100001000111010000011000110000111001001001100000011011101111111101010010000110010010111001001110101111100110100011000010001011000010110000010011010101000111111101001100100001000001010101101000111010100001110000100100111110010010000101100011111101001100110000000011010010100001000011010100010111101111111110010110010111111001000010100111101000101010100110001011001011001110100111100011010100100101000000000000010100110010001011000010110000010011010101000111111101000101101110110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100010101000101000010100100010101101111010011111111111000000000001111100110111110001101110100011111000000010001011000010110000010011010101000111111100111101011111100000010100000001011111011100011111001010010011110000111001111000101010100101001011110111101111111011000100001111101100010000110011101101110100001000011000000111101011011010000101111000001101111100100001001101010010001010110111100111111010000010001011000010110000010011010101000111111100110101000111000000000000001110000101110011011110010000100001111100110111100000000000001010110010011010101011111001010010100101111001010111011001111100110111011010000000111010111000000011110111110011111111101000100110011100100111101011000000111010001101000010001011000010110000010011010101000111111100101100111100110001111100001110101000101011010111100100010011100001110101101011011111111000100101110111111010101001101000100110000001100011101010111010010011111110100011100101110110100000011101110010100111100101101111110001001110011101110001010000101011100010001011000010110000010011010101000111111100100100100100010001010110001100110100011000000111011011000010110001110110000100000010110011001111111110001010100001000011001100001000011111010011100001111001110001011010101100111100011110010000010101111010010000010011100110111000010101000000000000000101110010001011000010110000010011010101000111111100011101100000110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100010101000101000010100100010101101111010011111111111000000000001111100110111110001101110100011111000000010001011000010110000010011010101000111111100010101010110010000010100000001011111011100011111001010010011110000111001111000101010100101001011110111101111111011000100001111101100010000110011101101110100001000011000000111101011011010000101111000001101111100100001001101010010001010110111100111111010000010001011000010110000010011010101000111111100001101000001010000000000001110000101110011011110010000100001111100110111100000000000001010110010011010101011111001010010100101111001010111011001111100110111011010000000111010111000000011110111110011111111101000100110011100100111101011000000111010001101000010001011000010110000010011010101000111111100000100110101000001111101101010101110000100110000011100000101000010010001000010010110011100001001100111111010010111101000110000101010101110010110110111110110000101000101101011100100001111100010101011000110001100010011111001000001101000000101100010010111100010001011000010110000010011010101000111111011111100100000010001010111010000110101011000101000100100000010010010111011011100000010101110111111111100111010100001000011011000111111010110101000100010000110000011101010001001001111111100000010111000110011010110001010111110101111110001000000000001101100010010001011000010110000010011010101000111111011110101101111100010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100010101000101000010100100010101101111010011111111111000000000001111100110111110001101110100011111000000010001011000010110000010011010101000111111011101101010110100000010100000001011111011100011111001010010011110000111001111000101010100101001011110111101111111011000100001111101100010000110011101101110100001000011000000111101011011010000101111000001101111100100001001101010010001010110111100111111010000010001011000010110000010011010101000111111011100101001110000000000000001110000101110011011110010000100001111100110111100000000000001010110010011010101011111001010010100101111001010111011001111100110111011010000000111010111000000011110111110011111111101000100110011100100111101011000000111010001101000010001011000010110000010011010101000111111011011100110001000001111110010011100010100010000110010100110110111001101100111000001011001100101101011100001001001010110010011000100101110010010110010101001000111010000011010100111001010000011110110100000100110100011001010110001010101010101010101010100011000010001011000010110000010011010101000111111011010100101001100001011000000001100011100101011000100100000010110001001101001100000010110010111111111110010010100001000011010010111011000010000001010010000000011100010000001000011011100000000010000111101011101111100100111001000011111111000000000001100000110010001011000010110000010011010101000111111011001101101001110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100010101000101000010100100010101101111010011111111111000000000001111100110111110001101110100011111000000010001011000010110000010011010101000111111011000101011111010000010100000001011111011100011111001010010011110000111001111000101010100101001011110111101111111011000100001111101100010000110011101101110100001000011000000111101011011010000101111000001101111100100001001101010010001010110111100111111010000010001011000010110000010011010101000111111010111101000000000000000000001110000101110011011110010000100001111100110111100000000000001010110010011010101011111001010010100101111001010111011001111100110111011010000000111010111000000011110111110011111111101000100110011100100111101011000000111010001101000010001011000010110000010011010101000111111010110100110100010001111111101110011001100110111010101001101010011001100101000010011010100110011010100111010101010110011001100110011010011010011011101110011010100110000010101001101010010000000011011000000011111100000000000001001111111111111111111111111010110010001011000010110000010011010101000111111010101100100011010001110011001001001100001011101000000000000000000000000010100111111111111111111111111101011000000000000000000000000010100111111111111111111111111101011000000000000000000000000010100111111111111111111111111101011000010101010101010101011001010010001011000010110000010011010101000111111010100101101100100010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100010101000101000010100100010101101111010011111111111000000000001111100110111110001101110100011111000000010001011000010110000010011010101000111111010011101010001010000010100000001011111011100011111001010010011110000111001111000101010100101001011110111101111111011000100001111101100010000110011101101110100001000011000000111101011011010000101111000001101111100100001001101010010001010110111100111111010000010001011000010110000010011010101000111111010010101001001110000000000001110000101110011011110010000100001111100110111100000000000001010110010011010101011111001010010100101111001010111011001111100110111011010000000111010111000000011110111110011111111101000100110011100100111101011000000111010001101000010001011000010110000010011010101000111111010001100110010000001111001111101001101001011111101000101000011100101100010011011000110110110001100000111110010111011101000101100110001010101100001000111010000011000110000111001001001100000011011101111111101010010000110010010111000100010001110001110110010110010001011000010110000010011010101000111111010000100101010100001000001000011010110000100110000100100000010111101000100101100000010101101001111111101110010100001000011010010101110100000000001100111010000100111101000001111100000000111000111010111111001011110100101011011011000000000000000000011010100010010001011000010110000010011010101000111111001111101101011000010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100010101000101000010100100010101101111010011111111111000000000001111100110111110001101110100011111000000010001011000010110000010011010101000111111001110101011101100000010100000001011111011100011111001010010011110000111001111000101010100101001011110111101111111011000100001111101100010000110011101101110100001000011000000111101011011010000101111000001101111100100001001101010010001010110111100111111010000010001011000010110000010011010101000111111001101101001010100000000000001110000101110011011110010000100001111100110111100000000000001010110010011010101011111001010010100101111001010111011001111100110111011010000000111010111000000011110111110011111111101000100110011100100111101011000000111010001101000010001011000010110000010011010101000111111001100100111110110001011001000100100110101110011000100100000100110010010001111011111101010011100000000011101010100001000011000100010101110010101011111101111001110110111111101001010110011100110001101011000010001111001100011001110111110110111111111100001101110010001011000010110000010011010101000111111001011100101101000001000010011000110010110111101000100100111111011101000011110100000010110001101111111100000010100001000011000111000100101111111111001100011001110100100101101100001001111110000100001000101010010001011100100110010111001000111111111111100010110010001011000010110000010011010101000111111001010101100010110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100010101000101000010100100010101101111010011111111111000000000001111100110111110001101110100011111000000010001011000010110000010011010101000111111001001101011011110000010100000001011111011100011111001010010011110000111001111000101010100101001011110111101111111011000100001111101100010000110011101101110100001000011000000111101011011010000101111000001101111100100001001101010010001010110111100111111010000010001011000010110000010011010101000111111001000101000011010000000000001110000101110011011110010000100001111100110111100000000000001010110010011010101011111001010010100101111001010111011001111100110111011010000000111010111000000011110111110011111111101000100110011100100111101011000000111010001101000010001011000010110000010011010101000111111000111100110000110001011010101011011111110001101000100100000110000101011001000011111101010101100000000001011010100001000011001110111000001110100101010100101001000001100111001110001110001100110011010000101111110111101111011011101100001010111111110110100101100010001011000010110000010011010101000111111000110100101000010001000011100000110000111101111000100100111110100010101000000011111101001101110000000000101010100001000011001111110000101011010001001101000100011000110111001100001001100101110000000000110000111100011111100110000000010111000000000010011101100010001011000010110000010011010101000111111000101101101000000010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100010101000101000010100100010101101111010011111111111000000000001111100110111110001101110100011111000000010001011000010110000010011010101000111111000100101011110100000010100000001011111011100011111001010010011110000111001111000101010100101001011110111101111111011000100001111101100010000110011101101110100001000011000000111101011011010000101111000001101111100100001001101010010001010110111100111111010000010001011000010110000010011010101000111111000011101001101010000000000001110000101110011011110010000100001111100110111100000000000001010110010011010101011111001010010100101111001010111011001111100110111011010000000111010111000000011110111110011111111101000100110011100100111101011000000111010001101000010001011000010110000010011010101000111111000010100111001000001000000101010101010101010110010101010101010101010101011110010101010101010101010101011110010101010101010101010101011110010101010101010101010101011110010101010101010101010101011110010101010101010101010101011110010101010101010101010101011110010001011000010110000010011010101000111111000001100101110000001000100010101001100000110000111011011000000101100111001110011111101001110000000000001100010100001000011011000110111100111111110100001010010110001110111011000110001000101111000010010101110010111000100010110000111101100111111111000001001010010001011000010110000010011010101000111111000000101100001110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100010101000101000010100100010101101111010011111111111000000000001111100110111110001101110100011111000000010001011000010110000010011010101000111110111111101010011110000010100000001011111011100011111001010010011110000111001111000101010100101001011110111101111111011000100001111101100010000110011101101110100001000011000000111101011011010000101111000001101111100100001001101010010001010110111100111111010000010001011000010110000010011010101000111110111110101001011010000000000001110000101110011011110010000100001111100110111100000000000001010110010011010101011111001010010100101111001010111011001111100110111011010000000111010111000000011110111110011111111101000100110011100100111101011000000111010001101000010001011000010110000010011010101000111110111101100110000100001011100100110001100110011101111011011111001100010100011110100000010101001011111111100001101011110111100110011011110001010101110011111100101000001001010110111100011000011000110011010000001011000011000010011111111100000111111111101011011000010001011000010110000010011010101000111110111100100101000000001000101000110010010110101001000100100000001000100100110110011111101010001110000000010000010100001000011000001111000101111010011110100100001010101111000001100110111000011011110011001111111000111010001011000001011001101000000000000110110110010001011000010110000010011010101000111110111011101101100100010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100010101000101000010100100010101101111010011111111111000000000001111100110111110001101110100011111000000010001011000010110000010011010101000111110111010101011010000000010100000001011111011100011111001010010011110000111001111000101010100101001011110111101111111011000100001111101100010000110011101101110100001000011000000111101011011010000101111000001101111100100001001101010010001010110111100111111010000010001011000010110000010011010101000111110111001101001101000000000000001110000101110011011110010000100001111100110111100000000000001010110010011010101011111001010010100101111001010111011001111100110111011010000000111010111000000011110111110011111111101000100110011100100111101011000000111010001101000010001011000010110000010011010101000111110111000100111001010001111001111101001101001011111101000101000011100101100010011011000110110110001100000111110010111011101000101100110001010101100001000111010000011000110000111001001001100000011011101111111101010010000110010010111011101101010000000101000010010010001011000010110000010011010101000111110110111100100110000001000110001111111111100110011000100100111111101000101101001011111101001111100000000001000010100001000011010000011110110011010100011010101100010001101011110000101010111110011111101001111101100010101101001001011111010100000000000111110100000010001011000010110000010011010101000111110110110101101001110010110000010100000000000001011001011001110000001101010011010010110111100110110000100111101110001110110000111000111011111101111111110111100010101000101000010100100010101101111010011111111111000000000001111100110111110001101110100011111000000010001011000010110000010011010101000111110110101101010000110000010100000001011111011100011111001010010011110000111001111000101010100101001011110111101111111011000100001111101100010000110011101101110100001000011000000111101011011010000101111000001101111100100001001101010010001010110111100111111010000010001011000010110000010011010101000111110110100101001000010000000000001110000101110011011110010000100001111100110111100000000000001010110010011010101011111001010010100101111001010111011001111100110111011010000000111010111000000011110111110011111111101000100110011100100111101011000000111010001101000010001011000010110000 \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/NAV/navascii3 b/GNSS_SDR_IQ/test/NAV/navascii3 new file mode 100644 index 0000000..5ff3d8c --- /dev/null +++ b/GNSS_SDR_IQ/test/NAV/navascii3 @@ -0,0 +1 @@ +00100010110000101100000100110101010001111111011101001001110100010101000010110110010110000111111100000000100100101001101010111111010011101000000000101110101000010000110111100001100110001010100011010010000110011000001101001001010011101101010010101101101001001000010011110011111101001111111111110101110100100010110000101100000100110101010001111111011011011001110000101100000101000100000000011000010100011100000011010100110010101101111001101100001001010010011100010011110001110001101001011111111101111000001001001100010000011000101011011110011100000000000000000000101100000100000101101111100100001010011100100010110000101100000100110101010001111111011001010100011000010000010000000001101001010111110000110000110100010001000111010010110001011000110011101010000000001001110100001000001101110011110100101100001001001110000011001011100010100001101110000011010110001100101111001101011101010010000110000010000000100010110000101100000100110101010001111111010111010000100100111111111011111111010110111100001011000110010100101011111000000000000100100000100110101001111101111010010110111010101010000111111001000100110011001000110101111100100100111100011101000000000110000101010100011100010000010001000111010100111100100010110000101100000100110101010001111111010101001101100000011110011111010011010010111111010001010000111001011000100110110001101101100011000001111100101110111010001011001100010101011000010001110100000110001100001110010010011000000110111011111111010100100001100100101110010011101011111001101000110000100010110000101100000100110101010001111111010011001000010000010101011010001110011001111010000011111111100100100011010111000000101100101111111111100100101000010000110101000111011110000000011010111000001111100101010110000110000001111010110100110011000001001100111110100010110110100000000000011101111100100010110000101100000100110101010001111111010001011011101100101100000101000100000000011000010100011100000011010100110010101101111001101100001001010010011100010011110001110001101001011111111101111000001001001100010000011000101011011110011100000000000000000000101100000100000101101111100100001010011100100010110000101100000100110101010001111111001111010111111000010000010000000001101001010111110000110000110100010001000111010010110001011000110011101010000000001001110100001000001101110011110100101100001001001110000011001011100010100001101110000011010110001100101111001101011101010010000110000010000000100010110000101100000100110101010001111111001101010001110000111111111011111111010110111100001011000110010100101011111000000000000100100000100110101001111101111010010110111010101010000111111001000100110011001000110101111100100100111100011101000000000110000101010100011100010000010001000111010100111100100010110000101100000100110101010001111111001011001111001100011111000011101010001010110101111001000100111000011101011010110111111110001001011101111110101010011010001001100000011000111010101110100100111111101000111001011101101000000111011100101001111001011011111100010011100111011100010100001010111000100010110000101100000100110101010001111111001001001001000100010101100011001100111111000011111100000000101100011011100101000000101100100011111111011100101000010000110011001001001110001011000111111111110000010011010100110000101001110100101101011110010111010000011101010010000101000000000000011101100000100010110000101100000100110101010001111111000111011000001100101100000101000100000000011000010100011100000011010100110010101101111001101100001001010010011100010011110001110001101001011111111101111000001001001100010000011000101011011110011100000000000000000000101100000100000101101111100100001010011100100010110000101100000100110101010001111111000101010101100100010000010000000001101001010111110000110000110100010001000111010010110001011000110011101010000000001001110100001000001101110011110100101100001001001110000011001011100010100001101110000011010110001100101111001101011101010010000110000010000000100010110000101100000100110101010001111111000011010000010100111111111011111111010110111100001011000110010100101011111000000000000100100000100110101001111101111010010110111010101010000111111001000100110011001000110101111100100100111100011101000000000110000101010100011100010000010001000111010100111100100010110000101100000100110101010001111111000001001101010000011111011010101011100001001100000111000001010000100100010000100101100111000010011001111110100101111010001100001010101011100101101101111101100001010001011010111001000011111000101010110001100011000100111110010000011010000001011000100101111000100010110000101100000100110101010001111110111111001000000100010101110100001101001101000110000011110000100100101110110100111111010011101000000000000111010111101111001001110101111011101010001000000011110010000001011101101100001001101010001000001101000011001011110111011010000011100000000000000100100000100010110000101100000100110101010001111110111101011011111000101100000101000100000000011000010100011100000011010100110010101101111001101100001001010010011100010011110001110001101001011111111101111000001001001100010000011000101011011110011100000000000000000000101100000100000101101111100100001010011100100010110000101100000100110101010001111110111011010101101000010000010000000001101001010111110000110000110100010001000111010010110001011000110011101010000000001001110100001000001101110011110100101100001001001110000011001011100010100001101110000011010110001100101111001101011101010010000110000010000000100010110000101100000100110101010001111110111001010011100000111111111011111111010110111100001011000110010100101011111000000000000100100000100110101001111101111010010110111010101010000111111001000100110011001000110101111100100100111100011101000000000110000101010100011100010000010001000111010100111100100010110000101100000100110101010001111110110111001100010000011111100100111000101000100001100101001101101110011011001110000010110011001011010111000010010010101100100110001001011100100101100101010010001110100000110101001110010100000111101101000001001101000110010101100010101010101010101010101000110000100010110000101100000100110101010001111110110101001010011000010110000000011000101111010100000011110000101100010110100001000000101100100011111111011100101000010000110100101110110000100000010100111101111010100010000010000100011001100010001010000101010011111000001100000111000000001111111111100110100000100010110000101100000100110101010001111110110011011010011100101100000101000100000000011000010100011100000011010100110010101101111001101100001001010010011100010011110001110001101001011111111101111000001001001100010000011000101011011110011100000000000000000000101100000100000101101111100100001010011100100010110000101100000100110101010001111110110001010111110100010000010000000001101001010111110000110000110100010001000111010010110001011000110011101010000000001001110100001000001101110011110100101100001001001110000011001011100010100001101110000011010110001100101111001101011101010010000110000010000000100010110000101100000100110101010001111110101111010000000000111111111011111111010110111100001011000110010100101011111000000000000100100000100110101001111101111010010110111010101010000111111001000100110011001000110101111100100100111100011101000000000110000101010100011100010000010001000111010100111100100010110000101100000100110101010001111110101101001101000100011111111011100110011001101110101010011010100110011001010000100110101001100110101001110101010101100110011001100110100110100110111011100110101001100000101010011010100100000000110110000000111111000000000000010011111111111111111111111110101100100010110000101100000100110101010001111110101011001000110100011100110000111111000010100111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111110101100100010110000101100000100110101010001111110101001011011001000101100000101000100000000011000010100011100000011010100110010101101111001101100001001010010011100010011110001110001101001011111111101111000001001001100010000011000101011011110011100000000000000000000101100000100000101101111100100001010011100100010110000101100000100110101010001111110100111010100010100010000010000000001101001010111110000110000110100010001000111010010110001011000110011101010000000001001110100001000001101110011110100101100001001001110000011001011100010100001101110000011010110001100101111001101011101010010000110000010000000100010110000101100000100110101010001111110100101010010011100111111111011111111010110111100001011000110010100101011111000000000000100100000100110101001111101111010010110111010101010000111111001000100110011001000110101111100100100111100011101000000000110000101010100011100010000010001000111010100111100100010110000101100000100110101010001111110100011001100100000011110011111010011010010111111010001010000111001011000100110110001101101100011000001111100101110111010001011001100010101011000010001110100000110001100001110010010011000000110111011111111010100100001100100101110001000100011100011101100101100100010110000101100000100110101010001111110100001001010101000010000010000110101100001001100000011110000101111010010111110111111010100110000000000100101010111101111001011010010010000000000011010010001000110011001111100000111000000010011011100000001110100101011100011011010000000000000000000010010100000100010110000101100000100110101010001111110011111011010110000101100000101000100000000011000010100011100000011010100110010101101111001101100001001010010011100010011110001110001101001011111111101111000001001001100010000011000101011011110011100000000000000000000101100000100000101101111100100001010011100100010110000101100000100110101010001111110011101010111011000010000010000000001101001010111110000110000110100010001000111010010110001011000110011101010000000001001110100001000001101110011110100101100001001001110000011001011100010100001101110000011010110001100101111001101011101010010000110000010000000100010110000101100000100110101010001111110011011010010101000111111111011111111010110111100001011000110010100101011111000000000000100100000100110101001111101111010010110111010101010000111111001000100110011001000110101111100100100111100011101000000000110000101010100011100010000010001000111010100111100100010110000101100000100110101010001111110011001001111101100010110010001001001011111011000000011110001001100100011001001000000101010100011111111001110101000010000110001001000010010101010111111111011001001100000000101101001101000111001011100110000110001010011010000110111111101101111111111100110110100100010110000101100000100110101010001111110010111001011010000010000100110001100100001100111111100000000001000101101001101000000101100010111111111010001010111101111001110001101101011111111110010101000100001010011011011000010111000001001000011110101001110011001010111011110001101110000000000000110000000100010110000101100000100110101010001111110010101011000101100101100000101000100000000011000010100011100000011010100110010101101111001101100001001010010011100010011110001110001101001011111111101111000001001001100010000011000101011011110011100000000000000000000101100000100000101101111100100001010011100100010110000101100000100110101010001111110010011010110111100010000010000000001101001010111110000110000110100010001000111010010110001011000110011101010000000001001110100001000001101110011110100101100001001001110000011001011100010100001101110000011010110001100101111001101011101010010000110000010000000100010110000101100000100110101010001111110010001010000110100111111111011111111010110111100001011000110010100101011111000000000000100100000100110101001111101111010010110111010101010000111111001000100110011001000110101111100100100111100011101000000000110000101010100011100010000010001000111010100111100100010110000101100000100110101010001111110001111001100001100010110101010110111111100011010000011110001100001010110010011000000101011001111111111110011010111101111001100011000010000010110101011010010100111101111110011100011101010100000110100001100001011001011100011001101000010101111111110001100100100100010110000101100000100110101010001111110001101001010000100010000111000001100001110001101111100000000010111011000110010111111010011000100000000000010101000010000110011110011100001001011101100010000100001111100001100111101100000110011010010001100011100010111101100000111111010001111111111011100101100100010110000101100000100110101010001111110001011011010000000101100000101000100000000011000010100011100000011010100110010101101111001101100001001010010011100010011110001110001101001011111111101111000001001001100010000011000101011011110011100000000000000000000101100000100000101101111100100001010011100100010110000101100000100110101010001111110001001010111101000010000010000000001101001010111110000110000110100010001000111010010110001011000110011101010000000001001110100001000001101110011110100101100001001001110000011001011100010100001101110000011010110001100101111001101011101010010000110000010000000100010110000101100000100110101010001111110000111010011010100111111111011111111010110111100001011000110010100101011111000000000000100100000100110101001111101111010010110111010101010000111111001000100110011001000110101111100100100111100011101000000000110000101010100011100010000010001000111010100111100100010110000101100000100110101010001111110000101001110010000010000001010101010101010101100101010101010101010101010111100101010101010101010101010111100101010101010101010101010111100101010101010101010101010111100101010101010101010101010111100101010101010101010101010111100101010101010101010101010111100100010110000101100000100110101010001111110000011001011100000010001000101010011000110011100000011111111110100110001001001000000101100010111111111010001010111101111001001110010101111111111100111110110101110100111110110001100100010111101010101010100001100011100011010101001111011001111111110000111001100100010110000101100000100110101010001111110000001011000011100101100000101000100000000011000010100011100000011010100110010101101111001101100001001010010011100010011110001110001101001011111111101111000001001001100010000011000101011011110011100000000000000000000101100000100000101101111100100001010011100100010110000101100000100110101010001111101111111010100111100010000010000000001101001010111110000110000110100010001000111010010110001011000110011101010000000001001110100001000001101110011110100101100001001001110000011001011100010100001101110000011010110001100101111001101011101010010000110000010000000100010110000101100000100110101010001111101111101010010110100111111111011111111010110111100001011000110010100101011111000000000000100100000100110101001111101111010010110111010101010000111111001000100110011001000110101111100100100111100011101000000000110000101010100011100010000010001000111010100111100100010110000101100000100110101010001111101111011001100001000010111001001100011001110011110000011110001100111010111101000111111010110000000000000111110101000010000110011001001100111010100010111101010011101010101010010000111010110001100101111011111011100101111011100010111111000001111111111111010110100100010110000101100000100110101010001111101111001001010000000010001010001100100101111110111111100001111101110110011100001000000101011010111111111111011010111101111001111011011110010001011000011010100111001000011111100110010010000010100111010011111111110100000001011111101001100101111111111110011000000100010110000101100000100110101010001111101110111011011001000101100000101000100000000011000010100011100000011010100110010101101111001101100001001010010011100010011110001110001101001011111111101111000001001001100010000011000101011011110011100000000000000000000101100000100000101101111100100001010011100100010110000101100000100110101010001111101110101010110100000010000010000000001101001010111110000110000110100010001000111010010110001011000110011101010000000001001110100001000001101110011110100101100001001001110000011001011100010100001101110000011010110001100101111001101011101010010000110000010000000100010110000101100000100110101010001111101110011010011010000111111111011111111010110111100001011000110010100101011111000000000000100100000100110101001111101111010010110111010101010000111111001000100110011001000110101111100100100111100011101000000000110000101010100011100010000010001000111010100111100100010110000101100000100110101010001111101110001001110010100011110011111010011010010111111010001010000111001011000100110110001101101100011000001111100101110111010001011001100010101011000010001110100000110001100001110010010011000000110111011111111010100100001100100101110111011010100000001010000100100100010110000101100000100110101010001111101101111001001100000010001100011111111110000100011111100000000000101110111110001000000101100100011111111011100101000010000110011111111010101001010111000110110110111110010111100001010100001100000110000011111100110000101001100100010001010111111111110000100111000100010110000101100000100110101010001111101101101011010011100101100000101000100000000011000010100011100000011010100110010101101111001101100001001010010011100010011110001110001101001011111111101111000001001001100010000011000101011011110011100000000000000000000101100000100000101101111100100001010011100100010110000101100000100110101010001111101101011010100001100010000010000000001101001010111110000110000110100010001000111010010110001011000110011101010000000001001110100001000001101110011110100101100001001001110000011001011100010100001101110000011010110001100101111001101011101010010000110000010000000100010110000101100000100110101010001111101101001010010000100111111111011111111010110111100001011000110010100101011111000000000000100100000100110101001111101111010010110111010101010000111111001000100110011001000110101111100100100111100011101000000000110000101010100011100010000010001000111010100111100100010110000101100000 \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/NAV/navascii6 b/GNSS_SDR_IQ/test/NAV/navascii6 new file mode 100644 index 0000000..bbfefd6 --- /dev/null +++ b/GNSS_SDR_IQ/test/NAV/navascii6 @@ -0,0 +1 @@ +1101110100111101001111101100101010111000000010001011011000101110101011110100100110101000111000100100111101101101010100100100000010110010101111111100001101011110111100100010011100000000101010001011010000001100010000110100100110001001101010101101001010010011001100011111010000001011000000000000101000101101110100111101001111101100101010111000000010010010011000111101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011111010110110000101110100010101101111001110000000000111111110111110111011000101011010110000011100010111101110100111101001111101100101010111000000010011010101110011111010001000000000010110011011111000101011111011100010010000001010100001000001011111000010100000000001101001111110001111011111110001001001111111101011111110100101101100101111011101111110010100100001010010001010010001010110111100111111111001101110100111101001111101100101010111000000010100010111101101111111111110001010010011010100110011101010001010101111000001111111111111000111101100111001110110001100001011100000001111111011111000000000000111101101010100100100101100001110010000011111111101000110001011001001111010001111101011101011110101101110100111101001111101100101010111000000010101011001001111110000110000010110010110100000010111010111100011010011101100100111001001001110011111000001101000100010111010011001110101010011110111000101111100111001111000110110110011111100100010000000010101101111001101101000110110001010000011001011100111101110100111101001111101100101010111000000010110011011110111110101010010111000101011110001111011011000001101101111010011100000010110010111111111110010010100001000011010100010010100000000001101001101000000110111101011000010111010101011001110100110100110001011000011100101011011010000000000001110010111101110100111101001111101100101010111000000010111010010001001101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011111010110110000101110100010101101111001110000000000111111110111110111011000101011010110000011100010111101110100111101001111101100101010111000000011000010100000011111010001000000000010110011011111000101011111011100010010000001010100001000001011111000010100000000001101001111110001111011111110001001001111111101011111110100101101100101111011101111110010100100001010010001010010001010110111100111111111001101110100111101001111101100101010111000000011001010111000111111111111110001010010011010100110011101010001010101111000001111111111111000111101100111001110110001100001011100000001111111011111000000000000111101101010100100100101100001110010000011111111101000110001011001001111010001111101011101011110101101110100111101001111101100101010111000000011010011000011001110000011110001010111010100101000011011101100011110001010010100100000000111011010001000000101010110010111011001111110011100010101000101101100000001011100011010001001011111100010001101011000011010010000001110110001100010001110101111010100011101110100111101001111101100101010111000000011011011011011101110101001110011001011100111111000100100111101001110001001111011111101001101000000000011001010100001000011001100001000011111010011100001111001110001011010101100111100011110010000010101111010010000010011100110111000010100000000000001110011001101110100111101001111101100101010111000000011100010011111001101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011111010110110000101110100010101101111001110000000000111111110111110111011000101011010110000011100010111101110100111101001111101100101010111000000011101010101001101111010001000000000010110011011111000101011111011100010010000001010100001000001011111000010100000000001101001111110001111011111110001001001111111101011111110100101101100101111011101111110010100100001010010001010010001010110111100111111111001101110100111101001111101100101010111000000011110010111110101111111111110001010010011010100110011101010001010101111000001111111111111000111101100111001110110001100001011100000001111111011111000000000000111101101010100100100101100001110010000011111111101000110001011001001111010001111101011101011110101101110100111101001111101100101010111000000011111011001010111110000010010101010001111011001111100011111010111101101110111101101001100011110110011000000101101000010111001111010101010001101001001000001001111010111010010100011011110000011101010100111001110011101100000110111110010111111010011101101000011101110100111101001111101100101010111000000100000011011111101110101000101111001010100111010111011011111101101101000100100011111101010001000000000011000101011110111100100111000000101001010111011101111001111100010101110110110000000011111101000111001100101001110101000001010000001110000000000000011111111101110100111101001111101100101010111000000100001010010000011101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011111010110110000101110100010101101111001110000000000111111110111110111011000101011010110000011100010111101110100111101001111101100101010111000000100010010101001011111010001000000000010110011011111000101011111011100010010000001010100001000001011111000010100000000001101001111110001111011111110001001001111111101011111110100101101100101111011101111110010100100001010010001010010001010110111100111111111001101110100111101001111101100101010111000000100011010110001111111111111110001010010011010100110011101010001010101111000001111111111111000111101100111001110110001100001011100000001111111011111000000000000111101101010100100100101100001110010000011111111101000110001011001001111010001111101011101011110101101110100111101001111101100101010111000000100100011001110111110000001101100011101011101111001101011001001000110010011000111110100110011010010100011110110110101001101100111011010001101101001101010110111000101111100101011000110101111100001001011111011001011100110101001110101010101010101010101011100111101110100111101001111101100101010111000000100101011010110011110100111111110011100011010100111011011111101001110110010110011111101001101000000000001101101011110111100101101001011100101111110101101111111100001001000001000011011100000000000100000010100010000011011000100011011111111000000000001101101001101110100111101001111101100101010111000000100110010010110001101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011111010110110000101110100010101101111001110000000000111111110111110111011000101011010110000011100010111101110100111101001111101100101010111000000100111010100000101111010001000000000010110011011111000101011111011100010010000001010100001000001011111000010100000000001101001111110001111011111110001001001111111101011111110100101101100101111011101111110010100100001010010001010010001010110111100111111111001101110100111101001111101100101010111000000101000010111111111111111111110001010010011010100110011101010001010101111000001111111111111000111101100111001110110001100001011100000001111111011111000000000000111101101010100100100101100001110010000011111111101000110001011001001111010001111101011101011110101101110100111101001111101100101010111000000101001011001011101110000000010001100110011001000101010110010101100110011010111101100101011001100101011000101010101001100110011001100101100101100100010001100101011001111101010110010101101111111100100111111100000011111111111110110000000000000000000000000101001101110100111101001111101100101010111000000101010011011100101110001100110110110011110100010111111111111111111111111101011000000000000000000000000010100111111111111111111111111101011000000000000000000000000010100111111111111111111111111101011000000000000000000000000010100111111111111111111111110111001101110100111101001111101100101010111000000101011010010011011101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011111010110110000101110100010101101111001110000000000111111110111110111011000101011010110000011100010111101110100111101001111101100101010111000000101100010101110101111010001000000000010110011011111000101011111011100010010000001010100001000001011111000010100000000001101001111110001111011111110001001001111111101011111110100101101100101111011101111110010100100001010010001010010001010110111100111111111001101110100111101001111101100101010111000000101101010110110001111111111110001010010011010100110011101010001010101111000001111111111111000111101100111001110110001100001011100000001111111011111000000000000111101101010100100100101100001110010000011111111101000110001011001001111010001111101011101011110101101110100111101001111101100101010111000000101110011001101111110000110000010110010110100000010111010111100011010011101100100111001001001110011111000001101000100010111010011001110101010011110111000101111100111001111000110110110011111100100010000000010101101111001101101000111011101110001110001001101001101110100111101001111101100101010111000000101111011010101011110111110111100101001111011001111011011111101000010111011010011111101010010110000000010001101011110111100101101010100010000000001100111010000100111101000001111100000000111000111010111111001011110100101011011011000000000000000000001011010101101110100111101001111101100101010111000000110000010010100111101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011111010110110000101110100010101101111001110000000000111111110111110111011000101011010110000011100010111101110100111101001111101100101010111000000110001010100010011111010001000000000010110011011111000101011111011100010010000001010100001000001011111000010100000000001101001111110001111011111110001001001111111101011111110100101101100101111011101111110010100100001010010001010010001010110111100111111111001101110100111101001111101100101010111000000110010010110101011111111111110001010010011010100110011101010001010101111000001111111111111000111101100111001110110001100001011100000001111111011111000000000000111101101010100100100101100001110010000011111111101000110001011001001111010001111101011101011110101101110100111101001111101100101010111000000110011011000001001110100110111011011001010001100111011011111011001101101110000100000010101100001111111100101101011110111100111011101000101010101011111101111001110100011000010110101001100011001100110011000010001111001100011011010000001001000000000001100100101101110100111101001111101100101010111000000110100011010010111110111101100111001101001000010111011011000000100010111100001011111101001110100000000010111010100001000011000111000110001000000000110011100110001001111101101100001001111110000110101111010101101110100011011011001111001000111111111111101111001101110100111101001111101100101010111000000110101010011101001101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011111010110110000101110100010101101111001110000000000111111110111110111011000101011010110000011100010111101110100111101001111101100101010111000000110110010100100001111010001000000000010110011011111000101011111011100010010000001010100001000001011111000010100000000001101001111110001111011111110001001001111111101011111110100101101100101111011101111110010100100001010010001010010001010110111100111111111001101110100111101001111101100101010111000000110111010111100101111111111110001010010011010100110011101010001010101111000001111111111111000111101100111001110110001100001011100000001111111011111000000000000111101101010100100100101100001110010000011111111101000110001011001001111010001111101011101011110101101110100111101001111101100101010111000000111000011001111001110100101010100100000001110010111011011111001111010100110111100000010101010011111111110100101011110111100110001001000101001011010101011010110111100111111001110001110001100110001110111010000001000010000100110110100001010111111111000100010111101110100111101001111101100101010111000000111001011010111101110111100011111001111000010000111011011000001011101010111111100000010110010001111111111010101011110111100110000010000100011010001001101000100011010010000110011110110011010001101011000110000111100011111100100100111101000111111111101111111001101110100111101001111101100101010111000000111010010010111111101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011111010110110000101110100010101101111001110000000000111111110111110111011000101011010110000011100010111101110100111101001111101100101010111000000111011010100001011111010001000000000010110011011111000101011111011100010010000001010100001000001011111000010100000000001101001111110001111011111110001001001111111101011111110100101101100101111011101111110010100100001010010001010010001010110111100111111111001101110100111101001111101100101010111000000111100010110010101111111111110001010010011010100110011101010001010101111000001111111111111000111101100111001110110001100001011100000001111111011111000000000000111101101010100100100101100001110010000011111111101000110001011001001111010001111101011101011110101101110100111101001111101100101010111000000111101011000110111110111111010101010101010101001101010101010101010101010100001101010101010101010101010100001101010101010101010101010100001101010101010101010101010100001101010101010101010101010100001101010101010101010101010100001101010101010101010101010100001101110100111101001111101100101010111000000111110011010001111110111011101010110011111001111000100100111111010011000110001100000010110001101111111110100101011110111100100111001111110000000001011110101101001100101111011000110001000101111010110101010001101000111011101011011111101100111111111000000100101101110100111101001111101100101010111000000111111010011110001101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011111010110110000101110100010101101111001110000000000111111110111110111011000101011010110000011100010111101110100111101001111101100101010111000001000000010101100001111010001000000000010110011011111000101011111011100010010000001010100001000001011111000010100000000001101001111110001111011111110001001001111111101011111110100101101100101111011101111110010100100001010010001010010001010110111100111111111001101110100111101001111101100101010111000001000001010110100101111111111110001010010011010100110011101010001010101111000001111111111111000111101100111001110110001100001011100000001111111011111000000000000111101101010100100100101100001110010000011111111101000110001011001001111010001111101011101011110101101110100111101001111101100101010111000001000010011001111011110100011011001110011001100010000100100000110011101011100001011111101010110100000000011110010100001000011001100100001110101010001100000011010111110110101001000011100111100111001100101111110100111100111101100000000011111000000000000010100101101110100111101001111101100101010111000001000011011010111111110111010111001101101001010110111011011111110111011011001001100000010101110001111111101111101011110111100111110000111010000101100001011011110101010000111110011001000111100100001100110000000111000101110100111110100110010111111111111001001001101110100111101001111101100101010111000001000100010010011011101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011111010110110000101110100010101101111001110000000000111111110111110111011000101011010110000011100010111101110100111101001111101100101010111000001000101010100101111111010001000000000010110011011111000101011111011100010010000001010100001000001011111000010100000000001101001111110001111011111110001001001111111101011111110100101101100101111011101111110010100100001010010001010010001010110111100111111111001101110100111101001111101100101010111000001000110010110010111111111111110001010010011010100110011101010001010101111000001111111111111000111101100111001110110001100001011100000001111111011111000000000000111101101010100100100101100001110010000011111111101000110001011001001111010001111101011101011110101101110100111101001111101100101010111000001000111011000110101110000110000010110010110100000010111010111100011010011101100100111001001001110011111000001101000100010111010011001110101010011110111000101111100111001111000110110110011111100100010000000010101101111001101101000100010010101111111010111101101101110100111101001111101100101010111000001001000011011001111110111001110000000000011001100111011011000000010111010010110100000010110000011111111110111101011110111100101111100001001100101011100101010011101110010100001111010101000001100000010110000010011101010010110110100000101010111111111001111000001101110100111101001111101100101010111000001001001010010110001101001111101011111111111110100110100110001111110010101100101101001000011001001111011000010001110001001111000111000100000010000000001000011111010110110000101110100010101101111001110000000000111111110111110111011000101011010110000011100010111101110100111101001111101100101010111000001001010010101111001111010001000000000010110011011111000101011111011100010010000001010100001000001011111000010100000000001101001111110001111011111110001001001111111101011111110100101101100101111011101111110010100100001010010001010010001010110111100111111111001101110100111101001111101100101010111000001001011010110111101111111111110001010010011010100110011101010001010101111000001111111111111000111101100111001110110001100001011100000001111111011111000000000000111101101010100100100101100001110010000011111111101000110001011001001111010001111101011101011110101101110100111101001111 \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/NAV3/navascii1 b/GNSS_SDR_IQ/test/NAV3/navascii1 new file mode 100644 index 0000000..532a155 --- /dev/null +++ b/GNSS_SDR_IQ/test/NAV3/navascii1 @@ -0,0 +1 @@ +1000111101101011101001111111111111111000100100000000011101000101000001110000000001111001101110100011001110101000001101110100101010011101010110010011110011010110010111101001101100100000010101101010100111001111010101011010110000011110010011110000000011110000000000000000000000000101001100111110101000111111111110011011101001111111111111111101101011111111100010101010011110101111111111101000111111111111011111111111111111111111111111111000000000001101111011000000110011111110100001000110011001010000111100101011110111011110000001010100110001011101000011000111111111010101100100010010000000000001111110101100000001011011101001111111111111111101101011111111100010011001111101100000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000100010110000000000000000010010100000000011101110110100111110000000000001000001100100110001011100001110111100110011000010111111010100101000000000000100101000010000110001001001101000100011001001011111100111010010111001010001000000111110000011000001010110111100110101101011000010001111111111001111010011100010110000000000000000010010100000000011100110100100001101111111111111111011111111011000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000011111001100100001010010110000010101110001100000000000000000000001111101011111000011010000001000111101101011101001111111111111111000100100000000011101000101000001110000000001111001101110100011001110101000001101110100101010011101010110010011110011010110010111101001101100100000010101101010100111001111010101011010110000011110010011110000000011110000000000000000000000000101001100111110101000111111111110011011101001111111111111111101101011111111100010101010011110101111111111101000111111111111011111111111111111111111111111111000000000001101111011000000110011111110100001000110011001010000111100101011110111011110000001010100110001011101000011000111111111010101100100010010000000000001111110101100000001011011101001111111111111111101101011111111100010011001111101100000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000100010110000000000000000010010100000000011101110110100111110000000000001000001100100110001011100001110111100110011000010111111010100101000000000000100101000010000110001001001101000100011001001011111100111010010111001010001000000111110000011000001010110111100110101101011000010001111111111001111010011 \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/NAV3/navascii14 b/GNSS_SDR_IQ/test/NAV3/navascii14 new file mode 100644 index 0000000..532a155 --- /dev/null +++ b/GNSS_SDR_IQ/test/NAV3/navascii14 @@ -0,0 +1 @@ +1000111101101011101001111111111111111000100100000000011101000101000001110000000001111001101110100011001110101000001101110100101010011101010110010011110011010110010111101001101100100000010101101010100111001111010101011010110000011110010011110000000011110000000000000000000000000101001100111110101000111111111110011011101001111111111111111101101011111111100010101010011110101111111111101000111111111111011111111111111111111111111111111000000000001101111011000000110011111110100001000110011001010000111100101011110111011110000001010100110001011101000011000111111111010101100100010010000000000001111110101100000001011011101001111111111111111101101011111111100010011001111101100000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000100010110000000000000000010010100000000011101110110100111110000000000001000001100100110001011100001110111100110011000010111111010100101000000000000100101000010000110001001001101000100011001001011111100111010010111001010001000000111110000011000001010110111100110101101011000010001111111111001111010011100010110000000000000000010010100000000011100110100100001101111111111111111011111111011000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000011111001100100001010010110000010101110001100000000000000000000001111101011111000011010000001000111101101011101001111111111111111000100100000000011101000101000001110000000001111001101110100011001110101000001101110100101010011101010110010011110011010110010111101001101100100000010101101010100111001111010101011010110000011110010011110000000011110000000000000000000000000101001100111110101000111111111110011011101001111111111111111101101011111111100010101010011110101111111111101000111111111111011111111111111111111111111111111000000000001101111011000000110011111110100001000110011001010000111100101011110111011110000001010100110001011101000011000111111111010101100100010010000000000001111110101100000001011011101001111111111111111101101011111111100010011001111101100000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000100010110000000000000000010010100000000011101110110100111110000000000001000001100100110001011100001110111100110011000010111111010100101000000000000100101000010000110001001001101000100011001001011111100111010010111001010001000000111110000011000001010110111100110101101011000010001111111111001111010011 \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/NAV3/navascii6 b/GNSS_SDR_IQ/test/NAV3/navascii6 new file mode 100644 index 0000000..ce9ab42 --- /dev/null +++ b/GNSS_SDR_IQ/test/NAV3/navascii6 @@ -0,0 +1 @@ +011101001111111111111111101101011111111100011001011011110010000000000000000100000000100111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111100000110011011110101101001111101010001110011111111111111111111110000010100000111100101111110111000010010100010110000000000000000111011011111111100010111010111110001111111110000110010001011100110001010111110010001011010101100010101001101100001100101001101000010110010011011111101010010101011000110000101010100101001111100001101100001111111100001111111111111111111111111010110011000001010111000000000001100100010110000000000000000010010100000000011101010101100001010000000000010111000000000000100000000000000000000000000000000111111111110010000100111111001100000001011110111001100110101111000011010100001000100001111110101011001110100010111100111000000000101010011011101101111111111110000001010011111110100100010110000000000000000010010100000000011101100110000010011111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111011101001111111111111111101101011111111100010001001011000001111111111110111110011011001110100011110001000011001100111101000000101011010111111111111011010111101111001110110110010111011100110110100000011000101101000110101110111111000001111100111110101001000011001010010100111101110000000000110000101100 \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/Test_Tracking.m b/GNSS_SDR_IQ/test/Test_Tracking.m new file mode 100644 index 0000000..f766d89 --- /dev/null +++ b/GNSS_SDR_IQ/test/Test_Tracking.m @@ -0,0 +1,17 @@ +%Test tracking only +clear; close all; clc; + +format ('compact'); +format ('long', 'g'); + +%--- Include folders with functions --------------------------------------- +addpath ..\include % The software receiver functions +addpath ..\geoFunctions % Position calculation related functions +addpath .. + +load trackingResults.mat; +settings=initSettings(); + +navSolutions = postNavigation(trackResults, settings); +%plot(navSolutions.latitude,navSolutions.longitude,'o'); +plotNavigation(navSolutions, settings); \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/TrackResults2NavBits.m b/GNSS_SDR_IQ/test/TrackResults2NavBits.m new file mode 100644 index 0000000..42c3467 --- /dev/null +++ b/GNSS_SDR_IQ/test/TrackResults2NavBits.m @@ -0,0 +1,167 @@ +function [firstSubFrame, activeChnList] =TrackResults2NavBits(trackResults, ... + settings) +% findPreambles finds the first preamble occurrence in the bit stream of +% each channel. The preamble is verified by check of the spacing between +% preambles (6sec) and parity checking of the first two words in a +% subframe. At the same time function returns list of channels, that are in +% tracking state and with valid preambles in the nav data stream. +% +%[firstSubFrame, activeChnList] = findPreambles(trackResults, settings) +% +% Inputs: +% trackResults - output from the tracking function +% settings - Receiver settings. +% +% Outputs: +% firstSubframe - the array contains positions of the first +% preamble in each channel. The position is ms count +% since start of tracking. Corresponding value will +% be set to 0 if no valid preambles were detected in +% the channel. +% activeChnList - list of channels containing valid preambles + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen +% Written by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen +%-------------------------------------------------------------------------- +% +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% CVS record: +% $Id: findPreambles.m,v 1.1.2.10 2006/08/14 11:38:22 dpl Exp $ + +% Preamble search can be delayed to a later point in the tracking results +% to avoid noise due to tracking loop transients +searchStartOffset = 0; + +%--- Initialize the firstSubFrame array ----------------------------------- +firstSubFrame = zeros(1, settings.numberOfChannels); + +%--- Generate the preamble pattern ---------------------------------------- +preamble_bits = [1 -1 -1 -1 1 -1 1 1]; + +% "Upsample" the preamble - make 20 vales per one bit. The preamble must be +% found with precision of a sample. +preamble_ms = kron(preamble_bits, ones(1, 20)); + +%--- Make a list of channels excluding not tracking channels -------------- +activeChnList = find([trackResults.status] ~= '-'); + +%=== For all tracking channels ... +for channelNr = activeChnList + +%% Correlate tracking output with preamble ================================ + % Read output from tracking. It contains the navigation bits. The start + % of record is skiped here to avoid tracking loop transients. + bits = trackResults(channelNr).I_P(1 + searchStartOffset : end); + + % Now threshold the output and convert it to -1 and +1 + bits(bits > 0) = 1; + bits(bits <= 0) = -1; + + % Correlate tracking output with the preamble + tlmXcorrResult = xcorr(bits, preamble_ms); + +%% Find all starting points off all preamble like patterns ================ + clear index + clear index2 + + xcorrLength = (length(tlmXcorrResult) + 1) /2; + + %--- Find at what index/ms the preambles start ------------------------ + index = find(... + abs(tlmXcorrResult(xcorrLength : xcorrLength * 2 - 1)) > 153)' + ... + searchStartOffset; + +%% Analyze detected preamble like patterns ================================ + for i = 1:size(index) % For each occurrence + + %--- Find distances in time between this occurrence and the rest of + %preambles like patterns. If the distance is 6000 milliseconds (one + %subframe), the do further verifications by validating the parities + %of two GPS words + + index2 = index - index(i); + + if (~isempty(find(index2 == 6000))) + + %=== Re-read bit vales for preamble verification ============== + % Preamble occurrence is verified by checking the parity of + % the first two words in the subframe. Now it is assumed that + % bit boundaries a known. Therefore the bit values over 20ms are + % combined to increase receiver performance for noisy signals. + % in Total 62 bits mast be read : + % 2 bits from previous subframe are needed for parity checking; + % 60 bits for the first two 30bit words (TLM and HOW words). + % The index is pointing at the start of TLM word. + bits = trackResults(channelNr).I_P(index(i)-40 : ... + index(i) + 20 * 60 -1)'; + + %--- Combine the 20 values of each bit ------------------------ + bits = reshape(bits, 20, (size(bits, 1) / 20)); + bits = sum(bits); + + % Now threshold and make it -1 and +1 + bits(bits > 0) = 1; + bits(bits <= 0) = -1; + + %--- Check the parity of the TLM and HOW words ---------------- + if (navPartyChk(bits(1:32)) ~= 0) && ... + (navPartyChk(bits(31:62)) ~= 0) + % Parity was OK. Record the preamble start position. Skip + % the rest of preamble pattern checking for this channel + % and process next channel. + + firstSubFrame(channelNr) = index(i); + + %% + Nsamp=floor((length(trackResults(channelNr).I_P)-index(i)+40)/20)*20 ; + tmpbits = trackResults(channelNr).I_P(index(i)-40 : index(i)-40+Nsamp-1)'; + + %--- Combine the 20 values of each bit ------------------------ + tmpbits = reshape(tmpbits, 20, (size(tmpbits, 1) / 20)); + tmpbits = sum(tmpbits); + + % Now threshold and make it -1 and +1 + tmpbits(tmpbits > 0) = '1'; + tmpbits(tmpbits <= 0) = '0'; + + fid=fopen(['NAV\navascii',num2str(trackResults(channelNr).PRN)],'wt'); + fwrite(fid,tmpbits,'char'); + fclose(fid); + %% + break; + end % if parity is OK ... + + end % if (~isempty(find(index2 == 6000))) + end % for i = 1:size(index) + + % Exclude channel from the active channel list if no valid preamble was + % detected + if firstSubFrame(channelNr) == 0 + + % Exclude channel from further processing. It does not contain any + % valid preamble and therefore nothing more can be done for it. + activeChnList = setdiff(activeChnList, channelNr); + + disp(['Could not find valid preambles in channel ', ... + num2str(channelNr),'!']); + end + +end % for channelNr = activeChnList \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/findPreambles2.m b/GNSS_SDR_IQ/test/findPreambles2.m new file mode 100644 index 0000000..5c5b02c --- /dev/null +++ b/GNSS_SDR_IQ/test/findPreambles2.m @@ -0,0 +1,150 @@ +function [firstSubFrame, activeChnList] = findPreambles2(trackResults, ... + settings) +% findPreambles finds the first preamble occurrence in the bit stream of +% each channel. The preamble is verified by check of the spacing between +% preambles (6sec) and parity checking of the first two words in a +% subframe. At the same time function returns list of channels, that are in +% tracking state and with valid preambles in the nav data stream. +% +%[firstSubFrame, activeChnList] = findPreambles(trackResults, settings) +% +% Inputs: +% trackResults - output from the tracking function +% settings - Receiver settings. +% +% Outputs: +% firstSubframe - the array contains positions of the first +% preamble in each channel. The position is ms count +% since start of tracking. Corresponding value will +% be set to 0 if no valid preambles were detected in +% the channel. +% activeChnList - list of channels containing valid preambles + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen +% Written by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen +%-------------------------------------------------------------------------- +% +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% CVS record: +% $Id: findPreambles.m,v 1.1.2.10 2006/08/14 11:38:22 dpl Exp $ + +% Preamble search can be delayed to a later point in the tracking results +% to avoid noise due to tracking loop transients +searchStartOffset = 0; + +%--- Initialize the firstSubFrame array ----------------------------------- +firstSubFrame = zeros(1, settings.numberOfChannels); + +%--- Generate the preamble pattern ---------------------------------------- +preamble_bits = [1 -1 -1 -1 1 -1 1 1]; + +% "Upsample" the preamble - make 20 vales per one bit. The preamble must be +% found with precision of a sample. +preamble_ms = kron(preamble_bits, ones(1, 20)); + +%--- Make a list of channels excluding not tracking channels -------------- +activeChnList = find([trackResults.status] ~= '-'); + +%=== For all tracking channels ... +for channelNr = activeChnList + +%% Correlate tracking output with preamble ================================ + % Read output from tracking. It contains the navigation bits. The start + % of record is skiped here to avoid tracking loop transients. + bits = trackResults(channelNr).I_P(1 + searchStartOffset : end); + + % Now threshold the output and convert it to -1 and +1 + bits(bits > 0) = 1; + bits(bits <= 0) = -1; + + % Correlate tracking output with the preamble + tlmXcorrResult = xcorr(bits, preamble_ms); + +%% Find all starting points off all preamble like patterns ================ + clear index + clear index2 + + xcorrLength = (length(tlmXcorrResult) + 1) /2; + + %--- Find at what index/ms the preambles start ------------------------ + index = find(... + abs(tlmXcorrResult(xcorrLength : xcorrLength * 2 - 1)) > 153)' + ... + searchStartOffset; + +%% Analyze detected preamble like patterns ================================ + for i = 1:size(index) % For each occurrence + + %--- Find distances in time between this occurrence and the rest of + %preambles like patterns. If the distance is 6000 milliseconds (one + %subframe), the do further verifications by validating the parities + %of two GPS words + + index2 = index - index(i); + + if (~isempty(find(index2 == 6000))) + + %=== Re-read bit vales for preamble verification ============== + % Preamble occurrence is verified by checking the parity of + % the first two words in the subframe. Now it is assumed that + % bit boundaries a known. Therefore the bit values over 20ms are + % combined to increase receiver performance for noisy signals. + % in Total 62 bits mast be read : + % 2 bits from previous subframe are needed for parity checking; + % 60 bits for the first two 30bit words (TLM and HOW words). + % The index is pointing at the start of TLM word. + bits = trackResults(channelNr).I_P(index(i)-40 : ... + index(i) + 20 * 60 -1)'; + + %--- Combine the 20 values of each bit ------------------------ + bits = reshape(bits, 20, (size(bits, 1) / 20)); + bits = sum(bits); + + % Now threshold and make it -1 and +1 + bits(bits > 0) = 1; + bits(bits <= 0) = -1; + + %--- Check the parity of the TLM and HOW words ---------------- + if (navPartyChk(bits(1:32)) ~= 0) && ... + (navPartyChk(bits(31:62)) ~= 0) + % Parity was OK. Record the preamble start position. Skip + % the rest of preamble pattern checking for this channel + % and process next channel. + + firstSubFrame(channelNr) = index(i); + break; + end % if parity is OK ... + + end % if (~isempty(find(index2 == 6000))) + end % for i = 1:size(index) + + % Exclude channel from the active channel list if no valid preamble was + % detected + if firstSubFrame(channelNr) == 0 + + % Exclude channel from further processing. It does not contain any + % valid preamble and therefore nothing more can be done for it. + activeChnList = setdiff(activeChnList, channelNr); + + disp(['Could not find valid preambles in channel ', ... + num2str(channelNr),'!']); + end + +end % for channelNr = activeChnList diff --git a/GNSS_SDR_IQ/test/generateCAcode.m b/GNSS_SDR_IQ/test/generateCAcode.m new file mode 100644 index 0000000..86f79f1 --- /dev/null +++ b/GNSS_SDR_IQ/test/generateCAcode.m @@ -0,0 +1,90 @@ +function CAcode = generateCAcode(PRN) +% generateCAcode.m generates one of the 32 GPS satellite C/A codes. +% +% CAcode = generateCAcode(PRN) +% +% Inputs: +% PRN - PRN number of the sequence. +% +% Outputs: +% CAcode - a vector containing the desired C/A code sequence +% (chips). + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Darius Plausinaitis +% Written by Darius Plausinaitis +% Based on Dennis M. Akos, Peter Rinder and Nicolaj Bertelsen +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: generateCAcode.m,v 1.1.2.5 2006/08/14 11:38:22 dpl Exp $ + +%--- Make the code shift array. The shift depends on the PRN number ------- +% The g2s vector holds the appropriate shift of the g2 code to generate +% the C/A code (ex. for SV#19 - use a G2 shift of g2s(19) = 471) +g2s = [ 5, 6, 7, 8, 17, 18, 139, 140, 141, 251, ... + 252, 254, 255, 256, 257, 258, 469, 470, 471, 472, ... + 473, 474, 509, 512, 513, 514, 515, 516, 859, 860, ... + 861, 862 ... end of shifts for GPS satellites + ... Shifts for the ground GPS transmitter are not included + ... Shifts for EGNOS and WAAS satellites (true_PRN = PRN + 87) + 145, 175, 52, 21, 237, 235, 886, 657, ... + 634, 762, 355, 1012, 176, 603, 130, 359, 595, 68, ... + 386]; + +%--- Pick right shift for the given PRN number ---------------------------- +g2shift = g2s(PRN); + +%--- Generate G1 code ----------------------------------------------------- + +%--- Initialize g1 output to speed up the function --- +g1 = zeros(1, 1023); +%--- Load shift register --- +reg = -1*ones(1, 10); + +%--- Generate all G1 signal chips based on the G1 feedback polynomial ----- +for i=1:1023 + g1(i) = reg(10); + saveBit = reg(3)*reg(10); + reg(2:10) = reg(1:9); + reg(1) = saveBit; +end + +%--- Generate G2 code ----------------------------------------------------- + +%--- Initialize g2 output to speed up the function --- +g2 = zeros(1, 1023); +%--- Load shift register --- +reg = -1*ones(1, 10); + +%--- Generate all G2 signal chips based on the G2 feedback polynomial ----- +for i=1:1023 + g2(i) = reg(10); + saveBit = reg(2)*reg(3)*reg(6)*reg(8)*reg(9)*reg(10); + reg(2:10) = reg(1:9); + reg(1) = saveBit; +end + +%--- Shift G2 code -------------------------------------------------------- +%The idea: g2 = concatenate[ g2_right_part, g2_left_part ]; +g2 = [g2(1023-g2shift+1 : 1023), g2(1 : 1023-g2shift)]; + +%--- Form single sample C/A code by multiplying G1 and G2 ----------------- +CAcode = -(g1 .* g2); diff --git a/GNSS_SDR_IQ/test/navPartyChk.m b/GNSS_SDR_IQ/test/navPartyChk.m new file mode 100644 index 0000000..59071fd --- /dev/null +++ b/GNSS_SDR_IQ/test/navPartyChk.m @@ -0,0 +1,103 @@ +function status = navPartyChk(ndat) +% This function is called to compute and status the parity bits on GPS word. +% Based on the flowchart in Figure 2-10 in the 2nd Edition of the GPS-SPS +% Signal Spec. +% +%status = navPartyChk(ndat) +% +% Inputs: +% ndat - an array (1x32) of 32 bits represent a GPS navigation +% word which is 30 bits plus two previous bits used in +% the parity calculation (-2 -1 0 1 2 ... 28 29) +% +% Outputs: +% status - the test value which equals EITHER +1 or -1 if parity +% PASSED or 0 if parity fails. The +1 means bits #1-24 +% of the current word have the correct polarity, while -1 +% means the bits #1-24 of the current word must be +% inverted. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Written by Darius Plausinaitis, Kristin Larson +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +% CVS record: +% $Id: navPartyChk.m,v 1.1.2.5 2006/08/14 11:38:22 dpl Exp $ + +% In order to accomplish the exclusive or operation using multiplication +% this program represents a '0' with a '-1' and a '1' with a '1' so that +% the exclusive or table holds true for common data operations +% +% a b xor a b product +% -------------- ----------------- +% 0 0 1 -1 -1 1 +% 0 1 0 -1 1 -1 +% 1 0 0 1 -1 -1 +% 1 1 1 1 1 1 + +%--- Check if the data bits must be inverted ------------------------------ +if (ndat(2) ~= 1) + ndat(3:26)= -1 .* ndat(3:26); % Also could just negate +end + +%--- Calculate 6 parity bits ---------------------------------------------- +% The elements of the ndat array correspond to the bits showed in the table +% 20-XIV (ICD-200C document) in the following way: +% The first element in the ndat is the D29* bit and the second - D30*. +% The elements 3 - 26 are bits d1-d24 in the table. +% The elements 27 - 32 in the ndat array are the received bits D25-D30. +% The array "parity" contains the computed D25-D30 (parity) bits. + +parity(1) = ndat(1) * ndat(3) * ndat(4) * ndat(5) * ndat(7) * ... + ndat(8) * ndat(12) * ndat(13) * ndat(14) * ndat(15) * ... + ndat(16) * ndat(19) * ndat(20) * ndat(22) * ndat(25); + +parity(2) = ndat(2) * ndat(4) * ndat(5) * ndat(6) * ndat(8) * ... + ndat(9) * ndat(13) * ndat(14) * ndat(15) * ndat(16) * ... + ndat(17) * ndat(20) * ndat(21) * ndat(23) * ndat(26); + +parity(3) = ndat(1) * ndat(3) * ndat(5) * ndat(6) * ndat(7) * ... + ndat(9) * ndat(10) * ndat(14) * ndat(15) * ndat(16) * ... + ndat(17) * ndat(18) * ndat(21) * ndat(22) * ndat(24); + +parity(4) = ndat(2) * ndat(4) * ndat(6) * ndat(7) * ndat(8) * ... + ndat(10) * ndat(11) * ndat(15) * ndat(16) * ndat(17) * ... + ndat(18) * ndat(19) * ndat(22) * ndat(23) * ndat(25); + +parity(5) = ndat(2) * ndat(3) * ndat(5) * ndat(7) * ndat(8) * ... + ndat(9) * ndat(11) * ndat(12) * ndat(16) * ndat(17) * ... + ndat(18) * ndat(19) * ndat(20) * ndat(23) * ndat(24) * ... + ndat(26); + +parity(6) = ndat(1) * ndat(5) * ndat(7) * ndat(8) * ndat(10) * ... + ndat(11) * ndat(12) * ndat(13) * ndat(15) * ndat(17) * ... + ndat(21) * ndat(24) * ndat(25) * ndat(26); + +%--- Compare if the received parity is equal the calculated parity -------- +if ((sum(parity == ndat(27:32))) == 6) + + % Parity is OK. Function output is -1 or 1 depending if the data bits + % must be inverted or not. The "ndat(2)" is D30* bit - the last bit of + % previous subframe. + status = -1 * ndat(2); +else + % Parity failure + status = 0; +end diff --git a/GNSS_SDR_IQ/test/pseudorange.mat b/GNSS_SDR_IQ/test/pseudorange.mat new file mode 100644 index 0000000..cc23602 Binary files /dev/null and b/GNSS_SDR_IQ/test/pseudorange.mat differ diff --git a/GNSS_SDR_IQ/test/test_RefinePhase.asv b/GNSS_SDR_IQ/test/test_RefinePhase.asv new file mode 100644 index 0000000..9be4695 --- /dev/null +++ b/GNSS_SDR_IQ/test/test_RefinePhase.asv @@ -0,0 +1,13 @@ +load tmpEndAcquisitionphase.mat +clc +%Refine the frequency range to choose the most suitable frequency +caCode=generateCAcode(PRN); +codeValueIndex=floor((1:10*samplesPerCode)/settings.samplingFreq*settings.codeFreqBasis); +longCaCode=caCode((rem(codeValueIndex,1023)+1)); +xCarrier=signal0DC(codePhase:codePhase+10*samplesPerCode-1).*longCaCode; +plot(xCarrier); +numPts=8*(2^(nextpow2(length(xCarrier)))); + +fftRst=abs(fft(xCarrier,fftNumPts)); + +plot(fftRst) diff --git a/GNSS_SDR_IQ/test/test_RefinePhase.m b/GNSS_SDR_IQ/test/test_RefinePhase.m new file mode 100644 index 0000000..564746b --- /dev/null +++ b/GNSS_SDR_IQ/test/test_RefinePhase.m @@ -0,0 +1,17 @@ +load tmpEndAcquisitionphase.mat +clc +%Refine the frequency range to choose the most suitable frequency +PRN=3; +caCode=generateCAcode(PRN); +codeValueIndex=floor((1:10*samplesPerCode)/settings.samplingFreq*settings.codeFreqBasis); +longCaCode=caCode((rem(codeValueIndex,1023)+1)); +xCarrier=signal0DC(codePhase:codePhase+10*samplesPerCode-1).*longCaCode; +plot(xCarrier); +numPts=8*(2^(nextpow2(length(xCarrier)))); + +fftRst=abs(fft(xCarrier,numPts)); +[fftMax2, fftMaxIndex2] = max(fftxc(5 : numPts/2-5)); +rangeFreq=(0:numPts)*settings.samplingFreq/numPts; +frequency=rangeFreq(fftMaxIndex2) +plot(fftRst) + diff --git a/GNSS_SDR_IQ/test/test_TrackResults2NavBits.m b/GNSS_SDR_IQ/test/test_TrackResults2NavBits.m new file mode 100644 index 0000000..e3caf1e --- /dev/null +++ b/GNSS_SDR_IQ/test/test_TrackResults2NavBits.m @@ -0,0 +1,2 @@ +%load('..\trackingResults.mat'); +TrackResults2NavBits(trackResults,settings) \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/test_alm2eph.asv b/GNSS_SDR_IQ/test/test_alm2eph.asv new file mode 100644 index 0000000..b9ba482 --- /dev/null +++ b/GNSS_SDR_IQ/test/test_alm2eph.asv @@ -0,0 +1,78 @@ +format ('compact'); +format ('long', 'g'); + +%--- Include folders with functions --------------------------------------- +addpath include % The software receiver functions +addpath geoFunctions % Position calculation related functions +% +eph(1)=alm2eph(01,000,0.6089210510E-003,319488.0000,0.9603762360,-0.7703178011E-008,5153.611328,-0.4829672597E+000,0.448441831,0.1078930031E+001,0.2498626709E-003,0.3637978807E-011,660); +eph(2)=alm2eph(02,000,0.1115465164E-001,319488.0000,0.9389244734,-0.7908900866E-008,5153.631348,-0.4958443094E+000,-2.809621010,0.2218454550E+001,0.3871917725E-003,0.0000000000E+000,660); +eph(3)=alm2eph(03,000,0.1515722275E-001,319488.0000,0.9305415081,-0.8491782288E-008,5153.643066,-0.1659458993E+001,1.181482789,0.2379413177E+001,0.3719329834E-004,0.3637978807E-011,660); +eph(4)=alm2eph(04,000,0.9991168976E-002,319488.0000,0.9379956960,-0.7943188009E-008,5153.538574,-0.4788974919E+000,0.847107933,-0.9274007423E+000,0.1573562622E-003,0.1091393642E-010,660); +eph(5)=alm2eph(05,000,0.2665519714E-002,319488.0000,0.9516277518,-0.8114623721E-008,5153.590820,0.5668834241E+000,0.270158135,-0.2060905436E+001,-0.3061294556E-003,-0.3637978807E-011,660); +eph(6)=alm2eph(06,000,0.7264614105E-002,319488.0000,0.9374384295,-0.8388920861E-008,5153.626465,-0.1583104500E+001,-0.571736286,-0.1994561141E+001,0.3051757812E-004,0.0000000000E+000,660); +eph(7)=alm2eph(07,000,0.5239486694E-002,319488.0000,0.9759377520,-0.7600316584E-008,5153.573242,0.2677930857E+001,-3.025051684,0.7919763793E+000,0.7343292236E-004,0.3637978807E-011,660); +eph(8)=alm2eph(08,000,0.1217508316E-001,319488.0000,0.9981205523,-0.7360306586E-008,5153.654297,0.2760384571E+001,-2.973606028,0.1349558547E+000,0.9536743164E-006,0.0000000000E+000,660); +eph(9)=alm2eph(09,000,0.1739597321E-001,319488.0000,0.9846802441,-0.7531742299E-008,5153.587402,0.2651920220E+001,1.608567982,0.1053926444E+000,0.1525878906E-003,0.3637978807E-011,660); +eph(10)=alm2eph(10,000,0.1123762131E-001,319488.0000,0.9464985035,-0.8160339911E-008,5153.638184,0.5886651274E+000,0.750733793,-0.1733515886E+001,-0.3623962402E-004,0.0000000000E+000,660); +eph(11)=alm2eph(11,000,0.1282930374E-001,319488.0000,0.8881173519,-0.8491782288E-008,5153.611328,-0.7499888984E+000,1.039330032,0.9152022994E+000,-0.2527236938E-003,-0.3637978807E-011,660); +eph(12)=alm2eph(12,000,0.4001140594E-002,319488.0000,0.9794311536,-0.7771752296E-008,5153.480469,-0.2549682797E+001,0.000377129,0.6203858726E+000,0.6389617920E-004,0.3637978807E-011,660); +eph(13)=alm2eph(13,000,0.4957675934E-002,319488.0000,0.9858367218,-0.7943188009E-008,5153.634766,0.1706998915E+001,1.966194481,0.3108981705E+001,0.2126693726E-003,-0.3637978807E-011,660); +eph(14)=alm2eph(14,000,0.6207942963E-002,319488.0000,0.9787001159,-0.8034620388E-008,5153.526367,0.1680551604E+001,-2.062171644,-0.3064875263E+001,0.2002716064E-003,0.0000000000E+000,660); +eph(15)=alm2eph(15,000,0.4376411438E-002,319488.0000,0.9455757182,-0.8423208003E-008,5153.482910,0.1593964081E+001,0.051864355,0.2731660256E+001,-0.9155273438E-004,0.0000000000E+000,660); +eph(16)=alm2eph(16,000,0.6625175476E-002,319488.0000,0.9802640572,-0.7760323249E-008,5153.603516,-0.2531562275E+001,-0.045607466,-0.1728320350E+001,-0.2317428589E-003,0.0000000000E+000,660); +eph(17)=alm2eph(17,000,0.7328033447E-002,319488.0000,0.9623176804,-0.8068907531E-008,5153.607910,-0.1490675042E+001,-2.304936462,-0.2711252245E+001,0.1344680786E-003,-0.3637978807E-011,660); +eph(18)=alm2eph(18,000,0.1311016083E-001,319488.0000,0.9303317841,-0.8308917528E-008,5153.628906,0.5780654549E+000,-2.186412103,-0.1227346417E+001,0.1916885376E-003,0.0000000000E+000,660); +eph(19)=alm2eph(19,000,0.8131980896E-002,319488.0000,0.9597111115,-0.8126052768E-008,5153.715820,-0.1437500287E+001,0.147952747,0.2737129183E+001,-0.2565383911E-003,-0.3637978807E-011,660); +eph(20)=alm2eph(20,000,0.5052089691E-002,319488.0000,0.9307991689,-0.8286059433E-008,5153.665039,0.5243289395E+000,1.294038254,-0.8728399369E+000,0.6294250488E-004,0.0000000000E+000,660); +eph(21)=alm2eph(21,000,0.1900386810E-001,319488.0000,0.9316919937,-0.7966046104E-008,5153.575684,-0.4726548342E+000,-2.265382153,-0.1699243249E+000,-0.2183914185E-003,-0.3637978807E-011,660); +eph(22)=alm2eph(22,000,0.5940437317E-002,319488.0000,0.9280487893,-0.8331775623E-008,5153.624023,0.5815221548E+000,-2.014531354,-0.2056676877E+001,0.1430511475E-003,0.0000000000E+000,660); +eph(23)=alm2eph(23,000,0.7733345032E-002,319488.0000,0.9602863543,-0.8251772291E-008,5153.587891,0.1629128043E+001,-3.022881416,0.2337312969E+001,0.2136230469E-003,-0.3637978807E-011,660); +eph(25)=alm2eph(25,000,0.1533985138E-002,319488.0000,0.9682918165,-0.7851755629E-008,5153.677246,-0.2583294803E+001,0.621595530,-0.6206161945E+000,0.3337860107E-004,0.0000000000E+000,660); +eph(26)=alm2eph(26,000,0.2096986771E-001,319488.0000,0.9833859478,-0.7954617056E-008,5153.609863,0.1701079083E+001,1.187253567,0.2193132632E+001,-0.3204345703E-003,-0.5456968211E-010,660); +eph(27)=alm2eph(27,000,0.2174186707E-001,319488.0000,0.9821335963,-0.7531742299E-008,5153.707031,0.2625158322E+001,-1.095356284,0.3105322022E+001,0.3786087036E-003,0.3637978807E-011,660); +eph(28)=alm2eph(28,000,0.1785039902E-001,319488.0000,0.9786162263,-0.7737465154E-008,5153.660156,-0.2524778080E+001,-1.828244817,-0.2137146453E+001,0.1344680786E-003,0.3637978807E-011,660); +eph(29)=alm2eph(29,000,0.2323150635E-002,319488.0000,0.9628449863,-0.8091765626E-008,5153.538574,-0.1482118306E+001,-1.161227201,0.9249274979E-001,0.3042221069E-003,0.3637978807E-011,660); +eph(30)=alm2eph(30,000,0.1097297668E-001,319488.0000,0.9595912692,-0.7908900866E-008,5153.588867,-0.2673797423E+001,1.557179251,-0.2834284919E+001,-0.1373291016E-003,-0.1818989404E-010,660); +eph(31)=alm2eph(31,000,0.7861614227E-002,319488.0000,0.9810130713,-0.7566029441E-008,5153.592285,0.2680669252E+001,-0.868619243,0.7378893253E+000,0.2326965332E-003,0.3637978807E-011,660); +eph(32)=alm2eph(32,000,0.1181650162E-001,319488.0000,0.9522149788,-0.8160339911E-008,5153.648438,0.6558501902E+000,-0.590281500,0.1336976234E+001,-0.4339218140E-003,-0.3637978807E-011,660); + +[x y z]=llh2xyz(21,105,6.4e3); +userxyz=[x y z]; +transmitTime=471007.5; +settings=initSettings(); +speedOfLight=299792458; +lengthOfChips=speedOfLight/1.023e6; +validPRN=[]; +PRNlist=[9 5 26 12 4]; +for PRN=PRNlist; + [satPosition, satClkCorr] = satpos(transmitTime, ... + PRN, ... + eph, settings); + satPositions(:,PRN)=satPosition; + [az(PRN), el(PRN), tmpDist] = topocent(userxyz', satPosition - userxyz'); + dist(PRN)=tmpDist; + distByChips(PRN)=tmpDist/lengthOfChips; + if (el(PRN)>0)&& (el(PRN)<=90) + validPRN=[validPRN PRN]; + end; +end; +plot(dist(validPRN),'o'); +xx=dist-min(dist); +ps=[dist(1) dist(2) dist(3) dist(4) dist(5)]; + +startOffset=68.802; +ps3=[6004.95552297165 6005.69098240469 6002.47195747801 6004.55846774194 6005.05083088954]; +ps3=[]; +minimum = floor(min(ps3)); +ps3=ps3-minimum+startOffset; +ps3=(ps3+startOffset)*speedOfLight/1000; + +satpos_ref=[15258895.3388724,6757743.69186398,-3907619.74040496,18272464.1559595,-16672583.5535756; +8945334.1891453,21235153.5042233,25286052.4794101,19125668.5414435,15211201.3079099; +19270542.1020887,-14566012.6011023,7190086.32956048,198778.602063672,-13989910.5530641]; + +satposs=[satPositions(:,PRNlist)]; +satClkCorr=[0.000152592247958558 -0.000306111922962724 -0.000320253229144185 6.38808423644018e-005 0.000157333359758884]; + +[xyzdt,el,az,dop]=leastSquarePos(satpos_ref,ps3+satClkCorr*speedOfLight,settings); \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/test_alm2eph.m b/GNSS_SDR_IQ/test/test_alm2eph.m new file mode 100644 index 0000000..3b5b131 --- /dev/null +++ b/GNSS_SDR_IQ/test/test_alm2eph.m @@ -0,0 +1,90 @@ +format ('compact'); +format ('long', 'g'); + +%--- Include folders with functions --------------------------------------- +addpath include % The software receiver functions +addpath geoFunctions % Position calculation related functions +% +eph(1)=alm2eph(01,000,0.6089210510E-003,319488.0000,0.9603762360,-0.7703178011E-008,5153.611328,-0.4829672597E+000,0.448441831,0.1078930031E+001,0.2498626709E-003,0.3637978807E-011,660); +eph(2)=alm2eph(02,000,0.1115465164E-001,319488.0000,0.9389244734,-0.7908900866E-008,5153.631348,-0.4958443094E+000,-2.809621010,0.2218454550E+001,0.3871917725E-003,0.0000000000E+000,660); +eph(3)=alm2eph(03,000,0.1515722275E-001,319488.0000,0.9305415081,-0.8491782288E-008,5153.643066,-0.1659458993E+001,1.181482789,0.2379413177E+001,0.3719329834E-004,0.3637978807E-011,660); +eph(4)=alm2eph(04,000,0.9991168976E-002,319488.0000,0.9379956960,-0.7943188009E-008,5153.538574,-0.4788974919E+000,0.847107933,-0.9274007423E+000,0.1573562622E-003,0.1091393642E-010,660); +eph(5)=alm2eph(05,000,0.2665519714E-002,319488.0000,0.9516277518,-0.8114623721E-008,5153.590820,0.5668834241E+000,0.270158135,-0.2060905436E+001,-0.3061294556E-003,-0.3637978807E-011,660); +eph(6)=alm2eph(06,000,0.7264614105E-002,319488.0000,0.9374384295,-0.8388920861E-008,5153.626465,-0.1583104500E+001,-0.571736286,-0.1994561141E+001,0.3051757812E-004,0.0000000000E+000,660); +eph(7)=alm2eph(07,000,0.5239486694E-002,319488.0000,0.9759377520,-0.7600316584E-008,5153.573242,0.2677930857E+001,-3.025051684,0.7919763793E+000,0.7343292236E-004,0.3637978807E-011,660); +eph(8)=alm2eph(08,000,0.1217508316E-001,319488.0000,0.9981205523,-0.7360306586E-008,5153.654297,0.2760384571E+001,-2.973606028,0.1349558547E+000,0.9536743164E-006,0.0000000000E+000,660); +eph(9)=alm2eph(09,000,0.1739597321E-001,319488.0000,0.9846802441,-0.7531742299E-008,5153.587402,0.2651920220E+001,1.608567982,0.1053926444E+000,0.1525878906E-003,0.3637978807E-011,660); +eph(10)=alm2eph(10,000,0.1123762131E-001,319488.0000,0.9464985035,-0.8160339911E-008,5153.638184,0.5886651274E+000,0.750733793,-0.1733515886E+001,-0.3623962402E-004,0.0000000000E+000,660); +eph(11)=alm2eph(11,000,0.1282930374E-001,319488.0000,0.8881173519,-0.8491782288E-008,5153.611328,-0.7499888984E+000,1.039330032,0.9152022994E+000,-0.2527236938E-003,-0.3637978807E-011,660); +eph(12)=alm2eph(12,000,0.4001140594E-002,319488.0000,0.9794311536,-0.7771752296E-008,5153.480469,-0.2549682797E+001,0.000377129,0.6203858726E+000,0.6389617920E-004,0.3637978807E-011,660); +eph(13)=alm2eph(13,000,0.4957675934E-002,319488.0000,0.9858367218,-0.7943188009E-008,5153.634766,0.1706998915E+001,1.966194481,0.3108981705E+001,0.2126693726E-003,-0.3637978807E-011,660); +eph(14)=alm2eph(14,000,0.6207942963E-002,319488.0000,0.9787001159,-0.8034620388E-008,5153.526367,0.1680551604E+001,-2.062171644,-0.3064875263E+001,0.2002716064E-003,0.0000000000E+000,660); +eph(15)=alm2eph(15,000,0.4376411438E-002,319488.0000,0.9455757182,-0.8423208003E-008,5153.482910,0.1593964081E+001,0.051864355,0.2731660256E+001,-0.9155273438E-004,0.0000000000E+000,660); +eph(16)=alm2eph(16,000,0.6625175476E-002,319488.0000,0.9802640572,-0.7760323249E-008,5153.603516,-0.2531562275E+001,-0.045607466,-0.1728320350E+001,-0.2317428589E-003,0.0000000000E+000,660); +eph(17)=alm2eph(17,000,0.7328033447E-002,319488.0000,0.9623176804,-0.8068907531E-008,5153.607910,-0.1490675042E+001,-2.304936462,-0.2711252245E+001,0.1344680786E-003,-0.3637978807E-011,660); +eph(18)=alm2eph(18,000,0.1311016083E-001,319488.0000,0.9303317841,-0.8308917528E-008,5153.628906,0.5780654549E+000,-2.186412103,-0.1227346417E+001,0.1916885376E-003,0.0000000000E+000,660); +eph(19)=alm2eph(19,000,0.8131980896E-002,319488.0000,0.9597111115,-0.8126052768E-008,5153.715820,-0.1437500287E+001,0.147952747,0.2737129183E+001,-0.2565383911E-003,-0.3637978807E-011,660); +eph(20)=alm2eph(20,000,0.5052089691E-002,319488.0000,0.9307991689,-0.8286059433E-008,5153.665039,0.5243289395E+000,1.294038254,-0.8728399369E+000,0.6294250488E-004,0.0000000000E+000,660); +eph(21)=alm2eph(21,000,0.1900386810E-001,319488.0000,0.9316919937,-0.7966046104E-008,5153.575684,-0.4726548342E+000,-2.265382153,-0.1699243249E+000,-0.2183914185E-003,-0.3637978807E-011,660); +eph(22)=alm2eph(22,000,0.5940437317E-002,319488.0000,0.9280487893,-0.8331775623E-008,5153.624023,0.5815221548E+000,-2.014531354,-0.2056676877E+001,0.1430511475E-003,0.0000000000E+000,660); +eph(23)=alm2eph(23,000,0.7733345032E-002,319488.0000,0.9602863543,-0.8251772291E-008,5153.587891,0.1629128043E+001,-3.022881416,0.2337312969E+001,0.2136230469E-003,-0.3637978807E-011,660); +eph(25)=alm2eph(25,000,0.1533985138E-002,319488.0000,0.9682918165,-0.7851755629E-008,5153.677246,-0.2583294803E+001,0.621595530,-0.6206161945E+000,0.3337860107E-004,0.0000000000E+000,660); +eph(26)=alm2eph(26,000,0.2096986771E-001,319488.0000,0.9833859478,-0.7954617056E-008,5153.609863,0.1701079083E+001,1.187253567,0.2193132632E+001,-0.3204345703E-003,-0.5456968211E-010,660); +eph(27)=alm2eph(27,000,0.2174186707E-001,319488.0000,0.9821335963,-0.7531742299E-008,5153.707031,0.2625158322E+001,-1.095356284,0.3105322022E+001,0.3786087036E-003,0.3637978807E-011,660); +eph(28)=alm2eph(28,000,0.1785039902E-001,319488.0000,0.9786162263,-0.7737465154E-008,5153.660156,-0.2524778080E+001,-1.828244817,-0.2137146453E+001,0.1344680786E-003,0.3637978807E-011,660); +eph(29)=alm2eph(29,000,0.2323150635E-002,319488.0000,0.9628449863,-0.8091765626E-008,5153.538574,-0.1482118306E+001,-1.161227201,0.9249274979E-001,0.3042221069E-003,0.3637978807E-011,660); +eph(30)=alm2eph(30,000,0.1097297668E-001,319488.0000,0.9595912692,-0.7908900866E-008,5153.588867,-0.2673797423E+001,1.557179251,-0.2834284919E+001,-0.1373291016E-003,-0.1818989404E-010,660); +eph(31)=alm2eph(31,000,0.7861614227E-002,319488.0000,0.9810130713,-0.7566029441E-008,5153.592285,0.2680669252E+001,-0.868619243,0.7378893253E+000,0.2326965332E-003,0.3637978807E-011,660); +eph(32)=alm2eph(32,000,0.1181650162E-001,319488.0000,0.9522149788,-0.8160339911E-008,5153.648438,0.6558501902E+000,-0.590281500,0.1336976234E+001,-0.4339218140E-003,-0.3637978807E-011,660); + +[x y z]=llh2xyz(21,105,6.4e3); +userxyz=[x y z]; +transmitTime=471007.5; +settings=initSettings(); +speedOfLight=299792458; +lengthOfChips=speedOfLight/1.023e6; +validPRN=[]; +PRNlist=[9 5 26 12 4]; +for PRN=PRNlist; + [satPosition, satClkCorr] = satpos(transmitTime, ... + PRN, ... + eph, settings); + satPositions(:,PRN)=satPosition; + [az(PRN), el(PRN), tmpDist] = topocent(userxyz', satPosition - userxyz'); + dist(PRN)=tmpDist; + distByChips(PRN)=tmpDist/lengthOfChips; + if (el(PRN)>0)&& (el(PRN)<=90) + validPRN=[validPRN PRN]; + end; +end; +plot(dist(validPRN),'o'); +xx=dist-min(dist); +ps=[distByChips(PRNlist)]; +ps=ps/1023; +ps=ps-min(ps)+68.802; +ps=ps*speedOfLight/1000; + +startOffset=68.802; + +ps3=[6004.95552297165 6005.69098240469 6002.47195747801 6004.55846774194 6005.05083088954]; + +prngen=[4 5 9 12 26]; +tmpps=[3143.9476 3798.775 3046.4926 2640.26467 505.75956]/1023; +%Rearrange +ps3=[]; +for i=1:length(prngen) + ps3=[ps3 tmpps(find(prngen==PRNlist(i),1))]; +end; + +minimum = floor(min(ps3)); +ps3=ps3-minimum; +ps3=(ps3+startOffset)*speedOfLight/1000; + +satpos_ref=[15258895.3388724,6757743.69186398,-3907619.74040496,18272464.1559595,-16672583.5535756; +8945334.1891453,21235153.5042233,25286052.4794101,19125668.5414435,15211201.3079099; +19270542.1020887,-14566012.6011023,7190086.32956048,198778.602063672,-13989910.5530641]; + +satposs=[satPositions(:,PRNlist)]; +satClkCorr=[0.000152592247958558 -0.000306111922962724 -0.000320253229144185 6.38808423644018e-005 0.000157333359758884]; + +[xyzdt,el,az,dop]=leastSquarePos(satposs,ps+satClkCorr*speedOfLight,settings); \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/test_compare_pseudorange.m b/GNSS_SDR_IQ/test/test_compare_pseudorange.m new file mode 100644 index 0000000..f5924f4 --- /dev/null +++ b/GNSS_SDR_IQ/test/test_compare_pseudorange.m @@ -0,0 +1,65 @@ +%Test compare pseudorange +clear all +%% + +%--- Include folders with functions --------------------------------------- +addpath ..\include % The software receiver functions +addpath ..\geoFunctions % Position calculation related functions +addpath .. + +load trackingResults + +navSolutions = postNavigation(trackResults, settings); +% plot(navSolutions.latitude,navSolutions.longitude,'o'); +pseudorange=navSolutions.channel.rawP; +save pseudorange pseudorange +PRNsa=[]; +for idxprn=1:length(trackResults) + PRNsa=[PRNsa,trackResults(idxprn).PRN]; +end; +%% +load InitCodePhase.mat +InitCodePhase=InitCodePhase(6000:end,:); +startOffset=68.802; +speedOfLight=299792458; + +PRNs=sort(PRNsa); +%PRNs=[8 15 17 26 27 28]; +%PRNsa=[17 15 26 27 28]; + +%Re-arrange the actual pseudorange measurement +load pseudorange; +psa=[]; +pseudorange=pseudorange'; +for prn=1:length(PRNs) + psa=[psa,pseudorange(:,find(PRNsa==PRNs(prn)))]; +end; +step=500; +for idx=1:150 + tmpps=InitCodePhase((idx-1)*step+1,:)/1023; + minimum = floor(min(tmpps)); + ps(idx,:)=(tmpps-minimum+startOffset)*speedOfLight/1000; +end; +%ps=ps(:,2:end); +figure;hold on; +plot(ps(:,1),'r--','LineWidth',2); hold on; +plot(psa(:,1),'b--');hold on; +plot(ps(:,2),'r-o','LineWidth',2); hold on; +plot(psa(:,2),'b-o');hold on; +plot(ps(:,3),'r-.','LineWidth',2); hold on; +plot(psa(:,3),'b-.');hold on; +plot(ps(:,4),'r','LineWidth',2); hold on; +plot(psa(:,4),'b');hold on; +plot(ps(:,5),'r-*','LineWidth',2); hold on; +plot(psa(:,5),'b-*'); + +figure; +sizea=min(size(psa,1),size(ps,1)); +ps=ps(1:sizea,:); +psa=psa(1:sizea,:); +plot(ps(:,1)-psa(:,1),'r--','LineWidth',2); hold on; +plot(ps(:,2)-psa(:,2),'b-o','LineWidth',2); hold on; +plot(ps(:,3)-psa(:,3),'m-.','LineWidth',2); hold on; +plot(ps(:,4)-psa(:,4),'r','LineWidth',2); hold on; +plot(ps(:,5)-psa(:,5),'r-*','LineWidth',2); hold on; + diff --git a/GNSS_SDR_IQ/test/test_compare_pseudorangv2.m b/GNSS_SDR_IQ/test/test_compare_pseudorangv2.m new file mode 100644 index 0000000..62c4c90 --- /dev/null +++ b/GNSS_SDR_IQ/test/test_compare_pseudorangv2.m @@ -0,0 +1,70 @@ +%Test compare pseudorange +clear all +%% + +%--- Include folders with functions --------------------------------------- +addpath ..\include % The software receiver functions +addpath ..\geoFunctions % Position calculation related functions +addpath .. + +load trackingResults + +navSolutions = postNavigation(trackResults, settings); +% plot(navSolutions.latitude,navSolutions.longitude,'o'); +pseudorange=navSolutions.channel.rawP; +save pseudorange pseudorange +PRNsa=[]; +for idxprn=1:length(trackResults) + PRNsa=[PRNsa,trackResults(idxprn).PRN]; +end; +%% +fid=fopen('C:\Users\Administrator\Google Drive\PhD\works\EstimatingCN0\GNSS_SIM\GenSatInfor\pseudorange.bin','rb'); +NumMS=100000; +InitCodePhase=zeros(NumMS,6); +for ii=1:NumMS + InitCodePhase(ii,:)=fread(fid,6,'double'); +end; +InitCodePhase=InitCodePhase(6000:end,:); +startOffset=68.802; +speedOfLight=299792458; + +PRNs=sort(PRNsa); +%PRNs=[8 15 17 26 27 28]; +%PRNsa=[17 15 26 27 28]; + +%Re-arrange the actual pseudorange measurement +load pseudorange; +psa=[]; +pseudorange=pseudorange'; +for prn=1:length(PRNs) + psa=[psa,pseudorange(:,find(PRNsa==PRNs(prn)))]; +end; +step=500; +for idx=1:150 + tmpps=InitCodePhase((idx-1)*step+1,:)/speedOfLight*1000; + minimum = floor(min(tmpps)); + ps(idx,:)=(tmpps-minimum+startOffset)*speedOfLight/1000; +end; +%ps=ps(:,2:end); +figure;hold on; +plot(ps(:,1),'r--','LineWidth',2); hold on; +plot(psa(:,1),'b--');hold on; +plot(ps(:,2),'r-o','LineWidth',2); hold on; +plot(psa(:,2),'b-o');hold on; +plot(ps(:,3),'r-.','LineWidth',2); hold on; +plot(psa(:,3),'b-.');hold on; +plot(ps(:,4),'r','LineWidth',2); hold on; +plot(psa(:,4),'b');hold on; +plot(ps(:,5),'r-*','LineWidth',2); hold on; +plot(psa(:,5),'b-*'); + +figure; +sizea=min(size(psa,1),size(ps,1)); +ps=ps(1:sizea,:); +psa=psa(1:sizea,:); +plot(ps(:,1)-psa(:,1),'r--','LineWidth',2); hold on; +plot(ps(:,2)-psa(:,2),'b-o','LineWidth',2); hold on; +plot(ps(:,3)-psa(:,3),'m-.','LineWidth',2); hold on; +plot(ps(:,4)-psa(:,4),'r','LineWidth',2); hold on; +plot(ps(:,5)-psa(:,5),'r-*','LineWidth',2); hold on; + diff --git a/GNSS_SDR_IQ/test/test_createGPSsignal.asv b/GNSS_SDR_IQ/test/test_createGPSsignal.asv new file mode 100644 index 0000000..be6e2d9 --- /dev/null +++ b/GNSS_SDR_IQ/test/test_createGPSsignal.asv @@ -0,0 +1,21 @@ +%Create signal +A=1; +N0=3; + +caCode = generateCAcode(1); +Fs=16.368e6; +nn=[0:2047]+3; +tt=rem(floor(nn*1.023e6/Fs),1023)+1; +caCode2 = caCode(tt); +carrier = cos(2*pi*4.092e6/Fs*nn+0.05); + +signal=caCode2.*carrier; + + +%Do tin hieu +%Do theo carrier +signal2=signal.*carrier; +signal3=signal2.*caCode2; +plot(signal);hold on; +plot(signal2,'r'); +plot(signal3,'m','LineWidth',1); \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/test_createGPSsignal.m b/GNSS_SDR_IQ/test/test_createGPSsignal.m new file mode 100644 index 0000000..158f3b6 --- /dev/null +++ b/GNSS_SDR_IQ/test/test_createGPSsignal.m @@ -0,0 +1,25 @@ +%Create signal +A=1; +N0=3; + +caCode = generateCAcode(1); +Fs=16.368e6; +nn=[0:16367]+16; +tt=rem(floor(nn*1.023e6/Fs),1023)+1; +caCode2 = caCode(tt); +carrier = cos(2*pi*(4.092e6+5e3)/Fs*nn+0.05); +carrier_sai = cos(2*pi*(4.092e6)/Fs*nn+0.05); + +signal=caCode2.*carrier; + +nn_sai=[0:2047]+30; +tt_sai=rem(floor(nn_sai*1.023e6/Fs),1023)+1; +caCode_sai = caCode(tt_sai); + +%Do tin hieu +%Do theo carrier +signal2=signal.*carrier; +signal3=signal2.*caCode2; +plot(signal);hold on; +plot(signal2,'r'); +plot(signal3,'m','LineWidth',1); \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/test_find_pseudorange.asv b/GNSS_SDR_IQ/test/test_find_pseudorange.asv new file mode 100644 index 0000000..9f8d4bf --- /dev/null +++ b/GNSS_SDR_IQ/test/test_find_pseudorange.asv @@ -0,0 +1,11 @@ +% +satPositions =[654521.924219358,-16672583.5535756,-18808700.1235432,-5625385.49668232;... + -26410472.714435,15211201.3079099,-7290186.59129119,16770256.8394436;... + 3463374.83499896,-13989910.5530641,17255070.2946831,-19831374.5989722]; +[x y z]=llh2xyz(21,105,0) +userxyz=[x y z]; +speedOfLight=299792458; +lengthOfChips=speedOfLight/1.023e6; +for i=1:4 + dist(i)=norm(userxyz-satPositions(:,i)')/; +end; \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/test_find_pseudorange.m b/GNSS_SDR_IQ/test/test_find_pseudorange.m new file mode 100644 index 0000000..a657be2 --- /dev/null +++ b/GNSS_SDR_IQ/test/test_find_pseudorange.m @@ -0,0 +1,13 @@ +% +satPositions =[654521.924219358,-16672583.5535756,-18808700.1235432,-5625385.49668232;... + -26410472.714435,15211201.3079099,-7290186.59129119,16770256.8394436;... + 3463374.83499896,-13989910.5530641,17255070.2946831,-19831374.5989722]; +[x y z]=llh2xyz(21,105,0) +userxyz=[x y z]; +speedOfLight=299792458; +lengthOfChips=speedOfLight/1.023e6; +for i=1:4 + dist(i)=norm(userxyz-satPositions(:,i)')/lengthOfChips; +end; +[mindist minidx]=min(dist); +diffDist=dist-min(dist) \ No newline at end of file diff --git a/GNSS_SDR_IQ/test/test_find_pseudorangeV2.m b/GNSS_SDR_IQ/test/test_find_pseudorangeV2.m new file mode 100644 index 0000000..dce0711 --- /dev/null +++ b/GNSS_SDR_IQ/test/test_find_pseudorangeV2.m @@ -0,0 +1,96 @@ +clear all; +format ('compact'); +format ('long', 'g'); + +%--- Include folders with functions --------------------------------------- +addpath include % The software receiver functions +addpath geoFunctions % Position calculation related functions +% +eph(1)=alm2eph(01,000,0.6089210510E-003,319488.0000,0.9603762360,-0.7703178011E-008,5153.611328,-0.4829672597E+000,0.448441831,0.1078930031E+001,0.2498626709E-003,0.3637978807E-011,660); +eph(2)=alm2eph(02,000,0.1115465164E-001,319488.0000,0.9389244734,-0.7908900866E-008,5153.631348,-0.4958443094E+000,-2.809621010,0.2218454550E+001,0.3871917725E-003,0.0000000000E+000,660); +eph(3)=alm2eph(03,000,0.1515722275E-001,319488.0000,0.9305415081,-0.8491782288E-008,5153.643066,-0.1659458993E+001,1.181482789,0.2379413177E+001,0.3719329834E-004,0.3637978807E-011,660); +eph(4)=alm2eph(04,000,0.9991168976E-002,319488.0000,0.9379956960,-0.7943188009E-008,5153.538574,-0.4788974919E+000,0.847107933,-0.9274007423E+000,0.1573562622E-003,0.1091393642E-010,660); +eph(5)=alm2eph(05,000,0.2665519714E-002,319488.0000,0.9516277518,-0.8114623721E-008,5153.590820,0.5668834241E+000,0.270158135,-0.2060905436E+001,-0.3061294556E-003,-0.3637978807E-011,660); +eph(6)=alm2eph(06,000,0.7264614105E-002,319488.0000,0.9374384295,-0.8388920861E-008,5153.626465,-0.1583104500E+001,-0.571736286,-0.1994561141E+001,0.3051757812E-004,0.0000000000E+000,660); +eph(7)=alm2eph(07,000,0.5239486694E-002,319488.0000,0.9759377520,-0.7600316584E-008,5153.573242,0.2677930857E+001,-3.025051684,0.7919763793E+000,0.7343292236E-004,0.3637978807E-011,660); +eph(8)=alm2eph(08,000,0.1217508316E-001,319488.0000,0.9981205523,-0.7360306586E-008,5153.654297,0.2760384571E+001,-2.973606028,0.1349558547E+000,0.9536743164E-006,0.0000000000E+000,660); +eph(9)=alm2eph(09,000,0.1739597321E-001,319488.0000,0.9846802441,-0.7531742299E-008,5153.587402,0.2651920220E+001,1.608567982,0.1053926444E+000,0.1525878906E-003,0.3637978807E-011,660); +eph(10)=alm2eph(10,000,0.1123762131E-001,319488.0000,0.9464985035,-0.8160339911E-008,5153.638184,0.5886651274E+000,0.750733793,-0.1733515886E+001,-0.3623962402E-004,0.0000000000E+000,660); +eph(11)=alm2eph(11,000,0.1282930374E-001,319488.0000,0.8881173519,-0.8491782288E-008,5153.611328,-0.7499888984E+000,1.039330032,0.9152022994E+000,-0.2527236938E-003,-0.3637978807E-011,660); +eph(12)=alm2eph(12,000,0.4001140594E-002,319488.0000,0.9794311536,-0.7771752296E-008,5153.480469,-0.2549682797E+001,0.000377129,0.6203858726E+000,0.6389617920E-004,0.3637978807E-011,660); +eph(13)=alm2eph(13,000,0.4957675934E-002,319488.0000,0.9858367218,-0.7943188009E-008,5153.634766,0.1706998915E+001,1.966194481,0.3108981705E+001,0.2126693726E-003,-0.3637978807E-011,660); +eph(14)=alm2eph(14,000,0.6207942963E-002,319488.0000,0.9787001159,-0.8034620388E-008,5153.526367,0.1680551604E+001,-2.062171644,-0.3064875263E+001,0.2002716064E-003,0.0000000000E+000,660); +eph(15)=alm2eph(15,000,0.4376411438E-002,319488.0000,0.9455757182,-0.8423208003E-008,5153.482910,0.1593964081E+001,0.051864355,0.2731660256E+001,-0.9155273438E-004,0.0000000000E+000,660); +eph(16)=alm2eph(16,000,0.6625175476E-002,319488.0000,0.9802640572,-0.7760323249E-008,5153.603516,-0.2531562275E+001,-0.045607466,-0.1728320350E+001,-0.2317428589E-003,0.0000000000E+000,660); +eph(17)=alm2eph(17,000,0.7328033447E-002,319488.0000,0.9623176804,-0.8068907531E-008,5153.607910,-0.1490675042E+001,-2.304936462,-0.2711252245E+001,0.1344680786E-003,-0.3637978807E-011,660); +eph(18)=alm2eph(18,000,0.1311016083E-001,319488.0000,0.9303317841,-0.8308917528E-008,5153.628906,0.5780654549E+000,-2.186412103,-0.1227346417E+001,0.1916885376E-003,0.0000000000E+000,660); +eph(19)=alm2eph(19,000,0.8131980896E-002,319488.0000,0.9597111115,-0.8126052768E-008,5153.715820,-0.1437500287E+001,0.147952747,0.2737129183E+001,-0.2565383911E-003,-0.3637978807E-011,660); +eph(20)=alm2eph(20,000,0.5052089691E-002,319488.0000,0.9307991689,-0.8286059433E-008,5153.665039,0.5243289395E+000,1.294038254,-0.8728399369E+000,0.6294250488E-004,0.0000000000E+000,660); +eph(21)=alm2eph(21,000,0.1900386810E-001,319488.0000,0.9316919937,-0.7966046104E-008,5153.575684,-0.4726548342E+000,-2.265382153,-0.1699243249E+000,-0.2183914185E-003,-0.3637978807E-011,660); +eph(22)=alm2eph(22,000,0.5940437317E-002,319488.0000,0.9280487893,-0.8331775623E-008,5153.624023,0.5815221548E+000,-2.014531354,-0.2056676877E+001,0.1430511475E-003,0.0000000000E+000,660); +eph(23)=alm2eph(23,000,0.7733345032E-002,319488.0000,0.9602863543,-0.8251772291E-008,5153.587891,0.1629128043E+001,-3.022881416,0.2337312969E+001,0.2136230469E-003,-0.3637978807E-011,660); +eph(25)=alm2eph(25,000,0.1533985138E-002,319488.0000,0.9682918165,-0.7851755629E-008,5153.677246,-0.2583294803E+001,0.621595530,-0.6206161945E+000,0.3337860107E-004,0.0000000000E+000,660); +eph(26)=alm2eph(26,000,0.2096986771E-001,319488.0000,0.9833859478,-0.7954617056E-008,5153.609863,0.1701079083E+001,1.187253567,0.2193132632E+001,-0.3204345703E-003,-0.5456968211E-010,660); +eph(27)=alm2eph(27,000,0.2174186707E-001,319488.0000,0.9821335963,-0.7531742299E-008,5153.707031,0.2625158322E+001,-1.095356284,0.3105322022E+001,0.3786087036E-003,0.3637978807E-011,660); +eph(28)=alm2eph(28,000,0.1785039902E-001,319488.0000,0.9786162263,-0.7737465154E-008,5153.660156,-0.2524778080E+001,-1.828244817,-0.2137146453E+001,0.1344680786E-003,0.3637978807E-011,660); +eph(29)=alm2eph(29,000,0.2323150635E-002,319488.0000,0.9628449863,-0.8091765626E-008,5153.538574,-0.1482118306E+001,-1.161227201,0.9249274979E-001,0.3042221069E-003,0.3637978807E-011,660); +eph(30)=alm2eph(30,000,0.1097297668E-001,319488.0000,0.9595912692,-0.7908900866E-008,5153.588867,-0.2673797423E+001,1.557179251,-0.2834284919E+001,-0.1373291016E-003,-0.1818989404E-010,660); +eph(31)=alm2eph(31,000,0.7861614227E-002,319488.0000,0.9810130713,-0.7566029441E-008,5153.592285,0.2680669252E+001,-0.868619243,0.7378893253E+000,0.2326965332E-003,0.3637978807E-011,660); +eph(32)=alm2eph(32,000,0.1181650162E-001,319488.0000,0.9522149788,-0.8160339911E-008,5153.648438,0.6558501902E+000,-0.590281500,0.1336976234E+001,-0.4339218140E-003,-0.3637978807E-011,660); + +[x y z]=llh2xyz(45,7.7,6.4e3); +userxyz=[x y z]; +transmitTime=471006; +settings=initSettings(); +speedOfLight=299792458; +lengthOfChips=speedOfLight/1.023e6; +validPRN=[]; +PRNlist=[1:32]; +for PRN=PRNlist; + if PRN==24 + continue; + end; + [satPosition, satClkCorr] = satpos(transmitTime, ... + PRN, ... + eph, settings); + satPositions(:,PRN)=satPosition; + satClkCorrs(PRN)=satClkCorr; + + [az(PRN), el(PRN), tmpDist] = topocent(userxyz', satPosition - userxyz'); + dist(PRN)=tmpDist-satClkCorr*speedOfLight; + distByChips(PRN)=dist(PRN)/lengthOfChips; + if (el(PRN)>=30)&& (el(PRN)<=90) + validPRN=[validPRN PRN]; + end; +end; +validPRN=validPRN(1:end); +plot(dist(validPRN),'o'); + +startOffset=68.802; +% prna=[17 26 27 15 28 8]; +% tmppsa=[6071.46755865103 6068.62744379277 6078.94049364614 6074.95430107527 6078.44605327468 6077.53574046921]; +% tmpsatpossa=[-15036649.0002903 -3907619.74040496 8231874.41562561 8900529.04635426 -13239060.1540972 -23821990.3839858;... +% 17589741.4128689 25286052.4794101 12970359.0398929 17767729.2571505 7698143.23982526 8633624.86765844;... +% 13421050.6855703 7190086.32956048 22234841.921193 17711061.7438771 22235906.105048 6948364.48670885]; +% tmppsa2=[21665868.6265664 20814423.6122064 23906198.1772253 22711167.7083428 23757968.682932 23485063.7694301]; +% %re-arrange the pseudorange list +% psa=[]; +% psa2=[]; +% for i=1:length(prna) +% psa=[psa tmppsa(find(prna==validPRN(i),1))]; +% satpossa(:,i)=tmpsatpossa(:,find(prna==validPRN(i),1)); +% +% psa2=[psa2 tmppsa2(find(prna==validPRN(i),1))]; +% end; +% startOffset=68.802; +% minimum = floor(min(psa)); +% psa=psa-minimum; +% psa=(psa+startOffset)*speedOfLight/1000; + +satposs=satPositions(:,validPRN); +ps=distByChips(validPRN)/1023; +minimum = floor(min(ps)); +ps=ps-minimum; +ps=(ps+startOffset)*speedOfLight/1000; +[xyzdt,el,az,dop]=leastSquarePos(satposs,ps+satClkCorrs(validPRN)*speedOfLight,settings); + diff --git a/GNSS_SDR_IQ/test/test_find_pseudorangeV3.m b/GNSS_SDR_IQ/test/test_find_pseudorangeV3.m new file mode 100644 index 0000000..c7d7e13 --- /dev/null +++ b/GNSS_SDR_IQ/test/test_find_pseudorangeV3.m @@ -0,0 +1,79 @@ +clear all; +format ('compact'); +format ('long', 'g'); + +%--- Include folders with functions --------------------------------------- +addpath ..\include % The software receiver functions +addpath ..\geoFunctions % Position calculation related functions +addpath ..\ +% +eph(1)=alm2eph(01,000,0.6089210510E-003,319488.0000,0.9603762360,-0.7703178011E-008,5153.611328,-0.4829672597E+000,0.448441831,0.1078930031E+001,0.2498626709E-003,0.3637978807E-011,660); +eph(2)=alm2eph(02,000,0.1115465164E-001,319488.0000,0.9389244734,-0.7908900866E-008,5153.631348,-0.4958443094E+000,-2.809621010,0.2218454550E+001,0.3871917725E-003,0.0000000000E+000,660); +eph(3)=alm2eph(03,000,0.1515722275E-001,319488.0000,0.9305415081,-0.8491782288E-008,5153.643066,-0.1659458993E+001,1.181482789,0.2379413177E+001,0.3719329834E-004,0.3637978807E-011,660); +eph(4)=alm2eph(04,000,0.9991168976E-002,319488.0000,0.9379956960,-0.7943188009E-008,5153.538574,-0.4788974919E+000,0.847107933,-0.9274007423E+000,0.1573562622E-003,0.1091393642E-010,660); +eph(5)=alm2eph(05,000,0.2665519714E-002,319488.0000,0.9516277518,-0.8114623721E-008,5153.590820,0.5668834241E+000,0.270158135,-0.2060905436E+001,-0.3061294556E-003,-0.3637978807E-011,660); +eph(6)=alm2eph(06,000,0.7264614105E-002,319488.0000,0.9374384295,-0.8388920861E-008,5153.626465,-0.1583104500E+001,-0.571736286,-0.1994561141E+001,0.3051757812E-004,0.0000000000E+000,660); +eph(7)=alm2eph(07,000,0.5239486694E-002,319488.0000,0.9759377520,-0.7600316584E-008,5153.573242,0.2677930857E+001,-3.025051684,0.7919763793E+000,0.7343292236E-004,0.3637978807E-011,660); +eph(8)=alm2eph(08,000,0.1217508316E-001,319488.0000,0.9981205523,-0.7360306586E-008,5153.654297,0.2760384571E+001,-2.973606028,0.1349558547E+000,0.9536743164E-006,0.0000000000E+000,660); +eph(9)=alm2eph(09,000,0.1739597321E-001,319488.0000,0.9846802441,-0.7531742299E-008,5153.587402,0.2651920220E+001,1.608567982,0.1053926444E+000,0.1525878906E-003,0.3637978807E-011,660); +eph(10)=alm2eph(10,000,0.1123762131E-001,319488.0000,0.9464985035,-0.8160339911E-008,5153.638184,0.5886651274E+000,0.750733793,-0.1733515886E+001,-0.3623962402E-004,0.0000000000E+000,660); +eph(11)=alm2eph(11,000,0.1282930374E-001,319488.0000,0.8881173519,-0.8491782288E-008,5153.611328,-0.7499888984E+000,1.039330032,0.9152022994E+000,-0.2527236938E-003,-0.3637978807E-011,660); +eph(12)=alm2eph(12,000,0.4001140594E-002,319488.0000,0.9794311536,-0.7771752296E-008,5153.480469,-0.2549682797E+001,0.000377129,0.6203858726E+000,0.6389617920E-004,0.3637978807E-011,660); +eph(13)=alm2eph(13,000,0.4957675934E-002,319488.0000,0.9858367218,-0.7943188009E-008,5153.634766,0.1706998915E+001,1.966194481,0.3108981705E+001,0.2126693726E-003,-0.3637978807E-011,660); +eph(14)=alm2eph(14,000,0.6207942963E-002,319488.0000,0.9787001159,-0.8034620388E-008,5153.526367,0.1680551604E+001,-2.062171644,-0.3064875263E+001,0.2002716064E-003,0.0000000000E+000,660); +eph(15)=alm2eph(15,000,0.4376411438E-002,319488.0000,0.9455757182,-0.8423208003E-008,5153.482910,0.1593964081E+001,0.051864355,0.2731660256E+001,-0.9155273438E-004,0.0000000000E+000,660); +eph(16)=alm2eph(16,000,0.6625175476E-002,319488.0000,0.9802640572,-0.7760323249E-008,5153.603516,-0.2531562275E+001,-0.045607466,-0.1728320350E+001,-0.2317428589E-003,0.0000000000E+000,660); +eph(17)=alm2eph(17,000,0.7328033447E-002,319488.0000,0.9623176804,-0.8068907531E-008,5153.607910,-0.1490675042E+001,-2.304936462,-0.2711252245E+001,0.1344680786E-003,-0.3637978807E-011,660); +eph(18)=alm2eph(18,000,0.1311016083E-001,319488.0000,0.9303317841,-0.8308917528E-008,5153.628906,0.5780654549E+000,-2.186412103,-0.1227346417E+001,0.1916885376E-003,0.0000000000E+000,660); +eph(19)=alm2eph(19,000,0.8131980896E-002,319488.0000,0.9597111115,-0.8126052768E-008,5153.715820,-0.1437500287E+001,0.147952747,0.2737129183E+001,-0.2565383911E-003,-0.3637978807E-011,660); +eph(20)=alm2eph(20,000,0.5052089691E-002,319488.0000,0.9307991689,-0.8286059433E-008,5153.665039,0.5243289395E+000,1.294038254,-0.8728399369E+000,0.6294250488E-004,0.0000000000E+000,660); +eph(21)=alm2eph(21,000,0.1900386810E-001,319488.0000,0.9316919937,-0.7966046104E-008,5153.575684,-0.4726548342E+000,-2.265382153,-0.1699243249E+000,-0.2183914185E-003,-0.3637978807E-011,660); +eph(22)=alm2eph(22,000,0.5940437317E-002,319488.0000,0.9280487893,-0.8331775623E-008,5153.624023,0.5815221548E+000,-2.014531354,-0.2056676877E+001,0.1430511475E-003,0.0000000000E+000,660); +eph(23)=alm2eph(23,000,0.7733345032E-002,319488.0000,0.9602863543,-0.8251772291E-008,5153.587891,0.1629128043E+001,-3.022881416,0.2337312969E+001,0.2136230469E-003,-0.3637978807E-011,660); +eph(25)=alm2eph(25,000,0.1533985138E-002,319488.0000,0.9682918165,-0.7851755629E-008,5153.677246,-0.2583294803E+001,0.621595530,-0.6206161945E+000,0.3337860107E-004,0.0000000000E+000,660); +eph(26)=alm2eph(26,000,0.2096986771E-001,319488.0000,0.9833859478,-0.7954617056E-008,5153.609863,0.1701079083E+001,1.187253567,0.2193132632E+001,-0.3204345703E-003,-0.5456968211E-010,660); +eph(27)=alm2eph(27,000,0.2174186707E-001,319488.0000,0.9821335963,-0.7531742299E-008,5153.707031,0.2625158322E+001,-1.095356284,0.3105322022E+001,0.3786087036E-003,0.3637978807E-011,660); +eph(28)=alm2eph(28,000,0.1785039902E-001,319488.0000,0.9786162263,-0.7737465154E-008,5153.660156,-0.2524778080E+001,-1.828244817,-0.2137146453E+001,0.1344680786E-003,0.3637978807E-011,660); +eph(29)=alm2eph(29,000,0.2323150635E-002,319488.0000,0.9628449863,-0.8091765626E-008,5153.538574,-0.1482118306E+001,-1.161227201,0.9249274979E-001,0.3042221069E-003,0.3637978807E-011,660); +eph(30)=alm2eph(30,000,0.1097297668E-001,319488.0000,0.9595912692,-0.7908900866E-008,5153.588867,-0.2673797423E+001,1.557179251,-0.2834284919E+001,-0.1373291016E-003,-0.1818989404E-010,660); +eph(31)=alm2eph(31,000,0.7861614227E-002,319488.0000,0.9810130713,-0.7566029441E-008,5153.592285,0.2680669252E+001,-0.868619243,0.7378893253E+000,0.2326965332E-003,0.3637978807E-011,660); +eph(32)=alm2eph(32,000,0.1181650162E-001,319488.0000,0.9522149788,-0.8160339911E-008,5153.648438,0.6558501902E+000,-0.590281500,0.1336976234E+001,-0.4339218140E-003,-0.3637978807E-011,660); + +[x y z]=llh2xyz(40.7127,74.0059,6.4e3); +userxyz=[x y z]; +obsTime=[471000:0.001:471006+90]; +settings=initSettings(); +speedOfLight=299792458; +lengthOfChips=speedOfLight/1.023e6; +validPRN=[]; +PRNlist=[9 12 15 17 26 27]; +idxTTime=1; +hwb=waitbar(0,'Please wait'); +distByChips=zeros(length(obsTime),length(PRNlist)); +for transmitTime=obsTime + waitbar((transmitTime-obsTime(1))/(obsTime(end)-obsTime(1)),hwb); + validPRN=[]; + for PRN=PRNlist; + if PRN==24 + continue; + end; + [satPosition, satClkCorr] = satpos(transmitTime, ... + PRN, ... + eph, settings); + + [az, el, tmpDist] = topocent(userxyz', satPosition - userxyz'); + dist=tmpDist-satClkCorr*speedOfLight; + distByChips(idxTTime,PRN)=dist/lengthOfChips; + if (el>=30)&& (el<=90) + validPRN=[validPRN PRN]; + end; + end; + idxTTime=idxTTime+1; + validPRN=validPRN(1:end); + %plot(dist(validPRN),'o'); +end; +startOffset=68.802; +InitCodePhase=distByChips(:,validPRN); +save InitCodePhase.mat InitCodePhase +close(hwb); +msgbox('Finish'); diff --git a/GNSS_SDR_IQ/test/test_genPseudorange.m b/GNSS_SDR_IQ/test/test_genPseudorange.m new file mode 100644 index 0000000..d09946a --- /dev/null +++ b/GNSS_SDR_IQ/test/test_genPseudorange.m @@ -0,0 +1,81 @@ +%function InitCodePhase=GenerateSatellitesPseudoranges(InitObsTime,DurationTime,LLH,PRNlist,ElevationMask) +clear all +addpath ..\include % The software receiver functions +addpath ..\geoFunctions % Position calculation related functions +addpath .. + +load trackingResults_HN.mat +%navSolutions = postNavigation(trackResults, settings); +%pseudorange=navSolutions.channel.rawP; + +%Gen ephemeris message for every satellite +[subFrameStart, activeChnList] = findPreambles(trackResults, settings); + +LLH=[21.004498873852768 105.8435979060256 50.956094086170197]; +PRNlist=[3 6 13 16 19 23]; +ElevationMask=10; +for channelNr = activeChnList + + %=== Convert tracking output to navigation bits ======================= + + %--- Copy 5 sub-frames long record from tracking output --------------- + navBitsSamples = trackResults(channelNr).I_P(subFrameStart(channelNr) - 20 : ... + subFrameStart(channelNr) + (1500 * 20) -1)'; + + %--- Group every 20 vales of bits into columns ------------------------ + navBitsSamples = reshape(navBitsSamples, ... + 20, (size(navBitsSamples, 1) / 20)); + + %--- Sum all samples in the bits to get the best estimate ------------- + navBits = sum(navBitsSamples); + + %--- Now threshold and make 1 and 0 ----------------------------------- + % The expression (navBits > 0) returns an array with elements set to 1 + % if the condition is met and set to 0 if it is not met. + navBits = (navBits > 0); + + %--- Convert from decimal to binary ----------------------------------- + % The function ephemeris expects input in binary form. In Matlab it is + % a string array containing only "0" and "1" characters. + navBitsBin = dec2bin(navBits); + + %=== Decode ephemerides and TOW of the first sub-frame ================ + [eph(trackResults(channelNr).PRN), TOW] = ... + ephemeris(navBitsBin(2:1501)', navBitsBin(1)); +end +[tmpx tmpy tmpz]=llh2xyz(LLH(1),LLH(2),LLH(3)); +userxyz=[tmpx tmpy tmpz]; +InitObsTime=TOW; +DurationTime=360; +obsTime=[InitObsTime:0.001:InitObsTime+DurationTime+1]; +speedOfLight=299792458; +lengthOfChips=speedOfLight/1.023e6; +idxTTime=1; +hwb=waitbar(0,'Please wait'); +distByChips=zeros(length(obsTime),length(PRNlist)); + +for transmitTime=obsTime + if rem(idxTTime,10)==0 + waitbar((transmitTime-obsTime(1))/(obsTime(end)-obsTime(1)),hwb); + end; + validPRN=[]; + for PRN=PRNlist; + if PRN==24 + continue; + end; + [satPosition, satClkCorr] = satpos(transmitTime, ... + PRN, ... + eph); + + [az, el, tmpDist] = topocent(userxyz', satPosition - userxyz'); + dist=tmpDist-satClkCorr*speedOfLight; + distByChips(idxTTime,PRN)=dist/lengthOfChips; + if (el>=ElevationMask)&& (el<=90) + validPRN=[validPRN PRN]; + end; + end; + idxTTime=idxTTime+1; +end; +InitCodePhase=distByChips(:,validPRN); +close(hwb); +save InitCodePhase InitCodePhase diff --git a/GNSS_SDR_IQ/test/test_nav_file.m b/GNSS_SDR_IQ/test/test_nav_file.m new file mode 100644 index 0000000..c821680 --- /dev/null +++ b/GNSS_SDR_IQ/test/test_nav_file.m @@ -0,0 +1,56 @@ +%Gen ephemeris message for every satellite + +%function InitCodePhase=GenerateSatellitesPseudoranges(InitObsTime,DurationTime,LLH,PRNlist,ElevationMask) +clear workspace +addpath ..\include % The software receiver functions +addpath ..\geoFunctions % Position calculation related functions +addpath .. + +load trackingResults_HN.mat + + +%fidASCII=fopen('..\..\..\works\EstimatingCN0\GNSS_SIM\NAV3\nav_test','rt'); +fidASCII=fopen('..\..\..\works\EstimatingCN0\GNSS_SIM\Ephemeris\nav02','rt'); +%fidASCII=fopen('..\..\..\works\EstimatingCN0\GNSS_SIM\NAV3\navascii2','rt'); +data=fread(fidASCII,'char')-48; +data(data==0)=-1; +data=data(532:end)'; +clear trackResults; +trackResults(1).I_P=kron(data,ones(1,20)); +%trackResults(1).I_P=trackResults(1).I_P(15000*20:end); +trackResults(2:end)=[]; +trackResults(1).status='T'; +[subFrameStart, activeChnList] = findPreambles2(trackResults, settings); + +LLH=[21.004498873852768 105.8435979060256 50.956094086170197]; +PRNlist=[3 6 13 16 19 23]; +ElevationMask=10; +for channelNr = activeChnList + + %=== Convert tracking output to navigation bits ======================= + + %--- Copy 5 sub-frames long record from tracking output --------------- + navBitsSamples = trackResults(channelNr).I_P(subFrameStart(channelNr) - 20 : ... + subFrameStart(channelNr) + (1500 * 20) -1)'; + + %--- Group every 20 vales of bits into columns ------------------------ + navBitsSamples = reshape(navBitsSamples, ... + 20, (size(navBitsSamples, 1) / 20)); + + %--- Sum all samples in the bits to get the best estimate ------------- + navBits = sum(navBitsSamples); + + %--- Now threshold and make 1 and 0 ----------------------------------- + % The expression (navBits > 0) returns an array with elements set to 1 + % if the condition is met and set to 0 if it is not met. + navBits = (navBits > 0); + + %--- Convert from decimal to binary ----------------------------------- + % The function ephemeris expects input in binary form. In Matlab it is + % a string array containing only "0" and "1" characters. + navBitsBin = dec2bin(navBits); + + %=== Decode ephemerides and TOW of the first sub-frame ================ + [eph(trackResults(channelNr).PRN), TOW] = ... + ephemeris(navBitsBin(2:1501)', navBitsBin(1)); +end \ No newline at end of file diff --git a/GNSS_SDR_IQ/testCorrelation.m b/GNSS_SDR_IQ/testCorrelation.m new file mode 100644 index 0000000..a5317c4 --- /dev/null +++ b/GNSS_SDR_IQ/testCorrelation.m @@ -0,0 +1,83 @@ +%Check autocorrelation + +clear; close all; clc; + +format ('compact'); +format ('long', 'g'); + +%--- Include folders with functions --------------------------------------- +addpath include % The software receiver functions +addpath geoFunctions % Position calculation related functions + + +CodePhase =-1:0.001:1; %[chips] +caCode = generateCAcode(1); +%caCode = [caCode caCode caCode]; + +codeFreq = 1.023e6; +earlyLateSpc = 0.5; +Td = 0.001; + +idxSamplingFreq = 1; +samplingFreqRange = [4]*1.023e6; +varDLLError = size(1,length(samplingFreqRange)); +maxDLLError = varDLLError; +minDLLError =varDLLError; +for samplingFreq = samplingFreqRange; + + codePhaseStep = codeFreq / samplingFreq; + blksize = Td*samplingFreq; + tcode = 0:codePhaseStep : ((blksize-1)*codePhaseStep); + %tcode2 =ceil(tcode)+1023; + tcode2 = floor(rem(tcode+1023,1023)) + 1; + incomingSignal = caCode(tcode2); + + I_E=size(1,length(CodePhase)); + I_P=I_E; + I_L=I_E; + idxCodePhase=1; + for remCodePhase=CodePhase + tcode = (remCodePhase-earlyLateSpc) : ... + codePhaseStep : ... + ((blksize-1)*codePhaseStep+remCodePhase-earlyLateSpc); + tcode2 = floor(rem(tcode+1023,1023)) + 1; + %tcode2 =ceil(tcode)+1023; + earlyCode = caCode(tcode2); + + % Define index into late code vector + tcode = (remCodePhase+earlyLateSpc) : ... + codePhaseStep : ... + ((blksize-1)*codePhaseStep+remCodePhase+earlyLateSpc); + tcode2 = floor(rem(tcode+1023,1023)) + 1; + %tcode2 =ceil(tcode)+1023; + lateCode = caCode(tcode2); + + % Define index into prompt code vector + tcode = remCodePhase : ... + codePhaseStep : ... + ((blksize-1)*codePhaseStep+remCodePhase); + tcode2 = floor(rem(tcode+1023,1023)) + 1; + %tcode2 =ceil(tcode)+1023; + promptCode = caCode(tcode2); + + I_P(idxCodePhase)=sum(promptCode.*incomingSignal)/blksize; + I_L(idxCodePhase)=sum(lateCode.*incomingSignal)/blksize; + I_E(idxCodePhase)=sum(earlyCode.*incomingSignal)/blksize; + + idxCodePhase =idxCodePhase+1; + + %Code Error + end; + figure(300); + plot(CodePhase,I_P,'r','LineWidth',2);grid on;hold on; + disp(sprintf('Processing Fs =%d',samplingFreq)); + codeError=(I_E-I_L)./(2*(I_E+I_L))- CodePhase; + codeError2=codeError(find(CodePhase>=-0.5 & CodePhase<=0.5)); + minDLLError(idxSamplingFreq) = min(abs(codeError2)); + maxDLLError(idxSamplingFreq) = max(codeError2); + varDLLError(idxSamplingFreq) = var(abs(codeError2)); + idxSamplingFreq = idxSamplingFreq+1; +end; + +figure(100); +plot(samplingFreqRange/1.023e6,sqrt(varDLLError)*299792458/codeFreq,'-o');grid on; diff --git a/GNSS_SDR_IQ/tracking.m b/GNSS_SDR_IQ/tracking.m new file mode 100644 index 0000000..5dfb9c1 --- /dev/null +++ b/GNSS_SDR_IQ/tracking.m @@ -0,0 +1,300 @@ +function [trackResults, channel]= tracking(fid, channel, settings) +% Performs code and carrier tracking for all channels. +% +%[trackResults, channel] = tracking(fid, channel, settings) +% +% Inputs: +% fid - file identifier of the signal record. +% channel - PRN, carrier frequencies and code phases of all +% satellites to be tracked (prepared by preRum.m from +% acquisition results). +% settings - receiver settings. +% Outputs: +% trackResults - tracking results (structure array). Contains +% in-phase prompt outputs and absolute starting +% positions of spreading codes, together with other +% observation data from the tracking loops. All are +% saved every millisecond. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Dennis M. Akos +% Written by Darius Plausinaitis and Dennis M. Akos +% Based on code by DMAkos Oct-1999 +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: tracking.m,v 1.14.2.32 2007/01/30 09:45:12 dpl Exp $ + +%% Initialize result structure ============================================ + +% Channel status +trackResults.status = '-'; % No tracked signal, or lost lock + +% The absolute sample in the record of the C/A code start: +trackResults.absoluteSample = zeros(1, settings.msToProcess); + +% Freq of the C/A code: +trackResults.codeFreq = inf(1, settings.msToProcess); + +% Frequency of the tracked carrier wave: +trackResults.carrFreq = inf(1, settings.msToProcess); + +% Outputs from the correlators (In-phase): +trackResults.I_P = zeros(1, settings.msToProcess); +trackResults.I_E = zeros(1, settings.msToProcess); +trackResults.I_L = zeros(1, settings.msToProcess); + +% Outputs from the correlators (Quadrature-phase): +trackResults.Q_E = zeros(1, settings.msToProcess); +trackResults.Q_P = zeros(1, settings.msToProcess); +trackResults.Q_L = zeros(1, settings.msToProcess); + +% Loop discriminators +trackResults.dllDiscr = inf(1, settings.msToProcess); +trackResults.dllDiscrFilt = inf(1, settings.msToProcess); +trackResults.pllDiscr = inf(1, settings.msToProcess); +trackResults.pllDiscrFilt = inf(1, settings.msToProcess); + +%--- Copy initial settings for all channels ------------------------------- +trackResults = repmat(trackResults, 1, settings.numberOfChannels); + +%% Initialize tracking variables ========================================== + +codePeriods = settings.msToProcess; % For GPS one C/A code is one ms + +%--- DLL variables -------------------------------------------------------- +% Define early-late offset (in chips) +earlyLateSpc = settings.dllCorrelatorSpacing; + +% Summation interval +PDIcode = 0.001; + +% Calculate filter coefficient values +[tau1code, tau2code] = calcLoopCoef(settings.dllNoiseBandwidth, ... + settings.dllDampingRatio, ... + 1.0); + +%--- PLL variables -------------------------------------------------------- +% Summation interval +PDIcarr = 0.001; + +% Calculate filter coefficient values +[tau1carr, tau2carr] = calcLoopCoef(settings.pllNoiseBandwidth, ... + settings.pllDampingRatio, ... + 0.25); +hwb = waitbar(0,'Tracking...'); + +%% Start processing channels ============================================== +for channelNr = 1:settings.numberOfChannels + + % Only process if PRN is non zero (acquisition was successful) + if (channel(channelNr).PRN ~= 0) + % Save additional information - each channel's tracked PRN + trackResults(channelNr).PRN = channel(channelNr).PRN; + + % Move the starting point of processing. Can be used to start the + % signal processing at any point in the data record (e.g. for long + % records). In addition skip through that data file to start at the + % appropriate sample (corresponding to code phase). Assumes sample + % type is schar (or 1 byte per sample) + fseek(fid, ... + settings.skipNumberOfBytes + (channel(channelNr).codePhase-1)*settings.dataTypeSize, ... + 'bof'); + + + % Get a vector with the C/A code sampled 1x/chip + caCode = generateCAcode(channel(channelNr).PRN); + % Then make it possible to do early and late versions + caCode = [caCode(1023) caCode caCode(1)]; + + %--- Perform various initializations ------------------------------ + + % define initial code frequency basis of NCO + codeFreq = settings.codeFreqBasis; + % define residual code phase (in chips) + remCodePhase = 0.0; + % define carrier frequency which is used over whole tracking period + carrFreq = channel(channelNr).acquiredFreq; + carrFreqBasis = channel(channelNr).acquiredFreq; + % define residual carrier phase + remCarrPhase = 0.0; + + %code tracking loop parameters + oldCodeNco = 0.0; + oldCodeError = 0.0; + + %carrier/Costas loop parameters + oldCarrNco = 0.0; + oldCarrError = 0.0; + + %=== Process the number of specified code periods ================= + for loopCnt = 1:codePeriods + +%% GUI update ------------------------------------------------------------- + % The GUI is updated every 50ms. This way Matlab GUI is still + % responsive enough. At the same time Matlab is not occupied + % all the time with GUI task. + if (rem(loopCnt, 50) == 0) + try + waitbar(loopCnt/codePeriods, ... + hwb, ... + ['Tracking: Ch ', int2str(channelNr), ... + ' of ', int2str(settings.numberOfChannels), ... + '; PRN#', int2str(channel(channelNr).PRN), ... + '; Completed ',int2str(loopCnt), ... + ' of ', int2str(codePeriods), ' msec']); + catch + % The progress bar was closed. It is used as a signal + % to stop, "cancel" processing. Exit. + disp('Progress bar closed, exiting...'); + return + end + end + +%% Read next block of data ------------------------------------------------ + % Find the size of a "block" or code period in whole samples + + % Update the phasestep based on code freq (variable) and + % sampling frequency (fixed) + codePhaseStep = codeFreq / settings.samplingFreq; + + blksize = ceil((settings.codeLength-remCodePhase) / codePhaseStep); + + % Read in the appropriate number of samples to process this + % interation + [rawSignal, samplesRead] = fread(fid, ... + blksize, settings.dataType); + rawSignal = rawSignal'; %transpose vector + + % If did not read in enough samples, then could be out of + % data - better exit + if (samplesRead ~= blksize) + disp('Not able to read the specified number of samples for tracking, exiting!') + fclose(fid); + return + end + +%% Set up all the code phase tracking information ------------------------- + % Define index into early code vector + tcode = (remCodePhase-earlyLateSpc) : ... + codePhaseStep : ... + ((blksize-1)*codePhaseStep+remCodePhase-earlyLateSpc); + tcode2 = ceil(tcode) + 1; + earlyCode = caCode(tcode2); + + % Define index into late code vector + tcode = (remCodePhase+earlyLateSpc) : ... + codePhaseStep : ... + ((blksize-1)*codePhaseStep+remCodePhase+earlyLateSpc); + tcode2 = ceil(tcode) + 1; + lateCode = caCode(tcode2); + + % Define index into prompt code vector + tcode = remCodePhase : ... + codePhaseStep : ... + ((blksize-1)*codePhaseStep+remCodePhase); + tcode2 = ceil(tcode) + 1; + promptCode = caCode(tcode2); + + remCodePhase = (tcode(blksize) + codePhaseStep) - 1023.0; + +%% Generate the carrier frequency to mix the signal to baseband ----------- + time = (0:blksize) ./ settings.samplingFreq; + + % Get the argument to sin/cos functions + trigarg = ((carrFreq * 2.0 * pi) .* time) + remCarrPhase; + remCarrPhase = rem(trigarg(blksize+1), (2 * pi)); + + % Finally compute the signal to mix the collected data to bandband + carrCos = cos(trigarg(1:blksize)); + carrSin = sin(trigarg(1:blksize)); + +%% Generate the six standard accumulated values --------------------------- + % First mix to baseband + qBasebandSignal = carrCos .* rawSignal; + iBasebandSignal = carrSin .* rawSignal; + + % Now get early, late, and prompt values for each + I_E = sum(earlyCode .* iBasebandSignal); + Q_E = sum(earlyCode .* qBasebandSignal); + I_P = sum(promptCode .* iBasebandSignal); + Q_P = sum(promptCode .* qBasebandSignal); + I_L = sum(lateCode .* iBasebandSignal); + Q_L = sum(lateCode .* qBasebandSignal); + +%% Find PLL error and update carrier NCO ---------------------------------- + + % Implement carrier loop discriminator (phase detector) + carrError = atan(Q_P / I_P) / (2.0 * pi); + + % Implement carrier loop filter and generate NCO command + carrNco = oldCarrNco + (tau2carr/tau1carr) * ... + (carrError - oldCarrError) + carrError * (PDIcarr/tau1carr); + oldCarrNco = carrNco; + oldCarrError = carrError; + + % Modify carrier freq based on NCO command + carrFreq = carrFreqBasis + carrNco; + + trackResults(channelNr).carrFreq(loopCnt) = carrFreq; + +%% Find DLL error and update code NCO ------------------------------------- + codeError = (sqrt(I_E * I_E + Q_E * Q_E) - sqrt(I_L * I_L + Q_L * Q_L)) / ... + (sqrt(I_E * I_E + Q_E * Q_E) + sqrt(I_L * I_L + Q_L * Q_L)); + + % Implement code loop filter and generate NCO command + codeNco = oldCodeNco + (tau2code/tau1code) * ... + (codeError - oldCodeError) + codeError * (PDIcode/tau1code); + oldCodeNco = codeNco; + oldCodeError = codeError; + + % Modify code freq based on NCO command + codeFreq = settings.codeFreqBasis - codeNco; + + trackResults(channelNr).codeFreq(loopCnt) = codeFreq; + +%% Record various measures to show in postprocessing ---------------------- + % Record sample number (based on 8bit samples) + trackResults(channelNr).absoluteSample(loopCnt) = (1/settings.dataTypeSize)*ftell(fid); + trackResults(channelNr).remCodePhase(loopCnt) = remCodePhase; + + trackResults(channelNr).dllDiscr(loopCnt) = codeError; + trackResults(channelNr).dllDiscrFilt(loopCnt) = codeNco; + trackResults(channelNr).pllDiscr(loopCnt) = carrError; + trackResults(channelNr).pllDiscrFilt(loopCnt) = carrNco; + + trackResults(channelNr).I_E(loopCnt) = I_E; + trackResults(channelNr).I_P(loopCnt) = I_P; + trackResults(channelNr).I_L(loopCnt) = I_L; + trackResults(channelNr).Q_E(loopCnt) = Q_E; + trackResults(channelNr).Q_P(loopCnt) = Q_P; + trackResults(channelNr).Q_L(loopCnt) = Q_L; + end % for loopCnt + + % If we got so far, this means that the tracking was successful + % Now we only copy status, but it can be update by a lock detector + % if implemented + trackResults(channelNr).status = channel(channelNr).status; + + end % if a PRN is assigned +end % for channelNr + +% Close the waitbar +close(hwb) diff --git a/GNSS_SDR_IQ/trackingResults.mat b/GNSS_SDR_IQ/trackingResults.mat new file mode 100644 index 0000000..72a60a8 Binary files /dev/null and b/GNSS_SDR_IQ/trackingResults.mat differ diff --git a/GNSS_SDR_IQ/tracking_V0_IQ.asv b/GNSS_SDR_IQ/tracking_V0_IQ.asv new file mode 100644 index 0000000..5e4cedb --- /dev/null +++ b/GNSS_SDR_IQ/tracking_V0_IQ.asv @@ -0,0 +1,368 @@ +function [trackResults, channel]= tracking_V0_IQ(fid, channel, settings) +% Performs code and carrier tracking for all channels. +% +%[trackResults, channel] = tracking(fid, channel, settings) +% +% Inputs: +% fid - file identifier of the signal record. +% channel - PRN, carrier frequencies and code phases of all +% satellites to be tracked (prepared by preRum.m from +% acquisition results). +% settings - receiver settings. +% Outputs: +% trackResults - tracking results (structure array). Contains +% in-phase prompt outputs and absolute starting +% positions of spreading codes, together with other +% observation data from the tracking loops. All are +% saved every millisecond. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Dennis M. Akos +% Written by Darius Plausinaitis and Dennis M. Akos +% Based on code by DMAkos Oct-1999 +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: tracking.m,v 1.14.2.32 2007/01/30 09:45:12 dpl Exp $ + +%% Initialize result structure ============================================ + +% Channel status: trạng thái kênh +trackResults.status = '-'; % No tracked signal, or lost lock + +% The absolute sample in the record of the C/A code start: khởi tạo mẫu tuyệt đối +% trong bản ghi C/A +trackResults.absoluteSample = zeros(1, settings.msToProcess); + +% Freq of the C/A code: tần số mã C/A +trackResults.codeFreq = inf(1, settings.msToProcess); + +% Frequency of the tracked carrier wave: tần số của sóng mang được theo dõi +trackResults.carrFreq = inf(1, settings.msToProcess); + +% Outputs from the correlators (In-phase): đầu ra của các bộ tương quan (I) +trackResults.I_P = zeros(1, settings.msToProcess); +trackResults.I_E = zeros(1, settings.msToProcess); +trackResults.I_L = zeros(1, settings.msToProcess); + +% Outputs from the correlators (Quadrature-phase): đầu ra của các bộ tương +% quan (Q) +trackResults.Q_E = zeros(1, settings.msToProcess); +trackResults.Q_P = zeros(1, settings.msToProcess); +trackResults.Q_L = zeros(1, settings.msToProcess); + +% Loop discriminators +trackResults.dllDiscr = inf(1, settings.msToProcess); +trackResults.dllDiscrFilt = inf(1, settings.msToProcess); +trackResults.pllDiscr = inf(1, settings.msToProcess); +trackResults.pllDiscrFilt = inf(1, settings.msToProcess); + +%--- Copy initial settings for all channels ------------------------------- +trackResults = repmat(trackResults, 1, settings.numberOfChannels); + +%% Initialize tracking variables ========================================== + +codePeriods = settings.msToProcess; % For GPS one C/A code is one ms (với GPS thì 1 mã C/A là 1 ms) + +%--- DLL variables -------------------------------------------------------- +% Define early-late offset (in chips): Xác định độ lệch sớm-muộn (tính +% bằng chip) +earlyLateSpc = settings.dllCorrelatorSpacing; + +% Summation interval: khoảng tính tổng +PDIcode = 0.001; + +% Calculate filter coefficient values: Tính toán giá trị hệ số bộ lọc +[tau1code, tau2code] = calcLoopCoef(settings.dllNoiseBandwidth, ... + settings.dllDampingRatio, ... + 1.0); + +%--- PLL variables -------------------------------------------------------- +% Summation interval +PDIcarr = 0.001; + +% Calculate filter coefficient values +[tau1carr, tau2carr] = calcLoopCoef(settings.pllNoiseBandwidth, ... + settings.pllDampingRatio, ... + 0.25); + % CARRIER TRACKING LOOP +Bl_carr =5; % Bandwith in hertz: băng thông tính bằng Hz +wn_carr = Bl_carr/0.7845; % Natural Frequency: tần số tự nhiên +% k_carr = 1; % Gain of the overall loop +% loop filter coefficients: hệ số lọc vòng lặp +a3=1.1*wn_carr^2; +b3=2.4*wn_carr; +wn3=wn_carr^3; + +% filter values initialization: khởi tạo giá trị bộ lọc +olderrort_carr = 0; +oldolderrort_carr = 0; +oldcarrier_nco = 0; +oldoldcarrier_nco = 0; +T_int = 1e-3; % integration time +Acoeff=((T_int^2)*wn3/4)+(a3*T_int/2)+b3; +Bcoeff=((T_int^2)*wn3/2)-2*b3; +Ccoeff=((T_int^2)*wn3/4)-(a3*T_int/2)+b3; + +hwb = waitbar(0,'Tracking...'); +try + %% Start processing channels ============================================== + for channelNr = 1:settings.numberOfChannels + + % Only process if PRN is non zero (acquisition was successful) + % Chỉ xử lý nếu PRN khác 0 (acquisition thành công) + if (channel(channelNr).PRN ~= 0) + % Save additional information - each channel's tracked PRN: + % Lưu thông tin bổ sung - PRN được tracked ở mỗi kênh + trackResults(channelNr).PRN = channel(channelNr).PRN; + + % Move the starting point of processing. Can be used to start the + % signal processing at any point in the data record (e.g. for long + % records). In addition skip through that data file to start at the + % appropriate sample (corresponding to code phase). Assumes sample + % type is schar (or 1 byte per sample) + + % Di chuyển điểm bắt đầu xử lý. Có thể được sử dụng để bắt đầu xử lý + % tín hiệu tại bất kỳ điểm nào trong bản ghi dữ liệu (ví dụ: đối với + % các bản ghi dài). Ngoài ra, hãy bỏ qua tệp dữ liệu đó để bắt đầu + % với mẫu thích hợp (tương ứng với giai đoạn mã). Giả sử loại mẫu là + % schar (hoặc 1 byte cho mỗi mẫu) + fseek(fid, ... + settings.skipNumberOfBytes + (channel(channelNr).codePhase-1)*settings.dataTypeSize*2, ... + 'bof'); + + + % Get a vector with the C/A code sampled 1x/chip + % Nhận 1 vector có mã C/A được lấy mẫu 1x/chip + caCode = generateCAcode(channel(channelNr).PRN); + % Then make it possible to do early and late versions: từ đó + % mới có thể tạo được early và late + caCode = [caCode(1023) caCode caCode(1)]; + + %--- Perform various initializations ------------------------------ + % Thực hiện các khởi tạo khác nhau + + % define initial code frequency basis of NCO + % Xác định tần số cơ sở mã ban đầu của NCO + codeFreq = settings.codeFreqBasis; + % define residual code phase (in chips) + % Xác định pha mã dư (trong chip) + remCodePhase = 0.0; + % define carrier frequency which is used over whole tracking period + % Xác định tần số sóng mang được sử dụng trong toàn bộ thời + % gian tracking + carrFreq = channel(channelNr).acquiredFreq; + carrFreqBasis = channel(channelNr).acquiredFreq; + % define residual carrier phase: xác định pha sóng mang dư + remCarrPhase = 0.0; + + %code tracking loop parameters: tham số vòng lặp tracking + oldCodeNco = 0.0; + oldCodeError = 0.0; + + %carrier/Costas loop parametersL: tham số vòng lặp sóng mang/ + %Costas + oldCarrNco = 0.0; + oldCarrError = 0.0; + + %=== Process the number of specified code periods ================= + % Xử lý khoảng thời gian mã được chỉ định + for loopCnt = 1:codePeriods + %% GUI update ------------------------------------------------------------- + % The GUI is updated every 50ms. This way Matlab GUI is still + % responsive enough. At the same time Matlab is not occupied + % all the time with GUI task. + + % GUI được cập nhật cứ sau 50ms. Bằng cách này, GUI Matlab vẫn + % đủ đáp ứng. Đồng thời Matlab không phải lúc nào cũng bận rộn + % với nhiệm vụ GUI. + if (rem(loopCnt, 50) == 0) + try + waitbar(loopCnt/codePeriods, ... + hwb, ... + ['Tracking: Ch ', int2str(channelNr), ... + ' of ', int2str(settings.numberOfChannels), ... + '; PRN#', int2str(channel(channelNr).PRN), ... + '; Completed ',int2str(loopCnt), ... + ' of ', int2str(codePeriods), ' msec']); + catch + % The progress bar was closed. It is used as a signal + % to stop, "cancel" processing. Exit. + disp('Progress bar closed, exiting...'); + return + end + end + + %% Read next block of data ------------------------------------------------ + %% Đọc khối dữ liệu tiếp theo + % Find the size of a "block" or code period in whole samples + % Tìm kích thước của một "khối" hoặc đoạn mã trong toàn bộ mẫu + + % Update the phasestep based on code freq (variable) and + % sampling frequency (fixed) + % Cập nhật bước pha dựa trên tần số mã (biến) và tần số lấy mẫu (cố định) + codePhaseStep = codeFreq / settings.samplingFreq; + + blksize = ceil((settings.codeLength-remCodePhase) / codePhaseStep); + + % Read in the appropriate number of samples to process this + % interation: Đọc số lượng mẫu thích hợp để xử lý sự tương tác này + [tmp, samplesRead] = fread(fid, ... + 2*blksize, settings.dataType); + rawSignal=tmp(1:2:end)+1i*tmp(2:2:end); + + rawSignal = transpose(rawSignal); %transpose vector: vector chuyển vị + + % If did not read in enough samples, then could be out of + % data - better exit: Nếu không đọc đủ mẫu thì có thể hết + % dữ liệu - tốt hơn là thoát + if (samplesRead < 2*blksize) + disp('Not able to read the specified number of samples for tracking, exiting!') + fclose(fid); + return + end + + %% Set up all the code phase tracking information ------------------------- + %% Thiết lập thông tin theo dõi pha mã + % Define index into early code vector + tcode = (remCodePhase-earlyLateSpc) : ... + codePhaseStep : ... + ((blksize-1)*codePhaseStep+remCodePhase-earlyLateSpc); + tcode2 = ceil(tcode) + 1; + earlyCode = caCode(tcode2); + + % Define index into late code vector + tcode = (remCodePhase+earlyLateSpc) : ... + codePhaseStep : ... + ((blksize-1)*codePhaseStep+remCodePhase+earlyLateSpc); + tcode2 = ceil(tcode) + 1; + lateCode = caCode(tcode2); + + % Define index into prompt code vector + tcode = remCodePhase : ... + codePhaseStep : ... + ((blksize-1)*codePhaseStep+remCodePhase); + tcode2 = ceil(tcode) + 1; + promptCode = caCode(tcode2); + + remCodePhase = (tcode(blksize) + codePhaseStep) - 1023.0; + + %% Generate the carrier frequency to mix the signal to baseband ----------- + %% Tạo tần số sóng mang để trộn tín hiệu vào băng cơ sở + time = (0:blksize) ./ settings.samplingFreq; + + % Get the argument to sin/cos functions + % Lấy đối số cho hàm sin/cos + trigarg = ((carrFreq * 2.0 * pi) .* time) + remCarrPhase; + remCarrPhase = rem(trigarg(blksize+1), (2 * pi)); + + % Finally compute the signal to mix the collected data to bandband + carrCos = cos(trigarg(1:blksize)); + carrSin = sin(trigarg(1:blksize)); + + %% Generate the six standard accumulated values --------------------------- + % First mix to baseband + iqBasebandSignal=(carrSin+1i*carrCos).* rawSignal; + + + + qBasebandSignal = imag(iqBasebandSignal); + iBasebandSignal = real(iqBasebandSignal); + + % Now get early, late, and prompt values for each + I_E = sum(earlyCode .* iBasebandSignal); + Q_E = sum(earlyCode .* qBasebandSignal); + I_P = sum(promptCode .* iBasebandSignal); + Q_P = sum(promptCode .* qBasebandSignal); + I_L = sum(lateCode .* iBasebandSignal); + Q_L = sum(lateCode .* qBasebandSignal); + + %% Find PLL error and update carrier NCO ---------------------------------- + + % Implement carrier loop discriminator (phase detector) + carrError = atan(Q_P / I_P) / (2.0 * pi); + + % Implement carrier loop filter and generate NCO command + carrNco = oldCarrNco + (tau2carr/tau1carr) * ... + (carrError - oldCarrError) + carrError * (PDIcarr/tau1carr); + oldCarrNco = carrNco; + oldCarrError = carrError; + + % Modify carrier freq based on NCO command + carrFreq = carrFreqBasis + carrNco; + + trackResults(channelNr).carrFreq(loopCnt) = carrFreq; + %% Implement phase loop filter and generate NCO command (second order open loop transfer function F(Z)) + % errort_carr=atan(Q_P / I_P) / (2.0 * pi); + % errort_carr_filt=(Acoeff*errort_carr + Bcoeff*olderrort_carr + Ccoeff*oldolderrort_carr); + % carrNco = 2*oldcarrier_nco - oldoldcarrier_nco + errort_carr_filt; + % carrFreq = carrFreqBasis + carrNco; %% NCO integrator (with the added pole of the integrator we complete the second order loop transfer function) + % + % oldolderrort_carr = olderrort_carr; + % olderrort_carr = errort_carr; + % oldoldcarrier_nco = oldcarrier_nco ; + % oldcarrier_nco = carrNco; + + %% Find DLL error and update code NCO ------------------------------------- + codeError = (sqrt(I_E * I_E + Q_E * Q_E) - sqrt(I_L * I_L + Q_L * Q_L)) / ... + (sqrt(I_E * I_E + Q_E * Q_E) + sqrt(I_L * I_L + Q_L * Q_L)); + + % Implement code loop filter and generate NCO command + codeNco = oldCodeNco + (tau2code/tau1code) * ... + (codeError - oldCodeError) + codeError * (PDIcode/tau1code); + oldCodeNco = codeNco; + oldCodeError = codeError; + + % Modify code freq based on NCO command + codeFreq = settings.codeFreqBasis - codeNco+carrFreq*codeFreq/(-carrFreq+1575.42e6); + + trackResults(channelNr).codeFreq(loopCnt) = codeFreq; + + %% Record various measures to show in postprocessing ---------------------- + % Record sample number (based on 8bit samples) + trackResults(channelNr).absoluteSample(loopCnt) = (1/settings.dataTypeSize/2)*ftell(fid); + trackResults(channelNr).remCodePhase(loopCnt) = remCodePhase; + + + trackResults(channelNr).dllDiscr(loopCnt) = codeError; + trackResults(channelNr).dllDiscrFilt(loopCnt) = codeNco; + trackResults(channelNr).pllDiscr(loopCnt) = carrError; + trackResults(channelNr).pllDiscrFilt(loopCnt) = carrNco; + + trackResults(channelNr).I_E(loopCnt) = I_E; + trackResults(channelNr).I_P(loopCnt) = I_P; + trackResults(channelNr).I_L(loopCnt) = I_L; + trackResults(channelNr).Q_E(loopCnt) = Q_E; + trackResults(channelNr).Q_P(loopCnt) = Q_P; + trackResults(channelNr).Q_L(loopCnt) = Q_L; + end % for loopCnt + + % If we got so far, this means that the tracking was successful + % Now we only copy status, but it can be update by a lock detector + % if implemented + trackResults(channelNr).status = channel(channelNr).status; + + end % if a PRN is assigned + end % for channelNr +catch exception + disp(exception.message); +end; +% Close the waitbar +close(hwb) diff --git a/GNSS_SDR_IQ/tracking_V0_IQ.m b/GNSS_SDR_IQ/tracking_V0_IQ.m new file mode 100644 index 0000000..6507bf4 --- /dev/null +++ b/GNSS_SDR_IQ/tracking_V0_IQ.m @@ -0,0 +1,381 @@ +function [trackResults, channel]= tracking_V0_IQ(fid, channel, settings) +% Performs code and carrier tracking for all channels. +% +%[trackResults, channel] = tracking(fid, channel, settings) +% +% Inputs: +% fid - file identifier of the signal record. +% channel - PRN, carrier frequencies and code phases of all +% satellites to be tracked (prepared by preRum.m from +% acquisition results). +% settings - receiver settings. +% Outputs: +% trackResults - tracking results (structure array). Contains +% in-phase prompt outputs and absolute starting +% positions of spreading codes, together with other +% observation data from the tracking loops. All are +% saved every millisecond. + +%-------------------------------------------------------------------------- +% SoftGNSS v3.0 +% +% Copyright (C) Dennis M. Akos +% Written by Darius Plausinaitis and Dennis M. Akos +% Based on code by DMAkos Oct-1999 +%-------------------------------------------------------------------------- +%This program is free software; you can redistribute it and/or +%modify it under the terms of the GNU General Public License +%as published by the Free Software Foundation; either version 2 +%of the License, or (at your option) any later version. +% +%This program is distributed in the hope that it will be useful, +%but WITHOUT ANY WARRANTY; without even the implied warranty of +%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%GNU General Public License for more details. +% +%You should have received a copy of the GNU General Public License +%along with this program; if not, write to the Free Software +%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +%USA. +%-------------------------------------------------------------------------- + +%CVS record: +%$Id: tracking.m,v 1.14.2.32 2007/01/30 09:45:12 dpl Exp $ + +%% Initialize result structure ============================================ + +% Channel status: trạng thái kênh +trackResults.status = '-'; % No tracked signal, or lost lock + +% The absolute sample in the record of the C/A code start: khởi tạo mẫu tuyệt đối +% trong bản ghi C/A +trackResults.absoluteSample = zeros(1, settings.msToProcess); + +% Freq of the C/A code: tần số mã C/A +trackResults.codeFreq = inf(1, settings.msToProcess); + +% Frequency of the tracked carrier wave: tần số của sóng mang được theo dõi +trackResults.carrFreq = inf(1, settings.msToProcess); + +% Outputs from the correlators (In-phase): đầu ra của các bộ tương quan (I) +trackResults.I_P = zeros(1, settings.msToProcess); +trackResults.I_E = zeros(1, settings.msToProcess); +trackResults.I_L = zeros(1, settings.msToProcess); + +% Outputs from the correlators (Quadrature-phase): đầu ra của các bộ tương +% quan (Q) +trackResults.Q_E = zeros(1, settings.msToProcess); +trackResults.Q_P = zeros(1, settings.msToProcess); +trackResults.Q_L = zeros(1, settings.msToProcess); + +% Loop discriminators +trackResults.dllDiscr = inf(1, settings.msToProcess); +trackResults.dllDiscrFilt = inf(1, settings.msToProcess); +trackResults.pllDiscr = inf(1, settings.msToProcess); +trackResults.pllDiscrFilt = inf(1, settings.msToProcess); + +%--- Copy initial settings for all channels ------------------------------- +trackResults = repmat(trackResults, 1, settings.numberOfChannels); + +%% Initialize tracking variables ========================================== + +codePeriods = settings.msToProcess; % For GPS one C/A code is one ms (với GPS thì 1 mã C/A là 1 ms) + +%--- DLL variables -------------------------------------------------------- +% Define early-late offset (in chips): Xác định độ lệch sớm-muộn (tính +% bằng chip) +earlyLateSpc = settings.dllCorrelatorSpacing; + +% Summation interval: khoảng tính tổng +PDIcode = 0.001; + +% Calculate filter coefficient values: Tính toán giá trị hệ số bộ lọc +[tau1code, tau2code] = calcLoopCoef(settings.dllNoiseBandwidth, ... + settings.dllDampingRatio, ... + 1.0); + +%--- PLL variables -------------------------------------------------------- +% Summation interval +PDIcarr = 0.001; + +% Calculate filter coefficient values +[tau1carr, tau2carr] = calcLoopCoef(settings.pllNoiseBandwidth, ... + settings.pllDampingRatio, ... + 0.25); + % CARRIER TRACKING LOOP +Bl_carr =5; % Bandwith in hertz: băng thông tính bằng Hz +wn_carr = Bl_carr/0.7845; % Natural Frequency: tần số tự nhiên +% k_carr = 1; % Gain of the overall loop +% loop filter coefficients: hệ số lọc vòng lặp +a3=1.1*wn_carr^2; +b3=2.4*wn_carr; +wn3=wn_carr^3; + +% filter values initialization: khởi tạo giá trị bộ lọc +olderrort_carr = 0; +oldolderrort_carr = 0; +oldcarrier_nco = 0; +oldoldcarrier_nco = 0; +T_int = 1e-3; % integration time +Acoeff=((T_int^2)*wn3/4)+(a3*T_int/2)+b3; +Bcoeff=((T_int^2)*wn3/2)-2*b3; +Ccoeff=((T_int^2)*wn3/4)-(a3*T_int/2)+b3; + +hwb = waitbar(0,'Tracking...'); +try + %% Start processing channels ============================================== + for channelNr = 1:settings.numberOfChannels + + % Only process if PRN is non zero (acquisition was successful) + % Chỉ xử lý nếu PRN khác 0 (acquisition thành công) + if (channel(channelNr).PRN ~= 0) + % Save additional information - each channel's tracked PRN: + % Lưu thông tin bổ sung - PRN được tracked ở mỗi kênh + trackResults(channelNr).PRN = channel(channelNr).PRN; + + % Move the starting point of processing. Can be used to start the + % signal processing at any point in the data record (e.g. for long + % records). In addition skip through that data file to start at the + % appropriate sample (corresponding to code phase). Assumes sample + % type is schar (or 1 byte per sample) + + % Di chuyển điểm bắt đầu xử lý. Có thể được sử dụng để bắt đầu xử lý + % tín hiệu tại bất kỳ điểm nào trong bản ghi dữ liệu (ví dụ: đối với + % các bản ghi dài). Ngoài ra, hãy bỏ qua tệp dữ liệu đó để bắt đầu + % với mẫu thích hợp (tương ứng với giai đoạn mã). Giả sử loại mẫu là + % schar (hoặc 1 byte cho mỗi mẫu) + fseek(fid, ... + settings.skipNumberOfBytes + (channel(channelNr).codePhase-1)*settings.dataTypeSize*2, ... + 'bof'); + + + % Get a vector with the C/A code sampled 1x/chip + % Nhận 1 vector có mã C/A được lấy mẫu 1x/chip + caCode = generateCAcode(channel(channelNr).PRN); + % Then make it possible to do early and late versions: từ đó + % mới có thể tạo được early và late + caCode = [caCode(1023) caCode caCode(1)]; + + %--- Perform various initializations ------------------------------ + % Thực hiện các khởi tạo khác nhau + + % define initial code frequency basis of NCO + % Xác định tần số cơ sở mã ban đầu của NCO + codeFreq = settings.codeFreqBasis; + % define residual code phase (in chips) + % Xác định pha mã dư (trong chip) + remCodePhase = 0.0; + % define carrier frequency which is used over whole tracking period + % Xác định tần số sóng mang được sử dụng trong toàn bộ thời + % gian tracking + carrFreq = channel(channelNr).acquiredFreq; + carrFreqBasis = channel(channelNr).acquiredFreq; + % define residual carrier phase: xác định pha sóng mang dư + remCarrPhase = 0.0; + + %code tracking loop parameters: tham số vòng lặp tracking + oldCodeNco = 0.0; + oldCodeError = 0.0; + + %carrier/Costas loop parametersL: tham số vòng lặp sóng mang/ + %Costas + oldCarrNco = 0.0; + oldCarrError = 0.0; + + %=== Process the number of specified code periods ================= + % Xử lý khoảng thời gian mã được chỉ định + for loopCnt = 1:codePeriods + %% GUI update ------------------------------------------------------------- + % The GUI is updated every 50ms. This way Matlab GUI is still + % responsive enough. At the same time Matlab is not occupied + % all the time with GUI task. + + % GUI được cập nhật cứ sau 50ms. Bằng cách này, GUI Matlab vẫn + % đủ đáp ứng. Đồng thời Matlab không phải lúc nào cũng bận rộn + % với nhiệm vụ GUI. + if (rem(loopCnt, 50) == 0) + try + waitbar(loopCnt/codePeriods, ... + hwb, ... + ['Tracking: Ch ', int2str(channelNr), ... + ' of ', int2str(settings.numberOfChannels), ... + '; PRN#', int2str(channel(channelNr).PRN), ... + '; Completed ',int2str(loopCnt), ... + ' of ', int2str(codePeriods), ' msec']); + catch + % The progress bar was closed. It is used as a signal + % to stop, "cancel" processing. Exit. + disp('Progress bar closed, exiting...'); + return + end + end + + %% Read next block of data ------------------------------------------------ + %% Đọc khối dữ liệu tiếp theo + % Find the size of a "block" or code period in whole samples + % Tìm kích thước của một "khối" hoặc đoạn mã trong toàn bộ mẫu + + % Update the phasestep based on code freq (variable) and + % sampling frequency (fixed) + % Cập nhật bước pha dựa trên tần số mã (biến) và tần số lấy mẫu (cố định) + codePhaseStep = codeFreq / settings.samplingFreq; + + blksize = ceil((settings.codeLength-remCodePhase) / codePhaseStep); + + % Read in the appropriate number of samples to process this + % interation: Đọc số lượng mẫu thích hợp để xử lý sự tương tác này + [tmp, samplesRead] = fread(fid, ... + 2*blksize, settings.dataType); + rawSignal=tmp(1:2:end)+1i*tmp(2:2:end); + + rawSignal = transpose(rawSignal); %transpose vector: vector chuyển vị + + % If did not read in enough samples, then could be out of + % data - better exit: Nếu không đọc đủ mẫu thì có thể hết + % dữ liệu - tốt hơn là thoát + if (samplesRead < 2*blksize) + disp('Not able to read the specified number of samples for tracking, exiting!') + fclose(fid); + return + end + + %% Set up all the code phase tracking information ------------------------- + %% Thiết lập thông tin theo dõi pha mã + % Define index into early code vector + tcode = (remCodePhase-earlyLateSpc) : ... + codePhaseStep : ... + ((blksize-1)*codePhaseStep+remCodePhase-earlyLateSpc); + tcode2 = ceil(tcode) + 1; + earlyCode = caCode(tcode2); + + % Define index into late code vector + tcode = (remCodePhase+earlyLateSpc) : ... + codePhaseStep : ... + ((blksize-1)*codePhaseStep+remCodePhase+earlyLateSpc); + tcode2 = ceil(tcode) + 1; + lateCode = caCode(tcode2); + + % Define index into prompt code vector + tcode = remCodePhase : ... + codePhaseStep : ... + ((blksize-1)*codePhaseStep+remCodePhase); + tcode2 = ceil(tcode) + 1; + promptCode = caCode(tcode2); + + remCodePhase = (tcode(blksize) + codePhaseStep) - 1023.0; + + %% Generate the carrier frequency to mix the signal to baseband ----------- + %% Tạo tần số sóng mang để trộn tín hiệu vào băng cơ sở + time = (0:blksize) ./ settings.samplingFreq; + + % Get the argument to sin/cos functions + % Lấy đối số cho hàm sin/cos + trigarg = ((carrFreq * 2.0 * pi) .* time) + remCarrPhase; + remCarrPhase = rem(trigarg(blksize+1), (2 * pi)); + + % Finally compute the signal to mix the collected data to bandband + % Cuối cùng tính toán tín hiệu để trộn dữ liệu thu thập được vào băng thông + carrCos = cos(trigarg(1:blksize)); + carrSin = sin(trigarg(1:blksize)); + + %% Generate the six standard accumulated values --------------------------- + %% Tạo sáu giá trị tích lũy tiêu chuẩn + % First mix to baseband + % Trộn đầu tiên vào baseband + iqBasebandSignal=(carrSin+1i*carrCos).* rawSignal; + + + + qBasebandSignal = imag(iqBasebandSignal); + iBasebandSignal = real(iqBasebandSignal); + + % Now get early, late, and prompt values for each + I_E = sum(earlyCode .* iBasebandSignal); + Q_E = sum(earlyCode .* qBasebandSignal); + I_P = sum(promptCode .* iBasebandSignal); + Q_P = sum(promptCode .* qBasebandSignal); + I_L = sum(lateCode .* iBasebandSignal); + Q_L = sum(lateCode .* qBasebandSignal); + + %% Find PLL error and update carrier NCO ---------------------------------- + + % Implement carrier loop discriminator (phase detector) + % Triển khai bộ phân biệt vòng lặp sóng mang (bộ dò pha) + carrError = atan(Q_P / I_P) / (2.0 * pi); + + % Implement carrier loop filter and generate NCO command + % Triển khai bộ lọc vòng lặp sóng mang và tạo lệnh NCO + carrNco = oldCarrNco + (tau2carr/tau1carr) * ... + (carrError - oldCarrError) + carrError * (PDIcarr/tau1carr); + oldCarrNco = carrNco; + oldCarrError = carrError; + + % Modify carrier freq based on NCO command + % Sửa đổi tần số sóng mang dựa trên lệnh NCO + carrFreq = carrFreqBasis + carrNco; + + trackResults(channelNr).carrFreq(loopCnt) = carrFreq; + %% Implement phase loop filter and generate NCO command (second order open loop transfer function F(Z)) + % errort_carr=atan(Q_P / I_P) / (2.0 * pi); + % errort_carr_filt=(Acoeff*errort_carr + Bcoeff*olderrort_carr + Ccoeff*oldolderrort_carr); + % carrNco = 2*oldcarrier_nco - oldoldcarrier_nco + errort_carr_filt; + % carrFreq = carrFreqBasis + carrNco; %% NCO integrator (with the added pole of the integrator we complete the second order loop transfer function) + % + % oldolderrort_carr = olderrort_carr; + % olderrort_carr = errort_carr; + % oldoldcarrier_nco = oldcarrier_nco ; + % oldcarrier_nco = carrNco; + + %% Find DLL error and update code NCO ------------------------------------- + %% Tìm lỗi DLL và cập nhật mã NCO + codeError = (sqrt(I_E * I_E + Q_E * Q_E) - sqrt(I_L * I_L + Q_L * Q_L)) / ... + (sqrt(I_E * I_E + Q_E * Q_E) + sqrt(I_L * I_L + Q_L * Q_L)); + + % Implement code loop filter and generate NCO command + % Triển khai bộ lọc vòng lặp mã và tạo lệnh NCO + codeNco = oldCodeNco + (tau2code/tau1code) * ... + (codeError - oldCodeError) + codeError * (PDIcode/tau1code); + oldCodeNco = codeNco; + oldCodeError = codeError; + + % Modify code freq based on NCO command + % Sửa đổi tần số mã dựa trên lệnh NCO + codeFreq = settings.codeFreqBasis - codeNco+carrFreq*codeFreq/(-carrFreq+1575.42e6); + + trackResults(channelNr).codeFreq(loopCnt) = codeFreq; + + %% Record various measures to show in postprocessing ---------------------- + %% Ghi lại các biện pháp khác nhau để hiển thị trong quá trình xử lý hậu kỳ + % Record sample number (based on 8bit samples) + trackResults(channelNr).absoluteSample(loopCnt) = (1/settings.dataTypeSize/2)*ftell(fid); + trackResults(channelNr).remCodePhase(loopCnt) = remCodePhase; + + + trackResults(channelNr).dllDiscr(loopCnt) = codeError; + trackResults(channelNr).dllDiscrFilt(loopCnt) = codeNco; + trackResults(channelNr).pllDiscr(loopCnt) = carrError; + trackResults(channelNr).pllDiscrFilt(loopCnt) = carrNco; + + trackResults(channelNr).I_E(loopCnt) = I_E; + trackResults(channelNr).I_P(loopCnt) = I_P; + trackResults(channelNr).I_L(loopCnt) = I_L; + trackResults(channelNr).Q_E(loopCnt) = Q_E; + trackResults(channelNr).Q_P(loopCnt) = Q_P; + trackResults(channelNr).Q_L(loopCnt) = Q_L; + end % for loopCnt + + % If we got so far, this means that the tracking was successful + % Now we only copy status, but it can be update by a lock detector + % if implemented + % Nếu chúng tôi đã đi xa đến thế, điều này có nghĩa là việc theo dõi đã thành công + % Bây giờ chúng tôi chỉ sao chép trạng thái, nhưng nó có thể được cập nhật + % bằng trình phát hiện khóa nếu được triển khai + trackResults(channelNr).status = channel(channelNr).status; + + end % if a PRN is assigned + end % for channelNr +catch exception + disp(exception.message); +end; +% Close the waitbar +close(hwb) diff --git a/MatrixFFT4096.py b/MatrixFFT4096.py new file mode 100644 index 0000000..b258c82 --- /dev/null +++ b/MatrixFFT4096.py @@ -0,0 +1,49 @@ +import torch +import numpy as np + + +class MatrixFFT4096: + def __init__(self, device="cpu"): + self.N = 4096 + self.device = torch.device(device) + + # Luôn xây ma trận bằng complex64 để nhẹ hơn + n = np.arange(self.N) + k = n.reshape((self.N, 1)) + W = np.exp(-2j * np.pi * k * n / self.N).astype(np.complex64) + W_inv = np.exp(2j * np.pi * k * n / self.N).astype(np.complex64) / self.N + + self.DFT = torch.tensor(W, dtype=torch.complex64, device=self.device) + self.IDFT = torch.tensor(W_inv, dtype=torch.complex64, device=self.device) + + def _to_tensor(self, x): + """Nhận numpy hoặc torch -> ép về torch.complex64 trên device""" + if isinstance(x, np.ndarray): + # numpy mặc định là complex128 -> ép về complex64 + x = torch.tensor(x, dtype=torch.complex64, device=self.device) + elif isinstance(x, torch.Tensor): + # nếu là torch nhưng sai dtype -> ép lại complex64 + if not torch.is_complex(x): + x = x.to(torch.complex64) + else: + x = x.to(torch.complex64) + x = x.to(self.device) + else: + raise TypeError("Input must be numpy.ndarray or torch.Tensor") + return x + + def fft(self, x): + """ + FFT bằng nhân ma trận. + x: numpy.ndarray hoặc torch.Tensor, shape (B,4096) hoặc (4096,) + """ + x = self._to_tensor(x) + return torch.matmul(x, self.DFT.T) + + def ifft(self, X): + """ + IFFT bằng nhân ma trận. + X: numpy.ndarray hoặc torch.Tensor + """ + X = self._to_tensor(X) + return torch.matmul(X, self.IDFT.T) diff --git a/README.md b/README.md new file mode 100644 index 0000000..076a073 --- /dev/null +++ b/README.md @@ -0,0 +1,26 @@ +# Getting Started + +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. + +# System requirements + +* python 2.7 +* matplotlib +* scipy +* numpy + +# Installation + +Coming soon! + +# Running the GNSS SDR + +1. Examine "main.py" +2. Tweak parameters for the "settings" class if necessary +3. Specify the binary file to be processed and run "main.py" +4. Wait until it is finished + + +# Resources +* The official homepage of the textbook + diff --git a/acq_2ms_ane.mlpackage.zip.png b/acq_2ms_ane.mlpackage.zip.png new file mode 100644 index 0000000..ddfba56 Binary files /dev/null and b/acq_2ms_ane.mlpackage.zip.png differ diff --git a/acq_2ms_ane.mlpackage.zip.svg b/acq_2ms_ane.mlpackage.zip.svg new file mode 100644 index 0000000..955bd24 --- /dev/null +++ b/acq_2ms_ane.mlpackage.zip.svg @@ -0,0 +1 @@ +2×40962×4096409629409629×11×409629×409629×409629×409629×40962×1×40962×1×40962×1×40962×1×40961×29×40961×29×40962×29×40961×29×40961×29×40962×29×40962×29×40962×29×40962×29×40962×29×409658×409658×409658×409658×409658×409658×409658×409658×409658×409658×409658×409658×40961×40961×40961×40961×40964096409640961×40961×409658×40961×40961×409658×409658×409658×409658×409658×409658×409658×409658×409658×409658×409658×409658×409658×409658×409658×409658×40962×29×40962×29×40962×29×40962×29×40962×29×409629×4096sig_realfloat16[2,4096]sig_imagfloat16[2,4096]ca_codefloat16[4096]freqBinsfloat16[29]phasePointsfloat16[4096]expand_dimsexpand_dimsmulcossinexpand_dimsexpand_dimsexpand_dimsmulexpand_dimsmulsubmulmuladdreshapereshapelinearfloat16[4096,4096]weight〈4096×4096〉float16[4096]bias〈4096〉linearfloat16[4096,4096]weight〈4096×4096〉float16[4096]bias〈4096〉sublinearfloat16[4096,4096]weight〈4096×4096〉float16[4096]bias〈4096〉linearfloat16[4096,4096]weight〈4096×4096〉float16[4096]bias〈4096〉addexpand_dimslinearfloat16[4096,4096]weight〈4096×4096〉float16[4096]bias〈4096〉linearfloat16[4096,4096]weight〈4096×4096〉float16[4096]bias〈4096〉squeezesqueezeconstmulexpand_dimsmulexpand_dimsmulsubmulmuladdlinearfloat16[4096,4096]weight〈4096×4096〉float16[4096]bias〈4096〉linearfloat16[4096,4096]weight〈4096×4096〉float16[4096]bias〈4096〉addconstmullinearfloat16[4096,4096]weight〈4096×4096〉float16[4096]bias〈4096〉constmullinearfloat16[4096,4096]weight〈4096×4096〉float16[4096]bias〈4096〉addconstmulreshapereshapeconstpowconstpowaddconstreduce_maxreduce_max_0float16[29,4096] \ No newline at end of file diff --git a/acq_2ms_ane_v2.mlpackage.zip.png b/acq_2ms_ane_v2.mlpackage.zip.png new file mode 100644 index 0000000..280c6ed Binary files /dev/null and b/acq_2ms_ane_v2.mlpackage.zip.png differ diff --git a/acquisition.py b/acquisition.py new file mode 100644 index 0000000..3b074a6 --- /dev/null +++ b/acquisition.py @@ -0,0 +1,354 @@ +import numpy as np + +from initialize import Result +import matplotlib.pyplot as plt + + + +class AcquisitionResult(Result): + def __init__(self, settings): + self._settings = settings + self._results = None + self._channels = None + + @property + def peakMetric(self): + assert isinstance(self._results, np.recarray) + return self._results.peakMetric + + @property + def carrFreq(self): + assert isinstance(self._results, np.recarray) + return self._results.carrFreq + + @property + def codePhase(self): + assert isinstance(self._results, np.recarray) + return self._results.codePhase + + def acquire(self, longSignal): + # ./acquisition.m + # Function performs cold start acquisition on the collected "data". It + # searches for GPS signals of all satellites, which are listed in field + # "acqSatelliteList" in the settings structure. Function saves code phase + # and frequency of the detected signals in the "acqResults" structure. + + # acqResults = acquisition(longSignal, settings) + + # Inputs: + # longSignal - 11 ms of raw signal from the front-end + # settings - Receiver settings. Provides information about + # sampling and intermediate frequencies and other + # parameters including the list of the satellites to + # be acquired. + # Outputs: + # acqResults - Function saves code phases and frequencies of the + # detected signals in the "acqResults" structure. The + # field "carrFreq" is set to 0 if the signal is not + # detected for the given PRN number. + + # Initialization ========================================================= + settings = self._settings + + # Find number of samples per spreading code + samplesPerCode = settings.samplesPerCode + + # Create two 1m sec vectors of data to correlate with and one with zero DC + signal1 = longSignal[0:samplesPerCode] + + signal2 = longSignal[samplesPerCode:2 * samplesPerCode] + + signal0DC = longSignal - longSignal.mean() + + # Find sampling period + ts = 1.0 / settings.samplingFreq + + # Find phase points of the local carrier wave + phasePoints = np.arange(samplesPerCode) * 2 * np.pi * ts + + # Number of the frequency bins for the given acquisition band (500Hz steps) + numberOfFrqBins = int(np.round(settings.acqSearchBand * 2) + 1) + + # Generate all C/A codes and sample them according to the sampling freq. + caCodesTable = settings.makeCaTable() + + # --- Initialize arrays to speed up the code ------------------------------- + # Search results of all frequency bins and code shifts (for one satellite) + results = np.zeros((numberOfFrqBins, samplesPerCode)) + + # Carrier frequencies of the frequency bins + frqBins = np.zeros(numberOfFrqBins) + + # --- Initialize acqResults ------------------------------------------------ + # Carrier frequencies of detected signals + carrFreq = np.zeros(32) + + # C/A code phases of detected signals + codePhase_ = np.zeros(32) + + # Correlation peak ratios of the detected signals + peakMetric = np.zeros(32) + + print ('(') + # Perform search for all listed PRN numbers ... + for PRN in range(len(settings.acqSatelliteList)): + # Correlate signals ====================================================== + # --- Perform DFT of C/A code ------------------------------------------ + caCodeFreqDom = np.fft.fft(caCodesTable[PRN, :]).conj() + + for frqBinIndex in range(numberOfFrqBins): + # --- Generate carrier wave frequency grid (0.5kHz step) ----------- + frqBins[frqBinIndex] = settings.IF - \ + settings.acqSearchBand / 2 * 1000 + \ + 500.0 * frqBinIndex + + sinCarr = np.sin(frqBins[frqBinIndex] * phasePoints) + + cosCarr = np.cos(frqBins[frqBinIndex] * phasePoints) + + I1 = sinCarr * signal1 + + Q1 = cosCarr * signal1 + + I2 = sinCarr * signal2 + + Q2 = cosCarr * signal2 + + IQfreqDom1 = np.fft.fft(I1 + 1j * Q1) + + IQfreqDom2 = np.fft.fft(I2 + 1j * Q2) + + # domain) + convCodeIQ1 = IQfreqDom1 * caCodeFreqDom + + convCodeIQ2 = IQfreqDom2 * caCodeFreqDom + + acqRes1 = abs(np.fft.ifft(convCodeIQ1)) ** 2 + + acqRes2 = abs(np.fft.ifft(convCodeIQ2)) ** 2 + + # "blend" 1st and 2nd msec but will correct data bit issues + if acqRes1.max() > acqRes2.max(): + results[frqBinIndex, :] = acqRes1 + + else: + results[frqBinIndex, :] = acqRes2 + + # Look for correlation peaks in the results ============================== + # Find the highest peak and compare it to the second highest peak + # The second peak is chosen not closer than 1 chip to the highest peak + # --- Find the correlation peak and the carrier frequency -------------- + peakSize = results.max(1).max() + frequencyBinIndex = results.max(1).argmax() + + peakSize = results.max(0).max() + codePhase = results.max(0).argmax() + + samplesPerCodeChip = np.long(round(settings.samplingFreq / settings.codeFreqBasis)) + + excludeRangeIndex1 = codePhase - samplesPerCodeChip + + excludeRangeIndex2 = codePhase + samplesPerCodeChip + + # boundaries + if excludeRangeIndex1 <= 0: + codePhaseRange = np.r_[excludeRangeIndex2:samplesPerCode + excludeRangeIndex1 + 1] + + elif excludeRangeIndex2 >= samplesPerCode - 1: + codePhaseRange = np.r_[excludeRangeIndex2 - samplesPerCode:excludeRangeIndex1] + + else: + codePhaseRange = np.r_[0:excludeRangeIndex1 + 1, excludeRangeIndex2:samplesPerCode] + + # --- Find the second highest correlation peak in the same freq. bin --- + secondPeakSize = results[frequencyBinIndex, codePhaseRange].max() + + peakMetric[PRN] = peakSize / secondPeakSize + + if (peakSize / secondPeakSize) > settings.acqThreshold: + # Fine resolution frequency search ======================================= + # --- Indicate PRN number of the detected signal ------------------- + print( '%02d ' % (PRN + 1)) + caCode = settings.generateCAcode(PRN) + + codeValueIndex = np.floor(ts * np.arange(1, 10 * samplesPerCode + 1) / (1.0 / settings.codeFreqBasis)) + + longCaCode = caCode[np.longlong(codeValueIndex % 1023)] + + # (Using detected C/A code phase) + xCarrier = signal0DC[codePhase:codePhase + 10 * samplesPerCode] * longCaCode + + fftNumPts = 8 * 2 ** (np.ceil(np.log2(len(xCarrier)))) + + # associated carrier frequency + fftxc = np.abs(np.fft.fft(xCarrier, np.long(fftNumPts))) + + uniqFftPts = np.long(np.ceil((fftNumPts + 1) / 2.0)) + + fftMax = fftxc[4:uniqFftPts - 5].max() + fftMaxIndex = fftxc[4:uniqFftPts - 5].argmax() + + fftFreqBins = np.arange(uniqFftPts) * settings.samplingFreq / fftNumPts + + carrFreq[PRN] = fftFreqBins[fftMaxIndex] + + codePhase_[PRN] = codePhase + + plot_acquisition_3d(results, frqBins, PRN, settings) + + else: + # --- No signal with this PRN -------------------------------------- + print ('. ') + + # === Acquisition is over ================================================== + print (')\n') + acqResults = np.core.records.fromarrays([carrFreq, codePhase_, peakMetric], + names='carrFreq,codePhase,peakMetric') + self._results = acqResults + return + + def plot(self): + assert isinstance(self._results, np.recarray) + import matplotlib as mpl + mpl.rcParams['text.usetex'] = False + import matplotlib.pyplot as plt + + # %% configure matplotlib + mpl.rcdefaults() + mpl.rc('savefig', bbox='tight', transparent=False, format='png') + mpl.rc('axes', grid=True, linewidth=1.5, axisbelow=True) + mpl.rc('lines', linewidth=1.5, solid_joinstyle='bevel') + mpl.rc('figure', figsize=[8, 6], autolayout=False, dpi=120) + # 🔴 xoá hoặc comment dòng dưới + # mpl.rc('text', usetex=True) + mpl.rc('font', family='serif', size=16) + mpl.rc('mathtext', fontset='cm') + + # ==== Plot acquisition results ==== + f, hAxes = plt.subplots() + + plt.bar(range(1, 33), self.peakMetric) + plt.title('Acquisition results') + plt.xlabel('PRN number (no bar - SV is not in the acquisition list)') + plt.ylabel('Acquisition Metric ($1^{st}$ to $2^{nd}$ Correlation Peaks Ratio)') + oldAxis = plt.axis() + + plt.axis([0, 33, 0, oldAxis[-1]]) + plt.xticks(range(1, 33), size=12) + hAxes.xaxis.grid() + + acquiredSignals = self.peakMetric * (self.carrFreq > 0) + plt.bar(range(1, 33), acquiredSignals, color=(0, 0.8, 0)) + plt.legend(['Not acquired signals', 'Acquired signals']) + plt.show() + + # preRun.m + def preRun(self): + assert isinstance(self._results, np.recarray) + # Function initializes tracking channels from acquisition data. The acquired + # signals are sorted according to the signal strength. This function can be + # modified to use other satellite selection algorithms or to introduce + # acquired signal properties offsets for testing purposes. + + # [channel] = preRun(acqResults, settings) + + # Inputs: + # acqResults - results from acquisition. + # settings - receiver settings + + # Outputs: + # channel - structure contains information for each channel (like + # properties of the tracked signal, channel status etc.). + + settings = self._settings + # Initialize all channels ================================================ + PRN = np.zeros(settings.numberOfChannels, dtype='int64') + acquiredFreq = np.zeros(settings.numberOfChannels) + codePhase = np.zeros(settings.numberOfChannels) + status = ['-' for _ in range(settings.numberOfChannels)] + + # --- Copy initial data to all channels ------------------------------------ + + # Copy acquisition results =============================================== + + # --- Sort peaks to find strongest signals, keep the peak index information + PRNindexes = sorted(enumerate(self.peakMetric), + key=lambda x: x[-1], reverse=True) + + # --- Load information about each satellite -------------------------------- + # Maximum number of initialized channels is number of detected signals, but + # not more as the number of channels specified in the settings. + for ii in range(min(settings.numberOfChannels, sum(self.carrFreq > 0))): + PRN[ii] = PRNindexes[ii][0] + 1 + + acquiredFreq[ii] = self.carrFreq[PRNindexes[ii][0]] + + codePhase[ii] = self.codePhase[PRNindexes[ii][0]] + + status[ii] = 'T' + + channel = np.core.records.fromarrays([PRN, acquiredFreq, codePhase, status], + names='PRN,acquiredFreq,codePhase,status') + self._channels = channel + return + + def showChannelStatus(self): + # Prints the status of all channels in a table. + + # showChannelStatus(channel, settings) + + # Inputs: + # channel - data for each channel. It is used to initialize and + # at the processing of the signal (tracking part). + # settings - receiver settings + + channel = self._channels + settings = self._settings + assert isinstance(channel, np.recarray) + print ('\n*=========*=====*===============*===========*=============*========*') + print ('| Channel | PRN | Frequency | Doppler | Code Offset | Status |') + print ('*=========*=====*===============*===========*=============*========*') + for channelNr in range(settings.numberOfChannels): + if channel[channelNr].status != '-': + print ('| %2d | %3d | %2.5e | %5.0f | %6d | %1s |' % ( + channelNr, + channel[channelNr].PRN, + channel[channelNr].acquiredFreq, + channel[channelNr].acquiredFreq - settings.IF, + channel[channelNr].codePhase, + channel[channelNr].status)) + else: + print ('| %2d | --- | ------------ | ----- | ------ | Off |' % channelNr) + + print ('*=========*=====*===============*===========*=============*========*\n') + + + +def plot_acquisition_3d(results, frqBins, PRN, settings): + """ + Plot 3D surface acquisition result cho 1 PRN. + results: matrix [numberOfFrqBins x samplesPerCode] + frqBins: list tần số Doppler + """ + # Tạo grid cho code phase và Doppler + codePhases = np.arange(results.shape[1]) # 0 .. samplesPerCode-1 + dopplers = np.array(frqBins) # frequency bins + + X, Y = np.meshgrid(codePhases, dopplers) + Z = results + + # Vẽ 3D + fig = plt.figure(figsize=(10, 6)) + ax = fig.add_subplot(111, projection='3d') + + surf = ax.plot_surface(X, Y/1000.0, Z, cmap='viridis') # chia 1000 để hiện Doppler kHz + fig.colorbar(surf, shrink=0.5, aspect=5) + + ax.set_title(f'Acquisition Result PRN {PRN+1}') + ax.set_xlabel('Code Phase (samples)') + ax.set_ylabel('Doppler (kHz)') + ax.set_zlabel('Correlation Power') + + plt.show() +if __name__ == '__main__': + pass diff --git a/acquisition_IQ.py b/acquisition_IQ.py new file mode 100644 index 0000000..d053fbb --- /dev/null +++ b/acquisition_IQ.py @@ -0,0 +1,389 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.io import savemat + +from initialize import Result + + +class AcquisitionResult(Result): + def __init__(self, settings): + self._settings = settings + self._results = None + self._channels = None + + @property + def peakMetric(self): + assert isinstance(self._results, np.recarray) + return self._results.peakMetric + + @property + def carrFreq(self): + assert isinstance(self._results, np.recarray) + return self._results.carrFreq + + @property + def codePhase(self): + assert isinstance(self._results, np.recarray) + return self._results.codePhase + + def acquire(self, longSignal): + # ./acquisition.m + # Function performs cold start acquisition on the collected "data". It + # searches for GPS signals of all satellites, which are listed in field + # "acqSatelliteList" in the settings structure. Function saves code phase + # and frequency of the detected signals in the "acqResults" structure. + + # acqResults = acquisition(longSignal, settings) + + # Inputs: + # longSignal - 11 ms of raw signal from the front-end + # settings - Receiver settings. Provides information about + # sampling and intermediate frequencies and other + # parameters including the list of the satellites to + # be acquired. + # Outputs: + # acqResults - Function saves code phases and frequencies of the + # detected signals in the "acqResults" structure. The + # field "carrFreq" is set to 0 if the signal is not + # detected for the given PRN number. + + # Initialization ========================================================= + settings = self._settings + + # Find number of samples per spreading code + samplesPerCode = settings.samplesPerCode + + # Create two 1m sec vectors of data to correlate with and one with zero DC + + rawSignal1 = longSignal[0:2 * samplesPerCode] # IQ + signalI1 = rawSignal1[0::2] + signalQ1 = rawSignal1[1::2] + signal1 = signalI1 + 1j * signalQ1 + + rawSignal2 = longSignal[2 * samplesPerCode:4 * samplesPerCode] + signalI2 = rawSignal2[0::2] + signalQ2 = rawSignal2[1::2] + signal2 = signalI2 + 1j * signalQ2 + + longI = longSignal[0::2] + longQ = longSignal[1::2] + longIQ = longI + 1j * longQ + signal0DC = longIQ - longIQ.mean() + + # Find sampling period + ts = 1.0 / settings.samplingFreq + + # Find phase points of the local carrier wave + phasePoints = np.arange(samplesPerCode) * 2 * np.pi * ts + + # Number of the frequency bins for the given acquisition band (500Hz steps) + numberOfFrqBins = int(np.round(settings.acqSearchBand * 2) + 1) + + # Generate all C/A codes and sample them according to the sampling freq. + caCodesTable = settings.makeCaTable() + + # --- Initialize arrays to speed up the code ------------------------------- + # Search results of all frequency bins and code shifts (for one satellite) + results = np.zeros((numberOfFrqBins, samplesPerCode)) + + # Carrier frequencies of the frequency bins + frqBins = np.zeros(numberOfFrqBins) + + # --- Initialize acqResults ------------------------------------------------ + # Carrier frequencies of detected signals + carrFreq = np.zeros(32) + + # C/A code phases of detected signals + codePhase_ = np.zeros(32) + + # Correlation peak ratios of the detected signals + peakMetric = np.zeros(32) + + print('(') + # Perform search for all listed PRN numbers ... + for PRN in range(len(settings.acqSatelliteList)): + # Correlate signals ====================================================== + # --- Perform DFT of C/A code ------------------------------------------ + caCodeFreqDom = np.fft.fft(caCodesTable[PRN, :]).conj() + + for frqBinIndex in range(numberOfFrqBins): + # --- Generate carrier wave frequency grid (0.5kHz step) ----------- + frqBins[frqBinIndex] = settings.IF - \ + settings.acqSearchBand / 2 * 1000 + \ + 500.0 * frqBinIndex + + sinCarr = np.sin(frqBins[frqBinIndex] * phasePoints) + + cosCarr = np.cos(frqBins[frqBinIndex] * phasePoints) + + IQ1 = (sinCarr + 1j * cosCarr) * signal1 + I1 = np.real(IQ1) + Q1 = np.imag(IQ1) + + IQ2 = (sinCarr + 1j * cosCarr) * signal2 + I2 = np.real(IQ2) + Q2 = np.imag(IQ2) + + IQfreqDom1 = np.fft.fft(I1 + 1j * Q1) + + IQfreqDom2 = np.fft.fft(I2 + 1j * Q2) + + # domain) + convCodeIQ1 = IQfreqDom1 * caCodeFreqDom + + convCodeIQ2 = IQfreqDom2 * caCodeFreqDom + + acqRes1 = abs(np.fft.ifft(convCodeIQ1)) ** 2 + + acqRes2 = abs(np.fft.ifft(convCodeIQ2)) ** 2 + + # "blend" 1st and 2nd msec but will correct data bit issues + if acqRes1.max() > acqRes2.max(): + results[frqBinIndex, :] = acqRes1 + + else: + results[frqBinIndex, :] = acqRes2 + + # Look for correlation peaks in the results ============================== + # Find the highest peak and compare it to the second highest peak + # The second peak is chosen not closer than 1 chip to the highest peak + # --- Find the correlation peak and the carrier frequency -------------- + peakSize = results.max(1).max() + frequencyBinIndex = results.max(1).argmax() + + peakSize = results.max(0).max() + codePhase = results.max(0).argmax() + + samplesPerCodeChip = int(round(settings.samplingFreq / settings.codeFreqBasis)) + + excludeRangeIndex1 = codePhase - samplesPerCodeChip + + excludeRangeIndex2 = codePhase + samplesPerCodeChip + + # boundaries + if excludeRangeIndex1 <= 0: + codePhaseRange = np.r_[excludeRangeIndex2:samplesPerCode + excludeRangeIndex1 + 1] + + elif excludeRangeIndex2 >= samplesPerCode - 1: + codePhaseRange = np.r_[excludeRangeIndex2 - samplesPerCode:excludeRangeIndex1] + + else: + codePhaseRange = np.r_[0:excludeRangeIndex1 + 1, excludeRangeIndex2:samplesPerCode] + + # --- Find the second highest correlation peak in the same freq. bin --- + secondPeakSize = results[frequencyBinIndex, codePhaseRange].max() + + peakMetric[PRN] = peakSize / secondPeakSize + + if (peakSize / secondPeakSize) > settings.acqMinThreshold and (peakSize / secondPeakSize) < settings.acqMaxThreshold: + # Fine resolution frequency search ======================================= + # --- Indicate PRN number of the detected signal ------------------- + print(f"{PRN + 1:02d} ", end="") + caCode = settings.generateCAcode(PRN) + + codeValueIndex = np.floor(ts * np.arange(1, 10 * samplesPerCode + 1) / (1.0 / settings.codeFreqBasis)) + + longCaCode = caCode[np.longlong(codeValueIndex % 1023)] + + # (Using detected C/A code phase) + xCarrier = signal0DC[codePhase:codePhase + 10 * samplesPerCode] * longCaCode + + fftNumPts = 8 * 2 ** (np.ceil(np.log2(len(xCarrier)))) + + # associated carrier frequency + fftxc = np.abs(np.fft.fft(xCarrier, np.int_(fftNumPts))) + + uniqFftPts = np.int_(np.ceil((fftNumPts + 1) / 2.0)) + + fftMax = fftxc[4:uniqFftPts - 5].max() + + fftFreqBins = np.arange(fftNumPts) * settings.samplingFreq / fftNumPts + fftMaxIndex = fftxc.argmax() + if (fftMaxIndex > (fftNumPts + 1) / 2.0): + dopFreq = fftFreqBins[fftMaxIndex] - settings.samplingFreq + else: + dopFreq = fftFreqBins[fftMaxIndex] + + carrFreq[PRN] = fftFreqBins[fftMaxIndex] + carrFreq[PRN] = dopFreq + + codePhase_[PRN] = codePhase + # savemat("results_" + str(PRN) + ".mat", {"results": results}) + # ================== Plot 3D Acquisition Result ================== + # plot_acquisition_3d(results, frqBins, PRN, settings) + + + else: + # --- No signal with this PRN -------------------------------------- + print(". ", end="") + + # === Acquisition is over ================================================== + print(')\n') + acqResults = np.core.records.fromarrays([carrFreq, codePhase_, peakMetric], + names='carrFreq,codePhase,peakMetric') + self._results = acqResults + return + + def plot(self): + assert isinstance(self._results, np.recarray) + import matplotlib as mpl + import matplotlib.pyplot as plt + # from scipy.io.matlab import loadmat + + # %% configure matplotlib + mpl.rcdefaults() + # mpl.rcParams['font.sans-serif'] + # mpl.rcParams['font.family'] = 'serif' + mpl.rc('savefig', bbox='tight', transparent=False, format='png') + mpl.rc('axes', grid=True, linewidth=1.5, axisbelow=True) + mpl.rc('lines', linewidth=1.5, solid_joinstyle='bevel') + mpl.rc('figure', figsize=[8, 6], autolayout=False, dpi=120) + mpl.rc('text', usetex=False) + mpl.rc('font', family='serif', size=16) + mpl.rc('mathtext', fontset='cm') + + # mpl.rc('font', size=16) + # mpl.rc('text.latex', preamble=r'\usepackage{cmbright}') + + # ./plotAcquisition.m + # Functions plots bar plot of acquisition results (acquisition metrics). No + # bars are shown for the satellites not included in the acquisition list (in + # structure SETTINGS). + + # plotAcquisition(acqResults) + + # Inputs: + # acqResults - Acquisition results from function acquisition. + + # Plot all results ======================================================= + f, hAxes = plt.subplots() + + plt.bar(range(1, 33), self.peakMetric) + plt.title('Acquisition results') + plt.xlabel('PRN number (no bar - SV is not in the acquisition list)') + plt.ylabel('Acquisition Metric ($1^{st}$ to $2^{nd}$ Correlation Peaks Ratio') + oldAxis = plt.axis() + + plt.axis([0, 33, 0, oldAxis[-1]]) + plt.xticks(range(1, 33), size=12) + # plt.minorticks_on() + hAxes.xaxis.grid() + # Mark acquired signals ================================================== + + acquiredSignals = self.peakMetric * (self.carrFreq > 0) + + plt.bar(range(1, 33), acquiredSignals, facecolor=(0, 0.8, 0)) + plt.legend(['Not acquired signals', 'Acquired signals']) + plt.show() + + # preRun.m + def preRun(self): + assert isinstance(self._results, np.recarray) + # Function initializes tracking channels from acquisition data. The acquired + # signals are sorted according to the signal strength. This function can be + # modified to use other satellite selection algorithms or to introduce + # acquired signal properties offsets for testing purposes. + + # [channel] = preRun(acqResults, settings) + + # Inputs: + # acqResults - results from acquisition. + # settings - receiver settings + + # Outputs: + # channel - structure contains information for each channel (like + # properties of the tracked signal, channel status etc.). + + settings = self._settings + # Initialize all channels ================================================ + PRN = np.zeros(settings.numberOfChannels, dtype='int64') + acquiredFreq = np.zeros(settings.numberOfChannels) + codePhase = np.zeros(settings.numberOfChannels) + status = ['-' for _ in range(settings.numberOfChannels)] + + # --- Copy initial data to all channels ------------------------------------ + + # Copy acquisition results =============================================== + + # --- Sort peaks to find strongest signals, keep the peak index information + PRNindexes = sorted(enumerate(self.peakMetric), + key=lambda x: x[-1], reverse=True) + + # --- Load information about each satellite -------------------------------- + # Maximum number of initialized channels is number of detected signals, but + # not more as the number of channels specified in the settings. + for ii in range(min(settings.numberOfChannels, sum(self.carrFreq > 0))): + PRN[ii] = PRNindexes[ii][0] + 1 + + acquiredFreq[ii] = self.carrFreq[PRNindexes[ii][0]] + + codePhase[ii] = self.codePhase[PRNindexes[ii][0]] + + status[ii] = 'T' + + channel = np.core.records.fromarrays([PRN, acquiredFreq, codePhase, status], + names='PRN,acquiredFreq,codePhase,status') + self._channels = channel + return + + def showChannelStatus(self): + # Prints the status of all channels in a table. + + # showChannelStatus(channel, settings) + + # Inputs: + # channel - data for each channel. It is used to initialize and + # at the processing of the signal (tracking part). + # settings - receiver settings + + channel = self._channels + settings = self._settings + assert isinstance(channel, np.recarray) + print('\n*=========*=====*===============*===========*=============*========*') + print('| Channel | PRN | Frequency | Doppler | Code Offset | Status |') + print('*=========*=====*===============*===========*=============*========*') + for channelNr in range(settings.numberOfChannels): + if channel[channelNr].status != '-': + print('| %2d | %3d | %2.5e | %5.0f | %6d | %1s |' % ( + channelNr, + channel[channelNr].PRN, + channel[channelNr].acquiredFreq, + channel[channelNr].acquiredFreq - settings.IF, + channel[channelNr].codePhase, + channel[channelNr].status)) + else: + print('| %2d | --- | ------------ | ----- | ------ | Off |' % channelNr) + + print('*=========*=====*===============*===========*=============*========*\n') + + + +def plot_acquisition_3d(results, frqBins, PRN, settings): + """ + Plot 3D surface acquisition result cho 1 PRN. + results: matrix [numberOfFrqBins x samplesPerCode] + frqBins: list tần số Doppler + """ + # Tạo grid cho code phase và Doppler + codePhases = np.arange(results.shape[1]) # 0 .. samplesPerCode-1 + dopplers = np.array(frqBins) # frequency bins + + X, Y = np.meshgrid(codePhases, dopplers) + Z = results + + # Vẽ 3D + fig = plt.figure(figsize=(10, 6)) + ax = fig.add_subplot(111, projection='3d') + + surf = ax.plot_surface(X, Y/1000.0, Z, cmap='viridis') # chia 1000 để hiện Doppler kHz + fig.colorbar(surf, shrink=0.5, aspect=5) + + ax.set_title(f'Acquisition Result PRN {PRN+1}') + ax.set_xlabel('Code Phase (samples)') + ax.set_ylabel('Doppler (kHz)') + ax.set_zlabel('Correlation Power') + + plt.show() + +if __name__ == '__main__': + pass diff --git a/acquisition_ane.py b/acquisition_ane.py new file mode 100644 index 0000000..ae7ff8d --- /dev/null +++ b/acquisition_ane.py @@ -0,0 +1,134 @@ +import numpy as np +import matplotlib.pyplot as plt +import coremltools as ct +from coremltools.models import MLModel +from initialize import Result + + +class AcquisitionResultANECoreML(Result): + """ + Run GNSS acquisition using CoreML model optimized for ANE + (cos/sin computed outside the model) + """ + def __init__(self, settings, mlpackage_path="acq_2ms_ane_v2.mlpackage"): + super().__init__(settings) + print(f"🔹 Loading CoreML model from {mlpackage_path} ...") + self.mlmodel = MLModel(mlpackage_path, compute_units=ct.ComputeUnit.CPU_ONLY) + self._results = None + self._channels = None + + def acquire(self, longSignal): + s = self._settings + N = s.samplesPerCode + F = int(round(s.acqSearchBand * 2) + 1) + + # === prepare 2 ms signals === + raw1, raw2 = longSignal[0:2*N], longSignal[2*N:4*N] + sigI1, sigQ1 = raw1[0::2], raw1[1::2] + sigI2, sigQ2 = raw2[0::2], raw2[1::2] + sig_real = np.stack([sigI1, sigI2], axis=0).astype(np.float32) + sig_imag = np.stack([sigQ1, sigQ2], axis=0).astype(np.float32) + + ts = 1.0 / s.samplingFreq + phasePoints = (np.arange(N) * 2 * np.pi * ts).astype(np.float32) + freqBins = np.linspace( + s.IF - s.acqSearchBand / 2 * 1e3, + s.IF + s.acqSearchBand / 2 * 1e3, + F, dtype=np.float32 + ) + + # === precompute cos/sin outside model === + phase_matrix = freqBins[:, None] * phasePoints[None, :] + cosFN = np.cos(phase_matrix).astype(np.float32) + sinFN = np.sin(phase_matrix).astype(np.float32) + + carrFreq = np.zeros(32) + codePhase = np.zeros(32) + peakMetric = np.zeros(32) + + print("(") + for PRN in range(len(s.acqSatelliteList)): + ca1023 = s.generateCAcode(PRN) + tc = 1.0 / s.codeFreqBasis + idx = np.ceil(ts * np.arange(1, N + 1) / tc).astype(int) - 1 + idx[-1] = 1022 + caN = ca1023[idx].astype(np.float32) + + inputs = { + "sig_real": sig_real, + "sig_imag": sig_imag, + "ca_code": caN, + "cosFN": cosFN, + "sinFN": sinFN, + } + + out_dict = self.mlmodel.predict(inputs) + powerFN = list(out_dict.values())[0] + + P = np.array(powerFN, dtype=np.float32) + # Lấy max giữa 2 ms → [F, N] + P = np.max(P, axis=0) + + peak = np.max(P) + fIdx, cIdx = np.unravel_index(np.argmax(P), P.shape) + + chipSpan = int(round(s.samplingFreq / s.codeFreqBasis)) + mask = np.ones(N, bool) + mask[max(0, cIdx - chipSpan):min(N, cIdx + chipSpan)] = False + second = np.max(P[fIdx, mask]) + ratio = peak / (second + 1e-9) + peakMetric[PRN] = ratio + + if s.acqMinThreshold < ratio < s.acqMaxThreshold: + print(f"{PRN + 1:02d} ", end="") + carrFreq[PRN] = float(freqBins[fIdx]) + codePhase[PRN] = cIdx + else: + print(". ", end="") + print(")\n") + + self._results = np.core.records.fromarrays( + [carrFreq, codePhase, peakMetric], + names="carrFreq,codePhase,peakMetric" + ) + + @staticmethod + def plot_heatmap(power, freqBins, PRN): + plt.figure(figsize=(8, 5)) + plt.imshow( + power, + origin="lower", + aspect="auto", + extent=[0, power.shape[1], freqBins[0] / 1e3, freqBins[-1] / 1e3], + cmap="viridis" + ) + plt.colorbar(label="Correlation Power") + plt.xlabel("Code Phase (samples)") + plt.ylabel("Doppler (kHz)") + plt.title(f"Acquisition Heatmap (PRN {PRN + 1}) - ANE inference") + plt.show() + + +# ================================================================ +# Quick demo +# ================================================================ +if __name__ == "__main__": + class DummySettings: + def __init__(self): + self.samplingFreq = 4.096e6 + self.codeFreqBasis = 1.023e6 + self.acqSearchBand = 14 + self.samplesPerCode = int(round(self.samplingFreq / (self.codeFreqBasis / 1023))) + self.acqSatelliteList = range(1, 5) + self.acqMinThreshold = 2.5 + self.acqMaxThreshold = 100.0 + self.IF = 0.0 + + def generateCAcode(self, prn): + np.random.seed(prn) + return 2 * (np.random.randint(0, 2, 1023) - 0.5) + + s = DummySettings() + longSignal = np.random.randn(4 * s.samplesPerCode).astype(np.float32) + acq = AcquisitionResultANECoreML(s) + acq.acquire(longSignal) diff --git a/acquisition_ane_create.py b/acquisition_ane_create.py new file mode 100644 index 0000000..51f9953 --- /dev/null +++ b/acquisition_ane_create.py @@ -0,0 +1,104 @@ +import torch +import torch.nn as nn +import numpy as np +import coremltools as ct + + +# ================================================================ +# ANE-ready Acquisition model (cos/sin moved outside the network) +# ================================================================ +class AcquisitionRealNet2ms(nn.Module): + def __init__(self, samplesPerCode: int, numBins: int): + super().__init__() + self.N = samplesPerCode + self.F = numBins + + # === Precompute real DFT matrices === + n = torch.arange(self.N).float() + k = n[:, None] + W = torch.exp(-2j * np.pi * k * n / self.N) + scale = 1024.0 # prevent fp16 underflow + + self.register_buffer("W_real", (W.real * scale).to(torch.float32)) + self.register_buffer("W_imag", (W.imag * scale).to(torch.float32)) + self.register_buffer("invN", torch.tensor(1.0 / (float(self.N) * scale), dtype=torch.float32)) + + def forward(self, sig_real, sig_imag, ca_code, cosFN, sinFN): + """ + sig_real, sig_imag: [2, N] + ca_code: [N] + cosFN, sinFN: [F, N] + Output: [F, N] + """ + B, N, F = sig_real.shape[0], self.N, cosFN.shape[0] + + # --- Doppler mixing (precomputed cos/sin) --- + sig_r = sig_real[:, None, :] # [B,1,N] + sig_i = sig_imag[:, None, :] # [B,1,N] + x_real = sig_r * cosFN[None, :, :] - sig_i * sinFN[None, :, :] + x_imag = sig_r * sinFN[None, :, :] + sig_i * cosFN[None, :, :] + + # --- FFT via MatMul --- + x_r2 = x_real.reshape(B * F, N) + x_i2 = x_imag.reshape(B * F, N) + X_real = torch.matmul(x_r2, self.W_real) - torch.matmul(x_i2, self.W_imag) + X_imag = torch.matmul(x_r2, self.W_imag) + torch.matmul(x_i2, self.W_real) + + # --- FFT(CA) and conj multiplication --- + if ca_code.ndim == 1: + ca_code = ca_code.unsqueeze(0) + CA_real = torch.matmul(ca_code, self.W_real) + CA_imag = torch.matmul(ca_code, self.W_imag) + CAc_real = CA_real.squeeze(0) + CAc_imag = -CA_imag.squeeze(0) + + conv_real = X_real * CAc_real[None, :] - X_imag * CAc_imag[None, :] + conv_imag = X_real * CAc_imag[None, :] + X_imag * CAc_real[None, :] + + # --- IFFT via MatMul --- + out_real = (torch.matmul(conv_real, self.W_real.T) + torch.matmul(conv_imag, self.W_imag.T)) * self.invN + out_imag = (-torch.matmul(conv_real, self.W_imag.T) + torch.matmul(conv_imag, self.W_real.T)) * self.invN + + # reshape and compute power + out_real = out_real.view(B, F, N) + out_imag = out_imag.view(B, F, N) + power = out_real *out_real + out_imag * out_imag + return power + # return torch.max(power, dim=0).values # [F, N] + + +# ================================================================ +# CoreML export +# ================================================================ +def export_to_coreml(samplesPerCode=8192, numBins=29, save_path="acq_2ms_ane_v2.mlpackage"): + model = AcquisitionRealNet2ms(samplesPerCode, numBins).eval() + sigR = torch.randn(2, samplesPerCode) + sigI = torch.randn(2, samplesPerCode) + ca = torch.randn(samplesPerCode) + cosFN = torch.randn(numBins, samplesPerCode) + sinFN = torch.randn(numBins, samplesPerCode) + + traced = torch.jit.trace(model, (sigR, sigI, ca, cosFN, sinFN)) + mlmodel = ct.convert( + traced, + inputs=[ + ct.TensorType(name="sig_real", shape=sigR.shape), + ct.TensorType(name="sig_imag", shape=sigI.shape), + ct.TensorType(name="ca_code", shape=ca.shape), + ct.TensorType(name="cosFN", shape=cosFN.shape), + ct.TensorType(name="sinFN", shape=sinFN.shape), + ], + convert_to="mlprogram", + compute_units=ct.ComputeUnit.ALL, + minimum_deployment_target=ct.target.macOS14, + compute_precision=ct.precision.FLOAT16, # ✅ ANE preferred + ) + mlmodel.save(save_path) + print(f"✅ Exported CoreML ANE model to {save_path}") + + +# ================================================================ +# Test export +# ================================================================ +if __name__ == "__main__": + export_to_coreml() diff --git a/acquisition_torch.py b/acquisition_torch.py new file mode 100644 index 0000000..35b67d9 --- /dev/null +++ b/acquisition_torch.py @@ -0,0 +1,192 @@ +import torch +import torch.fft as fft +import numpy as np +import matplotlib.pyplot as plt +from initialize import Result + + +class AcquisitionResultTorch(Result): + def __init__(self, settings, device="cuda"): + super().__init__(settings) + self.device = device + self._results = None + self._channels = None + + # ================================================================ + def acquire(self, longSignal): + """ + GPU/NPU-based FFT acquisition using PyTorch batch processing. + Equivalent to SoftGNSS acquisition.m but vectorized across Doppler bins. + """ + settings = self._settings + device = self.device + dtype_c = torch.complex64 + dtype_r = torch.float32 + + samplesPerCode = settings.samplesPerCode + + # --- Prepare two ms of signal (IQ interleaved) -------------------------- + rawSignal1 = longSignal[0:2 * samplesPerCode] + signalI1 = rawSignal1[0::2] + signalQ1 = rawSignal1[1::2] + signal1 = torch.tensor(signalI1 + 1j * signalQ1, dtype=dtype_c, device=device) + + rawSignal2 = longSignal[2 * samplesPerCode:4 * samplesPerCode] + signalI2 = rawSignal2[0::2] + signalQ2 = rawSignal2[1::2] + signal2 = torch.tensor(signalI2 + 1j * signalQ2, dtype=dtype_c, device=device) + + # DC removal (long segment) + longI = longSignal[0::2] + longQ = longSignal[1::2] + longIQ = torch.tensor(longI + 1j * longQ, dtype=dtype_c, device=device) + signal0DC = longIQ - torch.mean(longIQ) + + # --- Generate frequency bins ------------------------------------------- + numberOfFrqBins = int(round(settings.acqSearchBand * 2) + 1) + frqBins = torch.linspace( + settings.IF - settings.acqSearchBand / 2 * 1e3, + settings.IF + settings.acqSearchBand / 2 * 1e3, + numberOfFrqBins, device=device, dtype=dtype_r + ) + + # --- Time phase array --------------------------------------------------- + ts = 1.0 / settings.samplingFreq + phasePoints = torch.arange(samplesPerCode, device=device, dtype=dtype_r) * 2 * np.pi * ts + + # --- Generate CA code table (on CPU, then move one by one) ------------- + caCodesTable = settings.makeCaTable() + + carrFreq = np.zeros(32) + codePhase = np.zeros(32) + peakMetric = np.zeros(32) + + print("(") + for PRN in range(len(settings.acqSatelliteList)): + # --- Prepare CA code ------------------------------------------------ + caCode = torch.tensor(caCodesTable[PRN, :], dtype=dtype_r, device=device) + caCodeFreqDom = fft.fft(caCode).conj() + + # --- Generate carrier matrix [N, M] -------------------------------- + carr = torch.exp(1j * (frqBins[:, None] * phasePoints[None, :])) # (N, M) + + # --- Mix and FFT ---------------------------------------------------- + IQ1 = carr * signal1 + IQ2 = carr * signal2 + + IQfreqDom1 = fft.fft(IQ1, dim=1) + IQfreqDom2 = fft.fft(IQ2, dim=1) + + conv1 = IQfreqDom1 * caCodeFreqDom[None, :] + conv2 = IQfreqDom2 * caCodeFreqDom[None, :] + + acqRes1 = torch.abs(fft.ifft(conv1, dim=1)) ** 2 + acqRes2 = torch.abs(fft.ifft(conv2, dim=1)) ** 2 + + # --- Combine results (choose stronger between 1st & 2nd ms) -------- + results = torch.maximum(acqRes1, acqRes2) + + # --- Peak detection ------------------------------------------------- + max_per_bin, _ = torch.max(results, dim=1) + frequencyBinIndex = int(torch.argmax(max_per_bin).cpu()) + peakSize = float(torch.max(results).cpu()) + codePhaseIdx = int(torch.argmax(torch.max(results, dim=0).values).cpu()) + + samplesPerCodeChip = int(round(settings.samplingFreq / settings.codeFreqBasis)) + exclude1 = codePhaseIdx - samplesPerCodeChip + exclude2 = codePhaseIdx + samplesPerCodeChip + + if exclude1 <= 0: + codePhaseRange = torch.arange(exclude2, samplesPerCode + exclude1 + 1) % samplesPerCode + elif exclude2 >= samplesPerCode - 1: + codePhaseRange = torch.arange(exclude2 - samplesPerCode, exclude1) % samplesPerCode + else: + codePhaseRange = torch.cat([ + torch.arange(0, exclude1 + 1), + torch.arange(exclude2, samplesPerCode) + ]) + + secondPeakSize = float(results[frequencyBinIndex, codePhaseRange.long()].max().cpu()) + ratio = peakSize / (secondPeakSize + 1e-12) + + peakMetric[PRN] = ratio + + if settings.acqMinThreshold < ratio < settings.acqMaxThreshold: + print(f"{PRN + 1:02d} ", end="") + + # --- Fine Doppler refinement using 10 ms ------------------------ + caCode_cpu = settings.generateCAcode(PRN) + caCode_torch = torch.tensor(caCode_cpu, dtype=dtype_r, device=device) + + codeValueIndex = torch.floor( + ts * torch.arange(1, 10 * samplesPerCode + 1, device=device) / (1.0 / settings.codeFreqBasis) + ).long() % 1023 + + longCaCode = caCode_torch[codeValueIndex] + + xCarrier = signal0DC[codePhaseIdx:codePhaseIdx + 10 * samplesPerCode] * longCaCode + + fftNumPts = int(8 * 2 ** np.ceil(np.log2(len(xCarrier)))) + fftxc = torch.abs(fft.fft(xCarrier, fftNumPts)) # spectrum + fftMaxIdx = int(torch.argmax(fftxc).cpu()) + + fftFreqBins = torch.arange(fftNumPts, device=device, dtype=dtype_r) * settings.samplingFreq / fftNumPts + dopFreq = float(fftFreqBins[fftMaxIdx].cpu()) + if fftMaxIdx > (fftNumPts + 1) / 2.0: + dopFreq -= settings.samplingFreq + + carrFreq[PRN] = dopFreq + codePhase[PRN] = codePhaseIdx + + # self.plot_acquisition_3d(results.detach().cpu().numpy(), + # frqBins.detach().cpu().numpy(), + # PRN, settings) + else: + print(". ", end="") + + print(")\n") + + acqResults = np.core.records.fromarrays( + [carrFreq, codePhase, peakMetric], + names="carrFreq,codePhase,peakMetric" + ) + self._results = acqResults + return + + # ================================================================ + def plot(self): + """Bar plot of acquisition metric for all PRNs.""" + assert isinstance(self._results, np.recarray) + plt.figure() + plt.bar(range(1, 33), self._results.peakMetric) + plt.title("Acquisition Results (GPU-PyTorch)") + plt.xlabel("PRN Number") + plt.ylabel("Peak Metric (1st/2nd)") + plt.grid(True) + plt.show() + + # ================================================================ + @staticmethod + def plot_acquisition_3d(results, frqBins, PRN, settings): + """ + Plot 3D surface acquisition result for one PRN. + results: matrix [numberOfFrqBins x samplesPerCode] + frqBins: list of Doppler frequencies (Hz) + """ + from mpl_toolkits.mplot3d import Axes3D # noqa: F401 + + codePhases = np.arange(results.shape[1]) + dopplers = np.array(frqBins) + X, Y = np.meshgrid(codePhases, dopplers) + Z = results + + fig = plt.figure(figsize=(10, 6)) + ax = fig.add_subplot(111, projection='3d') + surf = ax.plot_surface(X, Y / 1000.0, Z, cmap='viridis', linewidth=0, antialiased=False) + fig.colorbar(surf, shrink=0.5, aspect=5) + + ax.set_title(f'Acquisition Surface (PRN {PRN + 1})') + ax.set_xlabel('Code Phase (samples)') + ax.set_ylabel('Doppler (kHz)') + ax.set_zlabel('Correlation Power') + plt.show() diff --git a/ephemeris.py b/ephemeris.py new file mode 100644 index 0000000..2a0e25d --- /dev/null +++ b/ephemeris.py @@ -0,0 +1,195 @@ +def bin2dec(binaryStr): + assert isinstance(binaryStr, str) + return int(binaryStr, 2) + + +# twosComp2dec.m +def twosComp2dec(binaryStr): + # TWOSCOMP2DEC(binaryNumber) Converts a two's-complement binary number + # BINNUMBER (in Matlab it is a string type), represented as a row vector of + # zeros and ones, to an integer. + + # intNumber = twosComp2dec(binaryNumber) + + # --- Check if the input is string ----------------------------------------- + if not isinstance(binaryStr, str): + raise IOError('Input must be a string.') + + # --- Convert from binary form to a decimal number ------------------------- + intNumber = int(binaryStr, 2) + + # --- If the number was negative, then correct the result ------------------ + if binaryStr[0] == '1': + intNumber -= 2 ** len(binaryStr) + return intNumber + + +# checkPhase.m + + +def checkPhase(word, d30star): + # Checks the parity of the supplied 30bit word. + # The last parity bit of the previous word is used for the calculation. + # A note on the procedure is supplied by the GPS standard positioning + # service signal specification. + + # word = checkPhase(word, D30Star) + + # Inputs: + # word - an array with 30 bit long word from the navigation + # message (a character array, must contain only '0' or + # '1'). + # D30Star - the last bit of the previous word (char type). + + # Outputs: + # word - word with corrected polarity of the data bits + # (character array). + + word_new = [] + if d30star == '1': + # Data bits must be inverted + for i in range(0, 24): + if word[i] == '1': + word[i] = '0' + elif word[i] == '0': + word[i] = '1' + return word + + +# ephemeris.m +def ephemeris(bits, d30star): + # Function decodes ephemerides and TOW from the given bit stream. The stream + # (array) in the parameter BITS must contain 1500 bits. The first element in + # the array must be the first bit of a subframe. The subframe ID of the + # first subframe in the array is not important. + + # Function does not check parity! + + # [eph, TOW] = ephemeris(bits, D30Star) + + # Inputs: + # bits - bits of the navigation messages (5 subframes). + # Type is character array and it must contain only + # characters '0' or '1'. + # D30Star - The last bit of the previous nav-word. Refer to the + # GPS interface control document ICD (IS-GPS-200D) for + # more details on the parity checking. Parameter type is + # char. It must contain only characters '0' or '1'. + # Outputs: + # TOW - Time Of Week (TOW) of the first sub-frame in the bit + # stream (in seconds) + # eph - SV ephemeris + + # Check if there is enough data ========================================== + if len(bits) < 1500: + raise TypeError('The parameter BITS must contain 1500 bits!') + + # Check if the parameters are strings ==================================== + if any([not isinstance(x, str) for x in bits]): + raise TypeError('The parameter BITS must be a character array!') + + if not isinstance(d30star, str): + raise TypeError('The parameter D30Star must be a char!') + + # Pi used in the GPS coordinate system + gpsPi = 3.1415926535898 + + # Decode all 5 sub-frames ================================================ + for i in range(5): + # --- "Cut" one sub-frame's bits --------------------------------------- + subframe = bits[300 * i:300 * (i + 1)] + + for j in range(10): + subframe[30 * j: 30 * (j + 1)] = checkPhase(subframe[30 * j: 30 * (j + 1)], d30star) + + d30star = subframe[30 * (j + 1) - 1] + + # --- Decode the sub-frame id ------------------------------------------ + # For more details on sub-frame contents please refer to GPS IS. + subframe = ''.join(subframe) + subframeID = bin2dec(subframe[49:52]) + + # The task is to select the necessary bits and convert them to decimal + # numbers. For more details on sub-frame contents please refer to GPS + # ICD (IS-GPS-200D). + if 1 == subframeID: + # It contains WN, SV clock corrections, health and accuracy + weekNumber = bin2dec(subframe[60:70]) + 1024 + + accuracy = bin2dec(subframe[72:76]) + + health = bin2dec(subframe[76:82]) + + T_GD = twosComp2dec(subframe[195:204]) * 2 ** (- 31) + + IODC = bin2dec(subframe[82:84] + subframe[196:204]) + + t_oc = bin2dec(subframe[218:234]) * 2 ** 4 + + a_f2 = twosComp2dec(subframe[240:248]) * 2 ** (- 55) + + a_f1 = twosComp2dec(subframe[248:264]) * 2 ** (- 43) + + a_f0 = twosComp2dec(subframe[270:292]) * 2 ** (- 31) + + elif 2 == subframeID: + # It contains first part of ephemeris parameters + IODE_sf2 = bin2dec(subframe[60:68]) + + C_rs = twosComp2dec(subframe[68:84]) * 2 ** (- 5) + + deltan = twosComp2dec(subframe[90:106]) * 2 ** (- 43) * gpsPi + + M_0 = twosComp2dec(subframe[106:114] + subframe[120:144]) * 2 ** (- 31) * gpsPi + + C_uc = twosComp2dec(subframe[150:166]) * 2 ** (- 29) + + e = bin2dec(subframe[166:174] + subframe[180:204]) * 2 ** (- 33) + + C_us = twosComp2dec(subframe[210:226]) * 2 ** (- 29) + + sqrtA = bin2dec(subframe[226:234] + subframe[240:264]) * 2 ** (- 19) + + t_oe = bin2dec(subframe[270:286]) * 2 ** 4 + + elif 3 == subframeID: + # It contains second part of ephemeris parameters + C_ic = twosComp2dec(subframe[60:76]) * 2 ** (- 29) + + omega_0 = twosComp2dec(subframe[76:84] + subframe[90:114]) * 2 ** (- 31) * gpsPi + + C_is = twosComp2dec(subframe[120:136]) * 2 ** (- 29) + + i_0 = twosComp2dec(subframe[136:144] + subframe[150:174]) * 2 ** (- 31) * gpsPi + + C_rc = twosComp2dec(subframe[180:196]) * 2 ** (- 5) + + omega = twosComp2dec(subframe[196:204] + subframe[210:234]) * 2 ** (- 31) * gpsPi + + omegaDot = twosComp2dec(subframe[240:264]) * 2 ** (- 43) * gpsPi + + IODE_sf3 = bin2dec(subframe[270:278]) + + iDot = twosComp2dec(subframe[278:292]) * 2 ** (- 43) * gpsPi + + elif 4 == subframeID: + # Almanac, ionospheric model, UTC parameters. + # SV health (PRN: 25-32). + # Not decoded at the moment. + pass + elif 5 == subframeID: + # SV almanac and health (PRN: 1-24). + # Almanac reference week number and time. + # Not decoded at the moment. + pass + + # Compute the time of week (TOW) of the first sub-frames in the array ==== + # Also correct the TOW. The transmitted TOW is actual TOW of the next + # subframe and we need the TOW of the first subframe in this data block + # (the variable subframe at this point contains bits of the last subframe). + TOW = bin2dec(subframe[30:47]) * 6 - 30 + # Initialize fields for ephemeris + eph = (weekNumber, accuracy, health, T_GD, IODC, t_oc, a_f2, a_f1, a_f0, + IODE_sf2, C_rs, deltan, M_0, C_uc, e, C_us, sqrtA, t_oe, + C_ic, omega_0, C_is, i_0, C_rc, omega, omegaDot, IODE_sf3, iDot) + return eph, TOW diff --git a/fft.py b/fft.py new file mode 100644 index 0000000..c2ee874 --- /dev/null +++ b/fft.py @@ -0,0 +1,28 @@ +import numpy as np +import time + +# Kích thước ma trận +N = 256 +M = 256 + +# === Tạo tín hiệu 2D (ví dụ: sóng sin theo hai chiều) === +x = np.zeros((N, M)) +for i in range(N): + for j in range(M): + x[i, j] = np.sin(2 * np.pi * 8 * i / N) + np.sin(2 * np.pi * 5 * j / M) + +# === FFT 2D === +t0 = time.time() +X = np.fft.fft2(x) +t1 = time.time() +print(f"FFT 2D time: {(t1 - t0) * 1e3:.2f} ms") + +# === IFFT 2D === +t2 = time.time() +x_rec = np.fft.ifft2(X) +t3 = time.time() +print(f"IFFT 2D time: {(t3 - t2) * 1e3:.2f} ms") + +# === Kiểm tra độ chính xác === +error = np.max(np.abs(x - x_rec.real)) +print(f"Max reconstruction error: {error:.3e}") diff --git a/geoFunctions/__init__.py b/geoFunctions/__init__.py new file mode 100644 index 0000000..0c73686 --- /dev/null +++ b/geoFunctions/__init__.py @@ -0,0 +1,1195 @@ +import numpy as np + + +# cart2geo.m + + +def cart2geo(X, Y, Z, i, *args, **kwargs): + # CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical + # coordinates (phi, lambda, h) on a selected reference ellipsoid. + + # [phi, lambda, h] = cart2geo(X, Y, Z, i); + + # Choices i of Reference Ellipsoid for Geographical Coordinates + # 1. International Ellipsoid 1924 + # 2. International Ellipsoid 1967 + # 3. World Geodetic System 1972 + # 4. Geodetic Reference System 1980 + # 5. World Geodetic System 1984 + + # Kai Borre 10-13-98 + # Copyright (c) by Kai Borre + # Revision: 1.0 Date: 1998/10/23 + + # ========================================================================== + + a = np.array([6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0]) + + f = np.array([1 / 297, 1 / 298.247, 1 / 298.26, 1 / 298.257222101, 1 / 298.257223563]) + + lambda_ = np.arctan2(Y, X) + + ex2 = (2 - f[i]) * f[i] / ((1 - f[i]) ** 2) + + c = a[i] * np.sqrt(1 + ex2) + + phi = np.arctan(Z / (np.sqrt(X ** 2 + Y ** 2) * (1 - (2 - f[i])) * f[i])) + + h = 0.1 + + oldh = 0 + + iterations = 0 + + while abs(h - oldh) > 1e-12: + + oldh = h + + N = c / np.sqrt(1 + ex2 * np.cos(phi) ** 2) + + phi = np.arctan(Z / (np.sqrt(X ** 2 + Y ** 2) * (1 - (2 - f[i]) * f[i] * N / (N + h)))) + + h = np.sqrt(X ** 2 + Y ** 2) / np.cos(phi) - N + + iterations += 1 + + if iterations > 100: + print ('Failed to approximate h with desired precision. h-oldh: %e.' % (h - oldh)) + break + + phi *= (180 / np.pi) + + # b = zeros(1,3); + # b(1,1) = fix(phi); + # b(2,1) = fix(rem(phi,b(1,1))*60); + # b(3,1) = (phi-b(1,1)-b(1,2)/60)*3600; + + lambda_ *= (180 / np.pi) + + # l = zeros(1,3); + # l(1,1) = fix(lambda); + # l(2,1) = fix(rem(lambda,l(1,1))*60); + # l(3,1) = (lambda-l(1,1)-l(1,2)/60)*3600; + + # fprintf('\n phi =#3.0f #3.0f #8.5f',b(1),b(2),b(3)) + # fprintf('\n lambda =#3.0f #3.0f #8.5f',l(1),l(2),l(3)) + # fprintf('\n h =#14.3f\n',h) + return phi, lambda_, h + + +############## end cart2geo.m ################### + + +# clsin.m +def clsin(ar, degree, argument, *args, **kwargs): + # Clenshaw summation of sinus of argument. + + # result = clsin(ar, degree, argument); + + # Written by Kai Borre + # December 20, 1995 + + # See also WGS2UTM or CART2UTM + + # ========================================================================== + + cos_arg = 2 * np.cos(argument) + + hr1 = 0 + + hr = 0 + + # TODO fix index range of t + for t in range(degree, 0, -1): + hr2 = hr1 + + hr1 = hr + + hr = ar[t - 1] + cos_arg * hr1 - hr2 + + result = hr * np.sin(argument) + return result + + +####################### end clsin.m ##################### + + +# clksin.m +def clksin(ar, degree, arg_real, arg_imag, *args, **kwargs): + # Clenshaw summation of sinus with complex argument + # [re, im] = clksin(ar, degree, arg_real, arg_imag); + + # Written by Kai Borre + # December 20, 1995 + + # See also WGS2UTM or CART2UTM + + # ========================================================================== + + sin_arg_r = np.sin(arg_real) + + cos_arg_r = np.cos(arg_real) + + sinh_arg_i = np.sinh(arg_imag) + + cosh_arg_i = np.cosh(arg_imag) + + r = 2 * cos_arg_r * cosh_arg_i + + i = - 2 * sin_arg_r * sinh_arg_i + + hr1 = 0 + + hr = 0 + + hi1 = 0 + + hi = 0 + + # TODO fix index range of t + for t in range(degree, 0, - 1): + hr2 = hr1 + + hr1 = hr + + hi2 = hi1 + + hi1 = hi + + z = ar[t - 1] + r * hr1 - i * hi - hr2 + + hi = i * hr1 + r * hi1 - hi2 + + hr = z + + r = sin_arg_r * cosh_arg_i + + i = cos_arg_r * sinh_arg_i + + re = r * hr - i * hi + + im = r * hi + i * hr + return re, im + + +# cart2utm.m +def cart2utm(X, Y, Z, zone, *args, **kwargs): + # CART2UTM Transformation of (X,Y,Z) to (N,E,U) in UTM, zone 'zone'. + + # [E, N, U] = cart2utm(X, Y, Z, zone); + + # Inputs: + # X,Y,Z - Cartesian coordinates. Coordinates are referenced + # with respect to the International Terrestrial Reference + # Frame 1996 (ITRF96) + # zone - UTM zone of the given position + + # Outputs: + # E, N, U - UTM coordinates (Easting, Northing, Uping) + + # Kai Borre -11-1994 + # Copyright (c) by Kai Borre + + # This implementation is based upon + # O. Andersson & K. Poder (1981) Koordinattransformationer + # ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren + # Vol. 30: 552--571 and Vol. 31: 76 + + # An excellent, general reference (KW) is + # R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der + # h\"oheren Geod\"asie und Kartographie. + # Erster Band, Springer Verlag + + # Explanation of variables used: + # f flattening of ellipsoid + # a semi major axis in m + # m0 1 - scale at central meridian; for UTM 0.0004 + # Q_n normalized meridian quadrant + # E0 Easting of central meridian + # L0 Longitude of central meridian + # bg constants for ellipsoidal geogr. to spherical geogr. + # gb constants for spherical geogr. to ellipsoidal geogr. + # gtu constants for ellipsoidal N, E to spherical N, E + # utg constants for spherical N, E to ellipoidal N, E + # tolutm tolerance for utm, 1.2E-10*meridian quadrant + # tolgeo tolerance for geographical, 0.00040 second of arc + + # B, L refer to latitude and longitude. Southern latitude is negative + # International ellipsoid of 1924, valid for ED50 + + a = 6378388.0 + + f = 1.0 / 297.0 + + ex2 = (2 - f) * f / (1 - f) ** 2 + + c = a * np.sqrt(1 + ex2) + + vec = np.array([X, Y, Z - 4.5]) + + alpha = 7.56e-07 + + R = np.array([[1, - alpha, 0], + [alpha, 1, 0], + [0, 0, 1]]) + + trans = np.array([89.5, 93.8, 127.6]) + + scale = 0.9999988 + + v = scale * R.dot(vec) + trans + + L = np.arctan2(v[1], v[0]) + + N1 = 6395000.0 + + B = np.arctan2(v[2] / ((1 - f) ** 2 * N1), np.linalg.norm(v[0:2]) / N1) + + U = 0.1 + + oldU = 0 + + iterations = 0 + + while abs(U - oldU) > 0.0001: + + oldU = U + + N1 = c / np.sqrt(1 + ex2 * (np.cos(B)) ** 2) + + B = np.arctan2(v[2] / ((1 - f) ** 2 * N1 + U), np.linalg.norm(v[0:2]) / (N1 + U)) + + U = np.linalg.norm(v[0:2]) / np.cos(B) - N1 + + iterations += 1 + + if iterations > 100: + print ('Failed to approximate U with desired precision. U-oldU: %e.' % (U - oldU)) + break + + # Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21) + m0 = 0.0004 + + n = f / (2 - f) + + m = n ** 2 * (1.0 / 4.0 + n ** 2 / 64) + + w = (a * (-n - m0 + m * (1 - m0))) / (1 + n) + + Q_n = a + w + + # Easting and longitude of central meridian + E0 = 500000.0 + + L0 = (zone - 30) * 6 - 3 + + # Check tolerance for reverse transformation + tolutm = np.pi / 2 * 1.2e-10 * Q_n + + tolgeo = 4e-05 + + # Coefficients of trigonometric series + + # ellipsoidal to spherical geographical, KW p. 186--187, (51)-(52) + # bg[1] = n*(-2 + n*(2/3 + n*(4/3 + n*(-82/45)))); + # bg[2] = n^2*(5/3 + n*(-16/15 + n*(-13/9))); + # bg[3] = n^3*(-26/15 + n*34/21); + # bg[4] = n^4*1237/630; + + # spherical to ellipsoidal geographical, KW p. 190--191, (61)-(62) + # gb[1] = n*(2 + n*(-2/3 + n*(-2 + n*116/45))); + # gb[2] = n^2*(7/3 + n*(-8/5 + n*(-227/45))); + # gb[3] = n^3*(56/15 + n*(-136/35)); + # gb[4] = n^4*4279/630; + + # spherical to ellipsoidal N, E, KW p. 196, (69) + # gtu[1] = n*(1/2 + n*(-2/3 + n*(5/16 + n*41/180))); + # gtu[2] = n^2*(13/48 + n*(-3/5 + n*557/1440)); + # gtu[3] = n^3*(61/240 + n*(-103/140)); + # gtu[4] = n^4*49561/161280; + + # ellipsoidal to spherical N, E, KW p. 194, (65) + # utg[1] = n*(-1/2 + n*(2/3 + n*(-37/96 + n*1/360))); + # utg[2] = n^2*(-1/48 + n*(-1/15 + n*437/1440)); + # utg[3] = n^3*(-17/480 + n*37/840); + # utg[4] = n^4*(-4397/161280); + + # With f = 1/297 we get + + bg = np.array([- 0.00337077907, 4.73444769e-06, -8.2991457e-09, 1.5878533e-11]) + + gb = np.array([0.00337077588, 6.6276908e-06, 1.78718601e-08, 5.49266312e-11]) + + gtu = np.array([0.000841275991, 7.67306686e-07, 1.2129123e-09, 2.48508228e-12]) + + utg = np.array([-0.000841276339, -5.95619298e-08, -1.69485209e-10, -2.20473896e-13]) + + # Ellipsoidal latitude, longitude to spherical latitude, longitude + neg_geo = False + + if B < 0: + neg_geo = True + + Bg_r = np.abs(B) + + res_clensin = clsin(bg, 4, 2 * Bg_r) + + Bg_r = Bg_r + res_clensin + + L0 = L0 * np.pi / 180 + + Lg_r = L - L0 + + # Spherical latitude, longitude to complementary spherical latitude + # i.e. spherical N, E + cos_BN = np.cos(Bg_r) + + Np = np.arctan2(np.sin(Bg_r), np.cos(Lg_r) * cos_BN) + + Ep = np.arctanh(np.sin(Lg_r) * cos_BN) + + # Spherical normalized N, E to ellipsoidal N, E + Np *= 2 + + Ep *= 2 + + dN, dE = clksin(gtu, 4, Np, Ep) + + Np /= 2 + + Ep /= 2 + + Np += dN + + Ep += dE + + N = Q_n * Np + + E = Q_n * Ep + E0 + + if neg_geo: + N = -N + 20000000 + return E, N, U + + +#################### end cart2utm.m #################### + + +# deg2dms.m +def deg2dms(deg, *args, **kwargs): + # DEG2DMS Conversion of degrees to degrees, minutes, and seconds. + # The output format (dms format) is: (degrees*100 + minutes + seconds/100) + + # Written by Kai Borre + # February 7, 2001 + # Updated by Darius Plausinaitis + + # Save the sign for later processing + neg_arg = False + + if deg < 0: + # Only positive numbers should be used while spliting into deg/min/sec + deg = -deg + + neg_arg = True + + # Split degrees minutes and seconds + int_deg = np.floor(deg) + + decimal = deg - int_deg + + min_part = decimal * 60 + + min_ = np.floor(min_part) + + sec_part = min_part - np.floor(min_part) + + sec = sec_part * 60 + + # Check for overflow + if sec == 60.0: + min_ = min_ + 1 + + sec = 0.0 + + if min_ == 60.0: + int_deg = int_deg + 1 + + min_ = 0.0 + + # Construct the output + dmsOutput = int_deg * 100 + min_ + sec / 100 + + # Correct the sign + if neg_arg: + dmsOutput = -dmsOutput + return dmsOutput + + +################### end deg2dms.m ################ + + +# dms2mat.m +def dms2mat(dmsInput, n, *args, **kwargs): + # DMS2MAT Splits a real a = dd*100 + mm + s/100 into[dd mm s.ssss] + # where n specifies the power of 10, to which the resulting seconds + # of the output should be rounded. E.g.: if a result is 23.823476 + # seconds, and n = -3, then the output will be 23.823. + + # Written by Kai Borre + # January 7, 2007 + # Updated by Darius Plausinaitis + + neg_arg = False + + if dmsInput < 0: + # Only positive numbers should be used while spliting into deg/min/sec + dmsInput = -dmsInput + + neg_arg = True + + # Split degrees minutes and seconds + int_deg = np.floor(dmsInput / 100) + + mm = np.floor(dmsInput - 100 * int_deg) + + # we assume n<7; hence #2.10f is sufficient to hold ssdec + ssdec = '%2.10f' % (dmsInput - 100 * int_deg - mm) * 100 + + # Check for overflow + if ssdec == 60.0: + mm = mm + 1 + + ssdec = 0.0 + + if mm == 60.0: + int_deg = int_deg + 1 + + mm = 0.0 + + # Corect the sign + if neg_arg: + int_deg = -int_deg + + # Compose the output + matOutput = [] + matOutput[0] = int_deg + + matOutput[1] = mm + + matOutput[2] = float(ssdec[0:- n + 3]) + + return matOutput + + +################### end dms2mat.m ################ + + +# e_r_corr.m + + +def e_r_corr(traveltime, X_sat, *args, **kwargs): + # E_R_CORR Returns rotated satellite ECEF coordinates due to Earth + # rotation during signal travel time + + # X_sat_rot = e_r_corr(traveltime, X_sat); + + # Inputs: + # travelTime - signal travel time + # X_sat - satellite's ECEF coordinates + + # Outputs: + # X_sat_rot - rotated satellite's coordinates (ECEF) + + # Written by Kai Borre + # Copyright (c) by Kai Borre + + # ========================================================================== + + Omegae_dot = 7.292115147e-05 + + # --- Find rotation angle -------------------------------------------------- + omegatau = Omegae_dot * traveltime + + # --- Make a rotation matrix ----------------------------------------------- + R3 = np.array([[np.cos(omegatau), np.sin(omegatau), 0.0], + [-np.sin(omegatau), np.cos(omegatau), 0.0], + [0.0, 0.0, 1.0]]) + + # --- Do the rotation ------------------------------------------------------ + X_sat_rot = R3.dot(X_sat) + return X_sat_rot + + +######## end e_r_corr.m #################### + +# findUtmZone.m + + +def findUtmZone(latitude, longitude, *args, **kwargs): + # Function finds the UTM zone number for given longitude and latitude. + # The longitude value must be between -180 (180 degree West) and 180 (180 + # degree East) degree. The latitude must be within -80 (80 degree South) and + # 84 (84 degree North). + + # utmZone = findUtmZone(latitude, longitude); + + # Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not + # 15 deg 30 min). + + # Check value bounds ===================================================== + + if longitude > 180 or longitude < - 180: + raise IOError('Longitude value exceeds limits (-180:180).') + + if latitude > 84 or latitude < - 80: + raise IOError('Latitude value exceeds limits (-80:84).') + + # Find zone ============================================================== + + # Start at 180 deg west = -180 deg + + utmZone = np.fix((180 + longitude) / 6) + 1 + + # Correct zone numbers for particular areas ============================== + + if latitude > 72: + # Corrections for zones 31 33 35 37 + if 0 <= longitude < 9: + utmZone = 31 + + elif 9 <= longitude < 21: + utmZone = 33 + + elif 21 <= longitude < 33: + utmZone = 35 + + elif 33 <= longitude < 42: + utmZone = 37 + + elif 56 <= latitude < 64: + # Correction for zone 32 + if 3 <= longitude < 12: + utmZone = 32 + return utmZone + + +# geo2cart.m +def geo2cart(phi, lambda_, h, i=4, *args, **kwargs): + # GEO2CART Conversion of geographical coordinates (phi, lambda, h) to + # Cartesian coordinates (X, Y, Z). + + # [X, Y, Z] = geo2cart(phi, lambda, h, i); + + # Format for phi and lambda: [degrees minutes seconds]. + # h, X, Y, and Z are in meters. + + # Choices i of Reference Ellipsoid + # 1. International Ellipsoid 1924 + # 2. International Ellipsoid 1967 + # 3. World Geodetic System 1972 + # 4. Geodetic Reference System 1980 + # 5. World Geodetic System 1984 + + # Inputs: + # phi - geocentric latitude (format [degrees minutes seconds]) + # lambda - geocentric longitude (format [degrees minutes seconds]) + # h - height + # i - reference ellipsoid type + + # Outputs: + # X, Y, Z - Cartesian coordinates (meters) + + # Kai Borre 10-13-98 + # Copyright (c) by Kai Borre + + # ========================================================================== + + b = phi[0] + phi[1] / 60. + phi[2] / 3600. + + b = b * np.pi / 180. + + l = lambda_[1] + lambda_[2] / 60. + lambda_[3] / 3600. + + l = l * np.pi / 180. + + a = [6378388, 6378160, 6378135, 6378137, 6378137] + + f = [1 / 297, 1 / 298.247, 1 / 298.26, 1 / 298.257222101, 1 / 298.257223563] + + ex2 = (2. - f[i]) * f[i] / (1. - f[i]) ** 2 + + c = a[i] * np.sqrt(1. + ex2) + + N = c / np.sqrt(1. + ex2 * np.cos(b) ** 2) + + X = (N + h) * np.cos(b) * np.cos(l) + + Y = (N + h) * np.cos(b) * np.sin(l) + + Z = ((1. - f[i]) ** 2 * N + h) * np.sin(b) + + return X, Y, Z + + +# leastSquarePos.m +def leastSquarePos(satpos_, obs, settings, *args, **kwargs): + # Function calculates the Least Square Solution. + + # [pos, el, az, dop] = leastSquarePos(satpos, obs, settings); + + # Inputs: + # satpos - Satellites positions (in ECEF system: [X; Y; Z;] - + # one column per satellite) + # obs - Observations - the pseudorange measurements to each + # satellite: + # (e.g. [20000000 21000000 .... .... .... .... ....]) + # settings - receiver settings + + # Outputs: + # pos - receiver position and receiver clock error + # (in ECEF system: [X, Y, Z, dt]) + # el - Satellites elevation angles (degrees) + # az - Satellites azimuth angles (degrees) + # dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP]) + + # === Initialization ======================================================= + nmbOfIterations = 7 + + dtr = np.pi / 180 + + pos = np.zeros(4) + + X = satpos_.copy() + + nmbOfSatellites = satpos_.shape[1] + + A = np.zeros((nmbOfSatellites, 4)) + + omc = np.zeros(nmbOfSatellites) + + az = np.zeros(nmbOfSatellites) + + el = np.zeros(nmbOfSatellites) + + dop = np.zeros(5) + # === Iteratively find receiver position =================================== + for iter_ in range(nmbOfIterations): + for i in range(nmbOfSatellites): + if iter_ == 0: + # --- Initialize variables at the first iteration -------------- + Rot_X = X[:, i].copy() + + trop = 2 + + else: + # --- Update equations ----------------------------------------- + rho2 = (X[0, i] - pos[0]) ** 2 + (X[1, i] - pos[1]) ** 2 + (X[2, i] - pos[2]) ** 2 + + traveltime = np.sqrt(rho2) / settings.c + + Rot_X = e_r_corr(traveltime, X[:, i]) + + az[i], el[i], dist = topocent(pos[0:3], Rot_X - pos[0:3]) + + if settings.useTropCorr: + # --- Calculate tropospheric correction -------------------- + trop = tropo(np.sin(el[i] * dtr), 0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0) + + else: + # Do not calculate or apply the tropospheric corrections + trop = 0 + + # --- Apply the corrections ---------------------------------------- + omc[i] = obs[i] - np.linalg.norm(Rot_X - pos[0:3]) - pos[3] - trop + + A[i, :] = np.array([-(Rot_X[0] - pos[0]) / obs[i], + -(Rot_X[1] - pos[1]) / obs[i], + -(Rot_X[2] - pos[2]) / obs[i], + 1]) + + # These lines allow the code to exit gracefully in case of any errors + if np.linalg.matrix_rank(A) != 4: + pos = np.zeros((4, 1)) + + return pos, el, az, dop + # --- Find position update --------------------------------------------- + x = np.linalg.lstsq(A, omc, rcond=None)[0] + + pos = pos + x.flatten() + + pos = pos.T + + # === Calculate Dilution Of Precision ====================================== + # --- Initialize output ------------------------------------------------ + # dop = np.zeros((1, 5)) + + Q = np.linalg.inv(A.T.dot(A)) + + dop[0] = np.sqrt(np.trace(Q)) + + dop[1] = np.sqrt(Q[0, 0] + Q[1, 1] + Q[2, 2]) + + dop[2] = np.sqrt(Q[0, 0] + Q[1, 1]) + + dop[3] = np.sqrt(Q[2, 2]) + + dop[4] = np.sqrt(Q[3, 3]) + + return pos, el, az, dop + + +# check_t.m + + +def check_t(time, *args, **kwargs): + # CHECK_T accounting for beginning or end of week crossover. + + # corrTime = check_t(time); + + # Inputs: + # time - time in seconds + + # Outputs: + # corrTime - corrected time (seconds) + + # Kai Borre 04-01-96 + # Copyright (c) by Kai Borre + + # ========================================================================== + + half_week = 302400.0 + + corrTime = time + + if time > half_week: + corrTime = time - 2 * half_week + + elif time < - half_week: + corrTime = time + 2 * half_week + return corrTime + + +####### end check_t.m ################# + + +# satpos.m + + +def satpos(transmitTime, prnList, eph, settings, *args, **kwargs): + # SATPOS Computation of satellite coordinates X,Y,Z at TRANSMITTIME for + # given ephemeris EPH. Coordinates are computed for each satellite in the + # list PRNLIST. + # [satPositions, satClkCorr] = satpos(transmitTime, prnList, eph, settings); + + # Inputs: + # transmitTime - transmission time + # prnList - list of PRN-s to be processed + # eph - ephemerides of satellites + # settings - receiver settings + + # Outputs: + # satPositions - position of satellites (in ECEF system [X; Y; Z;]) + # satClkCorr - correction of satellite clocks + + # Initialize constants =================================================== + numOfSatellites = prnList.size + + # GPS constatns + + gpsPi = 3.14159265359 + + # system + + # --- Constants for satellite position calculation ------------------------- + Omegae_dot = 7.2921151467e-05 + + GM = 3.986005e+14 + + # the mass of the Earth, [m^3/s^2] + F = - 4.442807633e-10 + + # Initialize results ===================================================== + satClkCorr = np.zeros(numOfSatellites) + + satPositions = np.zeros((3, numOfSatellites)) + + # Process each satellite ================================================= + + for satNr in range(numOfSatellites): + prn = prnList[satNr] - 1 + + # Find initial satellite clock correction -------------------------------- + # --- Find time difference --------------------------------------------- + dt = check_t(transmitTime - eph[prn].t_oc) + + satClkCorr[satNr] = (eph[prn].a_f2 * dt + eph[prn].a_f1) * dt + eph[prn].a_f0 - eph[prn].T_GD + + time = transmitTime - satClkCorr[satNr] + + # Find satellite's position ---------------------------------------------- + # Restore semi-major axis + a = eph[prn].sqrtA * eph[prn].sqrtA + + tk = check_t(time - eph[prn].t_oe) + + n0 = np.sqrt(GM / a ** 3) + + n = n0 + eph[prn].deltan + + M = eph[prn].M_0 + n * tk + + M = np.remainder(M + 2 * gpsPi, 2 * gpsPi) + + E = M + + for ii in range(10): + E_old = E + + E = M + eph[prn].e * np.sin(E) + + dE = np.remainder(E - E_old, 2 * gpsPi) + + if abs(dE) < 1e-12: + # Necessary precision is reached, exit from the loop + break + # Reduce eccentric anomaly to between 0 and 360 deg + E = np.remainder(E + 2 * gpsPi, 2 * gpsPi) + + dtr = F * eph[prn].e * eph[prn].sqrtA * np.sin(E) + + nu = np.arctan2(np.sqrt(1 - eph[prn].e ** 2) * np.sin(E), np.cos(E) - eph[prn].e) + + phi = nu + eph[prn].omega + + phi = np.remainder(phi, 2 * gpsPi) + + u = phi + eph[prn].C_uc * np.cos(2 * phi) + eph[prn].C_us * np.sin(2 * phi) + + 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) + + 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) + + Omega = eph[prn].omega_0 + (eph[prn].omegaDot - Omegae_dot) * tk - Omegae_dot * eph[prn].t_oe + + Omega = np.remainder(Omega + 2 * gpsPi, 2 * gpsPi) + + satPositions[0, satNr] = np.cos(u) * r * np.cos(Omega) - np.sin(u) * r * np.cos(i) * np.sin(Omega) + + satPositions[1, satNr] = np.cos(u) * r * np.sin(Omega) + np.sin(u) * r * np.cos(i) * np.cos(Omega) + + satPositions[2, satNr] = np.sin(u) * r * np.sin(i) + + # Include relativistic correction in clock correction -------------------- + satClkCorr[satNr] = (eph[prn].a_f2 * dt + eph[prn].a_f1) * dt + eph[prn].a_f0 - eph[prn].T_GD + dtr + return satPositions, satClkCorr + + +# topocent.m + + +# togeod.m +def togeod(a, finv, X, Y, Z, *args, **kwargs): + # TOGEOD Subroutine to calculate geodetic coordinates latitude, longitude, + # height given Cartesian coordinates X,Y,Z, and reference ellipsoid + # values semi-major axis (a) and the inverse of flattening (finv). + + # [dphi, dlambda, h] = togeod(a, finv, X, Y, Z); + + # The units of linear parameters X,Y,Z,a must all agree (m,km,mi,ft,..etc) + # The output units of angular quantities will be in decimal degrees + # (15.5 degrees not 15 deg 30 min). The output units of h will be the + # same as the units of X,Y,Z,a. + + # Inputs: + # a - semi-major axis of the reference ellipsoid + # finv - inverse of flattening of the reference ellipsoid + # X,Y,Z - Cartesian coordinates + + # Outputs: + # dphi - latitude + # dlambda - longitude + # h - height above reference ellipsoid + + # Copyright (C) 1987 C. Goad, Columbus, Ohio + # Reprinted with permission of author, 1996 + # Fortran code translated into MATLAB + # Kai Borre 03-30-96 + + # ========================================================================== + + h = 0.0 + + tolsq = 1e-10 + + maxit = 10 + + # compute radians-to-degree factor + rtd = 180 / np.pi + + # compute square of eccentricity + if finv < 1e-20: + esq = 0.0 + + else: + esq = (2 - 1 / finv) / finv + + oneesq = 1 - esq + + # first guess + # P is distance from spin axis + P = np.sqrt(X ** 2 + Y ** 2) + + # direct calculation of longitude + + if P > 1e-20: + dlambda = np.arctan2(Y, X) * rtd + + else: + dlambda = 0.0 + + if dlambda < 0: + dlambda = dlambda + 360 + + # r is distance from origin (0,0,0) + r = np.sqrt(P ** 2 + Z ** 2) + + if r > 1e-20: + sinphi = Z / r + + else: + sinphi = 0.0 + + dphi = np.arcsin(sinphi) + + # initial value of height = distance from origin minus + # approximate distance from origin to surface of ellipsoid + if r < 1e-20: + h = 0.0 + + return dphi, dlambda, h + + h = r - a * (1 - sinphi * sinphi / finv) + + # iterate + for i in range(maxit): + sinphi = np.sin(dphi) + + cosphi = np.cos(dphi) + + N_phi = a / np.sqrt(1 - esq * sinphi * sinphi) + + dP = P - (N_phi + h) * cosphi + + dZ = Z - (N_phi * oneesq + h) * sinphi + + h = h + sinphi * dZ + cosphi * dP + + dphi = dphi + (cosphi * dZ - sinphi * dP) / (N_phi + h) + + if (dP * dP + dZ * dZ) < tolsq: + break + # Not Converged--Warn user + if i == maxit - 1: + print( ' Problem in TOGEOD, did not converge in %2.0f iterations' % i) + + dphi *= rtd + return dphi, dlambda, h + + +######## end togeod.m ###################### + + +def topocent(X, dx, *args, **kwargs): + # TOPOCENT Transformation of vector dx into topocentric coordinate + # system with origin at X. + # Both parameters are 3 by 1 vectors. + + # [Az, El, D] = topocent(X, dx); + + # Inputs: + # X - vector origin corrdinates (in ECEF system [X; Y; Z;]) + # dx - vector ([dX; dY; dZ;]). + + # Outputs: + # D - vector length. Units like units of the input + # Az - azimuth from north positive clockwise, degrees + # El - elevation angle, degrees + + # Kai Borre 11-24-96 + # Copyright (c) by Kai Borre + + # ========================================================================== + + dtr = np.pi / 180 + + phi, lambda_, h = togeod(6378137, 298.257223563, X[0], X[1], X[2]) + + cl = np.cos(lambda_ * dtr) + + sl = np.sin(lambda_ * dtr) + + cb = np.cos(phi * dtr) + + sb = np.sin(phi * dtr) + + F = np.array([[- sl, -sb * cl, cb * cl], [cl, -sb * sl, cb * sl], [0.0, cb, sb]]) + + local_vector = F.T.dot(dx) + + E = local_vector[0] + + N = local_vector[1] + + U = local_vector[2] + + hor_dis = np.sqrt(E ** 2 + N ** 2) + + if hor_dis < 1e-20: + Az = 0.0 + + El = 90.0 + + else: + Az = np.arctan2(E, N) / dtr + + El = np.arctan2(U, hor_dis) / dtr + + if Az < 0: + Az = Az + 360 + + D = np.sqrt(dx[0] ** 2 + dx[1] ** 2 + dx[2] ** 2) + return Az, El, D + + +######### end topocent.m ######### + + +# tropo.m + + +def tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum): + # TROPO Calculation of tropospheric correction. + # The range correction ddr in m is to be subtracted from + # pseudo-ranges and carrier phases + + # ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum); + + # Inputs: + # sinel - sin of elevation angle of satellite + # hsta - height of station in km + # p - atmospheric pressure in mb at height hp + # tkel - surface temperature in degrees Kelvin at height htkel + # hum - humidity in # at height hhum + # hp - height of pressure measurement in km + # htkel - height of temperature measurement in km + # hhum - height of humidity measurement in km + + # Outputs: + # ddr - range correction (meters) + + # Reference + # Goad, C.C. & Goodman, L. (1974) A Modified Tropospheric + # Refraction Correction Model. Paper presented at the + # American Geophysical Union Annual Fall Meeting, San + # Francisco, December 12-17 + + # A Matlab reimplementation of a C code from driver. + # Kai Borre 06-28-95 + + # ========================================================================== + + a_e = 6378.137 + + b0 = 7.839257e-05 + + tlapse = -6.5 + + tkhum = tkel + tlapse * (hhum - htkel) + + atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15) + + e0 = 0.0611 * hum * 10 ** atkel + + tksea = tkel - tlapse * htkel + + em = -978.77 / (2870400.0 * tlapse * 1e-05) + + tkelh = tksea + tlapse * hhum + + e0sea = e0 * (tksea / tkelh) ** (4 * em) + + tkelp = tksea + tlapse * hp + + psea = p * (tksea / tkelp) ** em + + if sinel < 0: + sinel = 0 + + tropo_ = 0.0 + + done = False + + refsea = 7.7624e-05 / tksea + + htop = 1.1385e-05 / refsea + + refsea = refsea * psea + + ref = refsea * ((htop - hsta) / htop) ** 4 + + while 1: + rtop = (a_e + htop) ** 2 - (a_e + hsta) ** 2 * (1 - sinel ** 2) + + if rtop < 0: + rtop = 0 + + rtop = np.sqrt(rtop) - (a_e + hsta) * sinel + + a = -sinel / (htop - hsta) + + b = -b0 * (1 - sinel ** 2) / (htop - hsta) + + rn = np.zeros(8) + + for i in range(8): + rn[i] = rtop ** (i + 2) + + alpha = np.array([2 * a, 2 * a ** 2 + 4 * b / 3, a * (a ** 2 + 3 * b), + a ** 4 / 5 + 2.4 * a ** 2 * b + 1.2 * b ** 2, + 2 * a * b * (a ** 2 + 3 * b) / 3, + b ** 2 * (6 * a ** 2 + 4 * b) * 0.1428571, 0, 0]) + + if b ** 2 > 1e-35: + alpha[6] = a * b ** 3 / 2 + + alpha[7] = b ** 4 / 9 + + dr = rtop + + dr = dr + alpha.dot(rn) + + tropo_ += dr * ref * 1000 + + if done: + ddr = tropo_ + + break + done = True + + refsea = (0.3719 / tksea - 1.292e-05) / tksea + + htop = 1.1385e-05 * (1255.0 / tksea + 0.05) / refsea + + ref = refsea * e0sea * ((htop - hsta) / htop) ** 4 + return ddr + + +######### end tropo.m ################### + + +if __name__ == '__main__': + # print "This program is being run by itself" + pass +else: + print( 'Importing functions from ./geoFunctions/') diff --git a/initialize.py b/initialize.py new file mode 100644 index 0000000..0729e87 --- /dev/null +++ b/initialize.py @@ -0,0 +1,532 @@ +# ./initSettings.m + +# Functions initializes and saves settings. Settings can be edited inside of +# the function, updated from the command line or updated using a dedicated +# GUI - "setSettings". + +# All settings are described inside function code. + +# settings = initSettings() + +# Inputs: none + +# Outputs: +# settings - Receiver settings (a structure). +import datetime + +import numpy as np + + +class Result(object): + def __init__(self, settings): + self._settings = settings + self._results = None + self._channels = None + + @property + def settings(self): + return self._settings + + @property + def channels(self): + assert isinstance(self._channels, np.recarray) + return self._channels + + @property + def results(self): + assert isinstance(self._results, np.recarray) + return self._results + + @results.setter + def results(self, records): + assert isinstance(records, np.recarray) + self._results = records + + def plot(self): + pass + + +class TruePosition(object): + def __init__(self): + self._E = None + self._N = None + self._U = None + + @property + def E(self): + return self._E + + @E.setter + def E(self, e): + self._E = e + + @property + def N(self): + return self._N + + @N.setter + def N(self, n): + self._N = n + + @property + def U(self): + return self._U + + @U.setter + def U(self, u): + self._U = u + + +class Settings(object): + def __init__(self): + # Processing settings ==================================================== + # Number of milliseconds to be processed used 36000 + any transients (see + # below - in Nav parameters) to ensure nav subframes are provided + self.msToProcess = 37000.0 + + # Number of channels to be used for signal processing + self.numberOfChannels = 8 + + # Move the starting point of processing. Can be used to start the signal + # processing at any point in the data record (e.g. for long records). fseek + # function is used to move the file read point, therefore advance is byte + # based only. + self.skipNumberOfBytes = 0 + + # Raw signal file name and other parameter =============================== + # This is a "default" name of the data file (signal record) to be used in + # the post-processing mode + self.fileName = 'GPSdata-DiscreteComponents-fs38_192-if9_55.bin' + # self.fileName = 'gpssim.bin' + + # Data type used to store one sample + self.dataType = 'int8' + + # Intermediate, sampling and code frequencies + self.IF = 9548000.0 + # self.IF = 0.0 + + self.samplingFreq = 38192000.0 + # self.samplingFreq = 2500000.0 + self.codeFreqBasis = 1023000.0 + + # Define number of chips in a code period + self.codeLength = 1023 + + # Acquisition settings =================================================== + # Skips acquisition in the script postProcessing.m if set to 1 + self.skipAcquisition = False + + # List of satellites to look for. Some satellites can be excluded to speed + # up acquisition + self.acqSatelliteList = range(1, 33) + + # Band around IF to search for satellite signal. Depends on max Doppler + self.acqSearchBand = 14.0 + + # Threshold for the signal presence decision rule + self.acqThreshold = 2.5 + + # Tracking loops settings ================================================ + # Code tracking loop parameters + self.dllDampingRatio = 0.7 + + self.dllNoiseBandwidth = 2.0 + + self.dllCorrelatorSpacing = 0.5 + + # Carrier tracking loop parameters + self.pllDampingRatio = 0.7 + + self.pllNoiseBandwidth = 25.0 + + # Navigation solution settings =========================================== + + # Period for calculating pseudoranges and position + self.navSolPeriod = 500.0 + + # Elevation mask to exclude signals from satellites at low elevation + self.elevationMask = 10.0 + + # Enable/dissable use of tropospheric correction + self.useTropCorr = True + + # 1 - On + + # True position of the antenna in UTM system (if known). Otherwise enter + # all NaN's and mean position will be used as a reference . + self.truePosition = TruePosition() + # self.truePosition.E = np.nan + + # self.truePosition.N = np.nan + + # self.truePosition.U = np.nan + + # Plot settings ========================================================== + # Enable/disable plotting of the tracking results for each channel + self.plotTracking = False + + # 1 - On + + # Constants ============================================================== + + self._c = 299792458.0 + + self._startOffset = 68.802 + + @property + def c(self): + return self._c + + @property + def startOffset(self): + return self._startOffset + + @property + def samplesPerCode(self): + return np.int_(np.round(self.samplingFreq / (self.codeFreqBasis / self.codeLength))) + + # makeCaTable.m + def makeCaTable(self): + # Function generates CA codes for all 32 satellites based on the settings + # provided in the structure "settings". The codes are digitized at the + # sampling frequency specified in the settings structure. + # One row in the "caCodesTable" is one C/A code. The row number is the PRN + # number of the C/A code. + + # caCodesTable = makeCaTable(settings) + + # Inputs: + # settings - receiver settings + # Outputs: + # caCodesTable - an array of arrays (matrix) containing C/A codes + # for all satellite PRN-s + + # --- Find number of samples per spreading code ---------------------------- + samplesPerCode = self.samplesPerCode + + # --- Prepare the output matrix to speed up function ----------------------- + caCodesTable = np.zeros((32, samplesPerCode)) + + # --- Find time constants -------------------------------------------------- + ts = 1.0 / self.samplingFreq + + tc = 1.0 / self.codeFreqBasis + + # === For all satellite PRN-s ... + for PRN in range(32): + # --- Generate CA code for given PRN ----------------------------------- + caCode = self.generateCAcode(PRN) + + # --- Make index array to read C/A code values ------------------------- + # The length of the index array depends on the sampling frequency - + # number of samples per millisecond (because one C/A code period is one + # millisecond). + codeValueIndex = np.ceil(ts * np.arange(1, samplesPerCode + 1) / tc) - 1 + codeValueIndex = np.longlong(codeValueIndex) + + codeValueIndex[-1] = 1022 + + # The "upsampled" code is made by selecting values form the CA code + # chip array (caCode) for the time instances of each sample. + caCodesTable[PRN] = caCode[codeValueIndex] + return caCodesTable + + # generateCAcode.m + def generateCAcode(self, prn): + # generateCAcode.m generates one of the 32 GPS satellite C/A codes. + + # CAcode = generateCAcode(PRN) + + # Inputs: + # PRN - PRN number of the sequence. + + # Outputs: + # CAcode - a vector containing the desired C/A code sequence + # (chips). + + # --- Make the code shift array. The shift depends on the PRN number ------- + # The g2s vector holds the appropriate shift of the g2 code to generate + # the C/A code (ex. for SV#19 - use a G2 shift of g2s(19) = 471) + + assert prn in range(0, 32) + g2s = [5, 6, 7, 8, 17, 18, 139, 140, 141, 251, + 252, 254, 255, 256, 257, 258, 469, 470, 471, 472, + 473, 474, 509, 512, 513, 514, 515, 516, 859, 860, + 861, 862, + 145, 175, 52, 21, 237, 235, 886, 657, 634, 762, 355, 1012, 176, 603, 130, 359, 595, 68, 386] + + # --- Pick right shift for the given PRN number ---------------------------- + g2shift = g2s[prn] + + # --- Generate G1 code ----------------------------------------------------- + + # --- Initialize g1 output to speed up the function --- + g1 = np.zeros(1023) + + # --- Load shift register --- + reg = -1 * np.ones(10) + + # --- Generate all G1 signal chips based on the G1 feedback polynomial ----- + for i in range(1023): + g1[i] = reg[-1] + + saveBit = reg[2] * reg[9] + + reg[1:] = reg[:-1] + + reg[0] = saveBit + + # --- Generate G2 code ----------------------------------------------------- + + # --- Initialize g2 output to speed up the function --- + g2 = np.zeros(1023) + + # --- Load shift register --- + reg = -1 * np.ones(10) + + # --- Generate all G2 signal chips based on the G2 feedback polynomial ----- + for i in range(1023): + g2[i] = reg[-1] + + saveBit = reg[1] * reg[2] * reg[5] * reg[7] * reg[8] * reg[9] + + reg[1:] = reg[:-1] + + reg[0] = saveBit + + # --- Shift G2 code -------------------------------------------------------- + # The idea: g2 = concatenate[ g2_right_part, g2_left_part ]; + g2 = np.r_[g2[1023 - g2shift:], g2[:1023 - g2shift]] + + # --- Form single sample C/A code by multiplying G1 and G2 ----------------- + CAcode = -g1 * g2 + return CAcode + + @staticmethod + # calcLoopCoef.m + def calcLoopCoef(LBW, zeta, k): + # Function finds loop coefficients. The coefficients are used then in PLL-s + # and DLL-s. + + # [tau1, tau2] = calcLoopCoef(LBW, zeta, k) + + # Inputs: + # LBW - Loop noise bandwidth + # zeta - Damping ratio + # k - Loop gain + + # Outputs: + # tau1, tau2 - Loop filter coefficients + + # Solve natural frequency + Wn = LBW * 8.0 * zeta / (4.0 * zeta ** 2 + 1) + + # solve for t1 & t2 + tau1 = k / (Wn * Wn) + + tau2 = 2.0 * zeta / Wn + + return tau1, tau2 + + def probeData(self, fileNameStr=None): + + import matplotlib.pyplot as plt + from scipy.signal import welch + from scipy.signal.windows.windows import hamming + + # Function plots raw data information: time domain plot, a frequency domain + # plot and a histogram. + + # The function can be called in two ways: + # probeData(settings) + # or + # probeData(fileName, settings) + + # Inputs: + # fileName - name of the data file. File name is read from + # settings if parameter fileName is not provided. + + # settings - receiver settings. Type of data file, sampling + # frequency and the default filename are specified + # here. + + # Check the number of arguments ========================================== + if fileNameStr is None: + fileNameStr = self.fileName + if not isinstance(fileNameStr, str): + raise TypeError('File name must be a string') + settings = self + # Generate plot of raw data ============================================== + + try: + with open(fileNameStr, 'rb') as fid: + # Move the starting point of processing. Can be used to start the + # signal processing at any point in the data record (e.g. for long + # records). + fid.seek(settings.skipNumberOfBytes, 0) + samplesPerCode = settings.samplesPerCode + + try: + data = np.fromfile(fid, + settings.dataType, + 10 * samplesPerCode) + + except IOError: + # The file is too short + print ('Could not read enough data from the data file.') + # --- Initialization --------------------------------------------------- + plt.figure(100) + plt.clf() + timeScale = np.arange(0, 0.005, 1 / settings.samplingFreq) + + plt.subplot(2, 2, 1) + plt.plot(1000 * timeScale[1:samplesPerCode // 50], + data[1:samplesPerCode // 50]) + plt.axis('tight') + plt.grid() + plt.title('Time domain plot') + plt.xlabel('Time (ms)') + plt.ylabel('Amplitude') + plt.subplot(2, 2, 2) + f, Pxx = welch(data - np.mean(data), + settings.samplingFreq / 1000000.0, + hamming(16384, False), + 16384, + 1024, + 16384) + plt.semilogy(f, Pxx) + plt.axis('tight') + plt.grid() + plt.title('Frequency domain plot') + plt.xlabel('Frequency (MHz)') + plt.ylabel('Magnitude') + plt.show() + plt.subplot(2, 2, 4) + plt.hist(data, np.arange(- 128, 128)) + dmax = np.max(np.abs(data)) + 1 + + plt.axis('tight') + adata = plt.axis() + + plt.axis([-dmax, dmax, adata[2], adata[3]]) + plt.grid('on') + plt.title('Histogram') + plt.xlabel('Bin') + plt.ylabel('Number in bin') + # === Error while opening the data file ================================ + except IOError as e: + print( 'Unable to read file "%s": %s' % (fileNameStr, e)) + + # ./postProcessing.m + + # Script postProcessing.m processes the raw signal from the specified data + # file (in settings) operating on blocks of 37 seconds of data. + + # First it runs acquisition code identifying the satellites in the file, + # then the code and carrier for each of the satellites are tracked, storing + # the 1m sec accumulations. After processing all satellites in the 37 sec + # data block, then postNavigation is called. It calculates pseudoranges + # and attempts a position solutions. At the end plots are made for that + # block of data. + + # THE SCRIPT "RECIPE" + + # The purpose of this script is to combine all parts of the software + # receiver. + + # 1.1) Open the data file for the processing and seek to desired point. + + # 2.1) Acquire satellites + + # 3.1) Initialize channels (preRun.m). + # 3.2) Pass the channel structure and the file identifier to the tracking + # function. It will read and process the data. The tracking results are + # stored in the trackResults structure. The results can be accessed this + # way (the results are stored each millisecond): + # trackResults(channelNumber).XXX(fromMillisecond : toMillisecond), where + # XXX is a field name of the result (e.g. I_P, codePhase etc.) + + # 4) Pass tracking results to the navigation solution function. It will + # decode navigation messages, find satellite positions, measure + # pseudoranges and find receiver position. + + # 5) Plot the results. + + def postProcessing(self, fileNameStr=None): + # Initialization ========================================================= + import acquisition + import postNavigation + import tracking + print ('Starting processing...') + settings = self + if not fileNameStr: + fileNameStr = settings.fileName + if not isinstance(fileNameStr, str): + raise TypeError('File name must be a string') + try: + with open(fileNameStr, 'rb') as fid: + + # If success, then process the data + # Move the starting point of processing. Can be used to start the + # signal processing at any point in the data record (e.g. good for long + # records or for signal processing in blocks). + fid.seek(settings.skipNumberOfBytes, 0) + # Acquisition ============================================================ + # Do acquisition if it is not disabled in settings or if the variable + # acqResults does not exist. + if not settings.skipAcquisition: # or 'acqResults' not in globals(): + # Find number of samples per spreading code + samplesPerCode = settings.samplesPerCode + + # frequency estimation + data = np.fromfile(fid, settings.dataType, 11 * samplesPerCode) + + print( ' Acquiring satellites...') + acqResults = acquisition.AcquisitionResult(settings) + acqResults.acquire(data) + acqResults.plot() + # Initialize channels and prepare for the run ============================ + # Start further processing only if a GNSS signal was acquired (the + # field FREQUENCY will be set to 0 for all not acquired signals) + if np.any(acqResults.carrFreq): + acqResults.preRun() + acqResults.showChannelStatus() + else: + # No satellites to track, exit + print( 'No GNSS signals detected, signal processing finished.') + trackResults = None + + # Track the signal ======================================================= + startTime = datetime.datetime.now() + + print (' Tracking started at %s' % startTime.strftime('%X')) + trackResults = tracking.TrackingResult(acqResults) + try: + trackResults.results = np.load('trackingResults_python.npy', allow_pickle=True) + except IOError: + trackResults.track(fid) + np.save('trackingResults_python', trackResults.results) + # trackResults.track(fid) + # np.save('trackingResults_python', trackResults.results) + + print (' Tracking is over (elapsed time %s s)' % (datetime.datetime.now() - startTime).total_seconds()) + # Auto save the acquisition & tracking results to save time. + print (' Saving Acquisition & Tracking results to storage') + # Calculate navigation solutions ========================================= + print (' Calculating navigation solutions...') + trackResults.plot() + navResults = postNavigation.NavigationResult(trackResults) + navResults.postNavigate() + + print (' Processing is complete for this data block') + # Plot all results =================================================== + print (' Plotting results...') + # TODO turn off tracking plots for now + if settings.plotTracking: + trackResults.plot() + navResults.plot() + print ('Post processing of the signal is over.') + except IOError as e: + # Error while opening the data file. + print ('Unable to read file "%s": %s.' % (settings.fileName, e)) diff --git a/initialize_IQ.py b/initialize_IQ.py new file mode 100644 index 0000000..f0153d9 --- /dev/null +++ b/initialize_IQ.py @@ -0,0 +1,564 @@ +# ./initSettings.m + +# Functions initializes and saves settings. Settings can be edited inside of +# the function, updated from the command line or updated using a dedicated +# GUI - "setSettings". + +# All settings are described inside function code. + +# settings = initSettings() + +# Inputs: none + +# Outputs: +# settings - Receiver settings (a structure). +import datetime +import time + +import numpy as np + +from acquisition_ane import AcquisitionResultANECoreML +# from acquisition_ane_create import AcquisitionResultANE + + +class Result(object): + def __init__(self, settings): + self._settings = settings + self._results = None + self._channels = None + + @property + def settings(self): + return self._settings + + @property + def channels(self): + assert isinstance(self._channels, np.recarray) + return self._channels + + @property + def results(self): + assert isinstance(self._results, np.recarray) + return self._results + + @results.setter + def results(self, records): + assert isinstance(records, np.recarray) + self._results = records + + def plot(self): + pass + + +class TruePosition(object): + def __init__(self): + self._E = None + self._N = None + self._U = None + + @property + def E(self): + return self._E + + @E.setter + def E(self, e): + self._E = e + + @property + def N(self): + return self._N + + @N.setter + def N(self, n): + self._N = n + + @property + def U(self): + return self._U + + @U.setter + def U(self, u): + self._U = u + + +class Settings(object): + def __init__(self): + # Processing settings ==================================================== + # Number of milliseconds to be processed used 36000 + any transients (see + # below - in Nav parameters) to ensure nav subframes are provided + self.msToProcess = 37000.0 + + # Number of channels to be used for signal processing + self.numberOfChannels = 7 + + # Move the starting point of processing. Can be used to start the signal + # processing at any point in the data record (e.g. for long records). fseek + # function is used to move the file read point, therefore advance is byte + # based only. + # Raw signal file name and other parameter =============================== + # This is a "default" name of the data file (signal record) to be used in + # the post-processing mode + self.fileName = 'gps_8192_8.bin' + + # Data type used to store one sample + self.dataType = 'int8' + self.dataTypeSize = np.dtype(self.dataType).itemsize + + self.dataFormat = 'IQ' + + # Intermediate, sampling and code frequencies + self.IF = 0.0 + + self.samplingFreq = 8192000.0 + + self.codeFreqBasis = 1023000.0 + + # Define number of chips in a code period + self.codeLength = 1023 + + self.skipNumberOfBytes = 0*int(self.samplingFreq * 1e-3)*( 2 if self.dataFormat.upper() == 'IQ' else 1)*self.dataTypeSize + + + # Acquisition settings =================================================== + # Skips acquisition in the script postProcessing.m if set to 1 + self.skipAcquisition = False + + # List of satellites to look for. Some satellites can be excluded to speed + # up acquisition + self.acqSatelliteList = range(1, 33) + + # Band around IF to search for satellite signal. Depends on max Doppler + self.acqSearchBand = 14.0 + + # Threshold for the signal presence decision rule + self.acqMinThreshold = 2.5 + self.acqMaxThreshold = 100.0 + + # Tracking loops settings ================================================ + # Code tracking loop parameters + self.dllDampingRatio = 0.7 + + self.dllNoiseBandwidth = 2.0 + + self.dllCorrelatorSpacing = 0.5 + + # Carrier tracking loop parameters + self.pllDampingRatio = 0.7 + + self.pllNoiseBandwidth = 25.0 + + # Navigation solution settings =========================================== + + # Period for calculating pseudoranges and position + self.navSolPeriod = 100.0 + + # Elevation mask to exclude signals from satellites at low elevation + self.elevationMask = 15.0 + + # Enable/dissable use of tropospheric correction + self.useTropCorr = True + + # 1 - On + + # True position of the antenna in UTM system (if known). Otherwise enter + # all NaN's and mean position will be used as a reference . + self.truePosition = TruePosition() + # self.truePosition.E = np.nan + + # self.truePosition.N = np.nan + + # self.truePosition.U = np.nan + + # Plot settings ========================================================== + # Enable/disable plotting of the tracking results for each channel + self.plotTracking = True + + # 1 - On + + # Constants ============================================================== + + self._c = 299792458.0 + + self._startOffset = 68.802 + + @property + def c(self): + return self._c + + @property + def startOffset(self): + return self._startOffset + + @property + def samplesPerCode(self): + return np.int_(np.round(self.samplingFreq / (self.codeFreqBasis / self.codeLength))) + + # makeCaTable.m + def makeCaTable(self): + # Function generates CA codes for all 32 satellites based on the settings + # provided in the structure "settings". The codes are digitized at the + # sampling frequency specified in the settings structure. + # One row in the "caCodesTable" is one C/A code. The row number is the PRN + # number of the C/A code. + + # caCodesTable = makeCaTable(settings) + + # Inputs: + # settings - receiver settings + # Outputs: + # caCodesTable - an array of arrays (matrix) containing C/A codes + # for all satellite PRN-s + + # --- Find number of samples per spreading code ---------------------------- + samplesPerCode = self.samplesPerCode + + # --- Prepare the output matrix to speed up function ----------------------- + caCodesTable = np.zeros((32, samplesPerCode)) + + # --- Find time constants -------------------------------------------------- + ts = 1.0 / self.samplingFreq + + tc = 1.0 / self.codeFreqBasis + + # === For all satellite PRN-s ... + for PRN in range(32): + # --- Generate CA code for given PRN ----------------------------------- + caCode = self.generateCAcode(PRN) + + # --- Make index array to read C/A code values ------------------------- + # The length of the index array depends on the sampling frequency - + # number of samples per millisecond (because one C/A code period is one + # millisecond). + codeValueIndex = np.ceil(ts * np.arange(1, samplesPerCode + 1) / tc) - 1 + codeValueIndex = np.longlong(codeValueIndex) + + codeValueIndex[-1] = 1022 + + # The "upsampled" code is made by selecting values form the CA code + # chip array (caCode) for the time instances of each sample. + caCodesTable[PRN] = caCode[codeValueIndex] + return caCodesTable + + # generateCAcode.m + def generateCAcode(self, prn): + # generateCAcode.m generates one of the 32 GPS satellite C/A codes. + + # CAcode = generateCAcode(PRN) + + # Inputs: + # PRN - PRN number of the sequence. + + # Outputs: + # CAcode - a vector containing the desired C/A code sequence + # (chips). + + # --- Make the code shift array. The shift depends on the PRN number ------- + # The g2s vector holds the appropriate shift of the g2 code to generate + # the C/A code (ex. for SV#19 - use a G2 shift of g2s(19) = 471) + + assert prn in range(0, 32) + g2s = [5, 6, 7, 8, 17, 18, 139, 140, 141, 251, + 252, 254, 255, 256, 257, 258, 469, 470, 471, 472, + 473, 474, 509, 512, 513, 514, 515, 516, 859, 860, + 861, 862, + 145, 175, 52, 21, 237, 235, 886, 657, 634, 762, 355, 1012, 176, 603, 130, 359, 595, 68, 386] + + # --- Pick right shift for the given PRN number ---------------------------- + g2shift = g2s[prn] + + # --- Generate G1 code ----------------------------------------------------- + + # --- Initialize g1 output to speed up the function --- + g1 = np.zeros(1023) + + # --- Load shift register --- + reg = -1 * np.ones(10) + + # --- Generate all G1 signal chips based on the G1 feedback polynomial ----- + for i in range(1023): + g1[i] = reg[-1] + + saveBit = reg[2] * reg[9] + + reg[1:] = reg[:-1] + + reg[0] = saveBit + + # --- Generate G2 code ----------------------------------------------------- + + # --- Initialize g2 output to speed up the function --- + g2 = np.zeros(1023) + + # --- Load shift register --- + reg = -1 * np.ones(10) + + # --- Generate all G2 signal chips based on the G2 feedback polynomial ----- + for i in range(1023): + g2[i] = reg[-1] + + saveBit = reg[1] * reg[2] * reg[5] * reg[7] * reg[8] * reg[9] + + reg[1:] = reg[:-1] + + reg[0] = saveBit + + # --- Shift G2 code -------------------------------------------------------- + # The idea: g2 = concatenate[ g2_right_part, g2_left_part ]; + g2 = np.r_[g2[1023 - g2shift:], g2[:1023 - g2shift]] + + # --- Form single sample C/A code by multiplying G1 and G2 ----------------- + CAcode = -g1 * g2 + return CAcode + + @staticmethod + # calcLoopCoef.m + def calcLoopCoef(LBW, zeta, k): + # Function finds loop coefficients. The coefficients are used then in PLL-s + # and DLL-s. + + # [tau1, tau2] = calcLoopCoef(LBW, zeta, k) + + # Inputs: + # LBW - Loop noise bandwidth + # zeta - Damping ratio + # k - Loop gain + + # Outputs: + # tau1, tau2 - Loop filter coefficients + + # Solve natural frequency + Wn = LBW * 8.0 * zeta / (4.0 * zeta ** 2 + 1) + + # solve for t1 & t2 + tau1 = k / (Wn * Wn) + + tau2 = 2.0 * zeta / Wn + + return tau1, tau2 + + def probeData(self, fileNameStr=None): + + import matplotlib.pyplot as plt + from scipy.signal import welch + from scipy.signal.windows.windows import hamming + + # Function plots raw data information: time domain plot, a frequency domain + # plot and a histogram. + + # The function can be called in two ways: + # probeData(settings) + # or + # probeData(fileName, settings) + + # Inputs: + # fileName - name of the data file. File name is read from + # settings if parameter fileName is not provided. + + # settings - receiver settings. Type of data file, sampling + # frequency and the default filename are specified + # here. + + # Check the number of arguments ========================================== + if fileNameStr is None: + fileNameStr = self.fileName + if not isinstance(fileNameStr, str): + raise TypeError('File name must be a string') + settings = self + # Generate plot of raw data ============================================== + + SHOW_PLOTS = True # Đặt thành False nếu không muốn hiển thị đồ thị + + try: + with open(fileNameStr, 'rb') as fid: + # Move the starting point of processing. Can be used to start the + # signal processing at any point in the data record (e.g. for long + # records). + fid.seek(settings.skipNumberOfBytes, 0) + samplesPerCode = settings.samplesPerCode + + try: + data = np.fromfile(fid, + settings.dataType, + 10 * samplesPerCode) + + except IOError: + # The file is too short + print('Could not read enough data from the data file.') + # --- Initialization --------------------------------------------------- + if SHOW_PLOTS: + plt.figure(100) + plt.clf() + timeScale = np.arange(0, 0.005, 1 / settings.samplingFreq) + + plt.subplot(2, 2, 1) + plt.plot(1000 * timeScale[1:samplesPerCode // 50], + data[1:samplesPerCode // 50]) + plt.axis('tight') + plt.grid() + plt.title('Time domain plot') + plt.xlabel('Time (ms)') + plt.ylabel('Amplitude') + plt.subplot(2, 2, 2) + f, Pxx = welch(data - np.mean(data), + settings.samplingFreq / 1000000.0, + hamming(16384, False), + 16384, + 1024, + 16384) + plt.semilogy(f, Pxx) + plt.axis('tight') + plt.grid() + plt.title('Frequency domain plot') + plt.xlabel('Frequency (MHz)') + plt.ylabel('Magnitude') + plt.show() + plt.subplot(2, 2, 3) + plt.hist(data, np.arange(- 128, 128)) + dmax = np.max(np.abs(data)) + 1 + + plt.axis('tight') + adata = plt.axis() + + plt.axis([-dmax, dmax, adata[2], adata[3]]) + plt.grid('on') + plt.title('Histogram') + plt.xlabel('Bin') + plt.ylabel('Number in bin') + else: + print("Đã đọc dữ liệu thành công, bỏ qua hiển thị đồ thị.") + print(f"Đọc được {len(data)} mẫu, giá trị min/max: {np.min(data)}/{np.max(data)}") + # === Error while opening the data file ================================ + except IOError as e: + print('Unable to read file "%s": %s' % (fileNameStr, e)) + + # ./postProcessing.m + + # Script postProcessing.m processes the raw signal from the specified data + # file (in settings) operating on blocks of 37 seconds of data. + + # First it runs acquisition code identifying the satellites in the file, + # then the code and carrier for each of the satellites are tracked, storing + # the 1m sec accumulations. After processing all satellites in the 37 sec + # data block, then postNavigation is called. It calculates pseudoranges + # and attempts a position solutions. At the end plots are made for that + # block of data. + + # THE SCRIPT "RECIPE" + + # The purpose of this script is to combine all parts of the software + # receiver. + + # 1.1) Open the data file for the processing and seek to desired point. + + # 2.1) Acquire satellites + + # 3.1) Initialize channels (preRun.m). + # 3.2) Pass the channel structure and the file identifier to the tracking + # function. It will read and process the data. The tracking results are + # stored in the trackResults structure. The results can be accessed this + # way (the results are stored each millisecond): + # trackResults(channelNumber).XXX(fromMillisecond : toMillisecond), where + # XXX is a field name of the result (e.g. I_P, codePhase etc.) + + # 4) Pass tracking results to the navigation solution function. It will + # decode navigation messages, find satellite positions, measure + # pseudoranges and find receiver position. + + # 5) Plot the results. + + def postProcessing(self, fileNameStr=None): + # Initialization ========================================================= + import acquisition_IQ + import postNavigation + from acquisition_torch import AcquisitionResultTorch + import tracking_IQ + print('Starting processing...') + settings = self + if not fileNameStr: + fileNameStr = settings.fileName + if not isinstance(fileNameStr, str): + raise TypeError('File name must be a string') + try: + with open(fileNameStr, 'rb') as fid: + + # If success, then process the data + # Move the starting point of processing. Can be used to start the + # signal processing at any point in the data record (e.g. good for long + # records or for signal processing in blocks). + fid.seek(settings.skipNumberOfBytes, 0) + # Acquisition ============================================================ + # Do acquisition if it is not disabled in settings or if the variable + # acqResults does not exist. + if not settings.skipAcquisition: # or 'acqResults' not in globals(): + # Find number of samples per spreading code + samplesPerCode = settings.samplesPerCode + if settings.dataFormat == 'IQ': + numSample2Read = samplesPerCode * 2 + else: + numSample2Read = samplesPerCode + # frequency estimation + print("Numsample: " + str(numSample2Read)) + data = np.fromfile(fid, settings.dataType, 20 * numSample2Read) + + print(' Acquiring satellites...') + t0 = time.time() + acqResults = acquisition_IQ.AcquisitionResult(settings) + acqResults.acquire(data) + t1 = time.time() + acqResults.plot() + print(' Acquisition took CPU %.2f ms' % (t1 - t0)) + t0 = time.time() + acqResults = AcquisitionResultTorch(settings, device="mps") # hoặc "cuda"/"cpu" + acqResults.acquire(data) # data: IQ interleaved float32 + t1 = time.time() + print(' Acquisition took Torch FFT GPU %.2f ms' % (t1 - t0)) + acqResults.plot() + + acqResults = AcquisitionResultANECoreML(settings) + acqResults.acquire(data) # data: IQ interleaved float32 + t0 = time.time() # hoặc "cuda"/"cpu" + acqResults.acquire(data) # data: IQ interleaved float32 + t1 = time.time() + print(' Acquisition took ANE (Apple NPU) %.2f ms' % (t1 - t0)) + acqResults.plot() + # Initialize channels and prepare for the run ============================ + # Start further processing only if a GNSS signal was acquired (the + # field FREQUENCY will be set to 0 for all not acquired signals) + if np.any(acqResults.carrFreq): + acqResults.preRun() + acqResults.showChannelStatus() + else: + # No satellites to track, exit + print('No GNSS signals detected, signal processing finished.') + trackResults = None + + # Track the signal ======================================================= + startTime = datetime.datetime.now() + + print(' Tracking started at %s' % startTime.strftime('%X')) + trackResults = tracking_IQ.TrackingResultIQ(acqResults) + try: + trackResults.results = np.load('trackingResults_python.npy', allow_pickle=True) + except IOError: + trackResults.track_IQ(fid) + np.save('trackingResults_python', trackResults.results, allow_pickle=True) + + print(' Tracking is over (elapsed time %s s)' % (datetime.datetime.now() - startTime).total_seconds()) + # TODO turn off tracking plots for now + if settings.plotTracking: + trackResults.plot() + # Auto save the acquisition & tracking results to save time. + print(' Saving Acquisition & Tracking results to storage') + # Calculate navigation solutions ========================================= + print(' Calculating navigation solutions...') + navResults = postNavigation.NavigationResult(trackResults) + navResults.postNavigate() + + print(' Processing is complete for this data block') + # Plot all results =================================================== + print(' Plotting results...') + navResults.plot() + print('Post processing of the signal is over.') + except IOError as e: + # Error while opening the data file. + print('Unable to read file "%s": %s.' % (settings.fileName, e)) \ No newline at end of file diff --git a/main.py b/main.py new file mode 100644 index 0000000..98b820a --- /dev/null +++ b/main.py @@ -0,0 +1,73 @@ +import initialize +import initialize_IQ + +# ./init.m + +# -------------------------------------------------------------------------- +# SoftGNSS v3.0 +# +# Copyright (C) Darius Plausinaitis and Dennis M. Akos +# Written by Darius Plausinaitis and Dennis M. Akos +# -------------------------------------------------------------------------- +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public LicensePost processing of the signal is over. +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. + +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. + +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, +# USA. +# -------------------------------------------------------------------------- + +# Script initializes settings and environment of the software receiver. +# Then the processing is started. + +# -------------------------------------------------------------------------- + + +# Clean up the environment first ========================================= +# clear +# close_('all') +# clc +# format('compact') +# format('long','g') +# --- Include folders with functions --------------------------------------- +# addpath('include') +# addpath('geoFunctions') +# Print startup ========================================================== +print ('\n', \ + 'Welcome to: softGNSS\n\n', \ + 'An open source GNSS SDR software project initiated by:\n\n', \ + ' Danish GPS Center/Aalborg University\n\n', \ + 'The code was improved by GNSS Laboratory/University of Colorado.\n\n', \ + 'The software receiver softGNSS comes with ABSOLUTELY NO WARRANTY;\n', \ + 'for details please read license details in the file license.txt. This\n', \ + 'is free software, and you are welcome to redistribute it under\n', \ + 'the terms described in the license.\n\n', \ + ' -------------------------------\n\n') +# Initialize settings class========================================= +settings = initialize.Settings() +# settings = initialize_IQ.Settings() +# Generate plot of raw data and ask if ready to start processing ========= +try: + print ('Probing data "%s"...' % settings.fileName) + settings.probeData() + # settings.probeData('/Users/yangsu/Downloads/GNSS_signal_records/GPS_and_GIOVE_A-NN-fs16_3676-if4_1304.bin') +finally: + pass + +print (' Raw IF data plotted ') +print( ' (run setSettings or change settings in "initialize.py" to reconfigure)') +print (' ') +gnssStart = True +# gnssStart = int(raw_input('Enter "1" to initiate GNSS processing or "0" to exit : ').strip()) + +if gnssStart: + print( ' ') + settings.postProcessing() diff --git a/myplot.png b/myplot.png new file mode 100644 index 0000000..3cd2163 Binary files /dev/null and b/myplot.png differ diff --git a/postNavigation.py b/postNavigation.py new file mode 100644 index 0000000..f2d8825 --- /dev/null +++ b/postNavigation.py @@ -0,0 +1,446 @@ +import numpy as np + +import ephemeris +from geoFunctions import satpos, leastSquarePos, cart2geo, findUtmZone, cart2utm +from initialize import Result +import shutil + + + +class NavigationResult(Result): + def __init__(self, trackResult): + self._results = trackResult.results + self._channels = trackResult.channels + self._settings = trackResult.settings + self._solutions = None + self._eph = None + + @property + def solutions(self): + assert isinstance(self._solutions, np.recarray) + return self._solutions + + @property + def ephemeris(self): + # Không assert nhầm sang _solutions nữa + assert isinstance(self._solutions, np.recarray) + return self._eph + + # ./calculatePseudoranges.m + def calculatePseudoranges(self, msOfTheSignal, channelList): + trackResults = self._results + settings = self._settings + # calculatePseudoranges finds relative pseudoranges for all satellites + # listed in CHANNELLIST at the specified millisecond of the processed + # signal. The pseudoranges contain unknown receiver clock offset. It can be + # found by the least squares position search procedure. + + # --- Set initial travel time to infinity ---------------------------------- + travelTime = np.inf * np.ones(settings.numberOfChannels) + + # Find number of samples per spreading code + samplesPerCode = int(settings.samplesPerCode) + + # --- For all channels in the list ... + for ch in channelList: + # Ép chỉ số ms về int + ms_idx = int(msOfTheSignal[ch]) + travelTime[ch] = ( + self._results[ch].absoluteSample[ms_idx] / samplesPerCode + ) + + # --- Truncate the travelTime and compute pseudoranges --------------------- + minimum = np.floor(np.min(travelTime[channelList])) # thay vì toàn bộ + + travelTime = travelTime - minimum + settings.startOffset + + # --- Convert travel time to a distance ------------------------------------ + # The speed of light must be converted from meters per second to meters + # per millisecond. + pseudoranges = travelTime * settings.c / 1000.0 + return pseudoranges + + # ./postNavigation.m + def postNavigate(self): + trackResults = self._results + settings = self._settings + # Check minimal requirements + if settings.msToProcess < 36000.0 or np.sum(trackResults.status.astype('U') != '-') < 4: + # do something + print('Record is too short or too few satellites tracked. Exiting!') + self._solutions = None + self._eph = None + return + + # Find preamble start positions + subFrameStart, activeChnList = self.findPreambles() + + # Decode ephemerides + field_str = 'weekNumber,accuracy,health,T_GD,IODC,t_oc,a_f2,a_f1,a_f0,' \ + 'IODE_sf2,C_rs,deltan,M_0,C_uc,e,C_us,sqrtA,t_oe,' \ + 'C_ic,omega_0,C_is,i_0,C_rc,omega,omegaDot,IODE_sf3,iDot' + eph = np.recarray((32,), formats=['O'] * 27, names=field_str) + + TOW = None + for ch in activeChnList: + # === Convert tracking output to navigation bits ======================= + # Copy 5 sub-frames long record + start = int(subFrameStart[ch] - 20) + stop = int(subFrameStart[ch] + 1500 * 20) + + # Bảo vệ biên + start = max(0, start) + stop = min(stop, trackResults[ch].I_P.size) + if stop - start < 20 * 1500: + # Không đủ dữ liệu cho 5 subframes + continue + + navBitsSamples = trackResults[ch].I_P[start:stop].copy() + # reshape 20 x N (sum theo cột) + navBitsSamples = navBitsSamples.reshape(20, -1, order='F') + navBits = navBitsSamples.sum(0) + navBits = (navBits > 0).astype(int) + + # Binary string array (indexable) + navBitsBin = navBits.astype(str) + + eph[trackResults[ch].PRN - 1], TOW = ephemeris.ephemeris(navBitsBin[1:], navBitsBin[0]) + + if (eph[trackResults[ch].PRN - 1].IODC is None or + eph[trackResults[ch].PRN - 1].IODE_sf2 is None or + eph[trackResults[ch].PRN - 1].IODE_sf3 is None): + # Exclude channel from the list (from further processing) + activeChnList = np.setdiff1d(activeChnList, ch) + + # Need >= 4 SV for 3D position + if activeChnList.size < 4: + print('Too few satellites with ephemeris data for position calculations. Exiting!') + self._solutions = None + self._eph = None + return + + # Initialization + satElev = np.inf * np.ones(settings.numberOfChannels) # include all SV for first fix + readyChnList = activeChnList.copy() + transmitTime = float(TOW) if TOW is not None else 0.0 + + total_ms = float(settings.msToProcess - int(np.max(subFrameStart))) + num_iter = int(np.fix(total_ms) / settings.navSolPeriod) + # num_iter = 64 + + channel = np.rec.array([(np.zeros((settings.numberOfChannels, num_iter)), + np.nan * np.ones((settings.numberOfChannels, num_iter)), + np.nan * np.ones((settings.numberOfChannels, num_iter)), + np.nan * np.ones((settings.numberOfChannels, num_iter)), + np.nan * np.ones((settings.numberOfChannels, num_iter)) + )], formats=['O'] * 5, names='PRN,el,az,rawP,correctedP') + + navSolutions = np.rec.array([(channel, + np.zeros((5, num_iter)), + np.nan * np.ones(num_iter), + np.nan * np.ones(num_iter), + np.nan * np.ones(num_iter), + np.nan * np.ones(num_iter), + np.nan * np.ones(num_iter), + np.nan * np.ones(num_iter), + np.nan * np.ones(num_iter), + 0, + np.nan * np.ones(num_iter), + np.nan * np.ones(num_iter), + np.nan * np.ones(num_iter) + )], formats=['O'] * 13, + names='channel,DOP,X,Y,Z,dt,latitude,longitude,height,utmZone,E,N,U') + + for currMeasNr in range(num_iter): + # apply elevation mask + activeChnList = np.intersect1d((satElev >= settings.elevationMask).nonzero()[0], readyChnList) + + # PRN list + channel[0].PRN[activeChnList, currMeasNr] = trackResults[activeChnList].PRN + + # Pseudoranges + ms_vec = (subFrameStart + settings.navSolPeriod * currMeasNr).astype(int) + channel[0].rawP[:, currMeasNr] = self.calculatePseudoranges(ms_vec, activeChnList) + + # Satellite positions and clock corrections + satPositions, satClkCorr = satpos(transmitTime, trackResults[activeChnList].PRN, eph, settings) + + # Receiver position (need >= 4) + if activeChnList.size > 3: + (xyzdt, + channel[0].el[activeChnList, currMeasNr], + channel[0].az[activeChnList, currMeasNr], + navSolutions[0].DOP[:, currMeasNr]) = leastSquarePos( + satPositions, + channel[0].rawP[activeChnList, currMeasNr] + satClkCorr * settings.c, + settings + ) + + navSolutions[0].X[currMeasNr] = xyzdt[0] + navSolutions[0].Y[currMeasNr] = xyzdt[1] + navSolutions[0].Z[currMeasNr] = xyzdt[2] + navSolutions[0].dt[currMeasNr] = xyzdt[3] + + satElev = channel[0].el[:, currMeasNr] + + channel[0].correctedP[activeChnList, currMeasNr] = ( + channel[0].rawP[activeChnList, currMeasNr] + + satClkCorr * settings.c + + navSolutions[0].dt[currMeasNr] + ) + + # ECEF -> Geodetic + (navSolutions[0].latitude[currMeasNr], + navSolutions[0].longitude[currMeasNr], + navSolutions[0].height[currMeasNr]) = cart2geo( + navSolutions[0].X[currMeasNr], + navSolutions[0].Y[currMeasNr], + navSolutions[0].Z[currMeasNr], + 4 + ) + + navSolutions[0].utmZone = findUtmZone( + navSolutions[0].latitude[currMeasNr], + navSolutions[0].longitude[currMeasNr] + ) + + (navSolutions[0].E[currMeasNr], + navSolutions[0].N[currMeasNr], + navSolutions[0].U[currMeasNr]) = cart2utm( + xyzdt[0], xyzdt[1], xyzdt[2], navSolutions[0].utmZone + ) + + else: + print(' Measurement No. %d: Not enough information for position solution.' % currMeasNr) + navSolutions[0].X[currMeasNr] = np.nan + navSolutions[0].Y[currMeasNr] = np.nan + navSolutions[0].Z[currMeasNr] = np.nan + navSolutions[0].dt[currMeasNr] = np.nan + navSolutions[0].DOP[:, currMeasNr] = np.zeros(5) + navSolutions[0].latitude[currMeasNr] = np.nan + navSolutions[0].longitude[currMeasNr] = np.nan + navSolutions[0].height[currMeasNr] = np.nan + navSolutions[0].E[currMeasNr] = np.nan + navSolutions[0].N[currMeasNr] = np.nan + navSolutions[0].U[currMeasNr] = np.nan + channel[0].az[activeChnList, currMeasNr] = np.nan * np.ones(activeChnList.shape) + channel[0].el[activeChnList, currMeasNr] = np.nan * np.ones(activeChnList.shape) + + # Update TOW + transmitTime += settings.navSolPeriod / 1000.0 + + self._solutions = navSolutions + self._eph = eph + return + + def plot(self): + settings = self._settings + navSolutions = self._solutions + # assert isinstance(navSolutions, np.recarray) + + import matplotlib as mpl + import matplotlib.gridspec as gs + import matplotlib.pyplot as plt + from mpl_toolkits.mplot3d import Axes3D # noqa: F401 + import initialize + + mpl.rcdefaults() + mpl.rc('savefig', bbox='tight', transparent=False, format='png') + mpl.rc('axes', grid=True, linewidth=1.5, axisbelow=True) + mpl.rc('lines', linewidth=1.5, solid_joinstyle='bevel') + mpl.rc('figure', figsize=[8, 6], autolayout=False, dpi=120) + + # Dùng LaTeX nếu có sẵn, không thì fallback sang mathtext + if shutil.which("latex"): + mpl.rc('text', usetex=True) + else: + mpl.rc('text', usetex=False) + mpl.rc('mathtext', fontset='cm') # hiển thị công thức toán kiểu Computer Modern + mpl.rc('font', family='serif', serif='Computer Modern Roman', size=10) + + if navSolutions is not None: + refCoord = initialize.TruePosition() + # Nếu không có tọa độ tham chiếu, dùng trung bình + if (settings.truePosition.E is None or + settings.truePosition.N is None or + settings.truePosition.U is None): + + refCoord.E = np.nanmean(navSolutions[0].E) + refCoord.N = np.nanmean(navSolutions[0].N) + refCoord.U = np.nanmean(navSolutions[0].U) + + meanLongitude = np.nanmean(navSolutions[0].longitude) + meanLatitude = np.nanmean(navSolutions[0].latitude) + + refPointLgText = 'Mean Position' + '\\newline Lat: %.5f $^\\circ$' % meanLatitude + \ + '\\newline Lng: %.5f $^\\circ$' % meanLongitude + \ + '\\newline Hgt: %+6.1f' % np.nanmean(navSolutions[0].height) + + else: + refPointLgText = 'Reference Position' + refCoord.E = settings.truePosition.E + refCoord.N = settings.truePosition.N + refCoord.U = settings.truePosition.U + + figureNumber = 300 + f = plt.figure(figureNumber) + f.clf() + f.set_label('Navigation solutions') + spec = gs.GridSpec(2, 2) + h11 = plt.subplot(spec[0:2]) + h31 = plt.subplot(spec[2], projection='3d') + h32 = plt.subplot(spec[3], projection='polar') + + # ENU variations + h11.plot(navSolutions[0].E - refCoord.E, + navSolutions[0].N - refCoord.N, + navSolutions[0].U - refCoord.U, '-') + h11.legend(['E', 'N', 'U']) + h11.set(title='Coordinates variations in UTM system', + xlabel='Measurement period: %i ms' % settings.navSolPeriod, + ylabel='Variations (m)') + h11.grid(True) + h11.axis('tight') + + # 3D plot + h31.plot((navSolutions[0].E - refCoord.E).T, + (navSolutions[0].N - refCoord.N).T, + (navSolutions[0].U - refCoord.U).T, '+') + h31.plot([0], [0], [0], 'r+', lw=1.5, ms=10) + h31.set_box_aspect((1, 1, 1)) + h31.grid(True) + h31.legend(['Measurements', refPointLgText]) + h31.set(title='Positions in UTM system (3D plot)', + xlabel='East (m)', + ylabel='North (m)', + zlabel='Upping (m)') + + # Sky plot + az = np.deg2rad(navSolutions[0].channel[0].az.T) + el = navSolutions[0].channel[0].el.T + h32.plot(az, 90 - el) + # PRN labels tại epoch 0 + az0 = np.deg2rad(navSolutions[0].channel[0].az[:, 0]) + el0 = navSolutions[0].channel[0].el[:, 0] + prn0 = navSolutions[0].channel[0].PRN[:, 0] + for x, y, s in zip(az0, 90 - el0, prn0): + if np.isfinite(x) and np.isfinite(y) and s != 0: + h32.text(x, y, str(s)) + h32.set_theta_direction(-1) + h32.set_theta_zero_location('N') + h32.set_xlim([0, 2 * np.pi]) + h32.set_xticks(np.linspace(0, 2 * np.pi, 12, endpoint=False)) + h32.set_rlabel_position(0) + h32.set_ylim([0, 90]) + h32.set_yticks([0, 15, 30, 45, 60, 75]) + h32.set_yticklabels([90, 75, 60, 45, 30, 15]) + h32.set_title('Sky plot (mean PDOP: %f )' % np.nanmean(navSolutions[0].DOP[1, :])) + plt.show() + else: + print('plotNavigation: No navigation data to plot.') + + @staticmethod + # navPartyChk.m + def navPartyChk(ndat): + # Parity check cho một từ GPS (32 bits: 2 prev + 30 curr) + if ndat[1] != 1: + ndat[2:26] *= (-1) + + parity = np.zeros(6) + parity[0] = ndat[0] * ndat[2] * ndat[3] * ndat[4] * ndat[6] * \ + ndat[7] * ndat[11] * ndat[12] * ndat[13] * ndat[14] * \ + ndat[15] * ndat[18] * ndat[19] * ndat[21] * ndat[24] + + parity[1] = ndat[1] * ndat[3] * ndat[4] * ndat[5] * ndat[7] * \ + ndat[8] * ndat[12] * ndat[13] * ndat[14] * ndat[15] * \ + ndat[16] * ndat[19] * ndat[20] * ndat[22] * ndat[25] + + parity[2] = ndat[0] * ndat[2] * ndat[4] * ndat[5] * ndat[6] * \ + ndat[8] * ndat[9] * ndat[13] * ndat[14] * ndat[15] * \ + ndat[16] * ndat[17] * ndat[20] * ndat[21] * ndat[23] + + parity[3] = ndat[1] * ndat[3] * ndat[5] * ndat[6] * ndat[7] * \ + ndat[9] * ndat[10] * ndat[14] * ndat[15] * ndat[16] * \ + ndat[17] * ndat[18] * ndat[21] * ndat[22] * ndat[24] + + parity[4] = ndat[1] * ndat[2] * ndat[4] * ndat[6] * ndat[7] * \ + ndat[8] * ndat[10] * ndat[11] * ndat[15] * ndat[16] * \ + ndat[17] * ndat[18] * ndat[19] * ndat[22] * ndat[23] * \ + ndat[25] + + parity[5] = ndat[0] * ndat[4] * ndat[6] * ndat[7] * ndat[9] * \ + ndat[10] * ndat[11] * ndat[12] * ndat[14] * ndat[16] * \ + ndat[20] * ndat[23] * ndat[24] * ndat[25] + + if (parity == ndat[26:]).sum() == 6: + status = -1 * ndat[1] + else: + status = 0 + return status + + # ./findPreambles.m + def findPreambles(self): + assert isinstance(self._results, np.recarray) + trackResults = self._results + settings = self._settings + + searchStartOffset = 0 + + # --- Initialize the firstSubFrame array ----------------------------------- + firstSubFrame = np.zeros(settings.numberOfChannels, dtype=int) + + # --- Generate the preamble pattern ---------------------------------------- + preamble_bits = np.r_[1, -1, -1, -1, 1, -1, 1, 1] + preamble_ms = np.kron(preamble_bits, np.ones(20)) # upsample 20x + + # --- Make a list of channels excluding not tracking channels -------------- + activeChnList = np.nonzero(trackResults.status.astype('U') != '-')[0] + + # === For all tracking channels ... + for ch in activeChnList.tolist(): + bits = trackResults[ch].I_P[searchStartOffset:].copy() + # Chuẩn hóa sang {-1, +1} + bits = np.where(bits > 0, 1, -1) + + # Correlate với preamble (pad cho cùng độ dài) + tlmXcorrResult = np.correlate( + bits, + np.pad(preamble_ms, (0, max(0, bits.size - preamble_ms.size)), 'constant'), + mode='full' + ) + tlmXcorrResult = np.nan_to_num(tlmXcorrResult, nan=0.0, posinf=0.0, neginf=0.0) + + # Tìm các điểm giống preamble + xcorrLength = (len(tlmXcorrResult) + 1) // 2 # int + start = max(0, xcorrLength - 1) + stop = min(2 * xcorrLength, tlmXcorrResult.shape[0]) + idx_rel = (np.abs(tlmXcorrResult[start:stop]) > 153).nonzero()[0] + index = idx_rel.astype(np.intp) + int(searchStartOffset) + + found = False + for i in range(len(index)): + index2 = index - index[i] + # Khoảng cách 6000 ms (1 subframe) + if (index2 == 6000).any(): + s = int(index[i] - 40) # 2 bit prev * 20ms + e = int(index[i] + 20 * 60) # 60 bits * 20ms + if s < 0 or e > trackResults[ch].I_P.size: + continue + + bits2 = trackResults[ch].I_P[s:e].copy().reshape(20, -1, order='F').sum(0) + bits2 = np.where(bits2 > 0, 1, -1) + + if self.navPartyChk(bits2[:32]) != 0 and self.navPartyChk(bits2[30:62]) != 0: + firstSubFrame[ch] = int(index[i]) + found = True + break + + if not found: + activeChnList = np.setdiff1d(activeChnList, ch) + print('Could not find valid preambles in channel %2d !' % ch) + + return firstSubFrame, activeChnList + + +if __name__ == '__main__': + pass diff --git a/serial.py b/serial.py new file mode 100644 index 0000000..c9713b5 --- /dev/null +++ b/serial.py @@ -0,0 +1,38 @@ +import serial +import keyboard +import time + +# Thay 'COM3' bằng cổng Arduino của bạn, ví dụ 'COM3' trên Windows hoặc '/dev/ttyACM0' trên Linux/macOS +SERIAL_PORT = 'usbmodem1401' +BAUD_RATE = 9600 + +try: + ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1) +except serial.SerialException: + print(f"Không thể mở cổng {SERIAL_PORT}. Kiểm tra kết nối Arduino.") + exit() + +print("Đang lắng nghe phím W A S D, nhấn ESC để thoát...") + +while True: + if keyboard.is_pressed('w'): + ser.write(b'w') + print("Send: w") + time.sleep(0.2) + elif keyboard.is_pressed('a'): + ser.write(b'a') + print("Send: a") + time.sleep(0.2) + elif keyboard.is_pressed('s'): + ser.write(b's') + print("Send: s") + time.sleep(0.2) + elif keyboard.is_pressed('d'): + ser.write(b'd') + print("Send: d") + time.sleep(0.2) + elif keyboard.is_pressed('esc'): + print("Thoát chương trình.") + break + +ser.close() diff --git a/tracking.py b/tracking.py new file mode 100644 index 0000000..1be0db0 --- /dev/null +++ b/tracking.py @@ -0,0 +1,400 @@ +import numpy as np + +from initialize import Result + + +class TrackingResult(Result): + def __init__(self, acqResult): + self._results = None + self._channels = acqResult.channels + self._settings = acqResult.settings + + # ./tracking.m + def track(self, fid): + channel = self._channels + settings = self._settings + # Performs code and carrier tracking for all channels. + + # [trackResults, channel] = tracking(fid, channel, settings) + + # Inputs: + # fid - file identifier of the signal record. + # channel - PRN, carrier frequencies and code phases of all + # satellites to be tracked (prepared by preRum.m from + # acquisition results). + # settings - receiver settings. + # Outputs: + # trackResults - tracking results (structure array). Contains + # in-phase prompt outputs and absolute starting + # positions of spreading codes, together with other + # observation data from the tracking loops. All are + # saved every millisecond. + + # Initialize tracking variables ========================================== + + codePeriods = settings.msToProcess + + # --- DLL variables -------------------------------------------------------- + # Define early-late offset (in chips) + earlyLateSpc = settings.dllCorrelatorSpacing + + # Summation interval + PDIcode = 0.001 + + # Calculate filter coefficient values + tau1code, tau2code = settings.calcLoopCoef(settings.dllNoiseBandwidth, settings.dllDampingRatio, 1.0) + + # --- PLL variables -------------------------------------------------------- + # Summation interval + PDIcarr = 0.001 + + # Calculate filter coefficient values + tau1carr, tau2carr = settings.calcLoopCoef(settings.pllNoiseBandwidth, settings.pllDampingRatio, 0.25) + + # hwb=waitbar(0,'Tracking...') + + # Initialize a temporary list of records + rec = [] + # Start processing channels ============================================== + for channelNr in range(settings.numberOfChannels): + msToProcess = np.long(settings.msToProcess) + # Initialize fields for record(structured) array of tracked results + status = '-' + + # The absolute sample in the record of the C/A code start: + absoluteSample = np.zeros(msToProcess) + + # Freq of the C/A code: + codeFreq_ = np.inf * np.ones(msToProcess) + + # Frequency of the tracked carrier wave: + carrFreq_ = np.inf * np.ones(msToProcess) + + # Outputs from the correlators (In-phase): + I_P_ = np.zeros(msToProcess) + + I_E_ = np.zeros(msToProcess) + + I_L_ = np.zeros(msToProcess) + + # Outputs from the correlators (Quadrature-phase): + Q_E_ = np.zeros(msToProcess) + + Q_P_ = np.zeros(msToProcess) + + Q_L_ = np.zeros(msToProcess) + + # Loop discriminators + dllDiscr = np.inf * np.ones(msToProcess) + dllDiscrFilt = np.inf * np.ones(msToProcess) + pllDiscr = np.inf * np.ones(msToProcess) + pllDiscrFilt = np.inf * np.ones(msToProcess) + PRN = 0 + + # Only process if PRN is non zero (acquisition was successful) + if channel[channelNr].PRN != 0: + # Save additional information - each channel's tracked PRN + PRN = channel[channelNr].PRN + + # signal processing at any point in the data record (e.g. for long + # records). In addition skip through that data file to start at the + # appropriate sample (corresponding to code phase). Assumes sample + # type is schar (or 1 byte per sample) + # fid.seek(settings.skipNumberOfBytes + channel[channelNr].codePhase, 0) + fid.seek(int(settings.skipNumberOfBytes + channel[channelNr].codePhase), 0) + # Here PRN is the actual satellite ID instead of the 0-based index + caCode = settings.generateCAcode(channel[channelNr].PRN - 1) + + caCode = np.r_[caCode[-1], caCode, caCode[0]] + + # define initial code frequency basis of NCO + codeFreq = settings.codeFreqBasis + + remCodePhase = 0.0 + + carrFreq = channel[channelNr].acquiredFreq + + carrFreqBasis = channel[channelNr].acquiredFreq + + remCarrPhase = 0.0 + + oldCodeNco = 0.0 + + oldCodeError = 0.0 + + oldCarrNco = 0.0 + + oldCarrError = 0.0 + + for loopCnt in range(np.long(codePeriods)): + # GUI update ------------------------------------------------------------- + # The GUI is updated every 50ms. This way Matlab GUI is still + # responsive enough. At the same time Matlab is not occupied + # all the time with GUI task. + if loopCnt % 50 == 0: + try: + print ('Tracking: Ch %d' % (channelNr + 1) + ' of %d' % settings.numberOfChannels + \ + '; PRN#%02d' % channel[channelNr].PRN + \ + '; Completed %d' % loopCnt + ' of %d' % codePeriods + ' msec') + finally: + pass + # Read next block of data ------------------------------------------------ + # Find the size of a "block" or code period in whole samples + # Update the phasestep based on code freq (variable) and + # sampling frequency (fixed) + codePhaseStep = codeFreq / settings.samplingFreq + + blksize = np.ceil((settings.codeLength - remCodePhase) / codePhaseStep) + blksize = np.long(blksize) + + # interaction + rawSignal = np.fromfile(fid, settings.dataType, blksize) + samplesRead = len(rawSignal) + + # If did not read in enough samples, then could be out of + # data - better exit + if samplesRead != blksize: + print ('Not able to read the specified number of samples for tracking, exiting!') + fid.close() + trackResults = None + return trackResults + # Set up all the code phase tracking information ------------------------- + # Define index into early code vector + tcode = np.linspace(remCodePhase - earlyLateSpc, + blksize * codePhaseStep + remCodePhase - earlyLateSpc, + blksize, endpoint=False) + + tcode2 = np.ceil(tcode) + + earlyCode = caCode[np.longlong(tcode2)] + + tcode = np.linspace(remCodePhase + earlyLateSpc, + blksize * codePhaseStep + remCodePhase + earlyLateSpc, + blksize, endpoint=False) + + tcode2 = np.ceil(tcode) + + lateCode = caCode[np.longlong(tcode2)] + + tcode = np.linspace(remCodePhase, + blksize * codePhaseStep + remCodePhase, + blksize, endpoint=False) + + tcode2 = np.ceil(tcode) + + promptCode = caCode[np.longlong(tcode2)] + + remCodePhase = tcode[blksize - 1] + codePhaseStep - 1023.0 + + # Generate the carrier frequency to mix the signal to baseband ----------- + time = np.arange(0, blksize + 1) / settings.samplingFreq + + trigarg = carrFreq * 2.0 * np.pi * time + remCarrPhase + + remCarrPhase = trigarg[blksize] % (2 * np.pi) + + carrCos = np.cos(trigarg[0:blksize]) + + carrSin = np.sin(trigarg[0:blksize]) + + # Generate the six standard accumulated values --------------------------- + # First mix to baseband + qBasebandSignal = carrCos * rawSignal + + iBasebandSignal = carrSin * rawSignal + + I_E = (earlyCode * iBasebandSignal).sum() + + Q_E = (earlyCode * qBasebandSignal).sum() + + I_P = (promptCode * iBasebandSignal).sum() + + Q_P = (promptCode * qBasebandSignal).sum() + + I_L = (lateCode * iBasebandSignal).sum() + + Q_L = (lateCode * qBasebandSignal).sum() + + # Find PLL error and update carrier NCO ---------------------------------- + # Implement carrier loop discriminator (phase detector) + carrError = np.arctan(Q_P / I_P) / 2.0 / np.pi + + carrNco = oldCarrNco + \ + tau2carr / tau1carr * (carrError - oldCarrError) + \ + carrError * (PDIcarr / tau1carr) + + oldCarrNco = carrNco + + oldCarrError = carrError + + carrFreq = carrFreqBasis + carrNco + + carrFreq_[loopCnt] = carrFreq + + # Find DLL error and update code NCO ------------------------------------- + codeError = (np.sqrt(I_E * I_E + Q_E * Q_E) - np.sqrt(I_L * I_L + Q_L * Q_L)) / ( + np.sqrt(I_E * I_E + Q_E * Q_E) + np.sqrt(I_L * I_L + Q_L * Q_L)) + + codeNco = oldCodeNco + \ + tau2code / tau1code * (codeError - oldCodeError) + \ + codeError * (PDIcode / tau1code) + + oldCodeNco = codeNco + + oldCodeError = codeError + + codeFreq = settings.codeFreqBasis - codeNco + + codeFreq_[loopCnt] = codeFreq + + # Record various measures to show in postprocessing ---------------------- + # Record sample number (based on 8bit samples) + absoluteSample[loopCnt] = fid.tell() + + dllDiscr[loopCnt] = codeError + + dllDiscrFilt[loopCnt] = codeNco + + pllDiscr[loopCnt] = carrError + + pllDiscrFilt[loopCnt] = carrNco + + I_E_[loopCnt] = I_E + + I_P_[loopCnt] = I_P + + I_L_[loopCnt] = I_L + + Q_E_[loopCnt] = Q_E + + Q_P_[loopCnt] = Q_P + + Q_L_[loopCnt] = Q_L + + # If we got so far, this means that the tracking was successful + # Now we only copy status, but it can be update by a lock detector + # if implemented + status = channel[channelNr].status + rec.append((status, absoluteSample, codeFreq_, carrFreq_, + I_P_, I_E_, I_L_, Q_E_, Q_P_, Q_L_, + dllDiscr, dllDiscrFilt, pllDiscr, pllDiscrFilt, PRN)) + + trackResults = np.rec.fromrecords(rec, + dtype=[('status', 'S1'), ('absoluteSample', 'object'), ('codeFreq', 'object'), + ('carrFreq', 'object'), ('I_P', 'object'), ('I_E', 'object'), + ('I_L', 'object'), + ('Q_E', 'object'), ('Q_P', 'object'), ('Q_L', 'object'), + ('dllDiscr', 'object'), + ('dllDiscrFilt', 'object'), ('pllDiscr', 'object'), + ('pllDiscrFilt', 'object'), + ('PRN', 'int64')]) + self._results = trackResults + return + + def plot(self): + import matplotlib as mpl + import matplotlib.gridspec as gs + import matplotlib.pyplot as plt + import numpy as np + + # %% configure matplotlib (no LaTeX required) + mpl.rcdefaults() + mpl.rc('savefig', bbox='tight', transparent=False, format='png') + mpl.rc('axes', grid=True, linewidth=1.5, axisbelow=True) + mpl.rc('lines', linewidth=1.5, solid_joinstyle='bevel') + mpl.rc('figure', figsize=[8, 6], dpi=120) + mpl.rc('font', family='serif', size=8) + mpl.rc('mathtext', fontset='cm') # dùng mathtext thay cho LaTeX + + # ./plotTracking.m (Python version) + + trackResults = self._results + settings = self._settings + channelList = range(settings.numberOfChannels) + + # Protection - if the list contains incorrect channel numbers + channelList = np.intersect1d(channelList, range(settings.numberOfChannels)) + + # === For all listed channels ============================================== + for channelNr in channelList: + # Tạo figure cho mỗi channel + f = plt.figure(channelNr + 200) + f.set_label('Channel ' + str(channelNr) + + ' (PRN ' + str(trackResults[channelNr].PRN) + ') results') + + # Bố cục 3x3 + spec = gs.GridSpec(3, 3) + h11 = plt.subplot(spec[0, 0]) + h12 = plt.subplot(spec[0, 1:]) + h21 = plt.subplot(spec[1, 0]) + h22 = plt.subplot(spec[1, 1:]) + h31 = plt.subplot(spec[2, 0]) + h32 = plt.subplot(spec[2, 1]) + h33 = plt.subplot(spec[2, 2]) + + # Trục thời gian (s) + timeAxisInSeconds = np.arange(settings.msToProcess) / 1000.0 + + # --- Scatter plot (I_P vs Q_P) + h11.plot(trackResults[channelNr].I_P, trackResults[channelNr].Q_P, '.') + h11.grid() + h11.axis('equal') + h11.set(title='Discrete-Time Scatter Plot', + xlabel='I prompt', ylabel='Q prompt') + + # --- Bits of navigation message (I_P theo thời gian) + h12.plot(timeAxisInSeconds, trackResults[channelNr].I_P) + h12.grid() + h12.set(title='Bits of the navigation message', xlabel='Time (s)') + h12.axis('tight') + + # --- Raw PLL discriminator + h21.plot(timeAxisInSeconds, trackResults[channelNr].pllDiscr, 'r') + h21.grid() + h21.axis('tight') + h21.set(xlabel='Time (s)', ylabel='Amplitude', + title='Raw PLL discriminator') + + # --- Correlation results (Early, Prompt, Late) + h22.plot(timeAxisInSeconds, + np.sqrt(trackResults[channelNr].I_E ** 2 + + trackResults[channelNr].Q_E ** 2).T, + timeAxisInSeconds, + np.sqrt(trackResults[channelNr].I_P ** 2 + + trackResults[channelNr].Q_P ** 2).T, + timeAxisInSeconds, + np.sqrt(trackResults[channelNr].I_L ** 2 + + trackResults[channelNr].Q_L ** 2).T, + '-*') + h22.grid() + h22.set(title='Correlation results', xlabel='Time (s)') + h22.axis('tight') + h22.legend([r'$\sqrt{I_{E}^2 + Q_{E}^2}$', + r'$\sqrt{I_{P}^2 + Q_{P}^2}$', + r'$\sqrt{I_{L}^2 + Q_{L}^2}$']) + + # --- Filtered PLL discriminator + h31.plot(timeAxisInSeconds, trackResults[channelNr].pllDiscrFilt, 'b') + h31.grid() + h31.axis('tight') + h31.set(xlabel='Time (s)', ylabel='Amplitude', + title='Filtered PLL discriminator') + + # --- Raw DLL discriminator + h32.plot(timeAxisInSeconds, trackResults[channelNr].dllDiscr, 'r') + h32.grid() + h32.axis('tight') + h32.set(xlabel='Time (s)', ylabel='Amplitude', + title='Raw DLL discriminator') + + # --- Filtered DLL discriminator + h33.plot(timeAxisInSeconds, trackResults[channelNr].dllDiscrFilt, 'b') + h33.grid() + h33.axis('tight') + h33.set(xlabel='Time (s)', ylabel='Amplitude', + title='Filtered DLL discriminator') + + # Show figure + plt.show() + # f.show() + diff --git a/trackingResults_python.npy b/trackingResults_python.npy new file mode 100644 index 0000000..618e7bc Binary files /dev/null and b/trackingResults_python.npy differ diff --git a/tracking_IQ.py b/tracking_IQ.py new file mode 100644 index 0000000..05719f4 --- /dev/null +++ b/tracking_IQ.py @@ -0,0 +1,231 @@ +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.gridspec as gs +from initialize import Result + +class TrackingResultIQ(Result): + def __init__(self, acqResult): + self._results = None + self._channels = acqResult.channels + self._settings = acqResult.settings + + def track_IQ(self, fid): + channel = self._channels + settings = self._settings + codePeriods = settings.msToProcess + + earlyLateSpc = settings.dllCorrelatorSpacing + PDIcode = 0.001 + tau1code, tau2code = settings.calcLoopCoef(settings.dllNoiseBandwidth, + settings.dllDampingRatio, 1.0) + + PDIcarr = 0.001 + tau1carr, tau2carr = settings.calcLoopCoef(settings.pllNoiseBandwidth, + settings.pllDampingRatio, 0.25) + + rec = [] + for channelNr in range(settings.numberOfChannels): + msToProcess = int(settings.msToProcess) + + status = '-' + absoluteSample = np.zeros(msToProcess) + codeFreq_ = np.inf * np.ones(msToProcess) + carrFreq_ = np.inf * np.ones(msToProcess) + + # Outputs from the correlators (In-phase): + I_E_ = np.zeros(msToProcess) + + I_P_ = np.zeros(msToProcess) + + I_L_ = np.zeros(msToProcess) + + # Outputs from the correlators (Quadrature-phase): + Q_E_ = np.zeros(msToProcess) + + Q_P_ = np.zeros(msToProcess) + + Q_L_ = np.zeros(msToProcess) + + dllDiscr = np.inf * np.ones(msToProcess) + dllDiscrFilt = np.inf * np.ones(msToProcess) + pllDiscr = np.inf * np.ones(msToProcess) + pllDiscrFilt = np.inf * np.ones(msToProcess) + PRN = 0 + + if channel[channelNr].PRN != 0: + PRN = channel[channelNr].PRN + fid.seek(int(settings.skipNumberOfBytes + + channel[channelNr].codePhase * settings.dataTypeSize * 2), 0) + + caCode = settings.generateCAcode(channel[channelNr].PRN - 1) + + caCode = np.r_[caCode[-1], caCode, caCode[0]] + + codeFreq = settings.codeFreqBasis + + remCodePhase = 0.0 + + carrFreq = channel[channelNr].acquiredFreq + + carrFreqBasis = channel[channelNr].acquiredFreq + + remCarrPhase = 0.0 + + oldCodeNco = 0.0; + + oldCodeError = 0.0 + + oldCarrNco = 0.0; + + oldCarrError = 0.0 + + for loopCnt in range(msToProcess): + if loopCnt % 50 == 0: + print(f"Tracking: Ch {channelNr + 1} of {settings.numberOfChannels}; " + f"PRN#{channel[channelNr].PRN}; " + f"Completed {loopCnt} of {msToProcess} msec") + codePhaseStep = codeFreq / settings.samplingFreq + blksize = np.ceil((settings.codeLength - remCodePhase) / codePhaseStep) + blksize = np.long(blksize) + + raw = np.fromfile(fid, dtype=settings.dataType, count=2*blksize) + if len(raw) < 2*blksize: + print("End of file, stopping tracking") + break + rawSignal = raw[0::2] + 1j * raw[1::2] + + # Early, Late, Prompt + tcode = np.arange(blksize) * codePhaseStep + remCodePhase - earlyLateSpc + earlyCode = caCode[np.ceil(tcode).astype(int)] + + tcode = np.arange(blksize) * codePhaseStep + remCodePhase + earlyLateSpc + lateCode = caCode[np.ceil(tcode).astype(int)] + + tcode = np.arange(blksize) * codePhaseStep + remCodePhase + promptCode = caCode[np.ceil(tcode).astype(int)] + + remCodePhase = tcode[blksize - 1] + codePhaseStep - 1023.0 + + # Mix về baseband + time = np.arange(0, blksize + 1) / settings.samplingFreq + trigarg = carrFreq * 2*np.pi * time + remCarrPhase + # remCarrPhase = trigarg[-1] % (2*np.pi) + remCarrPhase = trigarg[blksize]% (2 * np.pi); + + carrCos = np.cos(trigarg[0:blksize]) + carrSin = np.sin(trigarg[0:blksize]) + iqBaseband = (carrSin + 1j*carrCos) * rawSignal + + iBaseband = iqBaseband.real + qBaseband = iqBaseband.imag + + I_E = np.sum(earlyCode * iBaseband); Q_E = np.sum(earlyCode * qBaseband) + I_P = np.sum(promptCode * iBaseband); Q_P = np.sum(promptCode * qBaseband) + I_L = np.sum(lateCode * iBaseband); Q_L = np.sum(lateCode * qBaseband) + + # PLL + carrError = np.arctan(Q_P / I_P) / (2*np.pi) + carrNco = oldCarrNco + (tau2carr/tau1carr) * (carrError - oldCarrError) + carrError * (PDIcarr/tau1carr) + oldCarrNco, oldCarrError = carrNco, carrError + carrFreq = carrFreqBasis + carrNco + + # DLL + codeError = (np.sqrt(I_E**2 + Q_E**2) - np.sqrt(I_L**2 + Q_L**2)) / \ + (np.sqrt(I_E**2 + Q_E**2) + np.sqrt(I_L**2 + Q_L**2)) + codeNco = oldCodeNco + (tau2code/tau1code) * (codeError - oldCodeError) + codeError * (PDIcode/tau1code) + oldCodeNco, oldCodeError = codeNco, codeError + codeFreq = settings.codeFreqBasis - codeNco + + # Save + absoluteSample[loopCnt] = fid.tell() + carrFreq_[loopCnt] = carrFreq + codeFreq_[loopCnt] = codeFreq + + I_E_[loopCnt], Q_E_[loopCnt] = I_E, Q_E + I_P_[loopCnt], Q_P_[loopCnt] = I_P, Q_P + I_L_[loopCnt], Q_L_[loopCnt] = I_L, Q_L + + dllDiscr[loopCnt], dllDiscrFilt[loopCnt] = codeError, codeNco + pllDiscr[loopCnt], pllDiscrFilt[loopCnt] = carrError, carrNco + + status = channel[channelNr].status + rec.append((status, absoluteSample, codeFreq_, carrFreq_, + I_P_, I_E_, I_L_, Q_E_, Q_P_, Q_L_, + dllDiscr, dllDiscrFilt, pllDiscr, pllDiscrFilt, PRN)) + + trackResults = np.rec.fromrecords(rec, + dtype=[('status','S1'), + ('absoluteSample','object'), + ('codeFreq','object'), + ('carrFreq','object'), + ('I_P','object'),('I_E','object'),('I_L','object'), + ('Q_E','object'),('Q_P','object'),('Q_L','object'), + ('dllDiscr','object'),('dllDiscrFilt','object'), + ('pllDiscr','object'),('pllDiscrFilt','object'), + ('PRN','int64')]) + self._results = trackResults + return trackResults + + def plot(self, channelList=None): + trackResults = self._results + settings = self._settings + + if channelList is None: + channelList = range(settings.numberOfChannels) + channelList = np.intersect1d(channelList, range(settings.numberOfChannels)) + + for channelNr in channelList: + f = plt.figure(channelNr + 200) + f.set_label(f'Channel {channelNr} (PRN {trackResults[channelNr].PRN}) results') + + spec = gs.GridSpec(3, 3) + h11 = plt.subplot(spec[0, 0]) + h12 = plt.subplot(spec[0, 1:]) + h21 = plt.subplot(spec[1, 0]) + h22 = plt.subplot(spec[1, 1:]) + h31 = plt.subplot(spec[2, 0]) + h32 = plt.subplot(spec[2, 1]) + h33 = plt.subplot(spec[2, 2]) + + timeAxisInSeconds = np.arange(settings.msToProcess) / 1000.0 + + # Scatter plot + h11.plot(trackResults[channelNr].I_P, trackResults[channelNr].Q_P, '.') + h11.grid(); h11.axis('equal') + h11.set(title='Scatter Plot', xlabel='I prompt', ylabel='Q prompt') + + # Nav bits + h12.plot(timeAxisInSeconds, trackResults[channelNr].I_P) + h12.grid(); h12.axis('tight') + h12.set(title='Navigation bits (I_P)', xlabel='Time (s)') + + # PLL raw discr + h21.plot(timeAxisInSeconds, trackResults[channelNr].pllDiscr, 'r') + h21.grid(); h21.axis('tight') + h21.set(title='Raw PLL discriminator', xlabel='Time (s)') + + # Correlation results + h22.plot(timeAxisInSeconds, np.sqrt(trackResults[channelNr].I_E**2 + trackResults[channelNr].Q_E**2), + timeAxisInSeconds, np.sqrt(trackResults[channelNr].I_P**2 + trackResults[channelNr].Q_P**2), + timeAxisInSeconds, np.sqrt(trackResults[channelNr].I_L**2 + trackResults[channelNr].Q_L**2), '-*') + h22.grid(); h22.axis('tight') + h22.set(title='Correlation results', xlabel='Time (s)') + h22.legend(['E', 'P', 'L']) + + # PLL filtered + h31.plot(timeAxisInSeconds, trackResults[channelNr].pllDiscrFilt, 'b') + h31.grid(); h31.axis('tight') + h31.set(title='Filtered PLL discriminator', xlabel='Time (s)') + + # DLL raw + h32.plot(timeAxisInSeconds, trackResults[channelNr].dllDiscr, 'r') + h32.grid(); h32.axis('tight') + h32.set(title='Raw DLL discriminator', xlabel='Time (s)') + + # DLL filtered + h33.plot(timeAxisInSeconds, trackResults[channelNr].dllDiscrFilt, 'b') + h33.grid(); h33.axis('tight') + h33.set(title='Filtered DLL discriminator', xlabel='Time (s)') + + plt.tight_layout() + f.show() \ No newline at end of file