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 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)
