diff --git a/coordinate_transform/src/coordinate_transform_component.cpp b/coordinate_transform/src/coordinate_transform_component.cpp
index 38ba747a..aaed41e6 100644
--- a/coordinate_transform/src/coordinate_transform_component.cpp
+++ b/coordinate_transform/src/coordinate_transform_component.cpp
@@ -23,6 +23,8 @@
 #include <tf2_ros/transform_listener.h>
 #include <string>
 // BEGIN STUDENT CODE
+#include <array>
+#include <vector>
 // END STUDENT CODE
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
@@ -80,6 +82,33 @@ class CoordinateTransformComponent : public rclcpp::Node
       getTransformationMatrixForOpticalFrame();
 
     // BEGIN STUDENT CODE
+    std::vector<stsl_interfaces::msg::Tag> new_tags;
+
+    for(const auto tag : tag_array_msg->tags) {
+        stsl_interfaces::msg::Tag new_tag;
+        new_tag.id = tag.id;
+
+        Eigen::Vector4d position(
+            tag.pose.position.x,
+            tag.pose.position.y,
+            tag.pose.position.z,
+            1
+        );
+
+        position = camera_to_base_transform * camera_optical_to_conventional_transform * position;
+
+        new_tag.pose.position.x = position.x();
+        new_tag.pose.position.y = position.y();
+        new_tag.pose.position.z = position.z();
+
+        Eigen::Matrix4d tag_orientation = quaternionMessageToTransformationMatrix(tag.pose.orientation);
+        
+        tag_orientation = camera_to_base_transform * camera_optical_to_conventional_transform * tag_orientation;
+
+        new_tag.pose.orientation = transformationMatrixToQuaternionMessage(tag_orientation);
+
+        new_tags.push_back(new_tag);
+    }
     // END STUDENT CODE
 
     // create a new tag array message
@@ -91,6 +120,7 @@ class CoordinateTransformComponent : public rclcpp::Node
 
     // BEGIN STUDENT CODE
     // set message tags to new_tags vector
+    new_tag_array_msg.tags = new_tags;
     // END STUDENT CODE
 
     // publish new tag message
@@ -100,7 +130,26 @@ class CoordinateTransformComponent : public rclcpp::Node
   Eigen::Matrix4d getTransformationMatrixForOpticalFrame()
   {
     // BEGIN STUDENT CODE
-    return {};
+    // Rotation about the X axis by pi/2
+    std::array<double, 16> R_roll_data = {
+        1, 0, 0, 0,
+        0, 0, -1, 0,
+        0, 1, 0, 0,
+        0, 0, 0, 1
+    };
+
+    // Rotation about the Z axis by pi/2
+    std::array<double, 16> R_yaw_data = {
+        0, -1, 0, 0,
+        1, 0, 0, 0,
+        0, 0, 1, 0,
+        0, 0, 0, 1
+    };
+
+    Eigen::Matrix4d R_roll(R_roll_data.data());
+    Eigen::Matrix4d R_yaw(R_yaw_data.data());
+
+    return R_yaw * R_roll;
     // END STUDENT CODE
   }
 
@@ -121,4 +170,4 @@ class CoordinateTransformComponent : public rclcpp::Node
 };
 }  // namespace coordinate_transform
 
-RCLCPP_COMPONENTS_REGISTER_NODE(coordinate_transform::CoordinateTransformComponent)
+RCLCPP_COMPONENTS_REGISTER_NODE(coordinate_transform::CoordinateTransformComponent)
\ No newline at end of file
diff --git a/localization/CMakeLists.txt b/localization/CMakeLists.txt
index bc664aa9..ef2203bd 100644
--- a/localization/CMakeLists.txt
+++ b/localization/CMakeLists.txt
@@ -18,6 +18,7 @@ add_library(${PROJECT_NAME}_component SHARED
   src/random_helpers.cpp
   src/particle_filter_localizer.cpp
 # BEGIN STUDENT CODE
+  src/odometry_sensor_model.cpp
 # END STUDENT CODE
 )
 ament_target_dependencies(${PROJECT_NAME}_component
diff --git a/localization/launch/particle_filter_localizer.launch.py b/localization/launch/particle_filter_localizer.launch.py
index 75c61c3b..da4a5648 100644
--- a/localization/launch/particle_filter_localizer.launch.py
+++ b/localization/launch/particle_filter_localizer.launch.py
@@ -22,11 +22,16 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 # BEGIN STUDENT CODE
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch.substitutions import LaunchConfiguration
 # END STUDENT CODE
 
 
 def generate_launch_description():
     # BEGIN STUDENT CODE
+    parameters_file_path = os.path.join(get_package_share_directory(
+        'localization'), 'config', 'localizer_params.yaml')
     # END STUDENT CODE
 
     return LaunchDescription([
@@ -35,6 +40,10 @@ def generate_launch_description():
             executable='localization_node',
             output='screen',
             # BEGIN STUDENT CODE
+            parameters=[
+                parameters_file_path,
+                {'use_sim_time': LaunchConfiguration('use_sim_time', default='false')}
+            ],
             # END STUDENT CODE
             remappings=[
                 ('/tags', '/coordinate_transformer/tags_transformed')
diff --git a/localization/src/odometry_sensor_model.cpp b/localization/src/odometry_sensor_model.cpp
new file mode 100644
index 00000000..4d9f1b63
--- /dev/null
+++ b/localization/src/odometry_sensor_model.cpp
@@ -0,0 +1,51 @@
+#include "odometry_sensor_model.hpp"
+#include <cmath>
+#include <vector>
+
+namespace localization
+{
+
+OdometrySensorModel::OdometrySensorModel(rclcpp::Node & node)
+{
+    covariance_ = node.declare_parameter<std::vector<double>>("sensors.odom.covariance", {0.1, 0.1});
+    timeout_ = node.declare_parameter<double>("sensors.odom.measurement_timeout", 0.1);
+
+    odom_sub_ = node.create_subscription<nav_msgs::msg::Odometry>(
+        "/odom",
+        rclcpp::SystemDefaultsQoS(),
+        std::bind(&OdometrySensorModel::UpdateMeasurement, this, std::placeholders::_1));
+}
+
+void OdometrySensorModel::UpdateMeasurement(const nav_msgs::msg::Odometry::SharedPtr msg)
+{
+    last_msg_ = *msg;
+}
+
+bool OdometrySensorModel::IsMeasurementAvailable(const rclcpp::Time & current_time)
+{
+    if(last_msg_.header.stamp.sec == 0)
+        return false;
+
+    const auto time_since_last_msg = current_time - rclcpp::Time(last_msg_.header.stamp);
+
+    return time_since_last_msg.seconds() < timeout_;
+}
+
+double OdometrySensorModel::ComputeLogProb(const Particle& particle)
+{
+    double log_prob = 0.0;
+    
+    log_prob += pow(last_msg_.twist.twist.linear.x - particle.x_vel, 2) / covariance_[0];
+    
+    log_prob += pow((-last_msg_.twist.twist.angular.z) - particle.yaw_vel, 2) / covariance_[1];
+    
+    return log_prob;
+}
+
+double OdometrySensorModel::ComputeLogNormalizer()
+{
+    return log(sqrt(pow(2 * M_PI, 2))) +
+           log(sqrt(covariance_[0])) + log(sqrt(covariance_[1]));
+}
+
+} // namespace localization
\ No newline at end of file
diff --git a/localization/src/odometry_sensor_model.hpp b/localization/src/odometry_sensor_model.hpp
new file mode 100644
index 00000000..40f861c4
--- /dev/null
+++ b/localization/src/odometry_sensor_model.hpp
@@ -0,0 +1,30 @@
+#ifndef ODOMETRY_SENSOR_MODEL_HPP_
+#define ODOMETRY_SENSOR_MODEL_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include "sensor_model.hpp"
+
+namespace localization
+{
+
+class OdometrySensorModel : public SensorModel
+{
+public:
+    OdometrySensorModel(rclcpp::Node& node);
+
+    void UpdateMeasurement(const nav_msgs::msg::Odometry::SharedPtr msg);
+
+    double ComputeLogProb(const Particle& particle) override;
+    double ComputeLogNormalizer() override;
+    bool IsMeasurementAvailable(const rclcpp::Time& current_time) override;
+
+private:
+    nav_msgs::msg::Odometry last_msg_;
+
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+};
+
+}  // namespace localization
+
+#endif  // ODOMETRY_SENSOR_MODEL_HPP_
\ No newline at end of file
diff --git a/localization/src/particle_filter_localizer.cpp b/localization/src/particle_filter_localizer.cpp
index 4be6d51d..a42475f1 100644
--- a/localization/src/particle_filter_localizer.cpp
+++ b/localization/src/particle_filter_localizer.cpp
@@ -27,6 +27,7 @@
 #include <rclcpp_components/register_node_macro.hpp>
 #include "aruco_sensor_model.hpp"
 // BEGIN STUDENT CODE
+#include "odometry_sensor_model.hpp"
 // END STUDENT CODE
 
 namespace localization
@@ -71,6 +72,7 @@ ParticleFilterLocalizer::ParticleFilterLocalizer(const rclcpp::NodeOptions & opt
 
   sensor_models_.push_back(std::make_unique<ArucoSensorModel>(*this));
   // BEGIN STUDENT CODE
+  sensor_models_.push_back(std::make_unique<OdometrySensorModel>(*this));
   // END STUDENT CODE
 }
 
diff --git a/mapping/src/mapping_node.cpp b/mapping/src/mapping_node.cpp
index af6a8cc3..f51ce693 100644
--- a/mapping/src/mapping_node.cpp
+++ b/mapping/src/mapping_node.cpp
@@ -26,6 +26,9 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <geometry_msgs/msg/point_stamped.hpp>
 // BEGIN STUDENT CODE
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 // END STUDENT CODE
 
 namespace mapping
@@ -46,8 +49,10 @@ class MappingNode : public rclcpp::Node
 public:
   explicit MappingNode(const rclcpp::NodeOptions & options)
   // BEGIN STUDENT CODE
-  : rclcpp::Node("mapping_node", options)
-    // END STUDENT CODE
+  : rclcpp::Node("mapping_node", options),
+    tf_buffer_(get_clock()),
+    tf_listener_(tf_buffer_)
+  // END STUDENT CODE
   {
     map_frame_id_ = declare_parameter<std::string>("map_frame", "map");
     robot_frame_id_ = declare_parameter<std::string>("robot_frame", "base_link");
@@ -74,11 +79,18 @@ class MappingNode : public rclcpp::Node
       rclcpp::SystemDefaultsQoS());
 
     // BEGIN STUDENT CODE
+    obstacles_subscription_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
+      "~/obstacles",
+      rclcpp::SystemDefaultsQoS(),
+      [this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) {ObstaclesCallback(msg);}
+      );
     // END STUDENT CODE
   }
 
 private:
   // BEGIN STUDENT CODE
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
   // END STUDENT CODE
   std::string map_frame_id_;
   std::string robot_frame_id_;
@@ -93,6 +105,12 @@ class MappingNode : public rclcpp::Node
   void ObstaclesCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr obstacles_msg)
   {
     // BEGIN STUDENT CODE
+    if (!tf_buffer_.canTransform(robot_frame_id_, map_frame_id_, tf2::TimePointZero) ||
+        !tf_buffer_.canTransform(obstacles_msg->header.frame_id, map_frame_id_, tf2::TimePointZero))
+    {
+      RCLCPP_INFO_ONCE(get_logger(), "Waiting for necessary TF data to be available.");
+      return;
+    }
     // END STUDENT CODE
 
     AddObstaclesToMap(*obstacles_msg);
@@ -116,7 +134,12 @@ class MappingNode : public rclcpp::Node
   geometry_msgs::msg::Point GetRobotLocation()
   {
     // BEGIN STUDENT CODE
-    return geometry_msgs::msg::Point{};
+    const auto robot_transform = tf_buffer_.lookupTransform(map_frame_id_, robot_frame_id_, tf2::TimePointZero);
+
+    geometry_msgs::msg::Point robot_location;
+    robot_location.x = robot_transform.transform.translation.x;
+    robot_location.y = robot_transform.transform.translation.y;
+    return robot_location;
     // END STUDENT CODE
   }
 
@@ -125,6 +148,32 @@ class MappingNode : public rclcpp::Node
     const auto robot_location = GetRobotLocation();
 
     // BEGIN STUDENT CODE
+    for (int y = 0; y <= obstacles.info.height - 1; ++y) {
+      for (int x = 0; x <= obstacles.info.width - 1; ++x) {
+
+        const auto obstacle_data_index = x + (y * obstacles.info.width);
+        const auto obstacle_data = obstacles.data.at(obstacle_data_index);
+
+        if (obstacle_data == -1) {
+          continue;
+        }
+
+        geometry_msgs::msg::PointStamped obstacle_location;
+        obstacle_location.header.frame_id = obstacles.header.frame_id;
+        obstacle_location.point.x = (x * obstacles.info.resolution) +
+          obstacles.info.origin.position.x;
+        obstacle_location.point.y = (y * obstacles.info.resolution) +
+          obstacles.info.origin.position.y;
+
+        const auto map_location = tf_buffer_.transform(obstacle_location, map_frame_id_);
+
+        if (!IsLocationInMapBounds(map_location.point)) {
+          continue;
+        }
+
+        UpdateProbability(robot_location, map_location.point, obstacle_data == 100);
+      }
+    }
     // END STUDENT CODE
   }
 
@@ -134,6 +183,19 @@ class MappingNode : public rclcpp::Node
     const bool & obstacle_detected)
   {
     // BEGIN STUDENT CODE
+    const int cell_x = (map_location.x - map_info_.origin.position.x) / map_info_.resolution;
+    const int cell_y = (map_location.y - map_info_.origin.position.y) / map_info_.resolution;
+    const auto data_index = MapDataIndexFromLocation(cell_x, cell_y);
+
+    const auto distance_robot_to_measurement = std::hypot(
+      robot_location.x - map_location.x,
+      robot_location.y - map_location.y);
+
+    auto probability = std::exp(-distance_coefficient_ * distance_robot_to_measurement);
+    probability *= (obstacle_detected ? hit_log_odds_ : miss_log_odds_);
+    map_data_[data_index] += probability;
+
+    map_data_[data_index] = std::clamp(map_data_[data_index], toLogOdds(0.01), toLogOdds(0.99));
     // END STUDENT CODE
   }
 
diff --git a/mineral_deposit_tracking/src/kalman_filter.hpp b/mineral_deposit_tracking/src/kalman_filter.hpp
new file mode 100644
index 00000000..8117651c
--- /dev/null
+++ b/mineral_deposit_tracking/src/kalman_filter.hpp
@@ -0,0 +1,94 @@
+#ifndef KALMAN_FILTER_HPP_
+#define KALMAN_FILTER_HPP_
+
+#include <Eigen/Dense>
+
+namespace mineral_deposit_tracking
+{
+
+template<int StateSize>
+class KalmanFilter
+{
+public:
+  // Type aliases for convenience
+  using VectorType = Eigen::Matrix<double, StateSize, 1>;
+  using MatrixType = Eigen::Matrix<double, StateSize, StateSize>;
+
+  // Constructor
+  KalmanFilter(
+    const MatrixType & transition_matrix,
+    const MatrixType & process_covariance,
+    const MatrixType & observation_matrix)
+  : transition_matrix_(transition_matrix),
+    process_covariance_(process_covariance),
+    observation_matrix_(observation_matrix),
+    estimate_(VectorType::Zero()),
+    estimate_covariance_(MatrixType::Identity() * 500)
+  {
+  }
+
+  // Getters
+  const VectorType & GetEstimate() const
+  {
+    return estimate_;
+  }
+
+  const MatrixType & GetEstimateCovariance() const
+  {
+    return estimate_covariance_;
+  }
+
+  // Reset function to set initial state
+  void Reset(const VectorType & initial_state, const MatrixType & initial_covariance)
+  {
+    estimate_ = initial_state;
+    estimate_covariance_ = initial_covariance;
+  }
+
+  // Prediction Step (Time Update)
+  // x = A * x
+  // P = A * P * A_transpose + Q
+  void TimeUpdate()
+  {
+    estimate_ = transition_matrix_ * estimate_;
+    estimate_covariance_ = (transition_matrix_ * estimate_covariance_ * transition_matrix_.transpose()) + process_covariance_;
+  }
+
+  // Correction Step (Measurement Update)
+  void MeasurementUpdate(const VectorType & measurement, const MatrixType & measurement_covariance)
+  {
+    // 1. Calculate the innovation covariance (S)
+    // S = H * P * H_transpose + R
+    const MatrixType innovation_covariance = (observation_matrix_ * estimate_covariance_ * observation_matrix_.transpose()) + measurement_covariance;
+
+    // 2. Calculate the Kalman gain (K)
+    // K = P * H_transpose * S_inverse
+    const MatrixType gain = estimate_covariance_ * observation_matrix_.transpose() * innovation_covariance.inverse();
+
+    // 3. Update the state estimate (x)
+    // x = x + K * (y - H * x)
+    estimate_ = estimate_ + (gain * (measurement - (observation_matrix_ * estimate_)));
+
+    // 4. Compute temporary variable (T) for readability
+    // T = I - K * H
+    const MatrixType tmp = MatrixType::Identity() - (gain * observation_matrix_);
+
+    // 5. Update the estimate covariance (P) using the Joseph form for numerical stability
+    // P = T * P * T_transpose + K * R * K_transpose
+    estimate_covariance_ = (tmp * estimate_covariance_ * tmp.transpose()) + (gain * measurement_covariance * gain.transpose());
+  }
+
+private:
+  // Model Constants
+  const MatrixType transition_matrix_;    // A
+  const MatrixType process_covariance_;   // Q
+  const MatrixType observation_matrix_;   // H
+
+  // State Variables
+  VectorType estimate_;                   // x
+  MatrixType estimate_covariance_;        // P
+};
+
+}  // namespace mineral_deposit_tracking
+
+#endif  // KALMAN_FILTER_HPP_
\ No newline at end of file
diff --git a/mineral_deposit_tracking/src/mineral_deposit_tracker.cpp b/mineral_deposit_tracking/src/mineral_deposit_tracker.cpp
index bf93e030..3742d031 100644
--- a/mineral_deposit_tracking/src/mineral_deposit_tracker.cpp
+++ b/mineral_deposit_tracking/src/mineral_deposit_tracker.cpp
@@ -26,6 +26,7 @@
 #include <stsl_interfaces/srv/reset_mineral_deposit_tracking.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 // BEGIN STUDENT CODE
+#include "kalman_filter.hpp"
 // END STUDENT CODE
 
 namespace mineral_deposit_tracking
@@ -38,7 +39,10 @@ class MineralDepositTracker : public rclcpp::Node
   // BEGIN STUDENT CODE
   : rclcpp::Node("mineral_deposit_tracker", options),
     tf_buffer_(get_clock()),
-    tf_listener_(tf_buffer_)
+    tf_listener_(tf_buffer_),
+    filter_(Eigen::Matrix2d::Identity(),
+      Eigen::Matrix2d::Identity() * 1e-4,
+      Eigen::Matrix2d::Identity())
     // END STUDENT CODE
   {
     tracked_deposit_publisher_ = create_publisher<geometry_msgs::msg::PoseStamped>(
@@ -61,6 +65,7 @@ class MineralDepositTracker : public rclcpp::Node
   rclcpp::Service<stsl_interfaces::srv::ResetMineralDepositTracking>::SharedPtr reset_service_;
   int deposit_id_;
   // BEGIN STUDENT CODE
+  KalmanFilter<2> filter_;
   // END STUDENT CODE
 
   void DepositMeasurementCallback(const stsl_interfaces::msg::MineralDepositArray::SharedPtr msg)
@@ -71,6 +76,7 @@ class MineralDepositTracker : public rclcpp::Node
       return;
     }
     // BEGIN STUDENT CODE
+    filter_.TimeUpdate();
     // END STUDENT CODE
 
     const auto found_deposit = std::find_if(
@@ -95,7 +101,8 @@ class MineralDepositTracker : public rclcpp::Node
     const Eigen::Matrix2d covariance = Eigen::Matrix2d::Identity() * 0.01;
 
     // BEGIN STUDENT CODE
-    PublishEstimate(measurement);
+    filter_.MeasurementUpdate(measurement, covariance);
+    PublishEstimate(filter_.GetEstimate());
     // END STUDENT CODE
   }
 
@@ -110,6 +117,7 @@ class MineralDepositTracker : public rclcpp::Node
     covariance << request->pose.covariance[0], request->pose.covariance[1],
       request->pose.covariance[3], request->pose.covariance[4];
     // BEGIN STUDENT CODE
+    filter_.Reset(position, covariance);
     // END STUDENT CODE
     PublishEstimate(position);
   }
diff --git a/obstacle_detector/CMakeLists.txt b/obstacle_detector/CMakeLists.txt
index 4410ef27..8f0232c1 100644
--- a/obstacle_detector/CMakeLists.txt
+++ b/obstacle_detector/CMakeLists.txt
@@ -14,6 +14,7 @@ find_package(OpenCV 4 REQUIRED)
 add_library(${PROJECT_NAME} SHARED
   src/obstacle_detector.cpp
 # BEGIN STUDENT CODE
+  src/student_functions.cpp
 # END STUDENT CODE
 )
 ament_target_dependencies(${PROJECT_NAME}
@@ -50,4 +51,4 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-ament_package()
+ament_package()
\ No newline at end of file
diff --git a/obstacle_detector/src/obstacle_detector.cpp b/obstacle_detector/src/obstacle_detector.cpp
index bf6ebac8..e2e96328 100644
--- a/obstacle_detector/src/obstacle_detector.cpp
+++ b/obstacle_detector/src/obstacle_detector.cpp
@@ -31,6 +31,7 @@
 #include <opencv2/core/eigen.hpp>
 
 // BEGIN STUDENT CODE
+#include "student_functions.hpp"
 // END STUDENT CODE
 
 namespace obstacle_detector
@@ -44,6 +45,13 @@ class ObstacleDetector : public rclcpp::Node
   {
     // BEGIN STUDENT CODE
     // Initialize publisher and subscriber
+    occupancy_grid_publisher_ = create_publisher<nav_msgs::msg::OccupancyGrid>("~/occupancy_grid", rclcpp::SystemDefaultsQoS());
+    camera_subscriber_ = image_transport::create_camera_subscription(
+        this, "/camera/image_raw",
+        std::bind(
+          &ObstacleDetector::ImageCallback, this, std::placeholders::_1,
+          std::placeholders::_2),
+        "raw", rclcpp::SensorDataQoS().get_rmw_qos_profile());
     // END STUDENT CODE
 
     declare_parameters<int>(
@@ -64,6 +72,8 @@ class ObstacleDetector : public rclcpp::Node
 
   // BEGIN STUDENT CODE
   // Declare subscriber and publisher members
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr occupancy_grid_publisher_ {};
+  image_transport::CameraSubscriber camera_subscriber_ {};
   // END STUDENT CODE
 
   void ImageCallback(
@@ -83,7 +93,7 @@ class ObstacleDetector : public rclcpp::Node
 
     // BEGIN STUDENT CODE
     // Call FindColors()
-    cv::Mat detected_colors;
+    cv::Mat detected_colors = FindColors(cv_image->image, min_color, max_color);
     // END STUDENT CODE
 
     std::string tf_error_string;
@@ -117,7 +127,7 @@ class ObstacleDetector : public rclcpp::Node
 
     // BEGIN STUDENT CODE
     // Call ReprojectToGroundPlane
-    cv::Mat projected_colors;
+    cv::Mat projected_colors = ReprojectToGroundPlane(detected_colors, homography, map_size);
     // END STUDENT CODE
 
     cv::rotate(projected_colors, projected_colors, cv::ROTATE_90_CLOCKWISE);
@@ -141,6 +151,7 @@ class ObstacleDetector : public rclcpp::Node
 
     // BEGIN STUDENT CODE
     // Publish occupancy_grid_msg
+    occupancy_grid_publisher_->publish(occupancy_grid_msg);
     // END STUDENT CODE
   }
 
diff --git a/obstacle_detector/src/student_functions.cpp b/obstacle_detector/src/student_functions.cpp
new file mode 100644
index 00000000..84caf88b
--- /dev/null
+++ b/obstacle_detector/src/student_functions.cpp
@@ -0,0 +1,26 @@
+#include "student_functions.hpp"
+
+cv::Mat FindColors(const cv::Mat input, const cv::Scalar range_min, const cv::Scalar range_max)
+{
+    cv::Mat input_hsv;
+    cv::cvtColor(input, input_hsv, cv::COLOR_BGR2HSV);
+
+    cv::Mat output(input.size(), CV_8UC1);
+
+    cv::inRange(input_hsv, range_min, range_max, output);
+
+    return output;
+}
+
+cv::Mat ReprojectToGroundPlane(const cv::Mat input,
+                             const cv::Mat homography,
+                             const cv::Size map_size)
+{
+    cv::Mat output(map_size, CV_8UC1);
+
+    cv::warpPerspective(
+        input, output, homography, map_size, cv::INTER_NEAREST, cv::BORDER_CONSTANT,
+        cv::Scalar(127));
+
+    return output;
+}
\ No newline at end of file
diff --git a/obstacle_detector/src/student_functions.hpp b/obstacle_detector/src/student_functions.hpp
new file mode 100644
index 00000000..fb311bfa
--- /dev/null
+++ b/obstacle_detector/src/student_functions.hpp
@@ -0,0 +1,14 @@
+#ifndef STUDENT_FUNCTIONS_HPP  // Check that the unique macro has not already been defined
+#define STUDENT_FUNCTIONS_HPP  // Define the unique macro
+
+#include <opencv2/opencv.hpp>
+
+cv::Mat FindColors(const cv::Mat input,
+                   const cv::Scalar range_min, 
+                   const cv::Scalar range_max);
+
+cv::Mat ReprojectToGroundPlane(const cv::Mat input,
+                             const cv::Mat homography,
+                             const cv::Size map_size);
+
+#endif  // Close the pre-processor if block
\ No newline at end of file
diff --git a/peak_finder/src/peak_finder_component.cpp b/peak_finder/src/peak_finder_component.cpp
index c80e035c..3a5fc082 100644
--- a/peak_finder/src/peak_finder_component.cpp
+++ b/peak_finder/src/peak_finder_component.cpp
@@ -28,6 +28,7 @@
 #include <rclcpp_action/rclcpp_action.hpp>
 #include <stsl_interfaces/action/park_at_peak.hpp>
 // BEGIN STUDENT CODE
+#include <stsl_interfaces/srv/sample_elevation.hpp>
 // END STUDENT CODE
 #include <tf2_eigen/tf2_eigen.hpp>
 #include <nav2_msgs/action/navigate_to_pose.hpp>
@@ -57,6 +58,7 @@ class PeakFinderComponent : public rclcpp::Node
       std::bind(&PeakFinderComponent::handle_accepted, this, std::placeholders::_1));
 
     // BEGIN STUDENT CODE
+    elevation_client_ = create_client<stsl_interfaces::srv::SampleElevation>("/sample_elevation");
     // END STUDENT CODE
   }
 
@@ -65,6 +67,7 @@ class PeakFinderComponent : public rclcpp::Node
   tf2_ros::TransformListener tf_listener_;
   rclcpp_action::Server<ParkAtPeak>::SharedPtr action_server_;
   // BEGIN STUDENT CODE
+  rclcpp::Client<stsl_interfaces::srv::SampleElevation>::SharedPtr elevation_client_;
   // END STUDENT CODE
   Navigator navigator_;
 
@@ -91,6 +94,13 @@ class PeakFinderComponent : public rclcpp::Node
   void execute(const std::shared_ptr<ParkAtPeakGoalHandle> goal_handle)
   {
     // BEGIN STUDENT CODE
+    if (!elevation_client_->service_is_ready()) {
+      RCLCPP_ERROR(
+        get_logger(), "%s service must be available to run peak_finder action!",
+        elevation_client_->get_service_name());
+      goal_handle->abort(std::make_shared<ParkAtPeak::Result>());
+      return;
+    }
     // END STUDENT CODE
 
     std::string tf_error_msg;
@@ -168,7 +178,24 @@ class PeakFinderComponent : public rclcpp::Node
   double SampleElevation(const Eigen::Vector2d & position)
   {
     // BEGIN STUDENT CODE
-    return 0.0;
+    auto request = std::make_shared<stsl_interfaces::srv::SampleElevation::Request>();
+
+    request->x = position.x();
+    request->y = position.y();
+
+    auto result_future = elevation_client_->async_send_request(request);
+
+    if (result_future.wait_for(std::chrono::seconds(2)) != std::future_status::ready) {
+      throw std::runtime_error("Elevation service call timed out.");
+    }
+
+    const auto response = result_future.get();
+
+    if (!response->success) {
+      throw std::runtime_error("Elevation server reported failure.");
+    }
+
+    return response->elevation;
     // END STUDENT CODE
   }
 
@@ -177,7 +204,29 @@ class PeakFinderComponent : public rclcpp::Node
     const double & current_elevation)
   {
     // BEGIN STUDENT CODE
-    return current_position;
+    const double search_radius = get_parameter("search_radius").as_double();
+    const int sample_count = get_parameter("sample_count").as_int();
+
+    std::vector<Eigen::Vector2d> sample_positions;
+
+    const auto angle_delta = (2 * M_PI) / sample_count;
+    for (auto angle = 0.0; angle < (2 * M_PI); angle += angle_delta) {
+      const Eigen::Vector2d pose = (search_radius * Eigen::Vector2d(std::cos(angle), std::sin(angle))) + current_position;
+      sample_positions.push_back(pose);
+    }
+
+    std::vector<double> elevations;
+    for (int j = 0; j <= sample_positions.size(); ++j) {
+      elevations.push_back(SampleElevation(sample_positions[j]));
+    }
+
+    const auto max_elevation_iter = std::max_element(elevations.begin(), elevations.end());
+
+    if (*max_elevation_iter <= current_elevation) {
+      return current_position;
+    }
+
+    return sample_positions[std::distance(elevations.begin(), max_elevation_iter)];
     // END STUDENT CODE
   }
 };
diff --git a/rj_training_bringup/launch/projects/week_3.launch.xml b/rj_training_bringup/launch/projects/week_3.launch.xml
index d40c2ad8..6c9eb5e4 100644
--- a/rj_training_bringup/launch/projects/week_3.launch.xml
+++ b/rj_training_bringup/launch/projects/week_3.launch.xml
@@ -13,7 +13,7 @@
   </node>
 
   <!-- BEGIN STUDENT CODE -->
-  <include file="$(find-pkg-share rj_training_bringup)/launch/fake_localizer.launch.py">
+  <include file="$(find-pkg-share localization)/launch/particle_filter_localizer.launch.py">
     <arg name="use_sim_time" value="$(var simulation)"/>
   </include>
   <!-- END STUDENT CODE -->