def test_kalman(): expected_states = np.array([ 0.35454545, 0.42380952, 0.44193548, 0.40487805, 0.3745098, 0.36557377, 0.36197183, 0.37654321, 0.38021978, 0.38712871 ]) expected_covariances = np.array([ 0.09090909, 0.04761905, 0.03225806, 0.02439024, 0.01960784, 0.01639344, 0.01408451, 0.01234568, 0.01098901, 0.00990099 ]) # set parameters Z = data A = np.array([[1]]) xk = np.array([[0]]) B = np.array([[0]]) U = np.zeros((len(Z), 1)) Pk = np.array([[1]]) H = np.array([[1]]) Q = 0 R = 0.1 kf = KalmanFilter(A=A, xk=xk, B=B, Pk=Pk, H=H, Q=Q, R=R) states, covariances = kf.run_filter(Z, U) np.testing.assert_allclose(states, expected_states, rtol=1e-06, atol=0) np.testing.assert_allclose(covariances, expected_covariances, rtol=1e-06, atol=0)
def __init__(self, d): self.initialized = False self.timestamp = 0 self.n = d['number_of_states'] self.P = d['initial_process_matrix'] self.F = d['inital_state_transition_matrix'] self.Q = d['initial_noise_matrix'] self.radar_R = d['radar_covariance_matrix'] self.lidar_R = d['lidar_covariance_matrix'] self.lidar_H = d['lidar_transition_matrix'] self.a = (d['acceleration_noise_x'], d['acceleration_noise_y']) self.kalmanFilter = KalmanFilter(self.n)
def __init__(self): self.mode = rospy.get_param("~mode", default=MODE.CMD_VEL_RGBD) rospy.loginfo("Using mode: " + str(self.mode)) if self.mode == MODE.CMD_VEL_SCAN or self.mode == MODE.CMD_VEL_RGBD: rospy.Subscriber("cmd_vel", Twist, self.on_cmdvel, queue_size=1) elif self.mode == MODE.POSE_SCAN or self.mode == MODE.POSE_RGBD: rospy.Subscriber("pose", PoseStamped, self.on_pose, queue_size=1) else: rospy.logerr("Unrecognized / invalid mode: " + self.mode) exit(1) rospy.Subscriber("scan", LaserScan, self.on_scan, queue_size=1) self.state_pub = rospy.Publisher("state_estimate", PoseWithCovarianceStamped, queue_size=1) self.tf_broadcaster = tf.TransformBroadcaster() outfile = rospy.get_param("~outfile", default="output.csv") self.outfile = open(outfile, "w") self.outfile.write("time, location, uncertainty\n") self.kfilter = KalmanFilter(INITIAL_BELIEF, INITIAL_UNCERTAINTY, STATE_EVOLUTION, MOTION_NOISE, SENSE_NOISE) self.prev_cmdvel_t = None self.prev_cmdvel = None self.cmdvel_count = 0 self.prev_pose = None self.pose_count = 0 self.scan_count = 0 self.prev_wall_loc = None
def valuesChanged(self): states = self['states'] _LTI_BASE.valuesChanged(self) # removes hold if len(self['x0']) != states: self['x0'] = [[0]]*states if len(self['P0']) != states: self['P0'] = eye(states).tolist() if len(self['Q']) != states: self['Q'] = eye(states, states).tolist() if len(self['R']) != 1: self['R'] = eye(1).tolist() #TODO: change it to qxq Q, R = self['Q'], self['R'] x0, P0 = array(self['x0']), self['P0'] A, B, C, D = self._lti.A, self._lti.B, self._lti.C, self._lti.D self.__kf = KalmanFilter(A, B, C, D, Q, R, x0, P0)
def kalman_filter(data, col_num, threshold, Q: float, EGM: bool, EGM_window_width=800, verbose=True): ''' ---------------- DESCRIPTION ---------------- Simple implementation of a Kalman filter based on: http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html ''' Measurement = effectiv_trans.gradient_filter(data=data, col_num=col_num, threshold=threshold, verbose=verbose) P = np.diag(np.array(1.0).reshape(-1)) F = np.matrix(1.0) H = F R = np.matrix(0.1**2) Q = Q G = np.matrix(1.0) Q = G * (G.T) * Q Z = np.matrix(Measurement[0]) X = Z kf = KalmanFilter(X, P, F, Q, Z, H, R) X_ = [X[0, 0]] for i in tqdm(range(1, len(Measurement)), desc="Kalman filter...", ascii=False, ncols=75): # Predict (X, P) = kf.predict(X, P, w=0) # Update (X, P) = kf.update(X, P, Z) Z = np.matrix(Measurement[i]) X_.append(X[0, 0]) if EGM: EGM = [X_[0]] n = EGM_window_width for i in tqdm(range(1, len(Measurement) - n + 1), desc="EGM filter...", ascii=False, ncols=80): EGM.append(np.mean(X_[i:i + n])) EGM.extend(X_[len(Measurement) - n + 1:]) return EGM else: return X_
uwb_thread = threading.Thread(target=UWB_dis) uwb_thread.start() print('UWB thread start!') ang_range = 0.05 anc_dis = [0.55, 0.9] string_time = datetime.now().strftime("%H_%M_%S") data_filename = 'follw_data_' + string_time +'.txt' #------------kalman filter parameter-------------------- dt = 1.0/20 F = np.array([[1, dt, 0], [0, 1, dt], [0, 0, 1]]) H = np.array([1, 0, 0]).reshape(1, 3) Q = np.array([[0.05, 0.05, 0.0], [0.05, 0.05, 0.0], [0.0, 0.0, 0.0]]) R = np.array([0.5]).reshape(1, 1) kf = KalmanFilter(F = F, H = H, Q = Q, R = R) def _main(): #file_num = int(input('experiment number: ')) try: print('Record initial distance !') time.sleep(2) if (len(dis_queue) > 0): dis_to_tag = dis_queue.popleft() if(dis_to_tag[0] != 0): ini_tag_dis = dis_to_tag[0] # ini_tag_dis = 35 print('--------------ini_tag_dis: ', ini_tag_dis) with open('ini_tag_dis_' + string_time +'.txt', 'a') as fout: json.dump({'time': [start_time], 'ini_tag_dis': [ini_tag_dis]}, fout)
class FusionEKF: """ A class that gets sensor measurements from class DataPoint and predicts the next state of the system using an extended Kalman filter algorithm The state variables we are considering in this system are the position and velocity in x and y cartesian coordinates, in essence there are 4 variables we are tracking. In particular, an instance of this class gets measurements from both lidar and radar sensors lidar sensors measure positions in cartesian coordinates (2 values) radar sensors measure position and velocity in polar coordinates (3 values) lidar sensor are linear and radar sensors are non-linear, so we use the jacobian algorithm to compute the state transition matrix H unlike a simple kalman filter. """ def __init__(self, d): self.initialized = False self.timestamp = 0 self.n = d['number_of_states'] self.P = d['initial_process_matrix'] self.F = d['inital_state_transition_matrix'] self.Q = d['initial_noise_matrix'] self.radar_R = d['radar_covariance_matrix'] self.lidar_R = d['lidar_covariance_matrix'] self.lidar_H = d['lidar_transition_matrix'] self.a = (d['acceleration_noise_x'], d['acceleration_noise_y']) self.kalmanFilter = KalmanFilter(self.n) def updateQ(self, dt): dt2 = dt * dt dt3 = dt * dt2 dt4 = dt * dt3 x, y = self.a r11 = dt4 * x / 4 r13 = dt3 * x / 2 r22 = dt4 * y / 4 r24 = dt3 * y / 2 r31 = dt3 * x / 2 r33 = dt2 * x r42 = dt3 * y / 2 r44 = dt2 * y Q = np.matrix([[r11, 0, r13, 0], [0, r22, 0, r24], [r31, 0, r33, 0], [0, r42, 0, r44]]) self.kalmanFilter.setQ(Q) def update(self, data): dt = time_difference(self.timestamp, data.get_timestamp()) self.timestamp = data.get_timestamp() self.kalmanFilter.updateF(dt) self.updateQ(dt) self.kalmanFilter.predict() z = np.matrix(data.get_raw()).T x = self.kalmanFilter.getx() if data.get_name() == 'radar': px, py, vx, vy = x[0, 0], x[1, 0], x[2, 0], x[3, 0] rho, phi, drho = cartesian_to_polar(px, py, vx, vy) H = calculate_jacobian(px, py, vx, vy) Hx = (np.matrix([[rho, phi, drho]])).T R = self.radar_R elif data.get_name() == 'lidar': H = self.lidar_H Hx = self.lidar_H * x R = self.lidar_R self.kalmanFilter.update(z, H, Hx, R) def start(self, data): self.timestamp = data.get_timestamp() x = np.matrix([data.get()]).T self.kalmanFilter.start(x, self.P, self.F, self.Q) self.initialized = True def process(self, data): if self.initialized: self.update(data) else: self.start(data) def get(self): return self.kalmanFilter.getx()
class Instrument_Kalman_Filter(_LTI_BASE):#, KalmanFilter): _subType = 'KalmanFilter' defaults = { 'NAME' : u'Kalman filter', 'DESCRIPTION_TYPE' : 1,#'tf', 'STATES' : 1,#2, #'NUM' : [1.], #'DEN' : [1., 6., 25.], 'A' : [[1.]],#[[-6., -25.], [1., 0.]], 'B' : [[0.]], #[[1.], [0.]], 'C' : [[1.]], #[[0., 1.]], 'D' : [[0.]], 'GENERATE_STATES' : False, 'x0' : [[1.]],#[[0.], [0.]], #[[0]]*KF_STATES,#array([[0.]]*2), # Nx1 'P0' : [[1.]], #*eye(2, typecode='d'), # NxN 'Q' : [[1.]], # * eye(KF_STATES), # NxN 'R' : [[1.]]} # * eye(KF_OUTPUTS), # qxq def __init__(self, *args, **kwords): self.__kf = None _LTI_BASE.__init__(self)#- , name=self.__class__.defaults['NAME']) af = self.parameters.append sc = self.__class__ # set the name attribute self.name = sc.defaults['NAME'] self['x0'] = sc.defaults['x0'] self['P0'] = sc.defaults['P0'] self['Q'] = sc.defaults['Q'] self['R'] = sc.defaults['R'] af(Parameter(name='signal_yv', value=0, tp=IntType)) af(Parameter(name='signal_u', value=1, tp=IntType)) # override some defaults self['states'] = sc.defaults['STATES'] #self._lti = lti(sc.defaults['NUM'], sc.defaults['DEN']) af(Parameter(name='description type', value=sc.defaults['DESCRIPTION_TYPE'], tp=IntType)) af(Parameter(name='generate states', value=sc.defaults['GENERATE_STATES'], tp=BooleanType)) ## self['description type'] = sc.defaults['DESCRIPTION_TYPE'] ## self['generate states'] = sc.defaults['GENERATE_STATES'] ## self._changeDescriptionType(sc.defaults['DESCRIPTION_TYPE']) ## self['xAxisLabel'] = tr('frequency') ## self['yAxisLabel'] = tr('magnitude') ## self['xUnit'] = u'Hz' ## self['yUnit'] = u'' self.setProperties(kwords) def valuesChanged(self): states = self['states'] _LTI_BASE.valuesChanged(self) # removes hold if len(self['x0']) != states: self['x0'] = [[0]]*states if len(self['P0']) != states: self['P0'] = eye(states).tolist() if len(self['Q']) != states: self['Q'] = eye(states, states).tolist() if len(self['R']) != 1: self['R'] = eye(1).tolist() #TODO: change it to qxq Q, R = self['Q'], self['R'] x0, P0 = array(self['x0']), self['P0'] A, B, C, D = self._lti.A, self._lti.B, self._lti.C, self._lti.D self.__kf = KalmanFilter(A, B, C, D, Q, R, x0, P0) def process(self, signals): if self.__kf==None: self.valuesChanged() if signals[self['signal_yv']]==None: if len(self.appendedSignalsList)==1: self['signal_yv']=self.appendedSignalsList[0] else: raise anasigError(tr('Connect signals first!') + ' (' + tr('signal_yv') + ' not set)') s_yv = signals[self['signal_yv']] dyv = s_yv.get_dataY() #self['signal_u'] if not len(signals) or len(signals)<(self['signal_u']+1):# or signals[self['signal_u']]==None: s_u = None du = None else: s_u = signals[self['signal_u']] du = s_u.get_dataY() # KalmanFilter.py yout, xout, K, P = self.__kf.process(dyv, du) # pNumeric version # Q, R = Matrix(self['Q']), Matrix(self['R']) # x0, P0 = Matrix(self['x0']), Matrix(self['P0']) # A, B, C, D = Matrix(self._lti.A.tolist()), Matrix(self._lti.B.tolist()), \ # Matrix(self._lti.C.tolist()), Matrix(self._lti.D.tolist()) # x_est, y_est, P_est = kf_process(A, B, C, D, dyv, du, x0, P0, Q, R) sout = [] # output signals list if self['generate states']=='yes': xout = transpose(xout) i=1 # take all system state variables for x in xout: out = Signal(fs=s_yv['fs']) out.set_data( x, None ) out.name = self.name + u': ' + tr('state') + u' ' + str(i) i += 1 sout.append(out) # output values out = Signal(fs=s_yv['fs']) out.set_data(transpose(yout)[0], None) out.name = self.name + u': ' + tr('output') sout.append(out) # # kalman gain # Kout = transpose(K) # i=1 # # take all system state variables # for k in Kout: # out = Signal(fs=s_yv['fs']) # out.set_data( k, None ) # out.name = self.name + u': ' + tr('txt_gain') + u' ' + str(i) # i += 1 # sout.append(out) # return sout
class PoseEstimator: def __init__(self): self.mode = rospy.get_param("~mode", default=MODE.CMD_VEL_RGBD) rospy.loginfo("Using mode: " + str(self.mode)) if self.mode == MODE.CMD_VEL_SCAN or self.mode == MODE.CMD_VEL_RGBD: rospy.Subscriber("cmd_vel", Twist, self.on_cmdvel, queue_size=1) elif self.mode == MODE.POSE_SCAN or self.mode == MODE.POSE_RGBD: rospy.Subscriber("pose", PoseStamped, self.on_pose, queue_size=1) else: rospy.logerr("Unrecognized / invalid mode: " + self.mode) exit(1) rospy.Subscriber("scan", LaserScan, self.on_scan, queue_size=1) self.state_pub = rospy.Publisher("state_estimate", PoseWithCovarianceStamped, queue_size=1) self.tf_broadcaster = tf.TransformBroadcaster() outfile = rospy.get_param("~outfile", default="output.csv") self.outfile = open(outfile, "w") self.outfile.write("time, location, uncertainty\n") self.kfilter = KalmanFilter(INITIAL_BELIEF, INITIAL_UNCERTAINTY, STATE_EVOLUTION, MOTION_NOISE, SENSE_NOISE) self.prev_cmdvel_t = None self.prev_cmdvel = None self.cmdvel_count = 0 self.prev_pose = None self.pose_count = 0 self.scan_count = 0 self.prev_wall_loc = None def spin(self): rospy.spin() def on_cmdvel(self, msg): curtime = rospy.Time.now() self.cmdvel_count += 1 rospy.loginfo("cmdvel #{0} linX:{1}".format( self.cmdvel_count, msg.linear.x)) if self.prev_cmdvel_t is not None: dt = curtime - self.prev_cmdvel_t avg_vel = 0.5 * (msg.linear.x + self.prev_cmdvel.linear.x) rospy.loginfo("\tavg_vel: {0} dt:{1}".format(avg_vel, dt)) self.kfilter.propogate(action_model=numpy.matrix([dt.to_sec()]), control=numpy.matrix([avg_vel])) self.publish_state_estimation(curtime) self.prev_cmdvel_t = curtime self.prev_cmdvel = msg def on_pose(self, msg): self.pose_count += 1 rospy.loginfo("pose #{0} location:{1}".format( self.pose_count, msg.pose.position.x)) if self.prev_pose is not None: dx = msg.pose.position.x - self.prev_pose.pose.position.x rospy.loginfo("\tdx: {0}".format(dx)) self.kfilter.propogate(action_model=numpy.matrix([1]), control=numpy.matrix([dx])) self.publish_state_estimation(msg.header.stamp) self.prev_pose = msg def on_scan(self, msg): self.scan_count += 1 cur_reading = None if self.mode == MODE.POSE_SCAN or self.mode == MODE.CMD_VEL_SCAN: # 0th range measurment is forward using the builtin lidar cur_reading = msg.ranges[0] elif self.mode == MODE.POSE_RGBD or self.mode == MODE.CMD_VEL_RGBD: # for RGBD conversion, the middle range measurement is forward # > for increased robustness, look at the middle few messages tries = 0 midI = int(len(msg.ranges) / 2) loffset = 0 roffset = 0 while cur_reading is None or math.isnan(cur_reading): if tries % 2 == 0: cur_reading = msg.ranges[midI + loffset] loffset -= 1 if tries % 2 == 1: cur_reading = msg.ranges[midI + roffset] roffset += 1 tries += 1 if tries > 10: break rospy.loginfo("scan #{0} dist:{1}".format(self.scan_count, cur_reading)) msg = LaserScan() if math.isnan(cur_reading): rospy.loginfo("\tUnusable scan reading. Ignoring scan msg") return if self.prev_wall_loc is not None: # Need to jump through some hoops to get sensor model, # because we choose to make 0 be the starting point of the robot robot_x = self.kfilter.belief.item(0) expected_wall_distance = self.prev_wall_loc - robot_x self.kfilter.update(expected_reading=expected_wall_distance, reading=cur_reading) self.publish_state_estimation(msg.header.stamp) # Setting odom origin as 0, the wall is located at (scan_reading), offset by # our current location (stored in self.kfilter.belief) self.prev_wall_loc = self.kfilter.belief.item(0) + cur_reading rospy.loginfo("\twall location: {0}".format(self.prev_wall_loc)) def publish_state_estimation(self, stamp): robot_x = self.kfilter.belief.item(0) uncertainty = self.kfilter.uncertainty.item(0) pose = PoseWithCovarianceStamped() pose.pose.pose.position.x = robot_x pose.pose.covariance = numpy.matrix([[uncertainty, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0]]) self.state_pub.publish(pose) self.tf_broadcaster.sendTransform( (robot_x, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0), stamp, "odom_kf", "base_footprint") rospy.loginfo("\tkfilter. pos: {0} uncertainty: {1}".format( self.kfilter.belief, self.kfilter.uncertainty)) stamp = rospy.Time.now() self.outfile.write("{0},{1},{2}\n".format(stamp.to_sec(), robot_x, uncertainty)) def __del__(self): self.outfile.close()