RubyでGPS姿勢推定

Aero & Astro

RubyでのGPS解析環境を充実させるへくgps_pvt gemを作っていますが、そのgemを応用して近接する複数アンテナを使った姿勢推定のプログラムを作ってみました。

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.
view raw !README.md hosted with ❤ by GitHub
#!/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}
view raw est_proximity.rb hosted with ❤ by GitHub

Integer Ambiguityを全組み合わせで計算してしまうので、衛星がたくさん見えていると明らかに処理速度が落ちます。その部分を改善していきたいと思います。

March 14, 2023 13:55 fenrir が投稿 : 固定リンク |
| このエントリーを含むはてなブックマーク

コメント

コメントする

NoYes
クリア
投稿