Skip to content

Instantly share code, notes, and snippets.

@fenrir-naru
Last active March 15, 2023 00:46
Show Gist options
  • Save fenrir-naru/6bf38327d895e433fb6269b787e4c108 to your computer and use it in GitHub Desktop.
Save fenrir-naru/6bf38327d895e433fb6269b787e4c108 to your computer and use it in GitHub Desktop.

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.
#!/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}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment