class Simulator(): def __init__(self): self.plant = DoubleIntegrator() # states: x y z dx dy dz accxbias accybias acczbias # inputs: roll pitch yaw accx accy accz # measurements: x y z self.kalman = ExtendedKalmanFilter(dim_x=9, dim_z=3, plant=self.plant) self.lc = lcm.LCM() self.max_blackout = 20 self.min_blackout = 5 self.blackout_prob = .01 self.current_blackout_count = 0 def run(self): self.lc.subscribe('kalman_args',self.handle_kalman_args) while True: self.lc.handle() def handle_kalman_args(self,channel,data): msg = kalman_args_t.decode(data) if random.random()<self.blackout_prob and self.current_blackout_count==0: self.current_blackout_count = int(random.random()*(self.max_blackout-self.min_blackout)+self.min_blackout) if self.current_blackout_count>0: self.current_blackout_count -= 1 msg.valid_vicon = False self.kalman.predict(np.array(msg.input_rpy + msg.input_acc), msg.input_dt) if msg.valid_vicon: self.kalman.update(np.array(msg.meas_xyz)) kalman_xhat = self.kalman.x.reshape(9).tolist() msg_out = kalman_out_t() msg_out.kalman_xyz = kalman_xhat[0:3] msg_out.kalman_dxyz = kalman_xhat[3:6] if msg.valid_vicon: msg_out.smooth_xyz = msg.smooth_xyz msg_out.smooth_dxyz = msg.smooth_dxyz msg_out.smooth_xyz_noblackout = msg.smooth_xyz msg_out.smooth_dxyz_noblackout = msg.smooth_dxyz self.lc.publish('kalman_out', msg_out.encode())
def __init__(self): self.plant = DoubleIntegrator() # states: x y z dx dy dz accxbias accybias acczbias # inputs: roll pitch yaw accx accy accz # measurements: x y z self.kalman = ExtendedKalmanFilter(dim_x=9, dim_z=3, plant=self.plant) self.lc = lcm.LCM() self.max_blackout = 20 self.min_blackout = 5 self.blackout_prob = .01 self.current_blackout_count = 0