├── LICENSE ├── README.md ├── data ├── phone │ ├── Pixel4_GnssLog.obs │ ├── nav_1350.nav │ └── slac1350.obs └── u-blox │ ├── demo5_b34f_f.pos │ ├── py_0315f.pos │ ├── rover.nav │ ├── rover.obs │ └── tmg23590.obs └── src ├── config_f9p.py ├── config_phone.py ├── ephemeris.py ├── mlambda.py ├── pntpos.py ├── postpos.py ├── rinex.py ├── rtkcmn.py ├── rtkpos.py └── run_ppk.py /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Rui Hirokawa (from CSSRLIB) 4 | Copyright (c) 2022 Tim Everett 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## **rtklib-py : A python subset of RTKLIB for PPK solutions** 2 | 3 | 4 | ### **What is rtklib-py?:** 5 | 6 | Rtklib-py is a free and open source subset of RTKLIB written in Python. It was originally based on CSSRlib but has been rewritten to align very closely with the demo5 version of RTKLIB. It currently supports only PPK solutions using GPS, GLonass, and Galileo constellations. It supports most but not all options available in PPK solutions in RTKLIB and produces solutions very similar but not identical to the RTKLIB solutions. Debug trace is supported and trace level 3 messages are aligned to closely match the demo5 RTKLIB messages. Funtction names, comments, and variables are also closely aligned to the demo5 RTKLIB code. 7 | 8 | ### **How to use:** 9 | 10 | Run_ppk.py is the top level script to run a PPK solution. The rtklib-py package includes two sample data sets, one is a u-blox F9P rover mounted on the roof of a car, the second is a data set from the 2021 Google Smartphone Decimeter Challenge. Run_ppk.py is configured to generate a solution for the u-blox data set but includes commented out lines to run the GSDC example. Config parameters are in config_f9p.py and config_phone.py and align closely to the config parameters in RTKLIB. 11 | 12 | ### **Purpose:** 13 | 14 | This code is not meant to be a replacement for RTKLIB but as a tool to either: 15 | 1) Use as a "map" to explore the inner details of how RTKLIB works 16 | 2) Use as a development environment to experiment with enhancements or adjustments to the RTKLIB algorithms. Due to the close alignment between the two packages, these can then be fairly easily ported to the C/C++ version of RTKLIB. In particular it is hoped that it will be useful to competitors in the Google Smartphone Decimeter Challenge. 17 | 3) Pieces of this code can be cut and paste into more custom solutions 18 | -------------------------------------------------------------------------------- /data/u-blox/rover.nav: -------------------------------------------------------------------------------- 1 | 3.03 N: GNSS NAV DATA M: Mixed RINEX VERSION / TYPE 2 | RTKCONV demo5 b34f.1 20220412 001045 UTC PGM / RUN BY / DATE 3 | format: u-blox UBX COMMENT 4 | log: C:\gps\data\ublox\ppk_f9p_1224\rover_2128.ubx COMMENT 5 | END OF HEADER 6 | G22 2020 12 24 22 00 00 -.709892716259D-03 .625277607469D-11 .000000000000D+00 7 | .580000000000D+02 .668750000000D+01 .522843207090D-08 -.114779932222D+01 8 | .415369868279D-06 .680570665281D-02 .720098614693D-05 .515364265060D+04 9 | .424800000000D+06 -.117346644402D-06 .261039981660D+00 .149011611938D-07 10 | .933333232093D+00 .227812500000D+03 -.106037935205D+01 -.845713798803D-08 11 | .482877256633D-09 .100000000000D+01 .213700000000D+04 .000000000000D+00 12 | .200000000000D+01 .000000000000D+00 -.181607902050D-07 .580000000000D+02 13 | .423006000000D+06 .400000000000D+01 14 | R06 2020 12 24 21 15 00 .189751386642D-03 .000000000000D+00 .422910000000D+06 15 | -.740158740234D+04 -.212037086487D+00 .000000000000D+00 .000000000000D+00 16 | -.206682856445D+05 -.176755714417D+01 .931322574615D-09 -.400000000000D+01 17 | .129489067383D+05 -.294115734100D+01 -.186264514923D-08 .000000000000D+00 18 | R16 2020 12 24 21 15 00 -.949110835791D-05 -.909494701773D-12 .422910000000D+06 19 | -.222022216797D+04 .266159534454D+01 .931322574615D-09 .000000000000D+00 20 | -.129479941406D+05 .137418651581D+01 .931322574615D-09 -.100000000000D+01 21 | .218170019531D+05 .109746074677D+01 -.186264514923D-08 .000000000000D+00 22 | R09 2020 12 24 21 15 00 .148760154843D-04 .272848410532D-11 .422910000000D+06 23 | -.173907675781D+05 .128048324585D+01 -.931322574615D-09 .000000000000D+00 24 | -.151404604492D+05 .788305282593D+00 .931322574615D-09 -.200000000000D+01 25 | .109964853516D+05 .310873985291D+01 -.186264514923D-08 .000000000000D+00 26 | R07 2020 12 24 21 15 00 -.419346615672D-04 .000000000000D+00 .422910000000D+06 27 | -.109031464844D+05 -.130529403687D+00 .000000000000D+00 .000000000000D+00 28 | -.485899951172D+04 -.307977771759D+01 .931322574615D-09 .500000000000D+01 29 | .225662275391D+05 -.731439590454D+00 -.279396772385D-08 .000000000000D+00 30 | R05 2020 12 24 21 15 00 .635022297502D-04 .909494701773D-12 .422910000000D+06 31 | .218185791016D+04 -.147454261780D+00 .000000000000D+00 .000000000000D+00 32 | -.246303710937D+05 .856179237366D+00 .931322574615D-09 .100000000000D+01 33 | -.620753564453D+04 -.344900512695D+01 .000000000000D+00 .000000000000D+00 34 | R17 2020 12 24 21 15 00 .385288149118D-03 .363797880709D-11 .422910000000D+06 35 | .191498818359D+05 -.202003002167D+01 .372529029846D-08 .000000000000D+00 36 | -.291266357422D+04 .105937194824D+01 .000000000000D+00 .400000000000D+01 37 | .166359541016D+05 .250929546356D+01 .000000000000D+00 .000000000000D+00 38 | R15 2020 12 24 21 15 00 .106218270957D-03 .000000000000D+00 .422910000000D+06 39 | .154658706055D+05 .246572017670D+01 .372529029846D-08 .000000000000D+00 40 | -.263840576172D+04 .122387695313D+01 .000000000000D+00 .000000000000D+00 41 | .201336059570D+05 -.173468780518D+01 -.931322574615D-09 .000000000000D+00 42 | R10 2020 12 24 21 15 00 -.649830326438D-04 .000000000000D+00 .422910000000D+06 43 | -.231698593750D+05 -.755181312561D+00 -.279396772385D-08 .000000000000D+00 44 | -.895314160156D+04 -.220763206482D+00 .000000000000D+00 -.700000000000D+01 45 | -.563787402344D+04 .346592521667D+01 -.931322574615D-09 .000000000000D+00 46 | R18 2020 12 24 21 15 00 .607930123806D-04 .909494701773D-12 .422910000000D+06 47 | .220076791992D+05 -.260876655579D+00 .372529029846D-08 .000000000000D+00 48 | -.127902641602D+05 -.116349220276D+00 .000000000000D+00 -.300000000000D+01 49 | .120270068359D+04 .360437488556D+01 .931322574615D-09 .000000000000D+00 50 | R08 2020 12 24 21 15 00 -.564567744732D-04 .000000000000D+00 .422910000000D+06 51 | -.904162060547D+04 -.250625610352D-02 .000000000000D+00 .000000000000D+00 52 | .132546889648D+05 -.271166992188D+01 .000000000000D+00 .600000000000D+01 53 | .198846879883D+05 .180852222443D+01 -.186264514923D-08 .000000000000D+00 54 | E19 2020 12 24 21 10 00 .161242962349D-03 .952127265919D-11 .000000000000D+00 55 | .630000000000D+02 -.411875000000D+02 .348300222388D-08 .312378157038D+00 56 | -.185705721378D-05 .137230381370D-03 .573322176933D-05 .544060756302D+04 57 | .421800000000D+06 .335276126862D-07 .117922388366D+01 .186264514923D-08 58 | .959673079424D+00 .216312500000D+03 .234242526824D+01 -.574559647001D-08 59 | -.115004790411D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 60 | .312000000000D+01 .000000000000D+00 -.395812094212D-08 -.372529029846D-08 61 | .422935000000D+06 .000000000000D+00 62 | E21 2020 12 24 20 40 00 -.639071222395D-03 -.207478478842D-11 .000000000000D+00 63 | .600000000000D+02 .141968750000D+03 .300905391060D-08 .124848843189D+01 64 | .675208866596D-05 .169608858414D-03 .603310763836D-05 .544062762451D+04 65 | .420000000000D+06 .558793544769D-07 -.300905658244D+01 -.335276126862D-07 66 | .979952961237D+00 .218406250000D+03 -.227410845100D+00 -.565702135193D-08 67 | .181078971237D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 68 | .312000000000D+01 .000000000000D+00 .209547579288D-08 .232830643654D-08 69 | .422935000000D+06 .000000000000D+00 70 | E04 2020 12 24 21 10 00 -.674740062095D-03 -.784439180279D-11 .000000000000D+00 71 | .630000000000D+02 -.415000000000D+02 .355943397900D-08 -.193715704602D+00 72 | -.198185443878D-05 .884484034032D-04 .519305467606D-05 .544061036682D+04 73 | .421800000000D+06 .391155481339D-07 .117952300067D+01 -.242143869400D-07 74 | .953976084362D+00 .222281250000D+03 .198169021804D+01 -.579381276413D-08 75 | -.102504269714D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 76 | .312000000000D+01 .000000000000D+00 -.325962901115D-08 -.349245965481D-08 77 | .422935000000D+06 .000000000000D+00 78 | E01 2020 12 24 21 10 00 -.101033999817D-02 -.801492205937D-11 .000000000000D+00 79 | .630000000000D+02 .142500000000D+03 .296155193195D-08 .924328497329D+00 80 | .667013227940D-05 .226532109082D-03 .602379441261D-05 .544061828804D+04 81 | .421800000000D+06 -.558793544769D-08 -.300626046030D+01 -.912696123123D-07 82 | .980383427808D+00 .217937500000D+03 -.443459629725D+00 -.561844831664D-08 83 | .162506769059D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 84 | .312000000000D+01 .000000000000D+00 .000000000000D+00 .000000000000D+00 85 | .422935000000D+06 .000000000000D+00 86 | E31 2020 12 24 21 10 00 -.476772431284D-03 -.369482222595D-12 .000000000000D+00 87 | .630000000000D+02 .132906250000D+03 .297976697640D-08 .731451002362D+00 88 | .618770718575D-05 .308267422952D-03 .631809234619D-05 .544062285614D+04 89 | .421800000000D+06 -.633299350739D-07 -.300911187196D+01 -.260770320892D-07 90 | .979929178578D+00 .213968750000D+03 -.105293749802D+01 -.555773150183D-08 91 | .170007081477D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 92 | .312000000000D+01 .000000000000D+00 .465661287308D-08 .535510480404D-08 93 | .422935000000D+06 .000000000000D+00 94 | E09 2020 12 24 21 10 00 .582181022037D-02 -.125908172777D-10 .000000000000D+00 95 | .630000000000D+02 -.443750000000D+02 .346192991756D-08 .524114904312D+00 96 | -.222958624363D-05 .258303596638D-03 .553391873837D-05 .544061374664D+04 97 | .421800000000D+06 .000000000000D+00 .117397449618D+01 -.279396772385D-07 98 | .959939865943D+00 .221156250000D+03 .464049442436D+00 -.572738142557D-08 99 | -.828605943335D-10 .513000000000D+03 .213700000000D+04 .000000000000D+00 100 | .312000000000D+01 .000000000000D+00 .186264514923D-08 .186264514923D-08 101 | .422935000000D+06 .000000000000D+00 102 | E27 2020 12 24 21 10 00 .654500909150D-04 -.784439180279D-11 .000000000000D+00 103 | .630000000000D+02 .139250000000D+03 .303191200559D-08 .257613186468D+01 104 | .654160976410D-05 .311276293360D-03 .569224357605D-05 .544062637138D+04 105 | .421800000000D+06 -.931322574615D-08 -.300906557353D+01 -.316649675369D-07 106 | .979925923585D+00 .228343750000D+03 -.540881850220D+00 -.562773441773D-08 107 | .210723063176D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 108 | .312000000000D+01 .000000000000D+00 .186264514923D-08 .209547579288D-08 109 | .422944000000D+06 .000000000000D+00 110 | G04 2020 12 24 22 00 00 -.167160294950D-03 -.318323145621D-11 .000000000000D+00 111 | .228000000000D+03 -.317812500000D+02 .471769651100D-08 -.585116333861D+00 112 | -.165030360222D-05 .100494129583D-02 .721216201782D-05 .515359848213D+04 113 | .424800000000D+06 .391155481339D-07 .143495621420D+01 -.167638063431D-07 114 | .959788739191D+00 .237937500000D+03 -.307413575716D+01 -.809783730743D-08 115 | -.220366321999D-09 .100000000000D+01 .213700000000D+04 .000000000000D+00 116 | .200000000000D+01 .000000000000D+00 -.419095158577D-08 .996000000000D+03 117 | .422946000000D+06 .400000000000D+01 118 | G09 2020 12 24 22 00 00 -.305205583572D-03 -.432009983342D-11 .000000000000D+00 119 | .160000000000D+02 -.327187500000D+02 .490556147918D-08 .298904311410D+00 120 | -.184588134289D-05 .203400733881D-02 .680610537529D-05 .515359122658D+04 121 | .424800000000D+06 -.111758708954D-07 .138533741961D+01 -.484287738800D-07 122 | .952600936978D+00 .241937500000D+03 .178507170946D+01 -.822069956799D-08 123 | -.190365072327D-09 .100000000000D+01 .213700000000D+04 .000000000000D+00 124 | .200000000000D+01 .000000000000D+00 .139698386192D-08 .160000000000D+02 125 | .422946000000D+06 .400000000000D+01 126 | G16 2020 12 24 22 00 00 -.255135353655D-03 -.545696821064D-11 .000000000000D+00 127 | .140000000000D+02 .712812500000D+02 .445304262996D-08 .778313236510D+00 128 | .399537384510D-05 .119238231564D-01 .455975532532D-05 .515362344551D+04 129 | .424800000000D+06 -.763684511185D-07 -.269722208818D+01 .124797224998D-06 130 | .976002463609D+00 .297968750000D+03 .635814759071D+00 -.831427489435D-08 131 | -.464305054455D-10 .100000000000D+01 .213700000000D+04 .000000000000D+00 132 | .200000000000D+01 .000000000000D+00 -.107102096081D-07 .140000000000D+02 133 | .422946000000D+06 .400000000000D+01 134 | G27 2020 12 24 22 00 00 -.418373383582D-04 -.795807864051D-11 .000000000000D+00 135 | .390000000000D+02 -.545000000000D+02 .436768193148D-08 -.126382303325D+00 136 | -.283867120743D-05 .871952157468D-02 .434741377831D-05 .515367447281D+04 137 | .424800000000D+06 .707805156708D-07 -.174032365233D+01 .104308128357D-06 138 | .976824584071D+00 .300687500000D+03 .564247830867D+00 -.830356016232D-08 139 | -.346800159904D-09 .100000000000D+01 .213700000000D+04 .000000000000D+00 140 | .200000000000D+01 .000000000000D+00 .186264514923D-08 .390000000000D+02 141 | .422946000000D+06 .400000000000D+01 142 | G07 2020 12 24 21 59 44 -.489968806505D-05 .150066625793D-10 .000000000000D+00 143 | .200000000000D+01 .413437500000D+02 .457090468224D-08 -.297573812872D+01 144 | .199861824512D-05 .142937914934D-01 .104885548353D-04 .515362238693D+04 145 | .424784000000D+06 .279396772385D-07 .245871505627D+01 -.249594449997D-06 146 | .952233334923D+00 .173062500000D+03 -.235821654580D+01 -.774032241550D-08 147 | -.650027076237D-10 .100000000000D+01 .213700000000D+04 .000000000000D+00 148 | .200000000000D+01 .630000000000D+02 -.111758708954D-07 .200000000000D+01 149 | .422946000000D+06 .400000000000D+01 150 | G26 2020 12 24 22 00 00 .195233151317D-04 .579802872380D-11 .000000000000D+00 151 | .680000000000D+02 .843437500000D+02 .516842957155D-08 .174030437985D+01 152 | .455603003502D-05 .549922895152D-02 .494532287121D-05 .515372993469D+04 153 | .424800000000D+06 .186264514923D-08 -.282677345682D+01 .931322574615D-07 154 | .944212614451D+00 .276593750000D+03 .232211780896D+00 -.852964100807D-08 155 | .500020827875D-10 .100000000000D+01 .213700000000D+04 .000000000000D+00 156 | .200000000000D+01 .000000000000D+00 .698491930962D-08 .680000000000D+02 157 | .422946000000D+06 .400000000000D+01 158 | G03 2020 12 24 22 00 00 -.384999439120D-04 -.989075488178D-11 .000000000000D+00 159 | .104000000000D+03 .365625000000D+01 .440518349357D-08 .278019632492D+01 160 | .210478901863D-06 .325109227560D-02 .697933137417D-05 .515368109322D+04 161 | .424800000000D+06 .149011611938D-07 .355033638588D+00 -.409781932831D-07 162 | .967965943621D+00 .247781250000D+03 .885500701436D+00 -.792997317236D-08 163 | .415374444870D-09 .100000000000D+01 .213700000000D+04 .000000000000D+00 164 | .200000000000D+01 .000000000000D+00 .186264514923D-08 .104000000000D+03 165 | .422946000000D+06 .400000000000D+01 166 | G08 2020 12 24 22 00 00 -.409083440900D-05 -.136424205266D-11 .000000000000D+00 167 | .910000000000D+02 -.547500000000D+02 .448090093322D-08 -.562383028710D-01 168 | -.298954546452D-05 .595481344499D-02 .419095158577D-05 .515376763916D+04 169 | .424800000000D+06 .121071934700D-06 -.175632312070D+01 .149011611938D-07 170 | .968317371654D+00 .302562500000D+03 -.573514474780D-01 -.832070373357D-08 171 | -.345014371233D-09 .100000000000D+01 .213700000000D+04 .000000000000D+00 172 | .280000000000D+01 .000000000000D+00 .512227416039D-08 .910000000000D+02 173 | .422946000000D+06 .400000000000D+01 174 | R06 2020 12 24 21 45 00 .189752317965D-03 .000000000000D+00 .423000000000D+06 175 | -.790920947266D+04 -.313253402710D+00 .000000000000D+00 .000000000000D+00 176 | -.231306713867D+05 -.950122833252D+00 .931322574615D-09 -.400000000000D+01 177 | .722159716797D+04 -.338109016418D+01 -.931322574615D-09 .000000000000D+00 178 | R16 2020 12 24 21 45 00 -.949297100306D-05 .000000000000D+00 .423000000000D+06 179 | .288844677734D+04 .297067356110D+01 .186264514923D-08 .000000000000D+00 180 | -.107623632812D+05 .102819538116D+01 .000000000000D+00 -.100000000000D+01 181 | .229200698242D+05 .120542526245D+00 -.186264514923D-08 .000000000000D+00 182 | R07 2020 12 24 21 45 00 -.419290736318D-04 .000000000000D+00 .423000000000D+06 183 | -.115108208008D+05 -.526154518127D+00 .000000000000D+00 .000000000000D+00 184 | -.101370839844D+05 -.273774528503D+01 .931322574615D-09 .500000000000D+01 185 | .203962021484D+05 -.166425132751D+01 -.279396772385D-08 .000000000000D+00 186 | R05 2020 12 24 21 45 00 .635031610727D-04 .909494701773D-12 .423000000000D+06 187 | .212050830078D+04 .114978790283D+00 .000000000000D+00 .000000000000D+00 188 | -.223425649414D+05 .166139507294D+01 .931322574615D-09 .100000000000D+01 189 | -.120949946289D+05 -.305007362366D+01 .931322574615D-09 .000000000000D+00 190 | R17 2020 12 24 21 45 00 .385294668376D-03 .363797880709D-11 .423000000000D+06 191 | .152738876953D+05 -.223753356934D+01 .279396772385D-08 .000000000000D+00 192 | -.439754882813D+03 .168400478363D+01 .000000000000D+00 .400000000000D+01 193 | .204532568359D+05 .170466423035D+01 -.931322574615D-09 .000000000000D+00 194 | R15 2020 12 24 21 45 00 .106218270957D-03 .000000000000D+00 .423000000000D+06 195 | .196385537109D+05 .212295532227D+01 .372529029846D-08 .000000000000D+00 196 | -.936584472656D+03 .672541618347D+00 .000000000000D+00 .000000000000D+00 197 | .162746220703D+05 -.252532863617D+01 .000000000000D+00 .000000000000D+00 198 | R10 2020 12 24 21 45 00 -.649839639664D-04 .000000000000D+00 .423000000000D+06 199 | -.238293007812D+05 .438880920410D-01 -.186264514923D-08 .000000000000D+00 200 | -.895770019531D+04 .180678367615D+00 .931322574615D-09 -.700000000000D+01 201 | .738772460938D+03 .357301902771D+01 -.186264514923D-08 .000000000000D+00 202 | R18 2020 12 24 21 45 00 .607948750257D-04 .909494701773D-12 .423000000000D+06 203 | .208929526367D+05 -.945283889771D+00 .372529029846D-08 .000000000000D+00 204 | -.124918759766D+05 .476171493530D+00 .000000000000D+00 -.300000000000D+01 205 | .755989257813D+04 .341320419312D+01 .000000000000D+00 .000000000000D+00 206 | R08 2020 12 24 21 45 00 -.564558431506D-04 .000000000000D+00 .423000000000D+06 207 | -.944211816406D+04 -.453409194946D+00 .000000000000D+00 .000000000000D+00 208 | .806034912109D+04 -.301100921631D+01 .000000000000D+00 .600000000000D+01 209 | .223334560547D+05 .894792556763D+00 -.186264514923D-08 .000000000000D+00 210 | E14 2020 12 24 21 10 00 -.127097580116D-02 -.131592514663D-10 .000000000000D+00 211 | .630000000000D+02 -.707812500000D+02 .169828502610D-08 -.211459797571D+01 212 | -.248663127422D-05 .165634189034D+00 .393204391003D-05 .528936037636D+04 213 | .421800000000D+06 .532902777195D-05 -.167806908570D+01 -.135414302349D-05 214 | .882318588362D+00 .230468750000D+03 .185592073402D+01 -.168899892501D-08 215 | .190007914592D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 216 | .312000000000D+01 .000000000000D+00 -.325962901115D-08 -.372529029846D-08 217 | .423055000000D+06 .000000000000D+00 218 | E19 2020 12 24 21 20 00 .161248608492D-03 .952127265919D-11 .000000000000D+00 219 | .640000000000D+02 -.412812500000D+02 .348121643521D-08 .387024425395D+00 220 | -.186450779438D-05 .137205701321D-03 .575743615627D-05 .544060747528D+04 221 | .422400000000D+06 .353902578354D-07 .117922042971D+01 .186264514923D-08 222 | .959673020908D+00 .215968750000D+03 .234216479567D+01 -.574381068134D-08 223 | -.115361948145D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 224 | .312000000000D+01 .000000000000D+00 -.395812094212D-08 -.372529029846D-08 225 | .423064000000D+06 .000000000000D+00 226 | E01 2020 12 24 21 20 00 -.101034482941D-02 -.801492205937D-11 .000000000000D+00 227 | .640000000000D+02 .143437500000D+03 .296190908969D-08 .998046023366D+00 228 | .671856105328D-05 .226532807574D-03 .606477260590D-05 .544061858749D+04 229 | .422400000000D+06 .745058059692D-08 -.300626385866D+01 -.875443220139D-07 230 | .980383480473D+00 .217187500000D+03 -.442791971469D+00 -.562273420945D-08 231 | .170007081477D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 232 | .312000000000D+01 .000000000000D+00 .000000000000D+00 .000000000000D+00 233 | .423064000000D+06 .000000000000D+00 234 | E04 2020 12 24 21 20 00 -.674744660500D-03 -.784439180279D-11 .000000000000D+00 235 | .640000000000D+02 -.409375000000D+02 .355836250580D-08 -.118989130821D+00 236 | -.193528831005D-05 .883841421455D-04 .519864261150D-05 .544061022377D+04 237 | .422400000000D+06 .409781932831D-07 .117951955111D+01 -.204890966415D-07 238 | .953976021456D+00 .222218750000D+03 .198134934350D+01 -.579631286827D-08 239 | -.104290058385D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 240 | .312000000000D+01 .000000000000D+00 -.325962901115D-08 -.349245965481D-08 241 | .423064000000D+06 .000000000000D+00 242 | E09 2020 12 24 21 20 00 .582180288620D-02 -.125908172777D-10 .000000000000D+00 243 | .640000000000D+02 -.442500000000D+02 .346085844436D-08 .598962480437D+00 244 | -.222213566303D-05 .258250045590D-03 .546686351299D-05 .544061348724D+04 245 | .422400000000D+06 .186264514923D-08 .117397106856D+01 -.298023223877D-07 246 | .959939823518D+00 .222156250000D+03 .463587337336D+00 -.572738142557D-08 247 | -.842892252703D-10 .513000000000D+03 .213700000000D+04 .000000000000D+00 248 | .312000000000D+01 .000000000000D+00 .186264514923D-08 .186264514923D-08 249 | .423064000000D+06 .000000000000D+00 250 | E31 2020 12 24 21 20 00 -.476772664115D-03 -.383693077310D-12 .000000000000D+00 251 | .640000000000D+02 .133031250000D+03 .297655255679D-08 .805731013085D+00 252 | .619515776634D-05 .308275921270D-03 .630505383015D-05 .544062292099D+04 253 | .422400000000D+06 -.596046447754D-07 -.300911521766D+01 -.204890966415D-07 254 | .979929311703D+00 .214187500000D+03 -.105283259656D+01 -.555308845128D-08 255 | .168578450541D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 256 | .312000000000D+01 .000000000000D+00 .465661287308D-08 .535510480404D-08 257 | .423064000000D+06 .000000000000D+00 258 | E27 2020 12 24 21 20 00 .654453760944D-04 -.784439180279D-11 .000000000000D+00 259 | .640000000000D+02 .137906250000D+03 .302584032411D-08 .265057883678D+01 260 | .648945569992D-05 .311065465212D-03 .565126538277D-05 .544062679100D+04 261 | .422400000000D+06 -.745058059692D-08 -.300906892362D+01 -.260770320892D-07 262 | .979926040619D+00 .229406250000D+03 -.540943892576D+00 -.563094883734D-08 263 | .208937274505D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 264 | .312000000000D+01 .000000000000D+00 .186264514923D-08 .209547579288D-08 265 | .423064000000D+06 .000000000000D+00 266 | E14 2020 12 24 21 20 00 -.127098360099D-02 -.131450406116D-10 .000000000000D+00 267 | .640000000000D+02 -.650312500000D+02 .170149944571D-08 -.203365451797D+01 268 | -.227615237236D-05 .165634823032D+00 .439397990704D-05 .528935822868D+04 269 | .422400000000D+06 .595860183239D-05 -.167807019166D+01 -.178813934326D-06 270 | .882319378338D+00 .222593750000D+03 .185592722645D+01 -.166506935682D-08 271 | -.113219001740D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 272 | .312000000000D+01 .000000000000D+00 -.325962901115D-08 -.372529029846D-08 273 | .423064000000D+06 .000000000000D+00 274 | G30 2020 12 24 22 00 00 -.357816927135D-03 -.625277607469D-11 .000000000000D+00 275 | .850000000000D+02 .429687500000D+02 .497985028790D-08 -.298084189880D+01 276 | .228174030781D-05 .474222050980D-02 .102706253529D-04 .515377185059D+04 277 | .424800000000D+06 -.391155481339D-07 .247820206281D+01 -.521540641785D-07 278 | .937497337704D+00 .172406250000D+03 -.284670366317D+01 -.806140721854D-08 279 | -.650027076237D-10 .100000000000D+01 .213700000000D+04 .000000000000D+00 280 | .200000000000D+01 .000000000000D+00 .372529029846D-08 .850000000000D+02 281 | .422976000000D+06 .400000000000D+01 282 | E19 2020 12 24 21 30 00 .161254312843D-03 .952127265919D-11 .000000000000D+00 283 | .650000000000D+02 -.414062500000D+02 .347943064654D-08 .461602881647D+00 284 | -.187195837498D-05 .137191032991D-03 .577233731747D-05 .544060741425D+04 285 | .423000000000D+06 .372529029846D-07 .117921697576D+01 .372529029846D-08 286 | .959672963854D+00 .215781250000D+03 .234197214692D+01 -.574166773494D-08 287 | -.115361948145D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 288 | .312000000000D+01 .000000000000D+00 -.395812094212D-08 -.372529029846D-08 289 | .423664000000D+06 .000000000000D+00 290 | E01 2020 12 24 21 30 00 -.101034966065D-02 -.801492205937D-11 .000000000000D+00 291 | .650000000000D+02 .144062500000D+03 .296262340516D-08 .107191438942D+01 292 | .675395131111D-05 .226523610763D-03 .610202550888D-05 .544061881828D+04 293 | .423000000000D+06 .186264514923D-07 -.300626726287D+01 -.838190317154D-07 294 | .980383538990D+00 .216500000000D+03 -.442275189805D+00 -.562630578679D-08 295 | .177150236161D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 296 | .312000000000D+01 .000000000000D+00 .000000000000D+00 .000000000000D+00 297 | .423664000000D+06 .000000000000D+00 298 | E04 2020 12 24 21 30 00 -.674749142490D-03 -.783018094808D-11 .000000000000D+00 299 | .650000000000D+02 -.404062500000D+02 .355764819033D-08 -.442006975479D-01 300 | -.189058482647D-05 .883202301338D-04 .521168112755D-05 .544061007881D+04 301 | .423000000000D+06 .428408384323D-07 .117951609862D+01 -.167638063431D-07 302 | .953975954162D+00 .222062500000D+03 .198094661239D+01 -.579917013014D-08 303 | -.106075847056D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 304 | .312000000000D+01 .000000000000D+00 -.325962901115D-08 -.349245965481D-08 305 | .423664000000D+06 .000000000000D+00 306 | E31 2020 12 24 21 30 00 -.476772955153D-03 -.383693077310D-12 .000000000000D+00 307 | .650000000000D+02 .133406250000D+03 .297476676812D-08 .879885201150D+00 308 | .621192157269D-05 .308287213556D-03 .628456473351D-05 .544062306213D+04 309 | .423000000000D+06 -.614672899246D-07 -.300911854872D+01 -.186264514923D-07 310 | .979929417034D+00 .214625000000D+03 -.105260185488D+01 -.555273129355D-08 311 | .168221292806D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 312 | .312000000000D+01 .000000000000D+00 .465661287308D-08 .535510480404D-08 313 | .423664000000D+06 .000000000000D+00 314 | E09 2020 12 24 21 30 00 .582179549383D-02 -.125766064230D-10 .000000000000D+00 315 | .650000000000D+02 -.440000000000D+02 .346014412889D-08 .673817157566D+00 316 | -.220537185669D-05 .258206157014D-03 .540353357792D-05 .544061322975D+04 317 | .423000000000D+06 .186264514923D-08 .117396764387D+01 -.335276126862D-07 318 | .959939779631D+00 .223125000000D+03 .463118138547D+00 -.572773858330D-08 319 | -.864321716755D-10 .513000000000D+03 .213700000000D+04 .000000000000D+00 320 | .312000000000D+01 .000000000000D+00 .186264514923D-08 .186264514923D-08 321 | .423664000000D+06 .000000000000D+00 322 | E14 2020 12 24 21 30 00 -.127099122619D-02 -.131308297568D-10 .000000000000D+00 323 | .650000000000D+02 -.600625000000D+02 .182471886401D-08 -.195270888047D+01 324 | -.201538205147D-05 .165635163430D+00 .477023422718D-05 .528935695267D+04 325 | .423000000000D+06 .613555312157D-05 -.167807056471D+01 .126101076603D-05 326 | .882318973110D+00 .217375000000D+03 .185593106808D+01 -.186293474157D-08 327 | -.436803908922D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 328 | .312000000000D+01 .000000000000D+00 -.325962901115D-08 -.372529029846D-08 329 | .423664000000D+06 .000000000000D+00 330 | E19 2020 12 24 21 40 00 .161260017194D-03 .952127265919D-11 .000000000000D+00 331 | .660000000000D+02 -.414687500000D+02 .347693054240D-08 .536076048759D+00 332 | -.187940895557D-05 .137185561471D-03 .578165054321D-05 .544060738754D+04 333 | .423600000000D+06 .409781932831D-07 .117921352620D+01 .745058059692D-08 334 | .959672909726D+00 .215687500000D+03 .234188479754D+01 -.573916763080D-08 335 | -.116076263614D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 336 | .312000000000D+01 .000000000000D+00 -.395812094212D-08 -.372529029846D-08 337 | .424264000000D+06 .000000000000D+00 338 | E01 2020 12 24 21 40 00 -.101035443367D-02 -.801492205937D-11 .000000000000D+00 339 | .660000000000D+02 .144406250000D+03 .296405203609D-08 .114595008405D+01 340 | .677257776260D-05 .226510805078D-03 .613369047642D-05 .544061897850D+04 341 | .423600000000D+06 .298023223877D-07 -.300627067001D+01 -.763684511185D-07 342 | .980383604821D+00 .215968750000D+03 -.441925773285D+00 -.562916304866D-08 343 | .184293390845D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 344 | .312000000000D+01 .000000000000D+00 .000000000000D+00 .000000000000D+00 345 | .424264000000D+06 .000000000000D+00 346 | E04 2020 12 24 21 40 00 -.674753799103D-03 -.783018094808D-11 .000000000000D+00 347 | .660000000000D+02 -.398750000000D+02 .355657671712D-08 .306446095910D-01 348 | -.184774398804D-05 .882574822754D-04 .523030757904D-05 .544060992813D+04 349 | .423600000000D+06 .428408384323D-07 .117951264321D+01 -.111758708954D-07 350 | .953975885405D+00 .221812500000D+03 .198048701180D+01 -.580167023428D-08 351 | -.107147320259D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 352 | .312000000000D+01 .000000000000D+00 -.325962901115D-08 -.349245965481D-08 353 | .424264000000D+06 .000000000000D+00 354 | E09 2020 12 24 21 40 00 .582178804325D-02 -.125908172777D-10 .000000000000D+00 355 | .660000000000D+02 -.436250000000D+02 .345942981342D-08 .748666221479D+00 356 | -.218115746975D-05 .258172512986D-03 .534392893314D-05 .544061298180D+04 357 | .423600000000D+06 .372529029846D-08 .117396422357D+01 -.353902578354D-07 358 | .959939738669D+00 .224031250000D+03 .462654563214D+00 -.572845289877D-08 359 | -.889322758148D-10 .513000000000D+03 .213700000000D+04 .000000000000D+00 360 | .312000000000D+01 .000000000000D+00 .186264514923D-08 .186264514923D-08 361 | .424264000000D+06 .000000000000D+00 362 | E31 2020 12 24 21 40 00 -.476773071568D-03 -.383693077310D-12 .000000000000D+00 363 | .660000000000D+02 .134062500000D+03 .297440961038D-08 .953916559688D+00 364 | .624358654022D-05 .308295479044D-03 .625848770142D-05 .544062327766D+04 365 | .423600000000D+06 -.633299350739D-07 -.300912187832D+01 -.242143869400D-07 366 | .979929491642D+00 .215187500000D+03 -.105224826319D+01 -.555701718636D-08 367 | .169649923743D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 368 | .312000000000D+01 .000000000000D+00 .465661287308D-08 .535510480404D-08 369 | .424264000000D+06 .000000000000D+00 370 | G09 2020 12 25 00 00 00 -.305236782879D-03 -.432009983342D-11 .000000000000D+00 371 | .170000000000D+02 -.301250000000D+02 .487270296764D-08 .134933298392D+01 372 | -.156089663506D-05 .203371921089D-02 .697746872902D-05 .515359030533D+04 373 | .432000000000D+06 .335276126862D-07 .138527823140D+01 -.596046447754D-07 374 | .952599596945D+00 .239531250000D+03 .178487723791D+01 -.820534178542D-08 375 | -.195365280605D-09 .100000000000D+01 .213700000000D+04 .000000000000D+00 376 | .200000000000D+01 .000000000000D+00 .139698386192D-08 .170000000000D+02 377 | .424806000000D+06 .400000000000D+01 378 | G04 2020 12 25 00 00 00 -.167183578014D-03 -.318323145621D-11 .000000000000D+00 379 | .229000000000D+03 -.294687500000D+02 .469769567788D-08 .464948411448D+00 380 | -.151246786118D-05 .100479461253D-02 .734440982342D-05 .515359819031D+04 381 | .432000000000D+06 .502914190292D-07 .143489794910D+01 .130385160446D-07 382 | .959787175331D+00 .236750000000D+03 -.307397198787D+01 -.808747973314D-08 383 | -.227509476683D-09 .100000000000D+01 .213700000000D+04 .000000000000D+00 384 | .200000000000D+01 .000000000000D+00 -.419095158577D-08 .997000000000D+03 385 | .424806000000D+06 .400000000000D+01 386 | G16 2020 12 25 00 00 00 -.255174934864D-03 -.545696821064D-11 .000000000000D+00 387 | .150000000000D+02 .685000000000D+02 .447590072495D-08 .182848753537D+01 388 | .357069075108D-05 .119238679763D-01 .492483377457D-05 .515362427711D+04 389 | .432000000000D+06 -.311061739922D-06 -.269728252133D+01 .428408384323D-07 390 | .976002725471D+00 .296468750000D+03 .635852081037D+00 -.819927010394D-08 391 | .867893294096D-10 .100000000000D+01 .213700000000D+04 .000000000000D+00 392 | .200000000000D+01 .000000000000D+00 -.107102096081D-07 .150000000000D+02 393 | .424806000000D+06 .400000000000D+01 394 | G27 2020 12 25 00 00 00 -.418946146965D-04 -.795807864051D-11 .000000000000D+00 395 | .400000000000D+02 -.461562500000D+02 .436553898508D-08 .923733707022D+00 396 | -.220350921154D-05 .872069480829D-02 .462494790554D-05 .515367694092D+04 397 | .432000000000D+06 .372529029846D-08 -.174038306436D+01 .115483999252D-06 398 | .976822040056D+00 .296812500000D+03 .564310317950D+00 -.826963017758D-08 399 | -.389659088008D-09 .100000000000D+01 .213700000000D+04 .000000000000D+00 400 | .200000000000D+01 .000000000000D+00 .186264514923D-08 .400000000000D+02 401 | .424806000000D+06 .400000000000D+01 402 | G07 2020 12 24 23 59 44 -.479118898511D-05 .150066625793D-10 .000000000000D+00 403 | .960000000000D+02 .478437500000D+02 .450804492102D-08 -.192551449588D+01 404 | .226125121117D-05 .142930077855D-01 .104345381260D-04 .515362369919D+04 405 | .431984000000D+06 .167638063431D-06 .245865835941D+01 -.100582838058D-06 406 | .952233403681D+00 .169062500000D+03 -.235822815113D+01 -.778210987040D-08 407 | .771460705864D-10 .100000000000D+01 .213700000000D+04 .000000000000D+00 408 | .200000000000D+01 .630000000000D+02 -.111758708954D-07 .960000000000D+02 409 | .424806000000D+06 .400000000000D+01 410 | G08 2020 12 25 00 00 00 -.410061329603D-05 -.136424205266D-11 .000000000000D+00 411 | .920000000000D+02 -.511562500000D+02 .454840374499D-08 .993830224081D+00 412 | -.251829624176D-05 .595516141038D-02 .389106571674D-05 .515376852036D+04 413 | .432000000000D+06 .856816768646D-07 -.175638285749D+01 .689178705215D-07 414 | .968314927118D+00 .303218750000D+03 -.572966611959D-01 -.829355974577D-08 415 | -.367872466222D-09 .100000000000D+01 .213700000000D+04 .000000000000D+00 416 | .280000000000D+01 .000000000000D+00 .512227416039D-08 .920000000000D+02 417 | .424806000000D+06 .400000000000D+01 418 | G26 2020 12 25 00 00 00 .195652246475D-04 .579802872380D-11 .000000000000D+00 419 | .690000000000D+02 .810312500000D+02 .509806949791D-08 .279038763254D+01 420 | .426173210144D-05 .549899612088D-02 .493973493576D-05 .515373074150D+04 421 | .432000000000D+06 -.126659870148D-06 -.282683501056D+01 -.614672899246D-07 422 | .944214001297D+00 .276687500000D+03 .232279576908D+00 -.838213486385D-08 423 | .161078138122D-09 .100000000000D+01 .213700000000D+04 .000000000000D+00 424 | .200000000000D+01 .000000000000D+00 .698491930962D-08 .690000000000D+02 425 | .424806000000D+06 .400000000000D+01 426 | G30 2020 12 25 00 00 00 -.357861630619D-03 -.625277607469D-11 .000000000000D+00 427 | .860000000000D+02 .481562500000D+02 .489556106263D-08 -.193052057814D+01 428 | .226870179176D-05 .474162062164D-02 .105109065771D-04 .515377383041D+04 429 | .432000000000D+06 .391155481339D-07 .247814354755D+01 -.596046447754D-07 430 | .937496922236D+00 .164718750000D+03 -.284690099619D+01 -.810283751571D-08 431 | -.117862052285D-10 .100000000000D+01 .213700000000D+04 .000000000000D+00 432 | .200000000000D+01 .000000000000D+00 .372529029846D-08 .860000000000D+02 433 | .424806000000D+06 .400000000000D+01 434 | R09 2020 12 24 22 15 00 .148853287101D-04 .272848410532D-11 .424800000000D+06 435 | -.101517270508D+05 .264739322662D+01 .931322574615D-09 .000000000000D+00 436 | -.122801166992D+05 .647577285767D+00 .931322574615D-09 -.200000000000D+01 437 | .199578237305D+05 .174031639099D+01 -.186264514923D-08 .000000000000D+00 438 | R07 2020 12 24 22 15 00 -.419300049543D-04 .000000000000D+00 .424800000000D+06 439 | -.127033002930D+05 -.766029357910D+00 .000000000000D+00 .000000000000D+00 440 | -.145640869141D+05 -.214564895630D+01 .931322574615D-09 .500000000000D+01 441 | .166515869141D+05 -.246960926056D+01 -.186264514923D-08 .000000000000D+00 442 | R06 2020 12 24 22 15 00 .189753249288D-03 .000000000000D+00 .424800000000D+06 443 | -.838337158203D+04 -.172764778137D+00 .000000000000D+00 .000000000000D+00 444 | -.240571977539D+05 -.803546905518D-01 .931322574615D-09 -.400000000000D+01 445 | .934643066406D+03 -.355894756317D+01 .000000000000D+00 .000000000000D+00 446 | R16 2020 12 24 22 15 00 -.949390232563D-05 .000000000000D+00 .424800000000D+06 447 | .830178808594D+04 .299386405945D+01 .279396772385D-08 .000000000000D+00 448 | -.931129150391D+04 .574748039246D+00 .000000000000D+00 -.100000000000D+01 449 | .222466884766D+05 -.863492012024D+00 -.931322574615D-09 .000000000000D+00 450 | R10 2020 12 24 22 15 00 -.649858266115D-04 .000000000000D+00 .424800000000D+06 451 | -.229678056641D+05 .915909767151D+00 -.186264514923D-08 .000000000000D+00 452 | -.844252001953D+04 .350553512573D+00 .931322574615D-09 -.700000000000D+01 453 | .705799951172D+04 .340252685547D+01 -.186264514923D-08 .000000000000D+00 454 | R08 2020 12 24 22 15 00 -.564558431506D-04 .000000000000D+00 .424800000000D+06 455 | -.106810976562D+05 -.916188240051D+00 .000000000000D+00 .000000000000D+00 456 | .259714648438D+04 -.300841140747D+01 .000000000000D+00 .600000000000D+01 457 | .230645498047D+05 -.877475738525D-01 -.279396772385D-08 .000000000000D+00 458 | R18 2020 12 24 22 15 00 .607967376709D-04 .909494701773D-12 .424800000000D+06 459 | .187439023437D+05 -.139898300171D+01 .372529029846D-08 .000000000000D+00 460 | -.110030698242D+05 .118949413300D+01 -.931322574615D-09 -.300000000000D+01 461 | .133312993164D+05 .295783329010D+01 .000000000000D+00 .000000000000D+00 462 | R17 2020 12 24 22 15 00 .385301187634D-03 .272848410532D-11 .424800000000D+06 463 | .112690976562D+05 -.216604232788D+01 .279396772385D-08 .000000000000D+00 464 | .310009423828D+04 .222618770599D+01 .000000000000D+00 .400000000000D+01 465 | .226935078125D+05 .768314361572D+00 -.931322574615D-09 .000000000000D+00 466 | E19 2020 12 24 21 50 00 .161265605129D-03 .950706180447D-11 .000000000000D+00 467 | .670000000000D+02 -.414375000000D+02 .347407328053D-08 .610425255508D+00 468 | -.188127160072D-05 .137188704684D-03 .578165054321D-05 .544060739899D+04 469 | .424200000000D+06 .428408384323D-07 .117921007810D+01 .111758708954D-07 470 | .959672858524D+00 .215718750000D+03 .234192142023D+01 -.573631036892D-08 471 | -.117147736816D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 472 | .312000000000D+01 .000000000000D+00 -.395812094212D-08 -.372529029846D-08 473 | .424864000000D+06 .000000000000D+00 474 | E01 2020 12 24 21 50 00 -.101035926491D-02 -.801492205937D-11 .000000000000D+00 475 | .670000000000D+02 .144500000000D+03 .296583782476D-08 .122015637248D+01 476 | .678189098835D-05 .226500444114D-03 .615604221821D-05 .544061906052D+04 477 | .424200000000D+06 .372529029846D-07 -.300627408007D+01 -.670552253723D-07 478 | .980383677967D+00 .215656250000D+03 -.441746981289D+00 -.563094883734D-08 479 | .191079387795D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 480 | .312000000000D+01 .000000000000D+00 .000000000000D+00 .000000000000D+00 481 | .424864000000D+06 .000000000000D+00 482 | E04 2020 12 24 21 50 00 -.674758455716D-03 -.783018094808D-11 .000000000000D+00 483 | .670000000000D+02 -.393750000000D+02 .355586240166D-08 .105574391471D+00 484 | -.180862843990D-05 .881952000782D-04 .525638461113D-05 .544060977173D+04 485 | .424200000000D+06 .428408384323D-07 .117950918194D+01 -.745058059692D-08 486 | .953975815185D+00 .221500000000D+03 .197994294087D+01 -.580417033842D-08 487 | -.108218793461D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 488 | .312000000000D+01 .000000000000D+00 -.325962901115D-08 -.349245965481D-08 489 | .424864000000D+06 .000000000000D+00 490 | E09 2020 12 24 21 50 00 .582178053446D-02 -.125908172777D-10 .000000000000D+00 491 | .670000000000D+02 -.431250000000D+02 .345942981342D-08 .823509316686D+00 492 | -.214949250221D-05 .258149229921D-03 .529177486897D-05 .544061273956D+04 493 | .424200000000D+06 .745058059692D-08 .117396080765D+01 -.391155481339D-07 494 | .959939694781D+00 .224843750000D+03 .462196966827D+00 -.572916721424D-08 495 | -.914323799542D-10 .513000000000D+03 .213700000000D+04 .000000000000D+00 496 | .312000000000D+01 .000000000000D+00 .186264514923D-08 .186264514923D-08 497 | .424864000000D+06 .000000000000D+00 498 | E31 2020 12 24 21 50 00 -.476773129776D-03 -.369482222595D-12 .000000000000D+00 499 | .670000000000D+02 .135062500000D+03 .297369529492D-08 .102783565974D+01 500 | .628642737865D-05 .308294547722D-03 .623427331448D-05 .544062356186D+04 501 | .424200000000D+06 -.633299350739D-07 -.300912521085D+01 -.335276126862D-07 502 | .979929553085D+00 .215750000000D+03 -.105178239254D+01 -.556308886784D-08 503 | .172507185617D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 504 | .312000000000D+01 .000000000000D+00 .465661287308D-08 .535510480404D-08 505 | .424864000000D+06 .000000000000D+00 506 | G03 2020 12 25 00 00 00 -.385707244277D-04 -.989075488178D-11 .000000000000D+00 507 | .173000000000D+03 .318750000000D+01 .441411243693D-08 -.245279702013D+01 508 | .264495611191D-06 .325111334678D-02 .707432627678D-05 .515368107033D+04 509 | .432000000000D+06 -.335276126862D-07 .354976889069D+00 -.335276126862D-07 510 | .967968888475D+00 .247093750000D+03 .885484267014D+00 -.790175771135D-08 511 | .378587198248D-09 .100000000000D+01 .213700000000D+04 .000000000000D+00 512 | .200000000000D+01 .000000000000D+00 .186264514923D-08 .173000000000D+03 513 | .424866000000D+06 .400000000000D+01 514 | E21 2020 12 24 21 50 00 -.639079662506D-03 -.203215222427D-11 .000000000000D+00 515 | .670000000000D+02 .145625000000D+03 .305369862738D-08 .177312806883D+01 516 | .684149563313D-05 .170014682226D-03 .591203570366D-05 .544062606239D+04 517 | .424200000000D+06 .298023223877D-07 -.300908051870D+01 .000000000000D+00 518 | .979953691233D+00 .223437500000D+03 -.231357843429D+00 -.564952103951D-08 519 | .207508643568D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 520 | .312000000000D+01 .000000000000D+00 .209547579288D-08 .232830643654D-08 521 | .425434000000D+06 .000000000000D+00 522 | E01 2020 12 24 22 00 00 -.101036403794D-02 -.801492205937D-11 .000000000000D+00 523 | .680000000000D+02 .144500000000D+03 .296833792890D-08 .129452847097D+01 524 | .678375363350D-05 .226497417316D-03 .616535544395D-05 .544061906624D+04 525 | .424800000000D+06 .428408384323D-07 -.300627749306D+01 -.558793544769D-07 526 | .980383759890D+00 .215625000000D+03 -.441734028613D+00 -.563237746827D-08 527 | .197865384745D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 528 | .312000000000D+01 .000000000000D+00 .000000000000D+00 .000000000000D+00 529 | .425464000000D+06 .000000000000D+00 530 | E19 2020 12 24 22 00 00 .161271309480D-03 .950706180447D-11 .000000000000D+00 531 | .680000000000D+02 -.412812500000D+02 .347050170318D-08 .684638160716D+00 532 | -.187754631042D-05 .137197785079D-03 .577792525291D-05 .544060744858D+04 533 | .424800000000D+06 .428408384323D-07 .117920663293D+01 .149011611938D-07 534 | .959672810247D+00 .215875000000D+03 .234209435617D+01 -.573273879158D-08 535 | -.118219210019D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 536 | .312000000000D+01 .000000000000D+00 -.395812094212D-08 -.372529029846D-08 537 | .425464000000D+06 .000000000000D+00 538 | E31 2020 12 24 22 00 00 -.476773246191D-03 -.369482222595D-12 .000000000000D+00 539 | .680000000000D+02 .136343750000D+03 .297226666398D-08 .110166475962D+01 540 | .634230673313D-05 .308280228637D-03 .621192157269D-05 .544062390327D+04 541 | .424800000000D+06 -.596046447754D-07 -.300912854923D+01 -.447034835815D-07 542 | .979929611602D+00 .216250000000D+03 -.105122651146D+01 -.556916054932D-08 543 | .176078762959D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 544 | .312000000000D+01 .000000000000D+00 .465661287308D-08 .535510480404D-08 545 | .425464000000D+06 .000000000000D+00 546 | E09 2020 12 24 22 00 00 .582177314209D-02 -.125766064230D-10 .000000000000D+00 547 | .680000000000D+02 -.425625000000D+02 .345942981342D-08 .898341004057D+00 548 | -.211037695408D-05 .258135492913D-03 .524707138538D-05 .544061250877D+04 549 | .424800000000D+06 .130385160446D-07 .117395739467D+01 -.428408384323D-07 550 | .959939649431D+00 .225531250000D+03 .461750788517D+00 -.573059584518D-08 551 | -.942896418278D-10 .513000000000D+03 .213700000000D+04 .000000000000D+00 552 | .312000000000D+01 .000000000000D+00 .186264514923D-08 .186264514923D-08 553 | .425464000000D+06 .000000000000D+00 554 | E21 2020 12 24 22 00 00 -.639080826659D-03 -.203215222427D-11 .000000000000D+00 555 | .680000000000D+02 .146406250000D+03 .306012746659D-08 .184791518986D+01 556 | .686012208462D-05 .170091167092D-03 .590458512306D-05 .544062586021D+04 557 | .424800000000D+06 .279396772385D-07 -.300908391706D+01 .000000000000D+00 558 | .979953814118D+00 .223718750000D+03 -.231760254156D+00 -.564880672405D-08 559 | .208222959036D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 560 | .312000000000D+01 .000000000000D+00 .209547579288D-08 .232830643654D-08 561 | .425464000000D+06 .000000000000D+00 562 | E14 2020 12 24 22 00 00 -.127101462567D-02 -.131166189021D-10 .000000000000D+00 563 | .680000000000D+02 -.406562500000D+02 .297405245265D-08 -.170985998377D+01 564 | -.646337866783D-06 .165635102545D+00 .572577118874D-05 .528935733032D+04 565 | .424800000000D+06 .344403088093D-05 -.167807113086D+01 .505894422531D-05 566 | .882309282740D+00 .214250000000D+03 .185592946764D+01 -.393052086483D-08 567 | -.114290474943D-08 .513000000000D+03 .213700000000D+04 .000000000000D+00 568 | .312000000000D+01 .000000000000D+00 -.325962901115D-08 -.372529029846D-08 569 | .425515000000D+06 .000000000000D+00 570 | R19 2020 12 24 22 15 00 -.125084072351D-03 -.909494701773D-12 .425640000000D+06 571 | .171214384766D+05 .668296813965D-01 .279396772385D-08 .000000000000D+00 572 | -.187429467773D+05 -.424275398254D+00 .000000000000D+00 .300000000000D+01 573 | -.252606689453D+04 .359011936188D+01 .931322574615D-09 .000000000000D+00 574 | E04 2020 12 24 22 00 00 -.674763112329D-03 -.783018094808D-11 .000000000000D+00 575 | .680000000000D+02 -.388750000000D+02 .355550524392D-08 .180585052241D+00 576 | -.177137553692D-05 .881338492036D-04 .528804957867D-05 .544060960579D+04 577 | .424800000000D+06 .409781932831D-07 .117950571629D+01 -.558793544769D-08 578 | .953975743502D+00 .221062500000D+03 .197931799982D+01 -.580631328483D-08 579 | -.108933108930D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 580 | .312000000000D+01 .000000000000D+00 -.325962901115D-08 -.349245965481D-08 581 | .425794000000D+06 .000000000000D+00 582 | E13 2020 12 24 22 00 00 .408994965255D-03 .724753590475D-12 .000000000000D+00 583 | .680000000000D+02 -.873437500000D+02 .238509934896D-08 .194093062419D+01 584 | -.411458313465D-05 .326936366037D-03 .999122858047D-05 .544061293411D+04 585 | .424800000000D+06 .372529029846D-08 -.926816547938D+00 .447034835815D-07 586 | .993507495575D+00 .138312500000D+03 -.159498233008D+01 -.529343477852D-08 587 | -.642883921553D-10 .513000000000D+03 .213700000000D+04 .000000000000D+00 588 | .312000000000D+01 .000000000000D+00 .232830643654D-09 -.232830643654D-09 589 | .426034000000D+06 .000000000000D+00 590 | G05 2020 12 25 00 00 00 -.292155891657D-04 -.102318153949D-11 .000000000000D+00 591 | .790000000000D+02 .300000000000D+01 .477162732886D-08 .156864415194D+01 592 | .132247805595D-06 .602429755963D-02 .696629285812D-05 .515378479576D+04 593 | .432000000000D+06 -.115483999252D-06 .320196708084D+00 .000000000000D+00 594 | .954915488429D+00 .245187500000D+03 .891547604559D+00 -.819712715753D-08 595 | .323227749447D-09 .100000000000D+01 .213700000000D+04 .000000000000D+00 596 | .200000000000D+01 .000000000000D+00 -.111758708954D-07 .790000000000D+02 597 | .427086000000D+06 .400000000000D+01 598 | E04 2020 12 24 22 10 00 -.674767652526D-03 -.781597009336D-11 .000000000000D+00 599 | .690000000000D+02 -.384375000000D+02 .355479092845D-08 .255692302176D+00 600 | -.173971056938D-05 .880737788975D-04 .532530248165D-05 .544060943031D+04 601 | .425400000000D+06 .409781932831D-07 .117950224479D+01 -.372529029846D-08 602 | .953975671819D+00 .220562500000D+03 .197859647399D+01 -.580809907350D-08 603 | -.109647424398D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 604 | .312000000000D+01 .000000000000D+00 -.325962901115D-08 -.349245965481D-08 605 | .426064000000D+06 .000000000000D+00 606 | E21 2020 12 24 22 10 00 -.639082049020D-03 -.203215222427D-11 .000000000000D+00 607 | .690000000000D+02 .146656250000D+03 .306084178206D-08 .192236768448D+01 608 | .685825943947D-05 .170114100911D-03 .590831041336D-05 .544062581062D+04 609 | .425400000000D+06 .186264514923D-07 -.300908735492D+01 -.372529029846D-08 610 | .979953934078D+00 .223687500000D+03 -.231827943376D+00 -.564487798897D-08 611 | .212866009581D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 612 | .312000000000D+01 .000000000000D+00 .209547579288D-08 .232830643654D-08 613 | .426064000000D+06 .000000000000D+00 614 | E19 2020 12 24 22 10 00 .161277013831D-03 .949285094975D-11 .000000000000D+00 615 | .690000000000D+02 -.409687500000D+02 .346693012584D-08 .758701078785D+00 616 | -.186637043953D-05 .137210357934D-03 .577047467232D-05 .544060753822D+04 617 | .425400000000D+06 .428408384323D-07 .117920319215D+01 .204890966415D-07 618 | .959672763434D+00 .216062500000D+03 .234241728802D+01 -.572916721424D-08 619 | -.120004998690D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 620 | .312000000000D+01 .000000000000D+00 -.395812094212D-08 -.372529029846D-08 621 | .426064000000D+06 .000000000000D+00 622 | E31 2020 12 24 22 10 00 -.476773362607D-03 -.369482222595D-12 .000000000000D+00 623 | .690000000000D+02 .137875000000D+03 .297012371757D-08 .117543094962D+01 624 | .640749931335D-05 .308249378577D-03 .619702041149D-05 .544062428474D+04 625 | .425400000000D+06 -.540167093277D-07 -.300913189785D+01 -.540167093277D-07 626 | .979929671581D+00 .216593750000D+03 -.105060771174D+01 -.557558938854D-08 627 | .180364655769D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 628 | .312000000000D+01 .000000000000D+00 .465661287308D-08 .535510480404D-08 629 | .426064000000D+06 .000000000000D+00 630 | E09 2020 12 24 22 10 00 .582176557509D-02 -.125766064230D-10 .000000000000D+00 631 | .690000000000D+02 -.419062500000D+02 .345978697116D-08 .973154533690D+00 632 | -.206939876080D-05 .258132116869D-03 .520981848240D-05 .544061228943D+04 633 | .425400000000D+06 .167638063431D-07 .117395398460D+01 -.428408384323D-07 634 | .959939599692D+00 .226125000000D+03 .461322778185D+00 -.573238163385D-08 635 | -.975040614355D-10 .513000000000D+03 .213700000000D+04 .000000000000D+00 636 | .312000000000D+01 .000000000000D+00 .186264514923D-08 .186264514923D-08 637 | .426064000000D+06 .000000000000D+00 638 | E14 2020 12 24 22 10 00 -.127102248371D-02 -.131024080474D-10 .000000000000D+00 639 | .690000000000D+02 -.330000000000D+02 .345085802780D-08 -.162890843755D+01 640 | -.184401869774D-06 .165635018959D+00 .613927841187D-05 .528935788727D+04 641 | .425400000000D+06 .196695327759D-05 -.167807281321D+01 .561214983463D-05 642 | .882304687715D+00 .213875000000D+03 .185592780284D+01 -.483913014062D-08 643 | -.123005123657D-08 .513000000000D+03 .213700000000D+04 .000000000000D+00 644 | .312000000000D+01 .000000000000D+00 -.325962901115D-08 -.372529029846D-08 645 | .426064000000D+06 .000000000000D+00 646 | E13 2020 12 24 22 10 00 .408995430917D-03 .738964445191D-12 .000000000000D+00 647 | .690000000000D+02 -.869062500000D+02 .238402787576D-08 .201522525824D+01 648 | -.409223139286D-05 .326882582158D-03 .100061297417D-04 .544061305618D+04 649 | .425400000000D+06 .186264514923D-08 -.926819722470D+00 .447034835815D-07 650 | .993507460465D+00 .138062500000D+03 -.159489198026D+01 -.529343477852D-08 651 | -.646455498895D-10 .513000000000D+03 .213700000000D+04 .000000000000D+00 652 | .312000000000D+01 .000000000000D+00 .232830643654D-09 -.232830643654D-09 653 | .426064000000D+06 .000000000000D+00 654 | G14 2020 12 25 00 00 00 .721351243556D-04 .875388650456D-11 .000000000000D+00 655 | .100000000000D+02 .775937500000D+02 .475019786481D-08 -.140446880393D+01 656 | .405870378017D-05 .209276448004D-03 .475905835629D-05 .515373771858D+04 657 | .432000000000D+06 -.502914190292D-07 -.274080875220D+01 .372529029846D-08 658 | .959402278659D+00 .288062500000D+03 .186233900329D+01 -.828320217148D-08 659 | .133934150324D-09 .100000000000D+01 .213700000000D+04 .000000000000D+00 660 | .200000000000D+01 .000000000000D+00 -.232830643654D-08 .522000000000D+03 661 | .427116000000D+06 .400000000000D+01 662 | R09 2020 12 24 22 45 00 .148890540004D-04 .272848410532D-11 .426600000000D+06 663 | -.500266064453D+04 .302940177917D+01 .186264514923D-08 .000000000000D+00 664 | -.114151943359D+05 .291909217834D+00 .000000000000D+00 -.200000000000D+01 665 | .222810566406D+05 .824217796326D+00 -.186264514923D-08 .000000000000D+00 666 | R07 2020 12 24 22 45 00 -.419300049543D-04 .000000000000D+00 .426600000000D+06 667 | -.141326235352D+05 -.780738830566D+00 .000000000000D+00 .000000000000D+00 668 | -.177622832031D+05 -.138911914825D+01 .931322574615D-09 .500000000000D+01 669 | .116198964844D+05 -.308497238159D+01 -.186264514923D-08 .000000000000D+00 670 | R16 2020 12 24 22 45 00 -.949483364820D-05 .000000000000D+00 .426600000000D+06 671 | .134850107422D+05 .271636104584D+01 .279396772385D-08 .000000000000D+00 672 | -.869456054688D+04 .119076728821D+00 -.931322574615D-09 -.100000000000D+01 673 | .198530117187D+05 -.177854061127D+01 .000000000000D+00 .000000000000D+00 674 | R08 2020 12 24 22 45 00 -.564549118280D-04 .000000000000D+00 .426600000000D+06 675 | -.126834267578D+05 -.128450202942D+01 .000000000000D+00 .000000000000D+00 676 | -.259451953125D+04 -.271494007111D+01 .931322574615D-09 .600000000000D+01 677 | .220213759766D+05 -.106394958496D+01 -.279396772385D-08 .000000000000D+00 678 | G11 2020 12 25 00 00 00 -.720419920981D-04 .105728759081D-10 .000000000000D+00 679 | .440000000000D+02 -.808750000000D+02 .546808491054D-08 -.184795356806D+01 680 | -.403262674809D-05 .154531339649D-01 .794045627117D-05 .515373176575D+04 681 | .432000000000D+06 -.253319740295D-06 -.117192483384D+01 .337138772011D-06 682 | .913872383984D+00 .199312500000D+03 .207657627694D+01 -.856642825469D-08 683 | -.244653047924D-09 .100000000000D+01 .213700000000D+04 .000000000000D+00 684 | .200000000000D+01 .630000000000D+02 -.125728547573D-07 .440000000000D+02 685 | .427026000000D+06 .400000000000D+01 686 | R10 2020 12 24 22 45 00 -.649876892567D-04 .000000000000D+00 .426630000000D+06 687 | -.205583447266D+05 .174423694611D+01 .000000000000D+00 .000000000000D+00 688 | -.784419628906D+04 .274483680725D+00 .931322574615D-09 -.700000000000D+01 689 | .128288886719D+05 .296777057648D+01 -.186264514923D-08 .000000000000D+00 690 | R06 2020 12 24 22 45 00 .189754180610D-03 .000000000000D+00 .426630000000D+06 691 | -.839130078125D+04 .199366569519D+00 .000000000000D+00 .000000000000D+00 692 | -.234581489258D+05 .726162910461D+00 .931322574615D-09 -.400000000000D+01 693 | -.542475390625D+04 -.346109771729D+01 .000000000000D+00 .000000000000D+00 694 | R18 2020 12 24 22 45 00 .607986003160D-04 .909494701773D-12 .426630000000D+06 695 | .160229184570D+05 -.157709980011D+01 .279396772385D-08 .000000000000D+00 696 | -.820317187500D+04 .191348648071D+01 -.931322574615D-09 -.300000000000D+01 697 | .180707207031D+05 .227410316467D+01 .000000000000D+00 .000000000000D+00 698 | R19 2020 12 24 22 45 00 -.125086866319D-03 -.909494701773D-12 .426630000000D+06 699 | .166768427734D+05 -.527506828308D+00 .372529029846D-08 .000000000000D+00 700 | -.188994833984D+05 .277282714844D+00 -.931322574615D-09 .300000000000D+01 701 | .395031347656D+04 .355914688110D+01 .931322574615D-09 .000000000000D+00 702 | R17 2020 12 24 22 45 00 .385307706892D-03 .272848410532D-11 .426630000000D+06 703 | .762644824219D+04 -.184535312653D+01 .931322574615D-09 .000000000000D+00 704 | .746213085938D+04 .258196544647D+01 .000000000000D+00 .400000000000D+01 705 | .231832197266D+05 -.227825164795D+00 -.931322574615D-09 .000000000000D+00 706 | E09 2020 12 24 22 20 00 .582175800810D-02 -.125908172777D-10 .000000000000D+00 707 | .700000000000D+02 -.412187500000D+02 .346050128663D-08 .104794794674D+01 708 | -.202283263206D-05 .258137821220D-03 .518187880516D-05 .544061208153D+04 709 | .426000000000D+06 .242143869400D-07 .117395057747D+01 -.447034835815D-07 710 | .959939548490D+00 .226562500000D+03 .460914894681D+00 -.573452458025D-08 711 | -.100718481043D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 712 | .312000000000D+01 .000000000000D+00 .186264514923D-08 .186264514923D-08 713 | .426664000000D+06 .000000000000D+00 714 | E14 2020 12 24 22 20 00 -.127103028353D-02 -.131024080474D-10 .000000000000D+00 715 | .700000000000D+02 -.256875000000D+02 .390980571624D-08 -.154795694253D+01 716 | .173225998878D-06 .165634969715D+00 .661797821522D-05 .528935827446D+04 717 | .426000000000D+06 .480562448502D-06 -.167807557813D+01 .575371086597D-05 718 | .882300120484D+00 .212500000000D+03 .185592685487D+01 -.574202489267D-08 719 | -.125005206969D-08 .513000000000D+03 .213700000000D+04 .000000000000D+00 720 | .312000000000D+01 .000000000000D+00 -.325962901115D-08 -.372529029846D-08 721 | .426664000000D+06 .000000000000D+00 722 | E04 2020 12 24 22 20 00 -.674772250932D-03 -.780175923865D-11 .000000000000D+00 723 | .700000000000D+02 -.380625000000D+02 .355407661299D-08 .330868626715D+00 724 | -.171363353729D-05 .880159204826D-04 .536814332008D-05 .544060925102D+04 725 | .426000000000D+06 .391155481339D-07 .117949876743D+01 -.186264514923D-08 726 | .953975601599D+00 .219968750000D+03 .197780588088D+01 -.580952770443D-08 727 | -.110004582132D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 728 | .312000000000D+01 .000000000000D+00 -.325962901115D-08 -.349245965481D-08 729 | .426664000000D+06 .000000000000D+00 730 | E21 2020 12 24 22 20 00 -.639083213173D-03 -.203215222427D-11 .000000000000D+00 731 | .700000000000D+02 .146062500000D+03 .305512725831D-08 .199646510547D+01 732 | .682845711708D-05 .170056126080D-03 .591389834881D-05 .544062595940D+04 733 | .426000000000D+06 .745058059692D-08 -.300909082496D+01 -.111758708954D-07 734 | .979954059889D+00 .223531250000D+03 -.231540462418D+00 -.563773483429D-08 735 | .220009164265D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 736 | .312000000000D+01 .000000000000D+00 .209547579288D-08 .232830643654D-08 737 | .426664000000D+06 .000000000000D+00 738 | E19 2020 12 24 22 20 00 .161282659974D-03 .949285094975D-11 .000000000000D+00 739 | .700000000000D+02 -.405000000000D+02 .346264423303D-08 .832617361261D+00 740 | -.184774398804D-05 .137223163620D-03 .576116144657D-05 .544060766792D+04 741 | .426000000000D+06 .428408384323D-07 .117919975721D+01 .260770320892D-07 742 | .959672716621D+00 .216281250000D+03 .234288686278D+01 -.572559563690D-08 743 | -.122147945095D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 744 | .312000000000D+01 .000000000000D+00 -.395812094212D-08 -.372529029846D-08 745 | .426664000000D+06 .000000000000D+00 746 | E31 2020 12 24 22 20 00 -.476773479022D-03 -.355271367880D-12 .000000000000D+00 747 | .700000000000D+02 .139531250000D+03 .296726645570D-08 .124916650174D+01 748 | .647827982903D-05 .308201299049D-03 .619329512119D-05 .544062469292D+04 749 | .426000000000D+06 -.465661287308D-07 -.300913525086D+01 -.633299350739D-07 750 | .979929733024D+00 .216687500000D+03 -.104995827998D+01 -.558166107002D-08 751 | .185364864048D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 752 | .312000000000D+01 .000000000000D+00 .465661287308D-08 .535510480404D-08 753 | .426664000000D+06 .000000000000D+00 754 | E13 2020 12 24 22 20 00 .408995954785D-03 .738964445191D-12 .000000000000D+00 755 | .700000000000D+02 -.870937500000D+02 .238545650670D-08 .208964832480D+01 756 | -.409595668316D-05 .326896435581D-03 .100079923868D-04 .544061301994D+04 757 | .426000000000D+06 .186264514923D-08 -.926822897002D+00 .447034835815D-07 758 | .993507422429D+00 .138093750000D+03 -.159493012877D+01 -.529307762079D-08 759 | -.650027076237D-10 .513000000000D+03 .213700000000D+04 .000000000000D+00 760 | .312000000000D+01 .000000000000D+00 .232830643654D-09 -.232830643654D-09 761 | .426664000000D+06 .000000000000D+00 762 | E26 2020 12 24 21 30 00 .234446412651D-02 -.437694325228D-10 .000000000000D+00 763 | .650000000000D+02 -.109156250000D+03 .252546233850D-08 .136083345108D+01 764 | -.520236790180D-05 .347210676409D-03 .103116035461D-04 .544061749268D+04 765 | .423000000000D+06 .186264514923D-07 -.921196598919D+00 .186264514923D-07 766 | .986731109814D+00 .130218750000D+03 -.205086643966D+01 -.534700843865D-08 767 | -.100718481043D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 768 | .312000000000D+01 .000000000000D+00 .605359673500D-08 .675208866596D-08 769 | .426925000000D+06 .000000000000D+00 770 | E14 2020 12 24 22 30 00 -.127103819977D-02 -.131024080474D-10 .000000000000D+00 771 | .710000000000D+02 -.192812500000D+02 .434625246743D-08 -.146700595075D+01 772 | .394880771637D-06 .165634937468D+00 .714696943760D-05 .528935834885D+04 773 | .426600000000D+06 -.908970832825D-06 -.167807939781D+01 .551342964172D-05 774 | .882295722953D+00 .209812500000D+03 .185592704359D+01 -.662527596934D-08 775 | -.120647882611D-08 .513000000000D+03 .213700000000D+04 .000000000000D+00 776 | .312000000000D+01 .000000000000D+00 -.325962901115D-08 -.372529029846D-08 777 | .427264000000D+06 .000000000000D+00 778 | E19 2020 12 24 22 30 00 .161288247909D-03 .949285094975D-11 .000000000000D+00 779 | .710000000000D+02 -.398437500000D+02 .345800118249D-08 .906385662258D+00 780 | -.181794166565D-05 .137232942507D-03 .575184822083D-05 .544060783577D+04 781 | .426600000000D+06 .391155481339D-07 .117919632667D+01 .335276126862D-07 782 | .959672668344D+00 .216468750000D+03 .234350442194D+01 -.572166690182D-08 783 | -.124648049234D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 784 | .312000000000D+01 .000000000000D+00 -.395812094212D-08 -.372529029846D-08 785 | .427264000000D+06 .000000000000D+00 786 | E04 2020 12 24 22 30 00 -.674776849337D-03 -.780175923865D-11 .000000000000D+00 787 | .710000000000D+02 -.377812500000D+02 .355336229752D-08 .406106939481D+00 788 | -.169314444065D-05 .879624858499D-04 .541470944881D-05 .544060906410D+04 789 | .426600000000D+06 .391155481339D-07 .117949528568D+01 -.186264514923D-08 790 | .953975529916D+00 .219281250000D+03 .197695330978D+01 -.580988486217D-08 791 | -.110004582132D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 792 | .312000000000D+01 .000000000000D+00 -.325962901115D-08 -.349245965481D-08 793 | .427264000000D+06 .000000000000D+00 794 | E09 2020 12 24 22 30 00 .582175044110D-02 -.125908172777D-10 .000000000000D+00 795 | .710000000000D+02 -.405000000000D+02 .346085844436D-08 .112272046639D+01 796 | -.197626650333D-05 .258152140304D-03 .515952706337D-05 .544061188316D+04 797 | .426600000000D+06 .298023223877D-07 .117394717326D+01 -.428408384323D-07 798 | .959939492899D+00 .226875000000D+03 .460527910425D+00 -.573702468439D-08 799 | -.103932900651D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 800 | .312000000000D+01 .000000000000D+00 .186264514923D-08 .186264514923D-08 801 | .427264000000D+06 .000000000000D+00 802 | E21 2020 12 24 22 30 00 -.639084493741D-03 -.203215222427D-11 .000000000000D+00 803 | .710000000000D+02 .144687500000D+03 .304476968402D-08 .207031632028D+01 804 | .677444040775D-05 .169913284481D-03 .591576099396D-05 .544062629318D+04 805 | .426600000000D+06 -.372529029846D-08 -.300909429793D+01 -.242143869400D-07 806 | .979954197403D+00 .223468750000D+03 -.231006705054D+00 -.562880589093D-08 807 | .226438003480D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 808 | .312000000000D+01 .000000000000D+00 .209547579288D-08 .232830643654D-08 809 | .427264000000D+06 .000000000000D+00 810 | E31 2020 12 24 22 30 00 -.476773420814D-03 -.326849658450D-12 .000000000000D+00 811 | .710000000000D+02 .141218750000D+03 .296405203609D-08 .132290805328D+01 812 | .654906034470D-05 .308136455715D-03 .620074570179D-05 .544062510872D+04 813 | .426600000000D+06 -.391155481339D-07 -.300913861118D+01 -.707805156708D-07 814 | .979929797392D+00 .216531250000D+03 -.104931486082D+01 -.558737559376D-08 815 | .190365072327D-09 .513000000000D+03 .213700000000D+04 .000000000000D+00 816 | .312000000000D+01 .000000000000D+00 .465661287308D-08 .535510480404D-08 817 | .427264000000D+06 .000000000000D+00 818 | E13 2020 12 24 22 30 00 .408996420447D-03 .738964445191D-12 .000000000000D+00 819 | .710000000000D+02 -.877500000000D+02 .238867092630D-08 .216416548917D+01 820 | -.412203371525D-05 .326984678395D-03 .999122858047D-05 .544061282730D+04 821 | .426600000000D+06 .000000000000D+00 -.926826068609D+00 .447034835815D-07 822 | .993507390245D+00 .138531250000D+03 -.159506243362D+01 -.529236330532D-08 823 | -.660741808263D-10 .513000000000D+03 .213700000000D+04 .000000000000D+00 824 | .312000000000D+01 .000000000000D+00 .232830643654D-09 -.232830643654D-09 825 | .427324000000D+06 .000000000000D+00 826 | -------------------------------------------------------------------------------- /src/config_f9p.py: -------------------------------------------------------------------------------- 1 | # configuration settings 2 | 3 | from rtkcmn import uGNSS, rSIG 4 | 5 | # ----------- PPK options ------------------------------- 6 | nf = 2 # num frequencies ( 1 or 2) 7 | pmode = 'kinematic' # static, kinematic 8 | filtertype = 'forward' # forward, backward, combined, combined_noreset 9 | use_sing_pos = False # run initial single precision sol each epoch, not 10 | # necessary unless receiever clock errors are large 11 | elmin = 15 # minimum elevation for float solution (degrees) 12 | cnr_min = [35, 35] # min signal strength [freq1, freq2] (dB-Hz) 13 | excsats = [] # excluded sats 14 | 15 | maxinno = 1 # outlier threshold for phase (m) 16 | maxcode = 10 # outlier threshold for code (m) 17 | maxage = 30 # mag age of differential 18 | maxout = 20 # maximum outage [epoch] 19 | thresdop = 6 # cycle slip detection by doppler method 20 | thresslip = 0.05 # cycle slip detection by geom-free LC 21 | interp_base = False # interpolate base observations 22 | 23 | # ------------ Kalman Filter Statistics ------------------------ 24 | eratio = [300, 300] # ratio between psuedorange noise and carrier phase noise for L1, L2 25 | efact = {uGNSS.GPS: 1.0, uGNSS.GLO: 1.5, uGNSS.GAL: 1.0} # relative weighting of each constellation 26 | err = [0, 0.003, 0.003, 0.0, 0, 0, 5e-12] # error sigmas [-, base, el, bl, snr, rcvstd, satclk] 27 | snrmax = 52 # max signal strength for variance calc (dB-Hz) 28 | accelh = 3 # horiz accel noise sigma (m/sec2) 29 | accelv = 1 # vert accel noise sigma (m/sec2) 30 | prnbias = 1e-4 # Carrier phase bias sigma ( cycles) 31 | sig_p0 = 30.0 # initial pos sigma (m) 32 | sig_v0 = 10.0 # initial vel/acc sigma (m/sec) 33 | sig_n0 = 30.0 # inital bias sigma (m) 34 | 35 | # -------------Ambiguity resolution options ---------------- 36 | armode = 3 # 0:off, 1:continuos,3:fix-and-hold 37 | thresar = 3 # AR threshold 38 | minlock = 0 # min consecutive fix samples to include sat in AR 39 | glo_hwbias = 0.0 # GLONASS HW bias 40 | thresar1 = 0.1 # max pos variation for AR (and accel update in kalman filter) 41 | elmaskar = 15 # elevation mask for AR 42 | var_holdamb = 0.1 # Hold ambiguity variance (m) 43 | minfix = 20 # min fix samples to set hold 44 | minfixsats = 4 # min sat pairs to test for fix 45 | minholdsats = 5 # min sat pairs to test for hold 46 | mindropsats = 10 # min sat pairs to drop sats from AR 47 | 48 | # ----------- Single precision parameters ---------------------------- 49 | sing_p0 = 100 # initial pos sigma 50 | sing_v0 = 10 # initial vel/acc sigma 51 | sing_elmin = 10 # minimum elevation (degrees) 52 | 53 | # -------------Base and Rover positions ------------------ 54 | # base position, set to zeros to use rinex header pos 55 | rb = [0, 0, 0] 56 | 57 | # initial rover position/velocity for alignment to RTKLIB solution for debug 58 | #rr_f =[-1276972.378274, -4717193.586414, 4087245.657488, 59 | # -0.010286, -0.015413, 0.015250] 60 | #rr_b = [-1276984.364211, -4717218.261086, 4087215.802648, 61 | # -0.005020, 0.000248, 0.008825] 62 | # Set to zero to use standard precision computed starting position 63 | rr_f = [0, 0, 0, 0, 0, 0] 64 | rr_b = [0, 0, 0, 0, 0, 0] 65 | 66 | 67 | # ----------- Configure observation signals ---------------- 68 | 69 | gnss_t = [uGNSS.GPS, uGNSS.GLO, uGNSS.GAL] 70 | 71 | # Valid signals 72 | sig_tbl = {'1C': rSIG.L1C, '1X': rSIG.L1X, '1W': rSIG.L1W, 73 | '2W': rSIG.L2W, '2C': rSIG.L2C, '2X': rSIG.L2X, 74 | '5Q': rSIG.L5Q, '5X': rSIG.L5X, '7Q': rSIG.L7Q, 75 | '7X': rSIG.L7X} 76 | 77 | skip_sig_tbl = {uGNSS.GPS: [], # skip these obs 78 | uGNSS.GLO: [], 79 | uGNSS.GAL: [], 80 | uGNSS.QZS: []} 81 | 82 | # set these from table below 83 | freq_ix0 = {uGNSS.GPS: 0, uGNSS.GLO: 4, uGNSS.GAL: 0} # L1 84 | freq_ix1 = {uGNSS.GPS: 1, uGNSS.GLO: 5, uGNSS.GAL: 3} # L2/E5b 85 | 86 | # ---------- Frequencies currently supported------------- 87 | freq = [1.57542e9, # L1/E1 88 | 1.22760e9, # L2 89 | 1.17645e9, # L5/E5a/B2a 90 | 1.20714e9, # E5b 91 | 1.60200E9, # G1 92 | 1.24600E9] # G2 93 | dfreq_glo = [0.56250E6, 0.43750E6] # L1, L2 94 | 95 | 96 | -------------------------------------------------------------------------------- /src/config_phone.py: -------------------------------------------------------------------------------- 1 | # configuration settings 2 | 3 | from rtkcmn import uGNSS, rSIG 4 | 5 | # ----------- PPK options ------------------------------- 6 | nf = 2 # num frequencies ( 1 or 2) 7 | pmode = 'kinematic' # static, kinematic 8 | filtertype = 'forward' # forward, backward, combined, combined_noreset 9 | use_sing_pos = False # run initial single precision sol each epoch, not 10 | # necessary unless receiever clock errors are large 11 | elmin = 15 # minimum elevation for float solution (degrees) 12 | cnr_min = [28, 20] # min signal strength [freq1, freq2] (dB-Hz) 13 | excsats = [] # excluded sats 14 | 15 | maxinno = 1 # outlier threshold for phase (m) 16 | maxcode = 10 # outlier threshold for code (m) 17 | maxage = 30 # mag age of differential 18 | maxout = 4 # maximum outage [epoch] 19 | thresdop = 5 # cycle slip detection by doppler method 20 | thresslip = 0.10 # cycle slip detection by geom-free LC 21 | interp_base = False # interpolate base observations 22 | 23 | # ------------ Kalman Filter Statistics ------------------------ 24 | eratio = [300, 100] # ratio between psuedorange noise and carrier phase noise for L1, L5 25 | efact = {uGNSS.GPS: 1.0, uGNSS.GLO: 1.5, uGNSS.GAL: 1.0} # relative weighting of each constellation 26 | err = [0, 0.003, 0.003, 0.0, 0, 0, 5e-12] # err sigmas [-, base, el, bl, snr, rcvstd, satclk] 27 | snrmax = 45 # max signal strength for variance calc (dB-Hz) 28 | accelh = 3 # horiz accel noise sigma (m/sec2) 29 | accelv = 1 # vert accel noise sigma (m/sec2) 30 | prnbias = 1e-2 # Carrier phase bias sigma ( cycles) 31 | sig_p0 = 30.0 # initial pos sigma (m) 32 | sig_v0 = 10.0 # initial vel/acc sigma (m/sec) 33 | sig_n0 = 30.0 # inital bias sigma (m) 34 | 35 | # -------------Ambiguity resolution options ---------------- 36 | armode = 0 # 0:off, 1:continuos,3:fix-and-hold 37 | thresar1 = 0.05 # max pos variation for AR (and accel update in kalman filter) 38 | # the rest of these aren't used since AR is disabled 39 | minlock = 0 # min consecutive fix samples to include sat in AR 40 | thresar = 3 # AR threshold 41 | glo_hwbias = 0.0 # GLONASS HW bias 42 | elmaskar = 15 # elevation mask for AR 43 | var_holdamb = 0.1 # Hold ambiguity variance (m) 44 | minfix = 20 # min fix samples to set hold 45 | minfixsats = 4 # min sat pairs to test for fix 46 | minholdsats = 5 # min sat pairs to test for hold 47 | mindropsats = 10 # min sat pairs to drop sats from AR 48 | 49 | # ----------- Single precision parameters ---------------------------- 50 | sing_p0 = 100 # initial pos sigma 51 | sing_v0 = 10 # initial vel/acc sigma 52 | sing_elmin = 10 # minimum elevation (degrees) 53 | 54 | # ------------- Base and Rover positions ------------------ 55 | # base position, set to zeros to use rinex header pos 56 | #rb = [-2703115.9211, -4291767.2078, 3854247.9065] # SLAC 57 | rb = [0, 0, 0] # will set based on base file name 58 | 59 | # initial rover position/velocity for alignment to RTKLIB 60 | #rr_f = [-2694556.682853, -4296492.691977, 3854819.048563, # forwards 61 | # -0.009033 , 0.042808, -0.027949] 62 | #rr_b = [-2709836.020965, -4269097.509128, 3874376.664473, # backwards 63 | # 0.018097, 0.051379, -0.027851 ] 64 | # Set to zero to use standard precision computed starting position 65 | rr_f = [0, 0, 0, 0, 0, 0] 66 | rr_b = [0, 0, 0, 0, 0, 0] 67 | 68 | 69 | # ----------- Configure observation signals ---------------- 70 | 71 | gnss_t = [uGNSS.GPS, uGNSS.GLO, uGNSS.GAL] 72 | 73 | # Valid signals 74 | sig_tbl = {'1C': rSIG.L1C, '1X': rSIG.L1X, '1W': rSIG.L1W, 75 | '2W': rSIG.L2W, '2L': rSIG.L2L, '2X': rSIG.L2X, 76 | '5Q': rSIG.L5Q, '5X': rSIG.L5X, '7Q': rSIG.L7Q, 77 | '7X': rSIG.L7X} 78 | 79 | skip_sig_tbl = {uGNSS.GPS: [], # skip these obs 80 | uGNSS.GLO: [], 81 | uGNSS.GAL: [], 82 | uGNSS.QZS: []} 83 | 84 | # set these from table below 85 | freq_ix0 = {uGNSS.GPS: 0, uGNSS.GLO: 4, uGNSS.GAL: 0} # L1 86 | freq_ix1 = {uGNSS.GPS: 2, uGNSS.GLO: 5, uGNSS.GAL: 2} # L5 87 | 88 | # ---------- Frequencies currently supported------------- 89 | freq = [1.57542e9, # L1/E1 90 | 1.22760e9, # L2 91 | 1.17645e9, # L5/E5a/B2a 92 | 1.20714e9, # E5b 93 | 1.60200E9, # G1 94 | 1.24600E9] # G2 95 | dfreq_glo = [0.56250E6, 0.43750E6] # L1, L2 96 | 97 | 98 | -------------------------------------------------------------------------------- /src/ephemeris.py: -------------------------------------------------------------------------------- 1 | """ 2 | module for ephemeris processing 3 | 4 | Copyright (c) 2021 Rui Hirokawa (from CSSRLIB) 5 | Copyright (c) 2022 Tim Everett 6 | """ 7 | 8 | import numpy as np 9 | from rtkcmn import uGNSS, rCST, timediff, timeadd, vnorm, time2epoch 10 | from rtkcmn import sat2prn, trace 11 | 12 | # ephemeris parameters 13 | MAX_ITER_KEPLER = 30 14 | RTOL_KEPLER = 1e-13 15 | TSTEP = 120 #60.0 # time step for Glonass orbital calcs 16 | ERREPH_GLO = 5.0 17 | 18 | 19 | def seleph(nav, t, sat): 20 | """ select ephemeric for sat, assumes ephemeris is sorted by sat, then time """ 21 | dt_p = 1e10 # timediff(t, nav.eph[nav.eph_index[sat]].toe) 22 | eph = None 23 | sys = sat2prn(sat)[0] 24 | i_p = 0 25 | if sys != uGNSS.GLO: 26 | # start with previous index for this sat 27 | for i, eph_ in enumerate(nav.eph[nav.eph_index[sat]:]): 28 | if eph_.sat != sat: 29 | continue 30 | # bit 8 set=E5a, bit 9 set=E5b 31 | if sys == uGNSS.GAL: 32 | # TODO: abstract hard coded freq 33 | if nav.obs_idx[1][uGNSS.GAL] == 2 and (eph_.code >> 8) & 1 == 0: 34 | continue 35 | elif nav.obs_idx[1][uGNSS.GAL] == 3 and (eph_.code >> 9) & 1 == 0: 36 | continue 37 | dt = timediff(t, eph_.toe) 38 | if abs(dt) <= dt_p: 39 | dt_p = abs(dt) 40 | i_p = i 41 | eph = eph_ 42 | else: 43 | break 44 | else: # GLONASS 45 | # start with previous index for this sat 46 | for i, eph_ in enumerate(nav.geph[nav.eph_index[sat]:]): 47 | if eph_.sat != sat: 48 | continue 49 | dt = timediff(t, eph_.toe) 50 | if abs(dt) <= dt_p: 51 | dt_p = abs(dt) 52 | i_p = i 53 | eph = eph_ 54 | else: 55 | break 56 | trace(4, 'seleph: sat=%d dt=%.0f\n' % (sat,dt_p)) 57 | nav.eph_index[sat] = max(nav.eph_index[sat] + i_p - 1, 0) # save index for next time 58 | return eph 59 | 60 | 61 | def dtadjust(t1, t2, tw=604800): 62 | """ calculate delta time considering week-rollover """ 63 | dt = timediff(t1, t2) 64 | if dt > tw: 65 | dt -= tw 66 | elif dt < -tw: 67 | dt += tw 68 | return dt 69 | 70 | def sva2ura(sys, sva): 71 | """ variance by ura ephemeris """ 72 | ura_nominal = [2.0, 2.8, 4.0, 5.76, 8.0, 11.3, 16.0, 32.0, 64.0, 128.0, 73 | 256.0, 512.0, 1024.0, 2048.0,4096.0, 8192.0] 74 | if sys == uGNSS.GAL: #galileo sisa 75 | if sva < 0 or sva > 6: return 500**2 76 | return sva**2 77 | else: # gps ura 78 | if sva < 0 or sva > 15: return 500**2 79 | return ura_nominal[int(sva) + 1] 80 | 81 | def eph2pos(t, eph): 82 | """ broadcast ephemeris to satellite position and clock bias ------------- 83 | * compute satellite position and clock bias with broadcast ephemeris (gps, 84 | * galileo, qzss) 85 | * args : gtime_t time I time (gpst) 86 | * eph_t *eph I broadcast ephemeris 87 | * double *rs O satellite position (ecef) {x,y,z} (m) 88 | * double *dts O satellite clock bias (s) 89 | * double *var O satellite position and clock variance (m^2) 90 | * return : none 91 | * notes : see ref [1],[7],[8] 92 | * satellite clock includes relativity correction without code bias 93 | * (tgd or bgd) """ 94 | tk = dtadjust(t, eph.toe) 95 | sys, _ = sat2prn(eph.sat) 96 | if sys == uGNSS.GAL: 97 | mu = rCST.MU_GAL 98 | omge = rCST.OMGE_GAL 99 | else: # GPS,QZS 100 | mu = rCST.MU_GPS 101 | omge = rCST.OMGE 102 | 103 | M = eph.M0 + (np.sqrt(mu / eph.A**3) + eph.deln) * tk 104 | E, Ek = M, 0 105 | for _ in range(MAX_ITER_KEPLER): 106 | if abs(E - Ek) < RTOL_KEPLER: 107 | break 108 | Ek = E 109 | E -= (E - eph.e * np.sin(E) - M) / (1.0 - eph.e * np.cos(E)) 110 | 111 | sinE, cosE = np.sin(E), np.cos(E) 112 | nus = np.sqrt(1.0 - eph.e**2) * sinE 113 | nuc = cosE - eph.e 114 | nue = 1.0 - eph.e * cosE 115 | u = np.arctan2(nus, nuc) + eph.omg 116 | r = eph.A * nue 117 | i = eph.i0 + eph.idot * tk 118 | sin2u, cos2u = np.sin(2*u), np.cos(2*u) 119 | u += eph.cus * sin2u + eph.cuc * cos2u 120 | r += eph.crs * sin2u +eph.crc * cos2u 121 | i += eph.cis * sin2u + eph.cic * cos2u 122 | x = r * np.cos(u) 123 | y = r * np.sin(u) 124 | cosi = np.cos(i) 125 | O = eph.OMG0 + (eph.OMGd - omge) * tk - omge * eph.toes 126 | sinO, cosO = np.sin(O), np.cos(O) 127 | rs = [x * cosO - y * cosi * sinO, x * sinO + y * cosi * cosO, y * np.sin(i)] 128 | tk = dtadjust(t, eph.toc) 129 | dts = eph.f0 + eph.f1 * tk + eph.f2 * tk**2 130 | # relativity correction 131 | dts -= 2 *np.sqrt(mu * eph.A) * eph.e * sinE / rCST.CLIGHT**2 132 | var = sva2ura(sys, eph.sva) 133 | trace(4, 'eph2pos: sat=%d, dts=%.10f rs=%.4f %.4f %.4f var=%.3f\n' % 134 | (eph.sat, dts,rs[0],rs[1],rs[2], var)) 135 | return rs, var, dts 136 | 137 | def deq(x, acc): 138 | """glonass orbit differential equations """ 139 | xdot = np.zeros(6) 140 | r2 = np.dot(x[0:3], x[0:3]) 141 | if r2 <= 0.0: 142 | return xdot 143 | r3 = r2 * np.sqrt(r2) 144 | omg2 = rCST.OMGE_GLO**2 145 | 146 | a = 1.5 * rCST.J2_GLO * rCST.MU_GLO * rCST.RE_GLO**2 / r2 / r3 147 | b = 5.0 * x[2]**2 / r2 148 | c = -rCST.MU_GLO / r3 - a * (1.0 - b) 149 | xdot[0:3] = x[3:6] 150 | xdot[3] = (c + omg2) * x[0] + 2.0 * rCST.OMGE_GLO * x[4] + acc[0] 151 | xdot[4] = (c + omg2) * x[1] - 2.0 * rCST.OMGE_GLO * x[3] + acc[1] 152 | xdot[5] = (c - 2.0 * a) * x[2] + acc[2] 153 | return xdot 154 | 155 | def glorbit(t, x, acc): 156 | """ glonass position and velocity by numerical integration """ 157 | k1 = deq(x, acc) 158 | w =x + k1 * t / 2 159 | k2 = deq(w, acc) 160 | w = x + k2 * t / 2 161 | k3 = deq(w, acc) 162 | w = x + k3 * t 163 | k4 = deq(w, acc) 164 | x += (k1 + 2 * k2 + 2 * k3 + k4) * t / 6 165 | return x 166 | 167 | 168 | def geph2pos(time, geph): 169 | """ GLONASS ephemeris to satellite position and clock bias """ 170 | t = timediff(time, geph.toe) 171 | dts = -geph.taun + geph.gamn * t 172 | x = np.array((*geph.pos, *geph.vel)) 173 | 174 | trace(4, 'geph2pos: sat=%d\n' % geph.sat) 175 | tt = -TSTEP if t < 0 else TSTEP 176 | while abs(t) > 1E-5: #1E-9 177 | if abs(t) < TSTEP: 178 | tt = t 179 | x = glorbit(tt, x, geph.acc) 180 | t -= tt 181 | 182 | var = ERREPH_GLO**2 183 | return x[0:3], var, dts 184 | 185 | def ephpos(time, eph): 186 | tt = 1e-3 # delta t to calculate velocity 187 | rs = np.zeros(6) 188 | 189 | if sat2prn(eph.sat)[0] != uGNSS.GLO: 190 | rs[0:3], var, dts = eph2pos(time, eph) 191 | # use delta t to determine velocity 192 | t = timeadd(time, tt) 193 | rs[3:6], _, dtst = eph2pos(t, eph) 194 | else: # GLONASS 195 | rs[0:3], var, dts = geph2pos(time, eph) 196 | # use delta t to determine velocity 197 | t = timeadd(time, tt) 198 | rs[3:6], _, dtst = geph2pos(t, eph) 199 | rs[3:6] = (rs[3:6] - rs[0:3]) / tt 200 | return rs, var, dts 201 | 202 | def satpos(t, eph): 203 | return ephpos(t, eph) 204 | 205 | def eph2clk(time, eph): 206 | """ calculate clock offset based on ephemeris """ 207 | t = ts = timediff(time, eph.toc) 208 | for _ in range(2): 209 | t = ts - (eph.f0 + eph.f1 * t + eph.f2 * t**2) 210 | dts = eph.f0 + eph.f1*t + eph.f2 * t**2 211 | trace(4, 'ephclk: t=%.12f ts=%.12f dts=%.12f f0=%.12f f1=%.9f f2=%.9f\n' % (t,ts,dts,eph.f0,eph.f1,eph.f2)) 212 | return dts 213 | 214 | def geph2clk(time, geph): 215 | """ calculate GLONASS clock offset based on ephemeris """ 216 | t = ts = timediff(time, geph.toe) 217 | for _ in range(2): 218 | t = ts - (-geph.taun + geph.gamn * t) 219 | trace(4, 'geph2clk: t=%.12f ts=%.12f taun=%.12f gamn=%.12f\n' % (t, ts, 220 | geph.taun, geph.gamn)) 221 | return -geph.taun + geph.gamn * t 222 | 223 | def ephclk(time, eph): 224 | if sat2prn(eph.sat)[0] != uGNSS.GLO: 225 | return eph2clk(time, eph) 226 | else: 227 | return geph2clk(time, eph) 228 | 229 | def satposs(obs, nav): 230 | """ satellite positions and clocks ---------------------------------------------- 231 | * compute satellite positions, velocities and clocks 232 | * args obs_t obs I observation data 233 | * nav_t nav I navigation data 234 | * double rs O satellite positions and velocities (ecef) 235 | * double dts O satellite clocks 236 | * double var O sat position and clock error variances (m^2) 237 | * int svh O sat health flag (-1:correction not available) 238 | * return : none 239 | * notes : rs [0:2] = obs[i] sat position {x,y,z} (m) 240 | * rs [3:5] = obs[i] sat velocity {vx,vy,vz} (m/s) 241 | * dts[0:1] = obs[i] sat clock {bias,drift} (s|s/s) 242 | * var[i] = obs[i] sat position and clock error variance (m^2) 243 | * svh[i] = obs[i] sat health flag 244 | * if no navigation data, set 0 to rs[], dts[], var[] and svh[] 245 | * satellite position and clock are values at signal transmission time 246 | * satellite position is referenced to antenna phase center 247 | * satellite clock does not include code bias correction (tgd or bgd) 248 | * any pseudorange and broadcast ephemeris are always needed to get 249 | * signal transmission time """ 250 | n = obs.sat.shape[0] 251 | rs = np.zeros((n, 6)) 252 | dts = np.zeros(n) 253 | var = np.zeros(n) 254 | svh = np.zeros(n, dtype=int) 255 | 256 | ep = time2epoch(obs.t) 257 | trace(3, 'satposs : teph= %04d/%02d/%02d %02d:%02d:%06.3f n=%d ephopt=%d\n' % 258 | (ep[0], ep[1], ep[2], ep[3], ep[4], ep[5], n, 0)) 259 | 260 | for i in np.argsort(obs.sat): 261 | sat = obs.sat[i] 262 | # search any pseudorange 263 | pr = obs.P[i,0] if obs.P[i,0] != 0 else obs.P[i,1] 264 | # transmission time by satellite clock 265 | t = timeadd(obs.t, -pr / rCST.CLIGHT) 266 | 267 | eph = seleph(nav, t, sat) 268 | if eph is None: 269 | svh[i] = 1 270 | trace(2, 'No broadcast ephemeris: sat=%d\n' % sat) 271 | continue 272 | svh[i] = eph.svh 273 | # satellite clock bias by broadcast ephemeris 274 | dt = ephclk(t, eph) 275 | t = timeadd(t, -dt) 276 | # satellite position and clock at transmission time 277 | rs[i], var[i], dts[i] = satpos(t, eph) 278 | trace(4,'satposs: %d,time=%.9f dt=%.9f pr=%.3f rs=%13.3f %13.3f %13.3f dts=%12.3f var=%7.3f\n' 279 | % (obs.sat[i],t.sec,dt,pr,*rs[i,0:3],dts[i]*1e9,var[i])) 280 | 281 | 282 | return rs, var, dts, svh 283 | -------------------------------------------------------------------------------- /src/mlambda.py: -------------------------------------------------------------------------------- 1 | """ 2 | integer ambiguity resolution by LAMBDA 3 | 4 | reference : 5 | [1] P.J.G.Teunissen, The least-square ambiguity decorrelation adjustment: 6 | a method for fast GPS ambiguity estimation, J.Geodesy, Vol.70, 65-82, 7 | 1995 8 | [2] X.-W.Chang, X.Yang, T.Zhou, MLAMBDA: A modified LAMBDA method for 9 | integer least-squares estimation, J.Geodesy, Vol.79, 552-565, 2005 10 | 11 | Copyright (c) 2021 Rui Hirokawa (from CSSRLIB) 12 | Copyright (c) 2022 Tim Everett 13 | 14 | """ 15 | 16 | import numpy as np 17 | from numpy.linalg import inv 18 | 19 | 20 | def LD(Q): 21 | """ LD factorization (Q=L'*diag(D)*L) """ 22 | n = len(Q) 23 | L = np.zeros((n, n)) 24 | d = np.zeros(n) 25 | A = Q.copy() 26 | for i in range(n-1, -1, -1): 27 | d[i] = A[i,i] 28 | if d[i] <= 0.0: 29 | print('LD Factorization error') 30 | raise SystemExit 31 | L[i,:i+1] = A[i,:i+1] / np.sqrt(d[i]) 32 | for j in range(i): 33 | A[j,:j+1] -= L[i,:j+1] * L[i,j] 34 | L[i,:i+1] /= L[i,i] 35 | 36 | return L, d 37 | 38 | 39 | def reduction(L, d): 40 | """ lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) (ref.[1]) """ 41 | n = len(d) 42 | Z = np.eye(n) 43 | j = k = n-2 44 | while j >= 0: 45 | if j <= k: 46 | for i in range(j+1, n): 47 | # L,Z=gauss(L,Z,i,j) 48 | mu = round(L[i,j]) 49 | L[i:,j] -= mu * L[i:,i] 50 | Z[:,j] -= mu * Z[:,i] 51 | 52 | delta = d[j] + L[j+1,j]**2 * d[j+1] 53 | if delta + 1e-6 < d[j+1]: # compared considering numerical error 54 | eta = d[j] / delta 55 | lam = d[j+1] * L[j+1,j] / delta 56 | d[j] = eta * d[j+1] 57 | d[j+1] = delta 58 | L[j:j+2,:j] = np.array([[-L[j+1, j],1], [eta,lam]]) @ L[j:j+2,:j] 59 | L[j+1,j] = lam 60 | # swap L's and Z's 61 | L[j+2:,j], L[j+2:,j+1] = L[j+2:,j+1].copy() , L[j+2:,j].copy() 62 | Z[:,j], Z[:,j+1] = Z[:,j+1].copy(), Z[:,j].copy() 63 | 64 | j, k = n -2, j 65 | 66 | else: 67 | j -= 1 68 | return L, d, Z 69 | 70 | 71 | def search(L, d, zs, m=2): 72 | """ modified lambda (mlambda) search (ref. [2]) 73 | * args m I number of fixed solution 74 | L,d I transformed covariance matrix 75 | zs I transformed double-diff phase biases 76 | zn O fixed solutions 77 | s O sum of residuals for fixed solutions """ 78 | 79 | n = len(d) 80 | nn = 0 81 | imax = 0 82 | Chi2 = 1e18 83 | S = np.zeros((n, n)) 84 | dist = np.zeros(n) 85 | zb = np.zeros(n) 86 | z = np.zeros(n) 87 | step = np.zeros(n) 88 | zn = np.zeros((n, m)) 89 | s = np.zeros(m) 90 | k = n - 1 91 | zb[-1] = zs[-1] 92 | z[-1] = round(zb[-1]) 93 | y = zb[-1] - z[-1] 94 | step[-1] = np.sign(y) # step towards closest integer 95 | if step[-1] == 0: 96 | step[-1] = 1 97 | for _ in range(10000): 98 | # newdist=sum(((z(j)-zb(j))^2/d(j))) 99 | newdist = dist[k] + y**2 / d[k] 100 | if newdist < Chi2: 101 | # Case 1: move down 102 | if k != 0: 103 | k -= 1 104 | dist[k] = newdist 105 | S[k, :k+1] = S[k+1, :k+1] + (z[k+1] - zb[k+1]) * L[k+1,:k+1] 106 | zb[k] = zs[k] + S[k,k] 107 | z[k] = round(zb[k]) 108 | y = zb[k] - z[k] 109 | step[k] = np.sign(y) 110 | if step[k] == 0: 111 | step[k] = 1 112 | # Case 2: store the found candidate and try next valid integer 113 | else: 114 | if nn < m: # store the first m initial points 115 | if nn == 0 or newdist > s[imax]: 116 | imax = nn 117 | zn[:,nn] = z 118 | s[nn] = newdist 119 | nn += 1 120 | else: 121 | if newdist < s[imax]: 122 | zn[:,imax] = z 123 | s[imax] = newdist 124 | imax = np.argmax(s) 125 | Chi2 = s[imax] 126 | z[0] += step[0] # next valid integer 127 | y = zb[0]-z[0] 128 | step[0] = -step[0] - np.sign(step[0]) 129 | # Case 3: exit or move up 130 | else: 131 | if k == n - 1: 132 | break 133 | k += 1 # move up 134 | z[k] += step[k] # next valid integer 135 | y = zb[k] - z[k] 136 | step[k] = -step[k] - np.sign(step[k]) 137 | 138 | order = np.argsort(s) # sort by s 139 | s = s[order] 140 | zn = zn[:, order] 141 | 142 | return zn, s 143 | 144 | 145 | def mlambda(a, Q, m=2): 146 | """lambda/mlambda integer least-square estimation ------------------------------ 147 | * integer least-square estimation. reduction is performed by lambda (ref.[1]), 148 | * and search by mlambda (ref.[2]) 149 | * args m I number of fixed solutions 150 | * a I float parameters (n x 1) (double-diff phase biases) 151 | * Q I covariance matrix of float parameters (n x n) 152 | * afix_ O fixed solutions (n x m) 153 | * s O sum of squared residulas of fixed solutions (1 x m) """ 154 | 155 | # LD (lower diaganol) factorization (Q=L'*diag(D)*L) 156 | L, d = LD(Q) 157 | L, d, Z = reduction(L, d) 158 | invZt = np.round(inv(Z.T)) 159 | z = Z.T @ a 160 | # mlambda search 161 | # z = transformed double-diff phase biases 162 | # L,D = transformed covariance matrix 163 | E, s = search(L, d, z, m) 164 | afix_ = invZt @ E 165 | return afix_, s 166 | -------------------------------------------------------------------------------- /src/pntpos.py: -------------------------------------------------------------------------------- 1 | """ 2 | pntpos.py : module for standalone positioning 3 | 4 | Copyright (c) 2021 Rui Hirokawa (from CSSRLIB) 5 | Copyright (c) 2022 Tim Everett 6 | """ 7 | import numpy as np 8 | from numpy.linalg import norm, lstsq 9 | from rtkcmn import rCST, ecef2pos, geodist, satazel, ionmodel, tropmodel, \ 10 | Sol, tropmapf, uGNSS, trace, timeadd 11 | import rtkcmn as gn 12 | from ephemeris import seleph, satposs 13 | from rinex import rcvstds 14 | 15 | NX = 6 # num of estimated parameters, pos + clock 16 | MAXITR = 10 # max number of iteration or point pos 17 | ERR_ION = 5.0 # ionospheric delay Std (m) 18 | ERR_TROP = 3.0 # tropspheric delay Std (m) 19 | ERR_SAAS = 0.3 # Saastamoinen model error Std (m) 20 | ERR_BRDCI = 0.5 # broadcast ionosphere model error factor 21 | ERR_CBIAS = 0.3 # code bias error Std (m) 22 | REL_HUMI = 0.7 # relative humidity for Saastamoinen model 23 | MIN_EL = np.deg2rad(5) # min elevation for measurement 24 | 25 | def varerr(nav, sys, el, rcvstd): 26 | """ variation of measurement """ 27 | s_el = np.sin(el) 28 | if s_el <= 0.0: 29 | return 0.0 30 | a = 0.003 # use simple weighting, since only used for approx location 31 | b = 0.003 32 | var = a**2 + (b / s_el)**2 33 | var *= nav.efact[sys] 34 | return var 35 | 36 | def gettgd(sat, eph, type=0): 37 | """ get tgd: 0=E5a, 1=E5b """ 38 | sys = gn.sat2prn(sat)[0] 39 | if sys == uGNSS.GLO: 40 | return eph.dtaun * rCST.CLIGHT 41 | else: 42 | return eph.tgd[type] * rCST.CLIGHT 43 | 44 | 45 | def prange(nav, obs, i): 46 | eph = seleph(nav, obs.t, obs.sat[i]) 47 | P1 = obs.P[i,0] 48 | if P1 == 0: 49 | return 0 50 | sys = gn.sat2prn(obs.sat[i])[0] 51 | if sys != uGNSS.GLO: 52 | b1 = gettgd(obs.sat[i], eph, 0) 53 | return P1 - b1 54 | else: # GLONASS 55 | # TODO: abstract hard coded freqs 56 | gamma = nav.freq[4] / nav.freq[5] # G1/G2 57 | b1 = gettgd(obs.sat[i], eph, 0) 58 | return P1 - b1 / (gamma - 1) 59 | 60 | def rescode(iter, obs, nav, rs, dts, svh, x): 61 | """ calculate code residuals """ 62 | ns = len(obs.sat) # measurements 63 | trace(4, 'rescode : n=%d\n' % ns) 64 | v = np.zeros(ns + NX - 3) 65 | H = np.zeros((ns + NX - 3, NX)) 66 | mask = np.zeros(NX - 3) # clk states 67 | azv = np.zeros(ns) 68 | elv = np.zeros(ns) 69 | var = np.zeros(ns + NX - 3) 70 | 71 | rr = x[0:3].copy() 72 | dtr = x[3] 73 | pos = ecef2pos(rr) 74 | trace(3, 'rescode: rr=%.3f %.3f %.3f\n' % (rr[0], rr[1], rr[2])) 75 | rcvstds(nav, obs) # decode stdevs from receiver 76 | 77 | nv = 0 78 | for i in np.argsort(obs.sat): 79 | sys = nav.sysprn[obs.sat[i]][0] 80 | if norm(rs[i,:]) < rCST.RE_WGS84: 81 | continue 82 | if gn.satexclude(obs.sat[i], var[i], svh[i], nav): 83 | continue 84 | # geometric distance and elevation mask 85 | r, e = geodist(rs[i], rr) 86 | if r < 0: 87 | continue 88 | [az, el] = satazel(pos, e) 89 | if el < nav.elmin: 90 | continue 91 | if iter > 0: 92 | # test CNR 93 | if obs.S[i,[0]] < nav.cnr_min[0]: 94 | continue 95 | # ionospheric correction 96 | dion = ionmodel(obs.t, pos, az, el, nav.ion) 97 | freq = gn.sat2freq(obs.sat[i], 0, nav) 98 | dion *= (nav.freq[0] / freq)**2 99 | # tropospheric correction 100 | trop_hs, trop_wet, _ = tropmodel(obs.t, pos, el, REL_HUMI) 101 | mapfh, mapfw = tropmapf(obs.t, pos, el) 102 | dtrp = mapfh * trop_hs + mapfw * trop_wet 103 | else: 104 | dion = dtrp = 0 105 | # psendorange with code bias correction 106 | P = prange(nav, obs, i) 107 | if P == 0: 108 | continue 109 | # pseudorange residual 110 | v[nv] = P - (r + dtr - rCST.CLIGHT * dts[i] + dion + dtrp) 111 | trace(4, 'sat=%d: v=%.3f P=%.3f r=%.3f dtr=%.6f dts=%.6f dion=%.3f dtrp=%.3f\n' % 112 | (obs.sat[i],v[nv],P,r,dtr,dts[i],dion,dtrp)) 113 | # design matrix 114 | H[nv, 0:3] = -e 115 | H[nv, 3] = 1 116 | # time system offset and receiver bias correction 117 | if sys == uGNSS.GLO: 118 | v[nv] -= x[4] 119 | H[nv, 4] = 1.0 120 | mask[1] = 1 121 | elif sys == uGNSS.GAL: 122 | v[nv] -= x[5] 123 | H[nv, 5] = 1.0 124 | mask[2] = 1 125 | else: 126 | mask[0] = 1 127 | 128 | azv[nv] = az 129 | elv[nv] = el 130 | var[nv] = varerr(nav, sys, el, nav.rcvstd[obs.sat[i]-1,0]) 131 | nv += 1 132 | 133 | # constraint to avoid rank-deficient 134 | for i in range(NX - 3): 135 | if mask[i] == 0: 136 | v[nv] = 0.0 137 | H[nv, i+3] = 1 138 | var[nv] = 0.01 139 | nv += 1 140 | v = v[0:nv] 141 | H = H[0:nv, :] 142 | azv = azv[0:nv] 143 | elv = elv[0:nv] 144 | var = var[0:nv] 145 | return v, H, azv, elv, var 146 | 147 | 148 | def estpos(obs, nav, rs, dts, svh): 149 | """ estimate position and clock errors with standard precision """ 150 | x = np.zeros(NX) 151 | x[0:3] = nav.x[0:3] 152 | sol = Sol() 153 | trace(3, 'estpos : n=%d\n' % len(rs)) 154 | for iter in range(MAXITR): 155 | v, H, az, el, var = rescode(iter, obs, nav, rs[:,0:3], dts, svh, x) 156 | nv = len(v) 157 | if nv < NX: 158 | trace(3, 'estpos: lack of valid sats nsat=%d nv=%d\n' % 159 | (len(obs.sat), nv)) 160 | return sol 161 | # weight by variance (lsq uses sqrt of weight 162 | std = np.sqrt(var) 163 | v /= std 164 | H /= std[:,None] 165 | # least square estimation 166 | dx = lstsq(H, v, rcond=None)[0] 167 | x += dx 168 | if norm(dx) < 1e-4: 169 | break 170 | else: # exceeded max iterations 171 | sol.stat = gn.SOLQ_NONE 172 | trace(3, 'estpos: solution did not converge\n') 173 | sol.stat = gn.SOLQ_SINGLE 174 | sol.t = timeadd(obs.t, -x[3] / rCST.CLIGHT ) 175 | sol.dtr = x[3:5] / rCST.CLIGHT 176 | sol.rr[0:3] = x[0:3] 177 | sol.rr[3:6] = 0 178 | return sol 179 | 180 | def pntpos(obs, nav): 181 | """ single-point positioning ---------------------------------------------------- 182 | * compute receiver position, velocity, clock bias by single-point positioning 183 | * with pseudorange and doppler observables 184 | * args : obs I observation data 185 | * nav I navigation data 186 | * return : sol O """ 187 | rs, _, dts, svh = satposs(obs, nav) 188 | sol = estpos(obs, nav, rs, dts, svh) 189 | return sol 190 | 191 | 192 | -------------------------------------------------------------------------------- /src/postpos.py: -------------------------------------------------------------------------------- 1 | """ 2 | post-processing solution from rinex data 3 | """ 4 | import numpy as np 5 | from copy import copy, deepcopy 6 | from rtkpos import rtkpos, rtkinit 7 | import __ppk_config as cfg 8 | import rinex as rn 9 | from pntpos import pntpos 10 | import rtkcmn as gn 11 | 12 | 13 | def combres(solf, solb): 14 | # combine forward/backward solutions 15 | pri = [7,1,2,3,4,5,1,6] # priority of solution status 16 | i, j, solc = 0, len(solb) - 1, [] 17 | gn.trace(3, 'combres: # forward = %d, # backward = %d\n' % (len(solf), 18 | len(solb))) 19 | while i < len(solf) or j >= 0: 20 | if i >= len(solf): 21 | sol = deepcopy(solb[j]) 22 | elif j < 0: 23 | sol = deepcopy(solf[i]) 24 | elif not (solf[i].stat == gn.SOLQ_NONE and solb[j].stat == gn.SOLQ_NONE): 25 | tt = gn.timediff(solf[i].t, solb[j].t) 26 | if tt < -gn.DTTOL: 27 | sol = deepcopy(solf[i]) 28 | j += 1 29 | elif tt > gn.DTTOL: 30 | sol = deepcopy(solb[j]) 31 | i -= 1 32 | elif pri[solf[i].stat] < pri[solb[j].stat]: 33 | sol = deepcopy(solf[i]) 34 | elif pri[solf[i].stat] > pri[solb[j].stat]: 35 | sol = deepcopy(solb[j]) 36 | else: 37 | sol = deepcopy(solf[i]) 38 | sol.t = gn.timeadd(sol.t, -tt / 2) 39 | sol.rr[0:3], sol.qr[0:3,0:3] = gn.smoother(solf[i].rr[0:3], 40 | solb[j].rr[0:3], solf[i].qr, solb[j].qr) 41 | solc.append(sol) 42 | gn.trace(4, ' %d: f=%d %d b=%d %d tt=%.3f\n' % (sol.t.time, 43 | solf[i].t.time, solf[i].stat, solb[j].t.time, solb[j].stat, tt)) 44 | i, j = i + 1, j - 1 45 | return solc 46 | 47 | 48 | def firstpos(nav, rov, base, dir): 49 | # find rover position from first obs, 50 | obsr, obsb = rn.first_obs(nav, rov, base, dir) 51 | sol = pntpos(obsr, nav) 52 | # repeat until get solution 53 | while sol.stat == gn.SOLQ_NONE: 54 | obsr, obsb = rn.next_obs(nav, rov, base, dir) 55 | sol = pntpos(obsr, nav) 56 | gn.trace(3, 'init rr: %.2f %.2f %.2f: %d\n' % (sol.rr[0], sol.rr[1], 57 | sol.rr[2], sol.stat)) 58 | nav.x[0:6] = copy(sol.rr[0:6]) 59 | nav.rr[0:3] = copy(sol.rr[0:3]) 60 | 61 | def sqrtvar(cov): 62 | " sqrt of covariance " 63 | return np.sqrt(abs(cov)) * np.sign(cov) 64 | 65 | def savesol(sol, solfile): 66 | D2R = gn.rCST.D2R 67 | solhdr = '% GPST latitude(deg) longitude(deg) height(m) Q ' \ 68 | 'ns sdn(m) sde(m) sdu(m) sdne(m) sdeu(m) sdun(m) age(s) ratio\n' 69 | with open(solfile, 'w') as outfile: 70 | outfile.write(solhdr) 71 | for s in sol: 72 | if s.stat == gn.SOLQ_NONE: 73 | continue 74 | wk, sec = gn.time2gpst(s.t) 75 | llh = gn.ecef2pos(s.rr[0:3]) 76 | std = sqrtvar(gn.covenu(llh, s.qr)) 77 | fmt = '%4d %10.3f %14.9f %14.9f %10.4f %3d %3d %8.4f' + \ 78 | ' %8.4f %8.4f %8.4f %8.4f %8.4f %6.2f %6.1f\n' 79 | outfile.write(fmt % (wk, sec, llh[0]/D2R, llh[1]/D2R, llh[2], 80 | s.stat, s.ns, std[1,1], std[0,0], std[2,2], std[0,1], 81 | std[2,0], std[1,2], s.age, s.ratio)) 82 | 83 | def procpos(nav, rov, base, fp_stat): 84 | 85 | try: 86 | if nav.filtertype != 'backward': 87 | # run forward solution 88 | firstpos(nav, rov, base, dir=1) 89 | rtkpos(nav, rov, base, fp_stat, dir=1) 90 | sol0 = deepcopy(nav.sol) 91 | savesol(sol0,'forward.pos') 92 | if nav.filtertype != 'forward': 93 | # run backward solution 94 | if nav.filtertype != 'combined_noreset': 95 | # reset filter states 96 | rb = nav.rb.copy() 97 | eph, geph = nav.eph.copy(), nav.geph.copy() 98 | glofrq = nav.glofrq.copy() 99 | maxepoch = nav.maxepoch 100 | nav = rtkinit(cfg) 101 | nav.rb = rb 102 | nav.eph, nav.geph = eph, geph 103 | nav.glofrq = glofrq 104 | nav.maxepoch = maxepoch 105 | firstpos(nav, rov, base, dir=-1) 106 | else: # combined_noreset 107 | nav.sol = [nav.sol[-1]] 108 | rtkpos(nav, rov, base, fp_stat, dir=-1) 109 | savesol(nav.sol,'backward.pos') 110 | if nav.filtertype == 'combined' or nav.filtertype == 'combined_noreset': 111 | sol = combres(sol0, nav.sol) 112 | savesol(sol,'combined.pos') 113 | return sol 114 | except KeyboardInterrupt: 115 | pass 116 | return nav.sol 117 | -------------------------------------------------------------------------------- /src/rinex.py: -------------------------------------------------------------------------------- 1 | """ 2 | module for RINEX 3.0x processing 3 | 4 | Copyright (c) 2021 Rui Hirokawa (from CSSRLIB) 5 | Copyright (c) 2022 Tim Everett 6 | """ 7 | 8 | import numpy as np 9 | from copy import deepcopy 10 | from rtkcmn import uGNSS, rSIG, Eph, Geph, prn2sat, gpst2time, time2gpst, Obs, \ 11 | epoch2time, timediff, timeadd, utc2gpst 12 | import rtkcmn as gn 13 | from ephemeris import satposs 14 | 15 | class rnx_decode: 16 | """ class for RINEX decoder """ 17 | MAXSAT = uGNSS.GPSMAX+uGNSS.GLOMAX+uGNSS.GALMAX+uGNSS.BDSMAX+uGNSS.QZSMAX 18 | 19 | def __init__(self, cfg): 20 | self.ver = -1.0 21 | self.fobs = None 22 | self.gnss_tbl = {'G': uGNSS.GPS, 'E': uGNSS.GAL, 'R': uGNSS.GLO, 'J': uGNSS.QZS} 23 | self.sig_tbl = cfg.sig_tbl 24 | self.skip_sig_tbl = cfg.skip_sig_tbl 25 | self.nf = 4 26 | self.sigid = np.ones((uGNSS.GNSSMAX, rSIG.SIGMAX*3), dtype=int) * rSIG.NONE 27 | self.typeid = np.ones((uGNSS.GNSSMAX, rSIG.SIGMAX*3), dtype=int) * rSIG.NONE 28 | self.nsig = np.zeros((uGNSS.GNSSMAX), dtype=int) 29 | self.nband = np.zeros((uGNSS.GNSSMAX), dtype=int) 30 | self.pos = np.array([0, 0, 0]) 31 | 32 | def flt(self, u, c=-1): 33 | if c >= 0: 34 | u = u[19*c+4:19*(c+1)+4] 35 | try: 36 | return float(u.replace("D", "E")) 37 | except: 38 | return 0 39 | 40 | 41 | def adjday(self, t, t0): 42 | """" adjust time considering week handover """ 43 | tt = timediff(t, t0) 44 | if tt < -43200.0: 45 | return timeadd(t, 86400.0) 46 | if tt > 43200.0: 47 | return timeadd(t,-86400.0) 48 | return t 49 | 50 | def decode_nav(self, navfile, nav): 51 | """decode RINEX Navigation message from file """ 52 | nav.eph = [] 53 | nav.geph = [] 54 | with open(navfile, 'rt') as fnav: 55 | for line in fnav: 56 | if line[60:73] == 'END OF HEADER': 57 | break 58 | elif line[60:80] == 'RINEX VERSION / TYPE': 59 | self.ver = float(line[4:10]) 60 | if self.ver < 3.02: 61 | return -1 62 | elif line[60:76] == 'IONOSPHERIC CORR': 63 | if line[0:4] == 'GPSA' or line[0:4] == 'QZSA': 64 | for k in range(4): 65 | nav.ion[0, k] = self.flt(line[5+k*12:5+(k+1)*12]) 66 | if line[0:4] == 'GPSB' or line[0:4] == 'QZSB': 67 | for k in range(4): 68 | nav.ion[1, k] = self.flt(line[5+k*12:5+(k+1)*12]) 69 | 70 | for line in fnav: 71 | if line[0] not in self.gnss_tbl: 72 | continue 73 | sys = self.gnss_tbl[line[0]] 74 | prn = int(line[1:3]) 75 | if sys == uGNSS.QZS: 76 | prn += 192 77 | sat = prn2sat(sys, prn) 78 | year = int(line[4:8]) 79 | month = int(line[9:11]) 80 | day = int(line[12:14]) 81 | hour = int(line[15:17]) 82 | minute = int(line[18:20]) 83 | sec = int(line[21:23]) 84 | toc = epoch2time([year, month, day, hour, minute, sec]) 85 | if sys != uGNSS.GLO: 86 | eph = Eph(sat) 87 | eph.toc = toc 88 | eph.f0 = self.flt(line, 1) 89 | eph.f1 = self.flt(line, 2) 90 | eph.f2 = self.flt(line, 3) 91 | 92 | line = fnav.readline() #3:6 93 | eph.iode = int(self.flt(line, 0)) 94 | eph.crs = self.flt(line, 1) 95 | eph.deln = self.flt(line, 2) 96 | eph.M0 = self.flt(line, 3) 97 | 98 | line = fnav.readline() #7:10 99 | eph.cuc = self.flt(line, 0) 100 | eph.e = self.flt(line, 1) 101 | eph.cus = self.flt(line, 2) 102 | sqrtA = self.flt(line, 3) 103 | eph.A = sqrtA**2 104 | 105 | line = fnav.readline() #11:14 106 | eph.toes = int(self.flt(line, 0)) 107 | eph.cic = self.flt(line, 1) 108 | eph.OMG0 = self.flt(line, 2) 109 | eph.cis = self.flt(line, 3) 110 | 111 | line = fnav.readline() #15:18 112 | eph.i0 = self.flt(line, 0) 113 | eph.crc = self.flt(line, 1) 114 | eph.omg = self.flt(line, 2) 115 | eph.OMGd = self.flt(line, 3) 116 | 117 | line = fnav.readline() #19:22 118 | eph.idot = self.flt(line, 0) 119 | eph.code = int(self.flt(line, 1)) # source for GAL NAV type 120 | eph.week = int(self.flt(line, 2)) 121 | 122 | line = fnav.readline() #23:26 123 | eph.sva = self.flt(line, 0) 124 | eph.svh = int(self.flt(line, 1)) 125 | tgd = np.zeros(2) 126 | tgd[0] = float(self.flt(line, 2)) 127 | if sys == uGNSS.GAL: 128 | tgd[1] = float(self.flt(line, 3)) 129 | else: 130 | eph.iodc = int(self.flt(line, 3)) 131 | eph.tgd = tgd 132 | 133 | line = fnav.readline() #27:30 134 | tot = int(self.flt(line, 0)) 135 | if len(line) >= 42: 136 | eph.fit = int(self.flt(line, 1)) 137 | 138 | eph.toe = gpst2time(eph.week, eph.toes) 139 | eph.tot = gpst2time(eph.week, tot) 140 | nav.eph.append(eph) 141 | else: # GLONASS 142 | if prn > uGNSS.GLOMAX: 143 | print('Reject nav entry: %s' % line[:3]) 144 | break 145 | geph = Geph(sat) 146 | # Toc rounded by 15 min in utc 147 | week, tow = time2gpst(toc) 148 | toc = gpst2time(week,np.floor((tow + 450.0) / 900.0) * 900) 149 | dow = int(np.floor(tow / 86400.0)) 150 | # time of frame in UTC 151 | tod = self.flt(line, 2) % 86400 152 | tof = gpst2time(week ,tod + dow * 86400.0) 153 | tof = self.adjday(tof, toc) 154 | geph.toe = utc2gpst(toc) 155 | geph.tof = utc2gpst(tof) 156 | # IODE = Tb (7bit), Tb =index of UTC+3H within current day 157 | geph.iode = int(((tow + 10800.0) % 86400) / 900.0 + 0.5) 158 | geph.taun = -self.flt(line, 1) 159 | geph.gamn = self.flt(line, 2) 160 | 161 | line = fnav.readline() #3:6 162 | pos =np.zeros(3) 163 | vel = np.zeros(3) 164 | acc = np.zeros(3) 165 | pos[0] = self.flt(line, 0) 166 | vel[0] = self.flt(line, 1) 167 | acc[0] = self.flt(line, 2) 168 | geph.svh = self.flt(line, 3) 169 | 170 | line = fnav.readline() #7:10 171 | pos[1] = self.flt(line, 0) 172 | vel[1] = self.flt(line, 1) 173 | acc[1] = self.flt(line, 2) 174 | geph.frq = self.flt(line, 3) 175 | nav.glofrq[sat - uGNSS.GPSMAX - 1] = int(geph.frq) 176 | 177 | line = fnav.readline() #11:14 178 | pos[2] = self.flt(line, 0) 179 | vel[2] = self.flt(line, 1) 180 | acc[2] = self.flt(line, 2) 181 | geph.age = self.flt(line, 2) 182 | 183 | geph.pos = pos * 1000 184 | geph.vel = vel * 1000 185 | geph.acc = acc * 1000 186 | 187 | nav.geph.append(geph) 188 | 189 | #nav.eph.sort(key=lambda x: (x.sat, x.toe.time)) 190 | nav.eph.sort(key=lambda x: x.toe.time) 191 | nav.geph.sort(key=lambda x: x.toe.time) 192 | return nav 193 | 194 | def decode_obsh(self, obsfile): 195 | self.fobs = open(obsfile, 'rt') 196 | for line in self.fobs: 197 | if line[60:73] == 'END OF HEADER': 198 | break 199 | if line[60:80] == 'RINEX VERSION / TYPE': 200 | self.ver = float(line[4:10]) 201 | if self.ver < 3.02: 202 | return -1 203 | elif line[60:79] == 'APPROX POSITION XYZ': 204 | self.pos = np.array([float(line[0:14]), 205 | float(line[14:28]), 206 | float(line[28:42])]) 207 | elif line[60:79] == 'SYS / # / OBS TYPES': 208 | if line[0] in self.gnss_tbl: 209 | sys = self.gnss_tbl[line[0]] 210 | else: 211 | continue 212 | self.nsig[sys] = int(line[3:6]) 213 | s = line[7:7+4*13] 214 | if self.nsig[sys] >= 14: 215 | line2 = self.fobs.readline() 216 | s += line2[7:7+4*13] 217 | 218 | for k in range(self.nsig[sys]): 219 | sig = s[4*k:3+4*k] 220 | if sig[1:3] not in self.sig_tbl: 221 | continue 222 | if self.sig_tbl[sig[1:3]] in self.skip_sig_tbl[sys]: 223 | continue 224 | if sig[0] == 'C': 225 | self.typeid[sys][k] = 0 226 | elif sig[0] == 'L': 227 | self.typeid[sys][k] = 1 228 | elif sig[0] == 'S': 229 | self.typeid[sys][k] = 2 230 | elif sig[0] == 'D': 231 | self.typeid[sys][k] = 3 232 | else: 233 | continue 234 | self.sigid[sys][k] = self.sig_tbl[sig[1:3]] 235 | self.nband[sys] = len(np.where(self.typeid[sys]==1)[0]) 236 | return 0 237 | 238 | def decode_obs(self, nav, maxepoch): 239 | """decode RINEX Observation message from file """ 240 | 241 | self.obslist = [] 242 | nepoch = 0 243 | for line in self.fobs: 244 | if line == '': 245 | break 246 | if line[0] != '>': 247 | continue 248 | obs = Obs() 249 | nsat = int(line[32:35]) 250 | year = int(line[2:6]) 251 | month = int(line[7:9]) 252 | day = int(line[10:12]) 253 | hour = int(line[13:15]) 254 | minute = int(line[16:18]) 255 | sec = float(line[19:29]) 256 | obs.t = epoch2time([year, month, day, hour, minute, sec]) 257 | obs.P = np.zeros((nsat, gn.MAX_NFREQ)) 258 | obs.L = np.zeros((nsat, gn.MAX_NFREQ)) 259 | obs.D = np.zeros((nsat, gn.MAX_NFREQ)) 260 | obs.S = np.zeros((nsat, gn.MAX_NFREQ)) 261 | obs.lli = np.zeros((nsat, gn.MAX_NFREQ), dtype=int) 262 | obs.Pstd = np.zeros((nsat, gn.MAX_NFREQ), dtype=int) 263 | obs.Lstd = np.zeros((nsat, gn.MAX_NFREQ), dtype=int) 264 | obs.mag = np.zeros((nsat, gn.MAX_NFREQ)) 265 | obs.sat = np.zeros(nsat, dtype=int) 266 | n = 0 267 | for k in range(nsat): 268 | line = self.fobs.readline() 269 | if line[0] not in self.gnss_tbl: 270 | continue 271 | sys = self.gnss_tbl[line[0]] 272 | if sys not in nav.gnss_t: 273 | continue 274 | prn = int(line[1:3]) 275 | if sys == uGNSS.QZS: 276 | prn += 192 277 | obs.sat[n] = prn2sat(sys, prn) 278 | if obs.sat[n] == 0: 279 | continue 280 | nsig_max = (len(line) - 4 + 2) // 16 281 | for i in range(self.nsig[sys]): 282 | if i >= nsig_max: 283 | break 284 | obs_ = line[16*i+4:16*i+17].strip() 285 | if obs_ == '' or self.sigid[sys][i] == 0: 286 | continue 287 | try: 288 | obsval = float(obs_) 289 | except: 290 | obsval = 0 291 | f = i // (self.nsig[sys] // self.nband[sys]) 292 | if f >= gn.MAX_NFREQ: 293 | print('Obs file too complex, please use RTKCONV to remove unused signals') 294 | raise SystemExit 295 | if self.typeid[sys][i] == 0: # code 296 | obs.P[n, f] = obsval 297 | Pstd = line[16*i+18] 298 | obs.Pstd[n, f] = int(Pstd) if Pstd != " " else 0 299 | elif self.typeid[sys][i] == 1: # carrier 300 | obs.L[n, f] = float(obs_) 301 | lli = line[16*i+17] 302 | obs.lli[n, f] = int(lli) if lli != " " else 0 303 | Lstd = line[16*i+18] 304 | obs.Lstd[n, f] = int(Lstd) if Lstd != " " else 0 305 | elif self.typeid[sys][i] == 2: # C/No 306 | obs.S[n, f] = obsval 307 | elif self.typeid[sys][i] == 3: # Doppler 308 | obs.D[n, f] = obsval 309 | n += 1 310 | obs.P = obs.P[:n, :] 311 | obs.L = obs.L[:n, :] 312 | obs.Pstd = obs.Pstd[:n, :] 313 | obs.Lstd = obs.Lstd[:n, :] 314 | obs.D = obs.D[:n, :] 315 | obs.S = obs.S[:n, :] 316 | obs.lli = obs.lli[:n, :] 317 | obs.mag = obs.mag[:n, :] 318 | obs.sat = obs.sat[:n] 319 | self.obslist.append(obs) 320 | nepoch += 1 321 | if maxepoch != None and nepoch >= maxepoch: 322 | break 323 | self.index = 0 324 | self.fobs.close() 325 | 326 | 327 | def decode_obsfile(self, nav, obsfile, maxepoch): 328 | self.decode_obsh(obsfile) 329 | self.decode_obs(nav, maxepoch) 330 | 331 | def first_obs(nav, rov, base, dir): 332 | if dir == 1: # forward solution 333 | rov.index = base.index = 0 334 | else: # backward solution 335 | rov.index = len(rov.obslist) - 1 336 | base.index = len(base.obslist) - 1 337 | # sync base and rover, step one obs to sync 338 | _, _ =next_obs(nav, rov, base, dir) 339 | # step back to first obs 340 | obsr, obsb = next_obs(nav, rov, base, -dir) 341 | return obsr, obsb 342 | 343 | def next_obs(nav, rov, base, dir): 344 | """ sync observations between rover and base """ 345 | rov.index += dir # 1=forward, -1=backward 346 | if abs(dir) != 1 or rov.index < 0 or rov.index >= len(rov.obslist): 347 | return [], [] 348 | obsr, obsb = rov.obslist[rov.index], base.obslist[base.index] 349 | dt = timediff(obsr.t, obsb.t) 350 | baseChange = False 351 | ixb = base.index + dir 352 | while True: 353 | if ixb < 0 or ixb >= len(base.obslist): 354 | ixb -= dir 355 | dt_next = dt 356 | break # hit end of obs list 357 | dt_next = timediff(obsr.t, base.obslist[ixb].t) 358 | if abs(dt_next) >= abs(dt): 359 | break # next base obs is not closer 360 | else: 361 | base.index = ixb 362 | ixb += dir 363 | baseChange = True 364 | dt = dt_next 365 | 366 | if baseChange and nav.interp_base and len(nav.sol) > 0: 367 | # save base residuals for next epoch 368 | nav.obsb = deepcopy(obsb) 369 | nav.rsb, nav.varb, nav.dtsb, nav.svhb = satposs(obsb, nav) 370 | obsb = base.obslist[base.index] 371 | return obsr, obsb 372 | 373 | def rcvstds(nav, obs): 374 | """ decode receiver stdevs from rinex fields """ 375 | # skip if weighting factor is zero 376 | if nav.err[5] == 0: 377 | return 378 | for i in np.argsort(obs.sat): 379 | for f in range(nav.nf): 380 | s = obs.sat[i] - 1 381 | # decode receiver stdevs, 382 | # Lstd: 0.004 cycles -> m 383 | nav.rcvstd[s,f] = obs.Lstd[i,f] * 0.004 * 0.2 384 | # Pstd: 0.01*2^(n+5) 385 | nav.rcvstd[s,f+nav.nf] = 0.01 * (1 << (obs.Pstd[i,f] + 5)) 386 | 387 | -------------------------------------------------------------------------------- /src/rtkcmn.py: -------------------------------------------------------------------------------- 1 | """ 2 | module for GNSS processing 3 | 4 | Copyright (c) 2021 Rui Hirokawa (from CSSRLIB) 5 | Copyright (c) 2022 Tim Everett 6 | """ 7 | 8 | from copy import copy, deepcopy 9 | from enum import IntEnum 10 | from math import floor, sin, cos, sqrt, asin, atan2, fabs 11 | import numpy as np 12 | from numpy.linalg import norm, inv 13 | import sys 14 | 15 | gpst0 = [1980, 1, 6, 0, 0, 0] 16 | ion_default = np.array([ # 2004/1/1 17 | [0.1118E-07,-0.7451E-08,-0.5961E-07, 0.1192E-06], 18 | [0.1167E+06,-0.2294E+06,-0.1311E+06, 0.1049E+07]]) 19 | # troposhere model 20 | nmf_coef = np.array([ 21 | [1.2769934E-3, 1.2683230E-3, 1.2465397E-3, 1.2196049E-3, 1.2045996E-3], 22 | [2.9153695E-3, 2.9152299E-3, 2.9288445E-3, 2.9022565E-3, 2.9024912E-3], 23 | [62.610505E-3, 62.837393E-3, 63.721774E-3, 63.824265E-3, 64.258455E-3], 24 | [0.0000000E-0, 1.2709626E-5, 2.6523662E-5, 3.4000452E-5, 4.1202191E-5], 25 | [0.0000000E-0, 2.1414979E-5, 3.0160779E-5, 7.2562722E-5, 11.723375E-5], 26 | [0.0000000E-0, 9.0128400E-5, 4.3497037E-5, 84.795348E-5, 170.37206E-5], 27 | [5.8021897E-4, 5.6794847E-4, 5.8118019E-4, 5.9727542E-4, 6.1641693E-4], 28 | [1.4275268E-3, 1.5138625E-3, 1.4572752E-3, 1.5007428E-3, 1.7599082E-3], 29 | [4.3472961E-2, 4.6729510E-2, 4.3908931E-2, 4.4626982E-2, 5.4736038E-2]]) 30 | nmf_aht = [2.53E-5, 5.49E-3, 1.14E-3] # height correction 31 | 32 | # global defines 33 | DTTOL = 0.025 34 | MAX_NFREQ = 2 35 | SOLQ_NONE = 0 36 | SOLQ_FIX = 1 37 | SOLQ_FLOAT = 2 38 | SOLQ_DGPS = 4 39 | SOLQ_SINGLE = 5 40 | 41 | MAX_VAR_EPH = 300**2 # max variance eph to reject satellite 42 | 43 | class rCST(): 44 | """ class for constants """ 45 | CLIGHT = 299792458.0 46 | MU_GPS = 3.9860050E14 47 | MU_GAL = 3.986004418E14 48 | MU_GLO = 3.9860044E14 49 | GME = 3.986004415E+14 50 | GMS = 1.327124E+20 51 | GMM = 4.902801E+12 52 | OMGE = 7.2921151467E-5 53 | OMGE_GAL = 7.2921151467E-5 54 | OMGE_GLO = 7.292115E-5 55 | RE_WGS84 = 6378137.0 56 | RE_GLO = 6378136.0 57 | FE_WGS84 = (1.0/298.257223563) 58 | J2_GLO = 1.0826257E-3 # 2nd zonal harmonic of geopot 59 | AU = 149597870691.0 60 | D2R = 0.017453292519943295 61 | AS2R = D2R/3600.0 62 | DAY_SEC = 86400.0 63 | CENTURY_SEC = DAY_SEC*36525.0 64 | 65 | 66 | class uGNSS(IntEnum): 67 | """ class for GNSS constants """ 68 | GPS = 0 69 | SBS = 1 70 | GLO = 2 71 | BDS = 3 72 | QZS = 5 73 | GAL = 6 74 | IRN = 7 75 | GNSSMAX = 8 76 | GPSMAX = 32 77 | GALMAX = 36 78 | QZSMAX = 10 79 | GLOMAX = 27 80 | # BDSMAX = 63 81 | # SBSMAX = 24 82 | # IRNMAX = 10 83 | BDSMAX = 0 84 | SBSMAX = 0 85 | IRNMAX = 0 86 | NONE = -1 87 | MAXSAT = GPSMAX+GLOMAX+GALMAX+BDSMAX+QZSMAX+SBSMAX+IRNMAX 88 | 89 | class uSIG(IntEnum): 90 | """ class for GNSS signals """ 91 | GPS_L1CA = 0 92 | GPS_L2W = 2 93 | GPS_L2CL = 3 94 | GPS_L2CM = 4 95 | GPS_L5Q = 6 96 | SBS_L1CA = 0 97 | GAL_E1C = 0 98 | GAL_E1B = 1 99 | GAL_E5BI = 5 100 | GAL_E5BQ = 6 101 | GLO_L1C = 0 102 | GLO_L2C = 1 103 | BDS_B1ID1 = 0 104 | BDS_B1ID2 = 1 105 | BDS_B2ID1 = 2 106 | BDS_B2ID2 = 3 107 | QZS_L1CA = 0 108 | QZS_L1S = 1 109 | QZS_L2CM = 4 110 | QZS_L2CL = 5 111 | GLO_L1OF = 0 112 | GLO_L2OF = 2 113 | NONE = -1 114 | SIGMAX = 8 115 | 116 | 117 | class rSIG(IntEnum): 118 | """ class to define signals """ 119 | NONE = 0 120 | L1C = 1 121 | L1X = 2 122 | L1W = 3 123 | L2C = 4 124 | L2L = 5 125 | L2X = 6 126 | L2W = 7 127 | L5Q = 8 128 | L5X = 9 129 | L7Q = 10 130 | L7X = 11 131 | SIGMAX = 16 132 | 133 | 134 | class gtime_t(): 135 | """ class to define the time """ 136 | 137 | def __init__(self, time=0, sec=0.0): 138 | self.time = time 139 | self.sec = sec 140 | 141 | 142 | class Obs(): 143 | """ class to define the observation """ 144 | 145 | def __init__(self): 146 | self.t = gtime_t() 147 | self.P = [] 148 | self.L = [] 149 | self.S = [] 150 | self.D = [] 151 | self.lli = [] 152 | self.Lstd = [] 153 | self.Pstd = [] 154 | self.sat = [] 155 | 156 | 157 | class Eph(): 158 | """ class to define GPS/GAL/QZS/CMP ephemeris """ 159 | sat = 0 160 | iode = 0 161 | iodc = 0 162 | f0 = 0.0 163 | f1 = 0.0 164 | f2 = 0.0 165 | toc = 0 166 | toe = 0 167 | tot = 0 168 | week = 0 169 | crs = 0.0 170 | crc = 0.0 171 | cus = 0.0 172 | cus = 0.0 173 | cis = 0.0 174 | cic = 0.0 175 | e = 0.0 176 | i0 = 0.0 177 | A = 0.0 178 | deln = 0.0 179 | M0 = 0.0 180 | OMG0 = 0.0 181 | OMGd = 0.0 182 | omg = 0.0 183 | idot = 0.0 184 | tgd = [0.0, 0.0] 185 | sva = 0 186 | health = 0 187 | fit = 0 188 | toes = 0 189 | 190 | def __init__(self, sat=0): 191 | self.sat = sat 192 | 193 | class Geph(): 194 | """ class to define GLO ephemeris """ 195 | sat = 0 196 | iode = 0 197 | frq = 0 198 | svh = 0 199 | sva = 0 200 | age = 0 201 | toe = 0 202 | tof = 0 203 | pos = np.zeros(3) 204 | vel = np.zeros(3) 205 | acc = np.zeros(3) 206 | taun = 0.0 207 | gamn = 0.0 208 | dtaun = 0.0 209 | 210 | def __init__(self, sat=0): 211 | self.sat = sat 212 | 213 | class Nav(): 214 | """ class to define the navigation message """ 215 | 216 | def __init__(self, cfg): 217 | self.eph = [] 218 | self.geph = [] 219 | self.ion = ion_default 220 | self.rb = [0, 0, 0] # base station position in ECEF [m] 221 | self.rr = [0, 0, 0] 222 | self.stat = SOLQ_NONE 223 | 224 | # no ant pcv for now 225 | self.ant_pcv = 3*[19*[0]] 226 | self.ant_pco = 3 * [0] 227 | self.ant_pcv_b = 3*[19*[0]] 228 | self.ant_pco_b = 3 * [0] 229 | 230 | # satellite observation status 231 | self.nf = cfg.nf 232 | self.fix = np.zeros((uGNSS.MAXSAT, self.nf), dtype=int) 233 | self.outc = np.zeros((uGNSS.MAXSAT, self.nf), dtype=int) 234 | self.vsat = np.zeros((uGNSS.MAXSAT, self.nf), dtype=int) 235 | self.rejc = np.zeros((uGNSS.MAXSAT, self.nf), dtype=int) 236 | self.lock = np.zeros((uGNSS.MAXSAT, self.nf), dtype=int) 237 | self.slip = np.zeros((uGNSS.MAXSAT, self.nf), dtype=int) 238 | self.prev_lli = np.zeros((uGNSS.MAXSAT, self.nf, 2), dtype=int) 239 | self.prev_fix = np.zeros((uGNSS.MAXSAT, self.nf), dtype=int) 240 | self.glofrq = np.zeros(uGNSS.GLOMAX, dtype=int) 241 | self.rcvstd = np.zeros((uGNSS.MAXSAT, self.nf*2)) 242 | self.resp = np.zeros((uGNSS.MAXSAT, self.nf)) 243 | self.resc = np.zeros((uGNSS.MAXSAT, self.nf)) 244 | 245 | self.prev_ratio1 = 0 246 | self.prev_ratio2 = 0 247 | self.nb_ar = 0 248 | 249 | self.eph_index = np.zeros(uGNSS.MAXSAT, dtype=int) 250 | self.tt = 0 251 | self.maxepoch = None 252 | self.ns = 0 253 | self.dt = 0 254 | self.obsb = Obs() 255 | self.rsb = [] 256 | self.dtsb = [] 257 | self.svhb = [] 258 | self.varb = [] 259 | 260 | class Sol(): 261 | """" class for solution """ 262 | def __init__(self): 263 | self.dtr = np.zeros(2) 264 | self.rr = np.zeros(6) 265 | self.qr = np.zeros((3,3)) 266 | self.qv = np.zeros((3,3)) 267 | self.stat = SOLQ_NONE 268 | self.ns = 0 269 | self.age = 0 270 | self.ratio = 0 271 | self.t = gtime_t() 272 | 273 | 274 | def leaps(tgps): 275 | """ return leap seconds (TBD) """ 276 | return -18.0 277 | 278 | def filter(x, P, H, v, R): 279 | """* kalman filter state update as follows: 280 | * 281 | * K=P*H*(H'*P*H+R)^-1, xp=x+K*v, Pp=(I-K*H')*P 282 | * 283 | * args : double *x I states vector (n x 1) 284 | * double *P I covariance matrix of states (n x n) 285 | * double *H I transpose of design matrix (n x m) 286 | * double *v I innovation (measurement - model) (m x 1) 287 | * double *R I covariance matrix of measurement error (m x m) 288 | * int n,m number of states and measurements 289 | * double *xp O states vector after update (n x 1) 290 | * double *Pp O covariance matrix of states after update (n x n)""" 291 | 292 | n = len(x) 293 | K = P @ H @ np.linalg.inv(H.T @ P @ H + R) 294 | xp = x + K @ v 295 | Pp = (np.eye(n) - K @ H.T) @ P 296 | 297 | return xp, Pp 298 | 299 | def smoother(xf, xb, Qf, Qb): 300 | """ smoother -------------------------------------------------------------------- 301 | * combine forward and backward filters by fixed-interval smoother as follows: 302 | * 303 | * xs=Qs*(Qf^-1*xf+Qb^-1*xb), Qs=(Qf^-1+Qb^-1)^-1) 304 | * 305 | * args : double xf I forward solutions (n x 1) 306 | * args : double Qf I forward solutions covariance matrix (n x n) 307 | * double xb I backward solutions (n x 1) 308 | * double Qb I backward solutions covariance matrix (n x n) 309 | * double xs O smoothed solutions (n x 1) 310 | * double Qs O smoothed solutions covariance matrix (n x n) """ 311 | 312 | invQf = inv(Qf) 313 | invQb = inv(Qb) 314 | Qs = inv(invQf + invQb) 315 | xs = Qs @ (invQf @ xf + invQb @ xb) 316 | return xs, Qs 317 | 318 | def epoch2time(ep): 319 | """ calculate time from epoch """ 320 | doy = [1, 32, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335] 321 | time = gtime_t() 322 | year = int(ep[0]) 323 | mon = int(ep[1]) 324 | day = int(ep[2]) 325 | 326 | if year < 1970 or year > 2099 or mon < 1 or mon > 12: 327 | return time 328 | days = (year-1970)*365+(year-1969)//4+doy[mon-1]+day-2 329 | if year % 4 == 0 and mon >= 3: 330 | days += 1 331 | sec = int(ep[5]) 332 | time.time = days*86400+int(ep[3])*3600+int(ep[4])*60+sec 333 | time.sec = ep[5]-sec 334 | return time 335 | 336 | 337 | def gpst2utc(tgps, leaps_=-18): 338 | """ calculate UTC-time from gps-time """ 339 | tutc = timeadd(tgps, leaps_) 340 | return tutc 341 | 342 | def utc2gpst(tutc, leaps_=-18): 343 | """ calculate UTC-time from gps-time """ 344 | tgps = timeadd(tutc, -leaps_) 345 | return tgps 346 | 347 | 348 | def timeadd(t: gtime_t, sec: float): 349 | """ return time added with sec """ 350 | tr = copy(t) 351 | tr.sec += sec 352 | tt = floor(tr.sec) 353 | tr.time += int(tt) 354 | tr.sec -= tt 355 | return tr 356 | 357 | 358 | def timediff(t1: gtime_t, t2: gtime_t): 359 | """ return time difference """ 360 | dt = t1.time - t2.time 361 | dt += (t1.sec - t2.sec) 362 | return dt 363 | 364 | 365 | def gpst2time(week, tow): 366 | """ convert to time from gps-time """ 367 | t = epoch2time(gpst0) 368 | if tow < -1e9 or tow > 1e9: 369 | tow = 0.0 370 | t.time += 86400*7*week+int(tow) 371 | t.sec = tow-int(tow) 372 | return t 373 | 374 | 375 | def time2gpst(t: gtime_t): 376 | """ convert to gps-time from time """ 377 | t0 = epoch2time(gpst0) 378 | sec = t.time-t0.time 379 | week = int(sec/(86400*7)) 380 | tow = sec-week*86400*7+t.sec 381 | return week, tow 382 | 383 | 384 | def time2epoch(t): 385 | """ convert time to epoch """ 386 | mday = [31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, 31, 28, 31, 30, 31, 387 | 30, 31, 31, 30, 31, 30, 31, 31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 388 | 30, 31, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31] 389 | 390 | days = int(t.time/86400) 391 | sec = int(t.time-days*86400) 392 | day = days % 1461 393 | for mon in range(48): 394 | if day >= mday[mon]: 395 | day -= mday[mon] 396 | else: 397 | break 398 | ep = [0, 0, 0, 0, 0, 0] 399 | ep[0] = 1970+days//1461*4+mon//12 400 | ep[1] = mon % 12+1 401 | ep[2] = day+1 402 | ep[3] = sec//3600 403 | ep[4] = sec % 3600//60 404 | ep[5] = sec % 60+t.sec 405 | return ep 406 | 407 | 408 | def time2doy(t): 409 | """ convert time to epoch """ 410 | ep = time2epoch(t) 411 | ep[1] = ep[2] = 1.0 412 | ep[3] = ep[4] = ep[5] = 0.0 413 | return timediff(t, epoch2time(ep))/86400+1 414 | 415 | 416 | def prn2sat(sys, prn): 417 | """ convert sys+prn to sat """ 418 | if sys == uGNSS.GPS: 419 | sat = prn 420 | elif sys == uGNSS.GLO: 421 | sat = prn+uGNSS.GPSMAX 422 | elif sys == uGNSS.GAL: 423 | sat = prn+uGNSS.GPSMAX+uGNSS.GLOMAX 424 | elif sys == uGNSS.BDS: 425 | sat = prn+uGNSS.GPSMAX+uGNSS.GLOMAX+uGNSS.GALMAX 426 | elif sys == uGNSS.QZS: 427 | sat = prn-192+uGNSS.GPSMAX+uGNSS.GLOMAX+uGNSS.GALMAX+uGNSS.BDSMAX 428 | else: 429 | sat = 0 430 | return sat 431 | 432 | 433 | def sat2prn(sat): 434 | """ convert sat to sys+prn """ 435 | if sat > uGNSS.GPSMAX+uGNSS.GLOMAX+uGNSS.GALMAX+uGNSS.BDSMAX: 436 | prn = sat-(uGNSS.GPSMAX+uGNSS.GLOMAX+uGNSS.GALMAX+uGNSS.BDSMAX)+192 437 | sys = uGNSS.QZS 438 | elif sat > uGNSS.GPSMAX+uGNSS.GLOMAX+uGNSS.GALMAX: 439 | prn = sat-(uGNSS.GPSMAX+uGNSS.GLOMAX+uGNSS.GALMAX) 440 | sys = uGNSS.BDS 441 | elif sat > uGNSS.GPSMAX+uGNSS.GLOMAX: 442 | prn = sat-(uGNSS.GPSMAX+uGNSS.GLOMAX) 443 | sys = uGNSS.GAL 444 | elif sat > uGNSS.GPSMAX: 445 | prn = sat-uGNSS.GPSMAX 446 | sys = uGNSS.GLO 447 | else: 448 | prn = sat 449 | sys = uGNSS.GPS 450 | return (sys, prn) 451 | 452 | 453 | def sat2id(sat): 454 | """ convert satellite number to id """ 455 | sys, prn = sat2prn(sat) 456 | gnss_tbl = {uGNSS.GPS: 'G', uGNSS.GAL: 'E', uGNSS.BDS: 'C', 457 | uGNSS.QZS: 'J', uGNSS.GLO: 'R'} 458 | if sys == uGNSS.QZS: 459 | prn -= 192 460 | elif sys == uGNSS.SBS: 461 | prn -= 100 462 | return '%s%02d' % (gnss_tbl[sys], prn) 463 | 464 | 465 | def id2sat(id_): 466 | """ convert id to satellite number """ 467 | # gnss_tbl={'G':uGNSS.GPS,'S':uGNSS.SBS,'E':uGNSS.GAL,'C':uGNSS.BDS, 468 | # 'I':uGNSS.IRN,'J':uGNSS.QZS,'R':uGNSS.GLO} 469 | gnss_tbl = {'G': uGNSS.GPS, 'E': uGNSS.GAL, 'C': uGNSS.BDS, 470 | 'J': uGNSS.QZS, 'R': uGNSS.GLO} 471 | if id_[0] not in gnss_tbl: 472 | return -1 473 | sys = gnss_tbl[id_[0]] 474 | prn = int(id_[1:3]) 475 | if sys == uGNSS.QZS: 476 | prn += 192 477 | elif sys == uGNSS.SBS: 478 | prn += 100 479 | sat = prn2sat(sys, prn) 480 | return sat 481 | 482 | def sat2freq(sat, frq, nav): 483 | sys = nav.sysprn[sat][0] 484 | j = nav.obs_idx[frq][sys] 485 | freq = nav.freq[j] 486 | if sys == uGNSS.GLO: 487 | # adjust freq for individual satellite 488 | freq += nav.glofrq[sat - uGNSS.GPSMAX - 1] * nav.dfreq_glo[frq] 489 | return freq 490 | 491 | def vnorm(r): 492 | """ calculate norm of a vector """ 493 | return r / norm(r) 494 | 495 | def satexclude(sat, var, svh, nav): 496 | """ test excluded satellite 497 | * test excluded satellite 498 | * args : int sat I satellite number 499 | * double var I variance of ephemeris (m^2) 500 | * return : status (1:excluded,0:not excluded) 501 | *-----------------------------------------------------------------""" 502 | 503 | if sat in nav.excsats: 504 | return 1 505 | if svh: 506 | trace(3, 'unhealthy satellite: sat=%d svh=%x\n' % (sat, svh)) 507 | return 1 508 | if var > MAX_VAR_EPH: 509 | trace(3, 'invalid ura satellite: sat=%3d ura=%.2f\n' % (sat, np.sqrt(var))) 510 | return 1 511 | return 0 512 | 513 | def geodist(rs, rr): 514 | """ geometric distance ---------------------------------------------------------- 515 | * compute geometric distance and receiver-to-satellite unit vector 516 | * args : double *rs I satellite position (ecef at transmission) (m) 517 | * double *rr I receiver position (ecef at reception) (m) 518 | * double *e O line-of-sight vector (ecef) 519 | * return : geometric distance (m) (0>:error/no satellite position) 520 | * notes : distance includes sagnac effect correction """ 521 | e = rs - rr 522 | r = norm(e) 523 | e /= r 524 | r += rCST.OMGE * (rs[0] * rr[1] -rs[1] * rr[0]) / rCST.CLIGHT 525 | return r, e 526 | 527 | def dops_h(H): 528 | """ calculate DOP from H """ 529 | Qinv = inv(np.dot(H.T, H)) 530 | dop = np.diag(Qinv) 531 | hdop = dop[0]+dop[1] # TBD 532 | vdop = dop[2] # TBD 533 | pdop = hdop+vdop 534 | gdop = pdop+dop[3] 535 | dop = np.array([gdop, pdop, hdop, vdop]) 536 | return dop 537 | 538 | 539 | def dops(az, el, elmin=0): 540 | """ calculate DOP from az/el """ 541 | nm = az.shape[0] 542 | H = np.zeros((nm, 4)) 543 | n = 0 544 | for i in range(nm): 545 | if el[i] < elmin: 546 | continue 547 | cel = cos(el[i]) 548 | sel = sin(el[i]) 549 | H[n, 0] = cel*sin(az[i]) 550 | H[n, 1] = cel*cos(az[i]) 551 | H[n, 2] = sel 552 | H[n, 3] = 1 553 | n += 1 554 | if n < 4: 555 | return None 556 | Qinv = inv(np.dot(H.T, H)) 557 | dop = np.diag(Qinv) 558 | hdop = dop[0]+dop[1] # TBD 559 | vdop = dop[2] # TBD 560 | pdop = hdop+vdop 561 | gdop = pdop+dop[3] 562 | dop = np.array([gdop, pdop, hdop, vdop]) 563 | return dop 564 | 565 | 566 | def xyz2enu(pos): 567 | """ return ECEF to ENU conversion matrix from LLH 568 | pos is LLH 569 | """ 570 | sp = sin(pos[0]) 571 | cp = cos(pos[0]) 572 | sl = sin(pos[1]) 573 | cl = cos(pos[1]) 574 | E = np.array([[-sl, cl, 0], 575 | [-sp*cl, -sp*sl, cp], 576 | [cp*cl, cp*sl, sp]]) 577 | return E 578 | 579 | 580 | def ecef2pos(r): 581 | """ ECEF to LLH position conversion """ 582 | pos = np.zeros(3) 583 | e2 = rCST.FE_WGS84*(2-rCST.FE_WGS84) 584 | r2 = r[0]**2+r[1]**2 585 | v = rCST.RE_WGS84 586 | z = r[2] 587 | zk = 0 588 | while abs(z - zk) >= 1e-4: 589 | zk = z 590 | sinp = z / np.sqrt(r2+z**2) 591 | v = rCST.RE_WGS84 / np.sqrt(1 - e2 * sinp**2) 592 | z = r[2] + v * e2 * sinp 593 | pos[0] = np.arctan(z / np.sqrt(r2)) if r2 > 1e-12 else np.pi / 2 * np.sign(r[2]) 594 | pos[1] = np.arctan2(r[1], r[0]) if r2 > 1e-12 else 0 595 | pos[2] = np.sqrt(r2 + z**2) - v 596 | return pos 597 | 598 | 599 | def pos2ecef(pos, isdeg: bool = False): 600 | """ LLH (rad/deg) to ECEF position conversion """ 601 | if isdeg: 602 | s_p = sin(pos[0]*np.pi/180.0) 603 | c_p = cos(pos[0]*np.pi/180.0) 604 | s_l = sin(pos[1]*np.pi/180.0) 605 | c_l = cos(pos[1]*np.pi/180.0) 606 | else: 607 | s_p = sin(pos[0]) 608 | c_p = cos(pos[0]) 609 | s_l = sin(pos[1]) 610 | c_l = cos(pos[1]) 611 | e2 = rCST.FE_WGS84 * (2.0 - rCST.FE_WGS84) 612 | v = rCST.RE_WGS84 / sqrt(1.0 - e2 * s_p**2) 613 | r = np.array([(v + pos[2]) * c_p*c_l, 614 | (v + pos[2]) * c_p*s_l, 615 | (v * (1.0 - e2) + pos[2]) * s_p]) 616 | return r 617 | 618 | 619 | def ecef2enu(pos, r): 620 | """ relative ECEF to ENU conversion """ 621 | E = xyz2enu(pos) 622 | e = E @ r 623 | return e 624 | 625 | def enu2ecef(pos, e): 626 | """ relative ECEF to ENU conversion """ 627 | E = xyz2enu(pos) 628 | r = E.T @ e 629 | return r 630 | 631 | def covenu(llh, P): 632 | """transform ecef covariance to local tangental coordinate -------------------------- 633 | * transform ecef covariance to local tangental coordinate 634 | * args : llh I geodetic position {lat,lon} (rad) 635 | * P I covariance in ecef coordinate 636 | * Q O covariance in local tangental coordinate """ 637 | E = xyz2enu(llh) 638 | return E @ P @ E.T 639 | 640 | def covecef(llh, Q): 641 | """transform local enu coordinate covariance to xyz-ecef -------------------------- 642 | * transform ecef covariance to local tangental coordinate 643 | * args : llh I geodetic position {lat,lon} (rad) 644 | * Q I covariance in local tangental coordinate 645 | * P O covariance in ecef coordinate """ 646 | E = xyz2enu(llh) 647 | return E.T @ Q @ E 648 | 649 | def deg2dms(deg): 650 | """ convert from deg to dms """ 651 | if deg < 0.0: 652 | sign = -1 653 | else: 654 | sign = 1 655 | a = fabs(deg) 656 | dms = np.zeros(3) 657 | dms[0] = floor(a) 658 | a = (a-dms[0])*60.0 659 | dms[1] = floor(a) 660 | a = (a-dms[1])*60.0 661 | dms[2] = a 662 | dms[0] *= sign 663 | return dms 664 | 665 | 666 | def satazel(pos, e): 667 | """ calculate az/el from LOS vector in ECEF (e) """ 668 | if pos[2] > -rCST.RE_WGS84 + 1: 669 | enu = ecef2enu(pos, e) 670 | az = atan2(enu[0], enu[1]) if np.dot(enu, enu) > 1e-12 else 0 671 | az = az if az > 0 else az + 2 * np.pi 672 | el = asin(enu[2]) 673 | return [az, el] 674 | else: 675 | return [0, np.pi / 2] 676 | 677 | 678 | def ionmodel(t, pos, az, el, ion=None): 679 | """ klobuchar model of ionosphere delay estimation """ 680 | psi = 0.0137 / (el / np.pi + 0.11) - 0.022 681 | phi = pos[0] / np.pi + psi * cos(az) 682 | phi = np.max((-0.416, np.min((0.416, phi)))) 683 | lam = pos[1]/np.pi + psi * sin(az) / cos(phi * np.pi) 684 | phi += 0.064 * cos((lam - 1.617) * np.pi) 685 | _, tow = time2gpst(t) 686 | tt = 43200.0 * lam + tow # local time 687 | tt -= np.floor(tt / 86400) * 86400 688 | f = 1.0 + 16.0 * np.power(0.53 - el/np.pi, 3.0) # slant factor 689 | 690 | h = [1, phi, phi**2, phi**3] 691 | amp = np.dot(h, ion[0, :]) 692 | per = np.dot(h, ion[1, :]) 693 | amp = max(amp, 0) 694 | per = max(per, 72000.0) 695 | x = 2.0 * np.pi * (tt - 50400.0) / per 696 | if np.abs(x) < 1.57: 697 | v = 5e-9 + amp * (1.0 + x * x * (-0.5 + x * x / 24.0)) 698 | else: 699 | v = 5e-9 700 | diono = rCST.CLIGHT * f * v 701 | return diono 702 | 703 | 704 | def interpc(coef, lat): 705 | """ linear interpolation (lat step=15) """ 706 | i = int(lat / 15.0) 707 | if i < 1: 708 | return coef[:, 0] 709 | if i > 4: 710 | return coef[:, 4] 711 | d = lat / 15.0 - i 712 | return coef[:, i-1] * (1.0 - d) + coef[:, i] * d 713 | 714 | 715 | def antmodel(nav, el, nf, rtype): 716 | """ antenna pco/pcv """ 717 | sE = sin(el) 718 | za = 90-np.rad2deg(el) 719 | za_t = np.arange(0, 90.1, 5) 720 | dant = np.zeros(nf) 721 | if rtype == 1: # for rover 722 | pcv_t = nav.ant_pcv 723 | pco_t = nav.ant_pco 724 | else: # for base 725 | pcv_t = nav.ant_pcv_b 726 | pco_t = nav.ant_pco_b 727 | for f in range(nf): 728 | pcv = np.interp(za, za_t, pcv_t[f]) 729 | pco = -pco_t[f] * sE 730 | dant[f] = (pco+pcv) * 1e-3 731 | return dant 732 | 733 | 734 | def mapf(el, a, b, c): 735 | """ simple tropospheric mapping function """ 736 | sinel = np.sin(el) 737 | return (1.0 + a / (1.0 + b / (1.0 + c))) / (sinel + (a / (sinel + b / (sinel + c)))) 738 | 739 | 740 | def tropmapf(t, pos, el): 741 | """ tropospheric mapping function Neil (NMF) """ 742 | if pos[2] < -1e3 or pos[2] > 20e3 or el <= 0.0: 743 | return 0.0, 0.0 744 | 745 | aht = nmf_aht 746 | lat = np.rad2deg(pos[0]) 747 | # year from doy 28, add half a year for southern latitudes 748 | y = (time2doy(t) - 28.0) / 365.25 749 | y += 0.5 if lat < 0 else 0 750 | cosy = np.cos(2.0 * np.pi * y) 751 | c = interpc(nmf_coef, np.abs(lat)) 752 | ah = c[0:3] - c[3:6] * cosy 753 | aw = c[6:9] 754 | # ellipsoidal height is used instead of height above sea level 755 | dm = (1.0 / np.sin(el) - mapf(el, aht[0], aht[1], aht[2])) * pos[2] * 1e-3 756 | mapfh = mapf(el, ah[0], ah[1], ah[2]) + dm 757 | mapfw = mapf(el, aw[0], aw[1], aw[2]) 758 | 759 | return mapfh, mapfw 760 | 761 | 762 | def tropmodel(t, pos, el, humi): 763 | """ saastamonien tropospheric delay model """ 764 | temp0 = 15 # temparature at sea level 765 | if pos[2] < -100 or pos[2] > 1e4 or el <= 0: 766 | return 0, 0, 0 767 | hgt = max(pos[2], 0) 768 | # standard atmosphere 769 | pres = 1013.25 * np.power(1 - 2.2557e-5 * hgt, 5.2568) 770 | temp = temp0 - 6.5e-3 * hgt + 273.16 771 | e = 6.108 * humi * np.exp((17.15 * temp - 4684.0) / (temp - 38.45)) 772 | # saastamoinen model 773 | z = np.pi / 2.0 - el 774 | trop_hs = 0.0022768 * pres / (1.0 - 0.00266 * np.cos(2 * pos[0]) - 775 | 0.00028e-3 * hgt) / np.cos(z) 776 | trop_wet = 0.002277 * (1255.0 / temp+0.05) * e / np.cos(z) 777 | return trop_hs, trop_wet, z 778 | 779 | def trace(level, msg): 780 | if level <= trace_level: 781 | sys.stderr.write('%d %s' % (level, msg)) 782 | 783 | def tracemat(level, msg, mat, fmt='.6f'): 784 | if level > trace_level: 785 | return 786 | fmt = '{:' + fmt + '}' 787 | if len(mat.shape) == 1 or mat.shape[1] == 1: 788 | trace(level, msg) 789 | sys.stderr.write(' '.join(map(fmt.format, mat))) 790 | sys.stderr.write('\n') 791 | else: 792 | trace(level, msg + '\n') 793 | for row in mat: 794 | sys.stderr.write(' '.join(map(fmt.format, row))) 795 | sys.stderr.write('\n') 796 | 797 | def tracelevel(level): 798 | global trace_level 799 | trace_level = level 800 | -------------------------------------------------------------------------------- /src/rtkpos.py: -------------------------------------------------------------------------------- 1 | """ 2 | module for PPK positioning 3 | 4 | Copyright (c) 2021 Rui Hirokawa (from CSSRLIB) 5 | Copyright (c) 2022 Tim Everett 6 | """ 7 | 8 | import numpy as np 9 | from numpy.linalg import inv, norm 10 | from sys import stdout 11 | from copy import copy, deepcopy 12 | import rtkcmn as gn 13 | from rtkcmn import rCST, DTTOL, sat2prn, sat2freq, timediff, xyz2enu 14 | import rinex as rn 15 | from pntpos import pntpos 16 | from ephemeris import satposs 17 | from mlambda import mlambda 18 | from rtkcmn import trace, tracemat, uGNSS 19 | import __ppk_config as cfg 20 | 21 | 22 | MAX_VAR_EPH = 300**2 23 | 24 | def outsolstat(nav, sol, fp_stat): 25 | week, tow = gn.time2gpst(sol.t) 26 | 27 | # save position to file 28 | fp_stat.write('$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n' % 29 | (week, tow, sol.stat, sol.rr[0], sol.rr[1], sol.rr[2], 0.0, 0.0, 0.0)) 30 | 31 | # save velocity to file 32 | pos = gn.ecef2pos(sol.rr) 33 | vel = gn.ecef2enu(pos, nav.x[3:6]) 34 | fp_stat.write('$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n' % 35 | (week, tow, sol.stat, vel[0], vel[1], vel[2], 0, 0, 0, 0, 0, 0, 0, 0, 0)) 36 | 37 | # save residuals to file 38 | for i in range(uGNSS.MAXSAT): 39 | if nav.vsat[i,0] == 0: 40 | continue 41 | id = gn.sat2id(i+1) 42 | for f in range(nav.nf): 43 | k = IB(i+1, f, nav.na) 44 | fp_stat.write('$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d,%.2f,%.6f\n' % 45 | (week, tow, id 46 | , f+1, np.rad2deg(nav.azel[i,0]), np.rad2deg(nav.azel[i,1]), 47 | nav.resp[i,f], nav.resc[i,f], nav.vsat[i,f], nav.SNR_rover[i,f], 48 | nav.fix[i,f], nav.slip[i,f], nav.lock[i,f], nav.outc[i,f], 49 | 0, nav.rejc[i,f], nav.x[k], 50 | nav.P[k,k])); 51 | 52 | def rtkinit(cfg): 53 | nav = gn.Nav(cfg) 54 | """ initalize RTK-GNSS parameters from config file """ 55 | nav.gnss_t = cfg.gnss_t 56 | nav.pmode = cfg.pmode 57 | nav.filtertype = cfg.filtertype 58 | # add rover vel and accel states for kinematic solution 59 | nav.na = nav.nq = 3 if nav.pmode == 'static' else 9 60 | nav.nx = nav.na + uGNSS.MAXSAT * nav.nf 61 | nav.x = np.zeros(nav.nx) 62 | nav.P = np.zeros((nav.nx, nav.nx)) 63 | nav.xa = np.zeros(nav.na) 64 | nav.Pa = np.zeros((nav.na, nav.na)) 65 | nav.azel = np.zeros((uGNSS.MAXSAT, 2)) 66 | nav.gf = np.zeros(uGNSS.MAXSAT) 67 | nav.ph = np.zeros((2, uGNSS.MAXSAT, nav.nf)) 68 | nav.pt = np.empty((2, uGNSS.MAXSAT, nav.nf), dtype=object) 69 | nav.SNR_rover = np.zeros((uGNSS.MAXSAT, nav.nf)) 70 | nav.SNR_base = np.zeros((uGNSS.MAXSAT, nav.nf)) 71 | nav.nfix = nav.neb = nav.tt = 0 72 | 73 | nav.rb = cfg.rb 74 | 75 | # parameter for RTK/PPK 76 | nav.use_sing_pos = cfg.use_sing_pos 77 | nav.cnr_min = cfg.cnr_min 78 | nav.maxout = cfg.maxout # maximum outage [epoch] 79 | nav.elmin = np.deg2rad(cfg.elmin) 80 | nav.nf = cfg.nf 81 | nav.excsats = cfg.excsats 82 | nav.freq = cfg.freq 83 | nav.dfreq_glo = cfg.dfreq_glo 84 | nav.interp_base = cfg.interp_base 85 | nav.gnss_t = cfg.gnss_t 86 | nav.maxinno = [cfg.maxinno, cfg.maxcode] 87 | nav.thresdop = cfg.thresdop 88 | nav.thresslip = cfg.thresslip 89 | nav.maxage = cfg.maxage 90 | nav.accelh = cfg.accelh 91 | nav.accelv = cfg.accelv 92 | nav.prnbias = cfg.prnbias 93 | 94 | # ambiguity resolution 95 | nav.armode = cfg.armode 96 | nav.glo_hwbias = cfg.glo_hwbias 97 | nav.thresar = cfg.thresar 98 | nav.thresar1 = cfg.thresar1 99 | nav.var_holdamb = cfg.var_holdamb 100 | nav.elmaskar = np.deg2rad(cfg.elmaskar) 101 | nav.minfix = cfg.minfix 102 | nav.minfixsats = cfg.minfixsats 103 | nav.minholdsats = cfg.minholdsats 104 | nav.mindropsats = cfg.mindropsats 105 | nav.excsat_ix = 0 106 | nav.nfix = 0 107 | nav.ratio = 0 108 | 109 | # statistics 110 | nav.efact = cfg.efact 111 | nav.eratio = cfg.eratio 112 | nav.err = np.array(cfg.err) 113 | nav.snrmax = cfg.snrmax 114 | nav.sig_p0 = cfg.sig_p0 115 | nav.sig_v0 = cfg.sig_v0 116 | nav.sig_n0 = cfg.sig_n0 117 | 118 | # solution parameters 119 | nav.sol = [] 120 | 121 | dP = np.diag(nav.P) 122 | dP.flags['WRITEABLE'] = True 123 | dP[0:3] = nav.sig_p0**2 124 | if nav.pmode == 'kinematic': 125 | dP[3:9] = nav.sig_v0**2 126 | 127 | # obs index 128 | ix0, ix1 = cfg.freq_ix0, cfg.freq_ix1 129 | freq0 = {k: cfg.freq[ix0[k]] for k in ix0.keys()} 130 | freq1 = {k: cfg.freq[ix1[k]] for k in ix1.keys()} 131 | nav.obs_idx = [ix0, ix1] 132 | nav.obs_freq = [freq0, freq1] 133 | 134 | # sat index 135 | nav.sysprn = {i: gn.sat2prn(i) for i in range(1, uGNSS.MAXSAT+1)} 136 | 137 | return nav 138 | 139 | def zdres_sat(nav, obs, r, rtype, dant, ix): 140 | _c = rCST.CLIGHT 141 | nf = nav.nf 142 | y = np.zeros(nf * 2) 143 | for f in range(nf): 144 | freq = sat2freq(obs.sat[ix], f, nav) 145 | if obs.S[ix,f] < nav.cnr_min[f]: 146 | continue 147 | # residuals = observable - estimated range (phase and code) 148 | y[f] = obs.L[ix,f] * _c / freq - r - dant[f] if obs.L[ix,f] else 0 149 | y[f+nf] = obs.P[ix,f] - r - dant[f] if obs.P[ix,f] else 0 150 | #trace(4, 'zdres_sat: %d: L=%.6f P=%.6f r=%.6f f=%.0f\n' % 151 | # (obs.sat[ix],obs.L[ix,f], obs.P[ix,f],r,freq)) 152 | return y 153 | 154 | def zdres(nav, obs, rs, dts, svh, var, rr, rtype): 155 | """ undifferenced phase/code residuals ---------------------------------------- 156 | calculate zero diff residuals [observed pseudorange - range] 157 | output is in y[0:nu-1], only shared input with base is nav 158 | args: I obs = sat observations 159 | I n = # of sats 160 | I rs = sat position {x,y,z} (m) 161 | I dts = sat clock {bias,drift} (s|s/s) 162 | I var = variance of ephemeris 163 | I svh = sat health flags 164 | I nav = sat nav data 165 | I rr = rcvr pos (x,y,z) 166 | I rtype: 0=base,1=rover 167 | O y[] = zero diff residuals {phase,code} (m) 168 | O e = line of sight unit vectors to sats 169 | O azel = [az, el] to sats """ 170 | if obs == []: 171 | return [], [], [] 172 | _c = rCST.CLIGHT 173 | nf = nav.nf 174 | n = len(obs.P) 175 | y = np.zeros((n, nf * 2)) 176 | azel = np.zeros((n,2)) 177 | e = np.zeros((n, 3)) 178 | rr_ = rr.copy() 179 | trace(3, 'zdres: n=%d rr=%.2f %.2f %.2f\n' % (n, rr[0], rr[1], rr[2])) 180 | pos = gn.ecef2pos(rr_) 181 | # loop through satellites 182 | ix = np.argsort(obs.sat) 183 | for i in ix: 184 | # excluded satellite? 185 | if gn.satexclude(obs.sat[i], var[i], svh[i], nav): 186 | continue 187 | # compute geometric-range and azimuth/elevation angle 188 | r, e[i,:] = gn.geodist(rs[i,0:3], rr_) 189 | azel[i] = gn.satazel(pos, e[i,:]) 190 | if azel[i,1] < nav.elmin: 191 | continue 192 | # adjust range for satellite clock-bias 193 | r += -_c * dts[i] 194 | # adjust range for troposphere delay model (hydrostatic) 195 | trophs, tropw, _ = gn.tropmodel(obs.t, pos, np.deg2rad(90.0), 0.0) 196 | zhd = trophs + tropw 197 | mapfh, _ = gn.tropmapf(obs.t, pos, azel[i,1]) 198 | r += mapfh * zhd 199 | # calc receiver antenna phase center correction 200 | dant = gn.antmodel(nav, azel[i,1], nav.nf, rtype) 201 | # calc undifferenced phase/code residual for satellite 202 | y[i] = zdres_sat(nav, obs, r, rtype, dant, i) 203 | if obs.L[i,0] == 0 or rtype == 1: continue 204 | trace(3, 'sat=%2d rs=%13.3f %13.3f %13.3f dts=%13.10f az=%6.1f el=%5.1f\n' % 205 | (obs.sat[i], rs[i,0], rs[i,1], rs[i,2], dts[i], 206 | np.rad2deg(azel[i,0]), np.rad2deg(azel[i,1]))) 207 | trace(4,'sat=%d r=%.6f c*dts=%.6f zhd=%.6f map=%.6f\n' % 208 | (obs.sat[i], r,_c*dts[i],zhd,mapfh)) 209 | 210 | 211 | tracemat(3, 'y=', y[ix,:].T, '13.3f') 212 | return y, e, azel 213 | 214 | 215 | def ddcov(nb, n, Ri, Rj, nv): 216 | """ double-differenced measurement error covariance --------------------------- 217 | * 218 | * nb[n]: # of sat pairs in group 219 | * n: # of groups (2 for each system, phase and code) 220 | * Ri[nv]: variances of first sats in double diff pairs 221 | * Rj[nv]: variances of 2nd sats in double diff pairs 222 | * nv: total # of sat pairs 223 | * R[nv][nv]: double diff measurement err covariance matrix """ 224 | R = np.zeros((nv, nv)) 225 | k = 0 226 | for b in range(n): 227 | block = R[k:nb[b]+k, k:nb[b]+k] # define subarray 228 | block += Ri[k:nb[b]+k] 229 | block[range(nb[b]), range(nb[b])] += Rj[k:nb[b]+k] 230 | k += nb[b] 231 | return R 232 | 233 | 234 | def sysidx(satlist, sys_ref): 235 | """ return index of satellites with sys=sys_ref """ 236 | idx = [] 237 | for k, sat in enumerate(satlist): 238 | sys, _ = sat2prn(sat) 239 | if sys == sys_ref: 240 | idx.append(k) 241 | return idx 242 | 243 | 244 | def IB(s, f, na=3): 245 | """ return index of phase ambguity """ 246 | return na + uGNSS.MAXSAT * f + s - 1 247 | 248 | 249 | def varerr(nav, sys, el, f, dt, rcvstd, snr_rover, snr_base): 250 | """ variation of measurement """ 251 | code = 1 * (f >= nav.nf) # 0 = phase, 1 = code 252 | freq = f % nav.nf 253 | sinel = np.sin(el) 254 | if code: # increase variance for pseudoranges 255 | fact = nav.eratio[freq] 256 | else: # adjust phase variance between freqs 257 | fact = nav.eratio[freq] / nav.eratio[0] 258 | # adjust variances for constellation 259 | fact *= nav.efact[sys] 260 | # adjust variance for config parameters 261 | a, b = fact * nav.err[1:3] 262 | c = fact * 0 # nav.err[3]*bl/1E4 # TODO: add baseline term 263 | d = rCST.CLIGHT * nav.err[6] * dt # clock term 264 | var = 2.0 * (a**2 + (b / sinel)**2 + c**2) + d**2 265 | 266 | if nav.err[4] > 0: # add SNR term 267 | e = fact * nav.err[4] 268 | var += e**2 * (10**(0.1 * max(nav.snrmax - snr_rover, 0)) + 269 | 10**(0.1 * max(nav.snrmax - snr_base, 0))) 270 | 271 | if nav.err[5] > 0: # add receiver error term 272 | var += (nav.err[5] * rcvstd)**2 273 | return var 274 | 275 | 276 | def ddres(nav, x, P, yr, er, yu, eu, sat, el, dt, obsr, save_res=False): 277 | """ /* double-differenced residuals and partial derivatives ----------------------------------- 278 | I nav = sat nav data 279 | I dt = time diff between base and rover observations 280 | I x = rover pos & vel and sat phase biases (float solution) 281 | I P = error covariance matrix of float states 282 | I sat = list of common sats 283 | I y = zero diff residuals (code and phase, base and rover) 284 | I e = line of sight unit vectors to sats 285 | I el = el to sats 286 | O v = double diff innovations (measurement-model) (phase and code) 287 | O H = linearized translation from innovations to states (az/el to sats) 288 | O R = measurement error covariances """ 289 | _c = rCST.CLIGHT 290 | nf = nav.nf 291 | ns = len(el) 292 | ny = ns * nf * 2 # phase and code 293 | nb = np.zeros(2 * len(nav.gnss_t) * nf, dtype=int) 294 | Ri = np.zeros(ny) 295 | Rj = np.zeros(ny) 296 | H = np.zeros((nav.nx, ny)) 297 | P_init = nav.sig_n0**2 # value used to initialize P states 298 | trace(3,"ddres : dt=%.4f ns=%d\n" % (dt, ns)) 299 | 300 | if save_res: 301 | # zero out residual phase and code biases for all satellites 302 | nav.resp.fill(0) 303 | nav.resc.fill(0) 304 | 305 | nv = b = 0 306 | v = np.zeros(ny) 307 | # step through sat systems 308 | for sys in nav.gnss_t: 309 | # step through phases/codes 310 | for f in range(0, nf*2): 311 | frq = f % nf 312 | code = 1 * (f >= nf) 313 | idx = sysidx(sat, sys) # find sats in sys 314 | # remove sats with missing base or rover residuals 315 | nozero = np.where((yr[:,f] != 0) & (yu[:,f] != 0))[0] 316 | idx = np.intersect1d(idx, nozero) 317 | if len(idx) == 0: 318 | continue # no common sats 319 | # find sat with max el and not just reset for reference 320 | i_el = idx[np.argsort(el[idx])] 321 | for i in i_el[::-1]: 322 | ii = IB(sat[i], frq, nav.na) 323 | # check if sat just reset 324 | if P[ii,ii] <= nav.sig_n0**2: 325 | break 326 | else: # check if none without reset 327 | i = i_el[0] # use highest sat if none without reset 328 | # calculate double differences of residuals (code/phase) for each sat 329 | freqi = sat2freq(sat[i], frq, nav) 330 | lami = _c / freqi 331 | for j in idx: # loop through sats 332 | if i == j: continue # skip ref sat 333 | # double-differenced measurements from 2 receivers and 2 sats in meters 334 | v[nv] = (yu[i,f] - yr[i,f]) - (yu[j,f] - yr[j,f]) 335 | # partial derivatives by rover position, combine unit vectors from two sats 336 | H[0:3, nv] = -eu[i,:] + er[j,:] 337 | 338 | jj = IB(sat[j], frq, nav.na) 339 | if not code: # carrier phase 340 | # adjust phase residual by double-differenced phase-bias term 341 | freqj = sat2freq(sat[j], frq, nav) 342 | lamj = _c / freqj 343 | v[nv] -= lami * x[ii] - lamj * x[jj] 344 | H[ii, nv], H[jj, nv] = lami, -lamj 345 | 346 | # adjust double-difference for glonass hw bias 347 | if sys == uGNSS.GLO and nav.glo_hwbias != 0: 348 | df = (freqi - freqj) / nav.dfreq_glo[frq] 349 | v[nv] -= df * nav.glo_hwbias 350 | 351 | # save residuals 352 | if save_res: 353 | if code: 354 | nav.resp[sat[j]-1,frq] = v[nv] 355 | else: 356 | nav.resc[sat[j]-1,frq] = v[nv] 357 | 358 | # use larger outlier thresh if just initialized phase 359 | thresadj = 10 if (P[ii,ii] >= P_init or P[jj,jj] >= P_init) else 1 360 | # if residual too large, flag as outlier 361 | if abs(v[nv]) > nav.maxinno[code] * thresadj: 362 | nav.vsat[sat[j]-1,frq] = 0 363 | nav.rejc[sat[j]-1,frq] += 1 364 | trace(3,"outlier rejected: (sat=%3d-%3d %s%d v=%13.3f x=%13.3f %13.3f P=%.6f %.6f)\n" 365 | % (sat[i], sat[j], 'LP'[code],frq+1, v[nv], x[ii], x[jj], P[ii,ii],P[jj,jj])) 366 | H[ii, nv], H[jj, nv] = 0, 0 367 | continue 368 | # single-differenced measurement error variances (m) 369 | si, sj = sat[i] - 1, sat[j] - 1 370 | Ri[nv] = varerr(nav, sys, el[i], f, dt, nav.rcvstd[si,f], 371 | nav.SNR_rover[si,frq], nav.SNR_base[si,frq]) 372 | Rj[nv] = varerr(nav, sys, el[j], f, dt, nav.rcvstd[sj,f], 373 | nav.SNR_rover[sj,frq], nav.SNR_base[sj,frq]) 374 | if not code: 375 | # increase variance if half cycle flags set 376 | if nav.slip[si,frq] & 2: Ri[nv]+=0.01 377 | if nav.slip[sj,frq] & 2: Rj[nv]+=0.01 378 | # set valid data flags 379 | nav.vsat[si,frq] = nav.vsat[sj,frq] = 1 380 | trace(3,"sat=%3d-%3d %s%d v=%13.3f R=%9.6f %9.6f lock=%2d x=%.3f P=%.3f\n" % 381 | (sat[i], sat[j], 'LP'[code], frq+1, v[nv], Ri[nv], Rj[nv], 382 | nav.lock[sat[j]-1,frq], x[jj], P[jj,jj])) 383 | nv += 1 384 | nb[b] += 1 385 | b += 1 386 | R = ddcov(nb, b, Ri[:nv], Rj[:nv], nv) 387 | 388 | return v[:nv], H[:,:nv], R 389 | 390 | 391 | def valpos(nav, v, R, thres=4.0): 392 | """ post-file residual test """ 393 | trace(3, 'valpos : nv=%d thres=%.1f\n' % (len(v), thres)) 394 | nv = len(v) 395 | fact = thres**2 396 | for i in range(nv): 397 | if v[i]**2 > fact * R[i, i]: 398 | trace(3, 'large residual (ix_sat=%d v=%.3f sig=%.3f)\n' % 399 | (i, v[i], np.sqrt(R[i, i]))) 400 | return True 401 | 402 | def intpres(time, nav, y0, y1, obs0, obs1): 403 | """ time-interpolation of residuals """ 404 | tt, ttb = timediff(time, obs1.t), timediff(time, obs0.t) 405 | if len(y0) == 0 or abs(ttb) > nav.maxage or abs(tt) < DTTOL: 406 | return y1, tt 407 | # find common sats 408 | _, ix0, ix1 = np.intersect1d(obs0.sat, obs1.sat, return_indices=True) 409 | for i in range(len(ix0)): 410 | for j in range(4): 411 | i0, i1 = ix0[i], ix1[i] 412 | if y1[i1,j] == 0: 413 | y1[i1,j] = y0[i0,j] 414 | elif y0[i0,j] != 0: 415 | y1[i1,j] = (ttb * y1[i1,j] - tt * y0[i0,j]) / (ttb - tt) 416 | dt = min(abs(tt), abs(ttb)) / np.sqrt(2) 417 | return y1, dt 418 | 419 | 420 | 421 | def ddidx(nav, sats): 422 | """ index for single to double-difference transformation matrix (D') """ 423 | nb, fix, ref = 0, [], [] 424 | ns = uGNSS.MAXSAT 425 | #na = nav.na 426 | ix = np.zeros((ns, 2), dtype=int) 427 | # clear fix flag for all sats (1=float, 2=fix) 428 | nav.fix[:,:] = 0 429 | # step through constellations 430 | for m in range(uGNSS.GNSSMAX): 431 | k = nav.na # state index for first sat 432 | # step through freqs 433 | for f in range(nav.nf): 434 | # look for first valid sat (i=state index, i-k=sat index) 435 | for i in range(k, k + ns): 436 | sati = i - k + 1 437 | # if sati not in sats: 438 | # xxx=1 439 | sys = nav.sysprn[sati][0] 440 | # skip if sat not active 441 | if nav.x[i] == 0.0 or sys != m or nav.vsat[sati-1,f] == 0: 442 | continue 443 | if nav.lock[sati-1,f] >= 0 and nav.slip[sati-1,f] & 2 == 0 and \ 444 | nav.azel[sati-1,1] >= nav.elmaskar: 445 | # set sat to use for fixing ambiguity if meets criteria 446 | nav.fix[sati-1,f] = 2 # fix 447 | break # break out of loop if find good sat 448 | else: # don't use this sat for fixing ambiguity 449 | nav.fix[sati-1,f] = 1 # float 450 | if nav.fix[sati-1,f] != 2: # no good sat found 451 | continue 452 | n = 0 # count of sat pairs for this freq/constellation 453 | # step through all sats (j=state index, j-k=sat index, i-k=first good sat) 454 | for j in range(k, k + ns): 455 | satj = j - k + 1 456 | sys = nav.sysprn[satj][0] 457 | if i == j or nav.x[j] == 0.0 or sys != m or nav.vsat[satj-1,f] <= 0: 458 | continue 459 | if nav.lock[satj-1,f] >= 0 and nav.slip[satj-1,f] & 2 == 0 and \ 460 | nav.azel[satj-1,1] >= nav.elmaskar: 461 | # set D coeffs to subtract sat j from sat i 462 | ix[nb, :] = [i,j] # state indices of ref bias and target bias 463 | ref.append(sati) 464 | fix.append(satj) 465 | nav.fix[satj-1,f] = 2 # fix 466 | nb += 1 # increment total count 467 | n += 1 # inc count in freq/constellation 468 | else: # don't use this sat for fixing ambiguity 469 | nav.fix[satj-1,f] = 1 # float 470 | if n == 0: # don't use ref sat if no sat pairs 471 | nav.fix[sati-1,f] = 1 472 | k += ns 473 | ix = np.resize(ix, (nb, 2)) 474 | if nb > 0: 475 | tracemat(3,'refSats= ', np.array(ref), '7d') 476 | tracemat(3,'fixSats= ', np.array(fix), '7d') 477 | return ix 478 | 479 | 480 | def restamb(nav, bias, nb): 481 | """ restore SD ambiguity """ 482 | trace(3,"restamb :\n") 483 | nv = 0 484 | xa = nav.x.copy() 485 | xa[0:nav.na] = nav.xa[0:nav.na] 486 | 487 | for m in range(uGNSS.GNSSMAX): 488 | for f in range(nav.nf): 489 | n = 0 490 | index = [] 491 | for i in range(uGNSS.MAXSAT): 492 | sys = nav.sysprn[i+1][0] 493 | if sys != m or (sys not in nav.gnss_t) or nav.fix[i, f] != 2: 494 | continue 495 | index.append(IB(i+1, f, nav.na)) 496 | n += 1 497 | if n < 2: 498 | continue 499 | xa[index[0]] = nav.x[index[0]] 500 | for i in range(1, n): 501 | xa[index[i]] = xa[index[0]] - bias[nv] 502 | nv += 1 503 | return xa 504 | 505 | 506 | def resamb_lambda(nav, sats): 507 | """ resolve integer ambiguity using LAMBDA method """ 508 | nx = nav.nx 509 | na = nav.na 510 | xa = np.zeros(na) 511 | ix = ddidx(nav, sats) 512 | nav.nb_ar = nb = len(ix) 513 | if nb <= nav.minfixsats - 1: # nb is sat pairs 514 | trace(3, 'resamb_lambda: not enough valid double-differences DD\n') 515 | return -1, -1 516 | 517 | # y=D*xc, Qb=D*Qc*D', Qab=Qac*D' 518 | y = nav.x[ix[:, 0]] - nav.x[ix[:, 1]] 519 | DP = nav.P[ix[:, 0], na:nx] - nav.P[ix[:, 1], na:nx] 520 | Qb = DP[:, ix[:, 0] - na] - DP[:, ix[:, 1] - na] 521 | Qab = nav.P[0:na, ix[:, 0]] - nav.P[0:na, ix[:, 1]] 522 | tracemat(3,'N(0)= ', y, '7.2f') 523 | tracemat(3, 'Qb*1000= ', 1000 * np.diag(Qb[0:nb]), '7.4f') 524 | 525 | # MLAMBDA ILS 526 | b, s = mlambda(y, Qb) 527 | tracemat(3,'N(1)= ', b[:,0], '7.2f') 528 | tracemat(3,'N(2)= ', b[:,1], '7.2f') 529 | nav.ratio = s[1] / s[0] 530 | if s[0] <= 0.0 or nav.ratio >= nav.thresar: 531 | trace(3,'resamb : validation ok (nb=%d ratio=%.2f thresh=%.2f s=%.2f/%.2f\n' 532 | % (nb, nav.ratio, nav.thresar, s[1], s[0])) 533 | nav.xa = nav.x[0:na].copy() 534 | nav.Pa = nav.P[0:na, 0:na].copy() 535 | bias = b[:, 0] 536 | y -= b[:, 0] 537 | K = Qab @ inv(Qb) 538 | nav.xa -= K @ y 539 | nav.Pa -= K @ Qab.T 540 | 541 | # restore single diff ambiguity 542 | xa = restamb(nav, bias, nb) 543 | else: 544 | trace(3,'ambiguity validation failed (nb=%d ratio=%.2f thresh=%.2f s=%.2f/%.2f' 545 | % (nb, nav.ratio, nav.thresar, s[1], s[0])) 546 | nb = 0 547 | return nb, xa 548 | 549 | 550 | def manage_amb_LAMBDA(nav, sats, stat, posvar): 551 | """ resolve integer ambiguity by LAMBDA using partial fix techniques and 552 | multiple attempts """ 553 | 554 | trace(3, 'posvar=%.6f\n' % posvar) 555 | trace(3, 'prevRatios = %.3f %.3f\n' % (nav.prev_ratio1, nav.prev_ratio2)) 556 | 557 | # skip AR if don't meet criteria 558 | if stat != gn.SOLQ_FLOAT or posvar > nav.thresar1: 559 | nav.ratio, nav.prev_ratio1, nav.prev_ratio2, nav.nb_ar = 0, 0, 0, 0 560 | trace(3, 'Skip AR\n') 561 | return 0, [] 562 | 563 | # if no fix on previous sample and enough sats, exclude next sat in list 564 | excflag = False 565 | if nav.prev_ratio2 < nav.thresar and nav.nb_ar >= nav.mindropsats: 566 | # find and count sats used last time for AR 567 | arsats = np.where(nav.prev_fix == 2)[0] 568 | excflag = 0 569 | if nav.excsat_ix < len(arsats): 570 | excsat = arsats[nav.excsat_ix] + 1 571 | lockc = copy(nav.lock[excsat-1]) # save lock count 572 | # remove sat from AR long enough to enable hold if stays fixed 573 | nav.lock[excsat-1] = -nav.nb_ar 574 | trace(3, 'AR: exclude sat %d\n' % excsat); 575 | excflag = True 576 | nav.excsat_ix += 1 577 | else: 578 | nav.excsat_ix = 0 # exclude none and reset to beginning of list 579 | 580 | # initial ambiguity resolution attempt, include all enabled sats 581 | nb, xa = resamb_lambda(nav, sats) 582 | ratio1 = nav.ratio 583 | rerun = False 584 | # if results are much poorer than previous epoch or dropped below AR ratio 585 | # thresh, remove new sats 586 | trace(3, 'lambda: nb=%d r1= %.3f r2=%.3f r=%.3f\n' % ((nb, nav.prev_ratio1, nav.prev_ratio2, nav.ratio))) 587 | if nb >= 0 and nav.prev_ratio2 >= nav.thresar and (nav.ratio < nav.thresar 588 | or (nav.ratio < nav.thresar * 1.1 and nav.ratio < nav.prev_ratio1 / 2.0)): 589 | trace(3, 'low ratio: check for new sat\n') 590 | dly = 2 591 | ix = np.where((nav.fix >= 2) & (nav.lock == 0)) 592 | for i,f in zip(ix[0],ix[1]): 593 | nav.lock[i,f] = -dly 594 | dly +=2 595 | trace(3, 'remove sat %d:%d lock=%d\n' % (i+1, f, nav.lock[i,f])) 596 | 597 | rerun = True 598 | 599 | # rerun if filter removed any sats 600 | if rerun: 601 | trace(3, 'rerun AR with new sat removed\n') 602 | nb, xa = resamb_lambda(nav, sats) 603 | 604 | # restore excluded sat if still no fix or significant increase in ar ratio 605 | if excflag and nav.ratio < nav.thresar and nav.ratio < 1.5* nav.prev_ratio2: 606 | nav.lock[excsat-1] = lockc 607 | trace(3, 'AR: restore sat %d\n' % excsat) 608 | 609 | nav.prev_ratio1, nav.prev_ratio2 = ratio1, nav.ratio 610 | return nb, xa 611 | 612 | 613 | def initx(nav, x0, v0, i): 614 | """ initialize x and P for index i """ 615 | nav.x[i] = x0 616 | nav.P[i,:] = 0 617 | nav.P[:,i] = 0 618 | nav.P[i,i] = v0 619 | 620 | 621 | def detslp_dop(rcv, nav, obs, ix): 622 | """ detect cycle slip with doppler measurement """ 623 | if nav.thresdop <= 0: 624 | return 625 | # calculate doppler differences for all sats and freqs 626 | ns = len(ix) 627 | mean_dop = ndop = 0 628 | dopdif = np.zeros((ns, nav.nf)) 629 | tt = np.zeros((ns, nav.nf)) 630 | for i, ii in enumerate(ix): 631 | sat = obs.sat[ii] - 1 632 | for f in range(nav.nf): 633 | if obs.L[ii,f] == 0.0 or obs.D[ii,f] == 0.0 or nav.ph[rcv,sat,f] == 0.0 \ 634 | or nav.pt[rcv,sat,f] == None: 635 | continue 636 | tt[i,f] = timediff(obs.t, nav.pt[rcv,sat,f]) 637 | if abs(tt[i,f]) < DTTOL: 638 | continue 639 | # calc phase difference and doppler x time (cycle) 640 | dph = (obs.L[ii,f] - nav.ph[rcv,sat,f]) / tt[i,f] 641 | dpt = -obs.D[ii,f] 642 | dopdif[i,f] = dph - dpt 643 | 644 | # if not outlier, use this to calculate mean 645 | if abs(dopdif[i,f]) < 3 * nav.thresdop: 646 | mean_dop += dopdif[i,f] 647 | ndop += 1 648 | # calc mean doppler diff, most likely due to clock error 649 | if ndop == 0: 650 | trace(4, 'detslp_dop rcv=%d: no valid doppler diffs\n' % (rcv+1)) 651 | return # unable to calc mean doppler, usually very large clock err 652 | mean_dop /= ndop 653 | 654 | # set slip if doppler difference with mean removed exceeds threshold 655 | for i, ii in enumerate(ix): 656 | sat = obs.sat[ii] - 1 657 | for f in range(nav.nf): 658 | if dopdif[i,f] == 0.0: 659 | continue 660 | if abs(dopdif[i,f] - mean_dop) > nav.thresdop: 661 | nav.slip[sat,f] |= 1 662 | trace(3, "slip detected doppler (sat=%2d rcv=%d dL%d=%.3f off=%.3f tt=%.2f)\n" 663 | % (sat+1, rcv+1, f+1, dopdif[i,f] - mean_dop, mean_dop, tt[i,f])) 664 | 665 | 666 | def detslp_gf(nav, obsb, obsr, iu, ir): 667 | """ detect cycle slip with geometry-free LC """ 668 | 669 | # skip if check disabled 670 | if nav.thresslip == 0 or nav.nf < 2: 671 | return 672 | ns = len(iu) 673 | _c = rCST.CLIGHT 674 | for i in range(ns): 675 | sat = obsr.sat[iu[i]] - 1 676 | # skip check if slip already detected 677 | if (nav.slip[sat,0] & 1) or (nav.slip[sat,1] & 1): 678 | continue 679 | # calc SD geomotry free LC of phase between freq0 and freq1 680 | L1R = obsr.L[iu[i],0] 681 | L2R = obsr.L[iu[i],1] 682 | L1B = obsb.L[ir[i],0] 683 | L2B = obsb.L[ir[i],1] 684 | if L1R == 0.0 or L1B == 0.0 or L2R == 0 or L2B == 0: 685 | trace(4, 'gf: skip sat %d, L=0\n' % sat) 686 | continue 687 | freq0 = sat2freq(sat + 1, 0, nav) 688 | freq1 = sat2freq(sat + 1, 1, nav) 689 | gf1 = ((L1R - L1B) * _c / freq0 - (L2R - L2B) * _c / freq1) 690 | if gf1 == 0: 691 | continue 692 | gf0 = nav.gf[sat] #retrieve previous gf 693 | nav.gf[sat] = gf1 # save current gf for next epoch 694 | if gf0 !=0.0 and abs(gf1 - gf0) > nav.thresslip: 695 | nav.slip[sat,0] |= 1 696 | nav.slip[sat,1] |= 1 697 | trace(3, "slip detected GF jump (sat=%2d L1-L2 dGF=%.3f)\n" % 698 | (sat + 1, gf0 - gf1)) 699 | 700 | def detslp_ll(nav, obs, ix, rcv): 701 | """ detect cycle slip from rinex file flags """ 702 | 703 | # retrieve previous LLI 704 | LLI = nav.prev_lli[:,:,rcv] 705 | 706 | ixsat = obs.sat[ix] - 1 707 | initP = (nav.sig_n0 / 2)**2 # init value for slips 708 | slip = np.zeros_like(nav.slip) 709 | for f in range(nav.nf): 710 | ixL = np.where(obs.L[ix,f] != 0)[0] 711 | if nav.tt >= 0: # forward 712 | slip[ixsat[ixL],f] |= (obs.lli[ix[ixL],f] & 3) 713 | else: # backward 714 | slip[ixsat[ixL],f] |= (LLI[ixsat[ixL],f] & 3) 715 | 716 | # detect slip by parity unknown flag transition in LLI 717 | hc_slip = np.where((obs.lli[ix[ixL],f] & 2) != 718 | (LLI[ixsat[ixL],f] & 2))[0] 719 | if len(hc_slip) > 0: 720 | slip[ixsat[ixL[hc_slip]],f] |= 1 721 | 722 | ixslip = np.where((slip[ixsat[ixL],f] & 1) != 0)[0] 723 | slipsats = ixsat[ixL[ixslip]] + 1 724 | ib = IB(slipsats, f, nav.na) 725 | for i in ib: 726 | nav.P[i,i] = max(nav.P[i,i], initP) 727 | # output results to trace 728 | 729 | if len(slipsats) > 0: 730 | trace(3, 'slip detected from LLI flags: f=%d, sats=%s slip=%s\n' 731 | % (f, str(slipsats), str(slip[ixsat[ixL[ixslip]], f]))) 732 | nav.slip = slip 733 | 734 | def udpos(nav, sol): 735 | """ states propagation for kalman filter """ 736 | tt = nav.tt 737 | trace(3, 'udpos : tt=%.3f\n' % tt) 738 | 739 | if nav.pmode == 'static': 740 | return 741 | 742 | # check variance of estimated position 743 | posvar = np.sum(np.diag(nav.P[0:3])) / 3 744 | if posvar > nav.sig_p0**2: 745 | #reset position with large variance 746 | for i in range(3): 747 | initx(nav, sol.rr[i], nav.sig_p0**2, i) 748 | initx(nav, 0, nav.sig_v0**2, i + 3) 749 | initx(nav, 1e-6, nav.sig_v0**2, i + 6) 750 | trace(2, 'reset rtk position due to large variance: var=%.3f\n' % posvar) 751 | return 752 | 753 | # state transition of position/velocity/acceleration 754 | F = np.eye(nav.nx) 755 | F[0:6, 3:9] += np.eye(6) * tt 756 | # include accel terms if filter is converged 757 | if posvar < nav.thresar1: 758 | F[0:3, 6:9] += np.eye(3) * np.sign(tt) * tt**2 / 2 759 | else: 760 | trace(3, 'pos var too high for accel term: %.4f\n' % posvar) 761 | # x=F*x, P=F*P*F 762 | nav.x = F @ nav.x 763 | nav.P = F @ nav.P @ F.T 764 | 765 | 766 | # process noise added to accel 767 | Q = np.zeros((3,3)) 768 | Q[0,0] = Q[1,1] = nav.accelh**2 * abs(tt) 769 | Q[2,2] = nav.accelv**2 * abs(tt) 770 | E = gn.xyz2enu(gn.ecef2pos(nav.x[0:3])) 771 | Qv = E.T @ Q @ E 772 | nav.P[6:9,6:9] += Qv 773 | 774 | def udbias(nav, obsb, obsr, iu, ir): 775 | 776 | trace(3, 'udbias : tt=%.3f ns=%d\n' % (nav.tt, len(iu))) 777 | 778 | # cycle slip detection from receiver flags 779 | detslp_ll(nav, obsb, ir, 0) 780 | detslp_ll(nav, obsr, iu, 1) 781 | # cycle slip detection by doppler and geom-free 782 | detslp_dop(0, nav, obsb, ir) # base 783 | detslp_dop(1, nav, obsr, iu) # rover 784 | detslp_gf(nav, obsb, obsr, iu, ir) 785 | 786 | # init sat and sys arrays 787 | ns = len(iu) 788 | sat = obsr.sat[iu] 789 | 790 | # update outage counters and reset phase-biases for sats with outage 791 | nav.outc += 1 792 | for f in range(nav.nf): 793 | for i in range(uGNSS.MAXSAT): 794 | ii = IB(i+1, f, nav.na) 795 | if nav.outc[i,f] > nav.maxout and nav.x[ii] != 0.0: 796 | trace(3, ' obs outage counter overflow ( sat=%d L%d: n=%d\n' 797 | % (i+1, f+1, nav.outc[i,f])) 798 | initx(nav, 0, 0, ii) 799 | # TODO: set AR minlock 800 | # update phase bias noise and check for cycle slips and outliers 801 | for i in range(ns): 802 | j = IB(sat[i], f, nav.na) 803 | nav.P[j,j] += nav.prnbias**2 * abs(nav.tt) 804 | if (nav.slip[sat[i]-1,f] & 1) or nav.rejc[sat[i]-1,f] > 1: 805 | trace(4, 'flag phase for reset: sat=%d f=%d slip=%d rejc=%d\n' % 806 | (sat[i], f, nav.slip[sat[i]-1,f], nav.rejc[sat[i]-1,f])) 807 | initx(nav, 0, 0, j) 808 | 809 | # estimate approximate phase-bias by delta phase - delta code 810 | bias = np.zeros(ns) 811 | offset = namb = 0 812 | for i in range(ns): 813 | freq = sat2freq(sat[i], f, nav) 814 | if obsr.L[iu[i], f] == 0 or obsb.L[ir[i], f] == 0 or \ 815 | obsr.P[iu[i], f] == 0 or obsb.P[ir[i], f] == 0: 816 | continue 817 | # calc single differences 818 | cp = obsr.L[iu[i], f] - obsb.L[ir[i], f] 819 | pr = obsr.P[iu[i], f] - obsb.P[ir[i], f] 820 | 821 | if cp == 0 or pr == 0 or freq == 0: 822 | continue 823 | # estimate bias in cycles 824 | bias[i] = cp - pr * freq / rCST.CLIGHT 825 | # offset = sum of (bias - phase-bias) for all valid sats in meters 826 | x = nav.x[IB(sat[i], f, nav.na)] 827 | if x != 0.0: 828 | offset += bias[i] - x 829 | namb += 1 830 | 831 | # correct phase-bias offset to ensure phase-code coherency 832 | offset = offset / namb if namb > 0 else 0 833 | trace(4, 'phase-code coherency adjust=%.2f, n=%d\n' % (offset, namb)) 834 | ib1 = IB(1, f, nav.na) # state offset for first sat 835 | # add offsest to non-zero states 836 | ix = np.where(nav.x[ib1:] != 0)[0] 837 | nav.x[ix+ib1] += offset 838 | 839 | # find sats that need to be reset 840 | for i in range(ns): 841 | j = IB(sat[i], f, nav.na) 842 | if bias[i] == 0.0 or nav.x[j] != 0.0: 843 | continue 844 | # set initial states of phase-bias 845 | freq = sat2freq(sat[i], f, nav) 846 | initx(nav, bias[i], nav.sig_n0**2, j) 847 | nav.outc[sat[i]-1,f] = 1 # make equal to others set above 848 | nav.rejc[sat[i]-1,f] = 0 849 | nav.lock[sat[i]-1,f] = 0 850 | trace(3," sat=%3d, F=%d: init phase=%.3f\n" % (sat[i],f+1, bias[i])) 851 | 852 | 853 | def udstate(nav, obsr, obsb, iu, ir, sol): 854 | """ temporal update of states """ 855 | trace(3, 'udstate : ns=%d\n' % len(iu)) 856 | # temporal update of position/velocity/acceleration 857 | udpos(nav, sol) # updates nav.x and nav.P 858 | # temporal update of phase-bias 859 | udbias(nav, obsb, obsr, iu, ir) # updates outxnav.x and nav.P 860 | 861 | def selsat(nav, obsr, obsb, elb): 862 | """ select common satellite between rover and base station """ 863 | 864 | trace(3, 'selsat : nu=%d nr=%d\n' % (len(obsr.sat), len(obsb.sat))); 865 | # exclude satellite with missing pseudorange or low elevation 866 | idx_u = np.unique(np.where(obsr.P!=0)[0]) 867 | idx_r = np.unique(np.where(obsb.P!=0)[0]) 868 | idx_r = list(set(idx_r).intersection(np.where(elb >= nav.elmin)[0])) 869 | 870 | idx = np.intersect1d(obsr.sat[idx_u], obsb.sat[idx_r], return_indices=True) 871 | k = len(idx[0]) 872 | iu = np.array(idx_u)[idx[1]] 873 | ir = np.array(idx_r)[idx[2]] 874 | return k, iu, ir 875 | 876 | 877 | def holdamb(nav, xa): 878 | """ hold integer ambiguity """ 879 | nb = nav.nx - nav.na 880 | v = np.zeros(nb) 881 | H = np.zeros((nav.nx, nb)) 882 | nv = 0 883 | for m in range(uGNSS.GNSSMAX): 884 | for f in range(nav.nf): 885 | n = 0 886 | index = [] 887 | for i in range(uGNSS.MAXSAT): 888 | sys = nav.sysprn[i+1][0] 889 | if sys != m or nav.fix[i, f] < 2: 890 | continue 891 | index.append(IB(i+1, f, nav.na)) 892 | n += 1 893 | nav.fix[i, f] = 3 # hold 894 | # use ambiguity resolution results to generate a set of pseudo-innovations 895 | # to feed to kalman filter based on error between fixed and float solutions 896 | for i in range(1, n): 897 | # phase-biases are single diff, so subtract errors to get 898 | # double diff: v(nv)=err(i)-err(0) 899 | v[nv] = (xa[index[0]] - xa[index[i]]) - \ 900 | (nav.x[index[0]] - nav.x[index[i]]) 901 | H[index[0], nv] = 1.0 902 | H[index[i], nv] = -1.0 903 | nv += 1 904 | if nv < nav.minholdsats: 905 | trace(3, 'holdamb: not enough sats to hold ambiguity\n') 906 | return 907 | trace(3, 'holdamb: hold on\n') 908 | R = np.eye(nv) * nav.var_holdamb 909 | # update states with constraints 910 | nav.x, nav.P = gn.filter(nav.x, nav.P, H[:,:nv], v[:nv], R) 911 | 912 | 913 | def relpos(nav, obsr, obsb, sol): 914 | """ relative positioning for PPK """ 915 | 916 | # time diff between rover and base 917 | nav.dt = timediff(obsr.t, obsb.t) 918 | trace(1,"\n---------------------------------------------------------\n") 919 | trace(1, "relpos: dt=%.3f nu=%d nr=%d\n" % (nav.dt, len(obsr.sat), len(obsb.sat))) 920 | trace(1,"---------------------------------------------------------\n") 921 | if abs(nav.dt) > nav.maxage: 922 | trace(3, 'Age of differential too large: %.2f\n' % nav.dt) 923 | return 924 | 925 | # clear valid sat status 926 | nav.vsat[:,:] = 0 927 | 928 | # compute satellite positions, velocities and clocks 929 | rs, var, dts, svh = satposs(obsr, nav) 930 | rsb, varb, dtsb, svhb = satposs(obsb, nav) 931 | 932 | # undifferenced residuals for base 933 | trace(3, 'base station:\n') 934 | yr, er, azelr = zdres(nav, obsb, rsb, dtsb, svhb, varb, nav.rb, 0) 935 | if nav.interp_base: 936 | # get residuals for previous base station obs 937 | yr0, _, _ = zdres(nav, nav.obsb, nav.rsb, nav.dtsb, nav.svhb, nav.varb, 938 | nav.rb, 0) 939 | # time-interpolation of base residuals 940 | yr, nav.dt = intpres(obsr.t, nav, yr0, yr, nav.obsb, obsb) 941 | 942 | # find common sats between base and rover 943 | ns, iu, ir = selsat(nav, obsr, obsb, azelr[:,1]) 944 | 945 | # kalman filter time propagation 946 | tracemat(3, 'before udstate: x=', nav.x[0:9], '.4f') 947 | #tracemat(3, ' Pdiag=', np.diag(nav.P[:9,:9]), '.6f') 948 | udstate(nav, obsr, obsb, iu, ir, sol) 949 | tracemat(3, 'after udstate x=', nav.x[0:9], '.4f') 950 | #tracemat(3, ' Pdiag=', np.diag(nav.P[:9,:9]), '.6f') 951 | 952 | if ns <= 0: 953 | trace(3, 'no common sats: %d\n' % ns) 954 | return 955 | 956 | # save SNR values 957 | for f in range(nav.nf): 958 | nav.SNR_rover[obsr.sat[iu]-1,f] = obsr.S[iu,f] 959 | nav.SNR_base[obsb.sat[ir]-1,f] = obsb.S[ir,f] 960 | 961 | # undifferenced residuals for rover 962 | trace(3, 'rover: dt=%.3f\n' % nav.dt) 963 | yu, eu, azel = zdres(nav, obsr, rs, dts, svh, var, nav.x[0:3], 1) 964 | # decode stdevs from receiver 965 | rn.rcvstds(nav, obsr) 966 | 967 | # remove non-common residuals 968 | yr, er = yr[ir,:], er[ir,:] 969 | yu, eu = yu[iu,:], eu[iu,:] 970 | sats = obsr.sat[iu] 971 | nav.azel[sats-1] = azel[iu] 972 | els = azel[iu,1] 973 | 974 | # calculate double-differenced residuals and create state matrix from sat angles 975 | v, H, R = ddres(nav, nav.x, nav.P, yr, er, yu, eu, sats, els, nav.dt, obsr, True) 976 | 977 | if len(v) < 4: 978 | trace(3, 'not enough double-differenced residual\n') 979 | stat = gn.SOLQ_NONE 980 | else: 981 | stat = gn.SOLQ_FLOAT 982 | 983 | if stat != gn.SOLQ_NONE: 984 | # kalman filter measurement update, updates x,y,z,sat phase biases, etc 985 | tracemat(3, 'before filter x=', nav.x[0:9], '.4f') 986 | #tracemat(3, ' Pdiag=', np.diag(nav.P[:9,:9]), '.6f') 987 | xp, Pp = gn.filter(nav.x, nav.P, H, v, R) 988 | tracemat(3, 'after filter x=', xp[0:9], '.4f') 989 | #tracemat(3, ' Pdiag=', np.diag(Pp[:9,:9]), '.6f') 990 | posvar = np.sum(np.diag(Pp[0:3])) / 3 991 | trace(3,"posvar=%.6f \n" % posvar) 992 | 993 | # calc zero diff residuals again after kalman filter update 994 | yu, eu, _ = zdres(nav, obsr, rs, dts, svh, var, xp[0:3], 1) 995 | yu, eu = yu[iu,:], eu[iu,:] 996 | # calc double diff residuals again after kalman filter update for float solution 997 | v, H, R = ddres(nav, xp, Pp, yr, er, yu, eu, sats, els, nav.dt, obsr) 998 | # validation of float solution, always returns 1, msg to trace file if large residual 999 | valpos(nav, v, R) 1000 | 1001 | # update state and covariance matrix from kalman filter update 1002 | nav.x = xp.copy() 1003 | nav.P = Pp.copy() 1004 | 1005 | # update valid satellite status for ambiguity control 1006 | for f in range(nav.nf): 1007 | ix = np.where(nav.vsat[:,f] > 0)[0] 1008 | nav.outc[ix,f] = 0 1009 | if f == 0: 1010 | nav.ns = len(ix) # valid satellite count by L1 1011 | # check for too few valid phases 1012 | if nav.ns < 4: 1013 | stat = gn.SOLQ_DGPS; 1014 | 1015 | # resolve integer ambiguity by LAMBDA 1016 | if nav.armode > 0 and stat == gn.SOLQ_FLOAT: 1017 | # if valid fixed solution, process it 1018 | nb, xa = manage_amb_LAMBDA(nav, sats, stat, posvar) 1019 | if nb > 0: 1020 | # find zero-diff residuals for fixed solution 1021 | yu, eu, azel = zdres(nav, obsr, rs, dts, svh, var, xa[0:3], 1) 1022 | yu, eu, el = yu[iu, :], eu[iu, :], azel[iu,1] 1023 | # post-fit residuals for fixed solution (xa includes fixed phase biases, rtk->xa does not) 1024 | v, H, R = ddres(nav, xa, nav.P, yr, er, yu, eu, sats, el, nav.dt, obsr) 1025 | # validation of fixed solution, always returns valid 1026 | if valpos(nav, v, R): 1027 | nav.nfix += 1 1028 | if nav.armode == 3 and nav.nfix >= nav.minfix: 1029 | holdamb(nav, xa) 1030 | stat = gn.SOLQ_FIX 1031 | 1032 | # save solution status (fixed or float) 1033 | if stat == gn.SOLQ_FIX: 1034 | sol.rr = nav.xa[0:6] 1035 | sol.qr = nav.Pa[0:3,0:3] 1036 | sol.qv = nav.Pa[3:6,3:6] 1037 | else: # SOLQ_FLOAT or SOLQ_DGPS 1038 | sol.rr = nav.x[0:6] 1039 | sol.qr = nav.P[0:3,0:3] 1040 | sol.qv = nav.P[3:6,3:6] 1041 | nav.nfix = 0 1042 | sol.stat = stat 1043 | sol.ratio = nav.ratio 1044 | sol.age = nav.dt 1045 | nav.sol.append(sol) 1046 | nav.rr = sol.rr[0:3] 1047 | tracemat(3, 'sol_rr= ', sol.rr, '15.3f') 1048 | 1049 | # save phases and times for cycle slip detection 1050 | for i, sat in enumerate(sats): 1051 | for f in range(nav.nf): 1052 | if obsb.L[ir[i],f] != 0: 1053 | nav.pt[0,sat-1,f] = obsb.t 1054 | nav.ph[0,sat-1,f] = obsb.L[ir[i],f] 1055 | if obsr.L[iu[i],f] != 0: 1056 | nav.pt[1,sat-1,f] = obsr.t 1057 | nav.ph[1,sat-1,f] = obsr.L[iu[i],f] 1058 | # save current LLI and fix status 1059 | nav.slip[:,:] = 0 1060 | for f in range(nav.nf): 1061 | ix0 = np.where((obsb.L[:,f] != 0) | (obsb.lli[:,f] != 0))[0] 1062 | ix1 = np.where((obsr.L[:,f] != 0) | (obsr.lli[:,f] != 0))[0] 1063 | nav.prev_lli[obsb.sat[ix0]-1,f,0] = obsb.lli[ix0,f] 1064 | nav.prev_lli[obsr.sat[ix1]-1,f,1] = obsr.lli[ix1,f] 1065 | if nav.armode > 0: 1066 | nav.prev_fix = copy(nav.fix) 1067 | # update lock counts for sats used in fix and disabled sats (lock < 0) 1068 | for f in range(nav.nf): 1069 | ix = np.where(((nav.nb_ar > 0) & (nav.fix[:,f] >= 2)) | (nav.lock[:,f] < 0))[0] 1070 | nav.lock[ix,f] += 1 1071 | 1072 | 1073 | def rtkpos(nav, rov, base, fp_stat, dir): 1074 | """ relative positioning for PPK """ 1075 | trace(3, 'rtkpos: start solution, dir=%d\n' % dir) 1076 | n = 0 1077 | sol = gn.Sol() 1078 | # loop through all epochs 1079 | while True: 1080 | if n== 0: 1081 | # first epoch 1082 | obsr, obsb = rn.first_obs(nav, rov, base, dir) 1083 | t = 0 1084 | # force initial rover position (for align to RTKLIB) 1085 | if dir == 1: 1086 | if cfg.rr_f[0] != 0: 1087 | nav.x[0:6] = cfg.rr_f 1088 | else: 1089 | if cfg.rr_b[0] != 0: 1090 | nav.x[0:6] = cfg.rr_b 1091 | nav.x[6:9] = 1E-6 # match RTKLIB 1092 | else: 1093 | # get next rover obs and next base obs if required 1094 | if len(nav.sol) > 0: 1095 | t = nav.sol[-1].t # previous epoch 1096 | obsr, obsb = rn.next_obs(nav, rov, base, dir) 1097 | if obsr == []: 1098 | break 1099 | # single precision solution, used to update solution time 1100 | if nav.use_sing_pos or sol.stat == gn.SOLQ_NONE or sol.rr[0] == 0.0: 1101 | sol = pntpos(obsr, nav) 1102 | else: 1103 | sol = gn.Sol() 1104 | if sol.t.time == 0: 1105 | sol.t = obsr.t 1106 | if t != 0: 1107 | nav.tt = timediff(sol.t, t) # timediff from previous epoch 1108 | # relative solution 1109 | relpos(nav, obsr, obsb, sol) 1110 | outsolstat(nav, sol, fp_stat) 1111 | ep = gn.time2epoch(sol.t) 1112 | stdout.write('\r %2d/%2d/%4d %02d:%02d:%05.2f: %d' % (ep[1], ep[2], ep[0], 1113 | ep[3], ep[4], ep[5], sol.stat)) 1114 | n += 1 1115 | if nav.maxepoch != None and n > nav.maxepoch: 1116 | break 1117 | trace(3, 'rtkpos: end solution\n') 1118 | 1119 | 1120 | 1121 | -------------------------------------------------------------------------------- /src/run_ppk.py: -------------------------------------------------------------------------------- 1 | """ 2 | RTKLIB-Py: Top level code for post-processing solution from rinex data 3 | 4 | Copyright (c) 2022 Tim Everett 5 | """ 6 | 7 | import sys, os, shutil 8 | 9 | # set run parameters 10 | maxepoch = None # max # of epochs, used for debug, None = no limit 11 | trace_level = 3 # debug trace level 12 | basepos = [] # default to not specified here 13 | 14 | ######## specify input files ###################################### 15 | 16 | # cell phone example from 2021 Google Smartphone Decimeter Challenge 17 | # datadir = r'C:\gps\python\rtklib-py\data\phone' 18 | # navfile = 'nav_1350.nav' 19 | # rovfile = 'Pixel4_GnssLog.obs' 20 | # basefile = 'slac1350.obs' 21 | # cfgfile = 'config_phone.py' # must be in src folder or absolute path 22 | 23 | # u-blox example 24 | datadir = '../data/u-blox' 25 | navfile = 'rover.nav' 26 | rovfile = 'rover.obs' 27 | basefile = 'tmg23590.obs' 28 | cfgfile = 'config_f9p.py' # must be in src folder or absolute path 29 | 30 | ################################################################### 31 | 32 | # Copy config file 33 | shutil.copyfile(cfgfile, '__ppk_config.py') 34 | 35 | # import rtklib files 36 | import __ppk_config as cfg 37 | import rinex as rn 38 | import rtkcmn as gn 39 | from rtkpos import rtkinit 40 | from postpos import procpos, savesol 41 | 42 | # generate output file names 43 | solfile = rovfile[:-4] + '.pos' 44 | statfile = os.path.join(datadir, rovfile[:-4] + '.pos.stat') 45 | fp_stat = open(statfile, 'w') 46 | if trace_level > 0: 47 | trcfile = os.path.join(datadir, rovfile[:-4] + '.trace') 48 | sys.stderr = open(trcfile, "w") 49 | 50 | # Read config file 51 | shutil.copyfile(cfgfile, '__ppk_config.py') 52 | 53 | 54 | # init solution 55 | os.chdir(datadir) 56 | gn.tracelevel(trace_level) 57 | nav = rtkinit(cfg) 58 | nav.maxepoch = maxepoch 59 | 60 | # load rover obs 61 | rov = rn.rnx_decode(cfg) 62 | print('Reading rover obs...') 63 | if nav.filtertype == 'backward': 64 | maxepoch = None # load all obs for 65 | rov.decode_obsfile(nav, rovfile, maxepoch) 66 | 67 | # load base obs 68 | base = rn.rnx_decode(cfg) 69 | print('Reading base obs...') 70 | base.decode_obsfile(nav, basefile, None) 71 | if basepos != []: 72 | nav.rb = basepos 73 | elif nav.rb[0] == 0: 74 | nav.rb = base.pos 75 | 76 | # load nav data from rover obs 77 | print('Reading nav data...') 78 | rov.decode_nav(navfile, nav) 79 | 80 | # calculate solution 81 | print('Calculating solution ...\n') 82 | sol = procpos(nav, rov, base, fp_stat) 83 | 84 | # save solution to file 85 | savesol(sol, solfile) 86 | fp_stat.close() 87 | --------------------------------------------------------------------------------