diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp index c48add4462dd1500feac0102cf1299f9c6ea9403..29b8234fdd2057a85224d891f301b84d36b9ba51 100644 --- a/src/sensor/sensor_gnss.cpp +++ b/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()