diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp
index a83e447..988c623 100644
--- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp
+++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp
@@ -120,7 +120,7 @@ public:
 
     // Scale the residuals by the square root information matrix to account for the measurement
     // uncertainty.
-    Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1>> residuals_map(residuals, A_.rows());
+    Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals);
     residuals_map.applyOnTheLeft(A_.template cast<T>());
 
     return true;
diff --git a/fuse_constraints/test/test_absolute_constraint.cpp b/fuse_constraints/test/test_absolute_constraint.cpp
index 9fe2099..a9f7921 100644
--- a/fuse_constraints/test/test_absolute_constraint.cpp
+++ b/fuse_constraints/test/test_absolute_constraint.cpp
@@ -59,9 +59,9 @@ TEST(AbsoluteConstraint, Constructor)
   {
     fuse_variables::AccelerationAngular2DStamped variable(
       rclcpp::Time(1234, 5678), fuse_core::uuid::generate("robby"));
-    fuse_core::Vector1d mean;
+    fuse_core::VectorXd mean(1);
     mean << 3.0;
-    fuse_core::Matrix1d cov;
+    fuse_core::MatrixXd cov(1, 1);
     cov << 1.0;
     EXPECT_NO_THROW(
       fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint constraint(
@@ -70,9 +70,9 @@ TEST(AbsoluteConstraint, Constructor)
   {
     fuse_variables::AccelerationLinear2DStamped variable(rclcpp::Time(1234, 5678),
       fuse_core::uuid::generate("bender"));
-    fuse_core::Vector2d mean;
+    fuse_core::VectorXd mean(2);
     mean << 1.0, 2.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 1.0, 0.1, 0.1, 2.0;
     EXPECT_NO_THROW(
       fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint constraint(
@@ -81,9 +81,9 @@ TEST(AbsoluteConstraint, Constructor)
   {
     fuse_variables::Orientation2DStamped variable(rclcpp::Time(1234, 5678),
       fuse_core::uuid::generate("johnny5"));
-    fuse_core::Vector1d mean;
+    fuse_core::VectorXd mean(1);
     mean << 3.0;
-    fuse_core::Matrix1d cov;
+    fuse_core::MatrixXd cov(1, 1);
     cov << 1.0;
     EXPECT_NO_THROW(
       fuse_constraints::AbsoluteOrientation2DStampedConstraint constraint(
@@ -92,9 +92,9 @@ TEST(AbsoluteConstraint, Constructor)
   {
     fuse_variables::Position2DStamped variable(rclcpp::Time(1234, 5678),
       fuse_core::uuid::generate("rosie"));
-    fuse_core::Vector2d mean;
+    fuse_core::VectorXd mean(2);
     mean << 1.0, 2.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 1.0, 0.1, 0.1, 2.0;
     EXPECT_NO_THROW(
       fuse_constraints::AbsolutePosition2DStampedConstraint constraint(
@@ -103,9 +103,9 @@ TEST(AbsoluteConstraint, Constructor)
   {
     fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678),
       fuse_core::uuid::generate("clank"));
-    fuse_core::Vector3d mean;
+    fuse_core::VectorXd mean(3);
     mean << 1.0, 2.0, 3.0;
-    fuse_core::Matrix3d cov;
+    fuse_core::MatrixXd cov(3, 3);
     cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0;
     EXPECT_NO_THROW(
       fuse_constraints::AbsolutePosition3DStampedConstraint constraint(
@@ -114,9 +114,9 @@ TEST(AbsoluteConstraint, Constructor)
   {
     fuse_variables::VelocityAngular2DStamped variable(rclcpp::Time(1234, 5678),
       fuse_core::uuid::generate("gort"));
-    fuse_core::Vector1d mean;
+    fuse_core::VectorXd mean(1);
     mean << 3.0;
-    fuse_core::Matrix1d cov;
+    fuse_core::MatrixXd cov(1, 1);
     cov << 1.0;
     EXPECT_NO_THROW(
       fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint constraint(
@@ -125,9 +125,9 @@ TEST(AbsoluteConstraint, Constructor)
   {
     fuse_variables::VelocityLinear2DStamped variable(rclcpp::Time(1234, 5678),
       fuse_core::uuid::generate("bishop"));
-    fuse_core::Vector2d mean;
+    fuse_core::VectorXd mean(2);
     mean << 1.0, 2.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 1.0, 0.1, 0.1, 2.0;
     EXPECT_NO_THROW(
       fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint constraint(
@@ -140,9 +140,9 @@ TEST(AbsoluteConstraint, PartialMeasurement)
 {
   fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678),
     fuse_core::uuid::generate("vici"));
-  fuse_core::Vector2d mean;
+  fuse_core::VectorXd mean(2);
   mean << 3.0, 1.0;
-  fuse_core::Matrix2d cov;
+  fuse_core::MatrixXd cov(2, 2);
   cov << 3.0, 0.2, 0.2, 1.0;
   auto indices = std::vector<size_t>{2, 0};
   EXPECT_NO_THROW(
@@ -157,9 +157,9 @@ TEST(AbsoluteConstraint, Covariance)
     // Verify the covariance <--> sqrt information conversions are correct
     fuse_variables::AccelerationLinear2DStamped variable(rclcpp::Time(1234, 5678),
       fuse_core::uuid::generate("chappie"));
-    fuse_core::Vector2d mean;
+    fuse_core::VectorXd mean(2);
     mean << 1.0, 2.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 1.0, 0.1, 0.1, 2.0;
     fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint constraint("test", variable,
       mean, cov);
@@ -178,9 +178,9 @@ TEST(AbsoluteConstraint, Covariance)
   {
     fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678),
       fuse_core::uuid::generate("astroboy"));
-    fuse_core::Vector2d mean;
+    fuse_core::VectorXd mean(2);
     mean << 3.0, 1.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 3.0, 0.2, 0.2, 1.0;
     auto indices = std::vector<size_t>{2, 0};
     fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov,
@@ -214,9 +214,9 @@ TEST(AbsoluteConstraint, Optimization)
     variable->x() = 10.7;
     variable->y() = -3.2;
     // Create an absolute constraint
-    fuse_core::Vector2d mean;
+    fuse_core::VectorXd mean(2);
     mean << 1.0, 2.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 1.0, 0.1, 0.1, 2.0;
     auto constraint = fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared(
       "test",
@@ -354,9 +354,9 @@ TEST(AbsoluteConstraint, PartialOptimization)
   var->z() = 0.9;
 
   // Create a partial constraint for the first and third indices
-  fuse_core::Vector2d mean1;
+  fuse_core::VectorXd mean1(2);
   mean1 << 1.0, 3.0;
-  fuse_core::Matrix2d cov1;
+  fuse_core::MatrixXd cov1(2, 2);
   /* *INDENT-OFF* */
   cov1 << 1.0, 0.0,
           0.0, 1.0;
@@ -367,9 +367,9 @@ TEST(AbsoluteConstraint, PartialOptimization)
     "test", *var, mean1, cov1, indices1);
 
   // Create another constraint for the second index
-  fuse_core::Vector1d mean2;
+  fuse_core::VectorXd mean2(1);
   mean2 << 2.0;
-  fuse_core::Matrix1d cov2;
+  fuse_core::MatrixXd cov2(1, 1);
   cov2 << 1.0;
   std::vector<size_t> indices2 = {fuse_variables::Position3DStamped::Y};
   auto constraint2 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared(
@@ -416,9 +416,9 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization)
     fuse_core::uuid::generate("tiktok"));
   variable->yaw() = 0.7;
   // Create an absolute constraint
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << 7.0;
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 0.10;
   auto constraint = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared(
     "test", *variable, mean, cov);
@@ -463,9 +463,9 @@ TEST(AbsoluteConstraint, Serialization)
   // Construct a constraint
   fuse_variables::AccelerationAngular2DStamped variable(rclcpp::Time(1234, 5678),
     fuse_core::uuid::generate("robby"));
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << 3.0;
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 1.0;
   fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint expected("test", variable, mean,
     cov);
diff --git a/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp
index dbe4b0a..b745c8c 100644
--- a/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp
+++ b/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp
@@ -58,9 +58,9 @@ TEST(AbsoluteOrientation2DStampedConstraint, Constructor)
   // Construct a constraint just to make sure it compiles.
   Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678),
     fuse_core::uuid::generate("walle"));
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << 1.0;
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 1.0;
   EXPECT_NO_THROW(
     AbsoluteOrientation2DStampedConstraint constraint(
@@ -73,9 +73,9 @@ TEST(AbsoluteOrientation2DStampedConstraint, Covariance)
   // Verify the covariance <--> sqrt information conversions are correct
   Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate(
       "mo"));
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << 1.0;
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 1.0;
   AbsoluteOrientation2DStampedConstraint constraint("test", orientation_variable, mean, cov);
 
@@ -99,10 +99,9 @@ TEST(AbsoluteOrientation2DStampedConstraint, Optimization)
   orientation_variable->yaw() = 1.0;
 
   // Create an absolute orientation constraint
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << 1.0;
-
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 1.0;
   auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared(
     "test",
@@ -168,10 +167,9 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationZero)
   orientation_variable->yaw() = 0.0;
 
   // Create an absolute orientation constraint
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << 0.0;
-
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 1.0;
   auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared(
     "test",
@@ -308,10 +306,9 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi)
   orientation_variable->yaw() = -M_PI;
 
   // Create an absolute orientation constraint
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << -M_PI;
-
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 1.0;
   auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared(
     "test",
@@ -370,9 +367,9 @@ TEST(AbsoluteOrientation2DStampedConstraint, Serialization)
   // Construct a constraint
   Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678),
     fuse_core::uuid::generate("walle"));
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << 1.0;
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 1.0;
   AbsoluteOrientation2DStampedConstraint expected("test", orientation_variable, mean, cov);
 
diff --git a/fuse_constraints/test/test_relative_constraint.cpp b/fuse_constraints/test/test_relative_constraint.cpp
index e221c4c..721fba5 100644
--- a/fuse_constraints/test/test_relative_constraint.cpp
+++ b/fuse_constraints/test/test_relative_constraint.cpp
@@ -62,9 +62,9 @@ TEST(RelativeConstraint, Constructor)
       fuse_core::uuid::generate("robby"));
     fuse_variables::AccelerationAngular2DStamped x2(rclcpp::Time(1235, 5678),
       fuse_core::uuid::generate("robby"));
-    fuse_core::Vector1d delta;
+    fuse_core::VectorXd delta(1);
     delta << 3.0;
-    fuse_core::Matrix1d cov;
+    fuse_core::MatrixXd cov(1, 1);
     cov << 1.0;
     EXPECT_NO_THROW(
       fuse_constraints::RelativeAccelerationAngular2DStampedConstraint constraint(
@@ -76,9 +76,9 @@ TEST(RelativeConstraint, Constructor)
       fuse_core::uuid::generate("bender"));
     fuse_variables::AccelerationLinear2DStamped x2(rclcpp::Time(1235, 5678),
       fuse_core::uuid::generate("bender"));
-    fuse_core::Vector2d delta;
+    fuse_core::VectorXd delta(2);
     delta << 1.0, 2.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 1.0, 0.1, 0.1, 2.0;
     EXPECT_NO_THROW(
       fuse_constraints::RelativeAccelerationLinear2DStampedConstraint constraint(
@@ -90,9 +90,9 @@ TEST(RelativeConstraint, Constructor)
       fuse_core::uuid::generate("johnny5"));
     fuse_variables::Orientation2DStamped x2(rclcpp::Time(1235, 5678),
       fuse_core::uuid::generate("johnny5"));
-    fuse_core::Vector1d delta;
+    fuse_core::VectorXd delta(1);
     delta << 3.0;
-    fuse_core::Matrix1d cov;
+    fuse_core::MatrixXd cov(1, 1);
     cov << 1.0;
     EXPECT_NO_THROW(
       fuse_constraints::RelativeOrientation2DStampedConstraint constraint(
@@ -104,9 +104,9 @@ TEST(RelativeConstraint, Constructor)
         "rosie"));
     fuse_variables::Position2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate(
         "rosie"));
-    fuse_core::Vector2d delta;
+    fuse_core::VectorXd delta(2);
     delta << 1.0, 2.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 1.0, 0.1, 0.1, 2.0;
     EXPECT_NO_THROW(
       fuse_constraints::RelativePosition2DStampedConstraint constraint("test", x1, x2, delta, cov));
@@ -116,9 +116,9 @@ TEST(RelativeConstraint, Constructor)
         "clank"));
     fuse_variables::Position3DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate(
         "clank"));
-    fuse_core::Vector3d delta;
+    fuse_core::VectorXd delta(3);
     delta << 1.0, 2.0, 3.0;
-    fuse_core::Matrix3d cov;
+    fuse_core::MatrixXd cov(3, 3);
     cov << 1.0, 0.1, 0.2, 0.3, 2.0, 0.3, 0.2, 0.3, 3.0;
     EXPECT_NO_THROW(
       fuse_constraints::RelativePosition3DStampedConstraint constraint("test", x1, x2, delta, cov));
@@ -128,9 +128,9 @@ TEST(RelativeConstraint, Constructor)
       fuse_core::uuid::generate("gort"));
     fuse_variables::VelocityAngular2DStamped x2(rclcpp::Time(1235, 5678),
       fuse_core::uuid::generate("gort"));
-    fuse_core::Vector1d delta;
+    fuse_core::VectorXd delta(1);
     delta << 3.0;
-    fuse_core::Matrix1d cov;
+    fuse_core::MatrixXd cov(1, 1);
     cov << 1.0;
     EXPECT_NO_THROW(
       fuse_constraints::RelativeVelocityAngular2DStampedConstraint constraint(
@@ -142,9 +142,9 @@ TEST(RelativeConstraint, Constructor)
       fuse_core::uuid::generate("bishop"));
     fuse_variables::VelocityLinear2DStamped x2(rclcpp::Time(1235, 5678),
       fuse_core::uuid::generate("bishop"));
-    fuse_core::Vector2d delta;
+    fuse_core::VectorXd delta(2);
     delta << 1.0, 2.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 1.0, 0.1, 0.1, 2.0;
     EXPECT_NO_THROW(
       fuse_constraints::RelativeVelocityLinear2DStampedConstraint constraint(
@@ -157,9 +157,9 @@ TEST(RelativeConstraint, PartialMeasurement)
 {
   fuse_variables::Position3DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("vici"));
   fuse_variables::Position3DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("vici"));
-  fuse_core::Vector2d delta;
+  fuse_core::VectorXd delta(2);
   delta << 3.0, 1.0;
-  fuse_core::Matrix2d cov;
+  fuse_core::MatrixXd cov(2, 2);
   cov << 3.0, 0.2, 0.2, 1.0;
   auto indices = std::vector<size_t>{2, 0};
   EXPECT_NO_THROW(
@@ -177,9 +177,9 @@ TEST(RelativeConstraint, Covariance)
       fuse_core::uuid::generate("chappie"));
     fuse_variables::AccelerationLinear2DStamped x2(rclcpp::Time(1235, 5678),
       fuse_core::uuid::generate("chappie"));
-    fuse_core::Vector2d delta;
+    fuse_core::VectorXd delta(2);
     delta << 1.0, 2.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 1.0, 0.1, 0.1, 2.0;
     fuse_constraints::RelativeAccelerationLinear2DStampedConstraint constraint("test", x1, x2,
       delta, cov);
@@ -198,9 +198,9 @@ TEST(RelativeConstraint, Covariance)
       fuse_core::uuid::generate("astroboy"));
     fuse_variables::Position3DStamped x2(rclcpp::Time(1235, 5678),
       fuse_core::uuid::generate("astroboy"));
-    fuse_core::Vector2d delta;
+    fuse_core::VectorXd delta(2);
     delta << 3.0, 1.0;
-    fuse_core::Matrix2d cov;
+    fuse_core::MatrixXd cov(2, 2);
     cov << 3.0, 0.2, 0.2, 1.0;
     auto indices = std::vector<size_t>{2, 0};
     fuse_constraints::RelativePosition3DStampedConstraint constraint("test", x1, x2, delta, cov,
@@ -240,9 +240,9 @@ TEST(RelativeConstraint, Optimization)
     x2->x() = -4.2;
     x2->y() = 1.9;
     // Create an absolute constraint
-    fuse_core::Vector2d mean;
+    fuse_core::VectorXd mean(2);
     mean << 1.0, 2.0;
-    fuse_core::Matrix2d cov1;
+    fuse_core::MatrixXd cov1(2, 2);
     cov1 << 1.0, 0.1, 0.1, 2.0;
     auto prior = fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared(
       "test",
@@ -250,9 +250,9 @@ TEST(RelativeConstraint, Optimization)
       mean,
       cov1);
     // Create an relative constraint
-    fuse_core::Vector2d delta;
+    fuse_core::VectorXd delta(2);
     delta << 0.1, 0.2;
-    fuse_core::Matrix2d cov2;
+    fuse_core::MatrixXd cov2(2, 2);
     cov2 << 1.0, 0.0, 0.0, 2.0;
     auto relative = fuse_constraints::RelativeAccelerationLinear2DStampedConstraint::make_shared(
       "test",
@@ -344,9 +344,9 @@ TEST(RelativeConstraint, Optimization)
     x2->y() = 1.9;
     x2->z() = 19.2;
     // Create an absolute constraint
-    fuse_core::Vector3d mean1;
+    fuse_core::VectorXd mean1(3);
     mean1 << 1.0, 2.0, 3.0;
-    fuse_core::Matrix3d cov1;
+    fuse_core::MatrixXd cov1(3, 3);
     cov1 << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0;
     auto c1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared(
       "test",
@@ -354,9 +354,9 @@ TEST(RelativeConstraint, Optimization)
       mean1,
       cov1);
     // Create an relative constraint
-    fuse_core::Vector3d delta2;
+    fuse_core::VectorXd delta2(3);
     delta2 << 0.1, 0.2, 0.3;
-    fuse_core::Matrix3d cov2;
+    fuse_core::MatrixXd cov2(3, 3);
     cov2 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0;
     auto c2 = fuse_constraints::RelativePosition3DStampedConstraint::make_shared(
       "test",
@@ -365,9 +365,9 @@ TEST(RelativeConstraint, Optimization)
       delta2,
       cov2);
     // Create an partial relative constraint
-    fuse_core::Vector2d delta3;
+    fuse_core::VectorXd delta3(2);
     delta3 << 0.1, 0.2;
-    fuse_core::Matrix2d cov3;
+    fuse_core::MatrixXd cov3(2, 2);
     cov3 << 1.0, 0.0, 0.0, 3.0;
     auto indices3 = std::vector<size_t>{2, 0};
     auto c3 = fuse_constraints::RelativePosition3DStampedConstraint::make_shared(
@@ -466,9 +466,9 @@ TEST(RelativeConstraint, RelativeOrientation2DOptimization)
     fuse_core::uuid::generate("t800"));
   x2->yaw() = -2.2;
   // Create an absolute constraint
-  fuse_core::Vector1d mean;
+  fuse_core::VectorXd mean(1);
   mean << 1.0;
-  fuse_core::Matrix1d cov1;
+  fuse_core::MatrixXd cov1(1, 1);
   cov1 << 2.0;
   auto prior = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared(
     "test",
@@ -476,9 +476,9 @@ TEST(RelativeConstraint, RelativeOrientation2DOptimization)
     mean,
     cov1);
   // Create an relative constraint
-  fuse_core::Vector1d delta;
+  fuse_core::VectorXd delta(1);
   delta << 0.1;
-  fuse_core::Matrix1d cov2;
+  fuse_core::MatrixXd cov2(1, 1);
   cov2 << 1.0;
   auto relative = fuse_constraints::RelativeOrientation2DStampedConstraint::make_shared(
     "test",
@@ -556,9 +556,9 @@ TEST(RelativeConstraint, Serialization)
     fuse_core::uuid::generate("robby"));
   fuse_variables::AccelerationAngular2DStamped x2(rclcpp::Time(1235, 5678),
     fuse_core::uuid::generate("robby"));
-  fuse_core::Vector1d delta;
+  fuse_core::VectorXd delta(1);
   delta << 3.0;
-  fuse_core::Matrix1d cov;
+  fuse_core::MatrixXd cov(1, 1);
   cov << 1.0;
   fuse_constraints::RelativeAccelerationAngular2DStampedConstraint expected("test", x1, x2, delta,
     cov);
diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp
index 40bb4bc..56d58ff 100644
--- a/fuse_models/include/fuse_models/common/sensor_proc.hpp
+++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp
@@ -329,7 +329,7 @@ inline bool processAbsolutePoseWithCovariance(
   orientation->yaw() = absolute_pose_2d.yaw();
 
   // Create the pose for the constraint
-  fuse_core::Vector3d pose_mean;
+  fuse_core::VectorXd pose_mean(3);
   pose_mean << absolute_pose_2d.x(), absolute_pose_2d.y(), absolute_pose_2d.yaw();
 
   // Create the covariance for the constraint
@@ -463,7 +463,7 @@ inline bool processDifferentialPoseWithCovariance(
   const double cy = ::cos(-pose1_2d.yaw());
   double x_diff = pose2_2d.x() - pose1_2d.x();
   double y_diff = pose2_2d.y() - pose1_2d.y();
-  fuse_core::Vector3d pose_relative_mean;
+  fuse_core::VectorXd pose_relative_mean(3);
   pose_relative_mean <<
     cy * x_diff - sy * y_diff,
     sy * x_diff + cy * y_diff,
@@ -825,7 +825,7 @@ inline bool processDifferentialPoseWithTwistCovariance(
 
   // Create the delta for the constraint
   const auto delta = pose1_2d.inverseTimes(pose2_2d);
-  fuse_core::Vector3d pose_relative_mean;
+  fuse_core::VectorXd pose_relative_mean(3);
   pose_relative_mean << delta.x(), delta.y(), delta.yaw();
 
   // Create the covariance components for the constraint
@@ -1016,12 +1016,12 @@ inline bool processTwistWithCovariance(
     velocity_linear->y() = transformed_message.twist.twist.linear.y;
 
     // Create the mean twist vectors for the constraints
-    fuse_core::Vector2d linear_vel_mean;
+    fuse_core::VectorXd linear_vel_mean(2);
     linear_vel_mean << transformed_message.twist.twist.linear.x,
       transformed_message.twist.twist.linear.y;
 
     // Create the covariances for the constraints
-    fuse_core::Matrix2d linear_vel_covariance;
+    fuse_core::MatrixXd linear_vel_covariance(2, 2);
     linear_vel_covariance <<
       transformed_message.twist.covariance[0],
       transformed_message.twist.covariance[1],
@@ -1074,10 +1074,10 @@ inline bool processTwistWithCovariance(
       fuse_variables::VelocityAngular2DStamped::make_shared(twist.header.stamp, device_id);
     velocity_angular->yaw() = transformed_message.twist.twist.angular.z;
 
-    fuse_core::Vector1d angular_vel_vector;
+    fuse_core::VectorXd angular_vel_vector(1);
     angular_vel_vector << transformed_message.twist.twist.angular.z;
 
-    fuse_core::Matrix1d angular_vel_covariance;
+    fuse_core::MatrixXd angular_vel_covariance(1, 1);
     angular_vel_covariance << transformed_message.twist.covariance[35];
 
     bool add_constraint = true;
@@ -1176,10 +1176,10 @@ inline bool processAccelWithCovariance(
   acceleration_linear->y() = transformed_message.accel.accel.linear.y;
 
   // Create the full mean vector and covariance for the constraint
-  fuse_core::Vector2d accel_mean;
+  fuse_core::VectorXd accel_mean(2);
   accel_mean << transformed_message.accel.accel.linear.x, transformed_message.accel.accel.linear.y;
 
-  fuse_core::Matrix2d accel_covariance;
+  fuse_core::MatrixXd accel_covariance(2, 2);
   accel_covariance <<
     transformed_message.accel.covariance[0],
     transformed_message.accel.covariance[1],
diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp
index 44e439c..c822c21 100644
--- a/fuse_models/src/unicycle_2d_ignition.cpp
+++ b/fuse_models/src/unicycle_2d_ignition.cpp
@@ -334,17 +334,27 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS
   // Create the covariances for each variable
   // The pose covariances are extracted from the provided PoseWithCovarianceStamped message.
   // The remaining covariances are provided as parameters to the parameter server.
-  auto position_cov = fuse_core::Matrix2d();
+  auto position_mean = fuse_core::VectorXd(2);
+  position_mean << position->x(), position->y();
+  auto position_cov = fuse_core::MatrixXd(2, 2);
   position_cov << pose.pose.covariance[0], pose.pose.covariance[1],
     pose.pose.covariance[6], pose.pose.covariance[7];
-  auto orientation_cov = fuse_core::Matrix1d();
+  auto orientation_mean = fuse_core::VectorXd(1);
+  orientation_mean << orientation->yaw();
+  auto orientation_cov = fuse_core::MatrixXd(1, 1);
   orientation_cov << pose.pose.covariance[35];
-  auto linear_velocity_cov = fuse_core::Matrix2d();
+  auto linear_velocity_mean = fuse_core::VectorXd(2);
+  linear_velocity_mean << linear_velocity->x(), linear_velocity->y();
+  auto linear_velocity_cov = fuse_core::MatrixXd(2, 2);
   linear_velocity_cov << params_.initial_sigma[3] * params_.initial_sigma[3], 0.0,
     0.0, params_.initial_sigma[4] * params_.initial_sigma[4];
-  auto angular_velocity_cov = fuse_core::Matrix1d();
+  auto angular_velocity_mean = fuse_core::VectorXd(1);
+  angular_velocity_mean << angular_velocity->yaw();
+  auto angular_velocity_cov = fuse_core::MatrixXd(1, 1);
   angular_velocity_cov << params_.initial_sigma[5] * params_.initial_sigma[5];
-  auto linear_acceleration_cov = fuse_core::Matrix2d();
+  auto linear_acceleration_mean = fuse_core::VectorXd(2);
+  linear_acceleration_mean << linear_acceleration->x(), linear_acceleration->y();
+  auto linear_acceleration_cov = fuse_core::MatrixXd(2, 2);
   linear_acceleration_cov << params_.initial_sigma[6] * params_.initial_sigma[6], 0.0,
     0.0, params_.initial_sigma[7] * params_.initial_sigma[7];
 
@@ -352,31 +362,31 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS
   auto position_constraint = fuse_constraints::AbsolutePosition2DStampedConstraint::make_shared(
     name(),
     *position,
-    fuse_core::Vector2d(position->x(), position->y()),
+    position_mean,
     position_cov);
   auto orientation_constraint =
     fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared(
     name(),
     *orientation,
-    fuse_core::Vector1d(orientation->yaw()),
+    orientation_mean,
     orientation_cov);
   auto linear_velocity_constraint =
     fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint::make_shared(
     name(),
     *linear_velocity,
-    fuse_core::Vector2d(linear_velocity->x(), linear_velocity->y()),
+    linear_velocity_mean,
     linear_velocity_cov);
   auto angular_velocity_constraint =
     fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint::make_shared(
     name(),
     *angular_velocity,
-    fuse_core::Vector1d(angular_velocity->yaw()),
+    angular_velocity_mean,
     angular_velocity_cov);
   auto linear_acceleration_constraint =
     fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared(
     name(),
     *linear_acceleration,
-    fuse_core::Vector2d(linear_acceleration->x(), linear_acceleration->y()),
+    linear_acceleration_mean,
     linear_acceleration_cov);
 
   // Create the transaction