def compare_obc_and_asol(atts, times, recs, ptol=2, ytol=2, rtol=50):
"""
Check that obc solution and ground solution have relatively close quats.
Note that because the ground aspect solution is not continuous (only run over science obsids
in aspect intervals) that this method only checks those intervals.
:param atts: attitudes from get_atts
:param times: times from get_atts
:param recs: list of dicts of header records from get_atts
:param ptol: dq quaternion pitch tolerance in arcsecs
:param ytol: dq quaternion yaw tolerance in arcsecs
:param rtol: dq quaternion roll tolerance in arcsecs
"""
for ai in recs:
telem = fetch.Msidset(['aoattqt*'], ai['TSTART'], ai['TSTOP'])
obc_atts = np.vstack([telem['aoattqt{}'.format(idx)].vals
for idx in [1, 2, 3, 4]]).transpose()
obc_times = telem['aoattqt1'].times
# Test that, at the obc times, the onboard solution and the ground solution
# are reasonably close
idxs = np.searchsorted(times[:-1], obc_times)
dps = []
dys = []
drs = []
for att, obc_att in zip(atts[idxs], obc_atts):
dq = Quat(normalize(att)).dq(Quat(normalize(obc_att)))
dps.append(dq.pitch * 3600)
dys.append(dq.yaw * 3600)
drs.append(dq.roll0 * 3600)
dps = np.array(dps)
dys = np.array(dys)
drs = np.array(drs)
assert np.all(np.abs(dys) < ytol)
assert np.all(np.abs(dps) < ptol)
assert np.all(np.abs(drs) < rtol)
def computeIMUMultiple(imuData):
utmLoc = np.zeros((len(imuData['x134_Position']), 2))
for i in range(len(imuData['x134_Position'])):
u = utm.from_latlon(imuData['x134_Position'][i, 0],
imuData['x134_Position'][i, 1])
utmLoc[i, 0] = u[0]
utmLoc[i, 1] = u[1]
qNorm = normalize(imuData['x131_Quaternion'][0, :])
eulerAngles = np.zeros((len(imuData['x131_Quaternion']), 3))
for i in range(len(imuData['x131_Quaternion'])):
qNorm = normalize(imuData['x131_Quaternion'][i, :])
Q = Quat(qNorm)
eulerAngles[i, :] = [Q.ra, Q.dec, Q.roll]
relPath = utmLoc - utmLoc[0, :]
rotAngle = (180 - eulerAngles[0, 2]) * np.pi / 180.
R2 = np.array([[cos(rotAngle), -1 * sin(rotAngle)],
[sin(rotAngle), cos(rotAngle)]])
pathR = np.dot(R2, relPath.T)
# else:
# utmLoc = np.full((5,2),np.NaN)
# eulerAngles = np.full((5,3),np.NaN)
# pathR = np.full((2,5),np.NaN)
# velocity = np.full((5,3),np.NaN)
# time = np.full((5,8),np.NaN)
return utmLoc, eulerAngles, pathR
def compare_obc_and_asol(atts, times, recs, ptol=2, ytol=2, rtol=65):
"""
Check that obc solution and ground solution have relatively close quats.
Note that because the ground aspect solution is not continuous (only run over science obsids
in aspect intervals) that this method only checks those intervals.
:param atts: attitudes from get_atts
:param times: times from get_atts
:param recs: list of dicts of header records from get_atts
:param ptol: dq quaternion pitch tolerance in arcsecs
:param ytol: dq quaternion yaw tolerance in arcsecs
:param rtol: dq quaternion roll tolerance in arcsecs
"""
for ai in recs:
telem = fetch.Msidset(['aoattqt*'], ai['TSTART'], ai['TSTOP'])
obc_atts = np.vstack([telem['aoattqt{}'.format(idx)].vals
for idx in [1, 2, 3, 4]]).transpose()
obc_times = telem['aoattqt1'].times
# Test that, at the obc times, the onboard solution and the ground solution
# are reasonably close
idxs = np.searchsorted(times[:-1], obc_times)
dps = []
dys = []
drs = []
for att, obc_att in zip(atts[idxs], obc_atts):
dq = Quat(normalize(att)).dq(Quat(normalize(obc_att)))
dps.append(dq.pitch * 3600)
dys.append(dq.yaw * 3600)
drs.append(dq.roll0 * 3600)
dps = np.array(dps)
dys = np.array(dys)
drs = np.array(drs)
assert np.all(np.abs(dys) < ytol)
assert np.all(np.abs(dps) < ptol)
assert np.all(np.abs(drs) < rtol)
def ground_truth(model_ply, scene_ply, gt_pos):
pos_i = gt_pos[os.path.basename(model_ply)]
pos_j = gt_pos[os.path.basename(scene_ply)]
q_i = Quat(normalize(numpy.array(pos_i[3::])))
q_j = Quat(normalize(numpy.array(pos_j[3::])))
q_ji = q_i / q_j
q_ij = q_j / q_i
return q_ij, q_ji
def lookup_ground_truth(i, j):
model_ply = PLY_FILES[i]
scene_ply = PLY_FILES[j]
pos_i = GT_POS[os.path.basename(model_ply)]
pos_j = GT_POS[os.path.basename(scene_ply)]
q_i = Quat(normalize(np.array(pos_i[3::])))
q_j = Quat(normalize(np.array(pos_j[3::])))
q_ji = q_i / q_j
q_ij = q_j / q_i
return q_ij, q_ji
def find_orient(theta,A,q1):
qw= cos(radians(theta)/2.0)
qi=A[0]*sin(radians(theta)/2.0)
qj=A[1]*sin(radians(theta)/2.0)
qk=A[2]*sin(radians(theta)/2.0)
q=normalize([qw,qi,qj,qk])
qInv=normalize(QuatInv(q))
q2=QuatMult(q1,qInv)
return q2
def compare_rotation(ply_files, reg_result, gt_pos):
res = {}
for i, j in reg_result:
model_ply = ply_files[i]
scene_ply = ply_files[j]
q_ij, q_ji = ground_truth(model_ply, scene_ply, gt_pos)
param = reg_result[(i,j)][0]
q = Quat(normalize(param[0:4]))
res[(i, j)] = abs(numpy.dot(q.q, q_ji.q))
return res
def get_cmd_quat(date):
date = DateTime(date)
cmd_quats = fetch.MSIDset(['AOCMDQT{}'.format(i) for i in [1, 2, 3]],
date.secs, date.secs + 120)
cmd_q4 = np.sqrt(np.abs(1 - cmd_quats['AOCMDQT1'].vals[0]**2
- cmd_quats['AOCMDQT2'].vals[0]**2
- cmd_quats['AOCMDQT3'].vals[0]**2))
return Quat(normalize([cmd_quats['AOCMDQT1'].vals[0],
cmd_quats['AOCMDQT2'].vals[0],
cmd_quats['AOCMDQT3'].vals[0],
cmd_q4]))
def stars_test(dwells, dur_thres=7000):
"""
Check that the appropriate stars are identified in agasc.
For a given obsid, star id's and mags can be compared with those in starcheck.
Example:
=======
>> from monwin import stars_test, get_dwells
>> from parce_cm import read_dot_as_list
>> dot = read_dot_as_list("/data/mpcrit1/mplogs/2017/JAN0917/scheduled_b/md007_2305.dot")
>> dwells = get_dwells(dot)
>> stars_test(dwells)
Obsid = 0, duration = 0 sec, skipped
Obsid = 50411, duration = 3258 sec, skipped
Obsid = 50410, duration = 3270 sec, skipped
Obsid = 50409, duration = 3248 sec, skipped
Obsid = 50408, duration = 1242 sec, skipped
Obsid = 50407, duration = 3243 sec, skipped
Obsid = 50406, duration = 11594 sec
agasc_id color mag
--------- -------- -------
389949248 0.95285 7.53174
389953880 0.12495 7.79542
389954848 0.113901 8.71616
389954920 0.54995 9.34569
390865160 1.0302 8.71366
390867952 1.5 8.68183
390339464 0.48875 9.34993
391254944 0.2176 7.91407
Obsid = 18048, duration = 71020 sec, skipped
...
...
...
"""
for dwell in dwells:
obsid = dwell['obsid']
duration = dwell['duration']
if obsid < 60000 and obsid > 50000 and duration > dur_thres:
strcat = dwell['strcat']
q1 = dwell['q1']
q2 = dwell['q2']
q3 = dwell['q3']
q4 = np.sqrt(1. - q1**2 - q2**2 - q3**2)
quat = Quat(normalize([q1, q2, q3, q4]))
stars = identify_stars(obsid, strcat, quat)
print('Obsid = {}, duration = {:.0f} sec'.format(obsid, duration))
print(Table(stars))
else:
print('Obsid = {}, duration = {:.0f} sec, skipped'.format(
obsid, duration))
return
def computeIMU(imuData):
utmLoc = np.zeros(2)
u = utm.from_latlon(imuData['x134_Position'][0],
imuData['x134_Position'][1])
utmLoc[0] = u[0]
utmLoc[1] = u[1]
eulerAngles = np.zeros(3)
qNorm = normalize(imuData['x131_Quaternion'][:])
Q = Quat(qNorm)
eulerAngles[:] = [Q.ra, Q.dec, Q.roll]
return utmLoc, eulerAngles
def compute_accuracy(reg_result):
error = {} # error in degrees.
total_run_time = {}
core_run_time = {}
for i, j in reg_result:
q_ij, q_ji = lookup_ground_truth(i, j)
param = reg_result[(i, j)][0]
q = Quat(normalize(param[0:4]))
similarity = np.abs(np.dot(q.q, q_ji.q))
error[(i, j)] = 2 * np.arccos(similarity) * 180 / np.pi
total_run_time[(i, j)] = reg_result[(i, j)][-1]
core_run_time[(i, j)] = reg_result[(i, j)][-2]
return error, total_run_time, core_run_time
def normalise_quaternions(quat_dict):
norm_quat_dict = OrderedDict()
for k, v in quat_dict.items():
if v.shape[1] == 4:
norm_quats = []
for r in v:
q = np.array([r[1], r[2], r[3], r[0]]) # [x, y, z, w]
nq = Quat(normalize(q))
nq_v = nq._get_q()
norm_quats.append([nq_v[3], nq_v[0], nq_v[1],
nq_v[2]]) # [w, x, y, z]
norm_quat_dict[k] = np.array(norm_quats)
else:
norm_quat_dict[k] = v
return norm_quat_dict
def get_trak_cat_from_telem(start, stop, cmd_quat):
start = DateTime(start)
stop = DateTime(stop)
msids = ["{}{}".format(m, i) for m in ['AOACYAN', 'AOACZAN', 'AOACFID', 'AOIMAGE', 'AOACFCT']
for i in range(0, 8)]
telem = fetch.MSIDset(['AOACASEQ', 'CORADMEN', 'AOPCADMD', 'AONSTARS', 'AOKALSTR']
+ msids, start, stop)
att = fetch.MSIDset(['AOATTQT{}'.format(i) for i in [1, 2, 3, 4]], start, stop)
cat = {}
for slot in range(0, 8):
track = telem['AOACFCT{}'.format(slot)].vals == 'TRAK'
fid = telem['AOACFID{}'.format(slot)].vals == 'FID '
star = telem['AOIMAGE{}'.format(slot)].vals == 'STAR'
n = 30
if np.count_nonzero(track) < n:
continue
if np.any(fid & track):
cat[slot] = {'type': 'FID',
'yag': telem['AOACYAN{}'.format(slot)].vals[fid & track][0],
'zag': telem['AOACZAN{}'.format(slot)].vals[fid & track][0]}
else:
n_samples = np.count_nonzero(track & star)
if n_samples < (n + 4):
continue
# If there is tracked data with a star, let's try to get our n samples from about
# the middle of the range
mid_point = int(n_samples / 2.)
yags = []
zags = []
for sample in range(mid_point - int(n / 2.), mid_point + int(n / 2.)):
qref = Quat(normalize([att['AOATTQT{}'.format(i)].vals[track & star][sample]
for i in [1, 2, 3, 4]]))
ra, dec = yagzag2radec(
telem['AOACYAN{}'.format(slot)].vals[track & star][sample] / 3600.,
telem['AOACZAN{}'.format(slot)].vals[track & star][sample] / 3600.,
qref)
yag, zag = radec2yagzag(ra, dec, cmd_quat)
yags.append(yag)
zags.append(zag)
# This doesn't detect MON just yet
cat[slot] = {'type': 'STAR',
'yag': np.median(yags) * 3600.,
'zag': np.median(zags) * 3600.}
return cat, telem
def plot_centroid_resids_by_flag(start, stop, slot, plot=False):
"""
Plot centroid residuals for a start/stop interval with color
coding to indicate readouts corresponding to a particular
combination of DP, IR, MS, and SP. The specified interval
must be a stable Kalman dwell at one attitude.
:param start: start time (any Chandra DateTime format)
:param stop: stop time
:param slot: ACA image slot
:param save: save images as png files
"""
pcad_msids = ['aoacfct',
'aoacisp',
'aoacidp',
'aoaciir',
'aoacims',
'aoacyan',
'aoaczan']
slot_msids = [msid + "%s" % slot for msid in pcad_msids]
msids = ['aoattqt1',
'aoattqt2',
'aoattqt3',
'aoattqt4',
]
msids.extend(slot_msids)
print('Fetching telemetry from {} to {}'.format(start, stop))
telems = fetch.MSIDset(msids, start, stop)
telems.interpolate(dt=2.05, filter_bad=False)
bads = np.zeros(len(telems.times), dtype=bool)
for msid in telems:
bads |= telems[msid].bads
bads |= telems['aoacfct{}'.format(slot)].vals != 'TRAK'
telems.bads = bads
for msid in telems:
telems[msid].bads = bads
telems.filter_bad()
vals = ([telems['aoattqt%d' % i].vals for i in range(1, 5)]
+ [telems['aoacyan{}'.format(slot)].vals / 3600.,
telems['aoaczan{}'.format(slot)].vals / 3600.])
print('Interpolating quaternions')
radecs = [yagzag2radec(yag, zag, Quat(normalize([q1, q2, q3, q4])))
for q1, q2, q3, q4, yag, zag in zip(*vals)]
coords = np.rec.fromrecords(radecs, names=('ra', 'dec'))
# ok = telems['aoacfct{}'.format(slot)].vals == 'TRAK'
flags = {'dp': telems['aoacidp%s' % slot].vals != 'OK ',
'ir': telems['aoaciir%s' % slot].vals != 'OK ',
'ms': telems['aoacims%s' % slot].vals != 'OK ',
'sp': telems['aoacisp%s' % slot].vals != 'OK ',
}
times = telems['aoacyan%s' % slot].times
dra = (coords['ra'] - np.mean(coords['ra'])) * 3600 * np.cos(np.radians(coords['dec']))
ddec = (coords['dec'] - np.mean(coords['dec'])) * 3600
dr = np.sqrt(dra ** 2 + ddec ** 2)
# fileroot = 'flags_{}'.format(DateTime(start).date[:14]) if save else None
# print('Making plots with output fileroot={}'.format(fileroot))
filt = ((flags['dp'] == False) & (flags['ir'] == False)
& (flags['ms'] == False) & (flags['sp'] == False))
if plot:
plot_axis('dR', times, dr, filt, title='No status flags')
if np.sum(filt) > 20:
clean_perc = np.percentile(dr[filt], [50, 84, 90, 95])
if clean_perc[2] > 1.0:
print '{} {} : {}'.format(start, stop, str(clean_perc))
else:
clean_perc = [-1, -1, -1, -1]
clean_perc.append(np.sum(filt))
filt = flags['dp'] == True
if plot:
plot_axis('dR', times, dr, filt, title='DP == True')
if np.sum(filt) > 20:
dp_perc = np.percentile(dr[filt], [50, 84, 90, 95])
else:
dp_perc = [-1, -1, -1, -1]
dp_perc.append(np.sum(filt))
return clean_perc, dp_perc
def monitor_window_stats(dwells, dur_thres=7000, t_ccd=-11, prob_thres=0.1):
"""
Get number of allowed monitor windows for each ER dwell with
duration longer than dur_thres in sec.
:dwells: list of dictionaries with dwells parsed from the DOT
:dur_thres: duration threshold in sec for an ER dwell
:t_ccd: temperature of the CCD in C
:prob_thres: acquisition probability threshold to detect exactly n stars
(n=6 for 1 monitor window, n=7 for 2 monitor windows)
:returns: list of dictionaries with dwell obsid and duration info
and star acquisition and monitor window stats.
This could be simplified so that only the numer of allowed
monitor windows is returned. Current output is for testing
purposes.
"""
stats = []
for dwell in dwells:
obsid = dwell['obsid']
if obsid < 60000 and obsid > 50000 and dwell['duration'] > dur_thres:
q1 = dwell['q1']
q2 = dwell['q2']
q3 = dwell['q3']
q4 = np.sqrt(1. - q1**2 - q2**2 - q3**2)
quat = Quat(normalize([q1, q2, q3, q4]))
strcat = dwell['strcat']
date = strcat['TIME']
duration = dwell['duration']
stars = identify_stars(obsid, strcat, quat)
t = Table(stars)
t['probs'] = star_probs.acq_success_prob(date, t_ccd, t['mag'],
t['color'])
t.sort('probs')
dwell_stats = {
'obsid': obsid,
'duration': duration,
'date': date,
'quat': quat,
'mon_windows_1': False,
'mon_windows_2': False,
'num_mon_windows': 0,
'stars_dropped_1': [],
'stars_dropped_2': []
}
for i in [6, 7]:
p, c = star_probs.prob_n_acq(t['probs'][:i])
if p[-1] > prob_thres:
num_mon_windows = 8 - i
dwell_stats['mon_windows_{}'.format(
num_mon_windows)] = True
if num_mon_windows > dwell_stats['num_mon_windows']:
dwell_stats['num_mon_windows'] = num_mon_windows
if num_mon_windows > 0:
# improve this: star probs could be equal to each other,
# and then it doesnt matter which star is dropped
stars_dropped = [
id for id in t['agasc_id'][-num_mon_windows:]
]
dwell_stats['stars_dropped_{}'.format(
num_mon_windows)] = stars_dropped
dwell_stats['prob_{}'.format(i)] = format(p[-1], '.3f')
stats.append(dwell_stats)
return stats
