def __init__(self, num_ownship_states, x0, P0, buffer_capacity, meas_space_table, delta_codebook_table, delta_multipliers, asset2id, my_name, default_meas_variance):
"""Constructor
Arguments:
num_ownship_states {int} -- Number of ownship states for each asset
x0 {np.ndarray} -- initial states
P0 {np.ndarray} -- initial uncertainty
buffer_capacity {int} -- capacity of measurement buffer
meas_space_table {dict} -- Hash that stores how much buffer space a measurement takes up. Str (meas type) -> int (buffer space)
delta_codebook_table {dict} -- Hash that stores delta trigger for each measurement type. Str(meas type) -> float (delta trigger)
delta_multipliers {list} -- List of delta trigger multipliers
asset2id {dict} -- Hash to get the id number of an asset from the string name
my_name {str} -- Name to loopkup in asset2id the current asset's ID#
default_meas_variance {dict} -- Hash to get measurement variance
"""
self.meas_ledger = []
self.asset2id = asset2id
self.my_name = my_name
self.default_meas_variance = default_meas_variance
self.filter = ETFilter(asset2id[my_name], num_ownship_states, 3, x0, P0, True)
# Remember for instantiating new LedgerFilters
self.num_ownship_states = num_ownship_states
self.buffer_capacity = buffer_capacity
self.meas_space_table = meas_space_table
self.last_update_time = None
def __init__(self,
num_ownship_states,
x0,
P0,
delta_codebook_table,
delta_multiplier,
is_main_filter,
asset2id,
my_name,
default_meas_variance,
common_filter=None):
"""Constructor
Arguments:
num_ownship_states {int} -- Number of ownship states for each asset
x0 {np.ndarray} -- initial states
P0 {np.ndarray} -- initial uncertainty
delta_codebook_table {dict} -- Hash thatp stores delta trigger for each measurement type. Str(meas type) -> float (delta trigger)
delta_multiplier {float} -- Delta trigger constant multiplier for this filter
is_main_filter {bool} -- Is this filter a common or main filter (if main the meas buffer does not matter)
asset2id {dict} -- Hash to get the id number of an asset from the string name
my_name {str} -- Name to loopkup in asset2id the current asset's ID#
default_meas_variance {dict} -- Hash to get measurement variance
common_filter {dict} -- asset to common ETFilter
"""
if delta_multiplier <= 0:
raise ValueError("Delta Multiplier must be greater than 0")
self.num_ownship_states = num_ownship_states
self.delta_codebook_table = delta_codebook_table
self.delta_multiplier = delta_multiplier
self.is_main_filter = is_main_filter
if self.is_main_filter:
assert common_filter is not None
self.filter = ETFilter_Main(asset2id[my_name], num_ownship_states,
3, x0, P0, True, {"": common_filter})
else:
self.filter = ETFilter(asset2id[my_name], num_ownship_states, 3,
x0, P0, True)
self.original_filter = deepcopy(self.filter)
self.asset2id = asset2id
self.my_name = my_name
self.default_meas_variance = default_meas_variance
# Initialize ledger with first update
self.ledger = {}
self._add_block()
self.explicit_count = 0
self.meas_types_received = []
def __init__(self, num_ownship_states, x0, P0, buffer_capacity,
meas_space_table, missed_meas_tolerance_table,
delta_codebook_table, delta_multiplier, is_main_filter,
my_id):
"""Constructor
Arguments:
num_ownship_states {int} -- Number of ownship states for each asset
x0 {np.ndarray} -- initial states
P0 {np.ndarray} -- initial uncertainty
buffer_capacity {int} -- capacity of measurement buffer
meas_space_table {dict} -- Hash that stores how much buffer space a measurement takes up. Str (meas type) -> int (buffer space)
Must have key entries "bookend", "bookstart" to indicate space needed for measurement implicitness filling in
missed_meas_tolerance_table {dict} -- Hash that determines how many measurements of each type do we need to miss before indicating a bookend
delta_codebook_table {dict} -- Hash thatp stores delta trigger for each measurement type. Str(meas type) -> float (delta trigger)
delta_multiplier {float} -- Delta trigger constant multiplier for this filter
is_main_filter {bool} -- Is this filter a common or main filter (if main the meas buffer does not matter)
my_id {int} -- ID# of the current asset (typically 0)
"""
if delta_multiplier <= 0:
raise ValueError("Delta Multiplier must be greater than 0")
self.original_estimate = [deepcopy(x0), deepcopy(P0)]
self.delta_codebook_table = delta_codebook_table
self.delta_multiplier = delta_multiplier
self.buffer = MeasurementBuffer(meas_space_table, buffer_capacity)
self.missed_meas_tolerance_table = missed_meas_tolerance_table
self.is_main_filter = is_main_filter
self.filter = ETFilter(my_id, num_ownship_states, 3, x0, P0, True)
# Initialize Ledgers
self.ledger_meas = [] # In internal measurement form
self.ledger_control = [
] ## elements with [u, Q, time_delta, use_control_input]
self.ledger_ci = [
] ## Covariance Intersection ledger, each element is of form [x, P]
self.ledger_update_times = [
] ## Update times of when correction step executed
self.expected_measurements = {
} # When we don't receive an expected measurement we need to insert a "bookend" into the measurement buffer
# Initialize first element of ledgers
self.ledger_meas.append([])
self.ledger_control.append([])
self.ledger_ci.append([])
def catch_up(self, delta_multiplier, shared_buffer):
"""Updates main estimate and common estimate using the shared buffer
Arguments:
delta_multiplier {float} -- multiplier to scale et_delta's with
shared_buffer {list} -- buffer shared from another asset
Returns:
int -- implicit measurement count in shared_buffer
int -- explicit measurement count in this shared_buffer
"""
# Fill in implicit measurements in the buffer and align the meas timestamps with our own
new_buffer, next_ledger_time_index = self._fillin_buffer(shared_buffer)
implicit_meas_cnt = 0
explicit_meas_cnt = 0
# Add all measurements in buffer to ledgers of all ledger_filters
for meas in new_buffer:
self.add_meas(meas, delta_multiplier, force_fuse=False)
if "implicit" in meas.meas_type:
implicit_meas_cnt += 1
else:
explicit_meas_cnt += 1
common_filters = {}
for mult in self.delta_tiers.keys():
my_id = self.delta_tiers[mult].filter.my_id
[x0, P0] = self.delta_tiers[mult].original_estimate
common_filters[mult] = ETFilter(my_id, self.num_ownship_states, 3,
x0, P0, True)
# Initialize asset's main filter
# Pair the main filter with the etfilter the other asset chose and use to update the main filter
my_id = self.main_filter.filter.my_id
[x0, P0] = self.main_filter.original_estimate
other_assets_common = {my_id: common_filters[delta_multiplier]}
main_filter = ETFilter_Main(my_id, self.num_ownship_states, 3, x0, P0,
True, other_assets_common)
# Extract full ledgers
common_meas_ledger = {}
for mult in self.delta_tiers.keys():
common_meas_ledger[mult] = self.delta_tiers[mult].ledger_meas
main_control_ledger = self.main_filter.ledger_control
main_ledger_meas = self.main_filter.ledger_meas
# TODO add covariance intersection support (happens before correction)
main_ci_ledger = self.main_filter.ledger_meas
# Grab lock, no updates for right now
# Initialize a new main and common filters (all are etfilters) using original estimate
for i_ledge in range(len(self.main_filter.ledger_update_times)):
[u, Q, delta_time, _] = main_control_ledger[i_ledge]
for mult in common_filters.keys():
common_filters[mult].predict(u,
Q,
delta_time,
use_control_input=False)
ledger_meas = common_meas_ledger[mult][i_ledge]
for meas in ledger_meas:
common_filters[mult].add_meas(meas)
common_filters[mult].correct()
main_filter.predict(u, Q, delta_time, use_control_input=True)
for meas in main_ledger_meas[i_ledge]:
main_filter.add_meas(meas)
main_filter.correct()
# Trim ledgers
if next_ledger_time_index != len(self.main_filter.ledger_update_times):
ledger_update_times = self.main_filter.ledger_update_times[
next_ledger_time_index:]
for mult in common_meas_ledger.keys():
common_meas_ledger[mult] = common_meas_ledger[mult][
next_ledger_time_index:]
main_ledger_meas = main_ledger_meas[next_ledger_time_index:]
main_control_ledger = main_control_ledger[next_ledger_time_index:]
else:
ledger_update_times = []
for mult in common_meas_ledger.keys():
common_meas_ledger[mult] = [[]]
main_ledger_meas = [[]]
main_control_ledger = [[]]
### Reset the ledger filters ###
# Reset the delta tier filters
for multiplier in self.delta_tiers.keys():
# Caught up estimate becomes new initial estimate
x0 = common_filters[multiplier].x_hat
P0 = common_filters[multiplier].P
buf = deepcopy(self.delta_tiers[multiplier].buffer)
# Instantiate new delta tier
self.delta_tiers[multiplier] = LedgerFilter(
self.num_ownship_states, x0, P0, \
self.buffer_capacity, self.meas_space_table, \
self.missed_meas_tolerance_table, \
self.delta_codebook_table, multiplier, \
False, self.asset2id[self.my_name]
)
self.delta_tiers[multiplier].reset(buf, ledger_update_times,
common_meas_ledger[multiplier])
### Reset the main filter ###
# Caught up estimate becomes new initial estimate
x0 = main_filter.x_hat
P0 = main_filter.P
mainbuf = deepcopy(self.main_filter.buffer)
# Instantiate new Main Filter
self.main_filter = LedgerFilter(
self.num_ownship_states, x0, P0, \
self.buffer_capacity, self.meas_space_table, \
self.missed_meas_tolerance_table, \
self.delta_codebook_table, 1.0, \
True, self.asset2id[self.my_name]
)
self.main_filter.reset(mainbuf, ledger_update_times, main_ledger_meas,
main_control_ledger)
return implicit_meas_cnt, explicit_meas_cnt