KalmanFilter_T.cpp
Go to the documentation of this file.
1 //==============================================================================
2 //
3 // This file is part of GNSSTk, the ARL:UT GNSS Toolkit.
4 //
5 // The GNSSTk is free software; you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published
7 // by the Free Software Foundation; either version 3.0 of the License, or
8 // any later version.
9 //
10 // The GNSSTk is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public
16 // License along with GNSSTk; if not, write to the Free Software Foundation,
17 // Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
18 //
19 // This software was developed by Applied Research Laboratories at the
20 // University of Texas at Austin.
21 // Copyright 2004-2022, The Board of Regents of The University of Texas System
22 //
23 //==============================================================================
24 
25 //==============================================================================
26 //
27 // This software was developed by Applied Research Laboratories at the
28 // University of Texas at Austin, under contract to an agency or agencies
29 // within the U.S. Department of Defense. The U.S. Government retains all
30 // rights to use, duplicate, distribute, disclose, or release this software.
31 //
32 // Pursuant to DoD Directive 523024
33 //
34 // DISTRIBUTION STATEMENT A: This software has been approved for public
35 // release, distribution is unlimited.
36 //
37 //==============================================================================
38 
40 
41 
42 // This program takes dual frequency range and phase data for a
43 // complete satellite pass and computes the ionospheric delay and the
44 // phase biases. The data has been edited of any bad points and there
45 // are no cycleslips. The filter design was inspired by the smoothing
46 // algorithm.
47 //
48 // This model from the smoothing algorithm does not work well for
49 // numerical reasons; the range of the data is too large,
50 // i.e. mag(data) too large compared to state
51 // [perhaps try this again...]
52 // [ P1 ] [ 1 1 0 0 ] [ R ] NB R is really range+clock
53 // [ P2 ] = [ 1 alpha+1 0 0 ] [ I ]
54 // [ L1 ] [ 1 -1 1 0 ] [ B1 ]
55 // [ L2 ] [ 1 -alpha-1 0 1 ] [ B2 ]
56 //
57 // instead use this model, which removes the large numbers from the problem
58 // Data = Partials * State
59 // [ L1-L2 ] [ alpha 1 -1 ] [ I ]
60 // [ L1-P1 ] = [ -2 1 0 ] [ B1 ]
61 // [ L2-P2 ] [ -2(alpha+1) 0 1 ] [ B2 ]
62 //
63 // this partials matrix has inverse
64 // [ I ] 1 [ -1 1 -1 ] [ L1-L2 ]
65 // [ B1 ] = - [ -2 (a+2) -2 ] [ L1-P1 ]
66 // [ B2 ] a [ (-2a-2) (2a+2) (-a-2) ] [ L2-P2 ]
67 // { Note operator D is the lower-right 2x2 matrix in partials }
68 //
69 // the partials matrix implies that the meas.cov. matrix is
70 // Cov(M) = H*Cov(L1,etc)*H^T
71 // [ L1^2+L2^2 L1^2 -L2^2 ]
72 // [ L1^2 L1^2+P1^2 0 ]
73 // [ -L2^2 0 L2^2+P2^2 ]
74 //
75 // note that once you have the biases, you can construct the debiased
76 // phase Ldb = L - B and smoothed range Psm = D*Ldb = Iono-free phase
77 // = Iono-free range
78 // = [(alpha+1)P1-P2]/alpha = [(alpha+1)L1-L2]/alpha
79 
80 // NB this problem is simple in the sense that there is no clock or
81 // trop state to complicate the estimation of biases. This is
82 // particularly relevant in the partitioned problem at the end -
83 // reduced weight and partials matrices are fixed! Consider the case
84 // with random time-varying clock in state
85 //
86 
87 
88 // system
89 #include <iostream>
90 #include <iomanip>
91 #include <string>
92 #include <map>
93 // gnsstk
94 #include "GNSSconstants.hpp"
95 #include "StringUtils.hpp"
96 #include "Matrix.hpp"
97 #include "Stats.hpp"
98 // geomatics
99 #include "SRIFilter.hpp"
100 #include "logstream.hpp"
101 
102 
103 
104 using namespace std;
105 using namespace gnsstk;
106 
107 
108 
109 int main(int argc, char **argv)
110 {
111  try
112  {
113  int i;
114 
115  // uncomment to see all the filter output
116  //ConfigureLOG::ReportingLevel() = ConfigureLOG::Level("VERBOSE");
117 
118  // satellite G01, first complete pass.
119  // The data are seconds_of_week, L1, L2, P1, P2, (L in
120  // cycles, P in meters.)
121  const unsigned int M=734;
122  const unsigned int C=5;
123  double data[C*M] = {
124  86400.000, 123335818.734, 96105816.038, 23470039.004, 23470042.870,
125  86430.000, 123265156.115, 96050754.313, 23456591.718, 23456595.395,
126  86460.000, 123194673.332, 95995832.714, 23443178.907, 23443183.384,
127  86490.000, 123124370.538, 95941051.365, 23429800.786, 23429805.463,
128  86520.000, 123054247.657, 95886410.203, 23416457.467, 23416461.670,
129  86550.000, 122984304.730, 95831909.269, 23403147.839, 23403151.894,
130  86580.000, 122914541.707, 95777548.509, 23389871.985, 23389875.676,
131  86610.000, 122844958.703, 95723328.035, 23376630.318, 23376634.588,
132  86640.000, 122775555.760, 95669247.865, 23363423.840, 23363427.962,
133  86670.000, 122706332.734, 95615307.878, 23350251.545, 23350255.081,
134  86700.000, 122637289.983, 95561508.370, 23337112.954, 23337116.936,
135  86730.000, 122568427.349, 95507849.223, 23324008.454, 23324012.788,
136  86760.000, 122499744.875, 95454330.466, 23310938.555, 23310942.689,
137  86790.000, 122431242.589, 95400952.098, 23297903.060, 23297907.173,
138  86820.000, 122362920.352, 95347714.021, 23284901.758, 23284904.901,
139  86850.000, 122294778.318, 95294616.362, 23271934.552, 23271938.345,
140  86880.000, 122226816.352, 95241659.037, 23259001.663, 23259005.725,
141  86910.000, 122159034.652, 95188842.186, 23246103.080, 23246107.502,
142  86940.000, 122091433.256, 95136165.812, 23233239.621, 23233243.320,
143  86970.000, 122024012.029, 95083629.828, 23220409.796, 23220413.464,
144  87000.000, 121956770.904, 95031234.188, 23207613.948, 23207617.509,
145  87030.000, 121889709.873, 94978978.878, 23194852.197, 23194856.350,
146  87060.000, 121822829.087, 94926864.024, 23182125.608, 23182129.376,
147  87090.000, 121756128.227, 94874889.374, 23169433.515, 23169436.665,
148  87120.000, 121689607.354, 94823054.965, 23156774.799, 23156778.296,
149  87150.000, 121623266.648, 94771360.946, 23144150.226, 23144153.495,
150  87180.000, 121557105.814, 94719807.103, 23131559.984, 23131563.345,
151  87210.000, 121491124.876, 94668393.421, 23119004.323, 23119007.681,
152  87240.000, 121425323.835, 94617119.925, 23106482.905, 23106486.377,
153  87270.000, 121359702.442, 94565986.421, 23093995.156, 23093999.520,
154  87300.000, 121294260.899, 94514993.062, 23081541.646, 23081546.020,
155  87330.000, 121228999.036, 94464139.709, 23069122.632, 23069126.706,
156  87360.000, 121163916.542, 94413426.110, 23056738.239, 23056742.189,
157  87390.000, 121099013.729, 94362852.518, 23044387.541, 23044390.974,
158  87420.000, 121034290.339, 94312418.734, 23032071.001, 23032075.119,
159  87450.000, 120969746.257, 94262124.684, 23019788.634, 23019792.820,
160  87480.000, 120905381.503, 94211970.365, 23007540.387, 23007544.888,
161  87510.000, 120841196.134, 94161955.832, 22995326.573, 22995330.493,
162  87540.000, 120777190.031, 94112080.977, 22983146.449, 22983150.086,
163  87570.000, 120713362.959, 94062345.623, 22971000.419, 22971004.006,
164  87600.000, 120649714.905, 94012749.762, 22958888.432, 22958892.127,
165  87630.000, 120586245.703, 93963293.279, 22946810.686, 22946814.778,
166  87660.000, 120522955.365, 93913976.159, 22934767.203, 22934770.787,
167  87690.000, 120459843.656, 93864798.241, 22922757.114, 22922761.467,
168  87720.000, 120396910.616, 93815759.547, 22910781.490, 22910785.295,
169  87750.000, 120334156.093, 93766859.959, 22898839.704, 22898843.437,
170  87780.000, 120271580.124, 93718099.492, 22886931.935, 22886935.211,
171  87810.000, 120209182.662, 93669478.128, 22875058.489, 22875061.290,
172  87840.000, 120146963.278, 93620995.521, 22863218.290, 22863221.703,
173  87870.000, 120084921.911, 93572651.643, 22851411.776, 22851415.677,
174  87900.000, 120023058.595, 93524446.497, 22839639.431, 22839643.587,
175  87930.000, 119961373.257, 93476380.044, 22827901.338, 22827904.862,
176  87960.000, 119899865.631, 93428452.048, 22816196.982, 22816200.200,
177  87990.000, 119838535.745, 93380662.554, 22804526.360, 22804529.370,
178  88020.000, 119777383.399, 93333011.417, 22792889.143, 22792892.833,
179  88050.000, 119716408.460, 93285498.526, 22781286.333, 22781289.757,
180  88080.000, 119655610.682, 93238123.671, 22769716.416, 22769720.343,
181  88110.000, 119594990.234, 93190887.001, 22758181.238, 22758184.757,
182  88140.000, 119534546.886, 93143788.323, 22746678.996, 22746682.795,
183  88170.000, 119474280.531, 93096827.552, 22735210.590, 22735214.167,
184  88200.000, 119414190.899, 93050004.499, 22723775.772, 22723779.293,
185  88230.000, 119354277.815, 93003319.014, 22712374.780, 22712378.172,
186  88260.000, 119294541.355, 92956771.165, 22701007.058, 22701010.784,
187  88290.000, 119234981.220, 92910360.699, 22689673.555, 22689677.123,
188  88320.000, 119175597.336, 92864087.585, 22678372.676, 22678376.603,
189  88350.000, 119116389.460, 92817951.602, 22667106.105, 22667109.588,
190  88380.000, 119057357.399, 92771952.634, 22655872.539, 22655875.823,
191  88410.000, 118998501.181, 92726090.674, 22644672.486, 22644676.166,
192  88440.000, 118939820.527, 92680365.526, 22633505.646, 22633509.033,
193  88470.000, 118881315.355, 92634777.103, 22622372.721, 22622376.284,
194  88500.000, 118822985.597, 92589325.377, 22611273.083, 22611276.575,
195  88530.000, 118764831.063, 92544010.182, 22600206.665, 22600209.565,
196  88560.000, 118706851.712, 92498831.501, 22589173.549, 22589176.797,
197  88590.000, 118649047.336, 92453789.160, 22578173.788, 22578176.679,
198  88620.000, 118591417.649, 92408882.943, 22567206.913, 22567210.031,
199  88650.000, 118533962.330, 92364112.600, 22556273.765, 22556276.867,
200  88680.000, 118476681.373, 92319478.119, 22545373.341, 22545376.451,
201  88710.000, 118419574.763, 92274979.481, 22534506.552, 22534509.433,
202  88740.000, 118362642.308, 92230616.560, 22523672.169, 22523675.939,
203  88770.000, 118305883.852, 92186389.224, 22512871.659, 22512874.860,
204  88800.000, 118249299.289, 92142297.390, 22502103.823, 22502107.151,
205  88830.000, 118192888.331, 92098340.831, 22491369.020, 22491372.276,
206  88860.000, 118136650.959, 92054519.528, 22480667.861, 22480670.649,
207  88890.000, 118080586.919, 92010833.288, 22469999.027, 22470002.063,
208  88920.000, 118024695.902, 91967281.870, 22459363.320, 22459366.664,
209  88950.000, 117968977.764, 91923865.164, 22448760.360, 22448763.646,
210  88980.000, 117913432.555, 91880583.208, 22438190.625, 22438193.703,
211  89010.000, 117858059.902, 91837435.712, 22427653.501, 22427656.684,
212  89040.000, 117802859.793, 91794422.665, 22417149.023, 22417152.115,
213  89070.000, 117747832.328, 91751544.139, 22406677.906, 22406680.719,
214  89100.000, 117692977.034, 91708799.776, 22396239.215, 22396242.478,
215  89130.000, 117638293.727, 91666189.437, 22385833.397, 22385836.769,
216  89160.000, 117583782.204, 91623712.952, 22375460.089, 22375463.619,
217  89190.000, 117529442.407, 91581370.278, 22365119.606, 22365122.975,
218  89220.000, 117475274.092, 91539161.237, 22354811.578, 22354814.976,
219  89250.000, 117421277.069, 91497085.658, 22344536.302, 22344539.454,
220  89280.000, 117367451.297, 91455143.526, 22334293.348, 22334296.510,
221  89310.000, 117313796.330, 91413334.480, 22324083.516, 22324086.403,
222  89340.000, 117260312.171, 91371658.539, 22313905.526, 22313908.782,
223  89370.000, 117206998.564, 91330115.491, 22303759.901, 22303763.755,
224  89400.000, 117153855.499, 91288705.340, 22293646.996, 22293651.011,
225  89430.000, 117100882.857, 91247427.985, 22283566.769, 22283570.561,
226  89460.000, 117048080.380, 91206283.216, 22273519.045, 22273522.548,
227  89490.000, 116995447.967, 91165270.967, 22263503.337, 22263506.589,
228  89520.000, 116942985.367, 91124391.040, 22253519.956, 22253523.413,
229  89550.000, 116890692.454, 91083643.340, 22243568.938, 22243572.526,
230  89580.000, 116838569.213, 91043027.857, 22233650.186, 22233653.652,
231  89610.000, 116786615.470, 91002544.446, 22223763.697, 22223767.365,
232  89640.000, 116734830.933, 90962192.875, 22213909.685, 22213913.191,
233  89670.000, 116683215.455, 90921973.049, 22204087.323, 22204090.880,
234  89700.000, 116631768.764, 90881884.741, 22194297.346, 22194300.742,
235  89730.000, 116580490.905, 90841927.986, 22184539.644, 22184542.920,
236  89760.000, 116529381.585, 90802102.566, 22174813.728, 22174816.898,
237  89790.000, 116478440.787, 90762408.458, 22165120.144, 22165123.114,
238  89820.000, 116427668.346, 90722845.537, 22155458.327, 22155461.507,
239  89850.000, 116377063.984, 90683413.591, 22145828.756, 22145831.625,
240  89880.000, 116326627.740, 90644112.641, 22136230.852, 22136233.871,
241  89910.000, 116276359.421, 90604942.548, 22126664.840, 22126668.118,
242  89940.000, 116226258.710, 90565903.056, 22117131.047, 22117134.337,
243  89970.000, 116176325.612, 90526994.181, 22107629.005, 22107632.158,
244  90000.000, 116126559.703, 90488215.569, 22098159.081, 22098161.944,
245  90030.000, 116076961.130, 90449567.347, 22088721.018, 22088723.636,
246  90060.000, 116027529.539, 90411049.247, 22079314.361, 22079317.152,
247  90090.000, 115978264.948, 90372661.272, 22069939.570, 22069942.414,
248  90120.000, 115929167.074, 90334403.207, 22060596.535, 22060599.465,
249  90150.000, 115880235.913, 90296275.059, 22051285.035, 22051288.363,
250  90180.000, 115831471.382, 90258276.739, 22042005.624, 22042009.002,
251  90210.000, 115782873.275, 90220408.106, 22032757.680, 22032760.827,
252  90240.000, 115734441.495, 90182669.071, 22023541.499, 22023544.517,
253  90270.000, 115686175.860, 90145059.509, 22014356.789, 22014359.952,
254  90300.000, 115638076.152, 90107579.229, 22005203.773, 22005206.640,
255  90330.000, 115590142.372, 90070228.252, 21996082.151, 21996085.266,
256  90360.000, 115542374.377, 90033006.457, 21986992.266, 21986995.408,
257  90390.000, 115494771.924, 89995913.654, 21977933.654, 21977936.998,
258  90420.000, 115447334.855, 89958949.723, 21968906.941, 21968910.027,
259  90450.000, 115400063.397, 89922114.838, 21959911.622, 21959914.762,
260  90480.000, 115352957.128, 89885408.676, 21950947.336, 21950950.509,
261  90510.000, 115306015.881, 89848831.094, 21942014.807, 21942017.584,
262  90540.000, 115259239.632, 89812382.094, 21933113.360, 21933116.356,
263  90570.000, 115212628.360, 89776061.636, 21924243.421, 21924246.305,
264  90600.000, 115166182.098, 89739869.764, 21915405.064, 21915407.928,
265  90630.000, 115119900.599, 89703806.276, 21906598.186, 21906601.128,
266  90660.000, 115073783.617, 89667870.983, 21897822.452, 21897825.236,
267  90690.000, 115027831.142, 89632063.874, 21889077.875, 21889080.716,
268  90720.000, 114982043.284, 89596385.044, 21880364.567, 21880367.605,
269  90750.000, 114936419.750, 89560834.262, 21871682.428, 21871685.810,
270  90780.000, 114890960.493, 89525411.482, 21863031.802, 21863034.990,
271  90810.000, 114845665.328, 89490116.570, 21854412.422, 21854415.429,
272  90840.000, 114800534.219, 89454949.485, 21845824.782, 21845827.478,
273  90870.000, 114755567.009, 89419910.113, 21837267.709, 21837270.385,
274  90900.000, 114710763.660, 89384998.427, 21828741.806, 21828744.762,
275  90930.000, 114666124.204, 89350214.447, 21820247.349, 21820250.086,
276  90960.000, 114621648.379, 89315557.977, 21811783.443, 21811786.796,
277  90990.000, 114577336.296, 89281029.101, 21803351.056, 21803354.448,
278  91020.000, 114533187.590, 89246627.538, 21794949.784, 21794953.083,
279  91050.000, 114489202.406, 89212353.374, 21786580.042, 21786582.846,
280  91080.000, 114445380.721, 89178206.625, 21778240.979, 21778243.657,
281  91110.000, 114401722.420, 89144187.186, 21769933.053, 21769935.933,
282  91140.000, 114358227.329, 89110294.916, 21761656.330, 21761659.035,
283  91170.000, 114314895.665, 89076529.993, 21753410.448, 21753413.220,
284  91200.000, 114271727.259, 89042892.292, 21745195.653, 21745198.880,
285  91230.000, 114228722.132, 89009381.818, 21737012.128, 21737015.316,
286  91260.000, 114185880.381, 88975998.657, 21728859.652, 21728862.910,
287  91290.000, 114143201.388, 88942742.314, 21720738.111, 21720741.319,
288  91320.000, 114100685.559, 88909613.106, 21712647.693, 21712650.603,
289  91350.000, 114058332.731, 88876610.915, 21704588.394, 21704591.127,
290  91380.000, 114016142.875, 88843735.713, 21696559.676, 21696562.574,
291  91410.000, 113974115.991, 88810987.505, 21688562.169, 21688565.015,
292  91440.000, 113932251.998, 88778366.222, 21680595.537, 21680598.509,
293  91470.000, 113890550.914, 88745871.875, 21672660.160, 21672663.141,
294  91500.000, 113849012.879, 88713504.598, 21664755.591, 21664758.858,
295  91530.000, 113807637.629, 88681264.160, 21656882.382, 21656885.470,
296  91560.000, 113766425.370, 88649150.724, 21649039.760, 21649042.989,
297  91590.000, 113725375.943, 88617164.172, 21641228.456, 21641231.313,
298  91620.000, 113684489.279, 88585304.452, 21633447.719, 21633450.854,
299  91650.000, 113643765.457, 88553571.610, 21625698.306, 21625701.409,
300  91680.000, 113603204.642, 88521965.795, 21617979.741, 21617982.845,
301  91710.000, 113562806.768, 88490486.937, 21610292.230, 21610295.281,
302  91740.000, 113522571.835, 88459135.053, 21602635.749, 21602638.925,
303  91770.000, 113482499.780, 88427910.089, 21595010.399, 21595013.612,
304  91800.000, 113442590.607, 88396812.037, 21587416.178, 21587419.171,
305  91830.000, 113402844.256, 88365840.868, 21579852.670, 21579855.703,
306  91860.000, 113363260.970, 88334996.762, 21572320.123, 21572323.144,
307  91890.000, 113323840.757, 88304279.726, 21564818.853, 21564821.691,
308  91920.000, 113284583.684, 88273689.816, 21557348.381, 21557351.237,
309  91950.000, 113245489.756, 88243227.023, 21549909.179, 21549911.861,
310  91980.000, 113206559.122, 88212891.478, 21542500.885, 21542503.582,
311  92010.000, 113167791.752, 88182683.146, 21535123.710, 21535126.276,
312  92040.000, 113129187.566, 88152601.975, 21527777.460, 21527780.140,
313  92070.000, 113090746.772, 88122648.119, 21520462.462, 21520465.203,
314  92100.000, 113052469.256, 88092821.492, 21513178.580, 21513181.303,
315  92130.000, 113014355.302, 88063122.312, 21505925.819, 21505928.587,
316  92160.000, 112976404.863, 88033550.550, 21498704.157, 21498706.834,
317  92190.000, 112938618.105, 88004106.333, 21491513.565, 21491516.157,
318  92220.000, 112900995.094, 87974789.711, 21484354.039, 21484356.666,
319  92250.000, 112863535.983, 87945600.800, 21477225.865, 21477228.455,
320  92280.000, 112826240.741, 87916539.580, 21470128.869, 21470131.307,
321  92310.000, 112789109.657, 87887606.275, 21463062.865, 21463065.249,
322  92340.000, 112752142.579, 87858800.764, 21456028.307, 21456030.867,
323  92370.000, 112715339.676, 87830123.182, 21449024.954, 21449027.516,
324  92400.000, 112678701.148, 87801573.685, 21442052.798, 21442055.506,
325  92430.000, 112642227.038, 87773152.310, 21435112.030, 21435114.730,
326  92460.000, 112605917.613, 87744859.255, 21428202.542, 21428205.274,
327  92490.000, 112569772.858, 87716694.518, 21421324.270, 21421327.162,
328  92520.000, 112533792.998, 87688658.276, 21414477.615, 21414480.316,
329  92550.000, 112497978.007, 87660750.497, 21407662.306, 21407664.857,
330  92580.000, 112462328.167, 87632971.407, 21400878.480, 21400880.871,
331  92610.000, 112426843.584, 87605321.095, 21394125.998, 21394128.499,
332  92640.000, 112391524.369, 87577799.634, 21387405.038, 21387407.470,
333  92670.000, 112356370.648, 87550407.134, 21380715.347, 21380717.906,
334  92700.000, 112321382.768, 87523143.866, 21374057.156, 21374059.959,
335  92730.000, 112286560.812, 87496009.881, 21367430.753, 21367433.701,
336  92760.000, 112251904.919, 87469005.297, 21360836.038, 21360838.734,
337  92790.000, 112217415.264, 87442130.254, 21354272.889, 21354275.654,
338  92820.000, 112183092.073, 87415384.912, 21347741.502, 21347744.082,
339  92850.000, 112148935.454, 87388769.371, 21341241.672, 21341244.205,
340  92880.000, 112114945.554, 87362283.735, 21334773.670, 21334775.995,
341  92910.000, 112081122.699, 87335928.274, 21328337.098, 21328339.912,
342  92940.000, 112047466.905, 87309702.988, 21321932.617, 21321935.429,
343  92970.000, 112013978.401, 87283608.051, 21315559.916, 21315562.776,
344  93000.000, 111980657.644, 87257643.835, 21309219.272, 21309222.100,
345  93030.000, 111947504.680, 87231810.357, 21302910.481, 21302913.319,
346  93060.000, 111914519.827, 87206107.882, 21296633.944, 21296636.434,
347  93090.000, 111881703.555, 87180536.766, 21290389.222, 21290391.554,
348  93120.000, 111849055.788, 87155096.951, 21284176.385, 21284179.135,
349  93150.000, 111816576.637, 87129788.526, 21277995.726, 21277998.368,
350  93180.000, 111784266.399, 87104611.717, 21271847.128, 21271850.023,
351  93210.000, 111752125.405, 87079566.789, 21265730.923, 21265733.884,
352  93240.000, 111720153.985, 87054654.002, 21259646.780, 21259649.701,
353  93270.000, 111688352.340, 87029873.499, 21253595.336, 21253598.047,
354  93300.000, 111656720.822, 87005225.566, 21247576.148, 21247578.888,
355  93330.000, 111625259.618, 86980710.347, 21241589.269, 21241592.014,
356  93360.000, 111593968.846, 86956327.929, 21235635.009, 21235637.435,
357  93390.000, 111562848.820, 86932078.559, 21229713.071, 21229715.418,
358  93420.000, 111531899.872, 86907962.498, 21223823.605, 21223826.111,
359  93450.000, 111501122.326, 86883979.998, 21217966.793, 21217969.318,
360  93480.000, 111470516.614, 86860131.396, 21212142.695, 21212145.240,
361  93510.000, 111440082.876, 86836416.799, 21206351.266, 21206353.976,
362  93540.000, 111409821.498, 86812836.512, 21200592.554, 21200595.478,
363  93570.000, 111379732.815, 86789390.789, 21194866.878, 21194869.848,
364  93600.000, 111349817.127, 86766079.863, 21189174.260, 21189176.956,
365  93630.000, 111320074.694, 86742903.952, 21183514.598, 21183517.105,
366  93660.000, 111290505.928, 86719863.355, 21177887.923, 21177890.342,
367  93690.000, 111261111.175, 86696958.359, 21172294.326, 21172296.677,
368  93720.000, 111231890.741, 86674189.190, 21166733.882, 21166736.273,
369  93750.000, 111202844.958, 86651556.117, 21161206.575, 21161209.165,
370  93780.000, 111173974.150, 86629059.388, 21155712.560, 21155715.346,
371  93810.000, 111145278.704, 86606699.305, 21150251.847, 21150254.706,
372  93840.000, 111116758.881, 86584476.068, 21144824.743, 21144827.599,
373  93870.000, 111088415.024, 86562389.953, 21139430.856, 21139433.950,
374  93900.000, 111060247.684, 86540441.387, 21134070.828, 21134073.782,
375  93930.000, 111032257.118, 86518630.556, 21128744.490, 21128747.223,
376  93960.000, 111004443.894, 86496957.922, 21123451.869, 21123454.554,
377  93990.000, 110976808.374, 86475423.754, 21118193.172, 21118195.560,
378  94020.000, 110949350.920, 86454028.337, 21112968.114, 21112970.533,
379  94050.000, 110922071.800, 86432771.883, 21107777.203, 21107779.570,
380  94080.000, 110894971.468, 86411654.746, 21102620.086, 21102622.612,
381  94110.000, 110868050.302, 86390677.216, 21097497.093, 21097499.741,
382  94140.000, 110841308.818, 86369839.698, 21092408.201, 21092410.983,
383  94170.000, 110814747.654, 86349142.694, 21087353.537, 21087356.444,
384  94200.000, 110788366.897, 86328586.264, 21082333.532, 21082336.543,
385  94230.000, 110762167.227, 86308170.939, 21077347.675, 21077350.904,
386  94260.000, 110736149.021, 86287897.012, 21072396.649, 21072399.812,
387  94290.000, 110710312.709, 86267764.823, 21067480.169, 21067483.530,
388  94320.000, 110684658.533, 86247774.558, 21062598.415, 21062601.583,
389  94350.000, 110659187.213, 86227926.778, 21057751.553, 21057754.404,
390  94380.000, 110633899.085, 86208221.742, 21052939.411, 21052942.317,
391  94410.000, 110608794.607, 86188659.809, 21048162.374, 21048165.139,
392  94440.000, 110583874.246, 86169241.344, 21043420.079, 21043422.978,
393  94470.000, 110559138.511, 86149966.749, 21038712.965, 21038715.931,
394  94500.000, 110534587.906, 86130836.411, 21034041.107, 21034044.104,
395  94530.000, 110510222.902, 86111850.689, 21029404.568, 21029407.465,
396  94560.000, 110486044.007, 86093009.999, 21024803.425, 21024806.266,
397  94590.000, 110462051.674, 86074314.674, 21020237.762, 21020240.679,
398  94620.000, 110438246.434, 86055765.144, 21015707.721, 21015710.540,
399  94650.000, 110414628.715, 86037361.730, 21011213.533, 21011216.221,
400  94680.000, 110391199.074, 86019104.866, 21006754.987, 21006757.699,
401  94710.000, 110367958.275, 86000995.153, 21002332.504, 21002335.372,
402  94740.000, 110344906.678, 85983032.867, 20997945.916, 20997948.755,
403  94770.000, 110322044.698, 85965218.344, 20993595.588, 20993598.238,
404  94800.000, 110299373.009, 85947552.093, 20989281.314, 20989284.080,
405  94830.000, 110276892.012, 85930034.437, 20985003.286, 20985006.131,
406  94860.000, 110254602.384, 85912665.897, 20980761.668, 20980764.490,
407  94890.000, 110232504.479, 85895446.751, 20976556.568, 20976559.329,
408  94920.000, 110210598.918, 85878377.490, 20972387.930, 20972390.935,
409  94950.000, 110188886.299, 85861458.561, 20968256.116, 20968259.150,
410  94980.000, 110167367.282, 85844690.506, 20964161.102, 20964164.220,
411  95010.000, 110146042.525, 85828073.809, 20960103.087, 20960106.188,
412  95040.000, 110124912.532, 85811608.883, 20956082.227, 20956085.324,
413  95070.000, 110103977.737, 85795296.053, 20952098.601, 20952101.562,
414  95100.000, 110083238.805, 85779135.839, 20948152.098, 20948154.980,
415  95130.000, 110062696.312, 85763128.700, 20944243.094, 20944245.866,
416  95160.000, 110042351.085, 85747275.280, 20940371.610, 20940374.331,
417  95190.000, 110022203.438, 85731575.818, 20936537.604, 20936540.446,
418  95220.000, 110002254.134, 85716030.904, 20932741.458, 20932744.261,
419  95250.000, 109982503.789, 85700641.032, 20928983.104, 20928985.850,
420  95280.000, 109962952.881, 85685406.559, 20925262.807, 20925265.444,
421  95310.000, 109943602.086, 85670328.024, 20921580.397, 20921583.054,
422  95340.000, 109924452.214, 85655406.050, 20917936.293, 20917938.909,
423  95370.000, 109905503.590, 85640640.892, 20914330.503, 20914333.147,
424  95400.000, 109886757.130, 85626033.261, 20910763.191, 20910765.780,
425  95430.000, 109868213.467, 85611583.657, 20907234.386, 20907236.940,
426  95460.000, 109849873.279, 85597292.607, 20903744.235, 20903747.044,
427  95490.000, 109831737.163, 85583160.580, 20900292.918, 20900295.778,
428  95520.000, 109813805.664, 85569187.988, 20896880.770, 20896883.547,
429  95550.000, 109796079.547, 85555375.430, 20893507.528, 20893510.376,
430  95580.000, 109778559.377, 85541723.350, 20890173.816, 20890176.496,
431  95610.000, 109761245.858, 85528232.298, 20886879.127, 20886881.669,
432  95640.000, 109744139.612, 85514902.753, 20883623.942, 20883626.499,
433  95670.000, 109727241.373, 85501735.296, 20880408.370, 20880410.797,
434  95700.000, 109710551.738, 85488730.387, 20877232.487, 20877234.994,
435  95730.000, 109694071.508, 85475888.647, 20874096.303, 20874098.644,
436  95760.000, 109677801.582, 85463210.774, 20871000.327, 20871002.763,
437  95790.000, 109661742.440, 85450697.160, 20867944.328, 20867946.910,
438  95820.000, 109645894.678, 85438348.251, 20864928.775, 20864931.177,
439  95850.000, 109630259.299, 85426164.834, 20861953.509, 20861955.872,
440  95880.000, 109614836.759, 85414147.272, 20859018.631, 20859021.207,
441  95910.000, 109599627.823, 85402296.151, 20856124.336, 20856127.045,
442  95940.000, 109584633.185, 85390612.011, 20853271.106, 20853273.621,
443  95970.000, 109569853.706, 85379095.526, 20850458.382, 20850461.188,
444  96000.000, 109555289.997, 85367747.165, 20847686.904, 20847689.876,
445  96030.000, 109540942.761, 85356567.497, 20844956.655, 20844959.536,
446  96060.000, 109526812.654, 85345557.021, 20842267.720, 20842270.751,
447  96090.000, 109512900.582, 85334716.438, 20839620.370, 20839623.366,
448  96120.000, 109499207.330, 85324046.365, 20837014.626, 20837017.768,
449  96150.000, 109485733.578, 85313547.338, 20834450.633, 20834453.734,
450  96180.000, 109472480.034, 85303219.900, 20831928.441, 20831931.542,
451  96210.000, 109459447.535, 85293064.698, 20829448.559, 20829451.644,
452  96240.000, 109446636.731, 85283082.248, 20827010.745, 20827013.872,
453  96270.000, 109434048.379, 85273273.137, 20824615.327, 20824618.283,
454  96300.000, 109421683.332, 85263638.035, 20822262.347, 20822265.321,
455  96330.000, 109409542.421, 85254177.575, 20819952.145, 20819954.942,
456  96360.000, 109397626.292, 85244892.273, 20817684.551, 20817687.331,
457  96390.000, 109385935.793, 85235782.796, 20815459.928, 20815462.687,
458  96420.000, 109374471.793, 85226849.809, 20813278.447, 20813281.191,
459  96450.000, 109363234.881, 85218093.777, 20811140.273, 20811142.957,
460  96480.000, 109352225.954, 85209515.395, 20809045.329, 20809048.128,
461  96510.000, 109341445.659, 85201115.161, 20806993.942, 20806996.523,
462  96540.000, 109330894.979, 85192893.845, 20804986.146, 20804988.912,
463  96570.000, 109320574.521, 85184851.933, 20803022.077, 20803024.893,
464  96600.000, 109310485.241, 85176990.153, 20801102.421, 20801104.998,
465  96630.000, 109300627.847, 85169309.069, 20799226.634, 20799229.009,
466  96660.000, 109291003.254, 85161809.379, 20797395.107, 20797397.544,
467  96690.000, 109281612.216, 85154491.676, 20795608.107, 20795610.568,
468  96720.000, 109272455.567, 85147356.619, 20793865.832, 20793868.097,
469  96750.000, 109263534.001, 85140404.735, 20792168.113, 20792170.461,
470  96780.000, 109254848.443, 85133636.760, 20790515.338, 20790517.633,
471  96810.000, 109246399.711, 85127053.322, 20788907.399, 20788909.924,
472  96840.000, 109238188.729, 85120655.145, 20787344.912, 20787347.545,
473  96870.000, 109230216.267, 85114442.828, 20785828.009, 20785830.448,
474  96900.000, 109222483.008, 85108416.907, 20784356.325, 20784358.773,
475  96930.000, 109214990.074, 85102578.254, 20782930.392, 20782932.857,
476  96960.000, 109207738.149, 85096927.398, 20781550.488, 20781552.965,
477  96990.000, 109200727.842, 85091464.818, 20780216.302, 20780218.921,
478  97020.000, 109193960.322, 85086191.425, 20778928.553, 20778931.155,
479  97050.000, 109187436.004, 85081107.531, 20777687.000, 20777689.618,
480  97080.000, 109181156.050, 85076214.052, 20776491.938, 20776494.562,
481  97110.000, 109175121.429, 85071511.743, 20775343.657, 20775346.148,
482  97140.000, 109169332.785, 85067001.111, 20774242.176, 20774244.645,
483  97170.000, 109163790.927, 85062682.771, 20773187.497, 20773190.109,
484  97200.000, 109158496.855, 85058557.519, 20772180.083, 20772182.651,
485  97230.000, 109153451.498, 85054626.072, 20771220.026, 20771222.547,
486  97260.000, 109148655.350, 85050888.810, 20770307.261, 20770309.894,
487  97290.000, 109144109.624, 85047346.686, 20769442.209, 20769444.919,
488  97320.000, 109139814.924, 85044000.168, 20768624.917, 20768627.623,
489  97350.000, 109135772.143, 85040849.941, 20767855.654, 20767858.365,
490  97380.000, 109131982.196, 85037896.750, 20767134.486, 20767137.139,
491  97410.000, 109128446.088, 85035141.370, 20766461.435, 20766464.168,
492  97440.000, 109125164.668, 85032584.428, 20765836.873, 20765839.679,
493  97470.000, 109122138.479, 85030226.370, 20765261.039, 20765263.701,
494  97500.000, 109119368.766, 85028068.153, 20764733.979, 20764736.519,
495  97530.000, 109116856.119, 85026110.254, 20764256.036, 20764258.537,
496  97560.000, 109114601.741, 85024353.612, 20763826.936, 20763829.579,
497  97590.000, 109112606.160, 85022798.624, 20763447.185, 20763449.809,
498  97620.000, 109110870.370, 85021446.066, 20763116.717, 20763119.675,
499  97650.000, 109109395.087, 85020296.479, 20762836.008, 20762838.817,
500  97680.000, 109108181.513, 85019350.830, 20762605.274, 20762607.800,
501  97710.000, 109107230.269, 85018609.581, 20762424.195, 20762426.804,
502  97740.000, 109106542.425, 85018073.581, 20762293.342, 20762295.928,
503  97770.000, 109106118.778, 85017743.477, 20762212.570, 20762215.370,
504  97800.000, 109105960.000, 85017619.746, 20762182.311, 20762185.200,
505  97830.000, 109106067.160, 85017703.245, 20762202.946, 20762205.544,
506  97860.000, 109106441.228, 85017994.735, 20762274.075, 20762276.761,
507  97890.000, 109107082.817, 85018494.678, 20762396.233, 20762398.896,
508  97920.000, 109107992.921, 85019203.858, 20762569.458, 20762572.042,
509  97950.000, 109109172.573, 85020123.062, 20762793.866, 20762796.475,
510  97980.000, 109110622.537, 85021252.881, 20763069.746, 20763072.442,
511  98010.000, 109112343.831, 85022594.140, 20763397.178, 20763399.829,
512  98040.000, 109114337.141, 85024147.368, 20763776.530, 20763779.212,
513  98070.000, 109116603.456, 85025913.316, 20764207.780, 20764210.544,
514  98100.000, 109119143.524, 85027892.587, 20764691.263, 20764694.042,
515  98130.000, 109121958.449, 85030086.038, 20765226.956, 20765229.636,
516  98160.000, 109125048.762, 85032494.079, 20765815.021, 20765817.758,
517  98190.000, 109128415.760, 85035117.726, 20766455.660, 20766458.457,
518  98220.000, 109132059.905, 85037957.330, 20767149.096, 20767151.986,
519  98250.000, 109135982.331, 85041013.774, 20767895.593, 20767898.398,
520  98280.000, 109140183.880, 85044287.707, 20768695.049, 20768697.889,
521  98310.000, 109144665.348, 85047779.761, 20769547.901, 20769550.627,
522  98340.000, 109149427.542, 85051490.567, 20770454.124, 20770456.779,
523  98370.000, 109154471.525, 85055420.940, 20771413.976, 20771416.727,
524  98400.000, 109159798.139, 85059571.545, 20772427.484, 20772430.360,
525  98430.000, 109165408.043, 85063942.899, 20773495.093, 20773497.852,
526  98460.000, 109171302.206, 85068535.756, 20774616.626, 20774619.365,
527  98490.000, 109177481.554, 85073350.831, 20775792.640, 20775795.357,
528  98520.000, 109183946.896, 85078388.752, 20777022.939, 20777025.476,
529  98550.000, 109190699.303, 85083650.368, 20778307.806, 20778310.563,
530  98580.000, 109197739.412, 85089136.163, 20779647.522, 20779650.339,
531  98610.000, 109205068.150, 85094846.849, 20781042.071, 20781044.889,
532  98640.000, 109212686.398, 85100783.135, 20782491.778, 20782494.474,
533  98670.000, 109220595.060, 85106945.727, 20783996.704, 20783999.485,
534  98700.000, 109228795.016, 85113335.305, 20785557.178, 20785559.919,
535  98730.000, 109237286.999, 85119952.426, 20787173.156, 20787175.977,
536  98760.000, 109246072.078, 85126797.936, 20788844.840, 20788847.775,
537  98790.000, 109255150.911, 85133872.345, 20790572.563, 20790575.378,
538  98820.000, 109264524.499, 85141176.433, 20792356.264, 20792359.102,
539  98850.000, 109274193.292, 85148710.554, 20794196.331, 20794199.058,
540  98880.000, 109284158.577, 85156475.701, 20796092.602, 20796095.354,
541  98910.000, 109294420.973, 85164472.372, 20798045.480, 20798048.228,
542  98940.000, 109304981.478, 85172701.330, 20800055.026, 20800057.787,
543  98970.000, 109315840.742, 85181163.085, 20802121.464, 20802124.213,
544  99000.000, 109326999.618, 85189858.306, 20804244.964, 20804247.806,
545  99030.000, 109338458.963, 85198787.661, 20806425.533, 20806428.522,
546  99060.000, 109350219.664, 85207951.842, 20808663.663, 20808666.488,
547  99090.000, 109362282.488, 85217351.439, 20810959.197, 20810961.947,
548  99120.000, 109374648.216, 85226987.072, 20813312.295, 20813314.999,
549  99150.000, 109387317.866, 85236859.531, 20815723.103, 20815725.986,
550  99180.000, 109400292.229, 85246969.423, 20818191.983, 20818194.901,
551  99210.000, 109413571.982, 85257317.281, 20820719.070, 20820722.090,
552  99240.000, 109427157.924, 85267903.725, 20823304.453, 20823307.274,
553  99270.000, 109441050.894, 85278729.409, 20825948.160, 20825951.036,
554  99300.000, 109455251.852, 85289795.083, 20828650.531, 20828653.496,
555  99330.000, 109469761.440, 85301101.246, 20831411.650, 20831414.598,
556  99360.000, 109484580.478, 85312648.537, 20834231.672, 20834234.554,
557  99390.000, 109499709.760, 85324437.579, 20837110.685, 20837113.482,
558  99420.000, 109515150.224, 85336469.106, 20840048.833, 20840051.689,
559  99450.000, 109530902.517, 85348743.611, 20843046.412, 20843049.357,
560  99480.000, 109546967.282, 85361261.600, 20846103.532, 20846106.398,
561  99510.000, 109563345.534, 85374023.866, 20849220.037, 20849223.022,
562  99540.000, 109580038.021, 85387030.991, 20852396.677, 20852399.551,
563  99570.000, 109597045.352, 85400283.442, 20855633.025, 20855636.021,
564  99600.000, 109614368.450, 85413781.951, 20858929.493, 20858932.310,
565  99630.000, 109632007.869, 85427526.946, 20862286.158, 20862289.142,
566  99660.000, 109649964.550, 85441519.157, 20865703.301, 20865706.233,
567  99690.000, 109668239.039, 85455759.010, 20869180.989, 20869183.587,
568  99720.000, 109686832.394, 85470247.319, 20872719.074, 20872721.906,
569  99750.000, 109705745.186, 85484984.544, 20876317.965, 20876320.907,
570  99780.000, 109724978.056, 85499971.191, 20879977.708, 20879980.793,
571  99810.000, 109744532.007, 85515208.049, 20883698.898, 20883701.810,
572  99840.000, 109764407.486, 85530695.419, 20887481.060, 20887484.120,
573  99870.000, 109784605.522, 85546434.135, 20891324.603, 20891327.514,
574  99900.000, 109805126.766, 85562424.709, 20895229.527, 20895232.680,
575  99930.000, 109825971.656, 85578667.470, 20899196.224, 20899199.214,
576  99960.000, 109847141.017, 85595163.072, 20903224.729, 20903227.704,
577  99990.000, 109868635.892, 85611912.339, 20907315.031, 20907318.067,
578  100020.000, 109890456.614, 85628915.486, 20911467.590, 20911470.379,
579  100050.000, 109912603.869, 85646173.073, 20915681.958, 20915684.735,
580  100080.000, 109935078.478, 85663685.749, 20919958.823, 20919961.534,
581  100110.000, 109957880.998, 85681453.944, 20924297.722, 20924300.794,
582  100140.000, 109981012.168, 85699478.224, 20928699.519, 20928702.398,
583  100170.000, 110004472.883, 85717759.304, 20933163.999, 20933166.777,
584  100200.000, 110028263.493, 85736297.439, 20937691.185, 20937694.098,
585  100230.000, 110052384.783, 85755093.246, 20942281.289, 20942284.144,
586  100260.000, 110076837.257, 85774147.117, 20946934.417, 20946937.322,
587  100290.000, 110101621.623, 85793459.608, 20951650.852, 20951653.705,
588  100320.000, 110126738.613, 85813031.285, 20956430.475, 20956433.242,
589  100350.000, 110152188.852, 85832862.635, 20961273.462, 20961276.377,
590  100380.000, 110177972.883, 85852954.093, 20966179.937, 20966182.833,
591  100410.000, 110204091.348, 85873306.141, 20971150.116, 20971152.976,
592  100440.000, 110230544.843, 85893919.246, 20976184.173, 20976186.876,
593  100470.000, 110257333.974, 85914793.891, 20981281.848, 20981284.686,
594  100500.000, 110284459.261, 85935930.464, 20986443.651, 20986446.476,
595  100530.000, 110311921.449, 85957329.572, 20991669.461, 20991672.516,
596  100560.000, 110339721.184, 85978991.709, 20996959.593, 20996962.583,
597  100590.000, 110367858.770, 86000917.100, 21002314.012, 21002317.088,
598  100620.000, 110396334.895, 86023106.277, 21007732.906, 21007735.932,
599  100650.000, 110425150.149, 86045559.719, 21013216.345, 21013219.328,
600  100680.000, 110454305.070, 86068277.834, 21018764.239, 21018767.347,
601  100710.000, 110483800.298, 86091261.121, 21024377.053, 21024380.176,
602  100740.000, 110513636.175, 86114509.847, 21030054.630, 21030057.789,
603  100770.000, 110543813.328, 86138024.511, 21035797.268, 21035800.296,
604  100800.000, 110574332.241, 86161805.476, 21041604.724, 21041607.820,
605  100830.000, 110605193.504, 86185853.204, 21047477.436, 21047480.564,
606  100860.000, 110636397.414, 86210167.931, 21053415.327, 21053418.446,
607  100890.000, 110667944.783, 86234750.292, 21059418.614, 21059421.660,
608  100920.000, 110699836.022, 86259600.589, 21065487.227, 21065490.445,
609  100950.000, 110732071.387, 86284719.039, 21071621.445, 21071624.552,
610  100980.000, 110764651.717, 86310106.298, 21077821.208, 21077824.337,
611  101010.000, 110797577.200, 86335762.519, 21084086.707, 21084089.793,
612  101040.000, 110830848.736, 86361688.369, 21090418.155, 21090421.324,
613  101070.000, 110864466.467, 86387883.997, 21096815.457, 21096818.626,
614  101100.000, 110898430.860, 86414349.740, 21103278.663, 21103281.745,
615  101130.000, 110932742.345, 86441085.956, 21109807.883, 21109810.948,
616  101160.000, 110967401.229, 86468092.869, 21116403.263, 21116406.294,
617  101190.000, 111002408.098, 86495370.947, 21123064.795, 21123067.839,
618  101220.000, 111037763.272, 86522920.428, 21129792.750, 21129795.734,
619  101250.000, 111073467.398, 86550741.812, 21136586.912, 21136590.171,
620  101280.000, 111109520.655, 86578835.256, 21143447.580, 21143450.757,
621  101310.000, 111145923.508, 86607201.118, 21150374.798, 21150377.863,
622  101340.000, 111182676.251, 86635839.609, 21157368.650, 21157371.744,
623  101370.000, 111219779.249, 86664751.025, 21164429.336, 21164432.288,
624  101400.000, 111257232.918, 86693935.687, 21171556.630, 21171559.413,
625  101430.000, 111295037.606, 86723393.883, 21178750.469, 21178753.482,
626  101460.000, 111333193.600, 86753125.819, 21186011.330, 21186014.417,
627  101490.000, 111371701.296, 86783131.813, 21193339.203, 21193342.181,
628  101520.000, 111410560.993, 86813412.083, 21200733.985, 21200736.952,
629  101550.000, 111449772.931, 86843966.818, 21208195.866, 21208198.734,
630  101580.000, 111489337.665, 86874796.478, 21215724.648, 21215727.649,
631  101610.000, 111529255.256, 86905901.084, 21223320.847, 21223323.743,
632  101640.000, 111569525.970, 86937280.865, 21230984.088, 21230987.098,
633  101670.000, 111610150.317, 86968936.193, 21238714.620, 21238717.398,
634  101700.000, 111651128.234, 87000867.029, 21246512.315, 21246515.401,
635  101730.000, 111692460.194, 87033073.743, 21254377.514, 21254380.402,
636  101760.000, 111734146.493, 87065556.573, 21262310.171, 21262313.067,
637  101790.000, 111776187.381, 87098315.698, 21270310.435, 21270313.200,
638  101820.000, 111818582.744, 87131351.037, 21278377.820, 21278380.961,
639  101850.000, 111861333.123, 87164663.009, 21286513.028, 21286515.976,
640  101880.000, 111904438.833, 87198251.863, 21294715.793, 21294718.866,
641  101910.000, 111947899.881, 87232117.613, 21302986.142, 21302989.146,
642  101940.000, 111991716.368, 87266260.320, 21311324.228, 21311327.130,
643  101970.000, 112035888.650, 87300680.269, 21319729.934, 21319732.996,
644  102000.000, 112080416.600, 87335377.353, 21328203.263, 21328206.454,
645  102030.000, 112125300.597, 87370351.876, 21336744.360, 21336747.598,
646  102060.000, 112170540.874, 87405604.027, 21345353.484, 21345356.654,
647  102090.000, 112216137.508, 87441133.849, 21354030.141, 21354033.341,
648  102120.000, 112262090.693, 87476941.507, 21362774.946, 21362778.128,
649  102150.000, 112308400.334, 87513026.931, 21371587.354, 21371590.456,
650  102180.000, 112355066.744, 87549390.347, 21380467.535, 21380470.683,
651  102210.000, 112402090.035, 87586031.855, 21389415.858, 21389419.045,
652  102240.000, 112449470.149, 87622951.407, 21398431.982, 21398435.003,
653  102270.000, 112497207.369, 87660149.211, 21407516.078, 21407519.167,
654  102300.000, 112545301.413, 87697625.077, 21416668.162, 21416670.956,
655  102330.000, 112593752.666, 87735379.287, 21425888.119, 21425890.923,
656  102360.000, 112642561.200, 87773411.911, 21435176.042, 21435179.110,
657  102390.000, 112691726.794, 87811722.741, 21444531.854, 21444534.961,
658  102420.000, 112741249.673, 87850311.982, 21453955.780, 21453958.955,
659  102450.000, 112791129.810, 87889179.607, 21463447.886, 21463450.732,
660  102480.000, 112841367.130, 87928325.558, 21473007.657, 21473010.463,
661  102510.000, 112891961.636, 87967749.830, 21482635.479, 21482638.306,
662  102540.000, 112942913.318, 88007452.424, 21492331.170, 21492334.218,
663  102570.000, 112994222.068, 88047433.239, 21502094.960, 21502098.082,
664  102600.000, 113045887.887, 88087692.291, 21511926.576, 21511929.740,
665  102630.000, 113097910.738, 88128229.560, 21521826.217, 21521829.432,
666  102660.000, 113150290.545, 88169044.972, 21531793.947, 21531797.203,
667  102690.000, 113203027.442, 88210138.617, 21541829.418, 21541832.548,
668  102720.000, 113256121.099, 88251510.287, 21551932.819, 21551935.839,
669  102750.000, 113309571.677, 88293160.073, 21562103.963, 21562107.179,
670  102780.000, 113363378.689, 88335087.601, 21572343.278, 21572346.438,
671  102810.000, 113417542.229, 88377292.935, 21582650.286, 21582653.359,
672  102840.000, 113472062.016, 88419775.855, 21593024.954, 21593028.296,
673  102870.000, 113526938.032, 88462536.373, 21603467.429, 21603470.934,
674  102900.000, 113582170.201, 88505574.402, 21613977.732, 21613981.396,
675  102930.000, 113637758.298, 88548889.784, 21624555.873, 21624559.295,
676  102960.000, 113693702.069, 88592482.311, 21635201.933, 21635205.150,
677  102990.000, 113750001.400, 88636351.895, 21645915.341, 21645918.490,
678  103020.000, 113806655.950, 88680498.258, 21656696.238, 21656699.563,
679  103050.000, 113863665.995, 88724921.646, 21667545.066, 21667548.167,
680  103080.000, 113921030.918, 88769621.555, 21678461.070, 21678464.428,
681  103110.000, 113978750.804, 88814598.071, 21689444.816, 21689448.058,
682  103140.000, 114036825.146, 88859850.784, 21700495.942, 21700499.197,
683  103170.000, 114095253.894, 88905379.661, 21711614.747, 21711617.745,
684  103200.000, 114154036.576, 88951184.316, 21722800.727, 21722803.816,
685  103230.000, 114213172.964, 88997264.623, 21734053.935, 21734057.185,
686  103260.000, 114272662.684, 89043620.231, 21745374.526, 21745377.675,
687  103290.000, 114332505.441, 89090250.931, 21756762.180, 21756765.541,
688  103320.000, 114392701.028, 89137156.561, 21768217.345, 21768220.445,
689  103350.000, 114453249.015, 89184336.802, 21779739.256, 21779742.436,
690  103380.000, 114514149.046, 89231791.341, 21791328.043, 21791331.273,
691  103410.000, 114575400.955, 89279520.065, 21802983.830, 21802987.169,
692  103440.000, 114637004.264, 89327522.617, 21814706.246, 21814709.785,
693  103470.000, 114698958.673, 89375798.742, 21826495.967, 21826499.333,
694  103500.000, 114761263.729, 89424348.101, 21838352.302, 21838355.652,
695  103530.000, 114823919.190, 89473170.521, 21850275.463, 21850278.669,
696  103560.000, 114886924.511, 89522265.529, 21862265.035, 21862267.988,
697  103590.000, 114950279.438, 89571632.987, 21874321.156, 21874324.059,
698  103620.000, 115013983.531, 89621272.493, 21886443.434, 21886446.521,
699  103650.000, 115078036.106, 89671183.565, 21898632.199, 21898635.476,
700  103680.000, 115142436.994, 89721366.034, 21910887.387, 21910890.602,
701  103710.000, 115207185.511, 89771819.377, 21923208.735, 21923211.928,
702  103740.000, 115272281.374, 89822543.403, 21935596.118, 21935599.372,
703  103770.000, 115337723.884, 89873537.537, 21948049.593, 21948052.401,
704  103800.000, 115403512.669, 89924801.518, 21960568.478, 21960571.605,
705  103830.000, 115469647.184, 89976334.874, 21973153.439, 21973156.850,
706  103860.000, 115536126.950, 90028137.247, 21985804.160, 21985807.526,
707  103890.000, 115602951.375, 90080208.188, 21998520.642, 21998523.808,
708  103920.000, 115670119.893, 90132547.271, 22011302.423, 22011305.668,
709  103950.000, 115737631.984, 90185154.070, 22024149.551, 22024152.728,
710  103980.000, 115805487.185, 90238028.223, 22037061.847, 22037065.188,
711  104010.000, 115873684.886, 90291169.263, 22050039.400, 22050042.784,
712  104040.000, 115942224.581, 90344576.774, 22063082.187, 22063085.762,
713  104070.000, 116011105.588, 90398250.254, 22076189.833, 22076193.378,
714  104100.000, 116080327.358, 90452189.257, 22089362.246, 22089365.925,
715  104130.000, 116149889.042, 90506393.123, 22102599.595, 22102602.869,
716  104160.000, 116219790.221, 90560861.535, 22115901.048, 22115904.681,
717  104190.000, 116290030.303, 90615594.020, 22129267.395, 22129270.808,
718  104220.000, 116360608.494, 90670589.955, 22142697.965, 22142701.238,
719  104250.000, 116431524.208, 90725848.919, 22156193.254, 22156196.197,
720  104280.000, 116502776.622, 90781370.232, 22169752.265, 22169755.370,
721  104310.000, 116574365.188, 90837153.487, 22183375.185, 22183378.306,
722  104340.000, 116646289.121, 90893198.077, 22197062.028, 22197064.997,
723  104370.000, 116718547.899, 90949503.596, 22210812.219, 22210815.542,
724  104400.000, 116791140.542, 91006069.256, 22224625.817, 22224629.535,
725  104430.000, 116864066.500, 91062894.640, 22238503.046, 22238506.523,
726  104460.000, 116937325.086, 91119979.217, 22252443.796, 22252447.254,
727  104490.000, 117010915.216, 91177322.147, 22266447.284, 22266451.134,
728  104520.000, 117084836.404, 91234923.044, 22280514.263, 22280517.835,
729  104550.000, 117159087.702, 91292781.186, 22294643.904, 22294647.425,
730  104580.000, 117233668.268, 91350895.888, 22308836.429, 22308839.759,
731  104610.000, 117308577.262, 91409266.514, 22323090.559, 22323094.280,
732  104640.000, 117383813.945, 91467892.475, 22337407.770, 22337411.575,
733  104670.000, 117459377.518, 91526773.148, 22351787.185, 22351791.053,
734  104700.000, 117535267.116, 91585907.861, 22366228.878, 22366232.398,
735  104730.000, 117611482.006, 91645296.075, 22380732.025, 22380735.972,
736  104760.000, 117688021.230, 91704937.000, 22395297.095, 22395300.830,
737  104790.000, 117764883.892, 91764829.962, 22409923.596, 22409927.214,
738  104820.000, 117842069.205, 91824974.330, 22424611.664, 22424615.207,
739  104850.000, 117919576.096, 91885369.290, 22439360.623, 22439363.996,
740  104880.000, 117997403.895, 91946014.304, 22454170.995, 22454174.302,
741  104910.000, 118075551.573, 92006908.570, 22469041.932, 22469045.101,
742  104940.000, 118154018.188, 92068051.341, 22483973.178, 22483976.482,
743  104970.000, 118232802.690, 92129441.832, 22498965.414, 22498968.868,
744  105000.000, 118311904.196, 92191079.332, 22514018.299, 22514021.454,
745  105030.000, 118391321.710, 92252963.090, 22529130.550, 22529133.785,
746  105060.000, 118471054.437, 92315092.441, 22544303.273, 22544306.380,
747  105090.000, 118551101.233, 92377466.530, 22559535.894, 22559538.826,
748  105120.000, 118631461.104, 92440084.581, 22574828.002, 22574831.235,
749  105150.000, 118712133.284, 92502945.990, 22590179.491, 22590182.564,
750  105180.000, 118793116.470, 92566049.749, 22605589.917, 22605593.139,
751  105210.000, 118874409.752, 92629395.149, 22621059.709, 22621063.120,
752  105240.000, 118956012.046, 92692981.324, 22636587.987, 22636591.490,
753  105270.000, 119037922.364, 92756807.497, 22652174.888, 22652178.791,
754  105300.000, 119120139.791, 92820872.992, 22667820.286, 22667824.137,
755  105330.000, 119202663.242, 92885176.950, 22683524.026, 22683527.968,
756  105360.000, 119285491.432, 92949718.350, 22699285.935, 22699289.588,
757  105390.000, 119368623.385, 93014496.460, 22715105.128, 22715109.193,
758  105420.000, 119452057.851, 93079510.283, 22730982.011, 22730986.228,
759  105450.000, 119535793.880, 93144759.089, 22746916.382, 22746920.655,
760  105480.000, 119619830.233, 93210241.942, 22762908.533, 22762912.078,
761  105510.000, 119704165.797, 93275957.927, 22778956.702, 22778960.730,
762  105540.000, 119788799.503, 93341906.216, 22795062.394, 22795066.010,
763  105570.000, 119873730.237, 93408085.957, 22811224.160, 22811228.091,
764  105600.000, 119958956.609, 93474496.081, 22827442.312, 22827446.041,
765  105630.000, 120044477.590, 93541135.765, 22843716.498, 22843720.231,
766  105660.000, 120130292.064, 93608004.154, 22860046.378, 22860050.214,
767  105690.000, 120216398.939, 93675100.397, 22876432.237, 22876435.798,
768  105720.000, 120302796.756, 93742423.349, 22892872.846, 22892876.875,
769  105750.000, 120389484.356, 93809972.099, 22909369.159, 22909372.825,
770  105780.000, 120476460.647, 93877745.788, 22925919.629, 22925924.059,
771  105810.000, 120563724.317, 93945743.411, 22942525.983, 22942529.615,
772  105840.000, 120651274.005, 94013963.926, 22959185.512, 22959189.907,
773  105870.000, 120739108.732, 94082406.536, 22975900.455, 22975904.428,
774  105900.000, 120827226.873, 94151069.982, 22992668.411, 22992673.112,
775  105930.000, 120915627.454, 94219953.520, 23009490.589, 23009495.055,
776  105960.000, 121004309.103, 94289056.073, 23026366.407, 23026370.254,
777  105990.000, 121093270.555, 94358376.645, 23043295.421, 23043299.091,
778  106020.000, 121182510.300, 94427914.079, 23060277.338, 23060281.034,
779  106050.000, 121272027.133, 94497667.431, 23077311.680, 23077315.484,
780  106080.000, 121361819.829, 94567635.716, 23094398.459, 23094402.478,
781  106110.000, 121451886.960, 94637817.860, 23111537.689, 23111541.905,
782  106140.000, 121542227.259, 94708212.856, 23128729.487, 23128733.002,
783  106170.000, 121632839.332, 94778819.647, 23145972.052, 23145975.879,
784  106200.000, 121723721.880, 94849637.179, 23163266.221, 23163270.247,
785  106230.000, 121814873.529, 94920664.385, 23180611.413, 23180616.129,
786  106260.000, 121906292.932, 94991900.248, 23198008.391, 23198012.546,
787  106290.000, 121997978.681, 95063343.652, 23215455.758, 23215459.829,
788  106320.000, 122089929.310, 95134993.462, 23232953.263, 23232957.531,
789  106350.000, 122182143.299, 95206848.462, 23250501.280, 23250505.506,
790  106380.000, 122274619.605, 95278907.893, 23268099.429, 23268102.842,
791  106410.000, 122367356.599, 95351170.466, 23285746.244, 23285750.139,
792  106440.000, 122460352.870, 95423635.059, 23303442.603, 23303447.066,
793  106470.000, 122553607.025, 95496300.602, 23321188.435, 23321192.764,
794  106500.000, 122647117.549, 95569165.915, 23338983.207, 23338987.285,
795  106530.000, 122740882.974, 95642229.847, 23356826.164, 23356830.194,
796  106560.000, 122834901.784, 95715491.220, 23374716.887, 23374721.279,
797  106590.000, 122929172.576, 95788948.955, 23392656.152, 23392660.459,
798  106620.000, 123023693.984, 95862601.983, 23410643.358, 23410647.006,
799  106650.000, 123118464.274, 95936448.951, 23428677.668, 23428681.580,
800  106680.000, 123213481.945, 96010488.686, 23446758.750, 23446763.010,
801  106710.000, 123308745.531, 96084720.028, 23464886.324, 23464890.807,
802  106740.000, 123404253.569, 96159141.844, 23483061.364, 23483065.326,
803  106770.000, 123500004.455, 96233752.911, 23501282.329, 23501286.471,
804  106800.000, 123595996.808, 96308552.127, 23519549.200, 23519553.281,
805  106830.000, 123692228.995, 96383538.220, 23537861.589, 23537865.817,
806  106860.000, 123788699.389, 96458709.945, 23556218.545, 23556223.738,
807  106890.000, 123885406.533, 96534066.122, 23574621.453, 23574626.334,
808  106920.000, 123982348.826, 96609605.536, 23593069.261, 23593073.500,
809  106950.000, 124079524.821, 96685327.089, 23611561.091, 23611565.473,
810  106980.000, 124176932.689, 96761229.287, 23630097.713, 23630102.034,
811  107010.000, 124274570.939, 96837311.001, 23648677.853, 23648681.542,
812  107040.000, 124372438.055, 96913571.070, 23667301.103, 23667304.938,
813  107070.000, 124470532.653, 96990008.405, 23685967.866, 23685972.133,
814  107100.000, 124568852.655, 97066621.349, 23704676.985, 23704681.264,
815  107130.000, 124667396.621, 97143408.830, 23723429.739, 23723433.618,
816  107160.000, 124766162.852, 97220369.504, 23742224.731, 23742228.386,
817  107190.000, 124865149.824, 97297502.178, 23761061.641, 23761065.073,
818  107220.000, 124964355.816, 97374805.519, 23779939.186, 23779943.183,
819  107250.000, 125063779.360, 97452278.367, 23798859.193, 23798862.912,
820  107280.000, 125163418.792, 97529919.455, 23817819.711, 23817823.943,
821  107310.000, 125263272.466, 97607727.507, 23836820.750, 23836825.416,
822  107340.000, 125363338.626, 97685701.086, 23855863.303, 23855868.027,
823  107370.000, 125463615.473, 97763838.852, 23874944.589, 23874950.004,
824  107400.000, 125564101.347, 97842139.497, 23894067.164, 23894071.664,
825  107430.000, 125664794.765, 97920601.845, 23913228.466, 23913232.434,
826  107460.000, 125765693.724, 97999224.356, 23932429.411, 23932432.795,
827  107490.000, 125866796.838, 98078005.980, 23951667.779, 23951671.945,
828  107520.000, 125968102.099, 98156945.101, 23970946.045, 23970949.778,
829  107550.000, 126069608.027, 98236040.592, 23990261.658, 23990266.709,
830  107580.000, 126171312.865, 98315291.095, 24009616.133, 24009620.745,
831  107610.000, 126273214.778, 98394695.165, 24029007.641, 24029011.505,
832  107670.000, 126477602.735, 98553958.498, 24067901.128, 24067904.830,
833  107700.000, 126580085.328, 98633815.056, 24087402.625, 24087407.188,
834  107730.000, 126682758.084, 98713819.776, 24106940.049, 24106944.927,
835  107760.000, 126785619.162, 98793971.231, 24126515.207, 24126518.730,
836  107790.000, 126888666.862, 98874268.154, 24146124.132, 24146127.789,
837  107820.000, 126991899.422, 98954709.105, 24165768.415, 24165773.258,
838  107880.000, 127198911.969, 99116017.602, 24205160.934, 24205166.891,
839  107910.000, 127302688.288, 99196882.263, 24224908.769, 24224913.950,
840  107940.000, 127406642.293, 99277885.403, 24244691.128, 24244696.492,
841  107970.000, 127510772.168, 99359025.596, 24264506.707, 24264511.340,
842  108000.000, 127615075.765, 99440301.138, 24284354.303, 24284358.972,
843  108060.000, 127824197.777, 99603253.364, 24324149.748, 24324154.054,
844  108090.000, 127929012.405, 99684927.098, 24344093.917, 24344099.457,
845  108180.000, 128244449.308, 99930721.989, 24404121.328, 24404125.347,
846  108210.000, 128349919.628, 100012906.642, 24424190.437, 24424195.906,
847  108240.000, 128455549.461, 100095215.550, 24444291.273, 24444296.146,
848  108270.000, 128561336.767, 100177647.197, 24464422.282, 24464426.916,
849  108300.000, 128667279.658, 100260200.104, 24484583.446, 24484587.304,
850  108330.000, 128773376.298, 100342872.808, 24504772.689, 24504776.554,
851  108360.000, 128879624.939, 100425663.935, 24524990.946, 24524994.743,
852  108420.000, 129092570.222, 100591595.281, 24565512.452, 24565517.667,
853  108450.000, 129199263.176, 100674732.620, 24585816.386, 24585819.277,
854  108480.000, 129306100.499, 100757982.489, 24606145.979, 24606149.902,
855  108540.000, 129520200.790, 100924813.902, 24646889.215, 24646892.419,
856  108570.000, 129627459.842, 101008392.409, 24667298.694, 24667303.302,
857  108600.000, 129734855.706, 101092077.499, 24687735.887, 24687740.707,
858  };
859 
861  //double sow[M],L1[M],L2[M],P1[M],P2[M];
862  //for (i=0; i<M; i++)
863  //{
864  // sow[i] = data[C*i];
865  // L1[i] = data[C*i+1];
866  // L2[i] = data[C*i+2];
867  // P1[i] = data[C*i+3];
868  // P2[i] = data[C*i+4];
869  // cout << " " << setw(3) << i << fixed << setprecision(3)
870  // << " " << setw(6) << data[C*i]
871  // << " L1=" << setw(13) << data[C*i+1]
872  // << " L2=" << setw(13) << data[C*i+2]
873  // << " P1=" << setw(13) << data[C*i+3]
874  // << " P2=" << setw(13) << data[C*i+4]
875  // << endl;
876  //}
877 
878  int ncount(0); // count errors, to be returned
879 
880  const double wl1=L1_WAVELENGTH_GPS;
881  const double wl2=L2_WAVELENGTH_GPS;
882  const double F1=L1_MULT_GPS;
883  const double F2=L2_MULT_GPS;
884  const double F1F2=(F1/F2)*(F1/F2);
885  const double alpha=(F1F2 - 1.0);
886  double big,small;
887  // these are arbitrary biases added to L1 and L2, for testing
888  const double bias1=1.0, bias2=2.0;
889  const double sigP1=3.0, sigP2=3.0, sigL1=0.01, sigL2=0.01;
890  Matrix<double> H(3,3,0.0), CM(3,3), PhiInv(3,3), Rw(3,3,0.0), G(3,3),
891  Rwx(3,3), Cov(3,3);
892  Vector<double> D(3), Dsave(3), Zw(3), X(3);
893  Namelist NL;
894  map<int, Matrix<double> > RwStore, RwxStore;
895  map<int, Vector<double> > ZwStore, DStore;
896  map<int, double> TimeStore;
897 
898  // build the state namelist
899  NL += "IONO";
900  NL += "BIAS_L1";
901  NL += "BIAS_L2";
902  SRIFilter SRIF(NL);
903 
904  // partials matrix (const) : state is [I, B1, B2], data is
905  // [L1-L2, L1-P1, L2-P2]
906  H(0,0)=alpha;
907  H(0,1)=1;
908  H(0,2)=-1;
909  H(1,0)=-2;
910  H(1,1)=1;
911  H(1,2)=0;
912  H(2,0)=-2*(alpha+1);
913  H(2,1)=0;
914  H(2,2)=1;
915 
916  // just for fun
917  Matrix<double> Hinv(3,3);
918  Hinv(0,0) = -1.0/alpha;
919  Hinv(0,1) = 1.0/alpha;
920  Hinv(0,2) = -1.0/alpha;
921  Hinv(1,0) = -2.0/alpha;
922  Hinv(1,1) = (alpha+2.0)/alpha;
923  Hinv(1,2) = -2.0/alpha;
924  Hinv(2,0) = -2.0*(alpha+1)/alpha;
925  Hinv(2,1) = -Hinv(2,0);
926  Hinv(2,2) = -(alpha+2)/alpha;
927  LOG(VERBOSE) << " H times Hinv\n" << H * Hinv;
928 
929  // measurement covariance - ought to multiply elevation angle
930  // dependent factor
931  CM(0,0) = sigL1*sigL1+sigL2*sigL2;
932  CM(0,1) = CM(1,0)=sigL1*sigL1;
933  CM(0,2) = CM(2,0)=-sigL2*sigL2;
934  CM(1,1) = sigP1*sigP1+sigL1*sigL1;
935  CM(2,2) = sigP2*sigP2+sigL2*sigL2;
936  CM(1,2) = CM(2,1) = 0.0;
937  LOG(VERBOSE) << " Measurement covariance\n" << CM;
938 
939  Matrix<double> InvMC(inverse(CM));
940  LOG(VERBOSE) << " Inverse measurement covariance\n" << InvMC;
941  LOG(VERBOSE) << " MC * inv\n" << CM * InvMC;
942 
943  // keep stats on various things, also used in smoother loop
944  Stats<double> Stat1,Stat2,Stat3;
945 
946  // loop over data in time order
947  LOG(VERBOSE) << "#KMU n cond I B1 B2 sigI sigB1 sigB2 resI resB1 resB2";
948  int n;
949  double m;
950  for (n=0,i=0; i<M; i++)
951  {
952 
953  n++;
954  //if (n==1)
955  //{
956  // // initial estimate - high uncertainty
957  // ident(Cov);
958  // Cov(0,0) = 100.0;
959  // Cov(1,1) = 0.0001;
960  // X(0) = 0.0; X(1) = 4.0; X(2) = -2.0;
961  // SRIF.addAPriori(Cov,X);
962  //}
963 
964  // fill the data vector
965  D(0) = wl1*data[C*i+1]+bias1 - wl2*data[C*i+2]-bias2; // L1-L2
966  D(1) = wl1*data[C*i+1]+bias1 - data[C*i+3]; // L1-P1
967  D(2) = wl2*data[C*i+2]+bias2 - data[C*i+4]; // L2-P2
968  Dsave = D;
969 
971  //LOG(VERBOSE) << "DAT " << setw(3) << n
972  // << fixed << setprecision(4)
973  // << " " << setw(8) << D(0)
974  // << " " << setw(8) << D(1)
975  // << " " << setw(8) << D(2);
976 
977  // MU - D will be replaced with residuals
978  SRIF.measurementUpdate(H, D, CM);
979 
980  // solution after MU
981  SRIF.getStateAndCovariance(X,Cov,&small,&big);
982 
983  // output after MU
984  LOG(VERBOSE) << "KMU " << setw(3) << n
985  << fixed << setprecision(6)
986  << " " << setw(11) << big/small
987  << " " << setw(9) << X(0)
988  << " " << setw(9) << X(1)
989  << " " << setw(9) << X(2)
990  << " " << setw(10) << sqrt(Cov(0,0))
991  << " " << setw(9) << sqrt(Cov(1,1))
992  << " " << setw(9) << sqrt(Cov(2,2))
993  << " " << setw(9) << D(0)*1000.0 // L1-L2 residuals in mm
994  << " " << setw(9) << D(1) // L1-P1 (m)
995  << " " << setw(9) << D(2) // L2-P2 (m)
996  ;
997 
998  Stat1.Add(D(0)*1000.0);
999  Stat2.Add(D(1));
1000  Stat3.Add(D(2));
1001 
1002  ident(PhiInv); // inverse state transition - trivial
1003  ident(G); // noise to state - trivial
1004  // process noise - NB Rw is an information matrix =
1005  // inverse sqrt p.n. cov. I think all that really matters
1006  // is that I be bumped relative to biases
1007  Rw = 0.0;
1008  Rw(0,0) = 1.e-2; // I
1009  Rw(1,1) = 1.0/sigL1; // bias 1
1010  Rw(2,2) = 1.0/sigL2; // bias 2
1011  Zw = 0.0; // process noise state - yes this is required
1012 
1013  // TU
1014  SRIF.timeUpdate(PhiInv, Rw, G, Zw, Rwx);
1015 
1016  // save for smoother
1017  RwStore[n] = Rw;
1018  RwxStore[n] = Rwx;
1019  ZwStore[n] = Zw;
1020  DStore[n] = Dsave;
1021 
1022  // solution after TU
1023  SRIF.getStateAndCovariance(X,Cov,&small,&big);
1024  D = Dsave;
1025  D = D - H * X; // compute residuals
1026  LOG(VERBOSE) << "KTU " << setw(3) << n
1027  << fixed << setprecision(4)
1028  << " " << setw(9) << big/small
1029  << " " << setw(7) << X(0)
1030  << " " << setw(7) << X(1)
1031  << " " << setw(7) << X(2)
1032  << " " << setw(8) << sqrt(Cov(0,0))
1033  << " " << setw(7) << sqrt(Cov(1,1))
1034  << " " << setw(7) << sqrt(Cov(2,2))
1035  << " " << setw(7) << D(0) * 1000.0
1036  << " " << setw(7) << D(1)
1037  << " " << setw(7) << D(2)
1038  ;
1039 
1040  } // end loop over data
1041 
1042  LOG(VERBOSE) << "L1-L2 data residuals " << Stat1;
1043  LOG(VERBOSE) << "L1-P1 data residuals " << Stat2;
1044  LOG(VERBOSE) << "L2-P2 data residuals " << Stat3;
1045  // test
1046  if (Stat1.Minimum() < -0.0817 || Stat1.Maximum() > 0.1119)
1047  {
1048  ncount++;
1049  LOG(INFO) << "L1-L2 data residuals bad";
1050  }
1051  if (Stat2.Minimum() < -0.9644 || Stat2.Maximum() > 1.070)
1052  {
1053  ncount++;
1054  LOG(INFO) << "L1-P1 data residuals bad";
1055  }
1056  if (Stat3.Minimum() < -1.0284 || Stat3.Maximum() > 1.001)
1057  {
1058  ncount++;
1059  LOG(INFO) << "L2-P2 data residuals bad";
1060  }
1061  if (::fabs(X(0)-6.76) > 0.01 ||
1062  ::fabs(X(1)-1.0) > 0.01 ||
1063  ::fabs(X(2)-2.0) > 0.01)
1064  {
1065  ncount++;
1066  LOG(INFO) << "FF TU solution bad";
1067  }
1068  if (::fabs(sqrt(Cov(0,0)) - 100.00) > 0.01 ||
1069  ::fabs(sqrt(Cov(1,1)) - 0.5778) > 0.01 ||
1070  ::fabs(sqrt(Cov(2,2)) - 0.7345) > 0.01)
1071  {
1072  ncount++;
1073  LOG(INFO) << "FF TU covariance bad";
1074  }
1075 
1076  // -------------------------------------------------------------------
1077  // Smoother
1078  // get final solution = apriori for smoother
1079  SRIF.getStateAndCovariance(X,Cov,&small,&big);
1080  big=0.0;
1081  Stat1.Reset();
1082  Stat2.Reset();
1083  Stat3.Reset();
1084 
1085  // loop over the stored smoother data in reverse order
1086  LOG(VERBOSE) << "#KSU n cond I B1 B2 sigI sigB1 sigB2 resI resB1 resB2";
1087  for (n=RwStore.size(); n>0; n--)
1088  {
1089  Rw = RwStore[n];
1090  Rwx = RwxStore[n];
1091  Zw = ZwStore[n];
1092  ident(PhiInv);
1093  ident(G);
1094 
1095  // could uses the SRIS but the DM smoother is faster and simpler
1096  //SRIF.smootherUpdate(Phi,Rw,G,Zw,Rwx); // Phi not PhiInv
1097  //SRIF.getStateAndCovariance(X,Cov,&small,&big);
1098  //
1099  SRIF.DMsmootherUpdate(Cov,X,PhiInv,Rw,G,Zw,Rwx);
1100 
1101  // output after SU
1102  D = DStore[n];
1103  D = D - H * X; // compute residuals
1104  LOG(VERBOSE) << "KSU " << setw(3) << n
1105  << fixed << setprecision(4)
1106  << " " << setw(8) << big/small
1107  << " " << setw(7) << X(0)
1108  << " " << setw(7) << X(1)
1109  << " " << setw(7) << X(2)
1110  << " " << setw(8) << sqrt(Cov(0,0))
1111  << " " << setw(7) << sqrt(Cov(1,1))
1112  << " " << setw(7) << sqrt(Cov(2,2))
1113  << " " << setw(7) << D(0) * 1000.0
1114  << " " << setw(7) << D(1)
1115  << " " << setw(7) << D(2)
1116  ;
1117 
1118  Stat1.Add(D(0)*1000.0);
1119  Stat2.Add(D(1));
1120  Stat3.Add(D(2));
1121  }
1122 
1123  LOG(VERBOSE) << "L1-L2 smoother residuals " << Stat1;
1124  LOG(VERBOSE) << "L1-P1 smoother residuals " << Stat2;
1125  LOG(VERBOSE) << "L2-P2 smoother residuals " << Stat3;
1126  // test
1127  if (Stat1.Minimum() < -0.0815 || Stat1.Maximum() > 0.114)
1128  {
1129  ncount++;
1130  LOG(INFO) << "L1-L2 smoother residuals bad";
1131  }
1132  if (Stat2.Minimum() < -0.9530 || Stat2.Maximum() > 1.075)
1133  {
1134  ncount++;
1135  LOG(INFO) << "L1-P1 smoother residuals bad";
1136  }
1137  if (Stat3.Minimum() < -1.0090 || Stat3.Maximum() > 0.993)
1138  {
1139  ncount++;
1140  LOG(INFO) << "L2-P2 smoother residuals bad";
1141  }
1142  if (::fabs(X(0)-6.325) > 0.01 ||
1143  ::fabs(X(1)-0.997) > 0.01 ||
1144  ::fabs(X(2)-2.0) > 0.01)
1145  {
1146  ncount++;
1147  LOG(INFO) << "BF SU solution bad";
1148  }
1149  if (::fabs(sqrt(Cov(0,0)) - 0.2769) > 0.01 ||
1150  ::fabs(sqrt(Cov(1,1)) - 0.5778) > 0.01 ||
1151  ::fabs(sqrt(Cov(2,2)) - 0.7345) > 0.01)
1152  {
1153  ncount++;
1154  LOG(INFO) << "BF SU covariance bad";
1155  }
1156 
1157  LOG(INFO) << "Return " << ncount;
1158  return ncount;
1159  }
1160  catch(Exception& e)
1161  {
1162  cout << "Exception: " << e << endl;
1163  }
1164 }
gnsstk::ident
BaseClass & ident(RefMatrixBase< T, BaseClass > &m)
Definition: MatrixBaseOperators.hpp:80
main
int main(int argc, char **argv)
Definition: KalmanFilter_T.cpp:109
StringUtils.hpp
gnsstk::Stats::Minimum
T Minimum(void) const
return minimum value
Definition: Stats.hpp:321
gnsstk::Stats::Reset
void Reset(void)
reset, i.e. ignore earlier data and restart sampling
Definition: Stats.hpp:146
logstream.hpp
gnsstk::SRIFilter::measurementUpdate
void measurementUpdate(const Matrix< double > &H, Vector< double > &D, const Matrix< double > &CM=SRINullMatrix)
Definition: SRIFilter.cpp:123
GNSSconstants.hpp
gnsstk::SRIFilter
Definition: SRIFilter.hpp:93
gnsstk
For Sinex::InputHistory.
Definition: BasicFramework.cpp:50
gnsstk::SRIFilter::timeUpdate
void timeUpdate(Matrix< double > &PhiInv, Matrix< double > &Rw, Matrix< double > &G, Vector< double > &Zw, Matrix< double > &Rwx)
Definition: SRIFilter.cpp:235
gnsstk::Stats< double >
Stats.hpp
gnsstk::L2_WAVELENGTH_GPS
const double L2_WAVELENGTH_GPS
GPS L2 carrier wavelength in meters.
Definition: DeprecatedConsts.hpp:59
gnsstk::Exception
Definition: Exception.hpp:151
gnsstk::Matrix< double >
gnsstk::Stats::Maximum
T Maximum(void) const
return maximum value
Definition: Stats.hpp:325
gnsstk::L1_MULT_GPS
const double L1_MULT_GPS
GPS L1 frequency in units of oscillator frequency.
Definition: GNSSconstants.hpp:96
wl1
double wl1
Definition: DiscCorr.cpp:656
gnsstk::VERBOSE
@ VERBOSE
Definition: logstream.hpp:57
gnsstk::SRIFilter::DMsmootherUpdate
static void DMsmootherUpdate(Matrix< double > &P, Vector< double > &X, Matrix< double > &Phinv, Matrix< double > &Rw, Matrix< double > &G, Vector< double > &Zw, Matrix< double > &Rwx)
Definition: SRIFilter.cpp:266
wl2
double wl2
Definition: DiscCorr.cpp:656
gnsstk::Vector< double >
example3.data
data
Definition: example3.py:22
LOG
#define LOG(level)
define the macro that is used to write to the log stream
Definition: logstream.hpp:315
std
Definition: Angle.hpp:142
gnsstk::INFO
@ INFO
Definition: logstream.hpp:57
SRIFilter.hpp
gnsstk::Stats::Add
void Add(const T &x)
Definition: Stats.hpp:158
gnsstk::Namelist
Definition: Namelist.hpp:287
Matrix.hpp
gnsstk::inverse
SparseMatrix< T > inverse(const SparseMatrix< T > &A)
Definition: SparseMatrix.hpp:1890
gnsstk::L1_WAVELENGTH_GPS
const double L1_WAVELENGTH_GPS
GPS L1 carrier wavelength in meters.
Definition: DeprecatedConsts.hpp:57
gnsstk::SRI::getStateAndCovariance
void getStateAndCovariance(Vector< double > &X, Matrix< double > &C, double *ptrSmall=NULL, double *ptrBig=NULL) const
Definition: SRI.cpp:1136
gnsstk::L2_MULT_GPS
const double L2_MULT_GPS
GPS L2 frequency in units of oscillator frequency.
Definition: GNSSconstants.hpp:98

gnsstk
Author(s):
autogenerated on Wed Oct 25 2023 02:40:39