diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index dc276494..5a90b88a 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -28,12 +28,12 @@ - + @@ -45,13 +45,16 @@ - + - + + - + - + + + + diff --git a/code/agent/config/dev_objects.json b/code/agent/config/dev_objects.json index cafac343..90b24361 100644 --- a/code/agent/config/dev_objects.json +++ b/code/agent/config/dev_objects.json @@ -179,6 +179,55 @@ { "type": "actor.pseudo.control", "id": "control" + }, + { + "type": "sensor.other.gnss", + "id": "Ideal_GPS", + "spawn_point": { + "x": 0.0, + "y": 0.0, + "z": 1.60 + }, + "noise_alt_stddev": 0.0, + "noise_lat_stddev": 0.0, + "noise_lon_stddev": 0.0, + "noise_alt_bias": 0.0, + "noise_lat_bias": 0.0, + "noise_lon_bias": 0.0 + }, + { + "type": "sensor.other.imu", + "id": "Ideal_IMU", + "spawn_point": { + "x": 0.7, + "y": 0.4, + "z": 1.60, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0 + }, + "noise_accel_stddev_x": 0.0, + "noise_accel_stddev_y": 0.0, + "noise_accel_stddev_z": 0.0, + "noise_gyro_stddev_x": 0.0, + "noise_gyro_stddev_y": 0.0, + "noise_gyro_stddev_z": 0.0 + }, + { + "type": "sensor.other.radar", + "id": "Ideal_RADAR", + "spawn_point": { + "x": 0.7, + "y": 0.4, + "z": 1.60, + "roll": 0.0, + "pitch": 0.0, + "yaw": 45.0 + }, + "horizontal_fov": 30.0, + "vertical_fov": 30.0, + "points_per_second": 1500, + "range": 100.0 } ] } diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 2bc653ab..3adc596e 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -2,11 +2,33 @@ + + + + + + @@ -71,4 +93,5 @@ + diff --git a/code/perception/src/Position_Publisher_Node.py b/code/perception/src/Position_Publisher_Node.py index 424a1527..8c0431c2 100755 --- a/code/perception/src/Position_Publisher_Node.py +++ b/code/perception/src/Position_Publisher_Node.py @@ -10,10 +10,10 @@ from geometry_msgs.msg import PoseStamped from sensor_msgs.msg import NavSatFix, Imu from nav_msgs.msg import Odometry -from std_msgs.msg import Float32 -from coordinate_transformation import CoordinateTransformer, GeoRef +from std_msgs.msg import Float32, String +from coordinate_transformation import CoordinateTransformer from tf.transformations import euler_from_quaternion - +from xml.etree import ElementTree as eTree GPS_RUNNING_AVG_ARGS: int = 10 @@ -37,9 +37,15 @@ def __init__(self): self.control_loop_rate = self.get_param("control_loop_rate", "0.05") # todo: automatically detect town - self.transformer = CoordinateTransformer(GeoRef.TOWN12) + self.transformer = None # Subscriber + self.map_sub = self.new_subscription( + String, + "/carla/" + self.role_name + "/OpenDRIVE", + self.get_geoRef, + qos_profile=1) + self.imu_subscriber = self.new_subscription( Imu, "/carla/" + self.role_name + "/IMU", @@ -79,6 +85,33 @@ def __init__(self): f"/paf/{self.role_name}/current_heading", qos_profile=1) + def get_geoRef(self, opendrive: String): + """_summary_ + Reads the reference values for lat and lon from the carla OpenDriveMap + Args: + opendrive (String): OpenDrive Map from carla + """ + root = eTree.fromstring(opendrive.data) + header = root.find("header") + geoRefText = header.find("geoReference").text + + latString = "+lat_0=" + lonString = "+lon_0=" + + indexLat = geoRefText.find(latString) + indexLon = geoRefText.find(lonString) + + indexLatEnd = geoRefText.find(" ", indexLat) + indexLonEnd = geoRefText.find(" ", indexLon) + + latValue = float(geoRefText[indexLat + len(latString):indexLatEnd]) + lonValue = float(geoRefText[indexLon + len(lonString):indexLonEnd]) + + CoordinateTransformer.la_ref = latValue + CoordinateTransformer.ln_ref = lonValue + CoordinateTransformer.ref_set = True + self.transformer = CoordinateTransformer() + def update_imu_data(self, data: Imu): """ This method is called when new IMU data is received. @@ -140,34 +173,38 @@ def update_gps_data(self, data: NavSatFix): :param data: GNSS measurement :return: """ - lat = data.latitude - lon = data.longitude - alt = data.altitude - x, y, z = self.transformer.gnss_to_xyz(lat, lon, alt) - # find reason for discrepancy - x *= 0.998 - y *= 1.003 + # Make sure position is only published when reference values have been + # read from the Map + if CoordinateTransformer.ref_set is False: + self.transformer = CoordinateTransformer() + CoordinateTransformer.ref_set = True + if CoordinateTransformer.ref_set is True: + lat = data.latitude + lon = data.longitude + alt = data.altitude + + x, y, z = self.transformer.gnss_to_xyz(lat, lon, alt) - self.avg_xyz = np.roll(self.avg_xyz, -1, axis=0) - self.avg_xyz[-1] = np.matrix([x, y, z]) + self.avg_xyz = np.roll(self.avg_xyz, -1, axis=0) + self.avg_xyz[-1] = np.matrix([x, y, z]) - avg_x, avg_y, avg_z = np.mean(self.avg_xyz, axis=0) + avg_x, avg_y, avg_z = np.mean(self.avg_xyz, axis=0) - cur_pos = PoseStamped() + cur_pos = PoseStamped() - cur_pos.header.stamp = data.header.stamp - cur_pos.header.frame_id = "global" + cur_pos.header.stamp = data.header.stamp + cur_pos.header.frame_id = "global" - cur_pos.pose.position.x = avg_x - cur_pos.pose.position.y = avg_y - cur_pos.pose.position.z = avg_z + cur_pos.pose.position.x = avg_x + cur_pos.pose.position.y = avg_y + cur_pos.pose.position.z = avg_z - cur_pos.pose.orientation.x = 0 - cur_pos.pose.orientation.y = 0 - cur_pos.pose.orientation.z = 1 - cur_pos.pose.orientation.w = 0 + cur_pos.pose.orientation.x = 0 + cur_pos.pose.orientation.y = 0 + cur_pos.pose.orientation.z = 1 + cur_pos.pose.orientation.w = 0 - self.cur_pos_publisher.publish(cur_pos) + self.cur_pos_publisher.publish(cur_pos) def run(self): """ diff --git a/code/perception/src/coordinate_transformation.py b/code/perception/src/coordinate_transformation.py index 5c7d4c75..203d6988 100755 --- a/code/perception/src/coordinate_transformation.py +++ b/code/perception/src/coordinate_transformation.py @@ -8,54 +8,63 @@ http://dirsig.cis.rit.edu/docs/new/coordinates.html """ import math -from enum import Enum from tf.transformations import euler_from_quaternion -# Class to choose a map with a predefined reference point -class GeoRef(Enum): - TOWN01 = 0, 0, 0 - TOWN02 = 0, 0, 0 - TOWN03 = 0, 0, 0 - TOWN04 = 0, 0, 0 - TOWN05 = 0, 0, 0 - TOWN06 = 0, 0, 0 # lat =, lon =, alt = #Town06/HD not found - TOWN07 = 0, 0, 0 # lat =, lon =, alt = #Town07/HD not found - TOWN08 = 0, 0, 0 # lat =, lon =, alt = #Town08/HD not found - TOWN09 = 0, 0, 0 # lat =, lon =, alt = #Town09/HD not found - TOWN10 = 0, 0, 0 # Town10HD - TOWN11 = 0, 0, 0 # lat =, lon =, alt = #Town11/HD not found - TOWN12 = 0, 0, 0 # 35.25000, -101.87500, 331.00000 - - -a = 6378137 +a = 6378137 # EARTH_RADIUS_EQUA in Pylot, used in geodetic_to_enu b = 6356752.3142 f = (a - b) / a e_sq = f * (2 - f) +alt_offset = 331.00000 +STD_LAT = 0.0 +STD_LON = 0.0 +STD_H = 0.0 class CoordinateTransformer: """Object that is used to transform Coordinates between xyz and gnss reference frame""" - la_ref: float - ln_ref: float - h_ref: float + la_ref = STD_LAT + ln_ref = STD_LON + h_ref = STD_H ref_set = False - def __init__(self, gps_ref: GeoRef): - self.la_ref = gps_ref.value[0] - self.ln_ref = gps_ref.value[1] - self.h_ref = gps_ref.value[2] + def __init__(self): + pass def gnss_to_xyz(self, lat, lon, h): - return geodetic_to_enu(lat, lon, h, - self.la_ref, self.ln_ref, self.h_ref) + return geodetic_to_enu(lat, lon, h) + + +def geodetic_to_enu(lat, lon, alt): + """ + Method from pylot project to calculate coordinates + https://github.com/erdos-project/pylot/blob/master/pylot/utils.py#L470 + + Args: + lat (float): latitude + lon (float): longitude + alt (float: altitude + + Returns: + x, y, z: coordinates + """ + + scale = math.cos(CoordinateTransformer.la_ref * math.pi / 180.0) + basex = scale * math.pi * a / 180.0 * CoordinateTransformer.ln_ref + basey = scale * a * math.log( + math.tan((90.0 + CoordinateTransformer.la_ref) * math.pi / 360.0)) + x = scale * math.pi * a / 180.0 * lon - basex + y = scale * a * math.log( + math.tan((90.0 + lat) * math.pi / 360.0)) - basey -def geodetic_to_enu(lat, lon, h, lat_ref, lon_ref, h_ref): - x, y, z = geodetic_to_ecef(lat, lon, h) - return ecef_to_enu(x, y, z, lat_ref, lon_ref, h_ref) + # Is not necessary in new version + # y *= -1 + # alt_offset is needed to keep the hight of the map in mind + # right now we don't really use the altitude anyways + return x, y, alt + alt_offset def geodetic_to_ecef(lat, lon, h): diff --git a/code/perception/src/dataset_generator.py b/code/perception/src/dataset_generator.py old mode 100644 new mode 100755 diff --git a/code/perception/src/kalman_filter.py b/code/perception/src/kalman_filter.py new file mode 100755 index 00000000..cc8c175a --- /dev/null +++ b/code/perception/src/kalman_filter.py @@ -0,0 +1,349 @@ +#!/usr/bin/env python + +import numpy as np +import ros_compatibility as roscomp +from ros_compatibility.node import CompatibleNode +from geometry_msgs.msg import PoseStamped +from std_msgs.msg import Float32, UInt32 +from sensor_msgs.msg import NavSatFix, Imu +from tf.transformations import euler_from_quaternion +from carla_msgs.msg import CarlaSpeedometer +import math + +''' +This class implements a Kalman filter for a 3D object tracked in 3D space. +It implements the data of the IMU and the GPS Sensors. +The IMU Sensor provides the acceleration +and the GPS Sensor provides the position. +The Carla Speedometer provides the current Speed in the headed direction. + +The Noise for the GPS Sensor is defined as: + "noise_alt_stddev": 0.000005, + "noise_lat_stddev": 0.000005, + "noise_lon_stddev": 0.000005 +The Noise for the IMU Sensor is defined as: + "noise_accel_stddev_x": 0.001, + "noise_accel_stddev_y": 0.001, + "noise_accel_stddev_z": 0.015, + +The state vector X is defined as: + [initial_x], + [initial_y], + [yaw], + [v_hy] in direction of heading hy, + [a_hy] in direction of v_hy (heading hy), + [omega_z], +The state transition matrix F is defined as: + A = I + I: Identity Matrix +The measurement matrix H is defined as: + H = [1, 0, 0, 0, 0, 0], + [0, 1, 0, 0, 0, 0], + [0, 0, 1, 0, 0, 0], + [0, 0, 0, 0, 0, 1] +The process covariance matrix Q is defined as: + Q = np.diag([0.005, 0.005, 0.001, 0.0001] +''' + + +class KalmanFilter(CompatibleNode): + """ + This class implements a Kalman filter for a 3D object tracked in 3D space. + """ + def __init__(self): + """ + Constructor / Setup + :return: + """ + + super(KalmanFilter, self).__init__('kalman_filter_node') + + self.loginfo('KalmanFilter node started') + # basic info + self.role_name = self.get_param("role_name", "hero") + self.control_loop_rate = self.get_param("control_loop_rate", "0.001") + self.publish_seq = UInt32(0) + self.frame_id = "map" + + self.dt = self.control_loop_rate + + # Initialize the state vector X + ''' + [ + [initial_x], + [initial_y], + [yaw], + [v_hy] in direction of heading hy, + [a_hy] in direction of v_hy (heading hy), + [omega_z], + ] + ''' + self.x0 = np.zeros((6, 1)) + + # Define initial state covariance matrix + self.P0 = np.eye(6) + + # Define state transition matrix + # [x, y, yaw, v_hy, a_hy, omega_z] + # [ ... ] + self.A = np.array([[1, 0, 0, self.dt, 0, 0], + [0, 1, 0, 0, self.dt, 0], + [0, 0, 1, 0, 0, self.dt], + [0, 0, 0, 1, 0, 0], + [0, 0, 0, 0, 1, 0], + [0, 0, 0, 0, 0, 1]]) + + # Define measurement matrix + ''' + 1. GPS: x, y + 2. IMU: yaw, omega_z + -> 4 measurements for a state vector of 6 + (v is not in the measurement here because its provided by Carla) + ''' + self.H = np.array([[1, 0, 0, 0, 0, 0], + [0, 1, 0, 0, 0, 0], + [0, 0, 1, 0, 0, 0], + [0, 0, 0, 0, 0, 1]]) + + # Define process noise covariance matrix + self.Q = np.diag([0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001]) + + # Define measurement noise covariance matrix + self.R = np.diag([0.005, 0.005, 0.001, 0.0001]) + + # Define Measurement Variables + self.z_gps = np.zeros((2, 1)) # GPS measurements (x, y) + self.z_imu = np.zeros((2, 1)) # IMU measurements (yaw, omega_z) + + self.v = np.zeros((2, 1)) # Velocity measurements (v_x, v_y) + + self.x_old_est = np.copy(self.x0) # old state vector + self.P_old_est = np.copy(self.P0) # old state covariance matrix + self.x_est = np.zeros((6, 1)) # estimated state vector + self.P_est = np.zeros((6, 6)) # estiamted state covariance matrix + self.x_pred = np.zeros((6, 1)) # Predicted state vector + self.P_pred = np.zeros((6, 6)) # Predicted state covariance matrix + + self.K = np.zeros((6, 4)) # Kalman gain + + self.latitude = 0 # latitude of the current position + + ''' + # Define control input vector + u = np.array([0, 0, -g, 0, 0, 0]) + + # Define control input matrix + B = np.array([[0, 0, 0], + [0, 0, 0], + [0, 0, 0], + [0, 0, dt], + [0, dt, 0], + [dt, 0, 0]]) + ''' + + # Subscriber + # Initialize the subscriber for the IMU Data + self.imu_subscriber = self.new_subscription( + Imu, + "/carla/" + self.role_name + "/Ideal_IMU", + self.update_imu_data, + qos_profile=1) + # Initialize the subscriber for the GPS Data + self.gps_subscriber = self.new_subscription( + NavSatFix, + "/carla/" + self.role_name + "/GPS", + self.update_gps_data, + qos_profile=1) + # Initialize the subscriber for the current_pos in XYZ + self.current_pos_subscriber = self.new_subscription( + PoseStamped, + "/paf/" + self.role_name + "/current_pos", + self.update_current_pos, + qos_profile=1) + # Initialize the subscriber for the velocity + self.velocity_subscriber = self.new_subscription( + CarlaSpeedometer, + "/carla/" + self.role_name + "/Speed", + self.update_velocity, + qos_profile=1) + + # Publisher + # Initialize the publisher for the kalman-position + self.kalman_position_publisher = self.new_publisher( + PoseStamped, + "/paf/" + self.role_name + "/kalman_pos", + qos_profile=1) + self.kalman_heading_publisher = self.new_publisher( + Float32, + "/paf/" + self.role_name + "/kalman_heading", + qos_profile=1) + + def run(self): + """ + Run the Kalman Filter + """ + def loop(): + """ + Loop for the Kalman Filter + """ + self.predict() + self.update() + + # Publish the kalman-data: + self.publish_kalman_heading() + self.publish_kalman_location() + + # roscomp.spin(loop, self.control_loop_rate) + self.new_timer(self.control_loop_rate, loop) + self.spin() + + def predict(self): + """ + Predict the next state + """ + # Update the old state and covariance matrix + self.x_old_est[:, :] = np.copy(self.x_est[:, :]) + self.P_old_est[:, :] = np.copy(self.P_est[:, :]) + + # Predict the next state and covariance matrix + self.x_pred = self.A @ self.x_est[:] # + B @ v[:, k-1] + u + self.P_pred = self.A @ self.P_est[:, :] @ self.A.T + self.Q + + def update(self): + """ + Update the state + """ + z = np.concatenate((self.z_gps[:], self.z_imu[:])) # Measurementvector + y = z - self.H @ self.x_pred # Measurement residual + S = self.H @ self.P_pred @ self.H.T + self.R # Residual covariance + self.K[:, :] = self.P_pred @ self.H.T @ np.linalg.inv(S) # Kalman gain + self.x_est[:] = self.x_pred + self.K[:, :] @ y # State estimate + # State covariance estimate + self.P_est[:, :] = (np.eye(6) - self.K[:, :] @ self.H) @ self.P_pred + + def publish_kalman_heading(self): + """ + Publish the kalman heading + """ + # Initialize the kalman-heading + kalman_heading = Float32() + + # Fill the kalman-heading + kalman_heading.data = self.x_est[2] + + # Publish the kalman-heading + self.kalman_heading_publisher.publish(kalman_heading) + + def publish_kalman_location(self): + """ + Publish the kalman location + """ + # Initialize the kalman-position + kalman_position = PoseStamped() + + # Fill the kalman-position + kalman_position.header.frame_id = self.frame_id + kalman_position.header.stamp = roscomp.Time.now() + kalman_position.header.seq = self.publish_seq + self.publish_seq.data += 1 + + kalman_position.pose.position.x = self.x_est[0] + kalman_position.pose.position.y = self.x_est[1] + kalman_position.pose.position.z = self.latitude + kalman_position.pose.orientation.x = 0 + kalman_position.pose.orientation.y = 0 + kalman_position.pose.orientation.z = 1 + kalman_position.pose.orientation.w = 0 + + # Publish the kalman-position + self.kalman_position_publisher.publish(kalman_position) + + def update_imu_data(self, imu_data): + """ + Update the IMU Data + """ + orientation_x = imu_data.orientation.x + orientation_y = imu_data.orientation.y + orientation_z = imu_data.orientation.z + orientation_w = imu_data.orientation.w + + # Calculate the heading based on the orientation given by the IMU + data_orientation_q = [orientation_x, + orientation_y, + orientation_z, + orientation_w] + + # Implementation by paf22 in Position_Publisher_Node.py + # TODO: Why were they using roll and pitch instead of yaw? + # Even though they are basically deriving the yaw that way? + roll, pitch, yaw = euler_from_quaternion(data_orientation_q) + raw_heading = math.atan2(roll, pitch) + + # transform raw_heading so that: + # --------------------------------------------------------------- + # | 0 = x-axis | pi/2 = y-axis | pi = -x-axis | -pi/2 = -y-axis | + # --------------------------------------------------------------- + heading = (raw_heading - (math.pi / 2)) % (2 * math.pi) - math.pi + + # update IMU Measurements: + self.z_imu[0] = heading + self.z_imu[1] = imu_data.angular_velocity.z + if imu_data.orientation_covariance[8] != 0: + # [8] because we want the diag z element of the covariance matrix + self.R[2, 2] = imu_data.orientation_covariance[8] + if imu_data.angular_velocity_covariance[8] != 0: + # [8] because we want the diag z element of the covariance matrix + self.R[3, 3] = imu_data.angular_velocity_covariance[8] + + def update_gps_data(self, gps_data): + """ + Update the GPS Data + Currently only used for covariance matrix + """ + # look up if covariance type is not 0 (0 = COVARANCE_TYPE_UNKNOWN) + # (1 = approximated, 2 = diagonal known or 3 = known) + # if it is not 0 -> update the covariance matrix + if gps_data.position_covariance_type != 0: + # [0] because we want the diag lat element of the covariance matrix + self.R[0, 0] = gps_data.pose.covariance[0] + # [5] because we want the diag lon element of the covariance matrix + self.R[1, 1] = gps_data.pose.covariance[5] + pass + + def update_current_pos(self, current_pos): + """ + Update the current position + """ + # update GPS Measurements: + self.z_gps[0] = current_pos.pose.position.x + self.z_gps[1] = current_pos.pose.position.y + + self.latitude = current_pos.pose.position.z + + def update_velocity(self, velocity): + """ + Update the velocity using the yaw angle in the predicted state x1 + x1[2] = yaw + """ + self.v[0] = velocity.speed * math.cos(self.x_est[2]) + self.v[1] = velocity.speed * math.sin(self.x_est[2]) + + +def main(args=None): + """ + Main function starts the node + :param args: + """ + roscomp.init('kalman_filter_node', args=args) + + try: + node = KalmanFilter() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() + + +if __name__ == '__main__': + main() diff --git a/code/perception/src/lidar_filter_utility.py b/code/perception/src/lidar_filter_utility.py old mode 100644 new mode 100755 diff --git a/code/perception/src/sensor_filter_debug.py b/code/perception/src/sensor_filter_debug.py new file mode 100755 index 00000000..6ea53e7a --- /dev/null +++ b/code/perception/src/sensor_filter_debug.py @@ -0,0 +1,390 @@ +#!/usr/bin/env python + +""" +This node publishes all relevant topics for the ekf node. +""" +import math +import numpy as np +import ros_compatibility as roscomp +from ros_compatibility.node import CompatibleNode +from geometry_msgs.msg import PoseStamped +from sensor_msgs.msg import NavSatFix, Imu +from nav_msgs.msg import Odometry +from std_msgs.msg import Float32, String +from coordinate_transformation import CoordinateTransformer +from tf.transformations import euler_from_quaternion +from std_msgs.msg import Float32MultiArray +from xml.etree import ElementTree as eTree +# import carl____a remove ___ to use +# import rospy + +GPS_RUNNING_AVG_ARGS: int = 10 + + +class SensorFilterDebugNode(CompatibleNode): + """ + Node publishes a filtered gps signal. + This is achieved using a rolling average. + """ + def __init__(self): + """ + Constructor / Setup + :return: + """ + + super(SensorFilterDebugNode, self).__init__('ekf_translation') + # self.current_pos = PoseStamped() + self.ideal_current_pos = PoseStamped() + self.carla_current_pos = PoseStamped() + self.ideal_heading = Float32() + + self.loginfo("Position publisher node started") + + # basic info + self.role_name = self.get_param("role_name", "hero") + self.control_loop_rate = self.get_param("control_loop_rate", "0.05") + + # todo: automatically detect town + self.transformer = None + + # remove comments to use carla + # Carla API hero car position + # Get parameters from the launch file + # host = rospy.get_param('~host', 'carla-simulator') + # port = rospy.get_param('~port', 2000) + # timeout = rospy.get_param('~timeout', 100.0) + + # Connect to the CARLA server + # client = carl___a.Client(host, port) + # client.set_timeout(timeout) + + # Get the world + # self.world = client.get_world() + + # Get the ego vehicle + self.vehicle = None + + # Subscriber START + self.map_sub = self.new_subscription( + String, + "/carla/" + self.role_name + "/OpenDRIVE", + self.get_geoRef, + qos_profile=1) + + self.imu_subscriber = self.new_subscription( + Imu, + "/carla/" + self.role_name + "/Ideal_IMU", + self.update_imu_data, + qos_profile=1) + + self.gps_subscriber = self.new_subscription( + NavSatFix, + "/carla/" + self.role_name + "/Ideal_GPS", + self.update_gps_data, + qos_profile=1) + + # Current_pos subscriber: + self.current_pos_subscriber = self.new_subscription( + PoseStamped, + f"/paf/{self.role_name}/current_pos", + self.update_location_error, + qos_profile=1) + + # Current_heading subscriber: + self.current_heading_subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/current_heading", + self.update_heading_error, + qos_profile=1) + # Subscriber END + + # Publisher START + # 2D Odometry (Maybe Speedometer?) + self.ekf_odom_publisher = self.new_publisher( + Odometry, + "/ideal_odom", + qos_profile=1) + + # IMU + self.ekf_imu_publisher = self.new_publisher( + Imu, + "/ideal_imu_data", + qos_profile=1) + + self.avg_xyz = np.zeros((GPS_RUNNING_AVG_ARGS, 3)) + self.avg_gps_counter: int = 0 + # 3D Odometry (GPS) + self.cur_pos_publisher = self.new_publisher( + PoseStamped, + f"/paf/{self.role_name}/ideal_current_pos", + qos_profile=1) + + self.__heading: float = 0 + self.__heading_publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/ideal_current_heading", + qos_profile=1) + + # Publish the carla location + self.carla_pos_publisher = self.new_publisher( + PoseStamped, + f"/paf/{self.role_name}/carla_current_pos", + qos_profile=1) + + # Error Publisher + """publish error distance between current_pos and ideal_corrent_pos & + current_pos and carla_current_pos: + # current_pos and ideal_corrent_pos in location_error[0] + # current_pos and carla_current_pos in location_error[1] + """ + self.location_error_publisher = self.new_publisher( + Float32MultiArray, + f"/paf/{self.role_name}/location_error", + qos_profile=1) + + # publish the error between current_heading and ideal_heading + self.heading_error_publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/heading_error", + qos_profile=1) + + # Publish x and y coordinates of ideal_GPS and carla_pos + self.ideal_x_publisher = self.new_publisher( + Float32MultiArray, + f"/paf/{self.role_name}/ideal_x", + qos_profile=1) + self.ideal_y_publisher = self.new_publisher( + Float32MultiArray, + f"/paf/{self.role_name}/ideal_y", + qos_profile=1) + # Publisher END + + def update_heading_error(self, data: Float32): + """ + This method is called when new current_heading data is received. + """ + current_heading = data.data + + # calculate the error between ideal_imu and imu + heading_error = self.ideal_heading.data - current_heading + self.heading_error_publisher.publish(heading_error) + + def update_location_error(self, data: PoseStamped): + """ + This method is called when new current_pos data is received. + It handles all necessary updates and publishes the error. + :param data: new current_pos measurement + :return: + """ + + error = Float32MultiArray() + + error.data = [0, 0, 0] + # calculate the error between ideal_current_pos and current_pos + error.data[0] = math.sqrt(( + self.ideal_current_pos.pose.position.x - data.pose.position.x)**2 + + (self.ideal_current_pos.pose.position.y - data.pose.position.y)**2) + # calculate the error between carla_current_pos and current_pos + error.data[1] = math.sqrt(( + self.carla_current_pos.pose.position.x - data.pose.position.x)**2 + + (self.carla_current_pos.pose.position.y - data.pose.position.y)**2) + + self.location_error_publisher.publish(error) + + def get_geoRef(self, opendrive: String): + """_summary_ + Reads the reference values for lat and lon from the carla OpenDriveMap + Args: + opendrive (String): OpenDrive Map from carla + """ + root = eTree.fromstring(opendrive.data) + header = root.find("header") + geoRefText = header.find("geoReference").text + + latString = "+lat_0=" + lonString = "+lon_0=" + + indexLat = geoRefText.find(latString) + indexLon = geoRefText.find(lonString) + + indexLatEnd = geoRefText.find(" ", indexLat) + indexLonEnd = geoRefText.find(" ", indexLon) + + latValue = float(geoRefText[indexLat + len(latString):indexLatEnd]) + lonValue = float(geoRefText[indexLon + len(lonString):indexLonEnd]) + + CoordinateTransformer.la_ref = latValue + CoordinateTransformer.ln_ref = lonValue + CoordinateTransformer.ref_set = True + self.transformer = CoordinateTransformer() + + def update_gps_data(self, data: NavSatFix): + """ + This method is called when new GNSS data is received. + The function calculates the average position and then publishes it. + Measurements are also transformed to global xyz-coordinates + :param data: GNSS measurement + :return: + """ + lat = data.latitude + lon = data.longitude + alt = data.altitude + + if self.transformer is None: + self.transformer = CoordinateTransformer() + x, y, z = self.transformer.gnss_to_xyz(lat, lon, alt) + # find reason for discrepancy + # x *= 0.998 + # y *= 1.003 + + # self.avg_xyz = np.roll(self.avg_xyz, -1, axis=0) + # self.avg_xyz[-1] = np.matrix([x, y, z]) + + # avg_x, avg_y, avg_z = np.mean(self.avg_xyz, axis=0) + + cur_pos = PoseStamped() + + cur_pos.header.stamp = data.header.stamp + cur_pos.header.frame_id = "global" + + cur_pos.pose.position.x = x + cur_pos.pose.position.y = y + cur_pos.pose.position.z = z + + cur_pos.pose.orientation.x = 0 + cur_pos.pose.orientation.y = 0 + cur_pos.pose.orientation.z = 1 + cur_pos.pose.orientation.w = 0 + + self.cur_pos_publisher.publish(cur_pos) + self.ideal_current_pos = cur_pos + + # also update carla_car_position: + if self.vehicle is None: + for actor in self.world.get_actors(): + if actor.attributes.get('role_name') == self.role_name: + self.vehicle = actor + break + + carla_pos = PoseStamped() + carla_pos.header.stamp = data.header.stamp + carla_pos.header.frame_id = "global" + + pos = self.vehicle.get_location() + carla_pos.pose.position.x = pos.x + carla_pos.pose.position.y = -pos.y + carla_pos.pose.position.z = pos.z + + carla_pos.pose.orientation.x = 0 + carla_pos.pose.orientation.y = 0 + carla_pos.pose.orientation.z = 1 + carla_pos.pose.orientation.w = 0 + + self.carla_pos_publisher.publish(carla_pos) + self.carla_current_pos = carla_pos + + # get x and y coordinates of ideal_GPS and carla_pos + # publish errors between ideal_x and carla_pos.x + # and ideal_y and carla_pos.y + ideal_x = Float32MultiArray() + ideal_y = Float32MultiArray() + x_error = ( + self.ideal_current_pos.pose.position.x + - self.carla_current_pos.pose.position.x + ) + y_error = ( + self.ideal_current_pos.pose.position.y + - self.carla_current_pos.pose.position.y + ) + ideal_x.data = [self.ideal_current_pos.pose.position.x, + self.carla_current_pos.pose.position.x, + x_error] + ideal_y.data = [self.ideal_current_pos.pose.position.y, + self.carla_current_pos.pose.position.y, + y_error] + + self.ideal_x_publisher.publish(ideal_x) + self.ideal_y_publisher.publish(ideal_y) + + def update_imu_data(self, data: Imu): + """ + This method is called when new IMU data is received. + The function calculates the average position and then publishes it. + :param data: IMU measurement + :return: + """ + imu_data = Imu() + + imu_data.header.stamp = data.header.stamp + imu_data.header.frame_id = "hero" + + imu_data.orientation.x = data.orientation.x + imu_data.orientation.y = data.orientation.y + imu_data.orientation.z = data.orientation.z + imu_data.orientation.w = data.orientation.w + imu_data.orientation_covariance = [0, 0, 0, + 0, 0, 0, + 0, 0, 0] + + imu_data.angular_velocity.x = data.angular_velocity.x + imu_data.angular_velocity.y = data.angular_velocity.y + imu_data.angular_velocity.z = data.angular_velocity.z + imu_data.angular_velocity_covariance = [0, 0, 0, + 0, 0, 0, + 0, 0, 0] + + imu_data.linear_acceleration.x = data.linear_acceleration.x + imu_data.linear_acceleration.y = data.linear_acceleration.y + imu_data.linear_acceleration.z = data.linear_acceleration.z + imu_data.linear_acceleration_covariance = [0, 0, 0, + 0, 0, 0, + 0, 0, 0] + + self.ekf_imu_publisher.publish(imu_data) + + # Calculate the heading based on the orientation given by the IMU + data_orientation_q = [data.orientation.x, + data.orientation.y, + data.orientation.z, + data.orientation.w] + + roll, pitch, yaw = euler_from_quaternion(data_orientation_q) + raw_heading = math.atan2(roll, pitch) + + # transform raw_heading so that: + # --------------------------------------------------------------- + # | 0 = x-axis | pi/2 = y-axis | pi = -x-axis | -pi/2 = -y-axis | + # --------------------------------------------------------------- + heading = (raw_heading - (math.pi / 2)) % (2 * math.pi) - math.pi + self.__heading = heading + self.__heading_publisher.publish(self.__heading) + + self.ideal_heading = Float32(heading) + + def run(self): + """ + Control loop + :return: + """ + self.spin() + + +def main(args=None): + """ + main function + :param args: + :return: + """ + + roscomp.init("position_publisher_node_2", args=args) + try: + node = SensorFilterDebugNode() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() + + +if __name__ == "__main__": + main() diff --git a/code/planning/global_planner/src/global_planner.py b/code/planning/global_planner/src/global_planner.py index d5926dc5..fdc542d6 100755 --- a/code/planning/global_planner/src/global_planner.py +++ b/code/planning/global_planner/src/global_planner.py @@ -187,7 +187,6 @@ def global_route_callback(self, data: CarlaRoute) -> None: self.path_backup.header.frame_id = "global" self.path_backup.poses = stamped_poses self.path_pub.publish(self.path_backup) - self.loginfo("PrePlanner: published trajectory") # def world_info_callback(self, data: CarlaWorldInfo) -> None: diff --git a/doc/00_assets/gnss_ohne_rolling_average.png b/doc/00_assets/gnss_ohne_rolling_average.png new file mode 100644 index 00000000..68fb5ebf Binary files /dev/null and b/doc/00_assets/gnss_ohne_rolling_average.png differ diff --git a/doc/03_research/03_planning/00_paf23/BT_paper.png b/doc/03_research/03_planning/00_paf23/BT_paper.png new file mode 100644 index 00000000..0a711e41 Binary files /dev/null and b/doc/03_research/03_planning/00_paf23/BT_paper.png differ diff --git a/doc/03_research/03_planning/00_paf23/BehaviorTree_medium.png b/doc/03_research/03_planning/00_paf23/BehaviorTree_medium.png new file mode 100644 index 00000000..edc11092 Binary files /dev/null and b/doc/03_research/03_planning/00_paf23/BehaviorTree_medium.png differ diff --git a/doc/03_research/03_planning/00_paf23/Local_planning_for_first_milestone.md b/doc/03_research/03_planning/00_paf23/Local_planning_for_first_milestone.md new file mode 100644 index 00000000..e47f05eb --- /dev/null +++ b/doc/03_research/03_planning/00_paf23/Local_planning_for_first_milestone.md @@ -0,0 +1,104 @@ +# Local Planning for first milestone + +**Summary:** This document states the implementation plan for the local planning. + +--- + +## Author + +Julius Miller + +## Date + +03.12.2023 + +## Research + +Paper: [Behavior Planning for Autonomous Driving: Methodologies, Applications, and Future Orientation](https://www.researchgate.net/publication/369181112_Behavior_Planning_for_Autonomous_Driving_Methodologies_Applications_and_Future_Orientation) + +![Overview_interfaces](overview_paper1.png) + +Rule-based planning + +Advantages: + +- Simple implementation. +- Low computational +power. +- Real-time operation. +- Adapt the rationality of +human thinking. +- Its behavior can be easily +traced and explained + +Disadvantages: + +- Inability to handle +complex environments. +- Risk of rules explosion. +- Inability to handle +uncertainty. +- Low ability to handle +unplanned situations + +Paper: [A Rule-Based Behaviour Planner for Autonomous Driving , pp 263 -279](https://link.springer.com/chapter/10.1007/978-3-031-21541-4_17) + +- Two-layer rule-based theory +- Behaviours: Emergency-Stop, Stop, Yield, Decelerate-To-Halt, Pass-Obstacle, Follow- +Leader, Track-Speed + +Github: [Decision Making with Behaviour Tree](https://github.com/kirilcvetkov92/Path-planning?source=post_page-----8db1575fec2c--------------------------------) + +![github_tree](BehaviorTree_medium.png) + +- No Intersection +- Collision Detection in behaviour Tree + +Paper: [Behavior Trees for +decision-making in Autonomous +Driving](https://www.diva-portal.org/smash/get/diva2:907048/FULLTEXT01.pdf) + +![Behaviour Tree](BT_paper.png) + +- simple simulation +- Car only drives straight + +## New Architecture for first milestone + +- Keeping it simple +- Iterative Progress +- Divide decisions into high level and low level to keep behaviour tree small. + +High Level Decisions: + +- Intersection +- Lane Change +- Cruise (NoOp) +- (Overtake - limit for multilane) + +Low Level Decision: + +- Emergency Brake +- ACC + +![localplan](localplan.png) + +Scenarios: + +![Intersection](intersection_scenario.png) + +Left: Behaviour Intersection is triggered for motion planning, acc publishes speed. -> Lower speed is used to approach intersection + +Right: Behaviour Intersection is used for motion planning, acc is ignored (no object in front) + +![Overtake](overtaking_scenario.png) + +Left: Overtake gets triggered to maintain speed, acc is ignored + +Right: Overtake not possible, acc reduces speed to avoid collision + +What needs to be done: + +- Implement ACC +- Implement motion planning +- Change publishers in behaviours (only publish name of task) diff --git a/doc/03_research/03_planning/00_paf23/intersection_scenario.png b/doc/03_research/03_planning/00_paf23/intersection_scenario.png new file mode 100644 index 00000000..a1250eeb Binary files /dev/null and b/doc/03_research/03_planning/00_paf23/intersection_scenario.png differ diff --git a/doc/03_research/03_planning/00_paf23/localplan.png b/doc/03_research/03_planning/00_paf23/localplan.png index 14424349..3f288dfe 100644 Binary files a/doc/03_research/03_planning/00_paf23/localplan.png and b/doc/03_research/03_planning/00_paf23/localplan.png differ diff --git a/doc/03_research/03_planning/00_paf23/overtaking_scenario.png b/doc/03_research/03_planning/00_paf23/overtaking_scenario.png new file mode 100644 index 00000000..8707de3e Binary files /dev/null and b/doc/03_research/03_planning/00_paf23/overtaking_scenario.png differ diff --git a/doc/03_research/03_planning/00_paf23/overview_paper1.png b/doc/03_research/03_planning/00_paf23/overview_paper1.png new file mode 100644 index 00000000..d189a78b Binary files /dev/null and b/doc/03_research/03_planning/00_paf23/overview_paper1.png differ diff --git a/doc/06_perception/10_sensor_filter_debug.md b/doc/06_perception/10_sensor_filter_debug.md new file mode 100644 index 00000000..83307e48 --- /dev/null +++ b/doc/06_perception/10_sensor_filter_debug.md @@ -0,0 +1,147 @@ +# sensor_filter_debug.py + +**Summary:** [sensor_filter_debug.py](.../code/perception/src/sensor_filter_debug.py): + +The sensor_filter_debug node is responsible for collecting sensor data from the IMU and GNSS and process the data in such a way, that it shows the errors between the real is-state and the measured state. +The data is the shown in multiple rqt_plots. + +--- + +## Author + +Robert Fischer + +## Date + +03.12.2023 + +## Prerequisite + +--- + +- [sensor\_filter\_debug.py](#sensor_filter_debugpy) + - [Author](#author) + - [Date](#date) + - [Prerequisite](#prerequisite) + - [Getting started](#getting-started) + - [Description](#description) + - [Inputs](#inputs) + - [Outputs](#outputs) + + +--- + +## Getting started + +Uncomment the sensor_filter_debug.py node in the [perception.launch](.../code/perception/launch/perception.launch) to start the node. +You can also uncomment the rqt_plots that seem useful to you, or create your own ones from the data published. +You have to add the following sensors to the sensors inside the [dev_objects.json](.../code/agent/config/dev_objects.json): + +```json +{ + "type": "sensor.other.gnss", + "id": "Ideal_GPS", + "spawn_point": { + "x": 0.0, + "y": 0.0, + "z": 1.60 + }, + "noise_alt_stddev": 0.0, + "noise_lat_stddev": 0.0, + "noise_lon_stddev": 0.0, + "noise_alt_bias": 0.0, + "noise_lat_bias": 0.0, + "noise_lon_bias": 0.0 + }, + { + "type": "sensor.other.imu", + "id": "Ideal_IMU", + "spawn_point": { + "x": 0.7, + "y": 0.4, + "z": 1.60, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0 + }, + "noise_accel_stddev_x": 0.0, + "noise_accel_stddev_y": 0.0, + "noise_accel_stddev_z": 0.0, + "noise_gyro_stddev_x": 0.0, + "noise_gyro_stddev_y": 0.0, + "noise_gyro_stddev_z": 0.0 + }, + { + "type": "sensor.other.radar", + "id": "Ideal_RADAR", + "spawn_point": { + "x": 0.7, + "y": 0.4, + "z": 1.60, + "roll": 0.0, + "pitch": 0.0, + "yaw": 45.0 + }, + "horizontal_fov": 30.0, + "vertical_fov": 30.0, + "points_per_second": 1500, + "range": 100.0 + } +``` + +You also have to launch it in dev mode, otherwise the added ideal sensors won't work. +No extra installation needed. + +--- + +## Description + +Running the node provides you with ideal Sensor topics that can be used to debug your sensor filters by giving you ideal values you should aim for. +Right now only the IMU and the GNSS sensor are available for debug. +Debug for the RADAR and LIDAR hasn't been implemented yet. + +An Example of Location Error Output can be seen here: +![Distance from current_pos to ideal_gps_pos (blue) and to carla_pos (red)](.../doc/00_assets/gnss_ohne_rolling_average.png) + +### Inputs + +This node subscribes to the following needed topics: + +- OpenDrive Map: + - `/carla/{role_name}/OpenDRIVE` ([String](http://docs.ros.org/en/melodic/api/std_msgs/html/msg/String.html)) or `/carla/world_info` ([CarlaWorldInfo](https://carla.readthedocs.io/projects/ros-bridge/en/latest/ros_msgs/#carlaworldinfomsg)) +- Ideal_IMU: + - `/carla/{role_name}/Ideal_IMU` ([IMU](https://docs.ros.org/en/api/sensor_msgs/html/msg/Imu.html)) +- Ideal_GPS: + - `/carla/{role_name}/Ideal_GPS` ([NavSatFix](http://docs.ros.org/en/melodic/api/std_msgs/html/msg/String.html)) +- current agent position: + - `/paf/{role_name}/current_pos` ([PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)) +- current agent heading: + - `/paf/{role_name}/current_heading` ([Float32](https://docs.ros.org/en/api/std_msgs/html/msg/Float32.html)) +- [Carla_API](https://carla.readthedocs.io/en/latest/python_api/) + - `get_location` + +### Outputs + +This node publishes the following topics: + +- Ideal_IMU: + - `/carla/{role_name}/Ideal_IMU` ([IMU](https://docs.ros.org/en/api/sensor_msgs/html/msg/Imu.html)) +- Ideal_GPS: + - `/carla/{role_name}/Ideal_GPS` ([NavSatFix](http://docs.ros.org/en/melodic/api/std_msgs/html/msg/String.html)) +- Ideal Odometry: + - `/ideal_odom` ([Odometry](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html)) +- Ideal current agent position (calculated from ideal GPS): + - `/paf/{role_name}/idealcurrent_pos` ([PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)) +- Ideal current agent heading (calculated from ideal IMU): + - `/paf/{role_name}/current_heading` ([PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)) +- Carla Position: + - `/paf/{self.role_name}/carla_current_pos` ([PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)) +- Location Error: + - `/paf/{self.role_name}/location_error` ([Float32MultiArray](http://docs.ros.org/en/melodic/api/std_msgs/html/msg/Float32MultiArray.html)) +- Heading Error: + - `/paf/{self.role_name}/heading_error` ([Float32](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)) +- Ideal Coordinate Publisher (used to debug [#105](https://github.com/una-auxme/paf23/issues/105)) + - Ideal X Publisher: + - `/paf/{self.role_name}/ideal_x`([Float32MultiArray](http://docs.ros.org/en/melodic/api/std_msgs/html/msg/Float32MultiArray.html)) + - Ideal Y Publisher: + - `/paf/{self.role_name}/ideal_y`([Float32MultiArray](http://docs.ros.org/en/melodic/api/std_msgs/html/msg/Float32MultiArray.html)) diff --git a/doc/06_perception/11_kalman_filter.md b/doc/06_perception/11_kalman_filter.md new file mode 100644 index 00000000..8eadb14b --- /dev/null +++ b/doc/06_perception/11_kalman_filter.md @@ -0,0 +1,143 @@ +# Kalman Filter + +**Summary:** [kalman_filter.py](.../code/perception/src/kalman_filter.py): + +The Kalman Filter node is responsible for filtering the location and heading data, by using an IMU and GNSS sensor together with the carla speedometer. + +--- + +## Author + +Robert Fischer + +## Date + +03.12.2023 + +## Prerequisite + +--- + +- [Kalman Filter](#kalman-filter) + - [Author](#author) + - [Date](#date) + - [Prerequisite](#prerequisite) + - [Getting started](#getting-started) + - [Description](#description) + - [1. Predict](#1-predict) + - [2. Update](#2-update) + - [3. Publish Data](#3-publish-data) + - [Inputs](#inputs) + - [Outputs](#outputs) + + +--- + +## Getting started + +Right now the Node does not work correctly. It creates topics to publish to, but doesn't yet. +This will be fixed in [#106](https://github.com/una-auxme/paf23/issues/106) + +Uncomment the kalman_filter.py node in the [perception.launch](.../code/perception/launch/perception.launch) to start the node. +You can also uncomment the rqt_plots that seem useful to you. +No extra installation needed. + +--- + +## Description + +Sources to understand the topic better: + +[Visally Explained Kalman Filters](https://www.youtube.com/watch?v=IFeCIbljreY&ab_channel=VisuallyExplained) + +[Understand & Code Kalman Filters](https://www.youtube.com/watch?v=TEKPcyBwEH8&ab_channel=CppMonk) + +Stackoverflow and other useful sites: + +[1](https://stackoverflow.com/questions/47210512/using-pykalman-on-raw-acceleration-data-to-calculate-position), +[2](https://robotics.stackexchange.com/questions/11178/kalman-filter-gps-imu), +[3](https://stackoverflow.com/questions/66167733/getting-3d-position-coordinates-from-an-imu-sensor-on-python), +[4](https://github.com/Janudis/Extended-Kalman-Filter-GPS_IMU) + +This script implements a Kalman Filter. It is a recursive algorithm used to estimate the state of a system that can be modeled with linear equations. +This Kalman Filter uses the location provided by a GNSS sensor (by using the current_pos provided by the [Position Publisher Node](.../code/perception/src/Position_Publisher_Node.py)) +the orientation and angular velocity provided by the IMU sensor and the current speed in the headed direction by the Carla Speedometer. + +```Python +The state vector X is defined as: + [initial_x], + [initial_y], + [yaw], + [v_hy] in direction of heading hy, + [a_hy] in direction of v_hy (heading hy), + [omega_z], +The state transition matrix F is defined as: + A = I + I: Identity Matrix +The measurement matrix H is defined as: + H = [1, 0, 0, 0, 0, 0], + [0, 1, 0, 0, 0, 0], + [0, 0, 1, 0, 0, 0], + [0, 0, 0, 0, 0, 1] +The process covariance matrix Q is defined as: + Q = np.diag([0.005, 0.005, 0.001, 0.0001]) +``` + +Then 3 Steps are run in the frequency of the `control_loop_rate`: + +### 1. Predict + +```Python + # Update the old state and covariance matrix + self.x_old_est[:, :] = np.copy(self.x_est[:, :]) + self.P_old_est[:, :] = np.copy(self.P_est[:, :]) + + # Predict the next state and covariance matrix + self.x_pred = self.A @ self.x_est[:] # + B @ v[:, k-1] + u + self.P_pred = self.A @ self.P_est[:, :] @ self.A.T + self.Q +``` + +### 2. Update + +```Python + z = np.concatenate((self.z_gps[:], self.z_imu[:])) # Measurementvector + y = z - self.H @ self.x_pred # Measurement residual + S = self.H @ self.P_pred @ self.H.T + self.R # Residual covariance + self.K[:, :] = self.P_pred @ self.H.T @ np.linalg.inv(S) # Kalman gain + self.x_est[:] = self.x_pred + self.K[:, :] @ y # State estimate + # State covariance estimate + self.P_est[:, :] = (np.eye(6) - self.K[:, :] @ self.H) @ self.P_pred +``` + +### 3. Publish Data + +```Python + # Publish the kalman-data: + self.publish_kalman_heading() + self.publish_kalman_location() +``` + +As stated before, the script does not publish viable data yet, which has to be fixed. +This means the predict and update steps might not be correct, because it wasn't possible to test it yet. + +### Inputs + +This node subscribes to the following needed topics: + +- IMU: + - `/carla/{role_name}/Ideal_IMU` ([IMU](https://docs.ros.org/en/api/sensor_msgs/html/msg/Imu.html)) +- GPS: + - `/carla/{role_name}/Ideal_GPS` ([NavSatFix](http://docs.ros.org/en/melodic/api/std_msgs/html/msg/String.html)) +- current agent position: + - `/paf/{role_name}/current_pos` ([PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)) +- Carla Speed: + - `/carla/" + self.role_name + "/Speed` CarlaSpeedometer + +### Outputs + +This node publishes the following topics: + +- Kalman Heading: + - `/paf/{role_name}/kalman_heading` ([PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)) +- Kalman Position: + - `/paf/{self.role_name}/kalman_pos` ([PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)) diff --git a/doc/06_perception/Readme.md b/doc/06_perception/Readme.md index 13219e73..f92978c3 100644 --- a/doc/06_perception/Readme.md +++ b/doc/06_perception/Readme.md @@ -3,3 +3,7 @@ This folder contains further documentation of the perception components. ## 1. [Dataset generator](./01_dataset_generator.md) + +## 10. [sensor_filter_debug.py](./10_sensor_filter_debug.md) + +## 11. [Kalman Filter](./11_kalman_filter.md)