def main():
rospy.init_node('ekf_node')
ekf = ekf_class.ExtendedKalmanFilter()
publisher_position = rospy.Publisher('estimated_pose',
PoseStamped,
queue_size=1)
publisher_mavros = rospy.Publisher('/mavros/vision_pose/pose',
PoseStamped,
queue_size=1)
#publisher_particles = rospy.Publisher('particle_poses', PoseArray, queue_size=1)
publisher_marker = rospy.Publisher('Sphere', MarkerArray, queue_size=1)
broadcaster = tf.TransformBroadcaster()
rospy.Subscriber("/tag_detections",
AprilTagDetectionArray,
callback, [
ekf, publisher_position, publisher_mavros,
broadcaster, publisher_marker
],
queue_size=1)
while not rospy.is_shutdown():
pass
def listener():
rospy.init_node('publisher', anonymous=True)
ekf = ekf_class.ExtendedKalmanFilter()
pub = rospy.Publisher("point_cloud2", PointCloud2, queue_size=2)
publisher_marker = rospy.Publisher('detection_net_plane',
MarkerArray,
queue_size=1)
rospy.Subscriber("/d435i/depth/color/points", PointCloud2, callback,
[pub, ekf, publisher_marker])
rospy.spin()
# video.release()
cv2.destroyAllWindows()
def listener():
geschwindigkeit = 0.5
# tracker = tracking_red_dots(308,410)
# tracker = tracking_red_dots(960, 1280,350,900,400,960)
global ekf, publisher_distance_net, rviz
ekf = ekf_class.ExtendedKalmanFilter()
rospy.init_node('publisher', anonymous=True)
publisher_distance_net = rospy.Publisher('/estimated_distance_to_net',
PoseStamped,
queue_size=1)
rospy.Subscriber("/multisense_sl/camera/left/image_raw", Image, callback)
if rviz:
rospy.Subscriber("/gazebo/model_states", ModelStates, draw_net_rviz)
rospy.spin()
# video.release()
cv2.destroyAllWindows()
def main():
global tags
rospy.init_node('ekf_node')
try:
#which_calibration=rospy.get_param('~calibration')
which_calibration = "water_tank"
except KeyError:
print(
"################## You have to set a calibration parameter ###########################"
)
exit(-1)
if which_calibration == "gazebo":
print("using gazebo calibration")
data_path = rospack.get_path(
"mu_auv_localization"
) + '/scripts/calibration_ground_truth_gazebo.csv' # in gazebo
else:
if which_calibration == "water_tank":
print("using real calibration")
data_path = rospack.get_path(
"mu_auv_localization"
) + '/scripts/calibration_tank.csv' # in real tank
else:
print("could not find correct parameter for calibration ")
exit(-1)
tags = genfromtxt(data_path, delimiter=',') # home PC
tags = tags[:, 0:4]
tags[:, 3] += 0.0
ekf = ekf_class.ExtendedKalmanFilter()
publisher_position = rospy.Publisher('estimated_pose',
PoseStamped,
queue_size=1)
publisher_twist = rospy.Publisher('estimated_twist',
TwistStamped,
queue_size=1)
publisher_mavros = rospy.Publisher('/mavros/vision_pose/pose',
PoseStamped,
queue_size=1)
# publisher_particles = rospy.Publisher('particle_poses', PoseArray, queue_size=1)
publisher_marker = rospy.Publisher('Sphere', MarkerArray, queue_size=1)
broadcaster = tf.TransformBroadcaster()
rospy.Subscriber("/tag_detections",
AprilTagDetectionArray,
callback, [
ekf, publisher_position, publisher_mavros,
broadcaster, publisher_marker, publisher_twist
],
queue_size=1)
rospy.Subscriber("/mavros/imu/data",
Imu,
callback_imu, [
ekf, publisher_position, publisher_mavros,
broadcaster, publisher_marker, publisher_twist
],
queue_size=1)
rospy.Subscriber("/mavros/local_position/pose_NED",
PoseStamped,
callback_orientation,
ekf,
queue_size=1)
rospy.spin()
import ekf_class
import numpy as np
ekf = ekf_class.ExtendedKalmanFilter()
ekf.prediction()
A=np.asarray([[1,1,0],[0,3,2],[3,2,1]])
for i in range(5000):
print(i)
ekf.prediction()
ekf.update(A)