!README.md

How to use this program to estimate relative position between two near GNSS antennas

1. Install Ruby
   • For Windows users, RubyInstaller is convinient, and please select "WITH DEVKIT" version because of native compilation required by a dependent gem.
2. Install gps_pvt gem: gem install gps_pvt
3. Download est_proximity.rb
4. Type the following on the directory in which est_proximity.rb and observation files are stored: ./est_proximity.rb base_obs_file aux_obs_file1 [aux_obs_file2] ... [options] ...
   • The supported formats of the observation files are u-blox(UBX), RINEX OBS(v2/v3). COM port and Ntrip stream can be used. The details are described in gps_pvt readme.
   • --online_ephemeris option is so useful that the corresponding ephemeris is automatically downloaded.
   • The default assumption of the distance between the antennas is a half cycle. To change the assumption, an option like --ant_lambda=3/2 specifying one and a half cycles should be used.

est_proximity.rb

#!/usr/bin/ruby
# coding: cp932

require 'gps_pvt'
require 'uri'

$stderr.puts <<__STRING__
Usage: #{__FILE__} GPS_file1 GPS_file2 ...
__STRING__

options = []
misc_options = {
  :acceptable_dt => 1E-1, # 100 ms
  :dump_dd => false,
  :ant_lambda => Rational(1,2), # antenna spacing, default is half cycle
}

# check options and file format
files = ARGV.collect{|arg|
  next [arg, nil] unless arg =~ /^--([^=]+)=?/
  k, v = [$1.downcase.to_sym, $']
  next [v, k] if [:rinex_nav, :rinex_obs, :ubx, :sp3, :antex, :rinex_clk, :rtcm3].include?(k) # file type
  options << [$1.to_sym, $']
  nil
}.compact

options.reject!{|opt|
  case opt[0]
  when :online_ephemeris
    (misc_options[opt[0]] ||= []) << opt[1]
  when :acceptable_dt
    misc_options[opt[0]] = Float(opt[1])
  when :dump_dd
    misc_options[opt[0]] = opt[1]
  when :ant_lambda
    misc_options[opt[0]] = Rational(opt[1])
  else
    next false
  end
  true
}

# Check file existence and extension
files.collect!{|fname, ftype|
  ftype ||= case fname
  when /\.\d{2}[nhqg](?:\.gz)?$/; :rinex_nav
  when /\.\d{2}o(?:\.gz)?$/; :rinex_obs
  when /\.ubx$/; :ubx
  when /\.sp3(?:\.Z)?$/; :sp3
  when /\.atx(?:\.Z)?$/; :antex
  when /\.clk$/; :rinex_clk
  end
  if (!(uri = URI::parse(fname)).instance_of?(URI::Generic) rescue false) then
    ftype ||= uri.read_format if uri.instance_of?(URI::Ntrip)
    fname = uri
  end
  raise "Format cannot be guessed, use --(format, ex. rinex_nav)=#{fname}" unless ftype
  [fname, ftype]
}

rcv = GPS_PVT::Receiver::new(options)

proc{|src|
  rcv.attach_online_ephemeris(src) if src
}.call(misc_options[:online_ephemeris])

# check file type
obs_files = files.select{|fname, ftype|
  case ftype
  when :rinex_nav; rcv.parse_rinex_nav(fname)
  when :sp3; rcv.attach_sp3(fname)
  when :antex; rcv.attach_antex(fname)
  when :rinex_clk; rcv.attach_rinex_clk(fname)
  when :ubx, :rinex_obs, :rtcm3
    next true
  end
  false
}.compact

LAMBDA = GPS_PVT::GPS::SpaceNode.L1_WaveLength
Mat = GPS_PVT::SylphideMath::MatrixD

dump_diff2 = proc{|fname| # double difference printer
  io = case fname
  when ''; $stdout
  when String; open(fname, 'w')
  else; next proc{}
  end
  keys = []
  header = nil
  proc{|t, ant_diff_list, meas_diff2_list, mat_G|
    values = []
    meas_diff2_list.zip(ant_diff_list).each{|meas_diff2, ants|
      meas_diff2.each{|prns, meas|
        k = [ants, prns].flatten
        i_k = keys.index(k) || ((keys << k).size - 1)
        values[i_k] = [:L1_CARRIER_PHASE, :L1_PSEUDORANGE].collect{|k2|
          meas[k2]
        } + mat_G.delta(*prns).to_a[0][0..2]
      }
    }
    header ||= proc{
      io.puts [:week, :second, :ant1, :ant0, :prn1, :prn0, :CP, :PR, :G_E, :G_N, :G_U].join(',')
      true
    }.call
    io.puts [t.to_a, keys.zip(values).collect{|k, v| v ? [k, v] : ([nil] * 9)}].flatten.join(',')
  }
}.call(misc_options[:dump_dd])
  
estimate_relative = proc{
  diff2_range, amb_range = proc{|v|
    [(-v)..v, (((-v).floor)..(v.ceil)).to_a]
  }.call(misc_options[:ant_lambda] * 2)
  puts [:week, :second, :ant1, :ant0,
      :E, :N, :U, :psi, :phi,
      '1st', '2nd', '3rd', '4th', '5th'].join(',')
  # ant_diff = [1nt1, ant0], meas_diff2 = {[prn1, prn0] => {PR => v, CR => v}, ...}
  proc{|t, ant_diff, meas_diff2, mat_G|
    y, mat_G2 = meas_diff2.collect{|(prn1, prn0), meas|
      next unless (cp = meas[:L1_CARRIER_PHASE])
      [[cp - cp.floor], mat_G.delta(prn1, prn0).to_a[0][0..2]]
    }.compact.transpose.collect{|v|
      Mat::new(v)
    }
    mat_inv = (mat_G2.t * mat_G2).inv
    mat_P = (mat_G2 * mat_inv * mat_G2.t * -1) + 1
    calc_J = proc{|y2| (y2.t * mat_P * y2)[0, 0]}
  
    y_int_candidates = y.rows.times.collect{|i|
      amb_range.select{|v_int| diff2_range.include?(y[i, 0] + v_int)}
    }
    sorted = y_int_candidates[0].product(*(y_int_candidates[1..-1])).collect{|y_int|
      [(calc_J.call(y + Mat::new([y_int]).t) rescue nil), y_int]
    }.select{|v| v[0]}.sort{|a, b| a[0] <=> b[0]}
    
    x = (mat_inv * mat_G2.t * LAMBDA * (y + Mat::new([sorted[0][1]]).t)).to_a.flatten
    psi = Math::atan2(x[0], x[1]) / Math::PI * 180
    phi = Math::atan2(x[2], Math::sqrt(x[0] ** 2 + x[1] ** 2)) / Math::PI * 180
    puts [t.to_a, ant_diff, x, psi, phi, sorted[0..4].transpose[0]].flatten.join(',')
  }
}.call

generate_diff2 = proc{|base, *aux|
  # info := [ant_i, t_meas, meas, pvt], base = info, aux = [info, ...]
  ant_list, meas_list = [base, *aux].collect{|i, t_meas, meas, pvt| [i, meas.to_hash2]}.transpose
  t, pvt = base.values_at(1, 3)
  $stderr.puts "Calculate relative position @ %dw%.1f"%[*(t.to_a)]
  satellites = pvt.used_satellite_list
  # [[elv_deg, prn], ...], elevation descending order
  mat_G = pvt.G_enu
  mat_G.instance_eval{
    define_singleton_method(:select){|prn| row_vector(satellites.index(prn))}
    define_singleton_method(:delta){|prn1, prn2| select(prn1) - select(prn2)}
  }
  
  elv_deg = mat_G.column_vector(2).to_a.flatten.collect{|v|
    Math::asin(-v) / Math::PI * 180
  }.zip(satellites).sort{|a, b| b[0] <=> a[0]}

  # difference between two antennas (single difference)
  # ant_diff_list = [[ant1, ant0], ...]
  ant_diff_list = ant_list[1..-1].zip([ant_list[0]] * (ant_list.size - 1))
  # meas_diff_list = [{prn => {PR => v, CP => v}, ...}, ]
  meas_diff_list = meas_list[1..-1].collect{|meas|
    Hash[*(meas_list[0].keys & meas.keys).collect{|prn|
      next nil unless row = satellites.index(prn)
      meas0, meas1 = [meas_list[0][prn], meas[prn]]
      items = meas0.keys & meas1.keys & [:L1_CARRIER_PHASE, :L1_PSEUDORANGE]
      next nil if items.empty?
      prop = Hash[*(items.collect{|k|
        [k, meas1[k] - meas0[k]]
      }.flatten(1))]
      [prn, prop]
    }.compact.flatten(1)]
  }
  #p meas_diff_list
  
  # Then, difference between two satellites (double difference)
  # meas_diff2_list = [{[prn1, prn0] => {PR => v, CR => v}, ...}, ]
  meas_diff2_list = meas_diff_list.collect{|meas_diff|
    prn_max_elv = elv_deg[0][1]
    # make combination (max elevation and others)
    Hash[*((meas_diff.keys - [prn_max_elv]).collect{|prn|
      meas0, meas1 = [meas_diff[prn_max_elv], meas_diff[prn]]
      [[prn, prn_max_elv], Hash[*((meas0.keys & meas1.keys).collect{|k|
        [k, meas1[k] - meas0[k]]
      }.flatten(1))]]
    }.flatten(1))]
  }
  #meas_diff2_list
  dump_diff2.call(t, ant_diff_list, meas_diff2_list, mat_G)
  
  ant_diff_list.zip(meas_diff2_list).each{|ant_diff, meas_diff2|
    estimate_relative.call(t, ant_diff, meas_diff2, mat_G)
  }
}

adjust_timing = proc{
  dt = misc_options[:acceptable_dt]
  obs = [] # buffer for observation. 
  t_tag_oldest = nil
  obs_oldest = []
  mtx = Thread::Mutex::new
  proc{|i, t_meas, meas, pvt|
    mtx.synchronize{
      (obs[i] ||= []) << [[t_meas.week, (t_meas.seconds + dt/2).div(dt)], t_meas, meas, pvt]
      #p obs.collect{|epochs| (epochs) ? epochs.collect{|v| v[0][1]} : nil} # for debug
      #puts "oldest: #{obs_oldest.collect{|v| v && v[0][1]}}" # for debug
      obs_active = obs.collect.with_index{|epochs, i|
        epochs ? [epochs[0].first, i, epochs.size] : nil
      }.compact # Ignore antennas not providing observation (ex, ephemeris only)
      # If 2 consecutive epoch data is obtained from each antenna, then time adjustment is performed.
      if (obs_active.size >= 2) && (obs_active.all?{|t_tag, i, size| size > 1}) then
        t_tag_shift, i_shift = obs_active.sort.first # time and index ascending order
        unless t_tag_shift == t_tag_oldest then
          obs_est = obs_oldest.collect.with_index{|(t_tag, *rest), i|
            t_tag ? [i, *rest] : nil
          }.compact
          generate_diff2.call(*obs_est) if obs_est.size >= 2 # perform estimation
          obs_oldest = []
        end
        t_tag_oldest = t_tag_shift
        obs_oldest[i_shift] = obs[i_shift].shift
      end
    }
    Thread::pass while (obs[i].size > 1)
  }
}.call

# observation files
threads = obs_files.collect.with_index{|(fname, ftype), i|
  th = Thread::new{
    rcv.send(case ftype
        when :ubx; :parse_ubx
        when :rinex_obs; :parse_rinex_obs
        when :rtcm3; :parse_rtcm3
        end, fname){|pvt, (meas, t_meas)|
      adjust_timing.call(i, t_meas, meas, pvt)
    }
    (threads - [th]).each{|th2| Thread::kill(th2)} # kill others
  }
}
threads.each{|th| th.join}