├── .gitignore ├── An-Extendable-Sensor-Fusion-Algorithm-for_Consumer-Drone-Positioning.pdf ├── README.md ├── data ├── FLY084.csv ├── android_fix_1.csv ├── dji_data_flight_1.csv ├── dji_data_flight_2.csv ├── dji_data_flight_3.csv ├── fix.csv ├── flight1.txt ├── python_data.csv ├── raw.csv ├── sat_data_flight_1.csv ├── sat_data_flight_2.csv ├── sat_data_flight_3.csv ├── sat_data_v2_flight_1.csv ├── sat_data_v2_flight_2.csv ├── sat_data_v2_flight_3.csv └── sat_toy.csv ├── data_processing.py ├── docs └── img │ └── fusion-3d.png ├── gnss_fusion_ekf.py ├── gnss_only_ekf.py ├── gnss_only_ekf_toy.py ├── lib ├── __init__.py ├── gpstime.py └── log_reader.py ├── parse_sat_data.py └── toy_data_creation.py /.gitignore: -------------------------------------------------------------------------------- 1 | # latex stuff 2 | */*.fls 3 | */*.fdb_latexmk 4 | */*.aux 5 | */*.zip 6 | */*.fdb_latexmk 7 | */*.out 8 | */*.gz 9 | */*.log 10 | */*.pytxcode 11 | 12 | 13 | 14 | # latex stuff 15 | */*/*.fls 16 | */*/*.fdb_latexmk 17 | */*/*.aux 18 | */*/*.zip 19 | */*/*.fdb_latexmk 20 | */*/*.out 21 | */*/*.gz 22 | */*/*.log 23 | */*/*.pytxcode 24 | 25 | # compilied python 26 | __pycache__/ 27 | *.pyc 28 | 29 | # don't add new calculated trajectory 30 | data/calculated_trajectory.csv 31 | data/wls_calculated_trajectory.csv 32 | -------------------------------------------------------------------------------- /An-Extendable-Sensor-Fusion-Algorithm-for_Consumer-Drone-Positioning.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/betaBison/gnss-sensor-fusion/db2b1a9a1134ae85ae023bc8c1360837b288533c/An-Extendable-Sensor-Fusion-Algorithm-for_Consumer-Drone-Positioning.pdf -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # gnss-sensor-fusion 2 | Extended Kalman Filter (EKF) for position estimation using raw GNSS signals, IMU data, and barometer. The provided raw GNSS data is from a Pixel 3 XL and the provided IMU & barometer data is from a consumer drone flight log. 3 | 4 | Project paper can be viewed [here](https://github.com/betaBison/gnss-sensor-fusion/blob/master/An-Extendable-Sensor-Fusion-Algorithm-for_Consumer-Drone-Positioning.pdf) and overview video presentation can be viewed [here](https://youtu.be/m7BPbx05Vro). 5 | 6 | ## Setup 7 | This repo was built using Python3. A number of packages are needed to run all files including at least: 8 | `pip3 install pandas numpy scipy pymap3d pyproj progress` 9 | 10 | ## Run 11 | The full gnss sensor fusion can be run with: 12 | `python3 gnss_fusion_ekf.py` 13 | Change the filepaths at the end of the file to specify odometry and satellite data files. 14 | The current default is to use raw GNSS signals and IMU velocity for an EKF that estimates latitude/longitude and the barometer and a static motion model for a second EKF that estimates altitude. Instead of raw GNSS psuedoranges, you can also use GPS LLA estimates which most receivers provide. States for all EKFs are [ECEF X, ECEF Y, ECEF Z, GNSS time bias]. It currently runs weighted least squares to obtain an initial estimate for the time bias. 15 | 16 | ![fusion-ekf](docs/img/fusion-3d.png) 17 | 18 | An EKF using ONLY raw GNSS signals to estimate position can be run with: 19 | `python3 gnss_only_ekf.py` 20 | It currently runs weighted least squares to obtain an initial estimate for the time bias. It assumes a static motion model. The weighted least square position solution is also saved to file. States for the EKF are [ECEF X, ECEF Y, ECEF Z, GNSS time bias]. 21 | 22 | A toy 3 DOF example can be run with: 23 | `python3 gnss_only_ekf_toy.py` 24 | -------------------------------------------------------------------------------- /data/android_fix_1.csv: -------------------------------------------------------------------------------- 1 | ,Provider,Latitude,Longitude,Altitude,Speed,Accuracy,(UTC)TimeInMs,seconds of week [s] 2 | 0,gps,37.426536,-122.175789,1.245148,0.0,3.216,1581800799000,594413.0 3 | 1,gps,37.426536999999996,-122.175789,1.26178,0.0,3.216,1581800800000,594414.0 4 | 2,gps,37.426536999999996,-122.175789,1.253906,0.0,3.216,1581800801000,594415.0 5 | 3,gps,37.426536999999996,-122.175789,1.25589,0.0,3.216,1581800802000,594416.0 6 | 4,gps,37.426536999999996,-122.175789,1.256592,0.0,3.216,1581800803000,594417.0 7 | 5,gps,37.426536999999996,-122.175789,1.254669,0.0,3.216,1581800804000,594418.0 8 | 6,gps,37.426536999999996,-122.175789,1.2546389999999998,0.0,3.216,1581800805000,594419.0 9 | 7,gps,37.426536999999996,-122.175789,1.253265,0.0,3.216,1581800806000,594420.0 10 | 8,gps,37.426536999999996,-122.175789,1.260681,0.0,3.216,1581800807000,594421.0 11 | 9,gps,37.426535,-122.17579099999999,1.568817,0.0,3.216,1581800808000,594422.0 12 | 10,gps,37.426536999999996,-122.175793,1.651123,0.0,3.216,1581800809000,594423.0 13 | 11,gps,37.426538,-122.17579199999999,2.277557,0.0,3.216,1581800810000,594424.0 14 | 12,gps,37.426541,-122.17579099999999,2.506287,0.0,3.216,1581800811000,594425.0 15 | 13,gps,37.426542,-122.17578999999999,2.661255,0.0,3.216,1581800812000,594426.0 16 | 14,gps,37.426547,-122.17578600000002,2.009155,0.0,3.216,1581800813000,594427.0 17 | 15,gps,37.426547,-122.175785,1.9468990000000002,0.21,3.216,1581800814000,594428.0 18 | 16,gps,37.426545000000004,-122.175785,1.9151919999999998,0.6,3.216,1581800815000,594429.0 19 | 17,gps,37.426536,-122.175789,1.9119259999999998,1.08,3.216,1581800816000,594430.0 20 | 18,gps,37.426524,-122.175793,3.250763,1.35,3.216,1581800817000,594431.0 21 | 19,gps,37.426511,-122.175794,2.90033,1.35,3.216,1581800818000,594432.0 22 | 20,gps,37.4265,-122.17579599999999,2.90387,1.17,3.216,1581800819000,594433.0 23 | 21,gps,37.426491,-122.175797,2.649506,1.02,3.216,1581800820000,594434.0 24 | 22,gps,37.426483000000005,-122.175799,2.751007,1.02,3.216,1581800821000,594435.0 25 | 23,gps,37.426475,-122.1758,2.826294,0.98,3.216,1581800822000,594436.0 26 | 24,gps,37.426464,-122.175803,2.947418,1.21,3.216,1581800823000,594437.0 27 | 25,gps,37.426452000000005,-122.17580900000002,2.929504,1.34,3.216,1581800824000,594438.0 28 | 26,gps,37.426441,-122.17581499999999,2.9963990000000003,1.31,3.216,1581800825000,594439.0 29 | 27,gps,37.426429999999996,-122.175821,3.178162,1.3,3.216,1581800826000,594440.0 30 | 28,gps,37.426418,-122.17582800000001,3.6480410000000005,1.42,3.216,1581800827000,594441.0 31 | 29,gps,37.426406,-122.17583300000001,3.8320309999999997,1.32,3.216,1581800828000,594442.0 32 | 30,gps,37.426396000000004,-122.17583799999998,3.796082,1.19,3.216,1581800829000,594443.0 33 | 31,gps,37.426387,-122.175843,3.5778809999999996,1.18,3.216,1581800830000,594444.0 34 | 32,gps,37.426378,-122.175848,3.8037410000000005,1.12,3.216,1581800831000,594445.0 35 | 33,gps,37.426369,-122.175854,3.571228,1.2,3.216,1581800832000,594446.0 36 | 34,gps,37.426359000000005,-122.17586200000001,3.72168,1.21,3.216,1581800833000,594447.0 37 | 35,gps,37.42635,-122.175868,3.324524,1.2,3.216,1581800834000,594448.0 38 | 36,gps,37.42634,-122.175874,3.216309,1.25,3.216,1581800835000,594449.0 39 | 37,gps,37.426331,-122.17588,3.228973,1.11,3.216,1581800836000,594450.0 40 | 38,gps,37.426323,-122.17588400000001,2.943695,0.91,3.216,1581800837000,594451.0 41 | 39,gps,37.426318,-122.17588700000002,3.005157,0.59,3.216,1581800838000,594452.0 42 | 40,gps,37.426317,-122.17588700000002,2.707001,0.0,3.216,1581800839000,594453.0 43 | 41,gps,37.426318,-122.175886,2.504089,0.0,3.216,1581800840000,594454.0 44 | 42,gps,37.426318,-122.175886,2.343964,0.0,3.216,1581800841000,594455.0 45 | 43,gps,37.426318,-122.17588700000002,2.841888,0.0,3.216,1581800842000,594456.0 46 | 44,gps,37.426319,-122.17588700000002,2.710571,0.0,3.216,1581800843000,594457.0 47 | 45,gps,37.426316,-122.175875,2.364716,1.17,3.216,1581800844000,594458.0 48 | 46,gps,37.426313,-122.17586200000001,2.33493,1.01,3.216,1581800845000,594459.0 49 | 47,gps,37.426311,-122.17585,2.307617,0.87,3.216,1581800846000,594460.0 50 | 48,gps,37.426308,-122.17584199999999,2.416962,0.69,3.216,1581800847000,594461.0 51 | 49,gps,37.426305,-122.175832,2.482269,0.7,3.216,1581800848000,594462.0 52 | 50,gps,37.426303000000004,-122.175825,2.1421509999999997,0.58,3.216,1581800849000,594463.0 53 | 51,gps,37.4263,-122.17582,2.134186,0.48,3.216,1581800850000,594464.0 54 | 52,gps,37.426297,-122.17581799999999,2.15509,0.39,3.216,1581800851000,594465.0 55 | 53,gps,37.426294,-122.17581499999999,1.2432860000000001,0.53,3.216,1581800852000,594466.0 56 | 54,gps,37.42629,-122.175808,1.308868,0.92,3.216,1581800853000,594467.0 57 | 55,gps,37.426288,-122.175799,1.283539,0.87,3.216,1581800854000,594468.0 58 | 56,gps,37.426286,-122.17578999999999,1.62439,0.88,3.216,1581800855000,594469.0 59 | 57,gps,37.426285,-122.175781,1.484314,0.86,3.216,1581800856000,594470.0 60 | 58,gps,37.426283000000005,-122.17577299999999,1.024841,0.85,3.216,1581800857000,594471.0 61 | 59,gps,37.426281,-122.17576399999999,1.011627,0.97,3.216,1581800858000,594472.0 62 | 60,gps,37.426278,-122.175753,0.813507,1.06,3.216,1581800859000,594473.0 63 | 61,gps,37.426275,-122.175742,0.884186,1.02,3.216,1581800860000,594474.0 64 | 62,gps,37.426272,-122.17573,1.194489,1.1,3.216,1581800861000,594475.0 65 | 63,gps,37.426269,-122.175719,1.365509,1.01,3.216,1581800862000,594476.0 66 | 64,gps,37.426266999999996,-122.17570900000001,1.550964,0.91,3.216,1581800863000,594477.0 67 | 65,gps,37.426265,-122.175699,1.7055049999999998,1.03,3.216,1581800864000,594478.0 68 | 66,gps,37.426263,-122.17568600000001,1.8025209999999998,1.12,3.216,1581800865000,594479.0 69 | 67,gps,37.42626,-122.175671,1.755371,1.35,3.216,1581800866000,594480.0 70 | 68,gps,37.426258000000004,-122.17566000000001,1.769714,0.97,3.216,1581800867000,594481.0 71 | 69,gps,37.426255,-122.175648,1.9422,1.07,3.216,1581800868000,594482.0 72 | 70,gps,37.426253,-122.17563700000001,1.555939,1.15,3.216,1581800869000,594483.0 73 | 71,gps,37.42625,-122.175626,1.1244809999999998,1.1,3.216,1581800870000,594484.0 74 | 72,gps,37.426247,-122.17561599999999,1.3844299999999998,0.93,3.216,1581800871000,594485.0 75 | 73,gps,37.426245,-122.175606,1.454742,0.88,3.216,1581800872000,594486.0 76 | 74,gps,37.426241999999995,-122.175597,1.465271,0.79,3.216,1581800873000,594487.0 77 | 75,gps,37.426241,-122.17558799999999,1.606354,0.8,3.216,1581800874000,594488.0 78 | 76,gps,37.42624,-122.17558100000001,1.803467,0.63,3.216,1581800875000,594489.0 79 | 77,gps,37.426241999999995,-122.17558000000001,2.016205,0.0,3.216,1581800876000,594490.0 80 | 78,gps,37.426241999999995,-122.17558000000001,2.347382,0.0,3.216,1581800877000,594491.0 81 | 79,gps,37.426245,-122.17558000000001,2.571899,0.27,3.216,1581800878000,594492.0 82 | 80,gps,37.426246,-122.175582,2.3775020000000002,0.0,3.216,1581800879000,594493.0 83 | 81,gps,37.426244,-122.17558400000001,2.325806,0.0,3.216,1581800880000,594494.0 84 | 82,gps,37.426251,-122.17558000000001,2.53595,0.74,3.216,1581800881000,594495.0 85 | 83,gps,37.426262,-122.175573,3.046722,1.47,3.216,1581800882000,594496.0 86 | 84,gps,37.426272999999995,-122.17556699999999,2.961761,1.43,3.216,1581800883000,594497.0 87 | 85,gps,37.426285,-122.17556,2.841736,1.23,3.216,1581800884000,594498.0 88 | 86,gps,37.426296,-122.175554,3.010132,1.24,3.216,1581800885000,594499.0 89 | 87,gps,37.426305,-122.175549,2.976532,1.07,3.216,1581800886000,594500.0 90 | 88,gps,37.426314,-122.17554299999999,3.249939,1.08,3.216,1581800887000,594501.0 91 | 89,gps,37.426323,-122.17553799999999,3.3339230000000004,1.15,3.216,1581800888000,594502.0 92 | 90,gps,37.426332,-122.175532,2.979004,1.31,3.216,1581800889000,594503.0 93 | 91,gps,37.426343,-122.175526,2.712341,1.36,3.216,1581800890000,594504.0 94 | 92,gps,37.426353999999996,-122.175522,2.564606,1.38,3.216,1581800891000,594505.0 95 | 93,gps,37.426364,-122.175517,2.763092,1.2,3.216,1581800892000,594506.0 96 | 94,gps,37.426373999999996,-122.17551399999999,2.687927,1.27,3.216,1581800893000,594507.0 97 | 95,gps,37.426384000000006,-122.17551,2.7948,1.31,3.216,1581800894000,594508.0 98 | 96,gps,37.426395,-122.175506,2.545074,1.3,3.216,1581800895000,594509.0 99 | 97,gps,37.426406,-122.175501,2.358276,1.28,3.216,1581800896000,594510.0 100 | 98,gps,37.426415999999996,-122.175497,2.406342,1.23,3.216,1581800897000,594511.0 101 | 99,gps,37.426429,-122.175494,2.092468,1.39,3.216,1581800898000,594512.0 102 | 100,gps,37.426442,-122.17548899999998,2.0328060000000003,1.39,3.216,1581800899000,594513.0 103 | 101,gps,37.42645,-122.175486,2.275513,0.87,3.216,1581800900000,594514.0 104 | 102,gps,37.426452000000005,-122.17548400000001,2.151184,0.0,3.216,1581800901000,594515.0 105 | 103,gps,37.426452000000005,-122.17548300000001,2.00589,0.0,3.216,1581800902000,594516.0 106 | 104,gps,37.426451,-122.175482,1.8751220000000002,0.0,3.216,1581800903000,594517.0 107 | 105,gps,37.426452000000005,-122.17548300000001,2.188629,0.0,3.216,1581800904000,594518.0 108 | 106,gps,37.426451,-122.17548500000001,1.8927,0.19,3.216,1581800905000,594519.0 109 | 107,gps,37.426451,-122.17548700000002,2.030304,0.27,3.216,1581800906000,594520.0 110 | 108,gps,37.426453,-122.17548899999998,1.419922,0.29,3.216,1581800907000,594521.0 111 | 109,gps,37.426456,-122.175491,1.071808,0.6,3.216,1581800908000,594522.0 112 | 110,gps,37.426458000000004,-122.175495,1.034668,0.68,3.216,1581800909000,594523.0 113 | 111,gps,37.426463,-122.1755,1.254211,1.06,3.216,1581800910000,594524.0 114 | 112,gps,37.426468,-122.17550800000001,1.145569,1.17,3.216,1581800911000,594525.0 115 | 113,gps,37.426474,-122.175518,1.236572,1.24,3.216,1581800912000,594526.0 116 | 114,gps,37.426481,-122.17553000000001,1.41687,1.46,3.216,1581800913000,594527.0 117 | 115,gps,37.42648,-122.17554299999999,1.2920530000000001,0.0,3.216,1581800914000,594528.0 118 | 116,gps,37.426485,-122.175552,1.6744689999999998,1.06,3.216,1581800915000,594529.0 119 | 117,gps,37.426486,-122.17556,2.079895,0.23,3.216,1581800916000,594530.0 120 | 118,gps,37.426487,-122.17556499999999,2.605957,0.16,3.216,1581800917000,594531.0 121 | 119,gps,37.426495,-122.175572,3.4513849999999997,1.36,3.216,1581800918000,594532.0 122 | 120,gps,37.426504,-122.175582,4.048523,1.41,3.216,1581800919000,594533.0 123 | 121,gps,37.426507,-122.175595,4.307251,1.07,3.216,1581800920000,594534.0 124 | 122,gps,37.426512,-122.17560900000001,4.492889,1.31,3.216,1581800921000,594535.0 125 | 123,gps,37.426514000000005,-122.175623,4.676758,0.0,3.216,1581800922000,594536.0 126 | 124,gps,37.426517,-122.17563799999999,4.9815059999999995,0.17,3.216,1581800923000,594537.0 127 | 125,gps,37.426520000000004,-122.17565400000001,4.575928,0.0,3.216,1581800924000,594538.0 128 | 126,gps,37.426519,-122.17566799999999,5.018585,0.37,3.216,1581800925000,594539.0 129 | 127,gps,37.426526,-122.175683,4.5132449999999995,0.25,3.216,1581800926000,594540.0 130 | 128,gps,37.426533,-122.175703,4.687225,0.28,3.216,1581800927000,594541.0 131 | 129,gps,37.426536999999996,-122.175721,5.382599,0.32,3.216,1581800928000,594542.0 132 | 130,gps,37.426536,-122.17573600000001,5.052551,0.18,3.216,1581800929000,594543.0 133 | 131,gps,37.426539,-122.17574499999999,4.982819,0.46,3.216,1581800930000,594544.0 134 | 132,gps,37.426546,-122.175751,4.538391000000001,0.91,3.216,1581800931000,594545.0 135 | 133,gps,37.426549,-122.175754,4.42392,0.25,3.216,1581800932000,594546.0 136 | 134,gps,37.426548,-122.175756,4.058441,0.0,3.216,1581800933000,594547.0 137 | 135,gps,37.426545000000004,-122.175757,3.635498,0.0,3.216,1581800934000,594548.0 138 | 136,gps,37.426543,-122.17575500000001,3.422211,0.0,3.216,1581800935000,594549.0 139 | 137,gps,37.426543,-122.175757,3.4967040000000003,0.2,3.216,1581800936000,594550.0 140 | 138,gps,37.426545000000004,-122.175756,3.671356,0.0,3.216,1581800937000,594551.0 141 | 139,gps,37.426545000000004,-122.175758,3.453674,0.0,3.216,1581800938000,594552.0 142 | 140,gps,37.426545000000004,-122.17575900000001,3.4456480000000003,0.0,3.216,1581800939000,594553.0 143 | 141,gps,37.426546,-122.17576499999998,3.3052370000000004,0.0,3.216,1581800940000,594554.0 144 | 142,gps,37.426546,-122.175766,3.369446,0.0,3.216,1581800941000,594555.0 145 | 143,gps,37.426546,-122.175767,3.371033,0.0,3.216,1581800942000,594556.0 146 | 144,gps,37.426546,-122.175767,3.531036,0.0,3.216,1581800943000,594557.0 147 | -------------------------------------------------------------------------------- /data/dji_data_flight_3.csv: -------------------------------------------------------------------------------- 1 | ,seconds of week [s],ECEF_vel_x,ECEF_vel_y,ECEF_vel_z,IMU_ATTI(0):velN[meters/Sec],IMU_ATTI(0):velE[meters/Sec],IMU_ATTI(0):velD[meters/Sec],IMU_ATTI(0):velH[meters/Sec],IMU_ATTI(0):velComposite[meters/Sec],GPS(0):Lat[degrees],GPS(0):Long[degrees],GPS(0):heightMSL[meters],Normalized barometer:Raw[meters] 2 | 0,594853.0,0.0028227451257407665,-0.0021215099841356277,0.0031400835141539574,0.002255561,0.0012299560000000001,0.00422734,0.002569114,0.004946792,37.426469399999995,-122.17578710000001,2.6782759999999968,0.8694799999999958 3 | 1,594853.2,0.0028895814903080463,-0.0007909825071692467,0.0015736971981823444,0.001717704,0.0005780000000000001,0.006101906,0.001812438,0.00636539,37.426469399999995,-122.17578710000001,2.6633609999999948,1.2257900000000035 4 | 2,594853.4,0.0020972019992768764,0.0003682579845190048,0.0029047499410808086,0.003114617,-0.0007099999999999999,0.005737085,0.0031944609999999996,0.006566485,37.426469399999995,-122.17578710000001,2.659875999999997,1.6475699999999875 5 | 3,594853.6,-0.0024191346019506454,-0.00648545753210783,0.00796485785394907,0.002145783,-0.000883,-0.0038549340000000004,0.002320301,0.004499368,37.426469299999994,-122.17578710000001,2.664135999999999,1.6292299999999926 6 | 4,594853.8,-0.0041823252104222775,-0.010362142696976662,0.010616795625537634,0.001686955,-0.00031099999999999997,-0.008818058,0.0017154720000000002,0.008983374,37.426469299999994,-122.17578710000001,2.6716359999999924,1.2577500000000015 7 | 5,594854.0,-0.0004192441701889038,-0.0034681865945458412,0.005386331118643284,0.002297425,-0.000797,0.000586,0.002431868,0.002501464,37.426469299999994,-122.17578710000001,2.6629859999999894,1.2289299999999912 8 | 6,594854.2,-0.0005079545080661774,-0.0060166260227561,0.0060972776263952255,0.0015223679999999999,0.00048499999999999997,-0.001596572,0.0015976410000000001,0.00225865,37.42646920000001,-122.17578689999999,2.6685099999999977,1.1309499999999986 9 | 7,594854.4,-0.0030450555495917797,-0.010969433933496475,0.010346810333430767,0.001528215,0.000975,-0.00858124,0.001813,0.00877067,37.42646920000001,-122.17578689999999,2.6778059999999897,1.1587199999999882 10 | 8,594854.6,0.0005774460732936859,-0.003970296122133732,0.002049738075584173,-0.000288,0.000314,0.00269778,0.000426,0.002731175,37.4264691,-122.17578689999999,2.676392000000007,1.3400199999999955 11 | 9,594854.8,-0.0002524377778172493,-0.002947661094367504,0.004554201848804951,0.001958349,-0.000933,0.0015121429999999999,0.0021691370000000002,0.0026441890000000003,37.4264691,-122.17578689999999,2.674126000000001,0.9973349999999925 12 | 10,594855.0,-0.0002399100922048092,-0.0039192670956254005,0.00518169766291976,0.001960918,-0.000405,0.00048300000000000003,0.002002297,0.002059825,37.426469,-122.1757867,2.671500999999992,1.1498199999999912 13 | 11,594855.2,0.0011224099434912205,-0.0064699891954660416,0.006169511470943689,0.001874159,0.002106396,-0.001255746,0.002819463,0.003086465,37.426469,-122.1757866,2.6725959999999986,1.156099999999995 14 | 12,594855.4,0.005756013095378876,0.00031800195574760437,-0.0015175309963524342,0.0007610000000000001,0.002413651,0.00993819,0.002530644,0.010255329,37.426469,-122.1757866,2.663175999999993,0.2584899999999948 15 | 13,594855.6,-0.0016694506630301476,-0.011462709866464138,0.00823033507913351,3.8900000000000004e-05,0.002402016,-0.007044801999999999,0.002402331,0.007443147,37.4264689,-122.1757865,2.6614299999999957,1.2394099999999924 16 | 14,594855.8,0.0053888000547885895,0.0025579985231161118,0.00025811512023210526,0.003204525,0.00091,0.010209419,0.003331346,0.010739185,37.426469,-122.1757867,2.6625159999999966,1.1351399999999927 17 | 15,594856.0,0.0029453332535922527,-0.0019690338522195816,0.0024081054143607616,0.001792381,0.0012525210000000001,0.004826525999999999,0.00218665,0.005298754,37.426469,-122.1757867,2.6583459999999945,0.569199999999995 18 | 16,594856.2,0.0017738239839673042,-0.002117512747645378,0.004409430082887411,0.002926181,0.00033999999999999997,0.003015012,0.0029458390000000004,0.004215242,37.4264689,-122.1757868,2.6578520000000054,0.489034999999987 19 | 17,594856.4,0.0023796148598194122,-0.0014625973999500275,-0.0012847441248595715,-0.0010628039999999999,0.000504,0.0071720080000000005,0.001176068,0.007267794,37.4264689,-122.1757868,2.6563759999999945,-0.01609000000000549 20 | 18,594856.6,0.007866789121180773,-0.001610427163541317,0.005615466274321079,0.006116671,0.005227174,0.005199545999999999,0.008045931,0.009579785,37.4264689,-122.1757867,2.6549459999999954,0.05307599999999013 21 | 19,594856.8,0.003763881977647543,-0.005944272503256798,0.015536259859800339,0.010437905,0.004062237,-0.00547791,0.011200518999999999,0.012468324,37.426468799999995,-122.1757868,2.657791000000003,-0.5427400000000091 22 | 20,594857.0,0.002582728397101164,-0.015824570320546627,0.028501867782324553,0.015269574,0.008323907,-0.020498414,0.017391012,0.026881821,37.426468799999995,-122.1757867,2.664685999999989,-0.6349660000000057 23 | 21,594857.2,-0.003115723840892315,-0.03174274507910013,0.035476379096508026,0.010775643,0.011977316000000002,-0.03784666,0.016111194,0.0411332,37.426468799999995,-122.1757867,2.672555999999986,0.023729999999986262 24 | 22,594857.4,-0.014150102622807026,-0.04327458143234253,0.04747385159134865,0.010800087,0.008778494,-0.05755572,0.013917752,0.059214565999999996,37.4264687,-122.1757867,2.684295999999989,0.470169999999996 25 | 23,594857.6,-0.009706804994493723,-0.04225149564445019,0.04809348937124014,0.013256444,0.011994578,-0.055365592000000005,0.017877449,0.05818034,37.4264687,-122.1757867,2.685366000000002,0.0687899999999928 26 | 24,594857.8,-0.00642448989674449,-0.02515353634953499,0.03366741444915533,0.011657787,0.005667849,-0.033717446,0.012962581,0.036123327999999996,37.4264686,-122.1757867,2.683255999999986,0.23699999999999477 27 | 25,594858.0,0.0014488422311842442,-0.01785644143819809,0.03034746879711747,0.015323056000000002,0.008446168,-0.023465337000000003,0.017496678999999998,0.029270392000000003,37.4264686,-122.1757866,2.6788859999999914,0.09865999999999531 28 | 26,594858.2,0.033426400274038315,0.04292525351047516,-0.02412370592355728,0.013681198,0.003145346,0.064017765,0.014038104,0.06553886,37.4264686,-122.1757867,2.6363059999999905,-0.6134800000000098 29 | 27,594858.4,0.06149125052616,0.0785395847633481,-0.05912799062207341,0.013286106,0.007934766999999999,0.12109860000000001,0.015475178999999999,0.12208338,37.4264686,-122.1757867,2.5815809999999857,-0.8126239999999996 30 | 28,594858.6,0.09287658846005797,0.12872162275016308,-0.10593676241114736,0.012085021999999999,0.00777717,0.19654971,0.014371225,0.19707441,37.4264686,-122.1757868,2.498685999999992,-2.1768400000000128 31 | 29,594858.8,0.07420617993921041,0.14294721744954586,-0.10941733047366142,0.010596352,-0.015601193999999999,0.20033151,0.018859478,0.20121728,37.4264685,-122.1757868,2.382239999999996,-2.2109200000000158 32 | 30,594859.0,-0.12959959777072072,-0.021459228359162807,0.07213201466947794,0.004240565,-0.10055726,-0.106700264,0.10064663,0.14667886,37.4264685,-122.1757868,2.348066000000003,-0.6024800000000141 33 | 31,594859.2,-0.16058726282790303,-0.08971397392451763,0.10062107723206282,-0.018274653999999998,-0.09043905,-0.18299744,0.092266924,0.20494206,37.4264684,-122.1757866,2.3748059999999924,0.40625 34 | 32,594859.4,-0.1319239903241396,-0.0636897161602974,0.0317785800434649,-0.050281316,-0.08003624,-0.11154473,0.094519891,0.14620614,37.4264686,-122.1757875,2.3240959999999973,0.470169999999996 35 | 33,594859.6,-0.08307487284764647,-0.10711946617811918,0.011994310654699802,-0.07252398,-0.01556233,-0.108055376,0.07417489,0.1310644,37.4264687,-122.1757872,2.269919999999999,0.6787099999999953 36 | 34,594859.8,-0.11829984746873379,-0.18510273937135935,0.025821513030678034,-0.11305842,-0.0038500609999999997,-0.18377303,0.11312395300000001,0.2157998,37.4264684,-122.1757875,2.2813759999999945,0.6525099999999924 37 | 35,594860.0,-0.15593046694993973,-0.1954095670953393,0.03954534884542227,-0.1196403,-0.030212799999999998,-0.21495512,0.12339617,0.24785544,37.4264679,-122.1757875,2.3148259999999965,2.4228199999999873 38 | 36,594860.2,-0.2695318306796253,-0.2033826606348157,0.22988469200208783,-0.009352164,-0.12212142,-0.38403273,0.122478999,0.40309083,37.4264682,-122.1757875,2.4918209999999874,2.485679999999988 39 | 37,594860.4,-0.23375087976455688,-0.19466977659612894,0.24416392529383302,0.01804933,-0.096475475,-0.37172294,0.098149352,0.38446227,37.4264684,-122.17578780000001,2.5810359999999974,1.9418999999999897 40 | 38,594860.6,-0.21036106953397393,-0.1450324235484004,0.20586544694378972,0.020738741,-0.10311059,-0.30519125,0.105175511,0.3228058,37.426468299999996,-122.17578780000001,2.6597459999999984,2.024669999999986 41 | 39,594860.8,-0.16374300280585885,-0.10944634955376387,0.15816024038940668,0.016247336,-0.082602315,-0.23256455,0.084185025,0.24733256,37.4264687,-122.1757881,2.7230459999999965,1.3876099999999951 42 | 40,594861.0,-0.1392159517854452,-0.12478774227201939,0.17213962879031897,0.0273949,-0.053672593,-0.24100024,0.060259669,0.24841969,37.4264691,-122.1757882,2.7840459999999894,2.733459999999994 43 | 41,594861.25,-0.12037090305238962,-0.13997927494347095,0.17148207081481814,0.025156982,-0.029632032000000003,-0.24284254,0.038870697,0.24593379,37.426469299999994,-122.17578859999999,2.8468660000000057,2.4364400000000046 44 | 42,594861.5,-0.13263480784371495,-0.1824328750371933,0.18251974740996957,0.008114952,-0.017405199,-0.28327292,0.019203995,0.28392315,37.426469399999995,-122.17578870000001,2.914035999999996,1.3849850000000004 45 | 43,594861.75,-0.14769635302945971,-0.2369478354230523,0.23577590892091393,0.017490167,-0.001123471,-0.3586517,0.017526212,0.3590797,37.4264695,-122.1757888,2.985221999999993,2.3589099999999945 46 | 44,594862.0,-0.19827596144750714,-0.3382059298455715,0.3137920037843287,0.010988258,0.009986561,-0.49551769999999995,0.01484834,0.49574012,37.4264695,-122.17578870000001,3.0790059999999926,2.115309999999994 47 | 45,594862.1666666666,-0.22217108821496367,-0.44207153376191854,0.39272153237834573,0.012506413999999999,0.045071404,-0.62340695,0.046774374,0.62515926,37.4264698,-122.17578859999999,3.201911999999993,2.065016 48 | 46,594862.3333333334,-0.28562367195263505,-0.5143881281837821,0.48355193017050624,0.026902331,0.029873671,-0.754051,0.040201638,0.75512195,37.42647,-122.17578870000001,3.348326,2.4332939999999894 49 | 47,594862.5,-0.35853441432118416,-0.5931640639901161,0.5880738752894104,0.045787454000000005,0.010110073,-0.90135753,0.046890346,0.9025764000000001,37.4264696,-122.1757883,3.5222359999999924,2.7973699999999866 50 | 48,594862.6666666666,-0.3816393748857081,-0.6443671621382236,0.6298897783271968,0.045178104000000004,0.017820075,-0.9709589,0.048565588,0.97217274,37.4264691,-122.17578789999999,3.7259519999999924,2.070775999999995 51 | 49,594862.8333333334,-0.3675441830419004,-0.6227275244891644,0.6356106959283352,0.06541452,0.018227058,-0.95392954,0.067906443,0.9563434999999999,37.4264687,-122.1757873,3.926316,3.0524699999999854 52 | 50,594863.0,-0.35837723361328244,-0.60603285767138,0.6444061980582774,0.083953895,0.017095958999999997,-0.9441767,0.085676882,0.9480559999999999,37.4264685,-122.17578689999999,4.124666000000005,2.397669999999991 53 | 51,594863.2,-0.3549802992492914,-0.6242151027545333,0.6523088850080967,0.08197598,0.029653536,-0.95976454,0.08717450199999999,0.96371543,37.4264685,-122.1757861,4.325271999999998,3.204404999999994 54 | 52,594863.4,-0.3584390957839787,-0.654261932708323,0.6513232737779617,0.06461761,0.042726375,-0.9808247999999999,0.077465985,0.9838792,37.426469,-122.17578519999999,4.528796,2.973879999999994 55 | 53,594863.6,-0.40031122136861086,-0.6874159779399633,0.6680010077543557,0.04725607,0.024940062000000002,-1.030953,0.053433539,1.0323368,37.4264687,-122.17578470000001,4.737545999999995,4.142119999999991 56 | 54,594863.8,-0.4755074246786535,-0.6613064473494887,0.5928193293511868,-0.023353301,-0.052611236,-0.9995113999999999,0.057561435,1.0011675,37.4264686,-122.1757841,4.951805999999991,4.246319999999997 57 | 55,594864.0,-0.42285820515826344,-0.6464306935667992,0.5521832429803908,-0.030932521,-0.015969487,-0.94255126,0.03481157,0.9431939,37.426468299999996,-122.1757835,5.163445999999993,4.028419999999997 58 | 56,594864.2,-0.3960119024850428,-0.6477188291028142,0.5859395004808903,0.0039001820000000003,0.007439704,-0.95257926,0.008400036999999999,0.9526163000000001,37.426468299999996,-122.1757828,5.375541999999996,5.013949999999994 59 | 57,594864.4,-0.25140208937227726,-0.5090503180399537,0.45938289258629084,0.021529958,0.055997017999999996,-0.72130257,0.059993375,0.7237932,37.4264681,-122.17578219999999,5.411565999999993,4.363459999999989 60 | 58,594864.6,-0.34462950471788645,-0.6602490358054638,0.5637888256460428,-0.0035069470000000003,0.057603315,-0.9258107,0.05770996900000001,0.92760766,37.4264677,-122.17578200000001,5.595321999999996,4.408909999999992 61 | 59,594864.8,-0.4639098923653364,-0.7822794942185283,0.6241564373485744,-0.05694342,0.021625286,-1.0949663,0.06091146099999999,1.0966592,37.42646729999999,-122.1757815,5.823465999999996,5.105499999999999 62 | 60,594865.0,-0.48874854017049074,-0.8404696919023991,0.6908352063037455,-0.041963633,0.031588607000000005,-1.1851076,0.052524152000000005,1.1862709999999999,37.4264675,-122.17578059999998,6.071795999999992,5.916661999999995 63 | 61,594865.2,-0.4517290717922151,-0.8407627306878567,0.8080481528304517,0.06294905,0.06307866,-1.2408848999999997,0.08911509,1.2440806999999998,37.42646729999999,-122.17578,6.349471999999992,5.1876239999999925 64 | 62,594865.4,-0.4650077326223254,-0.7926419256255031,0.7667003981769085,0.050569657000000004,0.026214217999999997,-1.1890261,0.056960297,1.1903896,37.426466999999995,-122.1757791,6.6192860000000024,5.474114999999998 65 | 63,594865.6,-0.416401871945709,-0.6842863773927093,0.5777208148501813,-0.028036007999999998,0.009654056,-0.9807867,0.02965162,0.9812348,37.4264667,-122.1757775,6.874876,5.78226699999999 66 | 64,594865.8,-0.4343932284973562,-0.7008729288354516,0.5386091829277575,-0.073450685,0.0032584309999999996,-0.97577417,0.073522926,0.9785401,37.4264664,-122.17577630000001,7.117263999999999,6.599929999999986 67 | 65,594866.0,-0.4111793148331344,-0.6586711509153247,0.4913827767595649,-0.08173307,0.000434,-0.90888894,0.081734225,0.9125565999999999,37.4264662,-122.175776,7.356366000000001,6.666509999999995 68 | 66,594866.2,-0.4207491325214505,-0.6964545855298638,0.5730479899793863,-0.039413176,0.012454221000000001,-0.98796433,0.041334077999999996,0.9888286000000001,37.42646679999999,-122.17577549999999,7.619887999999996,7.322877999999996 69 | 67,594866.4,-0.34559763222932816,-0.6139262653887272,0.5449908706359565,0.005080384,0.032116234,-0.883659,0.032515578999999996,0.884257,37.4264669,-122.175775,7.851638999999999,6.665326999999998 70 | 68,594866.6,-0.24344382155686617,-0.5262697050347924,0.5241457666270435,0.0666781,0.07190266,-0.7688706999999999,0.098061009,0.77509874,37.426467200000005,-122.17577450000002,8.044815999999997,7.447041999999989 71 | 69,594866.8,-0.23525070259347558,-0.5771287186071277,0.5561009785160422,0.06854416,0.10592074,-0.8190124,0.12616459900000002,0.8286728999999999,37.426467200000005,-122.1757737,8.250918999999989,7.02208499999999 72 | 70,594867.0,-0.2997086741961539,-0.6701416345313191,0.6120660970918834,0.044280707999999995,0.10089316,-0.9428039,0.110182629,0.9492204,37.42646729999999,-122.17577320000001,8.493566000000001,7.353490000000001 73 | 71,594867.25,-0.34105890430510044,-0.6389166908338666,0.5764398225583136,0.018668689,0.049265715999999994,-0.91765034,0.052684255,0.91916144,37.4264675,-122.1757733,8.718368999999988,7.807164999999998 74 | 72,594867.5,-0.40624832501634955,-0.7072969432920218,0.5657195677049458,-0.046117347,0.030501722999999998,-0.9846662,0.055291634000000006,0.98621744,37.426467200000005,-122.17577309999999,8.974442000000003,8.324197999999996 75 | 73,594867.75,-0.374949490185827,-0.6222165673971176,0.5326515985652804,-0.018482473,0.011686968,-0.89414495,0.021867487,0.89441234,37.426467200000005,-122.17577309999999,9.217257999999994,8.355737999999995 76 | 74,594868.0,-0.33888646168634295,-0.5948228258639574,0.5148444320075214,-0.006861009,0.027623804,-0.84965885,0.028463099,0.85013545,37.4264677,-122.1757726,9.391349999999996,9.101693999999988 77 | 75,594868.2,-0.4161780793219805,-0.6557689066976309,0.6283247345127165,0.026892233999999998,-0.005342545,-0.9922777,0.027417786,0.9926564,37.4264677,-122.1757726,9.632911999999997,8.834709999999994 78 | 76,594868.4,-0.3796893935650587,-0.6246521472930908,0.6504083946347237,0.072245225,0.008972006999999999,-0.9693525,0.07280020200000001,0.9720823000000001,37.4264685,-122.1757726,9.843772999999999,9.02575499999999 79 | 77,594868.6,-0.4604411004111171,-0.5897051217034459,0.6395783238112926,0.05548771,-0.07798769,-0.97342926,0.095712939,0.9781235,37.4264684,-122.17577220000001,10.069445999999992,9.049992999999986 80 | 78,594868.8,-0.7304025958292186,-0.6511488491669297,0.6969564110040665,-0.01792212,-0.2737686,-1.1637648,0.27435460899999997,1.1956668,37.426468799999995,-122.1757729,10.385112999999997,9.179662999999998 81 | 79,594869.0,-0.7148928944952786,-0.8012851262465119,0.7631433457136154,-0.03757248,-0.18069084,-1.2983476999999999,0.184555871,1.3113991000000003,37.426468799999995,-122.17577240000001,10.643042999999992,10.150441999999991 82 | 80,594869.2,-0.823790738824755,-0.8347066510468721,0.8005565763451159,-0.060296647,-0.25506660000000003,-1.389602,0.262096645,1.4141033,37.4264686,-122.1757723,10.941680999999988,9.552872999999991 83 | 81,594869.4,-0.7694099321961403,-0.858127498999238,0.7696897191926837,-0.079257414,-0.19656563,-1.3635886000000002,0.211942882,1.3799614,37.4264684,-122.17577069999999,11.215125999999998,9.981019999999994 84 | 82,594869.6,-0.6275689508765936,-0.8767091855406761,0.7448594192974269,-0.06262999999999999,-0.066613585,-1.3010051999999999,0.091432416,1.3042141,37.4264685,-122.17576979999998,11.472117999999988,9.43690999999999 85 | 83,594869.8,-0.4949133927002549,-0.9134577503427863,0.7626777719706297,-0.02445175,0.0652379,-1.2804368999999998,0.069669736,1.2823308999999998,37.426468299999996,-122.17576880000001,11.731273999999999,10.503639999999997 86 | 84,594870.0,-0.4110956909134984,-0.9663730543106794,0.7474985336884856,-0.036599826,0.16436112,-1.2713343000000001,0.168386832,1.2824372,37.4264679,-122.17576709999999,11.98905599999999,11.173123999999994 87 | 85,594870.2,-0.3014660472981632,-0.9751363098621368,0.6567801735363901,-0.07767031,0.26182038,-1.17573,0.273098122,1.2070309,37.4264674,-122.17576570000001,12.226661999999997,10.415654999999987 88 | 86,594870.4,-0.27507154177874327,-0.9417482782155275,0.5842803469859064,-0.109527655,0.26638153,-1.0980643,0.288019839,1.1352096,37.42646729999999,-122.1757644,12.438888999999996,10.704953999999987 89 | 87,594870.6,-0.2845397577621043,-0.9531243089586496,0.6711560385301709,-0.04945293,0.26442537,-1.1625131000000002,0.269009984,1.1932322,37.42646679999999,-122.1757637,12.660663999999997,10.925959999999996 90 | 88,594870.8,-0.31792429415509105,-0.8628028938546777,0.6170822381973267,-0.056737172999999995,0.1880704,-1.0830567,0.19644231199999998,1.1007277,37.4264665,-122.1757629,12.883339,10.877717999999987 91 | 89,594871.0,-0.4233884899877012,-0.9018470011651516,0.7182613448239863,-0.030603796000000003,0.119595066,-1.2153916,0.12344866199999999,1.221645,37.426466,-122.1757621,13.096645999999993,11.701772999999996 92 | 90,594871.2,-0.5037600733339787,-0.9558681435883045,0.7615216099657118,-0.050049156,0.08033413,-1.3119823999999998,0.094649298,1.3153920000000001,37.426466299999994,-122.1757619,13.342032999999994,11.79613299999999 93 | 91,594871.4,-0.560060634277761,-0.9102266691625118,0.8660831484012306,0.03824413,0.008375391999999999,-1.3686591000000001,0.039150486,1.369219,37.4264659,-122.17576070000001,13.608855999999989,12.060155999999992 94 | 92,594871.6,-0.7841935739852488,-0.9354549711570144,1.0173555007204413,0.07285995,-0.16790064,-1.572335,0.18302785600000002,1.5829518999999999,37.4264657,-122.1757597,13.904437999999999,11.89401999999999 95 | 93,594871.8,-0.7352690030820668,-0.945942098274827,0.9636398334987462,0.040641553999999996,-0.120905355,-1.5260491000000003,0.12755328400000002,1.5313706,37.4264654,-122.1757594,14.203547999999998,12.798581999999989 96 | 94,594872.0,-0.711384916678071,-1.0328754857182503,0.9600608861073852,0.00081,-0.054395933,-1.5722076999999999,0.05440195900000001,1.5731486000000001,37.426466,-122.17575910000001,14.497710999999995,13.059811999999994 97 | 95,594872.2,-0.600644204299897,-1.0536370584741235,0.9543075123801827,0.021400591,0.05039306,-1.5358353999999999,0.054748935,1.5368108999999999,37.4264661,-122.1757585,14.779769999999992,13.283379999999987 98 | 96,594872.4,-0.6112059699371457,-1.1467862334102392,1.0436734142713249,0.04103434,0.091056846,-1.6572255,0.09987575300000001,1.6602323,37.4264661,-122.17575719999999,15.085162999999994,13.545809999999996 99 | 97,594872.6,-0.5103796320036054,-1.1226303856819868,0.9360012137331069,0.000585,0.16353485,-1.5329131999999999,0.163535896,1.5416118,37.4264664,-122.1757564,15.380235999999996,13.782792999999991 100 | 98,594872.8,-0.5297218840569258,-1.2022606367245317,0.9836364481598139,-0.008808395,0.18956761,-1.6235678999999998,0.189772141,1.6346211000000002,37.4264665,-122.17575559999999,15.674910999999994,14.775262999999995 101 | 99,594873.0,-0.5824294202029705,-1.236741865053773,0.9844907931983471,-0.042925227,0.16331674,-1.6695538,0.168863652,1.6780717,37.42646679999999,-122.1757551,15.985714000000002,14.294972999999992 102 | 100,594873.2,-0.6648050211369991,-1.2643185798078775,1.0572288860566914,-0.026006654,0.10827747,-1.7671318999999999,0.111356886,1.7706369999999998,37.42646729999999,-122.17575459999999,16.32953299999999,15.269289999999998 103 | 101,594873.4,-0.7321909568272531,-1.2396064652130008,1.1446457556448877,0.034317687,0.038081082999999995,-1.8321450000000001,0.051262777999999995,1.832862,37.4264675,-122.1757547,16.709103,15.777394999999991 104 | 102,594873.6,-0.7566089327447116,-1.155478885397315,1.0796559271402657,0.018080396999999998,-0.027386095,-1.7464258999999998,0.032816139,1.7467341,37.4264677,-122.1757539,17.05051799999999,16.555209999999995 105 | 103,594873.8,-0.6990664144977927,-1.1381764002144337,1.0600187159143388,0.030009042000000003,0.012105163,-1.6985272,0.032358578,1.6988355,37.426468299999996,-122.17575359999998,17.391177999999996,16.09830999999999 106 | 104,594874.0,-0.7120925975032151,-1.0971187008544803,1.0799942640587687,0.062776916,-0.020784356,-1.6885781000000002,0.066128138,1.6898724999999999,37.4264681,-122.1757529,17.736449999999998,15.750179999999986 107 | 105,594874.2,-0.7288283188827336,-1.0581250144168735,0.9497637948952615,-0.026001051,-0.05571458,-1.5902983,0.06148308,1.5914863000000001,37.4264679,-122.1757523,18.061161,15.969395999999989 108 | 106,594874.4,-0.7824180135503411,-1.0846987813711166,0.917299997061491,-0.08279477,-0.08692298,-1.6110931999999998,0.12004407,1.6155591999999999,37.4264681,-122.1757524,18.370312,17.46501999999999 109 | 107,594874.6,-0.8310211626812816,-1.060812084004283,0.9725544918328524,-0.042357492999999996,-0.1407817,-1.6491716,0.147015797,1.6557113999999997,37.4264679,-122.1757538,18.685105999999998,16.487869999999994 110 | 108,594874.8,-0.8704625885002315,-0.929000330157578,0.874659868888557,-0.06505862,-0.24435763,-1.5177562,0.252870074,1.5386771000000001,37.4264684,-122.1757539,18.958209999999994,16.21324 111 | 109,594875.0,-0.8485880983062088,-0.9456658465787768,0.970682920422405,0.009702936,-0.21696797,-1.5780652,0.21718481899999997,1.5929403,37.4264686,-122.1757545,19.234555999999998,17.373709999999996 112 | 110,594875.2,-0.8570148735307157,-1.0207143910229206,0.9944561617448926,-0.012750583000000001,-0.18413597,-1.6465221999999997,0.18457691199999998,1.6568353999999998,37.4264684,-122.17575500000001,19.51423299999999,18.83465799999999 113 | 111,594875.4,-0.8656856510788202,-1.1511715557426214,1.0095482971519232,-0.07067957,-0.122004494,-1.7470505,0.140998926,1.7527311,37.4264678,-122.175756,19.84547,18.097771999999992 114 | 112,594875.6,-0.8624493610113859,-1.129376484081149,0.9190736431628466,-0.13026959,-0.13087147,-1.6760465,0.184655104,1.6861877,37.426468,-122.17575659999999,20.165844,18.623859999999993 115 | 113,594875.8,-0.8579972181469202,-1.2069136863574386,1.0501388744451106,-0.06463112,-0.08581325,-1.805936,0.10742948699999999,1.8091285,37.4264678,-122.1757554,20.51775,18.662799999999997 116 | 114,594876.0,-0.799233884550631,-1.272604906000197,1.1239780522882938,-0.020767175,-0.0010931530000000001,-1.8701166,0.020795926,1.8702322,37.4264676,-122.1757557,20.895863,20.022539999999992 117 | 115,594876.2,-0.8242353186942637,-1.1849306402727962,1.0731618530116975,-0.024113106000000002,-0.06894285,-1.7908741999999997,0.073038065,1.7923630000000002,37.4264674,-122.17575579999999,21.235499999999995,19.79853599999999 118 | 116,594876.4,-0.8253831705078483,-1.1615628013387322,1.0465615740977228,-0.033588186,-0.082358174,-1.7594863,0.08894399800000001,1.7617328999999997,37.4264676,-122.1757555,21.566039999999994,20.396249999999995 119 | 117,594876.6,-0.7863089363090694,-1.1184315839782357,1.1057032058015466,0.04821085,-0.07225306,-1.7499136999999998,0.08686075800000001,1.7520682,37.4264678,-122.17575520000001,21.902856,20.70780699999999 120 | 118,594876.8,-0.6196844251826406,-0.9112780150026083,0.9391099181957543,0.07640002,-0.041531496,-1.4389616000000003,0.08695877199999999,1.4415867,37.426468299999996,-122.1757551,22.228023999999998,20.49076999999999 121 | 119,594877.0,-0.3368640183471143,-0.49272124376147985,0.5960442968644202,0.110798724,-0.025035137000000002,-0.8295227,0.11359187800000001,0.83726406,37.4264691,-122.17575670000001,22.458209999999994,20.838719999999988 122 | 120,594877.2,-0.18072781153023243,-0.0535166272893548,0.24884849647060037,0.11153963,-0.12676209,-0.2572682,0.168848208,0.3077282,37.4264701,-122.17575910000001,22.545575999999997,20.28709599999999 123 | 121,594877.4,-0.2605512053705752,0.06097333412617445,0.1809385670349002,0.090670936,-0.25529397,-0.17279609,0.270917383,0.32133272,37.4264701,-122.1757604,22.588935999999997,21.193388 124 | 122,594877.6,-0.32164256274700165,0.05165998172014952,0.09335008403286338,-0.003448043,-0.30204338,-0.15165974,0.302063056,0.33799818,37.4264698,-122.1757618,22.624263999999997,20.88653999999999 125 | 123,594877.8,-0.3131170133128762,0.06795578170567751,-0.019423576537519693,-0.08186352,-0.30350497,-0.06856333,0.314351557,0.32174188,37.42646920000001,-122.1757622,22.677730999999994,20.87346999999999 126 | 124,594878.0,-0.2745649078860879,-0.012276307679712772,-0.05235972022637725,-0.13681434,-0.22814879999999998,-0.08617292,0.26602639,0.27963513,37.4264682,-122.17576329999999,22.798315999999993,21.385232999999992 127 | 125,594878.2,-0.198148300871253,0.04091292805969715,-0.1716392571106553,-0.1794463,-0.19179243,0.05438643,0.262650553,0.26822227,37.426466999999995,-122.17576399999999,22.731465999999998,20.846284999999995 128 | 126,594878.4,-0.27452899934723973,0.019911865703761578,-0.18985390663146973,-0.22943360000000002,-0.24525914,0.019039437,0.33584493600000004,0.33638418,37.4264658,-122.1757644,22.744431,21.806529999999995 129 | 127,594878.6,-0.3134558950550854,2.191215753555298e-05,-0.13056094525381923,-0.20517667,-0.2676159,-0.04682668,0.33721765299999995,0.34045336,37.4264644,-122.17576470000002,22.785735999999993,20.74696499999999 130 | 128,594878.8,-0.3007778003811836,-0.008724205195903778,-0.14422584604471922,-0.21642442,-0.25222746,-0.03903937,0.332352561,0.33463755,37.4264636,-122.17576509999998,22.789525999999995,21.807576999999995 131 | 129,594879.0,-0.1975906672887504,0.012976821511983871,-0.12212058948352933,-0.15431193,-0.17644400000000002,0.005749771,0.234402766,0.23447327,37.4264626,-122.17576580000001,22.792540999999993,20.926269999999988 132 | 130,594879.2,-0.14196049142628908,0.07010264322161674,-0.12379158521071076,-0.10824924,-0.15977797,0.06868895,0.19299455399999998,0.20485377,37.4264621,-122.17576609999999,22.76530799999999,20.357499999999995 133 | 131,594879.4,-0.06142828846350312,0.1500315424054861,-0.11694907303899527,-0.03563651,-0.13417739,0.15231243,0.138829149,0.20608884,37.4264614,-122.1757662,22.673325999999996,20.587521999999993 134 | 132,594879.6,-0.07687156787142158,0.12726664263755083,-0.008240850642323494,0.033983912000000005,-0.13512620000000003,0.064413,0.139334125,0.15350255,37.426460999999996,-122.17576559999999,22.628161999999996,21.550404999999998 135 | 133,594879.8,-0.04345140140503645,0.06544975843280554,0.09433640912175179,0.09446080000000001,-0.073920205,-0.025346128,0.11994598599999999,0.12259472,37.4264607,-122.1757652,22.64875,20.763692999999996 136 | 134,594880.0,-0.02044798480346799,0.1220115153118968,0.09069193573668599,0.12810701,-0.08456973,0.024615776000000002,0.153503897,0.15546505,37.42646,-122.17576340000001,22.65315199999999,20.298425999999992 137 | 135,594880.2,-0.025155730079859495,0.04424997232854366,0.19833137094974518,0.17206243,-0.047145154,-0.09506125,0.17840443600000003,0.20215039999999998,37.4264597,-122.1757625,22.700756,20.253989999999995 138 | 136,594880.4,0.027427482418715954,0.08532776962965727,0.19995819265022874,0.21150272,-0.024512295,-0.046201825,0.212918411,0.21787350000000003,37.4264601,-122.1757618,22.682975999999996,19.978474999999996 139 | 137,594880.6,0.06359786400571465,0.12466317042708397,0.2690909970551729,0.2983437,-0.014843723999999999,-0.04648074,0.298712727,0.30230737,37.42646,-122.1757608,22.659282999999995,20.425462999999993 140 | 138,594880.8,0.05302255414426327,0.1542271189391613,0.3374799406155944,0.36443898,-0.039538164,-0.07264389,0.366577453,0.37370598,37.426460600000006,-122.1757595,22.702140999999997,19.571199999999997 141 | 139,594881.0,0.0939916823990643,0.23582755867391825,0.3983259964734316,0.46799362,-0.04831467,-0.037447963,0.470480965,0.47196895,37.426460999999996,-122.17575829999998,22.693165999999998,19.625573999999993 142 | 140,594881.2,0.0820300062187016,0.21492158249020576,0.5010108612477779,0.5349136999999999,-0.04730649,-0.11896489,0.537001498,0.5500210999999999,37.4264619,-122.17575740000001,22.748347999999993,19.451994 143 | 141,594881.4,0.16229320503771305,0.2881320957094431,0.48677047016099095,0.58724076,-0.018355949,-0.02715806,0.587527572,0.5881549,37.426463,-122.175757,22.688745999999995,19.112664999999993 144 | 142,594881.6,0.15423443913459778,0.31754280161112547,0.4743319172412157,0.58988386,-0.040838732999999995,-0.0032375790000000004,0.5912958420000001,0.5913046999999999,37.426463899999995,-122.1757562,22.61264599999999,19.73745999999999 145 | 143,594881.8,0.12647223146632314,0.2771521518006921,0.5089680911041796,0.5876276,-0.042828523,-0.06317721,0.58918627,0.59256375,37.4264649,-122.1757563,22.574672999999997,19.09959599999999 146 | 144,594882.0,0.17981590703129768,0.3447784148156643,0.42832935554906726,0.5756409,-0.03368949,0.053845275,0.576625917,0.5791345,37.426466,-122.1757563,22.429955999999997,19.23971699999999 147 | 145,594882.2,0.10481058014556766,0.2514343187212944,0.5260606370866299,0.5809615,-0.047468199999999995,-0.100012325,0.58289754,0.5914153,37.4264667,-122.17575579999999,22.376670999999995,19.11527999999999 148 | 146,594882.4,0.10119687207043171,0.1716093961149454,0.5786970732733607,0.58052987,-0.008018825,-0.1871859,0.580585245,0.61001456,37.426468299999996,-122.1757557,22.341212999999996,19.094365999999994 149 | 147,594882.6,0.229261239990592,0.25322477240115404,0.5966741889715195,0.6782355,0.056915835,-0.08909498,0.680619401,0.68642604,37.4264691,-122.17575430000001,22.235862999999995,17.92570299999999 150 | 148,594882.8,0.21693443227559328,0.19977396447211504,0.6591003825888038,0.6963254,0.07494562,-0.16817501,0.700347011,0.72025603,37.4264701,-122.17575359999998,22.220638,17.47074299999999 151 | 149,594883.0,0.21935157338157296,0.1909499103203416,0.671025815885514,0.7020389,0.08169049,-0.1803317,0.7067757240000001,0.7294185999999999,37.426471299999996,-122.175753,22.126706,18.406259999999996 152 | 150,594883.2,0.14875594293698668,0.2002810537815094,0.6679322435520589,0.68153507,0.016967975,-0.2020336,0.68174627,0.7110524,37.4264723,-122.17575179999999,22.06822199999999,18.006234999999997 153 | 151,594883.4,0.14178065955638885,0.1395540302619338,0.6896983473561704,0.66532445,0.0434021,-0.25903046,0.6667386160000001,0.71528816,37.4264736,-122.17575079999999,22.020565999999995,17.52669699999999 154 | 152,594883.6,0.18962740944698453,0.06356963887810707,0.7355929045006633,0.6781688,0.12436347,-0.31776315,0.689477461,0.7591789000000001,37.4264753,-122.17575090000001,21.972407999999994,17.800195999999993 155 | 153,594883.8,0.2304020537994802,0.05278343614190817,0.6804223586805165,0.6420036,0.16461976,-0.2742406,0.662773182,0.71726996,37.426476,-122.17575020000001,21.852191999999995,17.99943299999999 156 | 154,594884.0,0.22378138825297356,-0.006837743334472179,0.6249461211264133,0.5651359,0.19076516,-0.28340062,0.5964645270000001,0.66036797,37.4264776,-122.17575,21.764251999999992,18.092511999999992 157 | 155,594884.25,0.15044457837939262,0.01672629825770855,0.6558165089227259,0.57803833,0.116143204,-0.3173362,0.589591016,0.66956687,37.42647879999999,-122.17574920000001,21.688145999999996,17.081654999999998 158 | 156,594884.5,0.1485643102787435,-0.11919092200696468,0.7652284353971481,0.59440076,0.18692987,-0.47598523,0.623101139,0.7841026999999999,37.4264802,-122.17574779999998,21.647231999999995,18.452271999999994 159 | 157,594884.75,0.19335321569815278,-0.16959529742598534,0.8041455578058958,0.613873,0.25168124,-0.51457644,0.663463255,0.8396263,37.42648,-122.1757447,21.667895999999992,17.439095999999992 160 | 158,594885.0,0.1896141623146832,-0.254125633276999,0.7500158720649779,0.5261938,0.29353023,-0.5400794,0.6025279339999999,0.80915123,37.4264814,-122.17574309999999,21.641459999999995,17.754746999999995 161 | 159,594885.1666666666,0.27418429125100374,-0.17901275400072336,0.6502464711666107,0.5129719,0.3251132,-0.39319277,0.607320943,0.723491,37.42648270000001,-122.17574240000002,21.59164599999999,17.15962799999999 162 | 160,594885.3333333334,0.25389147410169244,-0.13947859685868025,0.572602201718837,0.4650812,0.28688437,-0.32801288,0.546445923,0.63733476,37.4264838,-122.175742,21.464025999999997,17.170613999999993 163 | 161,594885.5,0.20383249083533883,-0.10450960695743561,0.5712965014390647,0.4658318,0.22589189999999998,-0.32488364,0.517712673,0.6112085,37.4264851,-122.1757412,21.343055999999997,16.711434999999994 164 | 162,594885.6666666666,0.1725436318665743,-0.1428533848375082,0.650378474034369,0.498783,0.21982709,-0.41195047,0.545076519,0.6832361,37.4264868,-122.1757412,21.259620999999996,16.93997799999999 165 | 163,594885.8333333334,0.15241836057975888,-0.1795758232474327,0.7360503496602178,0.5414144000000001,0.22234797,-0.4972115,0.585293225,0.76797616,37.426488299999995,-122.17574099999999,21.172976,17.161725999999987 166 | 164,594886.0,0.12569359643384814,-0.23805024661123753,0.7900793896988034,0.545592,0.23086620000000002,-0.58065367,0.59242706,0.82953507,37.4264898,-122.1757412,21.087849,16.64291999999999 167 | 165,594886.2,0.16117961425334215,-0.12685167789459229,0.7187838456593454,0.55765957,0.20168719,-0.44757327,0.593010894,0.7429561,37.4264917,-122.1757409,20.892975999999997,17.14289699999999 168 | 166,594886.4,0.15024440363049507,-0.1514799240976572,0.7413394586183131,0.5593638000000001,0.20554636,-0.48246005,0.595933863,0.76674956,37.4264936,-122.1757405,20.762595999999995,16.829629999999995 169 | 167,594886.6,0.07634098548442125,-0.28768367413431406,0.6714866934344172,0.4099097,0.2155238,-0.56281245,0.46311604,0.72885823,37.4264948,-122.17574029999999,20.62117,17.922952999999993 170 | 168,594886.8,-0.13819899037480354,-0.5980457719415426,0.6728129470720887,0.18187833,0.19920589,-0.8629610999999999,0.269745645,0.9041375,37.4264958,-122.17574099999999,20.658585999999993,17.835719999999988 171 | 169,594887.0,-0.2998336753807962,-0.6921786675229669,0.6785052833147347,0.08566582,0.11252244,-0.9980474,0.141421118,1.0080171999999998,37.4264966,-122.1757405,20.737965999999993,18.209039999999995 172 | 170,594887.2,-0.24354578228667378,-0.6964195175096393,0.6224995502270758,0.05722495,0.16242394,-0.9430573,0.172209847,0.95865184,37.426497399999995,-122.1757395,20.824805999999995,17.192739999999993 173 | 171,594887.4,-0.1893948232755065,-0.7203723257407546,0.6208937549963593,0.06115333,0.22101364,-0.9352819,0.229318025,0.9629844000000001,37.4264981,-122.17573859999999,20.910851,18.474427999999996 174 | 172,594887.6,-0.20158204901963472,-0.7437548264861107,0.6485335971228778,0.06713074,0.22314970000000003,-0.9729507,0.23302859,1.0004674999999998,37.42649829999999,-122.17573859999999,21.027198999999996,18.745946999999994 175 | 173,594887.8,-0.30395128298550844,-0.6889968579635024,0.5983556099236012,0.022320458999999997,0.10734285,-0.9489395,0.109638905,0.95525223,37.4264994,-122.17573940000001,21.126098999999996,19.234129999999993 176 | 174,594888.0,-0.4295545192435384,-0.6133898878470063,0.5954158874228597,0.018229201,-0.039232153,-0.9494484000000001,0.04326044,0.9504334999999999,37.4265,-122.1757408,21.243621999999995,18.975804999999994 177 | 175,594888.2,-0.5800447356887162,-0.6031551361083984,0.6156440451741219,-0.009145826,-0.1720602,-1.0185031,0.172303106,1.0329747,37.426500700000005,-122.17574209999998,21.402652999999994,20.292844999999993 178 | 176,594888.4,-0.5424872911535203,-0.46586305368691683,0.515488410834223,-0.005903956,-0.21338114,-0.8494679,0.21346280399999998,0.87587786,37.42650089999999,-122.1757442,21.553143,19.446626999999992 179 | 177,594888.6,-0.47415106324478984,-0.37702129036188126,0.3995889159850776,-0.030126909,-0.20284972,-0.6904153000000001,0.20507471,0.72022843,37.426501,-122.1757453,21.656112999999998,19.193389999999994 180 | 178,594888.8,-0.4710300983861089,-0.43530869390815496,0.3981027426198125,-0.06028049,-0.16916907,-0.7273713,0.17958816600000002,0.74921346,37.4265005,-122.17574599999999,21.718224,20.10430999999999 181 | 179,594889.0,-0.39693553885445,-0.328947595320642,0.22803257405757904,-0.11664652,-0.16309302,-0.5211857,0.200513689,0.5584266,37.42649960000001,-122.17574669999999,21.615705999999996,20.433667999999997 182 | 180,594889.2,-0.43957475246861577,-0.263200368732214,0.14661381905898452,-0.16128254,-0.23419517,-0.4455423,0.284357939,0.5285522,37.4264992,-122.1757489,21.65289599999999,19.701749999999997 183 | 181,594889.4,-0.4678013357333839,-0.19612185284495354,0.057799671310931444,-0.20644225,-0.29380712,-0.35841459999999997,0.35908359700000003,0.50734806,37.4264986,-122.17575020000001,21.562135999999995,20.767189999999992 184 | 182,594889.6,-0.4539197273552418,-0.2594180228188634,0.023082224652171135,-0.26207992,-0.24835120000000002,-0.37399068,0.361059842,0.51983964,37.4264981,-122.1757513,21.574298999999996,20.301404999999995 185 | 183,594889.8,-0.45592496870085597,-0.3126813219860196,0.053248998709023,-0.26617143,-0.22168489,-0.42897433,0.34639778,0.5513714,37.4264977,-122.17575249999999,21.672580999999994,20.531479999999995 186 | 184,594890.0,-0.4098943886347115,-0.3397966017946601,0.04941936116665602,-0.2682639,-0.16828437,-0.42540714,0.316678307,0.5303361,37.4264972,-122.17575249999999,21.768206,20.800479999999993 187 | 185,594890.2,-0.40441375924274325,-0.36124725732952356,0.1629216680303216,-0.1873886,-0.15222263,-0.50648826,0.241425384,0.56108516,37.4264964,-122.17575169999999,21.89794599999999,21.166417999999993 188 | 186,594890.4,-0.20473252795636654,-0.2685724012553692,0.13252477208152413,-0.09923168,-0.032559406,-0.3412785,0.104436784,0.3569006,37.4264958,-122.175752,22.004317999999998,21.364343999999996 189 | 187,594890.6,-0.1568201445043087,-0.3153920955955982,0.1426322408951819,-0.09978329400000001,0.032926835,-0.35863042,0.10507560199999999,0.37370664,37.426495,-122.1757524,22.137169,21.059139999999992 190 | 188,594890.8,-0.19832692900672555,-0.4684320166707039,0.1430240198969841,-0.19162987,0.07929096,-0.4792901,0.20738627,0.5222337,37.4264941,-122.17575149999999,22.288139,22.006615999999994 191 | 189,594891.0,-0.04006803501397371,-0.3371030995622277,-0.09277870180085301,-0.26011428,0.14330958,-0.18078116,0.29697992300000003,0.34767643,37.4264937,-122.1757522,22.376776,20.66588999999999 192 | 190,594891.2,-0.13506647804751992,-0.41446815337985754,-0.0224620895460248,-0.2748153,0.1040992,-0.3156919,0.293870872,0.43130204,37.426493,-122.17575190000001,22.497878999999998,21.921909999999997 193 | 191,594891.4,-0.15499433083459735,-0.49243577755987644,-0.013913906179368496,-0.31458327,0.12875092,-0.38172176,0.33991091700000003,0.5111271,37.4264924,-122.1757503,22.648756,22.56781999999999 194 | 192,594891.6,-0.13952874997630715,-0.5199700398370624,-0.023693958297371864,-0.33150855,0.15650374,-0.38774547,0.366594241,0.5336084,37.4264917,-122.1757503,22.833228999999996,22.641929999999995 195 | 193,594891.8,-0.033094958402216434,-0.33117128536105156,-0.2978726690635085,-0.4176781,0.14605294,-0.04920053,0.442477622,0.44520462,37.426490799999996,-122.17575049999999,22.950171999999995,21.925122999999992 196 | 194,594892.0,-0.11707377107813954,-0.3885565232485533,-0.3404194787144661,-0.50816363,0.10553021,-0.097429305,0.519005675,0.52807134,37.4264898,-122.1757504,23.022979999999997,23.054543999999993 197 | 195,594892.2,-0.11015570955350995,-0.3046759506687522,-0.4972551898099482,-0.58732444,0.066718005,0.057194203,0.591101764,0.5938623000000001,37.426488899999995,-122.17575079999999,23.10582,22.095149999999997 198 | 196,594892.4,-0.26623586751520634,-0.3431850438937545,-0.5724357832223177,-0.71734995,-0.044884577,0.010995521999999999,0.7187527779999999,0.71883684,37.4264874,-122.1757507,23.183440999999995,23.044736999999998 199 | 197,594892.6,-0.40323512395843863,-0.3431510478258133,-0.7407792275771499,-0.89535713,-0.16086154,0.05539259999999999,0.9096927359999999,0.91137767,37.42648629999999,-122.1757514,23.271159999999995,22.09273299999999 200 | 198,594892.8,-0.41503961104899645,-0.19951932691037655,-1.0450567035004497,-1.0669292,-0.24733935,0.3318683,1.095223606,1.1444,37.4264846,-122.17575190000001,23.25098,22.533036999999993 201 | 199,594893.0,-0.49677656264975667,-0.23712142929434776,-1.13207525620237,-1.1818291,-0.29649937,0.32491264,1.2184547159999999,1.2610313999999998,37.426482899999996,-122.175753,23.234107999999992,22.770529999999994 202 | 200,594893.2,-0.5499785067513585,-0.22968575358390808,-1.2263094861991704,-1.2700566,-0.34549016,0.36468244,1.31620941,1.3657967,37.4264804,-122.1757531,23.199809999999992,22.34841699999999 203 | 201,594893.4,-0.5560261937789619,-0.23542918264865875,-1.3372539984993637,-1.363073,-0.34755057,0.42569023,1.406683834,1.4696841,37.4264782,-122.1757537,23.114865999999992,22.176274999999997 204 | 202,594893.6,-0.5590606541372836,-0.24795498605817556,-1.367245311383158,-1.3943155,-0.3434488,0.43421456,1.435991906,1.500205,37.426475599999996,-122.17575479999999,23.04734599999999,21.512624999999993 205 | 203,594893.8,-0.5647430466488004,-0.1900762878358364,-1.4845554428175092,-1.4595412,-0.37907985,0.54201037,1.5079661869999998,1.602416,37.4264738,-122.17575559999999,22.934802999999995,20.579797999999997 206 | 204,594894.0,-0.5929260835982859,-0.3108328580856323,-1.3906984711065888,-1.4562452,-0.33862966,0.39188215,1.495098689,1.545604,37.426471,-122.17575690000001,22.799918999999996,21.449779999999997 207 | 205,594894.2,-0.6850826400332153,-0.47534387093037367,-1.3463753289543092,-1.5354972,-0.32902795,0.2153937,1.5703537969999999,1.5850568999999999,37.4264681,-122.17575719999999,22.718810999999995,21.65210499999999 208 | 206,594894.4,-0.6423150957562029,-0.5412781881168485,-1.3817483470775187,-1.5836641000000002,-0.25771755,0.21065830000000002,1.604496868,1.6182667,37.426465,-122.1757571,22.63597099999999,21.268842999999997 209 | 207,594894.6,-0.6020203079096973,-0.6499586356803775,-1.3777970396913588,-1.6233913999999998,-0.16573712,0.15224542,1.631829728,1.6389164,37.4264619,-122.17575659999999,22.570620999999996,21.246041999999996 210 | 208,594894.8,-0.569605203345418,-0.7495508166030049,-1.298282492440194,-1.6009864999999999,-0.08526586,0.05068607,1.60325548,1.6040565,37.4264588,-122.1757562,22.541812999999998,21.484105999999997 211 | 209,594895.0,-0.5515979966148734,-0.8615333260968328,-1.2019168096594512,-1.5762361,-0.010391674,-0.0755358,1.576270421,1.5780792,37.426456,-122.17575670000001,22.563975999999997,21.20415299999999 212 | 210,594895.2,-0.41701707569882274,-0.756265508942306,-1.2975573898293078,-1.5544821000000002,0.04746341,0.110259816,1.555206519,1.5591102,37.4264533,-122.175757,22.53539599999999,20.631566999999997 213 | 211,594895.4,-0.39832704328000546,-0.8579320972785354,-1.2024833806790411,-1.5252298999999998,0.117422156,-0.007954238,1.529743232,1.5297638999999998,37.426450200000005,-122.1757564,22.534115999999997,20.62647799999999 214 | 212,594895.6,-0.4259445811621845,-1.0097879953682423,-1.061401607003063,-1.5002458,0.17491189999999998,-0.20744793,1.5104077390000001,1.5245872,37.4264473,-122.17575590000001,22.583777999999995,21.983439999999995 215 | 213,594895.8,-0.3932528770528734,-0.972837065346539,-1.0327132530510426,-1.4478754999999999,0.18290582,-0.18622084,1.459382806,1.471216,37.426444599999996,-122.17575479999999,22.638025999999996,21.468587999999997 216 | 214,594896.0,-0.37168651446700096,-0.9713914897292852,-0.9973961422219872,-1.4121058000000002,0.20039022,-0.19759275,1.426253484,1.4398757,37.4264419,-122.1757545,22.70937,21.799275999999992 217 | 215,594896.2,-0.31524786865338683,-0.8169981176033616,-1.0518034929409623,-1.3576261,0.16594398,-0.036881167,1.367730256,1.3682273999999999,37.426439,-122.17575420000001,22.740063,22.205119999999994 218 | 216,594896.4,-0.28332768101245165,-0.7383776921778917,-1.08945989375934,-1.336757,0.15109518,0.052349254000000005,1.345269076,1.3462873,37.4264364,-122.17575359999998,22.742865999999992,21.449119999999994 219 | 217,594896.6,-0.33621957432478666,-0.7844905108213425,-0.9653362105600536,-1.2790246,0.13088228,-0.0764492,1.2857037340000002,1.2879746,37.4264341,-122.1757539,22.793178999999995,21.237225999999993 220 | 218,594896.8,-0.26430049538612366,-0.6316169062629342,-1.0177489472553134,-1.2187327,0.110348284,0.08857513,1.2237182,1.2269197,37.426432,-122.17575349999998,22.805659999999996,21.149987999999993 221 | 219,594897.0,-0.2975665256381035,-0.696844570338726,-0.8725484339520335,-1.1477437,0.11692607,-0.05758150000000001,1.1536842440000001,1.1551204,37.4264299,-122.1757534,22.851062999999996,21.43439699999999 222 | 220,594897.2,-0.21754400059580803,-0.5522930650040507,-0.961275621317327,-1.1179487,0.107682645,0.12734571,1.123122768,1.1303192,37.426428,-122.1757534,22.852202999999996,21.831014999999994 223 | 221,594897.4,-0.26474610064178705,-0.5614010617136955,-0.8912256509065628,-1.0822811,0.07258002,0.0586899,1.084712044,1.0862987,37.426426,-122.17575359999998,22.850409999999997,22.252669999999988 224 | 222,594897.6,-0.2478762329556048,-0.47197250463068485,-0.9404989732429385,-1.0699484,0.039236825,0.15588072,1.070667681,1.0819557,37.4264239,-122.17575359999998,22.854011999999997,21.640849999999993 225 | 223,594897.8,-0.3156025465577841,-0.5353798922151327,-0.8530074949376285,-1.0550042,0.015677424,0.031447064,1.0551207040000001,1.0555892,37.4264219,-122.1757539,22.876739,21.925721999999993 226 | 224,594898.0,-0.2979863160289824,-0.4667695239186287,-0.9024254269897938,-1.0532538,-0.005948006,0.11504804,1.053270589,1.0595353,37.4264201,-122.1757539,22.854468999999995,21.719254999999997 227 | 225,594898.2,-0.24425411969423294,-0.4068665215745568,-0.9175745653919876,-1.0170801999999999,0.007632643000000001,0.18724258,1.017108831,1.0342003,37.426418299999995,-122.17575430000001,22.833529999999996,21.579689999999992 228 | 226,594898.4,-0.24911097157746553,-0.3477951092645526,-0.9442229573614895,-1.0094277,-0.027934793,0.24109009,1.009814145,1.0381950999999998,37.426416700000004,-122.17575490000002,22.797432,21.190789999999993 229 | 227,594898.6,-0.2721663061529398,-0.38894832227379084,-0.8725474942475557,-0.9811388000000001,-0.025534516,0.16011797,0.9814710320000001,0.99444616,37.4264149,-122.1757557,22.758876,21.132245999999995 230 | 228,594898.8,-0.2589045539498329,-0.41228361055254936,-0.8347123102284968,-0.9588045000000001,-0.0018830829999999998,0.12704684,0.958806319,0.96718687,37.426413000000004,-122.1757557,22.714259999999996,20.59380999999999 231 | 229,594899.0,-0.21103657549247146,-0.39036827720701694,-0.8644902221858501,-0.95568705,0.026963037000000002,0.18011785,0.9560673120000001,0.97288597,37.4264116,-122.17575559999999,22.670123999999994,20.52166499999999 232 | 230,594899.2,-0.21716856583952904,-0.48661860078573227,-0.800997531041503,-0.9567617,0.073027685,0.07424084,0.959544715,0.9624125,37.42641,-122.17575559999999,22.633410999999995,20.759522999999994 233 | 231,594899.4,-0.21717322384938598,-0.6278563430532813,-0.7373400642536581,-0.9788643,0.14823517,-0.059384316,0.990024763,0.9918041999999999,37.4264085,-122.1757557,22.671813999999998,21.389947999999997 234 | 232,594899.6,-0.17720538284629583,-0.6643464714288712,-0.7805644115433097,-1.019026,0.20149633,-0.040740587,1.038756419,1.0395550999999998,37.426406899999996,-122.1757554,22.723819999999996,21.481419999999993 235 | 233,594899.8,-0.11569596221670508,-0.7031880272552371,-0.8447713754139841,-1.0700887,0.27424288,-0.001815664,1.104671447,1.1046729,37.426405100000004,-122.1757557,22.77362099999999,21.45998999999999 236 | 234,594900.0,-0.17395205050706863,-0.8137440709397197,-0.8228771081194282,-1.1284261,0.2838067,-0.114070065,1.163568481,1.1691465,37.4264031,-122.1757554,22.845248999999995,21.554599999999994 237 | 235,594900.2,-0.13426443608477712,-0.7270085299387574,-0.9219957101158798,-1.1496779,0.27121097,0.021253128,1.181234343,1.1814256,37.4264009,-122.17575479999999,22.885785999999996,21.512789999999995 238 | 236,594900.4,-0.08503340976312757,-0.5524432016536593,-1.0569028602913022,-1.1510818,0.21992216,0.24139893,1.171902313,1.1965066999999998,37.4263987,-122.1757544,22.862406,21.09826499999999 239 | 237,594900.6,-0.13574647530913353,-0.5395892411470413,-1.0560746677219868,-1.1602244,0.17015268,0.22808965,1.172634896,1.1946118000000001,37.4263968,-122.1757539,22.827060999999993,20.18603999999999 240 | 238,594900.8,-0.14205676084384322,-0.4195325830951333,-1.1225413447245955,-1.1532921,0.10087938,0.34651428,1.15769564,1.2084416999999998,37.4263947,-122.1757537,22.730189999999993,21.19653999999999 241 | 239,594901.0,-0.23676299303770065,-0.47543060313910246,-1.0596364978700876,-1.1627417,0.050484784000000005,0.23066106,1.163837129,1.1864743,37.4263929,-122.1757531,22.674065999999996,20.341829999999995 242 | 240,594901.2,-0.319624993018806,-0.6150083551183343,-0.9165789722464979,-1.1477516,0.054676216,0.014857343,1.149053188,1.1491493,37.4263908,-122.17575320000002,22.672690999999993,21.354924999999994 243 | 241,594901.4,-0.3136067148298025,-0.5717670824378729,-1.0064756204374135,-1.1949502,0.036743514,0.101102024,1.1955149859999998,1.1997824,37.426388700000004,-122.175753,22.640685999999995,21.047036999999996 244 | 242,594901.6,-0.2981543173082173,-0.5862686717882752,-0.9873028970323503,-1.1821833000000002,0.057545062,0.086237036,1.183582973,1.1867205,37.426386799999996,-122.1757524,22.626875999999996,21.429154999999994 245 | 243,594901.8,-0.25520801078528166,-0.6203733263537288,-0.9260337194427848,-1.1371722,0.11205695,0.044238467000000004,1.14267995,1.143536,37.4263847,-122.1757511,22.608931999999996,20.608969999999992 246 | 244,594902.0,-0.21885143220424652,-0.6511227004230022,-0.894687132909894,-1.1163303,0.15920442,0.019893765,1.127625528,1.1278011000000001,37.426382399999994,-122.1757513,22.621639000000002,21.583352999999995 247 | 245,594902.2,-0.21883388794958591,-0.6021841410547495,-0.9901863448321819,-1.1669893999999998,0.13315868,0.11083527,1.174561841,1.1797796,37.4263805,-122.1757521,22.591880999999994,21.57289699999999 248 | 246,594902.4,-0.32184315379709005,-0.6785578392446041,-0.9286891208030283,-1.1907768,0.08663987,-0.02143687,1.193924591,1.1941171,37.426378299999996,-122.1757524,22.599306,20.457883999999993 249 | 247,594902.6,-0.3395039443857968,-0.6227718964219093,-1.0009216200560331,-1.225158,0.041984495,0.052491065,1.225877169,1.2270005,37.426376,-122.1757527,22.594305999999996,20.779909999999994 250 | 248,594902.8,-0.3800387536175549,-0.6134777143597603,-1.0261879870668054,-1.2535603000000002,0.0027257320000000002,0.05695209,1.253563237,1.2548563,37.426374100000004,-122.17575320000002,22.585665999999996,20.70096999999999 251 | 249,594903.0,-0.39551690220832825,-0.6070793680846691,-1.007980838418007,-1.2408193,-0.013782502,0.043642044000000005,1.240895877,1.2416631,37.4263719,-122.17575330000001,22.564945999999992,21.85934199999999 252 | 250,594903.2,-0.4456038628704846,-0.6290070237591863,-0.9643419925123453,-1.2336538000000001,-0.04450025,-0.01879953,1.23445612,1.2345992000000001,37.426369799999996,-122.17575349999998,22.554778999999996,21.015145999999994 253 | 251,594903.4,-0.4481105380691588,-0.5881022531539202,-0.9889231738634408,-1.2329441,-0.06840441400000001,0.02257444,1.234840257,1.2350466,37.4263674,-122.1757539,22.542945999999993,20.608969999999992 254 | 252,594903.6,-0.46737025305628777,-0.5754528418183327,-0.9785992694087327,-1.2244716999999998,-0.091442235,0.016657981999999998,1.227881336,1.2279943000000002,37.4263651,-122.17575459999999,22.507987999999997,20.87713999999999 255 | 253,594903.8,-0.4684440949931741,-0.5478666462004185,-0.9784350441768765,-1.2104983,-0.10704126,0.034646668,1.215221776,1.2157156,37.4263625,-122.17575520000001,22.476965999999997,20.250339999999994 256 | 254,594904.0,-0.536143537145108,-0.6500056544318795,-0.8743962193839252,-1.2023284,-0.10995269,-0.1258664,1.20734557,1.2138886,37.426360200000005,-122.175756,22.503945999999992,21.225289999999994 257 | 255,594904.2,-0.47397474525496364,-0.5571746602654457,-0.9234764901921153,-1.1734319,-0.10676584,-0.007349475,1.17827896,1.1783019,37.4263578,-122.17575670000001,22.514115999999994,20.258706999999994 258 | 256,594904.4,-0.5087969885207713,-0.6197572592645884,-0.8412141618318856,-1.1515672,-0.102913804,-0.11413598,1.1561566909999998,1.1617768,37.4263556,-122.17575749999999,22.538565999999996,21.105061999999997 259 | 257,594904.6,-0.4579711970873177,-0.5260997219011188,-0.8889177138917148,-1.1248232,-0.10976804,-0.000697,1.130166535,1.1301668,37.4263535,-122.17575790000001,22.539606,21.22267699999999 260 | 258,594904.8,-0.4799373634159565,-0.5231091184541583,-0.8952342811971903,-1.13541,-0.12995318,-0.004137201,1.142822605,1.1428301,37.4263515,-122.1757584,22.536677999999995,21.12073999999999 261 | 259,594905.0,-0.48472619289532304,-0.5480799488723278,-0.8683339026756585,-1.1284425,-0.12070916,-0.039295524,1.134880283,1.1355604,37.4263494,-122.1757594,22.534270999999997,21.149491999999995 262 | 260,594905.2,-0.4919412126764655,-0.539120084606111,-0.8344490132294595,-1.0992594,-0.13158737,-0.05691746,1.107107236,1.1085694,37.4263474,-122.1757598,22.553275999999997,20.412927999999994 263 | 261,594905.4,-0.4506472898647189,-0.4963787570595741,-0.8221062631346285,-1.0541071000000002,-0.119395845,-0.018226516,1.060847343,1.0610038999999998,37.42634520000001,-122.1757602,22.557990999999994,20.28850699999999 264 | 262,594905.6,-0.4075865335762501,-0.4539882503449917,-0.8543154182843864,-1.0439436,-0.10552202,0.04805194,1.04926318,1.0503628,37.426343100000004,-122.1757605,22.555585999999998,20.11545999999999 265 | 263,594905.8,-0.3980746939778328,-0.4468502402305603,-0.8620678833685815,-1.0433499,-0.10127212,0.06158386,1.048253305,1.0500607,37.426341,-122.17576059999999,22.534858999999997,21.549374999999998 266 | 264,594906.0,-0.38135800836607814,-0.44912534207105637,-0.8477470530197024,-1.0277375,-0.08591126,0.05842051,1.0313220140000001,1.0329753000000002,37.42633920000001,-122.17576059999999,22.533765999999993,20.310459999999992 267 | 265,594906.2,-0.41797059355303645,-0.4829895431175828,-0.7822931315749884,-1.0050274,-0.098867625,-0.019604264,1.0098786579999999,1.0100689,37.426337100000005,-122.1757604,22.54864599999999,20.46938999999999 268 | 266,594906.4,-0.38318942254409194,-0.4188978374004364,-0.8180373148061335,-0.9891875,-0.10355805,0.05990813,0.994593429,0.99639606,37.426335200000004,-122.17576059999999,22.536965999999993,20.428609999999992 269 | 267,594906.6,-0.3963968940079212,-0.47480972576886415,-0.7477963506244123,-0.96644264,-0.08496311,-0.025947966,0.9701701490000001,0.9705171,37.4263334,-122.1757612,22.522915999999995,21.260835999999998 270 | 268,594906.8,-0.40704436833038926,-0.5127109019085765,-0.6922336402349174,-0.94526094,-0.07379234,-0.08969459,0.948136894,0.95237005,37.4263315,-122.1757618,22.53395799999999,21.185036999999994 271 | 269,594907.0,-0.3449925957247615,-0.42842207197099924,-0.7668230421841145,-0.9410540999999999,-0.066155694,0.038534082000000004,0.943376592,0.94416326,37.4263296,-122.175762,22.533623,20.22472599999999 272 | 270,594907.2,-0.35850773425772786,-0.4753704722970724,-0.716042032931,-0.92925173,-0.052594375,-0.029600467999999998,0.930738928,0.9312094999999999,37.4263279,-122.1757628,22.516231999999995,21.429154999999994 273 | 271,594907.4,-0.36525518354028463,-0.5091992057859898,-0.6565137398429215,-0.90156376,-0.04029119,-0.09137050000000001,0.902463633,0.90707725,37.426325899999995,-122.17576319999999,22.53671599999999,21.00469799999999 274 | 272,594907.6,-0.29749649902805686,-0.43046928104013205,-0.7252915874123573,-0.89375436,-0.024863892999999998,0.032003097,0.894100135,0.8946726999999999,37.4263239,-122.17576319999999,22.54593299999999,20.319349999999993 275 | 273,594907.8,-0.34083201410248876,-0.4742431240156293,-0.6866671615280211,-0.8996238000000001,-0.03823363,-0.03922023,0.9004359040000001,0.90128964,37.4263221,-122.17576319999999,22.547992999999998,20.714566999999995 276 | 274,594908.0,-0.3193847634829581,-0.4289992321282625,-0.7174868038855493,-0.8938839,-0.044173375,0.018991604,0.894974678,0.8951761999999999,37.426320399999994,-122.17576329999999,22.555565999999992,20.173489999999994 277 | 275,594908.2,-0.35705770133063197,-0.481475249864161,-0.6537785562686622,-0.8824773000000001,-0.048116144000000007,-0.07093096,0.8837880509999999,0.8866299000000001,37.4263185,-122.17576340000001,22.566382999999995,20.776254999999992 278 | 276,594908.4,-0.37732324562966824,-0.46504759788513184,-0.6604578224942088,-0.88588965,-0.074017294,-0.06439958,0.888976395,0.8913059999999999,37.4263167,-122.1757637,22.588557999999992,21.29324499999999 279 | 277,594908.6,-0.3564714780077338,-0.3794932784512639,-0.7391333882696927,-0.8976105,-0.101927035,0.04974003,0.9033790690000001,0.90474737,37.426314899999994,-122.1757638,22.578319999999998,20.879231999999988 280 | 278,594908.8,-0.3581449957564473,-0.3707946054637432,-0.7303216615691781,-0.88667977,-0.10797572,0.04952403,0.893229941,0.89460176,37.4263132,-122.17576409999998,22.562605999999995,20.562439999999995 281 | 279,594909.0,-0.3938682391308248,-0.41760312765836716,-0.6737642455846071,-0.8774055000000001,-0.113286264,-0.031418636,0.884688789,0.8852465,37.426311600000005,-122.1757644,22.544731,21.546761999999994 282 | 280,594909.2,-0.363002123311162,-0.35069298930466175,-0.7204172750934958,-0.8700459,-0.12279133,0.054962374,0.8786680790000001,0.8803854,37.4263101,-122.17576499999998,22.510890999999994,20.868779999999994 283 | 281,594909.4,-0.44019733229652047,-0.4438760941848159,-0.6190124223940074,-0.8624339,-0.13850933,-0.10194568,0.873485583,0.87941456,37.426308500000005,-122.17576499999998,22.536495999999993,20.752724999999998 284 | 282,594909.6,-0.41524245543405414,-0.4204901186749339,-0.6257710275240242,-0.8476950000000001,-0.12984045,-0.07156569,0.857581087,0.860562,37.4263068,-122.17576509999998,22.550395999999992,20.650789999999994 285 | 283,594909.8,-0.3698653425090015,-0.3939021360129118,-0.656956528313458,-0.84409785,-0.10559091,-0.015551772,0.8506765740000001,0.85081875,37.4263052,-122.1757653,22.542243999999997,22.34333999999999 286 | 284,594910.0,-0.4178980104625225,-0.4893793351948261,-0.5883957082405686,-0.8543105,-0.09540352,-0.14170857,0.8596210129999999,0.8712231,37.426303499999996,-122.1757655,22.582088,21.676396999999994 287 | 285,594910.2,-0.31567775271832943,-0.334801958873868,-0.771382543258369,-0.8870289000000001,-0.09119729,0.11663063,0.8917046409999999,0.8992996999999999,37.4263019,-122.17576580000001,22.559295999999996,21.15210499999999 288 | 286,594910.4,-0.38337044371291995,-0.4749001022428274,-0.6875587995164096,-0.9144367,-0.07388914,-0.057108976,0.91741706,0.91919285,37.426300399999995,-122.1757662,22.55957,21.537873999999995 289 | 287,594910.6,-0.39243421889841557,-0.5301160579547286,-0.6767218108288944,-0.93716747,-0.05215749,-0.10464256,0.938617739,0.9444328000000002,37.426298700000004,-122.17576640000001,22.590018999999998,21.46730999999999 290 | 288,594910.8,-0.43605427304282784,-0.6606836440041661,-0.6253455178812146,-0.97764957,-0.019548967,-0.24207622,0.977845014,1.0073638,37.4262971,-122.17576670000001,22.660541999999992,21.76107199999999 291 | 289,594911.0,-0.39767419220879674,-0.6408409783616662,-0.6892510647885501,-1.0057708,0.00237014,-0.17366986,1.005773567,1.0206575,37.426295,-122.1757672,22.734206,21.497105999999995 292 | 290,594911.2,-0.33256843453273177,-0.5428306506946683,-0.8255082638934255,-1.0424896000000001,0.005284815,0.0025517129999999997,1.042503053,1.0425062,37.4262932,-122.1757679,22.774963,21.346562999999996 293 | 291,594911.4,-0.3129233797080815,-0.4814339755102992,-0.8972976673394442,-1.0615593,-0.010781967,0.095757954,1.06161406,1.0659239999999999,37.4262912,-122.1757683,22.75423599999999,21.423924999999997 294 | 292,594911.6,-0.40698092756792903,-0.5543207069858909,-0.8281159563921392,-1.0745533999999999,-0.051580735,-0.03505497,1.075790642,1.0763617,37.426288799999995,-122.17576840000001,22.785720999999995,21.996804999999995 295 | 293,594911.8,-0.3891221657395363,-0.406758701428771,-0.9455551258288324,-1.0861292,-0.11504393,0.14305697,1.092204992,1.1015339,37.426286700000006,-122.1757692,22.771472999999993,21.580216999999998 296 | 294,594912.0,-0.38823798950761557,-0.3069099746644497,-0.9926869855262339,-1.0719092,-0.16746664,0.23919028,1.084912152,1.1109664,37.4262848,-122.1757699,22.725615999999995,20.51225999999999 297 | 295,594912.2,-0.42161565041169524,-0.3355271676555276,-0.8481764467433095,-0.9826716000000001,-0.18047902,0.118014224,0.9991077079999999,1.0060533999999999,37.4262826,-122.17577,22.70084599999999,21.272336999999993 298 | 296,594912.4,-0.3202234082855284,-0.25602785870432854,-0.6731519945897162,-0.7699699999999999,-0.13699333,0.10795888,0.7820619940000001,0.78947836,37.4262804,-122.1757706,22.672725999999997,21.234699999999997 299 | 297,594912.6,-0.1535500311292708,-0.09412523452192545,-0.31503066467121243,-0.34834915,-0.08213353,0.06962322,0.357900893,0.36461,37.4262782,-122.17577050000001,22.647405999999997,21.940354999999997 300 | 298,594912.8,-0.10010373080149293,-0.14206925500184298,-0.051035949029028416,-0.14606789999999997,-0.011364551,-0.10044263,0.146509334,0.17763363,37.42627720000001,-122.1757701,22.681393,21.91944699999999 301 | 299,594913.0,0.06413891818374395,-0.04519647918641567,-0.041583774611353874,-0.035575196,0.0760673,0.02838423,0.083975168,0.0886425,37.4262766,-122.17576979999998,22.693698999999995,22.191243999999998 302 | 300,594913.2,0.14077572710812092,-0.022984023205935955,-0.056803715880960226,-0.011433376,0.1291057,0.08497346,0.129610975,0.15498224,37.42627589999999,-122.1757696,22.712775999999998,20.76317999999999 303 | 301,594913.4,0.13987099984660745,-0.048108745366334915,-0.06243949290364981,-0.02912605,0.14171925,0.07112789,0.144681279,0.16121988,37.4262755,-122.17576909999998,22.702925999999998,20.947189999999992 304 | 302,594913.6,0.15864430414512753,0.016140789724886417,-0.09465574054047465,-0.015583996000000001,0.123395406,0.14183274,0.124375591,0.18864202,37.4262749,-122.1757689,22.655724,21.83947599999999 305 | 303,594913.8,0.11700019845739007,0.029893523082137108,-0.07253831624984741,-0.004422758,0.08082346,0.12002435,0.080944377,0.14476822,37.4262741,-122.1757689,22.612658999999994,21.10244499999999 306 | 304,594914.0,0.07212332729250193,0.018926444463431835,-0.029170661699026823,0.009851730999999999,0.048678945999999994,0.06731825,0.049665847,0.0836567,37.4262738,-122.17576840000001,22.579459999999997,21.096173999999998 307 | 305,594914.2,0.04358735354617238,-0.030578178353607655,0.04552661255002022,0.034470614,0.050887565999999995,-0.023421625,0.061463545999999994,0.06577492,37.4262735,-122.17576809999998,22.596065999999993,21.433335999999997 308 | 306,594914.4,0.14947927929461002,0.1496328031644225,-0.10605066223070025,0.04106995,0.04455125,0.23461145,0.060593355999999994,0.24230991,37.4262731,-122.1757685,22.555906,20.75900399999999 309 | 307,594914.6,0.18199741141870618,0.24431778211146593,-0.1719973129220307,0.04793005,0.021653922000000003,0.35208607,0.052594505,0.35599267,37.426272600000004,-122.17576869999999,22.483798999999998,21.042332999999992 310 | 308,594914.8,0.157205605879426,0.28081096801906824,-0.15788046550005674,0.06988957,-0.018763572,0.35755208,0.072364517,0.36480147,37.426272499999996,-122.17576880000001,22.412291999999994,21.537873999999995 311 | 309,594915.0,0.15683551412075758,0.365470158867538,-0.1852251160889864,0.09160373,-0.06415924,0.43091947,0.111837614,0.44519573,37.426272600000004,-122.1757703,22.335299999999997,20.78931999999999 312 | 310,594915.2,0.09620116092264652,0.3828678419813514,-0.1542282784357667,0.105545506,-0.12474589999999998,0.39813393,0.163405608,0.43036264,37.42627279999999,-122.1757716,22.284656,21.509649999999993 313 | 311,594915.4,0.10837021889165044,0.43205585703253746,-0.19838266680017114,0.09972199,-0.1406392,0.46317735,0.172406091,0.49422374,37.426272999999995,-122.17577279999999,22.24707,21.655489999999993 314 | 312,594915.6,0.1448804521933198,0.5329359406605363,-0.28182465117424726,0.097167246,-0.16345653,0.59713703,0.190156545,0.6266835,37.426272999999995,-122.1757742,22.19123599999999,20.41397299999999 315 | 313,594915.8,0.25728715350851417,0.7210093792527914,-0.4497288060374558,0.096953765,-0.1684655,0.8731325999999999,0.19437247100000002,0.8945061,37.4262736,-122.17577479999999,22.019370999999992,20.062659999999994 316 | 314,594916.0,0.2969795106910169,0.6409036349505186,-0.38155032554641366,0.12273543,-0.09221148,0.79463834,0.153515285,0.80933124,37.4262737,-122.175775,21.878656,19.724449999999997 317 | 315,594916.2,0.43584169168025255,0.753797659650445,-0.4648422319442034,0.15960445,-0.034793735,0.97986573,0.163352942,0.99338865,37.4262739,-122.17577539999999,21.683768999999998,18.9565 318 | 316,594916.4,0.6143718594685197,0.9735536752268672,-0.6530561647377908,0.1809598,-0.0007059999999999999,1.3174633999999998,0.180961189,1.3298333999999998,37.4262739,-122.17577539999999,21.410521999999993,19.20859699999999 319 | 317,594916.6,0.5923629971221089,0.9101635031402111,-0.6387984589673579,0.15255120000000003,0.014421578999999999,1.2568821000000001,0.153231364,1.2661881000000001,37.4262739,-122.1757753,21.146606,19.084129999999995 320 | 318,594916.8,0.6504203635267913,0.9800188895314932,-0.8111919588409364,0.07037119,0.026363328,1.4331597,0.075147384,1.4351286,37.426273200000004,-122.17577479999999,20.834750999999997,18.730767999999998 321 | 319,594917.0,0.6541711087338626,0.999571512453258,-0.9377364926040173,-0.01885015,0.019125938000000002,1.5247952,0.026853857999999998,1.5250317,37.42627279999999,-122.17577390000001,20.475688999999996,18.61979499999999 322 | 320,594917.2,0.6901248637586832,0.936418317258358,-0.9353743563406169,-0.03782486,0.08318802,1.4961143999999997,0.09138362300000001,1.4989027,37.426272600000004,-122.1757735,20.115075999999995,18.462349999999994 323 | 321,594917.4,0.7579372939653695,1.055339933373034,-1.0829510744661093,-0.071899995,0.07725807,1.6944158999999999,0.105538707,1.6976995,37.4262718,-122.1757736,19.723375999999995,17.564679999999996 324 | 322,594917.6,0.8786801192909479,1.0991163365542889,-1.180835091508925,-0.08803763,0.15614551,1.8343903000000001,0.17925413399999998,1.8431277,37.426271899999996,-122.1757726,19.31178299999999,16.88378999999999 325 | 323,594917.8,0.9809739924967289,1.125852563418448,-1.1805696249939501,-0.04096779,0.22849157,1.8954592000000001,0.23213521899999998,1.909621,37.426270200000005,-122.17577109999999,18.884345999999994,16.323903999999985 326 | 324,594918.0,0.9017339451238513,1.1243695467710495,-1.25068876799196,-0.12305934,0.16221097,1.9035671000000003,0.20360746300000002,1.9144251,37.4262686,-122.1757706,18.44005599999999,16.93408399999999 327 | 325,594918.2,0.7369655771180987,1.0145218493416905,-1.1445174552500248,-0.14857613,0.0812435,1.6955271,0.16933804,1.7039623,37.426267200000005,-122.17577040000002,18.052885999999994,15.85922999999999 328 | 326,594918.4,0.70827891305089,1.1010622968897223,-1.1631897874176502,-0.12817146,0.010878328999999999,1.7529137,0.128632269,1.757627,37.4262662,-122.17576969999999,17.63899599999999,15.655359999999995 329 | 327,594918.6,0.7218304350972176,1.0205371724441648,-1.0630222163163126,-0.085661985,0.06522956,1.6436417,0.107670194,1.6471646,37.4262649,-122.17576869999999,17.236133999999993,15.542959999999994 330 | 328,594918.8,0.713036750908941,0.9446075307205319,-0.9411194967105985,-0.030759625,0.0982202,1.5147997,0.10292406,1.5182923000000002,37.426263899999995,-122.17576740000001,16.905866000000003,15.164712999999999 331 | 329,594919.0,0.7861568578518927,0.9532427815720439,-0.8996977037750185,0.030240946,0.15551208,1.5263518999999999,0.158425128,1.5345517,37.4262627,-122.1757666,16.549823999999994,15.315269999999991 332 | 330,594919.2,0.7461347174830735,0.9406363647431135,-0.8164361100643873,0.07692439999999999,0.12834968,1.4503516,0.149636234,1.4580503999999999,37.4262623,-122.17576580000001,16.224399999999996,13.90446699999999 333 | 331,594919.4,0.6879266682080925,0.8736534900963306,-0.7477700305171311,0.078159906,0.11475073,1.3389808,0.138840559,1.3461598,37.4262609,-122.17576559999999,15.922259999999994,14.471899999999998 334 | 332,594919.6,0.6496403585188091,0.9440234005451202,-0.719649841543287,0.12429907,0.044871308,1.3530006,0.13215026800000002,1.3594389999999998,37.426260799999994,-122.17576399999999,15.641739000000001,13.482866000000001 335 | 333,594919.8,0.7480415985919535,0.9628587961196899,-0.7017225809395313,0.1800707,0.11812989,1.3963788000000001,0.21536045399999998,1.4128885,37.426260600000006,-122.17576319999999,15.350272999999994,14.041089999999997 336 | 334,594920.0,0.7310558664612472,0.914658228866756,-0.6660461146384478,0.17811073,0.12942043,1.3351146,0.22016603399999998,1.3531461,37.426260299999996,-122.1757626,15.075153,12.682139999999997 337 | 335,594920.2,0.7479802160523832,1.0530212800949812,-0.7313428805209696,0.20290853,0.070064925,1.4749589,0.214664775,1.4904982,37.426260799999994,-122.17576240000001,14.757461999999997,12.960255999999987 338 | 336,594920.4,0.6560430466197431,1.1245124880224466,-0.6411821749061346,0.2815297,-0.045822747000000004,1.4293393,0.28523445399999997,1.4575216999999998,37.4262612,-122.1757618,14.440245999999995,13.122389999999996 339 | 337,594920.6,0.586781844496727,0.9908325877040625,-0.5155583615414798,0.29011068,-0.033260036,1.2338468,0.292011018,1.2679307,37.4262612,-122.1757603,14.194445999999992,13.602242999999987 340 | 338,594920.8,0.6221939702518284,1.1054514003917575,-0.6194432303309441,0.27803347,-0.064322926,1.3890008999999999,0.285377025,1.4180138999999998,37.4262618,-122.1757608,13.93599299999999,12.004499999999993 341 | 339,594921.0,0.6148276510648429,1.2026394819840789,-0.6468334491364658,0.30389217,-0.12231218,1.4678589,0.327583138,1.5039681999999999,37.4262626,-122.1757601,13.630536,11.985555999999988 342 | 340,594921.2,0.5037084645591676,1.0129192173480988,-0.47249359218403697,0.308786,-0.11533647,1.1873896000000002,0.329622959,1.2322928,37.4262632,-122.17575990000002,13.417476,12.382981999999991 343 | 341,594921.4,0.5674645309336483,1.1407987605780363,-0.5969195105135441,0.29639092,-0.12947005,1.3759271999999998,0.323434801,1.4134305,37.4262641,-122.1757597,13.170566,11.74668299999999 344 | 342,594921.6,0.5049393675290048,0.9985675876960158,-0.497866281773895,0.28165250000000003,-0.10665213,1.1936835,0.301169071,1.2310902,37.4262645,-122.1757592,12.984603999999997,12.223389999999995 345 | 343,594921.8,0.5361078595742583,1.0529554216191173,-0.6247923248447478,0.21892095,-0.109232925,1.3205607,0.24465938399999998,1.3430334,37.4262649,-122.17575890000002,12.750366,11.613381999999987 346 | 344,594922.0,0.46502568991854787,1.0853090323507786,-0.6962683936581016,0.15579776,-0.1866271,1.3556871000000001,0.243110298,1.3773127,37.426264399999994,-122.1757585,12.506025999999999,10.619889999999998 347 | 345,594922.2,0.4054940026253462,1.0522514106705785,-0.7596015338785946,0.06923131,-0.21941216,1.3467818,0.230075355,1.3662928,37.4262645,-122.17575910000001,12.234478000000003,11.607029999999988 348 | 346,594922.4,0.415372300427407,0.9930637516081333,-0.7261240044608712,0.068567365,-0.17953257,1.2908293,0.19218072100000003,1.305057,37.426265,-122.1757594,11.982305999999994,10.222844999999992 349 | 347,594922.6,0.3865208472125232,0.8782620942220092,-0.6477963919751346,0.06237791,-0.14281923,1.153859,0.155847155,1.1643362,37.4262646,-122.17576059999999,11.744485999999995,10.539601999999995 350 | 348,594922.8,0.34069605451077223,0.8650265159085393,-0.6128278067335486,0.06850871,-0.17455807,1.1043317,0.187520562,1.1201395,37.426265,-122.17576090000001,11.511555999999992,10.190009999999994 351 | 349,594923.0,0.3337654978968203,0.8662615204229951,-0.5653351414948702,0.10461654,-0.18108189,1.0733676,0.209129791,1.0935508,37.426264700000004,-122.175762,11.29884599999999,9.871571999999993 352 | 350,594923.2,0.31591796362772584,0.904111628420651,-0.6111148279160261,0.08195565,-0.21634420000000001,1.1190841,0.23134722,1.142747,37.426264200000006,-122.1757622,11.068919999999991,9.681023999999994 353 | 351,594923.4,0.3453552769497037,0.9183685947209597,-0.6226918790489435,0.08962271,-0.19901995,1.1481518,0.21826858,1.1687145,37.426264,-122.1757627,10.815188999999997,8.81064 354 | 352,594923.6,0.3061150275170803,0.8188125109300017,-0.506676176097244,0.11784287,-0.17921841,0.9941310999999999,0.214490513,1.0170068,37.426264,-122.17576340000001,10.611626000000001,8.498944999999992 355 | 353,594923.8,0.4351690374314785,0.47760225646197796,-0.5442272271029651,-0.045732167000000004,0.111715525,0.8421768,0.12071366800000001,0.85078406,37.426264,-122.1757642,10.535579999999989,8.992219999999989 356 | 354,594924.0,0.3758632126264274,0.5548044433817267,-0.5000235703773797,0.009891355,0.020406473,0.8421255,0.022677369,0.8424308,37.4262636,-122.17576480000001,10.375905999999993,9.357781999999993 357 | 355,594924.2,0.4065742609091103,0.46994329150766134,-0.6025695656426251,-0.1052578,0.09159087,0.86039346,0.13952810300000001,0.8716335,37.426264700000004,-122.17576570000001,10.216258999999994,9.30176999999999 358 | 356,594924.4,0.42037126747891307,0.5736981378868222,-0.6055658292025328,-0.049799904000000006,0.04801776,0.9377899,0.069179011,0.9403379999999999,37.426264399999994,-122.1757662,10.044417999999993,7.269749999999995 359 | 357,594924.6,0.5052519510500133,0.8572483006864786,-0.7069487157277763,0.043018844,-0.031132702,1.2258937,0.05310241,1.2270434,37.4262646,-122.17576729999999,9.817402000000001,8.786519999999989 360 | 358,594924.8,0.4984922669827938,0.9696281785145402,-0.7088520382530987,0.097128615,-0.09669837,1.2997302,0.137056716,1.3069365,37.4262646,-122.17576840000001,9.556571999999996,9.244065999999997 361 | 359,594925.0,0.38753525679931045,0.932646730914712,-0.566732813604176,0.15505728,-0.17092133,1.1415778,0.230774483,1.1646702,37.426265,-122.1757693,9.322479000000001,7.585183999999991 362 | 360,594925.2,0.3733652853406966,0.9960003141313791,-0.5410571251995862,0.2034508,-0.21665189999999998,1.1625656,0.29720408600000003,1.1999538,37.426265,-122.1757702,9.081490000000002,7.689299999999996 363 | 361,594925.4,0.3775195125490427,0.9558618571609259,-0.4830992962233722,0.230174,-0.19176129,1.102119,0.299587158,1.1421115,37.4262662,-122.1757706,8.858180999999988,7.215962999999995 364 | 362,594925.6,0.3068845793604851,0.7764549190178514,-0.3592213178053498,0.21340169,-0.15601093,0.87637025,0.26434766600000004,0.91537124,37.4262661,-122.17577150000001,8.679465999999998,7.596289999999996 365 | 363,594925.8,0.33361375890672207,0.7564373752102256,-0.4603539891541004,0.1314422,-0.12272715599999999,0.9356813,0.17983049399999998,0.9528056,37.426266399999996,-122.17577150000001,8.507331999999991,7.09263399999999 366 | 364,594926.0,0.45795965660363436,0.5411877846345305,-0.5880828462541103,-0.040474754,0.09714561,0.92120796,0.105240084,0.9271999,37.4262667,-122.1757717,8.32745599999999,7.479707999999988 367 | 365,594926.2,0.48808449506759644,0.5798047557473183,-0.6040196330286562,-0.023516456,0.10207969,0.96959007,0.104753457,0.97523236,37.4262665,-122.1757729,8.150316000000004,6.368184999999997 368 | 366,594926.4,0.5034514586441219,0.5032151844352484,-0.6682928744703531,-0.10898286,0.15587179999999998,0.96366906,0.190192745,0.9822583,37.426266399999996,-122.17577299999999,7.979790000000001,6.605695999999995 369 | 367,594926.6,0.5039264834485948,0.5776306753978133,-0.6561917397193611,-0.060939413,0.116646394,1.0065354,0.131605448,1.0151027,37.426266399999996,-122.17577320000001,7.726305999999994,6.5811629999999965 370 | 368,594926.8,0.44439840549603105,0.5817455342039466,-0.6194357695057988,-0.04889886,0.06406943,0.9617893,0.080597704,0.9651603999999999,37.426266399999996,-122.17577340000001,7.542567999999989,6.278745999999991 371 | 369,594927.0,0.3381288764066994,0.5793949784711003,-0.5786925945430994,-0.052144893,-0.024627475,0.8905076999999999,0.057668037000000005,0.89237297,37.4262661,-122.17577450000002,7.365851999999997,5.772824999999997 372 | 370,594927.2,0.2713907170109451,0.6081228340044618,-0.546182079706341,-0.03314814,-0.09641402,0.8618368000000001,0.101953241,0.86784625,37.4262661,-122.17577549999999,7.196885999999999,6.2008399999999995 373 | 371,594927.4,0.2879560519941151,0.5962694818153977,-0.497956779319793,0.004412827,-0.07608069,0.831566,0.076208555,0.83505076,37.4262661,-122.175776,7.034942999999991,5.053139999999999 374 | 372,594927.6,0.26961297262459993,0.44560038670897484,-0.3413947890512645,0.045302305,-0.011372853999999998,0.6273840999999999,0.046708038,0.6291204,37.4262667,-122.1757769,6.891255999999984,4.858175000000003 375 | 373,594927.8,0.23726280638948083,0.42736375611275434,-0.22613726137205958,0.11698148,-0.029043363,0.5313984,0.12053291,0.5448967,37.4262662,-122.17577709999999,6.784366000000006,5.602475999999996 376 | 374,594928.0,0.2354213735088706,0.33935706038028,-0.18042135518044233,0.10741884,0.016263029,0.44368067,0.10864296400000001,0.45678859999999993,37.426266,-122.1757777,6.694295999999994,5.815259999999995 377 | 375,594928.2,0.19967058207839727,0.25169531162828207,-0.22788953827694058,0.013058865,0.032684185,0.39848697,0.035196447,0.40003833,37.4262659,-122.1757777,6.618781999999996,4.8358499999999935 378 | 376,594928.4,0.210746836848557,0.21415621135383844,-0.1801182720810175,0.035269905,0.062049534,0.34890565,0.071373041,0.35613096,37.4262657,-122.17577779999999,6.537645999999995,5.671869999999991 379 | 377,594928.6,0.18621169589459896,0.18067916203290224,-0.16140200477093458,0.024971907999999998,0.05910959,0.30465305,0.064168061,0.31133747,37.4262648,-122.17577800000001,6.469420999999997,4.611469999999997 380 | 378,594928.8,0.13144305953755975,0.15675193071365356,-0.18691233126446605,-0.02531998,0.025493987000000003,0.28091258,0.035931111,0.28320122,37.426264700000004,-122.17577779999999,6.405435999999995,4.858459999999994 381 | 379,594929.0,0.1538082961924374,0.13182008732110262,-0.16410382743924856,-0.012793948000000001,0.05770098,0.25975043,0.059102354,0.2663895,37.4262643,-122.17577759999999,6.343169999999986,4.5505099999999885 382 | 380,594929.2,0.17856802977621555,0.12721520476043224,-0.10644476162269711,0.038639326,0.08111028,0.23208384,0.08984361,0.248867,37.4262638,-122.1757775,6.294181999999992,5.526679999999992 383 | 381,594929.4,0.11995867127552629,0.11146264616400003,-0.08506092801690102,0.028549758999999997,0.039890666,0.18371429999999997,0.049054602999999995,0.19015072,37.4262635,-122.17577759999999,6.253385999999992,4.494469999999993 384 | 382,594929.6,0.07903274660930037,0.09234431758522987,-0.07135394681245089,0.01635533,0.015430986,0.14522608,0.022485822000000003,0.14695655,37.426263399999996,-122.17577730000001,6.225675999999993,4.699249999999992 385 | 383,594929.8,0.0938965673558414,0.08521415200084448,-0.055699681863188744,0.02992953,0.031808946,0.13720536,0.043675919,0.14398922,37.426263299999995,-122.17577709999999,6.198385999999999,4.370859999999993 386 | 384,594930.0,0.07674951665103436,0.03200066927820444,-0.04049065476283431,0.009084805,0.045632402999999995,0.08494237,0.046527945999999994,0.09685069,37.4262631,-122.1757767,6.171235999999993,4.775179999999992 387 | 385,594930.2,0.017057256773114204,0.0368688041344285,-0.07218065578490496,-0.032895497999999995,-0.007484659,0.08223079,0.033736241,0.08888215,37.4262627,-122.17577630000001,6.1544059999999945,4.81340999999999 388 | 386,594930.4,0.009262500330805779,0.0038431184366345406,-0.09930123388767242,-0.073944114,0.003504428,0.07321797,0.07402710900000001,0.10411957,37.4262627,-122.1757767,6.140726000000001,4.3813399999999945 389 | 387,594930.6,0.030552214942872524,0.0479425024241209,-0.1389202568680048,-0.07583182,-0.001959189,0.13594165,0.075857127,0.15567413,37.4262626,-122.175777,6.127065999999999,4.5458 390 | 388,594930.8,0.24156151711940765,0.36576126981526613,-0.39731587236747146,-0.049254622000000005,0.00739949,0.5958416999999999,0.049807330999999996,0.5979198,37.4262625,-122.17577730000001,6.0592459999999875,3.9482 391 | 389,594931.0,0.379579390399158,0.5485649248585105,-0.5631247977726161,-0.042226385,0.026874565,0.8778524,0.050053071,0.8792781999999999,37.4262624,-122.17577779999999,5.902925999999994,4.640584999999987 392 | 390,594931.2,0.3380711651407182,0.5687854662537575,-0.5734662148170173,-0.05347073,-0.019026583,0.8801756,0.05675499999999999,0.8820034999999999,37.4262626,-122.1757787,5.719476,4.636919999999989 393 | 391,594931.4,0.2833573129028082,0.44424949027597904,-0.48453082516789436,-0.064613536,0.00098,0.71927845,0.06462097,0.7221754,37.4262626,-122.17577990000001,5.568870999999987,4.637433999999999 394 | 392,594931.6,0.26738554425537586,0.4206096241250634,-0.3992317523807287,-0.014204218999999999,4.98e-05,0.64479417,0.014204306000000002,0.64495057,37.426262799999996,-122.17578020000002,5.437765999999996,3.7793999999999954 395 | 393,594931.8,0.3268694682046771,0.34364392329007387,-0.3612662646919489,-0.0043952159999999995,0.091383666,0.59514195,0.091489297,0.60213304,37.4262624,-122.175781,5.319816000000003,3.9438599999999866 396 | 394,594932.0,0.2870511463843286,0.3218652307987213,-0.30769840348511934,0.014055148,0.06927817,0.5311087,0.070689548,0.53579235,37.426262200000004,-122.17578119999999,5.216015999999996,4.036565999999993 397 | 395,594932.2,0.2515110787935555,0.25557937659323215,-0.29291032208129764,-0.019800974,0.07449467,0.46253648,0.077081346,0.46891528,37.4262626,-122.1757814,5.120345999999998,4.029749999999993 398 | 396,594932.4,0.25119103910401464,0.2892288491129875,-0.2635639985091984,0.02070984,0.056304883,0.46718425,0.059992812,0.47102043,37.4262627,-122.1757818,5.024335999999991,4.173269999999988 399 | 397,594932.6,0.17486898973584175,0.2903048237785697,-0.26355670066550374,-0.00343126,-0.008868572,0.4356273,0.009509212,0.43573108,37.4262624,-122.1757825,4.936790000000002,3.0420000000000016 400 | 398,594932.8,0.16088271886110306,0.2855336042121053,-0.26034391624853015,-0.007860655,-0.018166073,0.42455304,0.01979384,0.4250142,37.4262625,-122.17578229999998,4.852125999999998,3.431439999999995 401 | 399,594933.0,0.1881109937094152,0.24856832344084978,-0.21797371516004205,0.015583733999999998,0.024565095,0.3854704,0.029091178,0.38656658,37.4262626,-122.17578259999999,4.773041000000006,2.8332400000000035 402 | 400,594933.2,0.16452677780762315,0.24406186118721962,-0.1843809182755649,0.032310087,0.0070027169999999994,0.35205197,0.033060245,0.35360086,37.4262625,-122.1757827,4.701172,2.576039999999992 403 | 401,594933.4,0.14282068982720375,0.23109589703381062,-0.17614969611167908,0.025152177999999997,-0.004465152,0.32915490000000003,0.025545442999999998,0.3301447,37.426262799999996,-122.1757831,4.631686000000002,3.191014999999993 404 | 402,594933.6,0.11756986565887928,0.2308507664129138,-0.1598298354074359,0.029814206,-0.025707409,0.30839354,0.039366963,0.310896,37.4262625,-122.1757831,4.568205999999989,3.021819999999991 405 | 403,594933.8,0.09983092173933983,0.2130537796765566,-0.17100150138139725,0.0060466140000000005,-0.03124481,0.29571885,0.031824514,0.29742637,37.4262629,-122.17578300000001,4.508255999999989,3.555579999999992 406 | 404,594934.0,0.08949135476723313,0.2201566221192479,-0.15782197332009673,0.016820428999999998,-0.043778803,0.28811090000000006,0.04689894,0.29190308,37.4262627,-122.1757832,4.449566000000004,2.4534599999999926 407 | 405,594934.2,0.09260682156309485,0.2093411348760128,-0.17709233984351158,-0.003038092,-0.035382386,0.29386999999999996,0.035512579,0.29600796,37.426262799999996,-122.17578359999999,4.389415999999997,2.7300499999999914 408 | 406,594934.4,0.11129097081720829,0.17295774538069963,-0.19560657953843474,-0.03040989,-0.000193,0.28856742,0.030410502000000002,0.2901654,37.426262799999996,-122.17578370000001,4.329459999999997,2.152239999999992 409 | 407,594934.6,0.14382051583379507,0.14213087316602468,-0.19743305211886764,-0.037190236,0.043756462999999995,0.28271294,0.057425965999999995,0.2884863,37.4262627,-122.1757841,4.256820999999988,1.628354999999985 410 | 408,594934.8,0.1657846411690116,0.13562538474798203,-0.19595599407330155,-0.03225539,0.06581161,0.28673086,0.073291053,0.2959496,37.4262626,-122.17578400000001,4.183085999999989,2.085179999999994 411 | 409,594935.0,0.19760386645793915,0.14852175768464804,-0.1293125730007887,0.037600104,0.085876495,0.26835328,0.09374721300000001,0.28425696,37.4262623,-122.17578390000001,4.109666000000004,1.7860499999999888 412 | 410,594935.2,0.2080995012074709,0.1660080263391137,-0.07490315660834312,0.09320021,0.08544848,0.25147852,0.126442565,0.28147677,37.4262619,-122.1757838,4.031285999999994,0.7628199999999907 413 | 411,594935.4,0.26920503890141845,0.18884186632931232,-0.041960455011576414,0.15088278,0.12501001,0.27264687,0.195941618,0.33575204,37.4262618,-122.1757835,3.94592999999999,-0.7536400000000043 414 | 412,594935.6,0.05588123481720686,0.08657816518098116,-0.06241839379072189,0.012992595,-0.001094337,0.1261292,0.013038601,0.12680134,37.4262618,-122.1757832,3.868876,0.4290499999999895 415 | 413,594935.8,0.019683644641190767,-0.009151900187134743,0.018489934969693422,0.016285663,0.019245151000000002,-0.0026967659999999997,0.025211082000000003,0.025354905,37.426261700000005,-122.17578329999999,3.838805999999991,1.1615400000000022 416 | -------------------------------------------------------------------------------- /data/fix.csv: -------------------------------------------------------------------------------- 1 | Provider,Latitude,Longitude,Altitude,Speed,Accuracy,(UTC)TimeInMs 2 | gps,37.426536,-122.175789,1.245148,0.000000,3.216000,1581800799000 3 | gps,37.426537,-122.175789,1.261780,0.000000,3.216000,1581800800000 4 | gps,37.426537,-122.175789,1.253906,0.000000,3.216000,1581800801000 5 | gps,37.426537,-122.175789,1.255890,0.000000,3.216000,1581800802000 6 | gps,37.426537,-122.175789,1.256592,0.000000,3.216000,1581800803000 7 | gps,37.426537,-122.175789,1.254669,0.000000,3.216000,1581800804000 8 | gps,37.426537,-122.175789,1.254639,0.000000,3.216000,1581800805000 9 | gps,37.426537,-122.175789,1.253265,0.000000,3.216000,1581800806000 10 | gps,37.426537,-122.175789,1.260681,0.000000,3.216000,1581800807000 11 | gps,37.426535,-122.175791,1.568817,0.000000,3.216000,1581800808000 12 | gps,37.426537,-122.175793,1.651123,0.000000,3.216000,1581800809000 13 | gps,37.426538,-122.175792,2.277557,0.000000,3.216000,1581800810000 14 | gps,37.426541,-122.175791,2.506287,0.000000,3.216000,1581800811000 15 | gps,37.426542,-122.175790,2.661255,0.000000,3.216000,1581800812000 16 | gps,37.426547,-122.175786,2.009155,0.000000,3.216000,1581800813000 17 | gps,37.426547,-122.175785,1.946899,0.210000,3.216000,1581800814000 18 | gps,37.426545,-122.175785,1.915192,0.600000,3.216000,1581800815000 19 | gps,37.426536,-122.175789,1.911926,1.080000,3.216000,1581800816000 20 | gps,37.426524,-122.175793,3.250763,1.350000,3.216000,1581800817000 21 | gps,37.426511,-122.175794,2.900330,1.350000,3.216000,1581800818000 22 | gps,37.426500,-122.175796,2.903870,1.170000,3.216000,1581800819000 23 | gps,37.426491,-122.175797,2.649506,1.020000,3.216000,1581800820000 24 | gps,37.426483,-122.175799,2.751007,1.020000,3.216000,1581800821000 25 | gps,37.426475,-122.175800,2.826294,0.980000,3.216000,1581800822000 26 | gps,37.426464,-122.175803,2.947418,1.210000,3.216000,1581800823000 27 | gps,37.426452,-122.175809,2.929504,1.340000,3.216000,1581800824000 28 | gps,37.426441,-122.175815,2.996399,1.310000,3.216000,1581800825000 29 | gps,37.426430,-122.175821,3.178162,1.300000,3.216000,1581800826000 30 | gps,37.426418,-122.175828,3.648041,1.420000,3.216000,1581800827000 31 | gps,37.426406,-122.175833,3.832031,1.320000,3.216000,1581800828000 32 | gps,37.426396,-122.175838,3.796082,1.190000,3.216000,1581800829000 33 | gps,37.426387,-122.175843,3.577881,1.180000,3.216000,1581800830000 34 | gps,37.426378,-122.175848,3.803741,1.120000,3.216000,1581800831000 35 | gps,37.426369,-122.175854,3.571228,1.200000,3.216000,1581800832000 36 | gps,37.426359,-122.175862,3.721680,1.210000,3.216000,1581800833000 37 | gps,37.426350,-122.175868,3.324524,1.200000,3.216000,1581800834000 38 | gps,37.426340,-122.175874,3.216309,1.250000,3.216000,1581800835000 39 | gps,37.426331,-122.175880,3.228973,1.110000,3.216000,1581800836000 40 | gps,37.426323,-122.175884,2.943695,0.910000,3.216000,1581800837000 41 | gps,37.426318,-122.175887,3.005157,0.590000,3.216000,1581800838000 42 | gps,37.426317,-122.175887,2.707001,0.000000,3.216000,1581800839000 43 | gps,37.426318,-122.175886,2.504089,0.000000,3.216000,1581800840000 44 | gps,37.426318,-122.175886,2.343964,0.000000,3.216000,1581800841000 45 | gps,37.426318,-122.175887,2.841888,0.000000,3.216000,1581800842000 46 | gps,37.426319,-122.175887,2.710571,0.000000,3.216000,1581800843000 47 | gps,37.426316,-122.175875,2.364716,1.170000,3.216000,1581800844000 48 | gps,37.426313,-122.175862,2.334930,1.010000,3.216000,1581800845000 49 | gps,37.426311,-122.175850,2.307617,0.870000,3.216000,1581800846000 50 | gps,37.426308,-122.175842,2.416962,0.690000,3.216000,1581800847000 51 | gps,37.426305,-122.175832,2.482269,0.700000,3.216000,1581800848000 52 | gps,37.426303,-122.175825,2.142151,0.580000,3.216000,1581800849000 53 | gps,37.426300,-122.175820,2.134186,0.480000,3.216000,1581800850000 54 | gps,37.426297,-122.175818,2.155090,0.390000,3.216000,1581800851000 55 | gps,37.426294,-122.175815,1.243286,0.530000,3.216000,1581800852000 56 | gps,37.426290,-122.175808,1.308868,0.920000,3.216000,1581800853000 57 | gps,37.426288,-122.175799,1.283539,0.870000,3.216000,1581800854000 58 | gps,37.426286,-122.175790,1.624390,0.880000,3.216000,1581800855000 59 | gps,37.426285,-122.175781,1.484314,0.860000,3.216000,1581800856000 60 | gps,37.426283,-122.175773,1.024841,0.850000,3.216000,1581800857000 61 | gps,37.426281,-122.175764,1.011627,0.970000,3.216000,1581800858000 62 | gps,37.426278,-122.175753,0.813507,1.060000,3.216000,1581800859000 63 | gps,37.426275,-122.175742,0.884186,1.020000,3.216000,1581800860000 64 | gps,37.426272,-122.175730,1.194489,1.100000,3.216000,1581800861000 65 | gps,37.426269,-122.175719,1.365509,1.010000,3.216000,1581800862000 66 | gps,37.426267,-122.175709,1.550964,0.910000,3.216000,1581800863000 67 | gps,37.426265,-122.175699,1.705505,1.030000,3.216000,1581800864000 68 | gps,37.426263,-122.175686,1.802521,1.120000,3.216000,1581800865000 69 | gps,37.426260,-122.175671,1.755371,1.350000,3.216000,1581800866000 70 | gps,37.426258,-122.175660,1.769714,0.970000,3.216000,1581800867000 71 | gps,37.426255,-122.175648,1.942200,1.070000,3.216000,1581800868000 72 | gps,37.426253,-122.175637,1.555939,1.150000,3.216000,1581800869000 73 | gps,37.426250,-122.175626,1.124481,1.100000,3.216000,1581800870000 74 | gps,37.426247,-122.175616,1.384430,0.930000,3.216000,1581800871000 75 | gps,37.426245,-122.175606,1.454742,0.880000,3.216000,1581800872000 76 | gps,37.426242,-122.175597,1.465271,0.790000,3.216000,1581800873000 77 | gps,37.426241,-122.175588,1.606354,0.800000,3.216000,1581800874000 78 | gps,37.426240,-122.175581,1.803467,0.630000,3.216000,1581800875000 79 | gps,37.426242,-122.175580,2.016205,0.000000,3.216000,1581800876000 80 | gps,37.426242,-122.175580,2.347382,0.000000,3.216000,1581800877000 81 | gps,37.426245,-122.175580,2.571899,0.270000,3.216000,1581800878000 82 | gps,37.426246,-122.175582,2.377502,0.000000,3.216000,1581800879000 83 | gps,37.426244,-122.175584,2.325806,0.000000,3.216000,1581800880000 84 | gps,37.426251,-122.175580,2.535950,0.740000,3.216000,1581800881000 85 | gps,37.426262,-122.175573,3.046722,1.470000,3.216000,1581800882000 86 | gps,37.426273,-122.175567,2.961761,1.430000,3.216000,1581800883000 87 | gps,37.426285,-122.175560,2.841736,1.230000,3.216000,1581800884000 88 | gps,37.426296,-122.175554,3.010132,1.240000,3.216000,1581800885000 89 | gps,37.426305,-122.175549,2.976532,1.070000,3.216000,1581800886000 90 | gps,37.426314,-122.175543,3.249939,1.080000,3.216000,1581800887000 91 | gps,37.426323,-122.175538,3.333923,1.150000,3.216000,1581800888000 92 | gps,37.426332,-122.175532,2.979004,1.310000,3.216000,1581800889000 93 | gps,37.426343,-122.175526,2.712341,1.360000,3.216000,1581800890000 94 | gps,37.426354,-122.175522,2.564606,1.380000,3.216000,1581800891000 95 | gps,37.426364,-122.175517,2.763092,1.200000,3.216000,1581800892000 96 | gps,37.426374,-122.175514,2.687927,1.270000,3.216000,1581800893000 97 | gps,37.426384,-122.175510,2.794800,1.310000,3.216000,1581800894000 98 | gps,37.426395,-122.175506,2.545074,1.300000,3.216000,1581800895000 99 | gps,37.426406,-122.175501,2.358276,1.280000,3.216000,1581800896000 100 | gps,37.426416,-122.175497,2.406342,1.230000,3.216000,1581800897000 101 | gps,37.426429,-122.175494,2.092468,1.390000,3.216000,1581800898000 102 | gps,37.426442,-122.175489,2.032806,1.390000,3.216000,1581800899000 103 | gps,37.426450,-122.175486,2.275513,0.870000,3.216000,1581800900000 104 | gps,37.426452,-122.175484,2.151184,0.000000,3.216000,1581800901000 105 | gps,37.426452,-122.175483,2.005890,0.000000,3.216000,1581800902000 106 | gps,37.426451,-122.175482,1.875122,0.000000,3.216000,1581800903000 107 | gps,37.426452,-122.175483,2.188629,0.000000,3.216000,1581800904000 108 | gps,37.426451,-122.175485,1.892700,0.190000,3.216000,1581800905000 109 | gps,37.426451,-122.175487,2.030304,0.270000,3.216000,1581800906000 110 | gps,37.426453,-122.175489,1.419922,0.290000,3.216000,1581800907000 111 | gps,37.426456,-122.175491,1.071808,0.600000,3.216000,1581800908000 112 | gps,37.426458,-122.175495,1.034668,0.680000,3.216000,1581800909000 113 | gps,37.426463,-122.175500,1.254211,1.060000,3.216000,1581800910000 114 | gps,37.426468,-122.175508,1.145569,1.170000,3.216000,1581800911000 115 | gps,37.426474,-122.175518,1.236572,1.240000,3.216000,1581800912000 116 | gps,37.426481,-122.175530,1.416870,1.460000,3.216000,1581800913000 117 | gps,37.426480,-122.175543,1.292053,0.000000,3.216000,1581800914000 118 | gps,37.426485,-122.175552,1.674469,1.060000,3.216000,1581800915000 119 | gps,37.426486,-122.175560,2.079895,0.230000,3.216000,1581800916000 120 | gps,37.426487,-122.175565,2.605957,0.160000,3.216000,1581800917000 121 | gps,37.426495,-122.175572,3.451385,1.360000,3.216000,1581800918000 122 | gps,37.426504,-122.175582,4.048523,1.410000,3.216000,1581800919000 123 | gps,37.426507,-122.175595,4.307251,1.070000,3.216000,1581800920000 124 | gps,37.426512,-122.175609,4.492889,1.310000,3.216000,1581800921000 125 | gps,37.426514,-122.175623,4.676758,0.000000,3.216000,1581800922000 126 | gps,37.426517,-122.175638,4.981506,0.170000,3.216000,1581800923000 127 | gps,37.426520,-122.175654,4.575928,0.000000,3.216000,1581800924000 128 | gps,37.426519,-122.175668,5.018585,0.370000,3.216000,1581800925000 129 | gps,37.426526,-122.175683,4.513245,0.250000,3.216000,1581800926000 130 | gps,37.426533,-122.175703,4.687225,0.280000,3.216000,1581800927000 131 | gps,37.426537,-122.175721,5.382599,0.320000,3.216000,1581800928000 132 | gps,37.426536,-122.175736,5.052551,0.180000,3.216000,1581800929000 133 | gps,37.426539,-122.175745,4.982819,0.460000,3.216000,1581800930000 134 | gps,37.426546,-122.175751,4.538391,0.910000,3.216000,1581800931000 135 | gps,37.426549,-122.175754,4.423920,0.250000,3.216000,1581800932000 136 | gps,37.426548,-122.175756,4.058441,0.000000,3.216000,1581800933000 137 | gps,37.426545,-122.175757,3.635498,0.000000,3.216000,1581800934000 138 | gps,37.426543,-122.175755,3.422211,0.000000,3.216000,1581800935000 139 | gps,37.426543,-122.175757,3.496704,0.200000,3.216000,1581800936000 140 | gps,37.426545,-122.175756,3.671356,0.000000,3.216000,1581800937000 141 | gps,37.426545,-122.175758,3.453674,0.000000,3.216000,1581800938000 142 | gps,37.426545,-122.175759,3.445648,0.000000,3.216000,1581800939000 143 | gps,37.426546,-122.175765,3.305237,0.000000,3.216000,1581800940000 144 | gps,37.426546,-122.175766,3.369446,0.000000,3.216000,1581800941000 145 | gps,37.426546,-122.175767,3.371033,0.000000,3.216000,1581800942000 146 | gps,37.426546,-122.175767,3.531036,0.000000,3.216000,1581800943000 147 | -------------------------------------------------------------------------------- /data_processing.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import pandas as pd 4 | import math 5 | import numpy as np 6 | from lib import gpstime 7 | import datetime 8 | import matplotlib.pyplot as plt 9 | import pymap3d 10 | from mpl_toolkits.mplot3d import Axes3D 11 | 12 | # import warnings 13 | # warnings.simplefilter(action='ignore', category='DtypeWarning') 14 | 15 | # read csv file into a dataframe 16 | df = pd.read_csv("./data/FLY084.csv")[1:-4] 17 | 18 | 19 | time_utc = df['GPS:dateTimeStamp'].to_numpy() 20 | N = time_utc.shape[0] 21 | # gps time: (gpsWeek, gpsSOW, gpsDay, gpsSOD) 22 | time_gps = np.zeros((N,4)) 23 | for ii in range(N): 24 | utc = time_utc[ii][:-1] 25 | year = int(utc[0:4]) 26 | month = int(utc[5:7]) 27 | day = int(utc[8:10]) 28 | hour = int(utc[11:13]) 29 | min = int(utc[14:16]) 30 | sec = int(utc[17:19]) 31 | time_gps[ii,:] = gpstime.gpsFromUTC(year,month,day,hour,min,sec) 32 | # print(time_gps[ii,1]) 33 | 34 | 35 | # sec_offset = np.tile(np.array([0.0,0.2,0.4,0.6,0.8]),(1,int(N/5))).T 36 | tt_time = time_gps[0,1] 37 | tt = 0 38 | types = dict() 39 | while tt < N-1: 40 | tt_num = 0 41 | while time_gps[tt,1] == tt_time: 42 | tt_num +=1 43 | tt += 1 44 | if tt_num in types: 45 | types[tt_num] += 1 46 | else: 47 | types[tt_num] = 1 48 | # print(time_gps[tt-tt_num:tt,1].shape) 49 | # print(np.linspace(0.0,1.0-(1.0/tt_num),tt_num).reshape(tt_num,1).shape) 50 | time_gps[tt-tt_num:tt,1] += np.linspace(0.0,1.0-(1.0/tt_num),tt_num) 51 | tt_time = time_gps[tt,1] 52 | 53 | # plt.figure() 54 | # plt.plot(np.arange(N),time_gps[:,1]) 55 | # for ii in range(N): 56 | # print(ii,time_gps[ii,1]) 57 | 58 | # import velocities 59 | vel_n = df['IMU_ATTI(0):velN[meters/Sec]'].to_numpy() 60 | vel_e = df['IMU_ATTI(0):velE[meters/Sec]'].to_numpy() 61 | vel_d = df['IMU_ATTI(0):velD[meters/Sec]'].to_numpy() 62 | vel_h = df['IMU_ATTI(0):velH[meters/Sec]'].to_numpy() 63 | vel_comp = df['IMU_ATTI(0):velComposite[meters/Sec]'].to_numpy() 64 | print(vel_n.shape) 65 | 66 | # import gps lat, lon, h 67 | gps_lat = df['GPS(0):Lat[degrees]'].to_numpy() 68 | gps_lon = df['GPS(0):Long[degrees]'].to_numpy() 69 | baro_h = df['IMU_ATTI(0):barometer:Raw[meters]'].to_numpy() 70 | gps_h = df['IMU_ATTI(0):barometer:Smooth[meters]'].to_numpy().copy() 71 | 72 | vel_ecef = np.zeros((N,3)) 73 | 74 | for ii in range(N): 75 | vel_ecef[ii] = pymap3d.ned2ecef(n=vel_n[ii], e=vel_e[ii], d=vel_d[ii], lat0=gps_lat[0], lon0=gps_lon[0], h0=baro_h[0],deg=True) 76 | vel_ecef -= vel_ecef[0] 77 | vel_ecef_comp = np.linalg.norm(vel_ecef,axis=1) 78 | 79 | 80 | # parse flight 1 data 81 | f1_initial = np.argwhere(time_gps[:,1] == 594416.0).item(0) 82 | f1_final = np.argwhere(time_gps[:,1] == 594561.0).item(0) 83 | gps_h -= gps_h[f1_initial] 84 | baro_h -= (baro_h[f1_initial] + 0.95) # subtract initial baro plus arbitrary offset 85 | print(f1_initial,f1_final) 86 | 87 | # save to file 88 | df1 = pd.DataFrame() 89 | df1['seconds of week [s]'] = time_gps[f1_initial:f1_final,1] 90 | df1['ECEF_vel_x'] = vel_ecef[f1_initial:f1_final,0] 91 | df1['ECEF_vel_y'] = vel_ecef[f1_initial:f1_final,1] 92 | df1['ECEF_vel_z'] = vel_ecef[f1_initial:f1_final,2] 93 | df1['IMU_ATTI(0):velN[meters/Sec]'] = vel_n[f1_initial:f1_final] 94 | df1['IMU_ATTI(0):velE[meters/Sec]'] = vel_e[f1_initial:f1_final] 95 | df1['IMU_ATTI(0):velD[meters/Sec]'] = vel_d[f1_initial:f1_final] 96 | df1['IMU_ATTI(0):velH[meters/Sec]'] = vel_h[f1_initial:f1_final] 97 | df1['IMU_ATTI(0):velComposite[meters/Sec]'] = vel_comp[f1_initial:f1_final] 98 | df1['GPS(0):Lat[degrees]'] = gps_lat[f1_initial:f1_final] 99 | df1['GPS(0):Long[degrees]'] = gps_lon[f1_initial:f1_final] 100 | df1['GPS(0):heightMSL[meters]'] = gps_h[f1_initial:f1_final] 101 | df1['Normalized barometer:Raw[meters]'] = baro_h[f1_initial:f1_final] 102 | df1.to_csv('./data/dji_data_flight_1.csv') 103 | 104 | fig, ax = plt.subplots() 105 | ax.ticklabel_format(useOffset=False) 106 | plt.plot(gps_lon[f1_initial:f1_final],gps_lat[f1_initial:f1_final]) 107 | plt.title("DJI Proprietary Position Solution") 108 | # plt.show() 109 | 110 | # parse flight 2 data 111 | f2_initial = np.argwhere(time_gps[:,1] == 594595.0).item(0) 112 | f2_final = np.argwhere(time_gps[:,1] == 594790.0).item(0) 113 | print(f2_initial,f2_final) 114 | 115 | # save to file 116 | df2 = pd.DataFrame() 117 | df2['seconds of week [s]'] = time_gps[f2_initial:f2_final,1] 118 | df2['ECEF_vel_x'] = vel_ecef[f2_initial:f2_final,0] 119 | df2['ECEF_vel_y'] = vel_ecef[f2_initial:f2_final,1] 120 | df2['ECEF_vel_z'] = vel_ecef[f2_initial:f2_final,2] 121 | df2['IMU_ATTI(0):velN[meters/Sec]'] = vel_n[f2_initial:f2_final] 122 | df2['IMU_ATTI(0):velE[meters/Sec]'] = vel_e[f2_initial:f2_final] 123 | df2['IMU_ATTI(0):velD[meters/Sec]'] = vel_d[f2_initial:f2_final] 124 | df2['IMU_ATTI(0):velH[meters/Sec]'] = vel_h[f2_initial:f2_final] 125 | df2['IMU_ATTI(0):velComposite[meters/Sec]'] = vel_comp[f2_initial:f2_final] 126 | df2['GPS(0):Lat[degrees]'] = gps_lat[f2_initial:f2_final] 127 | df2['GPS(0):Long[degrees]'] = gps_lon[f2_initial:f2_final] 128 | df2['GPS(0):heightMSL[meters]'] = gps_h[f2_initial:f2_final] 129 | df2['Normalized barometer:Raw[meters]'] = baro_h[f2_initial:f2_final] 130 | df2.to_csv('./data/dji_data_flight_2.csv') 131 | 132 | # parse flight 3 data 133 | f3_initial = np.argwhere(time_gps[:,1] == 594853.0).item(0) 134 | f3_final = N-1 135 | print(f3_initial,f3_final) 136 | 137 | # save to file 138 | df3 = pd.DataFrame() 139 | df3['seconds of week [s]'] = time_gps[f3_initial:f3_final,1] 140 | df3['ECEF_vel_x'] = vel_ecef[f3_initial:f3_final,0] 141 | df3['ECEF_vel_y'] = vel_ecef[f3_initial:f3_final,1] 142 | df3['ECEF_vel_z'] = vel_ecef[f3_initial:f3_final,2] 143 | df3['IMU_ATTI(0):velN[meters/Sec]'] = vel_n[f3_initial:f3_final] 144 | df3['IMU_ATTI(0):velE[meters/Sec]'] = vel_e[f3_initial:f3_final] 145 | df3['IMU_ATTI(0):velD[meters/Sec]'] = vel_d[f3_initial:f3_final] 146 | df3['IMU_ATTI(0):velH[meters/Sec]'] = vel_h[f3_initial:f3_final] 147 | df3['IMU_ATTI(0):velComposite[meters/Sec]'] = vel_comp[f3_initial:f3_final] 148 | df3['GPS(0):Lat[degrees]'] = gps_lat[f3_initial:f3_final] 149 | df3['GPS(0):Long[degrees]'] = gps_lon[f3_initial:f3_final] 150 | df3['GPS(0):heightMSL[meters]'] = gps_h[f3_initial:f3_final] 151 | df3['Normalized barometer:Raw[meters]'] = baro_h[f3_initial:f3_final] 152 | df3.to_csv('./data/dji_data_flight_3.csv') 153 | 154 | plt.figure() 155 | plt.plot(time_gps[:,1],vel_n,label='N') 156 | plt.plot(time_gps[:,1],vel_e,label='E') 157 | plt.plot(time_gps[:,1],vel_d,label='D') 158 | # plt.plot(time_gps[:,1],vel_h,label='H') 159 | plt.legend() 160 | 161 | fig = plt.figure() 162 | ax = fig.gca(projection='3d') 163 | ax.plot(gps_lon, gps_lat, gps_h, label='ned') 164 | ax.legend() 165 | 166 | fig = plt.figure() 167 | plt.plot(time_gps[:,1],vel_ecef[:,0],label='x') 168 | plt.plot(time_gps[:,1],vel_ecef[:,1],label='y') 169 | plt.plot(time_gps[:,1],vel_ecef[:,2],label='z') 170 | plt.legend() 171 | 172 | plt.figure() 173 | plt.plot(time_gps[:,1],vel_comp) 174 | plt.plot(time_gps[:,1],vel_ecef_comp) 175 | 176 | 177 | plt.figure() 178 | plt.plot(time_gps[f1_initial:f1_final,1],vel_comp[f1_initial:f1_final]) 179 | 180 | plt.figure() 181 | plt.plot(time_gps[f2_initial:f2_final,1],vel_comp[f2_initial:f2_final]) 182 | 183 | plt.figure() 184 | plt.plot(time_gps[f3_initial:f3_final,1],vel_comp[f3_initial:f3_final]) 185 | 186 | # plt.show() 187 | 188 | 189 | 190 | 191 | 192 | # time_gps = gpstime.gpsfromutc(time_utc) 193 | -------------------------------------------------------------------------------- /docs/img/fusion-3d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/betaBison/gnss-sensor-fusion/db2b1a9a1134ae85ae023bc8c1360837b288533c/docs/img/fusion-3d.png -------------------------------------------------------------------------------- /gnss_fusion_ekf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | Author(s): D. Knowles 4 | Date: 14 Feb 2020 5 | Desc: AA272 sensor fusion project 6 | """ 7 | 8 | import numpy as np 9 | from scipy.io import loadmat 10 | import numpy as np 11 | import pandas as pd 12 | import matplotlib.pyplot as plt 13 | import pyproj 14 | import math 15 | import progress.bar 16 | from mpl_toolkits.mplot3d import Axes3D 17 | 18 | class EKF(): 19 | def __init__(self,sat_file,odom_file=None): 20 | # read in data files as dataframe 21 | self.sat_df = pd.read_csv(sat_file, index_col=0) 22 | self.odom_file = odom_file 23 | if odom_file != None: 24 | self.odom_df = pd.read_csv(odom_file, index_col=0) 25 | 26 | # initial lat and lon from dji data 27 | lat0 = np.mean(self.odom_df['GPS(0):Lat[degrees]'][0]) 28 | lon0 = np.mean(self.odom_df['GPS(0):Long[degrees]'][0]) 29 | h0 = 0.0 30 | 31 | # convert lat lon to ecef frame 32 | self.lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84') 33 | self.ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84') 34 | x0, y0, z0 = pyproj.transform(self.lla, self.ecef, lon0, lat0, h0 , radians=False) 35 | 36 | # concatenate possible time steps from each data file 37 | self.times = np.concatenate((self.odom_df['seconds of week [s]'].to_numpy(),self.sat_df['seconds of week [s]'].to_numpy())) 38 | 39 | # sort timesteps and force unique 40 | self.times = np.sort(np.unique(self.times)) 41 | 42 | self.initialized_odom = False 43 | 44 | def find_nearest(array, value): 45 | array = np.asarray(array) 46 | idx = (np.abs(array - value)).argmin() 47 | return idx 48 | 49 | # indexes for plotting truth later 50 | self.truth_indexes = [] 51 | for ii in range(len(self.times)): 52 | ix = find_nearest(self.odom_df['seconds of week [s]'].to_numpy(),self.times[ii]) 53 | self.truth_indexes.append(ix) 54 | 55 | else: 56 | # set initial positions 57 | x0 = 0. 58 | y0 = 0. 59 | z0 = 0. 60 | 61 | # initialize times 62 | self.times = self.sat_df['seconds of week [s]'].to_numpy() 63 | 64 | 65 | 66 | # initialize state vector [ x, y, z ] 67 | self.mu = np.array([[x0,y0,z0,0.0]]).T 68 | self.mu_n = self.mu.shape[0] 69 | self.mu_history = self.mu.copy() 70 | 71 | # initialize covariance matrix 72 | self.P = np.eye(self.mu_n)*10E2 73 | self.P_history = [np.trace(self.P)] 74 | 75 | if odom_file != None: 76 | t0 = self.sat_df['seconds of week [s]'].to_numpy()[0] 77 | x_calc = [x0,y0,z0] 78 | bu_calc = 0.0 79 | input = self.sat_df[self.sat_df['seconds of week [s]'] == t0] 80 | for ii in range(20): 81 | x_calc, bu_calc = self.least_squares(x_calc,bu_calc,input) 82 | 83 | self.mu[3][0] = bu_calc 84 | 85 | if odom_file != None and 'pr [m]' in self.sat_df.columns: 86 | # only use the best satellites 87 | self.cutoff_angle = 20.0 88 | self.check_data(self.mu,lat0,lon0,False) 89 | 90 | 91 | def ECEF_2_ENU(self,x_ECEF,xref,lat0,lon0): 92 | """ 93 | input(s) 94 | x_ECEF: 3 X N array in ECEF 95 | xref: reference [x,y,z] location 96 | lat0: latitude reference 97 | lon0: longitude reference 98 | output(s): 99 | x_ENU: 3 X N array in east north up 100 | """ 101 | x_ECEF_ref = xref 102 | 103 | x_REF = np.repeat(x_ECEF_ref,x_ECEF.shape[1],axis=1) 104 | theta_lat = np.radians(lat0) 105 | theta_long = np.radians(lon0) 106 | T_enu = np.array([[-np.sin(theta_long), 107 | np.cos(theta_long), 108 | 0.], 109 | [-np.sin(theta_lat)*np.cos(theta_long), 110 | -np.sin(theta_lat)*np.sin(theta_long), 111 | np.cos(theta_lat)], 112 | [np.cos(theta_lat)*np.cos(theta_long), 113 | np.cos(theta_lat)*np.sin(theta_long), 114 | np.sin(theta_lat)]]) 115 | x_ENU = np.dot(T_enu,(x_ECEF-x_REF)) 116 | return x_ENU 117 | 118 | def check_data(self,xref,lat0,lon0,plot_it = False): 119 | """ 120 | Desc: Filters self.sat_df to only be satellites whose elevation 121 | angles are above the cutoff angle (self.cutoff_angle) 122 | 123 | input(s) 124 | xref: reference [x,y,z] location 125 | lat0: latitude reference 126 | lon0: longitude reference 127 | plot_it: if True, it plots used satellites 128 | """ 129 | sv_x = self.sat_df['sat x ECEF [m]'].to_numpy().reshape((1,-1)) 130 | sv_y = self.sat_df['sat y ECEF [m]'].to_numpy().reshape((1,-1)) 131 | sv_z = self.sat_df['sat z ECEF [m]'].to_numpy().reshape((1,-1)) 132 | sv_time = self.sat_df['seconds of week [s]'].to_numpy() 133 | sv_xyz = np.vstack((sv_x,sv_y,sv_z)) 134 | sv_ENU = self.ECEF_2_ENU(sv_xyz,self.mu[:3],lat0,lon0) 135 | elev_angles = np.degrees(np.arctan2(sv_ENU[2,:],np.sqrt(sv_ENU[0,:]**2 + sv_ENU[1,:]**2))) 136 | self.sat_df['elevation_angle'] = elev_angles 137 | 138 | if plot_it: 139 | SVs = np.sort(np.unique(self.sat_df['SV'])) 140 | for sv in SVs: 141 | sv_subset = self.sat_df[self.sat_df['SV'] == sv] 142 | subset_time = sv_subset['seconds of week [s]'].to_numpy() 143 | subset_angle = sv_subset['elevation_angle'].to_numpy() 144 | if True: 145 | plt.plot(subset_time,subset_angle,label=sv) 146 | plt.ylabel('Elevation Angle [degrees]') 147 | plt.legend() 148 | plt.title("Elevation Angle vs. Time") 149 | plt.legend() 150 | plt.show() 151 | 152 | self.sat_df = self.sat_df[self.sat_df['elevation_angle'] > self.cutoff_angle] 153 | 154 | def least_squares(self,x_0,bu,sat_df): 155 | """ 156 | input(s) 157 | x: [3 x 1] state estimate 158 | bu: clock bias 159 | sat_df: satellite data frame 160 | output(s) 161 | x: [3 x 1] new state estimate 162 | bu: new clock bias 163 | """ 164 | numSats = len(sat_df) 165 | dist = np.zeros((numSats,1)) 166 | 167 | G = np.zeros((numSats,4)) 168 | W = np.eye(numSats) 169 | for ii in range(numSats): 170 | x_s = sat_df['sat x ECEF [m]'].to_numpy()[ii] 171 | y_s = sat_df['sat y ECEF [m]'].to_numpy()[ii] 172 | z_s = sat_df['sat z ECEF [m]'].to_numpy()[ii] 173 | 174 | dist[ii] = np.sqrt((x_s-x_0[0])**2 + \ 175 | (y_s-x_0[1])**2 + \ 176 | (z_s-x_0[2])**2) 177 | G[ii,:] = [-(x_s - x_0[0])/dist[ii], 178 | -(y_s - x_0[1])/dist[ii], 179 | -(z_s - x_0[2])/dist[ii], 180 | 1.0] 181 | W[ii,ii] *= 1./sat_df['Pr_sigma'].to_numpy()[ii] 182 | 183 | c = 299792458.0 184 | relativity = sat_df['idk wtf this is'].to_numpy().reshape(-1,1) * c # adjusting for relativity?? 185 | rho_0 = dist + bu - relativity 186 | rho_dif = sat_df['pr [m]'].to_numpy().reshape(-1,1) - rho_0 187 | # delta = np.linalg.inv(G.T.dot(G)).dot(G.T).dot(rho_dif) 188 | delta = np.linalg.pinv(W.dot(G)).dot(W).dot(rho_dif) 189 | x_new = x_0 + delta[0:3,0] 190 | bu_new = bu + delta[3,0] 191 | return x_new,bu_new 192 | 193 | def predict_imu(self,odom,dt): 194 | """ 195 | Desc: ekf predict imu step 196 | Input(s): 197 | odom: odometry [vel_x, vel_y, vel_z] [3 x 1] 198 | dt: time step difference 199 | Output(s): 200 | none 201 | """ 202 | # add zero to end of odom 203 | odom = np.vstack((odom,[0.0])) 204 | 205 | # build state transition model matrix 206 | F = np.eye(self.mu_n) 207 | 208 | # build odom transition matrix 209 | B = np.eye(self.mu_n) * dt 210 | 211 | # update predicted state 212 | self.mu = F.dot(self.mu) + B.dot(odom) 213 | 214 | # build process noise matrix 215 | Q_cov = 0.25 216 | Q = np.eye(self.mu_n) * Q_cov 217 | 218 | # propagate covariance matrix 219 | self.P = F.dot(self.P).dot(F.T) + Q 220 | 221 | def update_gnss_raw(self,mes,sat_x,sat_y,sat_z,sigmas,time_correction): 222 | """ 223 | Desc: ekf update gnss step 224 | Input(s): 225 | mes: psuedorange measurements [N x 1] 226 | sat_pos satellite position [N x 3] 227 | Output(s): 228 | none 229 | """ 230 | num_sats = mes.shape[0] 231 | zt = mes 232 | H = np.zeros((num_sats,self.mu_n)) 233 | h = np.zeros((num_sats,1)) 234 | R = np.eye(num_sats) 235 | for ii in range(num_sats): 236 | dist = np.sqrt((sat_x[ii]-self.mu[0])**2 + (sat_y[ii]-self.mu[1])**2 + (sat_z[ii]-self.mu[2])**2) 237 | H[ii,0] = (self.mu[0]-sat_x[ii])/dist 238 | H[ii,1] = (self.mu[1]-sat_y[ii])/dist 239 | H[ii,2] = (self.mu[2]-sat_z[ii])/dist 240 | H[ii,3] = 1.0 241 | c = 299792458.0 242 | h[ii] = dist + self.mu[3] - time_correction[ii] * c # adjusting for relativity?? 243 | R[ii,ii] *= sigmas[ii]**2 244 | yt = zt - h 245 | 246 | Kt = self.P.dot(H.T).dot(np.linalg.inv(R + H.dot(self.P).dot(H.T))) 247 | 248 | self.mu = self.mu.reshape((-1,1)) + Kt.dot(yt) 249 | self.P = (np.eye(self.mu_n)-Kt.dot(H)).dot(self.P).dot((np.eye(self.mu_n)-Kt.dot(H)).T) + Kt.dot(R).dot(Kt.T) 250 | 251 | yt = zt - H.dot(self.mu) 252 | 253 | def update_gnss(self,lat,lon,alt): 254 | """ 255 | Desc: ekf update gnss step 256 | Input(s): 257 | lat: latitude 258 | lon: longitude 259 | alt: altitude 260 | Output(s): 261 | none 262 | """ 263 | # convert lat lon to ecef frame 264 | self.lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84') 265 | self.ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84') 266 | xi, yi, zi = pyproj.transform(self.lla, self.ecef, lon, lat, alt , radians=False) 267 | zt = np.array([[xi,yi,zi]]).T 268 | 269 | H = np.eye(3) 270 | yt = zt - H.dot(self.mu[:3]) 271 | 272 | R_cov = 10.**2 273 | R = np.eye(3)*R_cov 274 | 275 | Kt = self.P[:3,:3].dot(H.T).dot(np.linalg.inv(R + H.dot(self.P[:3,:3]).dot(H.T))) 276 | 277 | self.mu[:3] = self.mu[:3].reshape((3,1)) + Kt.dot(yt) 278 | self.P[:3,:3] = (np.eye(3)-Kt.dot(H)).dot(self.P[:3,:3]).dot((np.eye(3)-Kt.dot(H)).T) + Kt.dot(R).dot(Kt.T) 279 | 280 | yt = zt - H.dot(self.mu[:3]) 281 | 282 | def run(self): 283 | """ 284 | Desc: run ekf 285 | Input(s): 286 | none 287 | Output(s): 288 | none 289 | """ 290 | t_odom_prev = 0.0 # initialize previous odom time 291 | 292 | # setup progress bar 293 | print("running kalman filter, please wait...") 294 | bar = progress.bar.IncrementalBar('Progress:', max=len(self.times)) 295 | 296 | 297 | for tt, timestep in enumerate(self.times): 298 | # predict step for odometry 299 | if self.odom_df['seconds of week [s]'].isin([timestep]).any(): 300 | dt_odom = timestep - t_odom_prev 301 | t_odom_prev = timestep 302 | if not self.initialized_odom: 303 | self.initialized_odom = True 304 | bar.next() 305 | else: 306 | odom_timestep = self.odom_df[self.odom_df['seconds of week [s]'] == timestep] 307 | odom_vel_x = odom_timestep['ECEF_vel_x'].values[0] 308 | odom_vel_y = odom_timestep['ECEF_vel_y'].values[0] 309 | odom_vel_z = odom_timestep['ECEF_vel_z'].values[0] 310 | self.predict_imu(np.array([[odom_vel_x,odom_vel_y,odom_vel_z]]).T,dt_odom) 311 | # update gnss step 312 | if self.sat_df['seconds of week [s]'].isin([timestep]).any(): 313 | sat_timestep = self.sat_df[self.sat_df['seconds of week [s]'] == timestep] 314 | if 'pr [m]' in self.sat_df.columns: 315 | pranges = sat_timestep['pr [m]'].to_numpy().reshape(-1,1) 316 | sat_x = sat_timestep['sat x ECEF [m]'].to_numpy().reshape(-1,1) 317 | sat_y = sat_timestep['sat y ECEF [m]'].to_numpy().reshape(-1,1) 318 | sat_z = sat_timestep['sat z ECEF [m]'].to_numpy().reshape(-1,1) 319 | sigmas = sat_timestep['Pr_sigma'].to_numpy().reshape(-1,1) 320 | time_correction = sat_timestep['idk wtf this is'].to_numpy().reshape(-1,1) 321 | self.update_gnss_raw(pranges,sat_x,sat_y,sat_z,sigmas,time_correction) 322 | else: 323 | lat_t = sat_timestep['Latitude'].to_numpy()[0] 324 | lon_t = sat_timestep['Longitude'].to_numpy()[0] 325 | alt_t = sat_timestep['Altitude'].to_numpy()[0] 326 | self.update_gnss(lat_t,lon_t,alt_t) 327 | 328 | # add values to history 329 | self.mu_history = np.hstack((self.mu_history,self.mu)) 330 | self.P_history.append(np.trace(self.P)) 331 | bar.next() # progress bar 332 | 333 | 334 | bar.finish() # end progress bar 335 | # if finish with different num of items, spoof it. 336 | if len(self.times) + 1 == self.mu_history.shape[1]: 337 | self.mu_history = self.mu_history[:,:-1] 338 | self.P_history = self.P_history[:-1] 339 | 340 | def plot(self,alt=np.array([None])): 341 | fig, ax = plt.subplots() 342 | ax.ticklabel_format(useOffset=False) 343 | plt.subplot(141) 344 | plt.plot(self.times,self.mu_history[0,:]) 345 | plt.title("X vs Time") 346 | plt.xlabel("Time [hrs]") 347 | plt.ylabel("X [m]") 348 | 349 | plt.subplot(142) 350 | plt.plot(self.times,self.mu_history[1,:]) 351 | plt.title("Y vs Time") 352 | plt.xlabel("Time [hrs]") 353 | plt.ylabel("Y [m]") 354 | 355 | plt.subplot(143) 356 | plt.plot(self.times,self.mu_history[2,:]) 357 | plt.title("Z vs Time") 358 | plt.xlabel("Time [hrs]") 359 | plt.ylabel("Z [m]") 360 | 361 | plt.subplot(144) 362 | plt.plot(self.times,self.mu_history[3,:]) 363 | plt.title("Time Bias vs Time") 364 | plt.xlabel("Time [hrs]") 365 | plt.ylabel("Time Bias [m]") 366 | 367 | # covariance plot 368 | plt.figure() 369 | plt.title("Trace of Covariance Matrix vs. Time") 370 | plt.xlabel("Time [hrs]") 371 | plt.ylabel("Trace") 372 | plt.plot(self.times,self.P_history) 373 | 374 | # trajectory plot 375 | lla_traj = np.zeros((len(self.times),3)) 376 | if alt.all() == None: 377 | lon, lat, alt = pyproj.transform(self.ecef, self.lla, self.mu_history[0,:], self.mu_history[1,:], self.mu_history[2,:], radians=False) 378 | else: 379 | lon, lat, reject = pyproj.transform(self.ecef, self.lla, self.mu_history[0,:], self.mu_history[1,:], self.mu_history[2,:], radians=False) 380 | print("alt shape",alt.shape) 381 | lla_traj[:,0] = lat 382 | lla_traj[:,1] = lon 383 | lla_traj[:,2] = alt 384 | fig, ax = plt.subplots() 385 | ax.ticklabel_format(useOffset=False) 386 | plt.plot(lla_traj[:,1],lla_traj[:,0],label="Our Position Solution") 387 | 388 | if self.odom_file != None: 389 | lat_truth = self.odom_df['GPS(0):Lat[degrees]'].to_numpy() 390 | lon_truth = self.odom_df['GPS(0):Long[degrees]'].to_numpy() 391 | plt.plot(lon_truth,lat_truth,'g',label="DJI's Position Solution") 392 | plt.legend() 393 | plt.xlim([-122.1759,-122.1754]) 394 | plt.ylim([37.42620,37.42660]) 395 | ax.set_yticks([37.4262,37.4263,37.4264,37.4265,37.4266]) 396 | plt.xlabel("Longitude [deg]") 397 | plt.ylabel("Latitude [deg]") 398 | 399 | fig = plt.figure() 400 | ax = fig.gca(projection='3d') 401 | ax.plot(lla_traj[:,1], lla_traj[:,0], lla_traj[:,2], label='our solution') 402 | if self.odom_file != None: 403 | lat_truth = self.odom_df['GPS(0):Lat[degrees]'].to_numpy()[self.truth_indexes] 404 | lon_truth = self.odom_df['GPS(0):Long[degrees]'].to_numpy()[self.truth_indexes] 405 | h_truth = self.odom_df['GPS(0):heightMSL[meters]'].to_numpy()[self.truth_indexes] 406 | latf = self.odom_df['GPS(0):Lat[degrees]'].values[-1] 407 | lonf = self.odom_df['GPS(0):Long[degrees]'].values[-1] 408 | hf = h_truth[-1] 409 | lat0 = self.odom_df['GPS(0):Lat[degrees]'][0] 410 | lon0 = self.odom_df['GPS(0):Long[degrees]'][0] 411 | h0 = h_truth[0] 412 | plt.plot([lon0],[lat0],[h0],'go') 413 | plt.plot([lonf],[latf],[hf],'ro') 414 | plt.plot(lon_truth,lat_truth,h_truth,'g',label="DJI's Position Solution") 415 | 416 | ax.legend() 417 | ax.ticklabel_format(useOffset=False) 418 | ax.set_xlim([-122.1759,-122.1754]) 419 | ax.set_ylim([37.42620,37.42660]) 420 | ax.set_zlim([0.,2.5]) 421 | ax.set_xlabel('\n\n Longitude [deg]') 422 | ax.set_ylabel('Latitude [deg]') 423 | ax.set_zlabel('Altitude [m]') 424 | ax.set_xticks([-122.1759, -122.17565, -122.1754]) 425 | ax.set_yticks([37.42630,37.42640,37.42650]) 426 | ax.view_init(elev=10., azim=20.) 427 | 428 | if self.odom_file != None: 429 | steps = np.arange(len(lat_truth)) 430 | lat_error = np.abs(lat_truth-lla_traj[:,0]) 431 | lon_error = np.abs(lon_truth-lla_traj[:,1]) 432 | h_error = np.abs(h_truth-lla_traj[:,2]) 433 | print("lat avg: ",np.mean(lat_error)) 434 | print("lon avg: ",np.mean(lon_error)) 435 | print("h avg: ",np.mean(h_error)) 436 | plt.figure() 437 | plt.subplot(131) 438 | plt.title("Latitude Error [degrees latitude]") 439 | plt.ylabel("Latitude Error [degrees latitude]") 440 | plt.xlabel("Time Step") 441 | plt.ticklabel_format(axis="y", style="sci", scilimits=(0,0)) 442 | plt.plot(steps,lat_error) 443 | plt.subplot(132) 444 | plt.xlabel("Time Step") 445 | plt.ticklabel_format(axis="y", style="sci", scilimits=(0,0)) 446 | plt.title("Longitude Error [degrees longitude]") 447 | plt.ylabel("Longitude Error [degrees longitude]") 448 | plt.plot(steps,lon_error) 449 | plt.subplot(133) 450 | plt.xlabel("Time Step") 451 | plt.title("Altitude Error [m]") 452 | plt.ylabel("Altitude Error [m]") 453 | plt.plot(steps,h_error) 454 | 455 | # save to file 456 | df_traj = pd.DataFrame() 457 | df_traj['latitude'] = lla_traj[:,0] 458 | df_traj['longitude'] = lla_traj[:,1] 459 | df_traj['elevation'] = lla_traj[:,2] 460 | df_traj.to_csv('./data/calculated_trajectory.csv',index=False) 461 | 462 | plt.show() 463 | 464 | class EKF_H(EKF): 465 | 466 | def __init__(self,sat_file,odom_file=None): 467 | # read in data files as dataframe 468 | self.sat_df = pd.read_csv(sat_file, index_col=0) 469 | self.odom_file = odom_file 470 | if odom_file != None: 471 | self.odom_df = pd.read_csv(odom_file, index_col=0) 472 | 473 | # concatenate possible time steps from each data file 474 | self.times = np.concatenate((self.odom_df['seconds of week [s]'].to_numpy(),self.sat_df['seconds of week [s]'].to_numpy())) 475 | 476 | self.initialized_odom = False 477 | 478 | else: 479 | # initialize times 480 | self.times = self.sat_df['seconds of week [s]'].to_numpy() 481 | 482 | # sort timesteps and force unique 483 | self.times = np.sort(np.unique(self.times)) 484 | 485 | # initialize state vector [ altitude ] 486 | h0 = 0.0 487 | self.mu = np.array([[h0]]).T 488 | self.mu_n = self.mu.shape[0] 489 | self.mu_history = self.mu.copy() 490 | 491 | # initialize covariance matrix 492 | self.P = np.eye(self.mu_n) 493 | self.P_history = [np.trace(self.P)] 494 | 495 | def predict_simple(self): 496 | """ 497 | Desc: ekf simple predict step 498 | Input(s): 499 | dt: time step difference 500 | Output(s): 501 | none 502 | """ 503 | # build state transition model matrix 504 | F = np.eye(self.mu_n) 505 | 506 | # update predicted state 507 | self.mu = F.dot(self.mu) 508 | 509 | # build process noise matrix 510 | Q_cov = 0.001 511 | Q = np.eye(self.mu_n) * Q_cov 512 | # Q = np.ones((3,3)) * Q_cov 513 | 514 | # propagate covariance matrix 515 | self.P = F.dot(self.P).dot(F.T) + Q 516 | 517 | def update_barometer(self,baro): 518 | """ 519 | Desc: ekf barometer update step 520 | Input(s): 521 | baro: barometer reading 522 | Output(s): 523 | none 524 | """ 525 | # add zero to end of odom 526 | zt = np.array([[baro]]) 527 | 528 | # create measurement noise matrix 529 | R = np.eye(self.mu_n)*0.5**2 530 | 531 | hu = self.mu.copy().reshape(-1,1) 532 | yt = zt - hu 533 | H = np.eye(self.mu_n) 534 | 535 | Kt = self.P.dot(H.T).dot(np.linalg.inv(R + H.dot(self.P).dot(H.T))) 536 | 537 | self.mu = self.mu.reshape((-1,1)) + Kt.dot(yt) 538 | self.P = (np.eye(self.mu_n)-Kt.dot(H)).dot(self.P).dot((np.eye(self.mu_n)-Kt.dot(H)).T) + Kt.dot(R).dot(Kt.T) 539 | 540 | yt = zt - H.dot(self.mu) 541 | 542 | def run(self): 543 | """ 544 | Desc: run ekf 545 | Input(s): 546 | none 547 | Output(s): 548 | none 549 | """ 550 | t_odom_prev = 0.0 # initialize previous odom time 551 | 552 | # setup progress bar 553 | print("running kalman filter, please wait...") 554 | bar = progress.bar.IncrementalBar('Progress:', max=len(self.times)) 555 | 556 | for tt, timestep in enumerate(self.times): 557 | # simple predict step 558 | self.predict_simple() 559 | 560 | # predict step for odometry 561 | if self.odom_df['seconds of week [s]'].isin([timestep]).any(): 562 | dt_odom = timestep - t_odom_prev 563 | t_odom_prev = timestep 564 | if not self.initialized_odom: 565 | self.initialized_odom = True 566 | bar.next() 567 | else: 568 | odom_timestep = self.odom_df[self.odom_df['seconds of week [s]'] == timestep] 569 | baro_meas = odom_timestep['Normalized barometer:Raw[meters]'].values[0] 570 | self.update_barometer(baro_meas) 571 | 572 | self.mu = np.clip(self.mu,0.0,np.inf) # force to be above zero altitude 573 | 574 | # add values to history 575 | self.mu_history = np.hstack((self.mu_history,self.mu)) 576 | self.P_history.append(np.trace(self.P)) 577 | bar.next() # progress bar 578 | 579 | bar.finish() # end progress bar 580 | # if finish with different num of items, spoof it. 581 | if len(self.times) + 1 == self.mu_history.shape[1]: 582 | self.mu_history = self.mu_history[:,:-1] 583 | self.P_history = self.P_history[:-1] 584 | 585 | def plot(self): 586 | # covariance plot 587 | plt.figure() 588 | plt.title("Trace of Covariance Matrix vs. Time") 589 | plt.xlabel("Time [hrs]") 590 | plt.ylabel("Trace") 591 | plt.plot(self.times,self.P_history) 592 | 593 | fig, ax = plt.subplots() 594 | ax.ticklabel_format(useOffset=False) 595 | plt.plot(self.times,self.mu_history[0,:]) 596 | plt.title("h vs Time") 597 | plt.xlabel("Time [hrs]") 598 | plt.ylabel("h [m]") 599 | 600 | plt.show() 601 | 602 | if __name__ == '__main__': 603 | # latitude and longitude EKF 604 | ekf = EKF('./data/sat_data_v2_flight_1.csv','./data/dji_data_flight_1.csv') 605 | ekf.run() 606 | #ekf.plot() 607 | 608 | # altitude EKF 609 | ekf_h = EKF_H('./data/sat_data_v2_flight_1.csv','./data/dji_data_flight_1.csv') 610 | ekf_h.run() 611 | #ekf_h.plot() 612 | 613 | # combine both EKFs 614 | ekf.plot(ekf_h.mu_history[0,:]) 615 | -------------------------------------------------------------------------------- /gnss_only_ekf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | Author(s): D. Knowles 4 | Date: 14 Feb 2020 5 | Desc: AA272 sensor fusion project 6 | """ 7 | 8 | import numpy as np 9 | from scipy.io import loadmat 10 | import numpy as np 11 | import pandas as pd 12 | import matplotlib.pyplot as plt 13 | import pyproj 14 | import math 15 | import progress.bar 16 | from mpl_toolkits.mplot3d import Axes3D 17 | 18 | class EKF(): 19 | def __init__(self,sat_file,odom_file=None): 20 | # read in data files as dataframe 21 | self.sat_df = pd.read_csv(sat_file, index_col=0) 22 | self.odom_file = odom_file 23 | if odom_file != None: 24 | self.odom_df = pd.read_csv(odom_file, index_col=0) 25 | 26 | # initial lat and lon from dji data 27 | self.lat0 = np.mean(self.odom_df['GPS(0):Lat[degrees]'][0]) 28 | self.lon0 = np.mean(self.odom_df['GPS(0):Long[degrees]'][0]) 29 | self.h0 = 0.0 30 | 31 | # convert lat lon to ecef frame 32 | self.lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84') 33 | self.ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84') 34 | x0, y0, z0 = pyproj.transform(self.lla, self.ecef, self.lon0, self.lat0, self.h0 , radians=False) 35 | 36 | # concatenate possible time steps from each data file 37 | self.times = self.sat_df['seconds of week [s]'].to_numpy() 38 | 39 | # sort timesteps and force unique 40 | self.times = np.sort(np.unique(self.times)) 41 | 42 | def find_nearest(array, value): 43 | array = np.asarray(array) 44 | idx = (np.abs(array - value)).argmin() 45 | return idx 46 | 47 | # indexes for comparing later 48 | self.sat_indexes = [] 49 | for ii in range(len(self.odom_df['seconds of week [s]'].to_numpy())): 50 | ix = find_nearest(self.times,self.odom_df['seconds of week [s]'].values[ii]) 51 | self.sat_indexes.append(ix) 52 | 53 | else: 54 | # set initial positions 55 | x0 = 0. 56 | y0 = 0. 57 | z0 = 0. 58 | 59 | self.lat0 = y0 60 | self.lon0 = x0 61 | self.h0 = z0 62 | 63 | # initialize times 64 | self.times = self.sat_df['seconds of week [s]'].to_numpy() 65 | 66 | 67 | 68 | # initialize state vector [ x, y, z ] 69 | self.mu = np.array([[x0,y0,z0,0.0]]).T 70 | self.mu_n = self.mu.shape[0] 71 | 72 | 73 | # initialize covariance matrix 74 | self.P = np.eye(self.mu_n)*10E2 75 | self.P_history = [np.trace(self.P)] 76 | 77 | self.wls_results = np.zeros((7,len(self.times))) 78 | 79 | # setup progress bar 80 | print("running weighted least squares, please wait...") 81 | bar = progress.bar.IncrementalBar('Progress:', max=len(self.times)) 82 | 83 | for tt in range(len(self.times)): 84 | x_calc = [self.mu.item(0), self.mu.item(1), self.mu.item(2)] 85 | x_calc = [0.,0.,0.] 86 | bu_calc = 0.0 87 | input = self.sat_df[self.sat_df['seconds of week [s]'] == self.times[tt]] 88 | for ii in range(20): 89 | x_calc, bu_calc = self.least_squares(x_calc,bu_calc,input) 90 | self.wls_results[:3,tt] = x_calc 91 | self.wls_results[3,tt] = bu_calc 92 | bar.next() 93 | bar.finish() 94 | 95 | wls_lon, wls_lat, wls_alt = pyproj.transform(self.ecef, self.lla, self.wls_results[0,:], self.wls_results[1,:], self.wls_results[2,:], radians=False) 96 | self.wls_results[4,:] = wls_lat 97 | self.wls_results[5,:] = wls_lon 98 | self.wls_results[6,:] = wls_alt 99 | 100 | # plot all of the weighted least squares results 101 | if False: 102 | plt.figure() 103 | plt.subplot(231) 104 | plt.title("ECEF X vs Time") 105 | plt.plot(self.times,self.wls_results[0,:]) 106 | 107 | plt.subplot(232) 108 | plt.title("ECEF Y vs Time") 109 | plt.plot(self.times,self.wls_results[1,:]) 110 | 111 | plt.subplot(233) 112 | plt.title("ECEF Z vs Time") 113 | plt.plot(self.times,self.wls_results[2,:]) 114 | 115 | plt.subplot(234) 116 | plt.title("Bu vs Time") 117 | plt.plot(self.times,self.wls_results[3,:]) 118 | 119 | 120 | plt.subplot(235) 121 | plt.title("Lat/Lon vs Time") 122 | plt.plot(self.wls_results[5,:],self.wls_results[4,:]) 123 | 124 | plt.subplot(236) 125 | plt.title("Altitude vs Time") 126 | plt.plot(self.times,self.wls_results[6,:]) 127 | plt.show() 128 | 129 | self.mu[3][0] = bu_calc 130 | self.mu_history = self.mu.copy() 131 | 132 | # save to file 133 | df_wls = pd.DataFrame() 134 | df_wls['latitude'] = wls_lat 135 | df_wls['longitude'] = wls_lon 136 | df_wls['elevation'] = wls_alt 137 | df_wls.to_csv('./data/wls_calculated_trajectory.csv',index=False) 138 | 139 | def ECEF_2_ENU(self,x_ECEF,xref,lat0,lon0): 140 | """ 141 | input(s) 142 | x_ECEF: 3 X N array in ECEF 143 | xref: reference [x,y,z] location 144 | lat0: latitude reference 145 | lon0: longitude reference 146 | output(s): 147 | x_ENU: 3 X N array in east north up 148 | """ 149 | x_ECEF_ref = xref 150 | 151 | x_REF = np.repeat(x_ECEF_ref,x_ECEF.shape[1],axis=1) 152 | theta_lat = np.radians(lat0) 153 | theta_long = np.radians(lon0) 154 | T_enu = np.array([[-np.sin(theta_long), 155 | np.cos(theta_long), 156 | 0.], 157 | [-np.sin(theta_lat)*np.cos(theta_long), 158 | -np.sin(theta_lat)*np.sin(theta_long), 159 | np.cos(theta_lat)], 160 | [np.cos(theta_lat)*np.cos(theta_long), 161 | np.cos(theta_lat)*np.sin(theta_long), 162 | np.sin(theta_lat)]]) 163 | x_ENU = np.dot(T_enu,(x_ECEF-x_REF)) 164 | return x_ENU 165 | 166 | def least_squares(self,x_0,bu,sat_df): 167 | """ 168 | input(s) 169 | x: [3 x 1] state estimate 170 | bu: clock bias 171 | sat_df: satellite data frame 172 | output(s) 173 | x: [3 x 1] new state estimate 174 | bu: new clock bias 175 | """ 176 | numSats = len(sat_df) 177 | dist = np.zeros((numSats,1)) 178 | 179 | G = np.zeros((numSats,4)) 180 | W = np.eye(numSats) 181 | for ii in range(numSats): 182 | x_s = sat_df['sat x ECEF [m]'].to_numpy()[ii] 183 | y_s = sat_df['sat y ECEF [m]'].to_numpy()[ii] 184 | z_s = sat_df['sat z ECEF [m]'].to_numpy()[ii] 185 | 186 | dist[ii] = np.sqrt((x_s-x_0[0])**2 + \ 187 | (y_s-x_0[1])**2 + \ 188 | (z_s-x_0[2])**2) 189 | G[ii,:] = [-(x_s - x_0[0])/dist[ii], 190 | -(y_s - x_0[1])/dist[ii], 191 | -(z_s - x_0[2])/dist[ii], 192 | 1.0] 193 | W[ii,ii] *= 1./sat_df['Pr_sigma'].to_numpy()[ii] 194 | 195 | c = 299792458.0 196 | relativity = sat_df['idk wtf this is'].to_numpy().reshape(-1,1) * c # adjusting for relativity?? 197 | rho_0 = dist + bu - relativity 198 | rho_dif = sat_df['pr [m]'].to_numpy().reshape(-1,1) - rho_0 199 | # delta = np.linalg.inv(G.T.dot(G)).dot(G.T).dot(rho_dif) 200 | delta = np.linalg.pinv(W.dot(G)).dot(W).dot(rho_dif) 201 | x_new = x_0 + delta[0:3,0] 202 | bu_new = bu + delta[3,0] 203 | return x_new,bu_new 204 | 205 | def predict_simple(self): 206 | """ 207 | Desc: ekf simple predict step 208 | Input(s): 209 | dt: time step difference 210 | Output(s): 211 | none 212 | """ 213 | # build state transition model matrix 214 | F = np.eye(self.mu_n) 215 | 216 | # update predicted state 217 | self.mu = F.dot(self.mu) 218 | 219 | # build process noise matrix 220 | Q_cov = 0.5 221 | Q = np.eye(self.mu_n) * Q_cov 222 | 223 | # propagate covariance matrix 224 | self.P = F.dot(self.P).dot(F.T) + Q 225 | 226 | def update_gnss(self,mes,sat_x,sat_y,sat_z,sigmas,time_correction): 227 | """ 228 | Desc: ekf update gnss step 229 | Input(s): 230 | mes: psuedorange measurements [N x 1] 231 | sat_pos satellite position [N x 3] 232 | Output(s): 233 | none 234 | """ 235 | num_sats = mes.shape[0] 236 | zt = mes 237 | H = np.zeros((num_sats,self.mu_n)) 238 | h = np.zeros((num_sats,1)) 239 | R = np.eye(num_sats) 240 | for ii in range(num_sats): 241 | dist = np.sqrt((sat_x[ii]-self.mu[0])**2 + (sat_y[ii]-self.mu[1])**2 + (sat_z[ii]-self.mu[2])**2) 242 | H[ii,0] = (self.mu[0]-sat_x[ii])/dist 243 | H[ii,1] = (self.mu[1]-sat_y[ii])/dist 244 | H[ii,2] = (self.mu[2]-sat_z[ii])/dist 245 | H[ii,3] = 1.0 246 | c = 299792458.0 247 | h[ii] = dist + self.mu[3] - time_correction[ii] * c # adjusting for relativity?? 248 | R[ii,ii] *= sigmas[ii]**2 249 | yt = zt - h 250 | 251 | Kt = self.P.dot(H.T).dot(np.linalg.inv(R + H.dot(self.P).dot(H.T))) 252 | 253 | self.mu = self.mu.reshape((-1,1)) + Kt.dot(yt) 254 | self.P = (np.eye(self.mu_n)-Kt.dot(H)).dot(self.P).dot((np.eye(self.mu_n)-Kt.dot(H)).T) + Kt.dot(R).dot(Kt.T) 255 | 256 | yt = zt - H.dot(self.mu) 257 | 258 | 259 | def run(self): 260 | """ 261 | Desc: run ekf 262 | Input(s): 263 | none 264 | Output(s): 265 | none 266 | """ 267 | # setup progress bar 268 | print("running kalman filter, please wait...") 269 | bar = progress.bar.IncrementalBar('Progress:', max=len(self.times)) 270 | 271 | for tt, timestep in enumerate(self.times): 272 | # update gnss step 273 | if self.sat_df['seconds of week [s]'].isin([timestep]).any(): 274 | sat_timestep = self.sat_df[self.sat_df['seconds of week [s]'] == timestep] 275 | pranges = sat_timestep['pr [m]'].to_numpy().reshape(-1,1) 276 | sat_x = sat_timestep['sat x ECEF [m]'].to_numpy().reshape(-1,1) 277 | sat_y = sat_timestep['sat y ECEF [m]'].to_numpy().reshape(-1,1) 278 | sat_z = sat_timestep['sat z ECEF [m]'].to_numpy().reshape(-1,1) 279 | sigmas = sat_timestep['Pr_sigma'].to_numpy().reshape(-1,1) 280 | time_correction = sat_timestep['idk wtf this is'].to_numpy().reshape(-1,1) 281 | self.predict_simple() 282 | self.update_gnss(pranges,sat_x,sat_y,sat_z,sigmas,time_correction) 283 | 284 | # add values to history 285 | self.mu_history = np.hstack((self.mu_history,self.mu)) 286 | self.P_history.append(np.trace(self.P)) 287 | bar.next() # progress bar 288 | 289 | bar.finish() # end progress bar 290 | if len(self.times) + 1 == self.mu_history.shape[1]: 291 | self.mu_history = self.mu_history[:,:-1] 292 | self.P_history = self.P_history[:-1] 293 | 294 | def plot(self): 295 | fig, ax = plt.subplots() 296 | ax.ticklabel_format(useOffset=False) 297 | plt.subplot(141) 298 | plt.plot(self.times,self.mu_history[0,:]) 299 | plt.title("X vs Time") 300 | plt.xlabel("Time [hrs]") 301 | plt.ylabel("X [m]") 302 | 303 | plt.subplot(142) 304 | plt.plot(self.times,self.mu_history[1,:]) 305 | plt.title("Y vs Time") 306 | plt.xlabel("Time [hrs]") 307 | plt.ylabel("Y [m]") 308 | 309 | plt.subplot(143) 310 | plt.plot(self.times,self.mu_history[2,:]) 311 | plt.title("Z vs Time") 312 | plt.xlabel("Time [hrs]") 313 | plt.ylabel("Z [m]") 314 | 315 | plt.subplot(144) 316 | plt.plot(self.times,self.mu_history[3,:]) 317 | plt.title("Time Bias vs Time") 318 | plt.xlabel("Time [hrs]") 319 | plt.ylabel("Time Bias [m]") 320 | 321 | # covariance plot 322 | plt.figure() 323 | plt.title("Trace of Covariance Matrix vs. Time") 324 | plt.xlabel("Time [hrs]") 325 | plt.ylabel("Trace") 326 | plt.plot(self.times,self.P_history) 327 | 328 | # trajectory plot 329 | lla_traj = np.zeros((len(self.times),3)) 330 | lon, lat, alt = pyproj.transform(self.ecef, self.lla, self.mu_history[0,:], self.mu_history[1,:], self.mu_history[2,:], radians=False) 331 | lla_traj[:,0] = lat 332 | lla_traj[:,1] = lon 333 | lla_traj[:,2] = alt 334 | fig, ax = plt.subplots() 335 | ax.ticklabel_format(useOffset=False) 336 | plt.plot(lla_traj[:,1],lla_traj[:,0],'b',label='Our Position Solution') 337 | 338 | plt.xlabel("Longitude [deg]") 339 | plt.ylabel("Latitude [deg]") 340 | 341 | if self.odom_file != None: 342 | lat_truth = self.odom_df['GPS(0):Lat[degrees]'].to_numpy() 343 | lon_truth = self.odom_df['GPS(0):Long[degrees]'].to_numpy() 344 | latf = self.odom_df['GPS(0):Lat[degrees]'].values[-1] 345 | lonf = self.odom_df['GPS(0):Long[degrees]'].values[-1] 346 | lat0 = self.odom_df['GPS(0):Lat[degrees]'][0] 347 | lon0 = self.odom_df['GPS(0):Long[degrees]'][0] 348 | plt.plot(lon0,lat0,'go') 349 | plt.plot(lonf,latf,'ro') 350 | plt.plot(lon_truth,lat_truth,'g',label="DJI's Position Solution") 351 | plt.legend() 352 | plt.xlim([-122.1759,-122.1754]) 353 | plt.ylim([37.42620,37.42660]) 354 | ax.set_yticks([37.4262,37.4263,37.4264,37.4265,37.4266]) 355 | 356 | fig = plt.figure() 357 | ax = fig.gca(projection='3d') 358 | ax.plot(lla_traj[:,1], lla_traj[:,0], lla_traj[:,2], label='our solution') 359 | if self.odom_file != None: 360 | lat_truth = self.odom_df['GPS(0):Lat[degrees]'].to_numpy() 361 | lon_truth = self.odom_df['GPS(0):Long[degrees]'].to_numpy() 362 | h_truth = self.odom_df['GPS(0):heightMSL[meters]'].to_numpy() 363 | latf = self.odom_df['GPS(0):Lat[degrees]'].values[-1] 364 | lonf = self.odom_df['GPS(0):Long[degrees]'].values[-1] 365 | hf = self.odom_df['GPS(0):heightMSL[meters]'].values[-1] 366 | lat0 = self.odom_df['GPS(0):Lat[degrees]'][0] 367 | lon0 = self.odom_df['GPS(0):Long[degrees]'][0] 368 | h0 = self.odom_df['GPS(0):heightMSL[meters]'][0] 369 | plt.plot([lon0],[lat0],[h0],'go') 370 | plt.plot([lonf],[latf],[hf],'ro') 371 | plt.plot(lon_truth,lat_truth,h_truth,'g',label="DJI's Position Solution") 372 | 373 | ax.legend() 374 | ax.ticklabel_format(useOffset=False) 375 | ax.set_xlim([-122.1759,-122.1754]) 376 | ax.set_ylim([37.42620,37.42660]) 377 | ax.set_zlim([0.,60.]) 378 | ax.set_xlabel('\n\n Longitude [deg]') 379 | ax.set_ylabel('Latitude [deg]') 380 | ax.set_zlabel('Altitude [m]') 381 | ax.set_xticks([-122.1759, -122.17565, -122.1754]) 382 | ax.set_yticks([37.42630,37.42640,37.42650]) 383 | ax.view_init(elev=10., azim=20.) 384 | 385 | if self.odom_file != None: 386 | steps = np.arange(len(lat_truth)) 387 | lat_error = np.abs(lat_truth-lla_traj[:,0][self.sat_indexes]) 388 | lon_error = np.abs(lon_truth-lla_traj[:,1][self.sat_indexes]) 389 | h_error = np.abs(h_truth-lla_traj[:,2][self.sat_indexes]) 390 | print("lat error avg: ",np.mean(lat_error)) 391 | print("lon error avg: ",np.mean(lon_error)) 392 | print("h error avg: ",np.mean(h_error)) 393 | plt.figure() 394 | plt.subplot(131) 395 | plt.title("Latitude Error [degrees latitude]") 396 | plt.ylabel("Latitude Error [degrees latitude]") 397 | plt.xlabel("Time Step") 398 | plt.ticklabel_format(axis="y", style="sci", scilimits=(0,0)) 399 | plt.plot(steps,lat_error) 400 | plt.subplot(132) 401 | plt.xlabel("Time Step") 402 | plt.ticklabel_format(axis="y", style="sci", scilimits=(0,0)) 403 | plt.title("Longitude Error [degrees longitude]") 404 | plt.ylabel("Longitude Error [degrees longitude]") 405 | plt.plot(steps,lon_error) 406 | plt.subplot(133) 407 | plt.xlabel("Time Step") 408 | plt.title("Altitude Error [m]") 409 | plt.ylabel("Altitude Error [m]") 410 | plt.plot(steps,h_error) 411 | 412 | # save to file 413 | df_traj = pd.DataFrame() 414 | df_traj['latitude'] = lla_traj[:,0] 415 | df_traj['longitude'] = lla_traj[:,1] 416 | df_traj['elevation'] = lla_traj[:,2] 417 | df_traj.to_csv('./data/calculated_trajectory.csv',index=False) 418 | 419 | plt.show() 420 | 421 | if __name__ == '__main__': 422 | ekf = EKF('./data/sat_data_v2_flight_1.csv','./data/dji_data_flight_1.csv') 423 | ekf.run() 424 | ekf.plot() 425 | -------------------------------------------------------------------------------- /gnss_only_ekf_toy.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | Author(s): D. Knowles 4 | Date: 14 Feb 2020 5 | Desc: AA272 sensor fusion project 6 | """ 7 | 8 | import numpy as np 9 | from scipy.io import loadmat 10 | import numpy as np 11 | import pandas as pd 12 | import matplotlib.pyplot as plt 13 | import pyproj 14 | import math 15 | import progress.bar 16 | 17 | class EKF(): 18 | def __init__(self,odom_file,sat_file): 19 | # read in data files as dataframe 20 | self.odom_df = pd.read_csv(odom_file, index_col=0) 21 | self.sat_df = pd.read_csv(sat_file, index_col=0) 22 | 23 | # initial lat and lon from dji data 24 | lat0 = np.mean(self.odom_df['GPS(0):Lat[degrees]'][0]) 25 | lon0 = np.mean(self.odom_df['GPS(0):Long[degrees]'][0]) 26 | h0 = 0.0 27 | 28 | # initial and final time values 29 | # self.ti = min(self.odom_df['seconds of week [s]'].min(),self.sat_df['seconds of week [s]'].min()) 30 | # self.tf = max(self.odom_df['seconds of week [s]'].max(),self.sat_df['seconds of week [s]'].max()) 31 | 32 | # concatenate possible time steps from each data file 33 | # self.times = np.concatenate((self.odom_df['seconds of week [s]'].to_numpy(),self.sat_df['seconds of week [s]'].to_numpy())) 34 | self.times = self.sat_df['seconds of week [s]'].to_numpy() 35 | self.times = np.sort(np.unique(self.times)) 36 | 37 | # convert lat lon to ecef frame 38 | self.lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84') 39 | self.ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84') 40 | self.x0, self.y0, self.z0 = pyproj.transform(self.lla, self.ecef, lon0, lat0, h0 , radians=False) 41 | 42 | # x0 += -96000. 43 | # y0 += -30000. 44 | # z0 += -90000. 45 | 46 | # initialize state vector [ x, y, z ] 47 | # self.mu = np.array([[x0,y0,z0]]).T 48 | self.mu = np.array([[10.,10.,10.]]).T 49 | self.mu_n = self.mu.shape[0] 50 | self.mu_history = self.mu.copy() 51 | 52 | # initialize covariance matrix 53 | self.P = np.eye(self.mu_n) 54 | # self.P = np.ones((3,3)) 55 | self.P_history = [np.trace(self.P)] 56 | 57 | # self.check_data(self.mu,lat0,lon0) 58 | 59 | 60 | 61 | def ECEF_2_ENU(self,x_ECEF,xref,lat0,lon0): 62 | """ 63 | input(s) 64 | x_ECEF: 3 X N array 65 | """ 66 | x_ECEF_ref = xref 67 | 68 | x_REF = np.repeat(x_ECEF_ref,x_ECEF.shape[1],axis=1) 69 | theta_lat = np.radians(lat0) 70 | theta_long = np.radians(lon0) 71 | T_enu = np.array([[-np.sin(theta_long), 72 | np.cos(theta_long), 73 | 0.], 74 | [-np.sin(theta_lat)*np.cos(theta_long), 75 | -np.sin(theta_lat)*np.sin(theta_long), 76 | np.cos(theta_lat)], 77 | [np.cos(theta_lat)*np.cos(theta_long), 78 | np.cos(theta_lat)*np.sin(theta_long), 79 | np.sin(theta_lat)]]) 80 | x_ENU = np.dot(T_enu,(x_ECEF-x_REF)) 81 | return x_ENU 82 | 83 | def check_data(self,xref,lat0,lon0): 84 | # ans = self.ECEF_2_ENU(self.mu,self.mu,lat0,lon0) 85 | SVs = np.sort(np.unique(self.sat_df['SV'])) 86 | for sv in SVs: 87 | sv_subset = self.sat_df[self.sat_df['SV'] == sv] 88 | sv_x = sv_subset['sat x ECEF [m]'].to_numpy().reshape((1,-1)) 89 | sv_y = sv_subset['sat y ECEF [m]'].to_numpy().reshape((1,-1)) 90 | sv_z = sv_subset['sat z ECEF [m]'].to_numpy().reshape((1,-1)) 91 | sv_time = sv_subset['seconds of week [s]'].to_numpy() 92 | sv_xyz = np.vstack((sv_x,sv_y,sv_z)) 93 | sv_ENU = self.ECEF_2_ENU(sv_xyz,self.mu,lat0,lon0) 94 | 95 | 96 | elev_angles = np.degrees(np.arctan2(sv_ENU[2,:],np.sqrt(sv_ENU[0,:]**2 + sv_ENU[1,:]**2))) 97 | # if sv in [7,30,28,9,8,5]: 98 | # if True: 99 | # plt.plot(sv_time,elev_angles,label=sv) 100 | # plt.ylabel('Elevation Angle [degrees]') 101 | # plt.legend() 102 | # plt.title("Elevation Angle vs. Time") 103 | # plt.legend() 104 | # plt.show() 105 | 106 | is7 = self.sat_df['SV'] == 7 107 | is30 = self.sat_df['SV'] == 30 108 | is28 = self.sat_df['SV'] == 28 109 | is9 = self.sat_df['SV'] == 9 110 | is8 = self.sat_df['SV'] == 8 111 | is5 = self.sat_df['SV'] == 5 112 | 113 | self.sat_df = self.sat_df[is7 | is30 | is28 | is9 | is8 | is5] 114 | 115 | 116 | 117 | def predict_imu(self,odom,dt): 118 | """ 119 | Desc: ekf predict imu step 120 | Input(s): 121 | odom: odometry [vel_x, vel_y, vel_z] [3 x 1] 122 | dt: time step difference 123 | Output(s): 124 | none 125 | """ 126 | # build state transition model matrix 127 | F = np.eye(self.mu_n) 128 | 129 | # build odom transition matrix 130 | B = np.eye(self.mu_n) * dt 131 | 132 | # update predicted state 133 | self.mu = F.dot(self.mu) + B.dot(odom) 134 | 135 | # build process noise matrix 136 | Q_cov = 0.5 137 | Q = np.eye(self.mu_n) * Q_cov 138 | 139 | # propagate covariance matrix 140 | self.P = F.dot(self.P).dot(F.T) + Q 141 | 142 | def predict_simple(self): 143 | """ 144 | Desc: ekf simple predict step 145 | Input(s): 146 | dt: time step difference 147 | Output(s): 148 | none 149 | """ 150 | # build state transition model matrix 151 | F = np.eye(self.mu_n) 152 | 153 | # update predicted state 154 | self.mu = F.dot(self.mu) 155 | 156 | # build process noise matrix 157 | Q_cov = 0.2 158 | Q = np.eye(self.mu_n) * Q_cov 159 | # Q = np.ones((3,3)) * Q_cov 160 | 161 | # propagate covariance matrix 162 | self.P = F.dot(self.P).dot(F.T) + Q 163 | 164 | def update_gnss(self,mes,sat_x,sat_y,sat_z): 165 | """ 166 | Desc: ekf update gnss step 167 | Input(s): 168 | mes: psuedorange measurements [N x 1] 169 | sat_pos satellite position [N x 3] 170 | Output(s): 171 | none 172 | """ 173 | num_sats = mes.shape[0] 174 | zt = mes 175 | H = np.zeros((num_sats,3)) 176 | h = np.zeros((num_sats,1)) 177 | for ii in range(num_sats): 178 | dist = np.sqrt((sat_x[ii]-self.mu[0])**2 + (sat_y[ii]-self.mu[1])**2 + (sat_z[ii]-self.mu[2])**2) 179 | H[ii,0] = (self.mu[0]-sat_x[ii])/dist 180 | H[ii,1] = (self.mu[1]-sat_y[ii])/dist 181 | H[ii,2] = (self.mu[2]-sat_z[ii])/dist 182 | h[ii] = dist 183 | yt = zt - h 184 | 185 | R_cov = 10.**2 186 | R = np.eye(num_sats)*R_cov 187 | 188 | Kt = self.P.dot(H.T).dot(np.linalg.inv(R + H.dot(self.P).dot(H.T))) 189 | 190 | self.mu = self.mu.reshape((3,1)) + Kt.dot(yt) 191 | self.P = (np.eye(self.mu_n)-Kt.dot(H)).dot(self.P).dot((np.eye(self.mu_n)-Kt.dot(H)).T) + Kt.dot(R).dot(Kt.T) 192 | 193 | yt = zt - H.dot(self.mu) 194 | 195 | # Kt = self.P.dot(H) 196 | 197 | 198 | def run(self): 199 | """ 200 | Desc: run ekf 201 | Input(s): 202 | none 203 | Output(s): 204 | none 205 | """ 206 | t_odom_prev = 0.0 # initialize previous odom time 207 | 208 | # setup progress bar 209 | print("running kalman filter, please wait...") 210 | bar = progress.bar.IncrementalBar('Progress:', max=len(self.times)) 211 | 212 | 213 | for tt, timestep in enumerate(self.times): 214 | # predict step for odometry 215 | # if self.odom_df['seconds of week [s]'].isin([timestep]).any(): 216 | # dt_odom = timestep - t_odom_prev 217 | # t_odom_prev = timestep 218 | # if tt == 0: 219 | # continue 220 | # odom_timestep = self.odom_df[self.odom_df['seconds of week [s]'] == timestep] 221 | # odom_vel_x = odom_timestep['ECEF_vel_x'].values[0] 222 | # odom_vel_y = odom_timestep['ECEF_vel_y'].values[0] 223 | # odom_vel_z = odom_timestep['ECEF_vel_z'].values[0] 224 | # self.predict_imu(np.array([[odom_vel_x,odom_vel_y,odom_vel_z]]).T,dt_odom) 225 | 226 | # update gnss step 227 | if self.sat_df['seconds of week [s]'].isin([timestep]).any(): 228 | sat_timestep = self.sat_df[self.sat_df['seconds of week [s]'] == timestep] 229 | pranges = sat_timestep['pr [m]'].to_numpy().reshape(-1,1) 230 | sat_x = sat_timestep['sat x ECEF [m]'].to_numpy().reshape(-1,1) 231 | sat_y = sat_timestep['sat y ECEF [m]'].to_numpy().reshape(-1,1) 232 | sat_z = sat_timestep['sat z ECEF [m]'].to_numpy().reshape(-1,1) 233 | self.predict_simple() 234 | self.update_gnss(pranges,sat_x,sat_y,sat_z) 235 | 236 | # add values to history 237 | self.mu_history = np.hstack((self.mu_history,self.mu)) 238 | self.P_history.append(np.trace(self.P)) 239 | bar.next() # progress bar 240 | 241 | bar.finish() # end progress bar 242 | 243 | self.mu_history = self.mu_history[:,:-1] 244 | self.mu_history[0,:] += self.x0 245 | self.mu_history[1,:] += self.y0 246 | self.mu_history[2,:] += self.z0 247 | self.P_history = self.P_history[:-1] 248 | 249 | 250 | def plot(self): 251 | fig, ax = plt.subplots() 252 | ax.ticklabel_format(useOffset=False) 253 | plt.subplot(131) 254 | plt.plot(self.times,self.mu_history[0,:]) 255 | plt.title("X vs Time") 256 | plt.xlabel("Time [hrs]") 257 | plt.ylabel("X [m]") 258 | 259 | plt.subplot(132) 260 | plt.plot(self.times,self.mu_history[1,:]) 261 | plt.title("Y vs Time") 262 | plt.xlabel("Time [hrs]") 263 | plt.ylabel("Y [m]") 264 | 265 | plt.subplot(133) 266 | plt.plot(self.times,self.mu_history[2,:]) 267 | plt.title("Z vs Time") 268 | plt.xlabel("Time [hrs]") 269 | plt.ylabel("Z [m]") 270 | 271 | # covariance plot 272 | plt.figure() 273 | plt.title("Trace of Covariance Matrix vs. Time") 274 | plt.xlabel("Time [hrs]") 275 | plt.ylabel("Trace") 276 | plt.plot(self.times,self.P_history) 277 | 278 | # trajectory plot 279 | lla_traj = np.zeros((len(self.times),3)) 280 | lon, lat, alt = pyproj.transform(self.ecef, self.lla, self.mu_history[0,:], self.mu_history[1,:], self.mu_history[2,:], radians=False) 281 | lla_traj[:,0] = lat 282 | lla_traj[:,1] = lon 283 | lla_traj[:,2] = alt 284 | fig, ax = plt.subplots() 285 | ax.ticklabel_format(useOffset=False) 286 | plt.plot(lla_traj[:,1],lla_traj[:,0]) 287 | plt.title("Trajectory") 288 | plt.xlabel("Longitude") 289 | plt.ylabel("Latitude") 290 | 291 | plt.show() 292 | 293 | if __name__ == '__main__': 294 | ekf = EKF('./data/dji_data_flight_1.csv','./data/sat_toy.csv') 295 | ekf.run() 296 | ekf.plot() 297 | -------------------------------------------------------------------------------- /lib/__init__.py: -------------------------------------------------------------------------------- 1 | from .gpstime import * 2 | -------------------------------------------------------------------------------- /lib/gpstime.py: -------------------------------------------------------------------------------- 1 | """ 2 | A Python implementation of GPS related time conversions. 3 | 4 | Copyright 2002 by Bud P. Bruegger, Sistema, Italy 5 | mailto:bud@sistema.it 6 | http://www.sistema.it 7 | 8 | 8 Modifications for GPS seconds by Duncan Brown 9 | 9 10 | 10 PyUTCFromGpsSeconds added by Ben Johnson 11 | 11 12 | 12 This program is free software; you can redistribute it and/or modify it under 13 | 13 the terms of the GNU Lesser General Public License as published by the Free 14 | 14 Software Foundation; either version 2 of the License, or (at your option) any 15 | 15 later version. 16 | 16 17 | 17 This program is distributed in the hope that it will be useful, but WITHOUT ANY 18 | 18 WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 19 | 19 PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 20 | 20 details. 21 | 21 22 | 22 You should have received a copy of the GNU Lesser General Public License along 23 | 23 with this program; if not, write to the Free Software Foundation, Inc., 59 24 | 24 Temple Place, Suite 330, Boston, MA 02111-1307 USA 25 | 25 26 | 26 GPS Time Utility functions 27 | 27 28 | 28 This file contains a Python implementation of GPS related time conversions. 29 | 29 30 | 30 The two main functions convert between UTC and GPS time (GPS-week, time of 31 | 31 week in seconds, GPS-day, time of day in seconds). The other functions are 32 | 32 convenience wrappers around these base functions. 33 | 33 34 | 34 A good reference for GPS time issues is: 35 | 35 http://www.oc.nps.navy.mil/~jclynch/timsys.html 36 | 36 37 | 37 Note that python time types are represented in seconds since (a platform 38 | 38 dependent Python) Epoch. This makes implementation quite straight forward 39 | 39 as compared to some algorigthms found in the literature and on the web. 40 | 40 41 | """ 42 | 43 | import time, math 44 | 45 | secsInWeek = 604800 46 | secsInDay = 86400 47 | gpsEpoch = (1980, 1, 6, 0, 0, 0) # (year, month, day, hh, mm, ss) 48 | 49 | 50 | def gpsFromUTC(year, month, day, hour, min, sec, leapSecs=18): 51 | """converts UTC to: gpsWeek, secsOfWeek, gpsDay, secsOfDay 52 | 53 | a good reference is: http://www.oc.nps.navy.mil/~jclynch/timsys.html 54 | 55 | This is based on the following facts (see reference above): 56 | 57 | GPS time is basically measured in (atomic) seconds since 58 | January 6, 1980, 00:00:00.0 (the GPS Epoch) 59 | 60 | The GPS week starts on Saturday midnight (Sunday morning), and runs 61 | for 604800 seconds. 62 | 63 | Currently, GPS time is 13 seconds ahead of UTC (see above reference). 64 | While GPS SVs transmit this difference and the date when another leap 65 | second takes effect, the use of leap seconds cannot be predicted. This 66 | routine is precise until the next leap second is introduced and has to be 67 | 68 | updated after that. 69 | SOW = Seconds of Week 70 | SOD = Seconds of Day 71 | 72 | Note: Python represents time in integer seconds, fractions are lost!!! 73 | """ 74 | secFract = sec % 1 75 | epochTuple = gpsEpoch + (-1, -1, 0) 76 | t0 = time.mktime(epochTuple) 77 | t = time.mktime((year, month, day, hour, min, sec, -1, -1, 0)) 78 | # Note: time.mktime strictly works in localtime and to yield UTC, it should be 79 | # corrected with time.timezone 80 | # However, since we use the difference, this correction is unnecessary. 81 | # Warning: trouble if daylight savings flag is set to -1 or 1 !!! 82 | t = t + leapSecs 83 | tdiff = t - t0 84 | gpsSOW = (tdiff % secsInWeek) + secFract 85 | gpsWeek = int(math.floor(tdiff/secsInWeek)) 86 | gpsDay = int(math.floor(gpsSOW/secsInDay)) 87 | gpsSOD = (gpsSOW % secsInDay) 88 | return (gpsWeek, gpsSOW, gpsDay, gpsSOD) 89 | -------------------------------------------------------------------------------- /lib/log_reader.py: -------------------------------------------------------------------------------- 1 | import csv 2 | import pandas as pd 3 | 4 | # Extract data different timesteps 5 | def Extract_timedata(input_path): 6 | raw_data = [] 7 | fix_data = [] 8 | header_fix = '' 9 | header_raw = '' 10 | with open(input_path, 'r') as f: 11 | t = -1 12 | for line in f: 13 | if line[0] == '#': 14 | line_data = line[2:].rstrip('\n').replace(" ","").split(",") 15 | if line_data[0] == 'Raw': 16 | header_raw = line_data[1:] 17 | elif line_data[0] == 'Fix': 18 | header_fix = line_data[1:] 19 | continue 20 | line_data = line.rstrip('\n').replace(" ","").split(",") 21 | if line_data[0] == 'Fix': 22 | fix_data.append(line_data[1:]) 23 | raw_data.append([]) 24 | t += 1 25 | elif line_data[0] == 'Raw': 26 | raw_data[t].append(line_data[1:]) 27 | return header_raw, raw_data, header_fix, fix_data 28 | 29 | 30 | # Extract data continuous and make a csv 31 | def MakeCsv(input_path): 32 | out_path = "./data/raw.csv" 33 | with open(out_path, 'w') as f: 34 | writer = csv.writer(f) 35 | with open(input_path, 'r') as f: 36 | for line in f: 37 | # Comments in the log file 38 | if line[0] == '#': 39 | # Remove initial '#', spaces, trailing newline and split using commas as delimiter 40 | line_data = line[2:].rstrip('\n').replace(" ","").split(",") 41 | if line_data[0] == 'Raw': 42 | writer.writerow(line_data[1:]) 43 | # Data in file 44 | else: 45 | # Remove spaces, trailing newline and split using commas as delimiter 46 | line_data = line.rstrip('\n').replace(" ","").split(",") 47 | if line_data[0] == 'Raw': 48 | writer.writerow(line_data[1:]) 49 | return out_path 50 | 51 | 52 | def MakeCsvFix(input_path): 53 | out_path = "./data/fix.csv" 54 | with open(out_path, 'w') as f: 55 | writer = csv.writer(f) 56 | with open(input_path, 'r') as f: 57 | for line in f: 58 | # Comments in the log file 59 | if line[0] == '#': 60 | # Remove initial '#', spaces, trailing newline and split using commas as delimiter 61 | line_data = line[2:].rstrip('\n').replace(" ","").split(",") 62 | if line_data[0] == 'Fix': 63 | writer.writerow(line_data[1:]) 64 | # Data in file 65 | else: 66 | # Remove spaces, trailing newline and split using commas as delimiter 67 | line_data = line.rstrip('\n').replace(" ","").split(",") 68 | if line_data[0] == 'Fix': 69 | writer.writerow(line_data[1:]) 70 | return out_path 71 | -------------------------------------------------------------------------------- /parse_sat_data.py: -------------------------------------------------------------------------------- 1 | from lib.log_reader import MakeCsv, MakeCsvFix 2 | import pandas as pd 3 | import math 4 | import numpy as np 5 | from lib import gpstime 6 | import time 7 | 8 | 9 | # parse txt data into csv and output path 10 | out_path = MakeCsv('./data/flight1.txt') 11 | 12 | # read csv file into a dataframe 13 | df = pd.read_csv(out_path) 14 | print(df.shape) 15 | # df = df[:30] # crop to the first 30 16 | 17 | TimeNanos = df.TimeNanos.to_numpy() 18 | FullBiasNanos = df.FullBiasNanos.to_numpy() 19 | BiasNanos = df.BiasNanos.to_numpy() 20 | TimeOffsetNanos = df.TimeOffsetNanos.to_numpy() 21 | ReceivedSvTimeNanos = df.ReceivedSvTimeNanos.to_numpy() 22 | 23 | ns_per_week = 604800E9 24 | c = 299792458. 25 | 26 | N_w = np.floor(-FullBiasNanos/ns_per_week) 27 | t_Rx_hw = TimeNanos+TimeOffsetNanos 28 | b_hw = FullBiasNanos + BiasNanos 29 | t_Rx_GPS = t_Rx_hw - b_hw 30 | t_Rx_w = t_Rx_GPS - N_w * ns_per_week 31 | p_ns = t_Rx_w - ReceivedSvTimeNanos 32 | p_ms = p_ns / 1.E6 33 | print("p_ms",p_ms) 34 | p_s = p_ms / 1000. 35 | p_m = c * p_s 36 | print("p_m",p_m) 37 | 38 | # result = pd.DataFrame{'week'N_w} 39 | 40 | results = pd.DataFrame({'SVid':df.Svid.to_numpy(),'ContellationType': df.ConstellationType.to_numpy(),'TDoA_[ms]':np.round(p_ms,decimals=3),'Pseudoranges_[m]':p_m.astype(dtype='int')}) 41 | results.to_csv('./data/python_data.csv') 42 | 43 | # parse txt data into csv and output path 44 | fix_out_path = MakeCsvFix('./data/flight1.txt') 45 | 46 | fix_df = pd.read_csv(fix_out_path) 47 | 48 | time_utc = fix_df['(UTC)TimeInMs'].to_numpy() 49 | N = time_utc.shape[0] 50 | # gps time: (gpsWeek, gpsSOW, gpsDay, gpsSOD) 51 | time_gps = np.zeros((N,4)) 52 | for ii in range(N): 53 | dt_utc = time.gmtime(time_utc[ii]/1000.) 54 | year = dt_utc.tm_year 55 | month = dt_utc.tm_mon 56 | day = dt_utc.tm_mday 57 | hour = dt_utc.tm_hour 58 | min = dt_utc.tm_min 59 | sec = dt_utc.tm_sec 60 | print(year,month,day,hour,min,sec) 61 | time_gps[ii,:] = gpstime.gpsFromUTC(year,month,day,hour,min,sec) 62 | # print(time_gps[ii,1]) 63 | 64 | fix_df['seconds of week [s]'] = time_gps[:,1] 65 | 66 | fix_df.to_csv('./data/android_fix_1.csv') 67 | -------------------------------------------------------------------------------- /toy_data_creation.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Author(s): D. Knowles 5 | Date: 19 Feb 2020 6 | Desc: toy data for AA272 sensor fusion project 7 | """ 8 | 9 | import pandas as pd 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from mpl_toolkits.mplot3d import Axes3D 13 | 14 | 15 | # create time vector 16 | time = np.arange(401) 17 | 18 | # create square 19 | x_truth = np.concatenate((np.arange(101), 100*np.ones(99), np.flip(np.arange(101)), np.zeros(100))).reshape((-1,1)) 20 | y_truth = np.concatenate((np.zeros(100), np.arange(100), 100*np.ones(101), np.flip(np.arange(100)))).reshape((-1,1)) 21 | z_truth = np.zeros((401,1)) 22 | 23 | pos_truth = np.hstack((x_truth,y_truth,z_truth)) 24 | 25 | # plt.figure() 26 | # plt.plot(time,x_truth) 27 | # plt.plot(time,y_truth) 28 | # 29 | # plt.figure() 30 | # plt.plot(x_truth,y_truth) 31 | # plt.show() 32 | 33 | pos_sat = np.array([[ 15000000., -25000000., -7500000.], 34 | [-20000000., 4000000., 15000000.], 35 | [ 8000000., 15000000., -1000000.], 36 | [ 2000000., -40000000., -1500000.], 37 | [ -1200000., -10000000., 20000000.], 38 | [ 800000., -25000000., 6000000.]]) 39 | 40 | # fig = plt.figure() 41 | # ax = fig.gca(projection='3d') 42 | # plt.xlabel('x') 43 | # for ii in range(6): 44 | # ax.plot([0.0,pos_sat[ii,0]], [0.0,pos_sat[ii,1]], [0.0,pos_sat[ii,2]]) 45 | # plt.show() 46 | 47 | pr = [] 48 | 49 | for ii in range(401): 50 | for jj in range(6): 51 | pr.append(np.linalg.norm(pos_sat[jj,:]-pos_truth[ii]) + np.random.normal(0.0,8.0)) 52 | 53 | sv = np.arange(1,7).reshape(-1,1) 54 | 55 | # save to file 56 | df1 = pd.DataFrame() 57 | df1['seconds of week [s]'] = np.repeat(time,6) 58 | df1['SV'] = np.tile(sv,(401,1)) 59 | df1['pr [m]'] = pr 60 | df1['sat x ECEF [m]'] = np.tile(pos_sat[:,0].reshape(-1,1),(401,1))# + np.random.normal(1E2,1.,(401*6,1)) 61 | df1['sat y ECEF [m]'] = np.tile(pos_sat[:,1].reshape(-1,1),(401,1))# + np.random.normal(-1E2,1.,(401*6,1)) 62 | df1['sat z ECEF [m]'] = np.tile(pos_sat[:,2].reshape(-1,1),(401,1))# + np.random.normal(-1E2,1.,(401*6,1)) 63 | df1.to_csv('./data/sat_toy.csv') 64 | 65 | 66 | # time_gps = gpstime.gpsfromutc(time_utc) 67 | --------------------------------------------------------------------------------