require 'rspec' require 'tempfile' $: << File::join(File::dirname(__FILE__), '..', 'build_SWIG') require 'GPS.so' describe 'GPS solver' do let(:input){{ :rinex_nav => Tempfile::open{|f| if true then f.write(<<-__RINEX_NAV_TEXT__) 2.10 N: GPS NAV DATA RINEX VERSION / TYPE teqc 2013Mar15 BKG Frankfurt 20150617 00:16:14UTCPGM / RUN BY / DATE Linux 2.4.21-27.ELsmp|Opteron|gcc -static|Linux x86_64|=+ COMMENT 2.10 N: GPS NAV DATA COMMENT teqc 2008Feb15 20150617 00:10:22UTCCOMMENT Linux 2.4.20-8|Pentium IV|gcc -static|Linux|486/DX+ COMMENT 1.3970D-08 2.2352D-08 -1.1921D-07 -1.1921D-07 ION ALPHA 1.1059D+05 1.6384D+05 -6.5536D+04 -5.2429D+05 ION BETA 3.725290298462D-09 1.509903313490D-14 319488 1849 DELTA-UTC: A0,A1,T,W 16 LEAP SECONDS Concatenated RINEX files (28/) COMMENT Original file name: brdc1670.15n COMMENT END OF HEADER 31 15 6 16 0 0 0.0 3.128303214908D-04-1.136868377216D-12 0.000000000000D+00 7.800000000000D+01 1.292812500000D+02 4.074098273975D-09 1.410690806649D+00 6.768852472305D-06 8.027532137930D-03 4.271045327187D-06 5.153535821915D+03 1.728000000000D+05 9.872019290924D-08 8.650894583569D-01 8.940696716309D-08 9.750288799237D-01 3.013750000000D+02-5.268559212524D-01-8.036049019412D-09 3.664438352852D-10 1.000000000000D+00 1.849000000000D+03 0.000000000000D+00 2.000000000000D+00 0.000000000000D+00-1.350417733192D-08 7.800000000000D+01 1.663560000000D+05 4.000000000000D+00 24 15 6 16 0 0 0.0-4.998780786991D-05-5.684341886081D-13 0.000000000000D+00 9.000000000000D+01 1.401250000000D+02 4.642336229081D-09 2.380519254182D+00 7.258728146553D-06 3.228595596738D-03 3.971159458160D-06 5.153673912048D+03 1.728000000000D+05-2.980232238770D-08 8.160388274984D-01-1.490116119385D-08 9.533772739559D-01 2.969375000000D+02 3.612098431884D-01-8.379634759709D-09 4.025167664390D-10 1.000000000000D+00 1.849000000000D+03 0.000000000000D+00 2.900000000000D+00 0.000000000000D+00 2.328306436539D-09 9.000000000000D+01 1.656600000000D+05 4.000000000000D+00 12 15 6 16 0 0 0.0 3.018006682396D-04 3.183231456205D-12 0.000000000000D+00 9.000000000000D+01-8.984375000000D+01 4.146244136282D-09 1.019955493515D+00 -4.906207323074D-06 5.596161005087D-03 2.166256308556D-06 5.153664655685D+03 1.728000000000D+05-3.352761268616D-08 1.926955253672D+00 9.685754776001D-08 9.895896897770D-01 3.531562500000D+02 6.166481681571D-01-8.222842514396D-09 -2.525105180766D-10 1.000000000000D+00 1.849000000000D+03 0.000000000000D+00 2.000000000000D+00 0.000000000000D+00-1.210719347000D-08 9.000000000000D+01 1.708800000000D+05 4.000000000000D+00 25 15 6 16 0 0 0.0-2.291053533554D-06-4.206412995700D-12 0.000000000000D+00 4.300000000000D+01-8.231250000000D+01 4.456614188797D-09 3.514302168106D-01 -4.393979907036D-06 4.659408819862D-03 1.480802893639D-06 5.153666315079D+03 1.728000000000D+05 0.000000000000D+00 1.880081553172D+00 1.080334186554D-07 9.780612252959D-01 3.641250000000D+02 7.406192694225D-01-8.389992700586D-09 -1.742929689463D-10 1.000000000000D+00 1.849000000000D+03 0.000000000000D+00 2.000000000000D+00 0.000000000000D+00 5.587935447693D-09 4.300000000000D+01 1.728000000000D+05 18 15 6 15 23 59 44.0 4.096841439605D-04 2.955857780762D-12 0.000000000000D+00 0.000000000000D+00-2.200000000000D+01 5.964177003340D-09-4.658085121885D-01 -1.063570380211D-06 1.631921075750D-02 5.243346095085D-06 5.153597631454D+03 1.727840000000D+05-9.499490261078D-08-1.282443549024D+00 1.769512891769D-07 9.246030417022D-01 2.601875000000D+02-1.955724914307D+00-8.981445541829D-09 -4.760912596834D-10 1.000000000000D+00 1.849000000000D+03 0.000000000000D+00 2.000000000000D+00 0.000000000000D+00-1.117587089539D-08 0.000000000000D+00 1.696560000000D+05 4.000000000000D+00 29 15 6 16 0 0 0.0 6.168931722641D-04 2.273736754432D-12 0.000000000000D+00 7.300000000000D+01-1.041875000000D+02 4.061597753278D-09 5.907941646969D-01 -5.280598998070D-06 1.195417135023D-03 1.201778650284D-05 5.153755130768D+03 1.728000000000D+05 9.313225746155D-09 2.981267878597D+00-1.303851604462D-08 9.739471617229D-01 1.573125000000D+02-5.419126340021D-01-7.659961925303D-09 2.107230631757D-10 1.000000000000D+00 1.849000000000D+03 0.000000000000D+00 2.000000000000D+00 0.000000000000D+00-9.778887033463D-09 7.300000000000D+01 1.656180000000D+05 4.000000000000D+00 __RINEX_NAV_TEXT__ else # equivalent open(File::join(File::dirname(__FILE__), "..", "..", "test_log", "brdc1670.15n"), 'r'){|io| while (str = io.readline) f.puts("%-80s"%[str.chomp]) break if str =~ /END OF HEADER/ end while !io.eof? entry = 8.times.collect{io.readline} next unless entry[0] =~ /^([ \d]\d) / prn = $1.to_i next unless entry[6] =~ /(\d\.\d{12})D([+-]\d{2})\s*$/ iode = ($1.to_f * (10 ** $2.to_i)).to_i next unless [[12, 90], [18, 0], [24, 90], [25, 43], [29, 73], [31, 78]] \ .include?([prn, iode]) entry.each{|str| f.print("%-80s\n"%[str.chomp]) } end } end f.path }, :measurement => proc{ l1p = GPS::Measurement::L1_PSEUDORANGE l1d = GPS::Measurement::L1_RANGE_RATE { 12 => {l1p => 23707858.8131, l1d => -466.953123206}, 18 => {l1p => 25319310.9754, l1d => -453.797772239}, 24 => {l1p => 25683081.3903, l1d => -579.768048832}, 25 => {l1p => 21551698.5927, l1d => -847.86674626}, 29 => {l1p => 23637198.3968, l1d => -1560.30646593}, 31 => {l1p => 23707474.1231, l1d => -1423.67588761}, } }.call, :rinex_obs => Tempfile::open{|f| if true then f.write(<<-__RINEX_OBS_TEXT__) 2.11 OBSERVATION DATA M (MIXED) RINEX VERSION / TYPE BINEX2RINEX 1.00 GSI, JAPAN 20150617 18:30:58UTCPGM / RUN BY / DATE 0228 MARKER NAME GSI, JAPAN GEOSPATIAL INFORMATION AUTHORITY OF JAPAOBSERVER / AGENCY 00000 TRIMBLE NETR9 4.93,26/NOV/2014 REC # / TYPE / VERS TRM59800.80 GSI ANT # / TYPE -3952590.4754 3360273.8926 3697987.2632 APPROX POSITION XYZ 0.0000 0.0000 0.0000 ANTENNA: DELTA H/E/N 1 1 WAVELENGTH FACT L1/2 6 L1 C1 L2 P2 S1 S2 # / TYPES OF OBSERV 2015 6 16 0 0 0.0000000 GPS TIME OF FIRST OBS 30.000 INTERVAL 16 LEAP SECONDS smtt on - smooth time tag (ms jumps in phase and range) COMMENT END OF HEADER 15 6 16 0 0 0.0000000 0 13R04R14G22G18G25G14R03G12R13G29R23R24 0.000000000 G26 107962537.996 8 20161244.813 83970896.187 7 20161248.340 53.100 45.700 108292688.703 7 20315410.305 84227597.491 6 20315417.836 44.900 38.600 120164532.762 7 22866559.250 93634735.08845 22866565.0164 44.800 31.9004 127657315.100 5 24292417.195 99473356.27642 24292426.4844 33.000 16.9004 107108718.011 8 20382088.883 83461307.35447 20382098.2114 52.700 44.8004 106665594.451 8 20297781.844 83116060.16147 20297788.3284 48.500 43.6004 112247722.253 8 20968815.328 87303792.973 7 20968819.930 48.200 43.900 119180856.024 7 22679363.352 92868240.86845 22679370.8554 46.000 34.4004 113219403.456 7 21202362.125 88059583.077 7 21202366.582 43.300 42.600 116573807.952 8 22183256.359 90836814.38846 22183263.8554 48.000 37.2004 118068213.681 8 22071607.477 91830908.595 5 22071616.434 48.300 34.300 122821571.168 6 22968227.047 95527813.562 5 22968231.008 41.000 33.400 126887720.863 7 24145939.008 98873654.81144 24145951.6954 42.000 24.8004 15 6 16 0 0 30.0000000 0 13R04R14G22G18G25G14R03G12R13G29R23R24 0.000000000 G26 107953906.562 8 20159632.578 83964182.831 7 20159636.547 52.900 45.500 108275524.990 7 20312191.531 84214247.928 6 20312198.477 46.000 37.900 120243117.022 7 22881512.617 93695969.52045 22881519.3444 43.200 30.0004 127752623.271 5 24310554.234 99547622.25842 24310562.6254 34.300 14.8004 107148475.172 8 20389654.469 83492286.95147 20389663.6914 53.000 44.7004 106649633.973 8 20294744.781 83103623.41147 20294751.4144 49.800 44.5004 112354437.822 8 20988750.797 87386793.909 7 20988756.371 48.000 44.200 119276901.051 7 22697639.727 92943081.07945 22697647.7704 47.500 34.6004 113277695.919 7 21213278.297 88104921.616 7 21213283.438 43.800 42.700 116499708.794 8 22169155.719 90779074.87246 22169163.7704 48.500 37.6004 118027431.830 8 22063984.773 91799189.430 5 22063991.707 49.300 35.100 122710898.974 6 22947531.234 95441735.252 5 22947536.609 40.900 33.600 126789541.785 6 24127256.484 98797151.68444 24127268.9224 41.900 25.3004 15 6 16 0 1 0.0000000 0 13R04R14G22G18G25G14R03G12R13G29R23R24 0.000000000 G26 107946098.362 8 20158174.969 83958109.762 7 20158178.289 53.200 44.900 108258590.494 7 20309014.148 84201076.653 6 20309021.145 46.000 38.700 120321906.821 7 22896506.344 93757364.10145 22896512.2934 44.600 30.2004 127847866.993 6 24328677.648 99621838.11742 24328687.3954 37.300 16.9004 107188670.264 8 20397303.531 83523607.79247 20397312.3054 53.700 44.3004 106634143.554 8 20291797.133 83091552.94947 20291803.6724 49.900 44.0004 112461605.174 7 21008771.203 87470146.236 7 21008775.176 47.500 43.900 119373111.958 8 22715948.289 93018050.54445 22715955.9534 48.100 34.1004 113336078.148 7 21224212.609 88150329.998 7 21224216.957 43.000 42.500 116425964.067 7 22155122.617 90721611.48746 22155131.1914 46.200 37.6004 117987330.119 7 22056486.641 91767999.260 5 22056494.809 46.900 34.500 122600398.977 6 22926865.672 95355790.881 5 22926871.977 39.700 32.500 126691542.694 6 24108607.188 98720788.84544 24108620.5234 41.700 24.8004 __RINEX_OBS_TEXT__ else # equivalent # GEONET Setagaya from https://terras.gsi.go.jp/data_service.php#11/35.663712/139.630394 open(File::join(File::dirname(__FILE__), "..", "..", "test_log", "02281670.15o"), 'r'){|io| 99.times{|i| break unless str = io.readline f.write(str) } } end f.path }, }} let(:solver){ res = GPS::Solver::new res.correction = {:gps_ionospheric => :klobuchar, :gps_tropospheric => :hopfield} res } describe 'demo' do it 'calculates position without any error' do sn = solver.gps_space_node puts "RINEX NAV read: %d items."%[sn.read(input[:rinex_nav])] meas = GPS::Measurement::new input[:measurement].each{|prn, items| items.each{|k, v| meas.add(prn, k, v) } } expect(meas.to_hash).to eq(proc{|array| res = {} array.each{|prn, k, v| (res[prn][k] = v) rescue (res[prn] = {k => v}) } res }.call(meas.to_a)) expect(GPS::Measurement::new(meas.to_a).to_a.sort).to eq(meas.to_a.sort) # accept [[prn, k, v], ...] expect(GPS::Measurement::new(meas.to_hash).to_a.sort).to eq(meas.to_a.sort) # accept {prn => {k => v, ...}, ...} expect{GPS::Measurement::new({:sym => {1 => 2}})}.to raise_error expect{GPS::Measurement::new({1 => {:sym => 2}})}.to raise_error expect{GPS::Measurement::new({1 => [2, 3]})}.to raise_error t_meas = GPS::Time::new(1849, 172413) puts "Measurement time: #{t_meas.to_a} (a.k.a #{"%d/%d/%d %02d:%02d:%02d UTC"%[*t_meas.c_tm]})" expect(t_meas.c_tm).to eq([2015, 6, 15, 23, 53, 33]) expect(GPS::Time::new(0, t_meas.serialize)).to eq(t_meas) sn.update_all_ephemeris(t_meas) [:alpha, :beta].each{|k| puts "Iono #{k}: #{sn.iono_utc.send(k)}" } meas.each{|prn, k, v| eph = sn.ephemeris(prn) puts "XYZ(PRN:#{prn}): #{eph.constellation(t_meas)[0].to_a} (iodc: #{eph.iodc}, iode: #{eph.iode})" } run_solver = proc{ pvt = solver.solve(meas, t_meas) [ :error_code, :position_solved?, [:receiver_time, proc{|v| v.to_a}], :used_satellite_list, [:llh, proc{|llh| llh.to_a.zip([180.0 / Math::PI] * 2 + [1]).collect{|v, sf| v * sf}}], :receiver_error, [:velocity, proc{|xyz| xyz.to_a}], :receiver_error_rate, [:G_enu, proc{|mat| mat.to_s}], :fd, :fde_min, :fde_2nd, ].each{|fun, task| task ||= proc{|v| v} puts "pvt.#{fun}: #{task.call(pvt.send(fun))}" } pvt } puts "Normal solution ..." pvt = run_solver.call puts expect(pvt.position_solved?).to be(true) expect(pvt.receiver_time.to_a).to eq([1849, 172413]) expect(pvt.llh.to_a).to eq([:lat, :lng, :alt].collect{|k| pvt.llh.send(k)}) expect(pvt.llh.lat / Math::PI * 180).to be_within(1E-9).of(35.6992591268) # latitude expect(pvt.llh.lng / Math::PI * 180).to be_within(1E-9).of(139.541502292) # longitude expect(pvt.llh.alt) .to be_within(1E-4).of(104.279402455) # altitude expect(pvt.receiver_error).to be_within(1E-4).of(1259087.83603) expect(pvt.gdop).to be_within(1E-10).of(3.83282723293) expect(pvt.pdop).to be_within(1E-10).of(3.30873220653) expect(pvt.hdop).to be_within(1E-10).of(2.05428293774) expect(pvt.vdop).to be_within(1E-10).of(2.59376761222) expect(pvt.tdop).to be_within(1E-10).of(1.9346461648) expect(pvt.velocity.to_a).to eq([:e, :n, :u].collect{|k| pvt.velocity.send(k)}) expect(pvt.velocity.north).to be_within(1E-7).of(-0.839546227836) # north expect(pvt.velocity.east) .to be_within(1E-7).of(-1.05805616381) # east expect(pvt.velocity.down) .to be_within(1E-7).of(-0.12355474006) # down expect(pvt.receiver_error_rate).to be_within(1E-7).of(-1061.92654151) expect(pvt.G.rows).to eq(6) expect(pvt.W.rows).to eq(6) expect(pvt.delta_r.rows).to eq(6) expect(pvt.G_enu.rows).to eq(6) expect(Math::sqrt(pvt.C[3, 3])).to be_within(1E-10).of(pvt.tdop) expect(Math::sqrt(pvt.C_enu[2, 2])).to be_within(1E-10).of(pvt.vdop) pvt.S.to_a.flatten.zip( ((pvt.G.t * pvt.W * pvt.G).inv * (pvt.G.t * pvt.W)).to_a.flatten).each{|a, b| expect(a).to be_within(1E-10).of(b) } pvt.S_enu.to_a.flatten.zip( ((pvt.G_enu.t * pvt.W * pvt.G_enu).inv * (pvt.G_enu.t * pvt.W)).to_a.flatten).each{|a, b| expect(a).to be_within(1E-10).of(b) } expect([:rows, :columns].collect{|f| pvt.slope_HV_enu.send(f)}).to eq([6, 2]) expect(pvt.used_satellites).to eq(6) expect(pvt.used_satellite_list).to eq([12,18, 24, 25, 29, 31]) meas.each{|prn, k, v| solver.gps_options.exclude(prn) puts "Excluded(PRN: #{solver.gps_options.excluded.join(', ')}) solution ..." run_solver.call solver.gps_options.excluded.each{|prn| solver.gps_options.include(prn) } puts } end it 'can be modified through hooks' do sn = solver.gps_space_node expect(solver.correction[:gps_ionospheric]).to include(:klobuchar) expect(solver.correction[:gps_tropospheric]).to include(:hopfield) expect{solver.correction = nil}.to raise_error(RuntimeError) expect{solver.correction = { :gps_ionospheric => [proc{|t, usr_pos, sat_pos| expect(t).to be_a_kind_of(GPS::Time) expect(usr_pos).to be_a_kind_of(Coordinate::XYZ) unless usr_pos expect(sat_pos).to be_a_kind_of(Coordinate::ENU) unless sat_pos false }, :klobuchar, :no_correction], :options => {:f_10_7 => 10}, }}.not_to raise_error expect(solver.correction[:gps_ionospheric]).to include(:no_correction) expect(solver.correction[:options][:f_10_7]).to eq(10) sn.read(input[:rinex_nav]) t_meas = GPS::Time::new(1849, 172413) sn.update_all_ephemeris(t_meas) solver.hooks[:relative_property] = proc{|prn, rel_prop, meas, rcv_e, t_arv, usr_pos, usr_vel| expect(input[:measurement]).to include(prn) expect(meas).to be_a_kind_of(Hash) expect(t_arv).to be_a_kind_of(GPS::Time) expect(usr_pos).to be_a_kind_of(Coordinate::XYZ) expect(usr_vel).to be_a_kind_of(Coordinate::XYZ) weight, range_c, range_r, rate_rel_neg, *los_neg = rel_prop weight = 1 [weight, range_c, range_r, rate_rel_neg] + los_neg } solver.hooks[:update_position_solution] = proc{|mat_G, mat_W, mat_delta_r, temp_pvt| expect(temp_pvt).to be_a_kind_of(GPS::PVT) [mat_G, mat_W, mat_delta_r].each{|mat| expect(mat).to be_a_kind_of(SylphideMath::MatrixD) expect(mat.rows).to be >= temp_pvt.used_satellites expect(mat).to respond_to(:resize!) } } pvt = solver.solve( input[:measurement].collect{|prn, items| items.collect{|k, v| [prn, k, v]} }.flatten(1), t_meas) expect(pvt.W).to eq(SylphideMath::MatrixD::I(pvt.W.rows)) end it 'calculates position without any error with RINEX obs file' do sn = solver.gps_space_node puts "RINEX NAV read: %d items."%[sn.read(input[:rinex_nav])] GPS::RINEX_Observation::read(input[:rinex_obs]){|item| t_meas = item[:time] puts "Measurement time: #{t_meas.to_a} (a.k.a #{"%d/%d/%d %02d:%02d:%02d UTC"%[*t_meas.c_tm]})" sn.update_all_ephemeris(t_meas) meas = GPS::Measurement::new types = (item[:meas_types]['G'] || item[:meas_types][' ']).collect.with_index{|type_, i| case type_ when "C1" [i, GPS::Measurement::L1_PSEUDORANGE] when "D1" [i, GPS::Measurement::L1_RANGE_RATE] else nil end }.compact item[:meas].each{|k, v| sys, prn = k next unless sys == 'G' # GPS only types.each{|i, type_| meas.add(prn, type_, v[i][0]) if v[i] } } pvt = solver.solve(meas, t_meas) expect(pvt.position_solved?).to eq(true) puts pvt.llh.to_a.zip([180.0 / Math::PI] * 2 + [1]).collect{|v, sf| v * sf}.inspect if approx_pos = proc{ next false unless res = item[:header].select{|k, v| k =~ /APPROX POSITION XYZ/}.values[0] next false unless res = res.collect{|item| item.scan(/([+-]?\d+(?:\.\d+)?)\s*/).flatten }.reject{|item| item.empty? }[0] Coordinate::XYZ::new(*(res.collect{|str| str.to_f})) }.call then approx_pos.to_a.zip(pvt.xyz.to_a).each{|a, b| expect(a).to be_within(1E+1).of(b) # 10 m } end pvt.used_satellite_list.each{|prn| eph = sn.ephemeris(prn) puts "XYZ(PRN:#{prn}): #{eph.constellation(t_meas)[0].to_a} (iodc: #{eph.iodc}, iode: #{eph.iode})" } } end end end