class KF_system(object):
"""
This is the very high level system that keeps track of communication
between the ins and the kf, updating both when necessary.
It keeps both the ins and the kf as separate objects.
The ins is updated when the imu inputs arrive, and
the kf is updated when the gps inputs arrive.
The kf takes in the current values of the ins,
then the output of the kf is used to correct the ins.
Finally, the ins value is returned.
"""
def __init__(self, lever_arm, timestamp, num_inputs_ins, ins_body_to_ECEF):
self.ins = INS(timestamp, num_inputs_ins)
self.ins_time = timestamp
self.kf = Filter(lever_arm, ins_body_to_ECEF)
self.kf_time = timestamp
def update_imu(self, ax, ay, yaw, timestamp):
self.ins.update(ax,ay,yaw,timestamp)
self.ins_time = timestamp
return self.ins.state
def update_gps(self, specific_force, gravity_model_value, timestamp,
sigma_x, sigma_y, sigma_z):
corrections = self.kf.update(specific_force, gravity_model_value,
timestamp - self.kf_time, sgma_x, sigma_y, sigma_z)
self.kf_time = timestamp
self.ins.correct(corrections)
return self.ins.state
def __init__(self, lever_arm, timestamp, num_inputs_ins, ins_body_to_ECEF):
self.ins = INS(timestamp, num_inputs_ins)
self.ins_time = timestamp
self.kf = Filter(lever_arm, ins_body_to_ECEF)
self.kf_time = timestamp
