def shr_compute(df_row): st_contrib = df_row['traj_interp'].st.contribs[:, 1] gh_contrib = df_row['traj_interp'].gh.contribs[:, 1] ht_traj = df_row['ht'] st_traj = df_row['st'] gh_traj = df_row['gh'] up_down = df_row['up_down_analysis'] start_idx = up_down.max_run_up_start_idx end_idx = up_down.max_run_up_end_idx sec = slice(start_idx, end_idx + 1) st_elev = -np.rad2deg(st_contrib[sec] - st_contrib[start_idx]) run_vals_new, run_starts_new, run_lengths_new = find_runs(st_elev > 1) first_gt_1_new = run_starts_new[np.nonzero( np.logical_and(run_lengths_new > 10, run_vals_new))[0][0]] st_elev_diff = np.rad2deg(-(st_traj.euler.st_isb[sec, 1] - st_traj.euler.st_isb[start_idx, 1])) run_vals_old, run_starts_old, run_lengths_old = find_runs(st_elev_diff > 1) first_gt_1_old = run_starts_old[np.nonzero( np.logical_and(run_lengths_old > 10, run_vals_old))[0][0]] start_idx_comp = max(first_gt_1_new, first_gt_1_old) ht_elev_diff = np.rad2deg(-(ht_traj.euler.ht_isb[sec, 1] - ht_traj.euler.ht_isb[start_idx, 1])) gh_elev_diff = np.rad2deg(-(gh_traj.euler.gh_isb[sec, 1] - gh_traj.euler.gh_isb[start_idx, 1])) ht_elev_range = ht_elev_diff[start_idx_comp:] - ht_elev_diff[start_idx_comp] old_shr = gh_elev_diff[start_idx_comp:] / st_elev_diff[start_idx_comp:] new_shr = ((gh_contrib[sec] - gh_contrib[start_idx])[start_idx_comp:] / (st_contrib[sec] - st_contrib[start_idx])[start_idx_comp:]) ht_interp_range = np.arange(0, 100 + 0.25, 0.25) old_shr_interp = np.interp(ht_interp_range, ht_elev_range, old_shr, left=np.nan, right=np.nan) new_shr_interp = np.interp(ht_interp_range, ht_elev_range, new_shr, left=np.nan, right=np.nan) return old_shr, new_shr, old_shr_interp, new_shr_interp
def sig_string(x: np.ndarray, sig: np.ndarray) -> str: """Create a string describing signifiance based on the domain x and boolean vector of signifiance (sig).""" run_values, run_starts, run_lengths = find_runs(sig) run_starts_sign = run_starts[run_values] run_lengths_sign = run_lengths[run_values] sec = [] for run, length in zip(run_starts_sign, run_lengths_sign): sec.append('{:.2f} - {:.2f}'.format(x[run], x[run + length - 1])) return '\n'.join(sec)
def extract_sig(spm_test: _SPM0Dinference, x: np.ndarray) -> str: """Create a string describing where spm_test is significant in the domain of x.""" sig = np.logical_or(spm_test.z < -spm_test.zstar, spm_test.z > spm_test.zstar) run_values, run_starts, run_lengths = find_runs(sig) run_starts_sign = run_starts[run_values] run_lengths_sign = run_lengths[run_values] sec = [] for run, length in zip(run_starts_sign, run_lengths_sign): sec.append('{:.2f} - {:.2f}'.format(x[run], x[run + length - 1])) return '\n'.join(sec)
def analyze_up_down(ht_traj: PoseTrajectory) -> UpDownAnalysis: """Performing an analysis of ht_traj to determine where minimum and maximal HT elevation is achieved.""" ht_elev = np.rad2deg(-ht_traj.euler.ht_isb[:, 1]) num_frames = ht_elev.size max_elev = ht_elev.max() max_elev_idx = np.argmax(ht_elev) # first level of analysis - determine minimum for up and down up = ht_elev[:max_elev_idx+1] down = np.flip(ht_elev[max_elev_idx:]) min_elev_up = up.min() min_elev_down = down.min() min_elev_up_idx = np.argmin(up) min_elev_down_idx = np.argmin(down) # second level of analysis - determine longest run up_diff = np.diff(up) down_diff = np.diff(down) up_run_vals, up_run_starts, up_run_lengths = find_runs(up_diff >= 0) down_run_vals, down_run_starts, down_run_lengths = find_runs(down_diff >= 0) up_run_starts_inc = up_run_starts[up_run_vals] up_run_lengths_inc = up_run_lengths[up_run_vals] down_run_starts_inc = down_run_starts[down_run_vals] down_run_lengths_inc = down_run_lengths[down_run_vals] # note that the down indices are for the flipped trajectory max_run_up_run_idx = np.argmax(up_run_lengths_inc) max_run_down_run_idx = np.argmax(down_run_lengths_inc) max_run_up_start_idx = up_run_starts_inc[max_run_up_run_idx] max_run_up_end_idx = max_run_up_start_idx + up_run_lengths_inc[max_run_up_run_idx] max_run_down_start_idx = down_run_starts_inc[max_run_down_run_idx] max_run_down_end_idx = max_run_down_start_idx + down_run_lengths_inc[max_run_down_run_idx] max_run_up_start_val = up[max_run_up_start_idx] max_run_up_end_val = up[max_run_up_end_idx] max_run_down_start_val = down[max_run_down_start_idx] max_run_down_end_val = down[max_run_down_end_idx] return UpDownAnalysis(max_elev, max_elev_idx, min_elev_up, min_elev_down, min_elev_up_idx, num_frames - 1 - min_elev_down_idx, max_run_up_start_idx, max_run_up_end_idx, num_frames - 1 - max_run_down_start_idx, num_frames - 1 - max_run_down_end_idx, max_run_up_start_val, max_run_up_end_val, max_run_down_start_val, max_run_down_end_val)
def kf_filter_marker_piecewise( marker_pos_labeled: np.ndarray, marker_pos_filled: np.ndarray, dt: float, max_gap: int = 75, max_gap_secondary: Tuple[int, int] = (30, 10), min_length: int = 75, white_noise_var: float = 10000 ) -> Tuple[Sequence[FilterStep], Sequence[FilterStep]]: """Filter raw (labeled) Vicon marker data, accounting for gaps. There are two conditions that create a gap: 1. The marker is not visible for more than or equal to max_gap frames 2. Periods where marker is not visible for >= max_gap_secondary[0] frames are separated by an interval where the marker is visible for at most max_gap_secondary[1] frames Subsequently, all gaps are combined. Raises ------ biplane_kine.smoothing.kalman_filtering.InsufficientDataError """ start_idx, stop_idx = init_point(marker_pos_labeled, marker_pos_filled) nans_labeled = ~np.isnan(marker_pos_labeled[start_idx:stop_idx, 0]) runs = find_runs(nans_labeled) # primary gaps - no data for longer than max_gap primary_runs_gaps_idx_start = np.nonzero( ((~runs[0]) & (runs[2] >= max_gap)))[0] primary_runs_gaps_idx_end = primary_runs_gaps_idx_start + 1 # secondary gaps - gaps of max_gap_secondary[0] separated by spaces where at most max_gap_secondary[1] data exists runs_secondary_gaps_idx = np.nonzero( ((~runs[0]) & (runs[2] >= max_gap_secondary[0])))[0] secondary_runs_gaps_idx_start = [] secondary_runs_gaps_idx_end = [] for i in range(runs_secondary_gaps_idx.size - 1): if np.sum(runs[0][runs_secondary_gaps_idx[i] + 1:runs_secondary_gaps_idx[i + 1]] * runs[2] [runs_secondary_gaps_idx[i] + 1:runs_secondary_gaps_idx[i + 1]]) < max_gap_secondary[1]: secondary_runs_gaps_idx_start.append(runs_secondary_gaps_idx[i]) secondary_runs_gaps_idx_end.append(runs_secondary_gaps_idx[i + 1] + 1) # now let's combine the gaps all_runs_gaps_idx = \ list(itertools.chain.from_iterable([zip(primary_runs_gaps_idx_start, primary_runs_gaps_idx_end), zip(secondary_runs_gaps_idx_start, secondary_runs_gaps_idx_end)])) def gaps_overlap(gap1: Tuple[int, int], gap2: Tuple[int, int]) -> bool: """Do the gaps overlap?""" return (gap1[0] < gap2[1]) and (gap2[0] < gap1[1]) def combine_gaps(gap1: Tuple[int, int], gap2: Tuple[int, int]) -> Tuple[int, int]: """Combined the two gaps.""" min_start = min(gap1, gap2, key=itemgetter(0)) max_end = max(gap1, gap2, key=itemgetter(1)) return min_start[0], max_end[1] # this only works if the list is sorted by the start index! def recursive_combine(initial_gap_list, combined_gap_list): # if there are no more gaps to combine then return the combined_gap_list if not initial_gap_list: return combined_gap_list # if we can combine the current gap (combined_gap_list[-1]) with the next gap in the list to process # (initial_gap_list[0]) if gaps_overlap(combined_gap_list[-1], initial_gap_list[0]): # combine the gaps and update the current gap combined = combine_gaps(combined_gap_list[-1], initial_gap_list[0]) combined_gap_list[-1] = combined else: # can't combine so add the considered gap becomes the current gap combined_gap_list.append(initial_gap_list[0]) # either way we have taken care of this gap so remove it from the list of gaps to be considered del (initial_gap_list[0]) # march forward return recursive_combine(initial_gap_list, combined_gap_list) def recursive_combine_start(initial_gap_list): # no gaps if not initial_gap_list: return [] # the first combination is easy, it's just the first gap by itself initial_gap_list_copy = initial_gap_list.copy() combined_gap_list = [initial_gap_list_copy[0]] del (initial_gap_list_copy[0]) return recursive_combine(initial_gap_list_copy, combined_gap_list) # first sort by the start index all_runs_gaps_idx.sort(key=itemgetter(0)) all_runs_gaps_ids_merged = recursive_combine_start(all_runs_gaps_idx) # break the list of tuples apart into two lists runs_gaps_idx_start_final = [gap[0] for gap in all_runs_gaps_ids_merged] runs_gaps_idx_end_final = [gap[1] for gap in all_runs_gaps_ids_merged] # number of pieces to filter will always be one greater than the number of gaps num_pieces = len(all_runs_gaps_ids_merged) + 1 pieces_end_idx = np.full((num_pieces, ), stop_idx) pieces_start_idx = np.full((num_pieces, ), start_idx) # there may not be any gaps so check first if all_runs_gaps_ids_merged: # interior pieces run from the end index of a gap to the start index of the next gap pieces_end_idx[:-1] = runs[1][runs_gaps_idx_start_final] + start_idx pieces_start_idx[1:] = runs[1][runs_gaps_idx_end_final] + start_idx filtered_pieces = [] smoothed_pieces = [] # filter each piece for i in range(num_pieces): if (pieces_end_idx[i] - pieces_start_idx[i]) < min_length: log.info('Skipping Filtering piece %d running from %d to %d', i, pieces_start_idx[i], pieces_end_idx[i]) continue log.info('Filtering piece %d running from %d to %d ', i, pieces_start_idx[i], pieces_end_idx[i]) piece_filtered, piece_smoothed = kf_filter_marker_piece( marker_pos_labeled, marker_pos_filled, pieces_start_idx[i], pieces_end_idx[i], dt, white_noise_var) filtered_pieces.append(piece_filtered) smoothed_pieces.append(piece_smoothed) if not filtered_pieces: raise InsufficientDataError('No resulting segments to filter.') return filtered_pieces, smoothed_pieces
ht_traj = df_row['ht'] st_traj = df_row['st'] gh_traj = df_row['gh'] up_down = df_row['up_down_analysis'] start_idx = up_down.max_run_up_start_idx end_idx = up_down.max_run_up_end_idx sec = slice(start_idx, end_idx + 1) fig = plt.figure() ax = fig.subplots() ax.xaxis.set_major_locator(plticker.MultipleLocator(base=10.0)) plot_utils.style_axes(ax, 'HT Elevation Angle', 'SHR') st_elev = np.rad2deg(st_contrib[sec] - st_contrib[start_idx]) run_vals_new, run_starts_new, run_lengths_new = find_runs( -st_elev > 1) first_gt_1_new = run_starts_new[np.nonzero( np.logical_and(run_lengths_new > 10, run_vals_new))[0][0]] st_elev_diff = np.rad2deg( -(st_traj.euler.st_isb[sec, 1] - st_traj.euler.st_isb[start_idx, 1])) run_vals_old, run_starts_old, run_lengths_old = find_runs( st_elev_diff > 1) first_gt_1_old = run_starts_old[np.nonzero( np.logical_and(run_lengths_old > 10, run_vals_old))[0][0]] start_idx_comp = max(first_gt_1_new, first_gt_1_old) ht_elev = np.rad2deg(-ht_traj.euler.ht_isb[sec, 1]) ht_elev_diff = np.rad2deg( -(ht_traj.euler.ht_isb[sec, 1] -