Fix Array contruction

src/processor/processor_force_torque_inertial_preint_dynamics.cpp

@@ -116,10 +116,14 @@ void ProcessorForceTorqueInertialPreintDynamics::emplaceFeaturesAndFactors(Captu
     FactorBase::emplace<FactorForceTorqueInertialDynamics>(
         ftr_fti, ftr_fti, _capture_origin, shared_from_this(), params_->apply_loss_function);
 
-    // 2. Feature and factor L -- angular moment
-    //-------------------------------------------
+    // 2. Feature and factor L -- angular momentum
+    //---------------------------------------------
     // - feature has the current gyro measurement
     // - factor relates w_measured, IMU_bias(last capture), L(last capture) and i(sensor)
+    
+    // auto measurement_gyro = motion.data_.segment<3>(3);
+    // auto meas_cov = sensor_fti_->getNoiseCov().block<3,3>(3,3); // ??
+
 
     // 3. Feature and factor bias -- IMU bias drift for acc and gyro
     //---------------------------------------------------------------
@@ -173,12 +177,10 @@ void ProcessorForceTorqueInertialPreintDynamics::configure(SensorBasePtr _sensor
     auto acc_drift_std  = sensor_fti_->getParamsSensorForceTorqueInertial()->acc_drift_std;
     auto gyro_drift_std = sensor_fti_->getParamsSensorForceTorqueInertial()->gyro_drift_std;
 
-    gyro_noise_cov_ = Eigen::Array3d(gyro_noise_std, gyro_noise_std, gyro_noise_std).square().matrix().asDiagonal();
-    imu_drift_cov_ =
-        Eigen::Array<double,6,1>(acc_drift_std, acc_drift_std, acc_drift_std, gyro_drift_std, gyro_drift_std, gyro_drift_std)
-            .square()
-            .matrix()
-            .asDiagonal();
+    ArrayXd imu_drift_sigmas(6);
+    imu_drift_sigmas << acc_drift_std, acc_drift_std, acc_drift_std, gyro_drift_std, gyro_drift_std, gyro_drift_std;
+    imu_drift_cov_  = imu_drift_sigmas.square().matrix().asDiagonal();
+    gyro_noise_cov_ = Array3d(gyro_noise_std, gyro_noise_std, gyro_noise_std).square().matrix().asDiagonal();
 }
 
 void ProcessorForceTorqueInertialPreintDynamics::data2tangent(const Eigen::VectorXd& _data,

test/gtest_solve_problem_force_torque_inertial_dynamics.cpp

@@ -101,6 +101,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri
     S->getStateBlock('C')->fix();
     S->getStateBlock('i')->fix();
     S->getStateBlock('m')->unfix();
+    // S->setStateBlockDynamic('I');
 
     CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
     CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.2, S, data, data_cov, nullptr);
@@ -145,6 +146,9 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri
         WOLF_INFO("-----------------------------");
     }
 
+    P->print(4, 1, 1, 1);
+
+
     ASSERT_NEAR(S->getStateBlock('m')->getState()(0), mass_true, 1e-3);
 }