Attitude estimation code 2022/10/25

ambiguity_test.rb

#!/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
  }
}

test.rb

#!/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
}