def update_function(self, datum, parameters):
"""
An state update function to be called by the SITL analyzer.
Parameters
----------
datum : DataFrame
A DataFrame of data necessary to create a set of sdiff_t.
"""
mgmt.dgnss_update(datum.apply(ut.mk_swiftnav_sdiff, axis=0).dropna(),
parameters.rover_ecef)
def update_function(self, datum, parameters):
"""
An state update function to be called by the SITL analyzer.
Parameters
----------
datum : DataFrame
A DataFrame of data necessary to create a set of sdiff_t.
"""
mgmt.dgnss_update(
datum.apply(ut.mk_swiftnav_sdiff, axis=0).dropna(),
parameters.rover_ecef)
def analyze_datum(datum, i, time, ag):
f2 = utils.get_non_nans(datum)
# TODO use a libswiftnav-python version
t = gpstime.datetime2gpst(time)
sats = list(f2.index)
# measurements = np.concatenate(([f2.ix[sats,'L1']], [f2.ix[sats,'C1']]), axis=0).T
numeric_sats = map(lambda x: int(x[1:]), list(sats))
# alms = [ag.alm[j] for j in numeric_sats]
ref_ecef = ag.ecef.copy()
mgmt.dgnss_update(f2, ref_ecef)
# mgmt.dgnss_update(alms, t_,
# measurements,
# ag.ecef + np.array([0, 0, 1e-1]), 1)
# get ILSQ ambiguity from 'known' baseline
if len(sats) <= 1:
return pd.Series([], index=[])
float_de, float_phase = mgmt.get_float_de_and_phase(f2, ag.ecef + 0.5 * ag.b)
float_N_i_from_b = utils.get_N_from_b(float_phase, float_de, ag.b)
# TODO save it in the Series or DataFrame output, along with its
# sats, so that we may analyze its variation and use dynamic sat
# sets
# TODO the two baseline computations loop to find DE, which could
# be fused
float_b = mgmt.measure_float_b(f2, ag.ecef)
faux_resolved_b \
= mgmt.measure_b_with_external_ambs(f2, float_N_i_from_b, ag.ecef)
# NOTE: maybe use ag.ecef + 0.5*b or something
float_b_NED = cs.wgsecef2ned(float_b, ag.ecef)
faux_resolved_b_NED = cs.wgsecef2ned(faux_resolved_b, ag.ecef)
# check convergence/resolution
num_hyps = mgmt.dgnss_iar_num_hyps()
num_hyp_sats = mgmt.dgnss_iar_num_sats()
float_converged = num_hyp_sats > 0
resolution_done = float_converged and num_hyps == 1
float_ambs = mgmt.get_amb_kf_mean()
float_amb_cov = mgmt.get_amb_kf_cov(len(float_ambs))
float_prns = mgmt.get_amb_kf_prns()
float_ref_prn = float_prns[0]
float_prns = float_prns[1:]
cov_labels_cols = map(lambda x: 'float_amb_cov_' + str(x), float_prns)
cov_labels \
= map(lambda col: map(lambda row: col + '_' + str(row), float_prns),
cov_labels_cols)
if not ag.resolution_ended:
ag.kf_weighted_log_likelihood = \
i / (i + 1.0) * ag.kf_weighted_log_likelihood \
+ utils.neg_log_likelihood(float_ambs - float_N_i_from_b,
float_amb_cov) / (i + 1.0)
output_data = np.concatenate((float_b_NED,
faux_resolved_b_NED,
[len(numeric_sats), num_hyps, num_hyp_sats],
float_ambs,
reduce(lambda x, y: np.concatenate((x, y)),
float_amb_cov),
float_N_i_from_b,
[float_ref_prn]))
output_labels = ['float_b_N', 'float_b_E', 'float_b_D',
'faux_resolved_b_N', 'faux_resolved_b_E', 'faux_resolved_b_D',
'num_sats', 'num_hyps', 'num_hyp_sats'] \
+ map(lambda x: 'float_amb_' + str(x), float_prns) \
+ reduce(operator.add, cov_labels) \
+ map(lambda x: 'float_amb_from_b_' + str(x), float_prns) \
+ ['float_ref_sat']
if float_converged and (num_hyps > 0):
iar_de, iar_phase = mgmt.get_iar_de_and_phase(f2, ag.ecef + 0.5 * ag.b)
iar_N_i_from_b = utils.get_N_from_b(iar_phase, iar_de, ag.b)
iar_prns = mgmt.get_amb_test_prns()
iar_ref_prn = iar_prns[0]
iar_prns = iar_prns[1:]
iar_faux_resolved_b = mgmt.measure_iar_b_with_external_ambs(f2, iar_N_i_from_b, ag.ecef)
iar_faux_resolved_b_NED = cs.wgsecef2ned(iar_faux_resolved_b, ag.ecef)
iar_MLE_ambs = mgmt.dgnss_iar_MLE_ambs()
iar_MLE_b = mgmt.measure_iar_b_with_external_ambs(f2, iar_MLE_ambs, ag.ecef)
iar_MLE_b_NED = cs.wgsecef2ned(iar_MLE_b, ag.ecef)
output_data = np.concatenate((output_data,
iar_MLE_b_NED,
iar_faux_resolved_b_NED,
iar_MLE_ambs,
iar_N_i_from_b,
[iar_ref_prn]))
output_labels += ['iar_MLE_b_N', 'iar_MLE_b_E', 'iar_MLE_b_D'] \
+ ['iar_faux_resolved_b_N',
'iar_faux_resolved_b_E',
'iar_faux_resolved_b_D'] \
+ map(lambda x: 'iar_MLE_amb_' + str(x), iar_prns) \
+ map(lambda x: 'iar_amb_from_b_' + str(x), iar_prns) \
+ ['iar_ref_sat']
# if the resolution just started
if (not ag.resolution_started):
ag.resolution_started = True
ag.resolution_contains_ilsq_N \
= (mgmt.dgnss_iar_pool_contains(iar_N_i_from_b) == 1)
ag.float_convergence_time_delta = time - ag.t_0
ag.float_convergence_i = i
# if the integer ambiguity resolution just finished
if ag.resolution_started and (not ag.resolution_ended) and resolution_done:
n = mgmt.dgnss_iar_get_single_hyp(len(iar_N_i_from_b))
ag.resolution_ended = True
ag.resolution_time_delta = time - ag.t_0
ag.resolution_i = i
ag.N = n
ag.resolution_matches_ilsq_N = True
for j in xrange(len(ag.N)):
if int(round(ag.N[j])) != int(round(iar_N_i_from_b[j])):
ag.resolution_matches_ilsq_N = False
break
pass
return pd.Series(output_data, index=output_labels)
def analyze_datum(datum, i, time, ag):
f2 = utils.get_non_nans(datum)
# TODO use a libswiftnav-python version
t = gpstime.datetime2gpst(time)
sats = list(f2.index)
# measurements = np.concatenate(([f2.ix[sats,'L1']], [f2.ix[sats,'C1']]), axis=0).T
numeric_sats = map(lambda x: int(x[1:]), list(sats))
# alms = [ag.alm[j] for j in numeric_sats]
ref_ecef = ag.ecef.copy()
mgmt.dgnss_update(f2, ref_ecef)
# mgmt.dgnss_update(alms, t_,
# measurements,
# ag.ecef + np.array([0, 0, 1e-1]), 1)
# get ILSQ ambiguity from 'known' baseline
if len(sats) <= 1:
return pd.Series([], index=[])
float_de, float_phase = mgmt.get_float_de_and_phase(
f2, ag.ecef + 0.5 * ag.b)
float_N_i_from_b = utils.get_N_from_b(float_phase, float_de, ag.b)
# TODO save it in the Series or DataFrame output, along with its
# sats, so that we may analyze its variation and use dynamic sat
# sets
# TODO the two baseline computations loop to find DE, which could
# be fused
float_b = mgmt.measure_float_b(f2, ag.ecef)
faux_resolved_b \
= mgmt.measure_b_with_external_ambs(f2, float_N_i_from_b, ag.ecef)
# NOTE: maybe use ag.ecef + 0.5*b or something
float_b_NED = cs.wgsecef2ned(float_b, ag.ecef)
faux_resolved_b_NED = cs.wgsecef2ned(faux_resolved_b, ag.ecef)
# check convergence/resolution
num_hyps = mgmt.dgnss_iar_num_hyps()
num_hyp_sats = mgmt.dgnss_iar_num_sats()
float_converged = num_hyp_sats > 0
resolution_done = float_converged and num_hyps == 1
float_ambs = mgmt.get_amb_kf_mean()
float_amb_cov = mgmt.get_amb_kf_cov(len(float_ambs))
float_prns = mgmt.get_amb_kf_prns()
float_ref_prn = float_prns[0]
float_prns = float_prns[1:]
cov_labels_cols = map(lambda x: 'float_amb_cov_' + str(x), float_prns)
cov_labels \
= map(lambda col: map(lambda row: col + '_' + str(row), float_prns),
cov_labels_cols)
if not ag.resolution_ended:
ag.kf_weighted_log_likelihood = \
i / (i + 1.0) * ag.kf_weighted_log_likelihood \
+ utils.neg_log_likelihood(float_ambs - float_N_i_from_b,
float_amb_cov) / (i + 1.0)
output_data = np.concatenate(
(float_b_NED, faux_resolved_b_NED,
[len(numeric_sats), num_hyps, num_hyp_sats], float_ambs,
reduce(lambda x, y: np.concatenate((x, y)),
float_amb_cov), float_N_i_from_b, [float_ref_prn]))
output_labels = ['float_b_N', 'float_b_E', 'float_b_D',
'faux_resolved_b_N', 'faux_resolved_b_E', 'faux_resolved_b_D',
'num_sats', 'num_hyps', 'num_hyp_sats'] \
+ map(lambda x: 'float_amb_' + str(x), float_prns) \
+ reduce(operator.add, cov_labels) \
+ map(lambda x: 'float_amb_from_b_' + str(x), float_prns) \
+ ['float_ref_sat']
if float_converged and (num_hyps > 0):
iar_de, iar_phase = mgmt.get_iar_de_and_phase(f2, ag.ecef + 0.5 * ag.b)
iar_N_i_from_b = utils.get_N_from_b(iar_phase, iar_de, ag.b)
iar_prns = mgmt.get_amb_test_prns()
iar_ref_prn = iar_prns[0]
iar_prns = iar_prns[1:]
iar_faux_resolved_b = mgmt.measure_iar_b_with_external_ambs(
f2, iar_N_i_from_b, ag.ecef)
iar_faux_resolved_b_NED = cs.wgsecef2ned(iar_faux_resolved_b, ag.ecef)
iar_MLE_ambs = mgmt.dgnss_iar_MLE_ambs()
iar_MLE_b = mgmt.measure_iar_b_with_external_ambs(
f2, iar_MLE_ambs, ag.ecef)
iar_MLE_b_NED = cs.wgsecef2ned(iar_MLE_b, ag.ecef)
output_data = np.concatenate(
(output_data, iar_MLE_b_NED, iar_faux_resolved_b_NED, iar_MLE_ambs,
iar_N_i_from_b, [iar_ref_prn]))
output_labels += ['iar_MLE_b_N', 'iar_MLE_b_E', 'iar_MLE_b_D'] \
+ ['iar_faux_resolved_b_N',
'iar_faux_resolved_b_E',
'iar_faux_resolved_b_D'] \
+ map(lambda x: 'iar_MLE_amb_' + str(x), iar_prns) \
+ map(lambda x: 'iar_amb_from_b_' + str(x), iar_prns) \
+ ['iar_ref_sat']
# if the resolution just started
if (not ag.resolution_started):
ag.resolution_started = True
ag.resolution_contains_ilsq_N \
= (mgmt.dgnss_iar_pool_contains(iar_N_i_from_b) == 1)
ag.float_convergence_time_delta = time - ag.t_0
ag.float_convergence_i = i
# if the integer ambiguity resolution just finished
if ag.resolution_started and (
not ag.resolution_ended) and resolution_done:
n = mgmt.dgnss_iar_get_single_hyp(len(iar_N_i_from_b))
ag.resolution_ended = True
ag.resolution_time_delta = time - ag.t_0
ag.resolution_i = i
ag.N = n
ag.resolution_matches_ilsq_N = True
for j in xrange(len(ag.N)):
if int(round(ag.N[j])) != int(round(iar_N_i_from_b[j])):
ag.resolution_matches_ilsq_N = False
break
pass
return pd.Series(output_data, index=output_labels)