diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp
index e3aff25310..324be7357f 100644
--- a/src/lib/collision_prevention/CollisionPrevention.cpp
+++ b/src/lib/collision_prevention/CollisionPrevention.cpp
@@ -151,7 +151,7 @@ void CollisionPrevention::_updateObstacleMap()
 	}
 
 	// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
-	_obstacle_distance_pub.publish(_obstacle_map_body_frame);
+	_obstacle_distance_fused_pub.publish(_obstacle_map_body_frame);
 }
 
 void CollisionPrevention::_updateObstacleData()
@@ -228,27 +228,51 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel
 	}
 }
 
+// TODO this gives false output if the offset is not a multiple of the resolution. to be fixed...
 void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw)
 {
-	int msg_index = 0;
+
 	float vehicle_orientation_deg = math::degrees(vehicle_yaw);
-	float increment_factor = 1.f / obstacle.increment;
+
 
 	if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) {
 		// Obstacle message arrives in local_origin frame (north aligned)
 		// corresponding data index (convert to world frame and shift by msg offset)
 		for (int i = 0; i < BIN_COUNT; i++) {
-			float bin_angle_deg = (float)i * BIN_SIZE + _obstacle_map_body_frame.angle_offset;
-			msg_index = ceil(_wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor);
+			for (int j = 0; j < 360 / obstacle.increment; j++) {
+				float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
+								  - (float)_obstacle_map_body_frame.increment / 2.f);
+				float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
+								  + (float)_obstacle_map_body_frame.increment / 2.f);
+				float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg -
+								  obstacle.increment / 2.f);
+				float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg +
+								  obstacle.increment / 2.f);
 
-			//add all data points inside to FOV
-			if (obstacle.distances[msg_index] != UINT16_MAX) {
-				if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) {
-					_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
-					_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
-					_data_maxranges[i] = obstacle.max_distance;
-					_data_fov[i] = 1;
+				// if a bin stretches over the 0/360 degree line, adjust the angles
+				if (bin_lower_angle > bin_upper_angle) {
+					bin_lower_angle -= 360;
 				}
+
+				if (msg_lower_angle > msg_upper_angle) {
+					msg_lower_angle -= 360;
+				}
+
+				// Check for overlaps.
+				if ((msg_lower_angle > bin_lower_angle && msg_lower_angle < bin_upper_angle) ||
+				    (msg_upper_angle > bin_lower_angle && msg_upper_angle < bin_upper_angle) ||
+				    (msg_lower_angle <= bin_lower_angle && msg_upper_angle >= bin_upper_angle) ||
+				    (msg_lower_angle >= bin_lower_angle && msg_upper_angle <= bin_upper_angle)) {
+					if (obstacle.distances[j] != UINT16_MAX) {
+						if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[j] * 0.01f)) {
+							_obstacle_map_body_frame.distances[i] = obstacle.distances[j];
+							_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
+							_data_maxranges[i] = obstacle.max_distance;
+							_data_fov[i] = 1;
+						}
+					}
+				}
+
 			}
 		}
 
@@ -256,18 +280,39 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst
 		// Obstacle message arrives in body frame (front aligned)
 		// corresponding data index (shift by msg offset)
 		for (int i = 0; i < BIN_COUNT; i++) {
-			float bin_angle_deg = (float)i * BIN_SIZE +
-					      _obstacle_map_body_frame.angle_offset;
-			msg_index = ceil(_wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor);
+			for (int j = 0; j < 360 / obstacle.increment; j++) {
+				float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
+								  - (float)_obstacle_map_body_frame.increment / 2.f);
+				float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
+								  + (float)_obstacle_map_body_frame.increment / 2.f);
+				float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - obstacle.increment / 2.f);
+				float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset + obstacle.increment / 2.f);
 
-			//add all data points inside to FOV
-			if (obstacle.distances[msg_index] != UINT16_MAX) {
-				if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) {
-					_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
-					_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
-					_data_maxranges[i] = obstacle.max_distance;
-					_data_fov[i] = 1;
+				// if a bin stretches over the 0/360 degree line, adjust the angles
+				if (bin_lower_angle > bin_upper_angle) {
+					bin_lower_angle -= 360;
 				}
+
+				if (msg_lower_angle > msg_upper_angle) {
+					msg_lower_angle -= 360;
+				}
+
+				// Check for overlaps.
+				if ((msg_lower_angle > bin_lower_angle && msg_lower_angle < bin_upper_angle) ||
+				    (msg_upper_angle > bin_lower_angle && msg_upper_angle < bin_upper_angle) ||
+				    (msg_lower_angle <= bin_lower_angle && msg_upper_angle >= bin_upper_angle) ||
+				    (msg_lower_angle >= bin_lower_angle && msg_upper_angle <= bin_upper_angle)) {
+					if (obstacle.distances[j] != UINT16_MAX) {
+
+						if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[j] * 0.01f)) {
+							_obstacle_map_body_frame.distances[i] = obstacle.distances[j];
+							_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
+							_data_maxranges[i] = obstacle.max_distance;
+							_data_fov[i] = 1;
+						}
+					}
+				}
+
 			}
 		}
 
@@ -357,18 +402,14 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
 		float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad));
 
 		// calculate the field of view boundary bin indices
-		int lower_bound = (int)floor((sensor_yaw_body_deg  - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
-		int upper_bound = (int)floor((sensor_yaw_body_deg  + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
+		int lower_bound = (int)round((sensor_yaw_body_deg  - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
+		int upper_bound = (int)round((sensor_yaw_body_deg  + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
 
-		// floor values above zero, ceil values below zero
-		if (lower_bound < 0) { lower_bound++; }
-
-		if (upper_bound < 0) { upper_bound++; }
 
 		// rotate vehicle attitude into the sensor body frame
 		Quatf attitude_sensor_frame = vehicle_attitude;
 		attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad));
-		float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta());
+		float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta()); // verify
 
 		if (distance_reading < distance_sensor.max_distance) {
 			distance_reading = distance_reading * sensor_dist_scale;
diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp
index 0023f7716c..80500ecc34 100644
--- a/src/lib/collision_prevention/CollisionPrevention.hpp
+++ b/src/lib/collision_prevention/CollisionPrevention.hpp
@@ -80,6 +80,9 @@ public:
 	 */
 	void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel);
 
+	static constexpr int BIN_COUNT = 72;
+	static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly
+
 protected:
 	/** Aggregates the sensor data into an internal obstacle map in body frame */
 	void _updateObstacleMap();
@@ -90,9 +93,6 @@ protected:
 	/** Calculate the constrained setpoint considering the current obstacle distances, acceleration setpoint and velocity setpoint */
 	void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel);
 
-	static constexpr int BIN_COUNT = 36;
-	static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly
-
 	obstacle_distance_s _obstacle_map_body_frame{};
 	bool _data_fov[BIN_COUNT] {};
 	uint64_t _data_timestamps[BIN_COUNT] {};
@@ -179,7 +179,7 @@ private:
 	float _vehicle_yaw{0.f};
 
 	uORB::Publication<collision_constraints_s>	_constraints_pub{ORB_ID(collision_constraints)};		/**< constraints publication */
-	uORB::Publication<obstacle_distance_s>		_obstacle_distance_pub{ORB_ID(obstacle_distance_fused)};	/**< obstacle_distance publication */
+	uORB::Publication<obstacle_distance_s>		_obstacle_distance_fused_pub{ORB_ID(obstacle_distance_fused)};	/**< obstacle_distance publication */
 	uORB::Publication<vehicle_command_s>	_vehicle_command_pub{ORB_ID(vehicle_command)};			/**< vehicle command do publication */
 
 	uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */
diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp
index 955293dad5..aa98ef821d 100644
--- a/src/lib/collision_prevention/CollisionPreventionTest.cpp
+++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp
@@ -38,6 +38,8 @@ using namespace matrix;
 
 // to run: make tests TESTFILTER=CollisionPrevention
 hrt_abstime mocked_time = 0;
+const uint bin_size = CollisionPrevention::BIN_SIZE;
+const uint bin_count = CollisionPrevention::BIN_COUNT;
 
 class CollisionPreventionTest : public ::testing::Test
 {
@@ -246,6 +248,7 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage)
 
 	EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100);
 	EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000);
+
 	EXPECT_GT(0.f, modified_setpoint1(0)) << modified_setpoint1(0);
 	EXPECT_EQ(0.f, fabsf(modified_setpoint1(1))) << modified_setpoint1(1);
 	EXPECT_GT(0.f, modified_setpoint2(0))  << original_setpoint2(0);
@@ -609,14 +612,14 @@ TEST_F(CollisionPreventionTest, jerkLimit)
 	Vector2f modified_setpoint_limited_jerk = original_setpoint;
 	cp.modifySetpoint(modified_setpoint_limited_jerk, curr_vel);
 
-	// THEN: the new setpoint should be much slower than the one with default jerk
-	EXPECT_LT(modified_setpoint_limited_jerk.norm() * 10, modified_setpoint_default_jerk.norm());
+	// THEN: the new setpoint should be much higher than the one with default jerk, as the rate of change in acceleration is more limmited
+	EXPECT_GT(modified_setpoint_limited_jerk.norm(), modified_setpoint_default_jerk.norm());
+
 }
 TEST_F(CollisionPreventionTest, addOutOfRangeDistanceSensorData)
 {
 	// GIVEN: a vehicle attitude and a distance sensor message
 	TestCollisionPrevention cp;
-	cp.getObstacleMap().increment = 10.f;
 	Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
 	distance_sensor_s distance_sensor {};
 	distance_sensor.min_distance = 0.2f;
@@ -641,11 +644,65 @@ TEST_F(CollisionPreventionTest, addOutOfRangeDistanceSensorData)
 	}
 }
 
+TEST_F(CollisionPreventionTest, addDistanceSensorDataNarrow)
+{
+	// GIVEN: a vehicle attitude and a distance sensor message
+	TestCollisionPrevention cp;
+	Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
+	distance_sensor_s distance_sensor {};
+	distance_sensor.min_distance = 0.2f;
+	distance_sensor.max_distance = 20.f;
+	distance_sensor.current_distance = 5.f;
+	distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING;
+	distance_sensor.h_fov = math::radians(0.1 * bin_size);
+
+	uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
+
+	// WHEN the sensor has a very narrow field of view
+	cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
+
+	//THEN: the correct bins in the map should be filled
+	for (uint32_t i = 0; i < distances_array_size; i++) {
+		if (i == 0) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
+		}
+	}
+}
+TEST_F(CollisionPreventionTest, addDistanceSensorDataSlightlyLarger)
+{
+	// GIVEN: a vehicle attitude and a distance sensor message
+	TestCollisionPrevention cp;
+	Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
+	distance_sensor_s distance_sensor {};
+	distance_sensor.min_distance = 0.2f;
+	distance_sensor.max_distance = 20.f;
+	distance_sensor.current_distance = 5.f;
+	distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING;
+	distance_sensor.h_fov = math::radians(1.1 * bin_size);
+
+	uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
+
+	// WHEN the sensor has a very narrow field of view
+	cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
+
+	//THEN: the the bins corresponding to -5°, 0° and 5° should be filled
+	for (uint32_t i = 0; i < distances_array_size; i++) {
+		if (i == 71 || i <= 1) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
+		}
+	}
+}
+
 TEST_F(CollisionPreventionTest, addDistanceSensorData)
 {
 	// GIVEN: a vehicle attitude and a distance sensor message
 	TestCollisionPrevention cp;
-	cp.getObstacleMap().increment = 10.f;
 	Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
 	distance_sensor_s distance_sensor {};
 	distance_sensor.min_distance = 0.2f;
@@ -656,21 +713,24 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData)
 	uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
 
 	for (uint32_t i = 0; i < distances_array_size; i++) {
-		EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+		EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
 	}
 
 	//WHEN: we add distance sensor data to the right
 	distance_sensor.orientation = distance_sensor_s::ROTATION_RIGHT_FACING;
 	distance_sensor.h_fov = math::radians(19.99f);
 	cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
+	uint fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2);
+	uint start = (90 - fov) / bin_size;
+	uint end = (90 + fov) / bin_size;
 
 	//THEN: the correct bins in the map should be filled
 	for (uint32_t i = 0; i < distances_array_size; i++) {
-		if (i == 8 || i == 9) {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+		if (i >= start && i <= end) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
 
 		} else {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
 		}
 	}
 
@@ -679,17 +739,20 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData)
 	distance_sensor.h_fov = math::radians(50.f);
 	distance_sensor.current_distance = 8.f;
 	cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
+	fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2);
+	uint start2 = (270 - fov) / bin_size;
+	uint end2 = (270 + fov) / bin_size;
 
 	//THEN: the correct bins in the map should be filled
 	for (uint32_t i = 0; i < distances_array_size; i++) {
-		if (i == 8 || i == 9) {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+		if (i >= start && i <= end) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
 
-		} else if (i >= 24 && i <= 29) {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800);
+		} else if (i >= start2 && i <= end2) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800) << i;
 
 		} else {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
 		}
 	}
 
@@ -698,20 +761,23 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData)
 	distance_sensor.h_fov = math::radians(10.1f);
 	distance_sensor.current_distance = 3.f;
 	cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
+	fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2);
+	uint start3 = (360 - fov) / bin_size;
+	uint end3 = (fov) / bin_size;
 
 	//THEN: the correct bins in the map should be filled
 	for (uint32_t i = 0; i < distances_array_size; i++) {
-		if (i == 8 || i == 9) {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+		if (i >= start && i <= end) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500)  << i;
 
-		} else if (i >= 24 && i <= 29) {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800);
+		} else if (i >= start2 && i <= end2) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800) << i;
 
-		} else if (i == 0) {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 300);
+		} else if (i >= start3 || i <= end3) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 300) << i;
 
 		} else {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
 		}
 	}
 
@@ -722,7 +788,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude)
 {
 	// GIVEN: a vehicle attitude and obstacle distance message
 	TestCollisionPrevention cp;
-	cp.getObstacleMap().increment = 10.f;
 	obstacle_distance_s obstacle_msg {};
 	obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned
 	obstacle_msg.increment = 5.f;
@@ -733,11 +798,15 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude)
 	//obstacle at 10-30 deg world frame, distance 5 meters
 	memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
 
-	for (int i = 2; i < 6 ; i++) {
+	int start = 2;
+	int end = 6;
+
+	for (int i = start; i <= end ; i++) {
 		obstacle_msg.distances[i] = 500;
 	}
 
 
+
 	//THEN: at initialization the internal obstacle map should only contain UINT16_MAX
 	int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
 
@@ -750,11 +819,11 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude)
 
 	//THEN: the correct bins in the map should be filled
 	for (int i = 0; i < distances_array_size; i++) {
-		if (i == 1 || i == 2) {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+		if (i >= start && i <= end) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
 
 		} else {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
 		}
 
 		//reset array to UINT16_MAX
@@ -762,44 +831,50 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude)
 	}
 
 
-	//WHEN: we add distance sensor data while vehicle yaw 90deg to the right
+	//WHEN: we add obstacle distance sensor data while vehicle yaw 90deg to the right
 	cp.test_addObstacleSensorData(obstacle_msg, M_PI_2);
 
 	//THEN: the correct bins in the map should be filled
+	int offset =  bin_count - 90 / bin_size;
+
 	for (int i = 0; i < distances_array_size; i++) {
-		if (i == 28 || i == 29) {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+		if (i >= offset + start && i <= offset + end) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
 
 		} else {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
 		}
 
 		//reset array to UINT16_MAX
 		cp.getObstacleMap().distances[i] = UINT16_MAX;
 	}
 
-	//WHEN: we add distance sensor data while vehicle yaw 45deg to the left
+	//WHEN: we add obstacle distance sensor data while vehicle yaw 45deg to the left
 	cp.test_addObstacleSensorData(obstacle_msg, -M_PI_4);
 
 	//THEN: the correct bins in the map should be filled
+	offset =  45 / bin_size;
+
 	for (int i = 0; i < distances_array_size; i++) {
-		if (i == 6 || i == 7) {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+		if (i >= offset + start && i <= offset + end) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
 
 		} else {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
 		}
 
 		//reset array to UINT16_MAX
 		cp.getObstacleMap().distances[i] = UINT16_MAX;
 	}
 
-	//WHEN: we add distance sensor data while vehicle yaw 180deg
+	//WHEN: we add obstacle distance sensor data while vehicle yaw 180deg
 	cp.test_addObstacleSensorData(obstacle_msg, M_PI);
 
 	//THEN: the correct bins in the map should be filled
+	offset =  180 / bin_size;
+
 	for (int i = 0; i < distances_array_size; i++) {
-		if (i == 19 || i == 20) {
+		if (i >= offset + start && i <= offset + end) {
 			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
 
 		} else {
@@ -811,11 +886,84 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude)
 	}
 }
 
+TEST_F(CollisionPreventionTest, addObstacleSensorData_offset_bodyframe)
+{
+	// GIVEN: a vehicle attitude and obstacle distance message
+	TestCollisionPrevention cp;
+	obstacle_distance_s obstacle_msg {};
+	obstacle_msg.frame = obstacle_msg.MAV_FRAME_BODY_FRD; // Body Frame
+	obstacle_msg.increment = 6.f;
+	obstacle_msg.min_distance = 20;
+	obstacle_msg.max_distance = 2000;
+	obstacle_msg.angle_offset = 0.f;
+
+	//obstacle at 363°-39° deg world frame, distance 5 meters
+	memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
+
+	for (int i = 0; i <= 6 ; i++) { // 36° at 6° increment
+		obstacle_msg.distances[i] = 500;
+	}
+
+	//WHEN: we add distance sensor data
+	cp.test_addObstacleSensorData(obstacle_msg, 0.f);
+
+	//THEN: the the bins from 0 to 40 in map should be filled, which correspond to the angles from -2.5° to 42.5°
+	int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
+
+	for (int i = 0; i < distances_array_size; i++) {
+		if (i >= 0 && i <= 8) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
+		}
+
+		//reset array to UINT16_MAX
+		cp.getObstacleMap().distances[i] = UINT16_MAX;
+	}
+
+	//WHEN: we add the same obstacle distance sensor data with an angle offset of 30.5°
+	obstacle_msg.angle_offset = 30.5f;
+	// This then means our obstacle is between 27.5° and 69.5°
+	cp.test_addObstacleSensorData(obstacle_msg, 0.f);
+
+	//THEN: the bins from 30° to 70° in map should be filled, which correspond to the angles from 27.5° to 72.5°
+	for (int i = 0; i < distances_array_size; i++) {
+		if (i >= 6 && i <= 14) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
+		}
+
+		//reset array to UINT16_MAX
+		cp.getObstacleMap().distances[i] = UINT16_MAX;
+	}
+
+	//WHEN: we increase the offset to -30.5°
+	obstacle_msg.angle_offset = -30.5f;
+	// This then means our obstacle is between 326.5° and 8.5°
+	cp.test_addObstacleSensorData(obstacle_msg, 0.f);
+
+	//THEN: the bins from 325° to 10° in map should be filled, which correspond to the angles from 322.5° to 12.5°
+
+	for (int i = 0; i < distances_array_size; i++) {
+		if (i >= 65 || i <= 2) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
+		}
+
+		//reset array to UINT16_MAX
+		cp.getObstacleMap().distances[i] = UINT16_MAX;
+	}
+}
+
 TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe)
 {
 	// GIVEN: a vehicle attitude and obstacle distance message
 	TestCollisionPrevention cp;
-	cp.getObstacleMap().increment = 10.f;
 	obstacle_distance_s obstacle_msg {};
 	obstacle_msg.frame = obstacle_msg.MAV_FRAME_BODY_FRD; //north aligned
 	obstacle_msg.increment = 5.f;
@@ -825,8 +973,10 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe)
 
 	//obstacle at 10-30 deg body frame, distance 5 meters
 	memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
+	int start = 2;
+	int end = 6;
 
-	for (int i = 2; i < 6 ; i++) {
+	for (int i = start; i <= end ; i++) {
 		obstacle_msg.distances[i] = 500;
 	}
 
@@ -842,12 +992,13 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe)
 	cp.test_addObstacleSensorData(obstacle_msg, 0.f);
 
 	//THEN: the correct bins in the map should be filled
+
 	for (int i = 0; i < distances_array_size; i++) {
-		if (i == 1 || i == 2) {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+		if (i >= start && i <= end) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
 
 		} else {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
 		}
 
 		//reset array to UINT16_MAX
@@ -859,11 +1010,11 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe)
 
 	//THEN: the correct bins in the map should be filled
 	for (int i = 0; i < distances_array_size; i++) {
-		if (i == 1 || i == 2) {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+		if (i >= start && i <= end) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
 
 		} else {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
 		}
 
 		//reset array to UINT16_MAX
@@ -875,11 +1026,11 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe)
 
 	//THEN: the correct bins in the map should be filled
 	for (int i = 0; i < distances_array_size; i++) {
-		if (i == 1 || i == 2) {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+		if (i >= start && i <= end) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
 
 		} else {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
 		}
 
 		//reset array to UINT16_MAX
@@ -891,16 +1042,17 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe)
 
 	//THEN: the correct bins in the map should be filled
 	for (int i = 0; i < distances_array_size; i++) {
-		if (i == 1 || i == 2) {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+		if (i >= start && i <= end) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
 
 		} else {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
 		}
 
 		//reset array to UINT16_MAX
 		cp.getObstacleMap().distances[i] = UINT16_MAX;
 	}
+
 }
 
 
@@ -908,7 +1060,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset)
 {
 	// GIVEN: a vehicle attitude and obstacle distance message
 	TestCollisionPrevention cp;
-	cp.getObstacleMap().increment = 10.f;
 	obstacle_distance_s obstacle_msg {};
 	obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned
 	obstacle_msg.increment = 6.f;
@@ -916,42 +1067,62 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset)
 	obstacle_msg.max_distance = 2000;
 	obstacle_msg.angle_offset = 0.f;
 
-	//obstacle at 0-30 deg world frame, distance 5 meters
+	//obstacle at 363°-39° deg world frame, distance 5 meters
 	memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
 
-	for (int i = 0; i < 5 ; i++) {
+	for (int i = 0; i <= 6 ; i++) { // 36° at 6° increment
 		obstacle_msg.distances[i] = 500;
 	}
 
 	//WHEN: we add distance sensor data
 	cp.test_addObstacleSensorData(obstacle_msg, 0.f);
 
-	//THEN: the correct bins in the map should be filled
+	//THEN: the the bins from 0 to 40 in map should be filled, which correspond to the angles from -2.5° to 42.5°
 	int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
 
 	for (int i = 0; i < distances_array_size; i++) {
-		if (i >= 0 && i <= 2) {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+		if (i >= 0 && i <= 8) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
 
 		} else {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
 		}
 
 		//reset array to UINT16_MAX
 		cp.getObstacleMap().distances[i] = UINT16_MAX;
 	}
 
-	//WHEN: we add distance sensor data with an angle offset
-	obstacle_msg.angle_offset = 30.f;
+	//WHEN: we add the same obstacle distance sensor data with an angle offset of 30.5°
+	obstacle_msg.angle_offset = 30.5f;
+	// This then means our obstacle is between 27.5° and 69.5°
 	cp.test_addObstacleSensorData(obstacle_msg, 0.f);
 
-	//THEN: the correct bins in the map should be filled
+	//THEN: the bins from 30° to 70° in map should be filled, which correspond to the angles from 27.5° to 72.5°
 	for (int i = 0; i < distances_array_size; i++) {
-		if (i >= 3 && i <= 5) {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
+		if (i >= 6 && i <= 14) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
 
 		} else {
-			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
+		}
+
+		//reset array to UINT16_MAX
+		cp.getObstacleMap().distances[i] = UINT16_MAX;
+	}
+
+	//WHEN: we increase the offset to -30.5°
+	obstacle_msg.angle_offset = -30.5f;
+	// This then means our obstacle is between 326.5° and 8.5°
+	cp.test_addObstacleSensorData(obstacle_msg, 0.f);
+
+	//THEN: the bins from 325° to 10° in map should be filled, which correspond to the angles from 322.5° to 12.5°
+
+	for (int i = 0; i < distances_array_size; i++) {
+		if (i >= 65 || i <= 2) {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
+
+		} else {
+			EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
 		}
 
 		//reset array to UINT16_MAX
@@ -963,10 +1134,9 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum)
 {
 	// GIVEN: a vehicle attitude and obstacle distance message
 	TestCollisionPrevention cp;
-	cp.getObstacleMap().increment = 10.f;
 	obstacle_distance_s obstacle_msg {};
 	obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned
-	obstacle_msg.increment = 10.f;
+	obstacle_msg.increment = 5.f;
 	obstacle_msg.min_distance = 20;
 	obstacle_msg.max_distance = 2000;
 	obstacle_msg.angle_offset = 0.f;
@@ -976,7 +1146,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum)
 	//obstacle at 0-30 deg world frame, distance 5 meters
 	memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
 
-	for (int i = 0; i < 7 ; i++) {
+	for (int i = 0; i <= 6 ; i++) {
 		obstacle_msg.distances[i] = 500;
 	}
 
@@ -1001,18 +1171,17 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum)
 
 	//THEN: the setpoint direction should be modified correctly
 	EXPECT_EQ(sp_index, 2);
-	EXPECT_FLOAT_EQ(setpoint_dir(0), 0.93969262);
-	EXPECT_FLOAT_EQ(setpoint_dir(1), 0.34202012);
+	EXPECT_FLOAT_EQ(setpoint_dir(0), 0.98480773f);
+	EXPECT_FLOAT_EQ(setpoint_dir(1), 0.17364818f);
 }
 
 TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum)
 {
 	// GIVEN: a vehicle attitude and obstacle distance message
 	TestCollisionPrevention cp;
-	cp.getObstacleMap().increment = 10.f;
 	obstacle_distance_s obstacle_msg {};
 	obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned
-	obstacle_msg.increment = 10.f;
+	obstacle_msg.increment = 5.f;
 	obstacle_msg.min_distance = 20;
 	obstacle_msg.max_distance = 2000;
 	obstacle_msg.angle_offset = 0.f;
@@ -1049,8 +1218,8 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum)
 
 	//THEN: the setpoint direction should be modified correctly
 	EXPECT_EQ(sp_index, 2);
-	EXPECT_FLOAT_EQ(setpoint_dir(0), 0.93969262f);
-	EXPECT_FLOAT_EQ(setpoint_dir(1), 0.34202012f);
+	EXPECT_FLOAT_EQ(setpoint_dir(0), 0.98480773f);
+	EXPECT_FLOAT_EQ(setpoint_dir(1), 0.17364818f);
 }
 
 TEST_F(CollisionPreventionTest, overlappingSensors)
@@ -1146,16 +1315,15 @@ TEST_F(CollisionPreventionTest, enterData)
 {
 	// GIVEN: a simple setup condition
 	TestCollisionPrevention cp;
-	cp.getObstacleMap().increment = 10.f;
 	Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
 
 	//THEN: just after initialization all bins are at UINT16_MAX and any data should be accepted
-	EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range
-	EXPECT_TRUE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range
-	EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range
-	EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range
-	EXPECT_TRUE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range
-	EXPECT_TRUE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range
+	EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range
+	EXPECT_TRUE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range
+	EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range
+	EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range
+	EXPECT_TRUE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range
+	EXPECT_TRUE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range
 
 	//WHEN: we add distance sensor data to the right with a valid reading
 	distance_sensor_s distance_sensor {};
@@ -1167,36 +1335,38 @@ TEST_F(CollisionPreventionTest, enterData)
 	cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
 
 	//THEN: the internal map should contain the distance sensor readings
-	EXPECT_EQ(500, cp.getObstacleMap().distances[8]);
-	EXPECT_EQ(500, cp.getObstacleMap().distances[9]);
+	for (int i = 16; i < 20; i++) {
+		EXPECT_EQ(500, cp.getObstacleMap().distances[i]) << i;
+	}
 
 	//THEN: bins 8 & 9 contain valid readings
 	// a valid reading should only be accepted from sensors with shorter or equal range
 	// a out of range reading should only be accepted from sensors with the same range
 
-	EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range
-	EXPECT_FALSE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range
-	EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range
-	EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range
-	EXPECT_FALSE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range
-	EXPECT_FALSE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range
+	EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range
+	EXPECT_FALSE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range
+	EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range
+	EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range
+	EXPECT_FALSE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range
+	EXPECT_FALSE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range
 
 	//WHEN: we add distance sensor data to the right with an out of range reading
 	distance_sensor.current_distance = 21.f;
 	cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
 
 	//THEN: the internal map should contain the distance sensor readings
-	EXPECT_EQ(2000, cp.getObstacleMap().distances[8]);
-	EXPECT_EQ(2000, cp.getObstacleMap().distances[9]);
+	for (int i = 16; i < 20; i++) {
+		EXPECT_EQ(2000, cp.getObstacleMap().distances[i]) << i;
+	}
 
 	//THEN: bins 8 & 9 contain readings out of range
 	// a reading in range will be accepted in any case
 	// out of range readings will only be accepted from sensors with bigger or equal range
 
-	EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range
-	EXPECT_FALSE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range
-	EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range
-	EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range
-	EXPECT_TRUE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range
-	EXPECT_TRUE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range
+	EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range
+	EXPECT_FALSE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range
+	EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range
+	EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range
+	EXPECT_TRUE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range
+	EXPECT_TRUE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range
 }
diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp
index 77858bb964..95be696835 100644
--- a/src/modules/simulation/gz_bridge/GZBridge.cpp
+++ b/src/modules/simulation/gz_bridge/GZBridge.cpp
@@ -816,7 +816,7 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan)
 
 void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan)
 {
-	static constexpr int SECTOR_SIZE_DEG = 10; // PX4 Collision Prevention only has 36 sectors of 10 degrees each
+	static constexpr int SECTOR_SIZE_DEG = 5; // PX4 Collision Prevention uses 5 degree sectors
 
 	double angle_min_deg = scan.angle_min() * 180 / M_PI;
 	double angle_step_deg = scan.angle_step() * 180 / M_PI;