diff --git a/control/autoware_pid_longitudinal_controller/README.md b/control/autoware_pid_longitudinal_controller/README.md
index b147b3b795391..aba0f36f04d65 100644
--- a/control/autoware_pid_longitudinal_controller/README.md
+++ b/control/autoware_pid_longitudinal_controller/README.md
@@ -68,6 +68,8 @@ There are two sources of the slope information, which can be switched by a param
   - Cons: z-coordinates of high-precision map is needed.
   - Cons: Does not support free space planning (for now)
 
+We also offer the options to switch between these, depending on driving conditions.
+
 **Notation:** This function works correctly only in a vehicle system that does not have acceleration feedback in the low-level control system.
 
 This compensation adds gravity correction to the target acceleration, resulting in an output value that is no longer equal to the target acceleration that the autonomous driving system desires. Therefore, it conflicts with the role of the acceleration feedback in the low-level controller.
diff --git a/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml b/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml
index bd905cf9be0f1..ec6a6f11b437d 100644
--- a/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml
+++ b/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml
@@ -74,7 +74,7 @@
 
     # slope compensation
     lpf_pitch_gain: 0.95
-    slope_source: "raw_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive
+    slope_source: "trajectory_goal_adaptive" # raw_pitch, trajectory_pitch, trajectory_adaptive or trajectory_goal_adaptive
     adaptive_trajectory_velocity_th: 1.0
     max_pitch_rad: 0.1
     min_pitch_rad: -0.1
diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp
index 8796664947a5a..1d03d1b339af6 100644
--- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp
+++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp
@@ -204,7 +204,12 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   double m_max_acc_cmd_diff;
 
   // slope compensation
-  enum class SlopeSource { RAW_PITCH = 0, TRAJECTORY_PITCH, TRAJECTORY_ADAPTIVE };
+  enum class SlopeSource {
+    RAW_PITCH = 0,
+    TRAJECTORY_PITCH,
+    TRAJECTORY_ADAPTIVE,
+    TRAJECTORY_GOAL_ADAPTIVE
+  };
   SlopeSource m_slope_source{SlopeSource::RAW_PITCH};
   double m_adaptive_trajectory_velocity_th;
   std::shared_ptr<LowpassFilter1d> m_lpf_pitch{nullptr};
diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
index be969e5a4af10..ec95ce656fa6f 100644
--- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
+++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -196,6 +196,8 @@ PidLongitudinalController::PidLongitudinalController(
     m_slope_source = SlopeSource::TRAJECTORY_PITCH;
   } else if (slope_source == "trajectory_adaptive") {
     m_slope_source = SlopeSource::TRAJECTORY_ADAPTIVE;
+  } else if (slope_source == "trajectory_goal_adaptive") {
+    m_slope_source = SlopeSource::TRAJECTORY_GOAL_ADAPTIVE;
   } else {
     RCLCPP_ERROR(logger_, "Slope source is not valid. Using raw_pitch option as default");
     m_slope_source = SlopeSource::RAW_PITCH;
@@ -529,21 +531,30 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
   // NOTE: getPitchByTraj() calculates the pitch angle as defined in
   // ../media/slope_definition.drawio.svg while getPitchByPose() is not, so `raw_pitch` is reversed
   const double raw_pitch = (-1.0) * longitudinal_utils::getPitchByPose(current_pose.orientation);
+  m_lpf_pitch->filter(raw_pitch);
   const double traj_pitch = longitudinal_utils::getPitchByTraj(
     control_data.interpolated_traj, control_data.target_idx, m_wheel_base);
 
   if (m_slope_source == SlopeSource::RAW_PITCH) {
-    control_data.slope_angle = m_lpf_pitch->filter(raw_pitch);
+    control_data.slope_angle = m_lpf_pitch->getValue();
   } else if (m_slope_source == SlopeSource::TRAJECTORY_PITCH) {
     control_data.slope_angle = traj_pitch;
-  } else if (m_slope_source == SlopeSource::TRAJECTORY_ADAPTIVE) {
+  } else if (
+    m_slope_source == SlopeSource::TRAJECTORY_ADAPTIVE ||
+    m_slope_source == SlopeSource::TRAJECTORY_GOAL_ADAPTIVE) {
     // if velocity is high, use target idx for slope, otherwise, use raw_pitch
-    if (control_data.current_motion.vel > m_adaptive_trajectory_velocity_th) {
-      control_data.slope_angle = traj_pitch;
-      m_lpf_pitch->filter(raw_pitch);
-    } else {
-      control_data.slope_angle = m_lpf_pitch->filter(raw_pitch);
-    }
+    const bool is_vel_slow = control_data.current_motion.vel < m_adaptive_trajectory_velocity_th &&
+                             m_slope_source == SlopeSource::TRAJECTORY_ADAPTIVE;
+
+    const double goal_dist = autoware::motion_utils::calcSignedArcLength(
+      control_data.interpolated_traj.points, current_pose.position,
+      control_data.interpolated_traj.points.size() - 1);
+    const bool is_close_to_trajectory_end =
+      goal_dist < m_wheel_base && m_slope_source == SlopeSource::TRAJECTORY_GOAL_ADAPTIVE;
+
+    control_data.slope_angle =
+      (is_close_to_trajectory_end || is_vel_slow) ? m_lpf_pitch->getValue() : traj_pitch;
+
     if (m_previous_slope_angle.has_value()) {
       constexpr double gravity_const = 9.8;
       control_data.slope_angle = std::clamp(
@@ -551,13 +562,13 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
         m_previous_slope_angle.value() + m_min_jerk * control_data.dt / gravity_const,
         m_previous_slope_angle.value() + m_max_jerk * control_data.dt / gravity_const);
     }
+    m_previous_slope_angle = control_data.slope_angle;
   } else {
     RCLCPP_ERROR_THROTTLE(
       logger_, *clock_, 3000, "Slope source is not valid. Using raw_pitch option as default");
-    control_data.slope_angle = m_lpf_pitch->filter(raw_pitch);
+    control_data.slope_angle = m_lpf_pitch->getValue();
   }
 
-  m_previous_slope_angle = control_data.slope_angle;
   updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch, m_lpf_pitch->getValue());
 
   return control_data;