diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h b/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h
index 4f2d42f8..5975eb32 100644
--- a/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h
+++ b/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h
@@ -147,7 +147,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2,
     const fuse_core::Matrix2d R_yaw_inv = fuse_core::rotationMatrix2D(-parameters[1][0]);
 
     Eigen::Map<fuse_core::Vector8d> residuals_map(residuals);
-    residuals_map<2, 1>(0, 0) = R_yaw_inv * position_diff - delta_position_pred;
+    residuals_map.block<2, 1>(0, 0) = R_yaw_inv * position_diff - delta_position_pred;
     residuals_map(2) = delta_yaw_est - delta_yaw_pred;
     residuals_map(3) = parameters[7][0] - vel_linear_pred.x();
     residuals_map(4) = parameters[7][1] - vel_linear_pred.y();