sensor constructor improved

src/sensor/sensor_gnss.cpp

@@ -23,24 +23,46 @@ SensorGnss::SensorGnss(const Eigen::VectorXd& _extrinsics,
 {
     assert(_extrinsics.size() == 3 && "Bad extrinsics size");
 
-    // ENU
+    // STATE BLOCKS
+    // ENU-MAP
     addStateBlock('t', std::make_shared<StateBlock>(3, params_->translation_fixed), false);
     addStateBlock('r', std::make_shared<StateAngle>(0.0, params_->roll_fixed), false);
     addStateBlock('p', std::make_shared<StateAngle>(0.0, params_->pitch_fixed), false);
     addStateBlock('y', std::make_shared<StateAngle>(0.0, params_->yaw_fixed), false);
-
     // clock bias
     addStateBlock(CLOCK_BIAS_KEY, std::make_shared<StateBlock>(1,false), true); // receiver clock bias
-    addStateBlock(CLOCK_BIAS_GPS_GLO_KEY, std::make_shared<StateBlock>(1,false), params_->clock_bias_GPS_GLO_dynamic); // GPS-GLO clock bias
-    addStateBlock(CLOCK_BIAS_GPS_GAL_KEY, std::make_shared<StateBlock>(1,false), params_->clock_bias_GPS_GAL_dynamic); // GPS-GAL clock bias
-    addStateBlock(CLOCK_BIAS_GPS_CMP_KEY, std::make_shared<StateBlock>(1,false), params_->clock_bias_GPS_CMP_dynamic); // GPS-CMP clock bias
+    addStateBlock(CLOCK_BIAS_GPS_GLO_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_GLO_dynamic); // GPS-GLO clock bias
+    addStateBlock(CLOCK_BIAS_GPS_GAL_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_GAL_dynamic); // GPS-GAL clock bias
+    addStateBlock(CLOCK_BIAS_GPS_CMP_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_CMP_dynamic); // GPS-CMP clock bias
 
+    // ENU-ECEF
     // Mode "manual": ENU provided via params
     if (params_->ENU_mode == "manual")
         setEcefEnu(params_->ENU_latlonalt, false);
     // Mode "ECEF": ENU = ECEF (disabling this coordinates system)
     if (params_->ENU_mode == "ECEF")
         setEnuEcef(Eigen::Matrix3d::Identity(),Eigen::Vector3d::Zero());
+
+    // PRIORS
+    // prior to ENU-MAP (avoid non-observable problem)
+    if (params->ENU_mode != "ECEF")
+    {
+        if (not params_->translation_fixed)
+            addPriorParameter('t', Eigen::Vector3d::Zero(), 10*Eigen::Matrix3d::Identity());
+        if (not params_->roll_fixed)
+            addPriorParameter('r', Eigen::Vector1d::Zero(), M_PI*Eigen::Matrix1d::Identity());
+        if (not params_->pitch_fixed)
+            addPriorParameter('p', Eigen::Vector1d::Zero(), M_PI*Eigen::Matrix1d::Identity());
+        if (not params_->yaw_fixed)
+            addPriorParameter('y', Eigen::Vector1d::Zero(), M_PI*Eigen::Matrix1d::Identity());
+    }
+    // prior to inter-constellation clock bias (avoid non-observable problem)
+    if (not params_->clock_bias_GPS_GLO_dynamic)
+        addPriorParameter(CLOCK_BIAS_GPS_GLO_KEY, Eigen::Vector1d::Zero(), Eigen::Matrix1d::Identity());
+    if (not params_->clock_bias_GPS_GAL_dynamic)
+        addPriorParameter(CLOCK_BIAS_GPS_GAL_KEY, Eigen::Vector1d::Zero(), Eigen::Matrix1d::Identity());
+    if (not params_->clock_bias_GPS_CMP_dynamic)
+        addPriorParameter(CLOCK_BIAS_GPS_CMP_KEY, Eigen::Vector1d::Zero(), Eigen::Matrix1d::Identity());
 }
 
 SensorGnss::~SensorGnss()