constant position xyz model

vortexntnu · Nov 16, 2024 · 9192a0c · 9192a0c
1 parent 6088856
commit 9192a0c
Showing 1 changed file with 6 additions and 9 deletions.
diff --git a/vortex-filtering/include/vortex_filtering/models/dynamic_models.hpp b/vortex-filtering/include/vortex_filtering/models/dynamic_models.hpp
@@ -103,10 +103,10 @@ class ConstantPosition : public interface::DynamicModelLTV<2, UNUSED, 2> {
 };
 
 /** (Nearly) Constant Pose model.
- * State x = [position, position, position, orientation, orientation, orientation]
+ * State x = [position, position, position]
  */
-class ConstantPose : public interface::DynamicModelLTV<6, UNUSED, 6> {
-  using Parent = interface::DynamicModelLTV<6, UNUSED, 6>;
+class ConstantPose : public interface::DynamicModelLTV<3, UNUSED, 3> {
+  using Parent = interface::DynamicModelLTV<3, UNUSED, 3>;
 
 public:
   static constexpr int N_DIM_x = Parent::N_DIM_x;
@@ -116,12 +116,11 @@ class ConstantPose : public interface::DynamicModelLTV<6, UNUSED, 6> {
   using T = Types_xuv<N_DIM_x, N_DIM_u, N_DIM_v>;
 
   /** Constant Pose Model
-   * x = [x, y, z, theta_x, theta_y, theta_z]
+   * x = [x, y, z]
    * @param std_pos Standard deviation of position
-   * @param std_orient Standard deviation of orientation
    */
-  ConstantPose(double std_pos, double std_orient)
-      : std_pos_(std_pos), std_orient_(std_orient)
+  ConstantPose(double std_pos)
+      : std_pos_(std_pos)
   {
   }
 
@@ -152,13 +151,11 @@ class ConstantPose : public interface::DynamicModelLTV<6, UNUSED, 6> {
   T::Mat_vv Q_d(double /*dt*/, const T::Vec_x& /*x*/ = T::Vec_x::Zero()) const override {
     T::Mat_vv Q = T::Mat_vv::Zero();
     Q.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() * std_pos_ * std_pos_;
-    Q.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() * std_orient_ * std_orient_;
     return Q;
   }
 
 private:
   double std_pos_;
-  double std_orient_;
 };
 
 /** (Nearly) Constant Velocity Model.