Skip to content

Instantly share code, notes, and snippets.

@fenrir-naru
Last active October 25, 2022 02:14
Show Gist options
  • Save fenrir-naru/ef56e9e526f5f0c905ec6a4d3fbea1a6 to your computer and use it in GitHub Desktop.
Save fenrir-naru/ef56e9e526f5f0c905ec6a4d3fbea1a6 to your computer and use it in GitHub Desktop.
Attitude estimation code 2022/10/25
#!/usr/bin/ruby
require 'gps_pvt'
LAMBDA = GPS_PVT::GPS::SpaceNode.L1_WaveLength
Mat = GPS_PVT::SylphideMath::MatrixD
candidates = [-1, 0, 1]
run = proc{
prop = {}
puts [:week, :second, :E, :N, :U, :psi, :phi, '1st', '2nd', '3rd', '4th', '5th'].join(',')
proc{|t, data|
mat_G = Mat::new(data.collect{|v| v[-3..-1]})
y = Mat::new(data.collect{|v|
[v[3] - (prop[v[0..2]] ||= [v[3].floor, prop.size])[0]]
})
mat_inv = (mat_G.t * mat_G).inv
mat_P = (mat_G * mat_inv * mat_G.t * -1) + 1
calc_J = proc{|y2| (y2.t * mat_P * y2)[0, 0]}
sorted = (candidates.product(*([candidates] * (data.size - 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]}
int_list = []
data.each.with_index{|v, i|
offset, i2 = prop[v[0..2]]
int_list[i2] = sorted[0][1][i] + offset
}
x = (mat_inv * mat_G.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, x, psi, phi, sorted[0..4].transpose[0], int_list].flatten.join(',')
}
}.call
open("double_delta.csv"){|io|
head = io.readline
io.each.with_index{|line, i|
v = line.chomp.split(/, */)
t = [v[0].to_i, v[1].to_f]
v = v[2..-1]
data = []
while v.size >= 8
(data << (v[0..2].collect{|v2| Integer(v2)} + v[3..7].collect{|v2| v2.to_f})) rescue nil
v = v[8..-1]
end
run.call(t, data)
#break if i > 2
}
}
#!/usr/bin/ruby
# coding: cp932
require 'gps_pvt'
=begin
~/desktop/資料/辻井さん/江口さんのデータで試してみる
付属プログラムによると、このデータセットのアンテナ間距離は0.09514684; //半波長(λ/2)
=end
nav = 'NMND21060005Z_2021-06-20_08-54-53.nav' #'BRDC00IGS_R_20211710000_01D_MN.rnx.gz'
obs = [
'NMND21060005Z_2021-06-20_08-54-53.obs',
'NMND21060010S_2021-06-20_08-54-55.obs',
]
LAMBDA = GPS_PVT::GPS::SpaceNode.L1_WaveLength
rcv = GPS_PVT::Receiver::new
rcv.parse_rinex_nav(nav)
current = [nil] * obs.size # [[t_meas, meas, pvt]_rcv0, []_rcv1, ...]
previous = [] # For ambiguity resolution
dump_diff2 = proc{|io| # double difference printer
keys = []
header = nil
io ||= $stdout
proc{|t, meas_diff2_list, mat_G|
values = []
meas_diff2_list.each.with_index{|meas_diff2, i|
meas_diff2.each{|prns, meas|
k = [i, 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, :rcv_set, :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] * 8)}].flatten.join(',')
}
}.call(open('double_delta.csv', 'w'))
estimate_attitude = proc{
header = nil
proc{
meas_list = current.collect{|t_meas, meas, pvt|
meas.to_hash2
}
t, pvt = [0, 2].collect{|i| current[0][i]}
satellites = pvt.used_satellite_list
# [[elv_deg, prn], ...], elevation decending 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]}
# Receiver difference (single difference)
# meas_diff_list = [{prn => {PR => v, CR => 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, satellite difference (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))]
}
#p meas_diff2_list
dump_diff2.call(t, meas_diff2_list, mat_G)
# Solution with PR
pr_solution = meas_diff2_list.collect{|meas_diff2|
mat, y = meas_diff2.collect{|(prn1, prn0), meas|
next nil unless (v = meas[:L1_PSEUDORANGE])
[mat_G.delta(prn1, prn0).to_a[0][0..2], [v]]
}.compact.transpose.collect{|array| GPS_PVT::SylphideMath::MatrixD::new(array)}
# Least square
((mat.t * mat).inv * mat.t * y).to_a.flatten
}
# Solution with CP using some epochs
cp_solution = []
previous << [t, mat_G, meas_diff2_list]
if previous.size > 90 then
previous2 = (-1..-(previous.size)).step(-15).to_a.reverse.collect{|i| previous[i]}
meas_diff2_list.collect.with_index{|meas_diff2, i|
# find shared combination [sat1, sat0] having CP information
prns_list = previous2.collect{|t_, mat_, meas_|
meas_[i].keys.select{|k| meas_[i][k][:L1_CARRIER_PHASE]}
}.inject{|memo, v| memo & v}
#prns_list = prns_list & [[5, 2], [6, 2], [7, 2], [9, 2], [13, 2], [19, 2], [20, 2], [30, 2]]
mat = []
(previous2.size * prns_list.size).times{
mat << [0] * (3 * previous2.size + prns_list.size)
}
y = [previous2.collect.with_index{|(t_, mat_, meas_), i2|
prns_list.collect.with_index{|prns, i3|
mat_.delta(*prns).to_a[0][0..2].each.with_index{|v, i4|
mat[(i2 * prns_list.size) + i3][i2 * 3 + i4] = v / LAMBDA
}
mat[(i2 * prns_list.size) + i3][(3 * previous2.size) + i3] = -1
meas_[i][prns][:L1_CARRIER_PHASE]
}
}.flatten].transpose
mat, y = [mat, y].collect{|array| GPS_PVT::SylphideMath::MatrixD::new(array)}
#puts (y.to_a.flatten + mat.to_a.flatten).join(',')
# Least square
x = ((mat.t * mat).inv * mat.t * y).to_a.flatten
cp_solution << (x[0..2] + x[(-prns_list.size)..(-1)])
}
previous.shift
end
puts (header = [:week, :second, :E, :N, :U]).join(',') unless header
puts [t.to_a, pr_solution, cp_solution].flatten.join(',')
}
}.call
adjust_timing = proc{
next false unless current.all?
t_i = current.collect.with_index{|(t_meas, *rest), i|
[t_meas, i]
}.sort{|a, b| b[0] <=> a[0]} # time decending order
t_i[1..-1].each{|t, i|
if (t_i[0][0] - t) >= 1E-1 then # threshold is 100 ms
current[i] = nil
end
}
next false unless current.all?
$stderr.puts "Calculate attitude @ %dw%.1f"%[*(t_i[0][0].to_a)]
estimate_attitude.call # attitude estimation
current = [nil] * obs.size # re-initialization
true
}
threads = obs.collect.with_index{|fname, i|
th = Thread::new{
rcv.parse_rinex_obs(fname){|pvt, (meas, t_meas)|
current[i] = [t_meas, meas, pvt]
adjust_timing.call
Thread::pass while current[i]
}
(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