diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h
index e9a769f07cca7bac2badfab645edacde7f1a2191..ff3a0a5e022fd00cd5abbc1f70d319fa7db3dfde 100644
--- a/include/gnss/processor/processor_tracker_gnss.h
+++ b/include/gnss/processor/processor_tracker_gnss.h
@@ -87,6 +87,7 @@ class ProcessorTrackerGnss : public ProcessorTrackerFeature
ParamsProcessorTrackerGnssPtr params_tracker_gnss_;
SensorGnssPtr sensor_gnss_;
std::map<int, FeatureGnssSatellitePtr> untracked_incoming_features_, untracked_last_features_;
+ Eigen::Vector4d fix_incoming_, fix_last_;
/** \brief Track provided features in \b _capture
* \param _features_in input list of features in \b last to track
@@ -173,10 +174,12 @@ class ProcessorTrackerGnss : public ProcessorTrackerFeature
* - resetting and/or clearing variables and/or algorithms at the end of processing
* - drawing / printing / logging the results of the processing
*/
- virtual void postProcess() override{};
+ virtual void postProcess() override;
void advanceDerived() override;
void resetDerived() override;
+
+ void removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap) const;
};
inline ProcessorTrackerGnss::ProcessorTrackerGnss(ParamsProcessorTrackerGnssPtr _params_tracker_gnss) :
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index 51cd4ff79fd2502810adfa61790f2007394b0467..070e48d973db98766dfd6527e3aa0dfb3103e5ab 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -16,14 +16,23 @@ void ProcessorTrackerGnss::preProcess()
if (!inc_snapshot ->satellitesPositionsComputed())
inc_snapshot ->computeSatellitesPositions(params_tracker_gnss_->gnss_opt.sateph);
- // auxiliar fix (for computing elevation, if error: resulting zero position -> elevation not filtered)
- Eigen::Vector3d inc_pos = GnssUtils::computePos(*inc_snapshot->getObservations(),
- *inc_snapshot->getNavigation(),
- prcopt_default).pos; // take default (less restrictive?)
+ // Compute (and store) auxiliar fix (for computing elevation)
+ GnssUtils::ComputePosOutput fix_output = GnssUtils::computePos(*inc_snapshot->getObservations(),
+ *inc_snapshot->getNavigation(),
+ prcopt_default); // take default (less restrictive?)
+
+ fix_incoming_.head<3>() = fix_output.pos;
+ fix_incoming_(3) = fix_output.rcv_bias(0);
+
+ // Set ECEF-ENU
+ if (!sensor_gnss_->isEnuDefined())
+ sensor_gnss_->setEcefEnu(Eigen::Vector3d::Zero(),true);
+
+ WOLF_INFO("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.transpose());
// filter observations (available ephemeris, constellations and elevation&SNR)
inc_snapshot->filterObservations(std::set<int>(), // discarded sats
- inc_pos,
+ fix_incoming_.head<3>(),
false, // check code
false, // check carrier phase
params_tracker_gnss_->gnss_opt);
@@ -118,6 +127,8 @@ void ProcessorTrackerGnss::establishFactors()
{
WOLF_INFO("ProcessorTrackerGnss::establishFactors: Frame = ", last_ptr_->getFrame()->getState().transpose());
+ FactorBasePtrList new_factors;
+
if (last_ptr_->getStateBlock("T") == nullptr)
{
WOLF_DEBUG("last clock offset state block nullptr, adding it");
@@ -131,62 +142,76 @@ void ProcessorTrackerGnss::establishFactors()
if (ftr_sat == nullptr)
continue;
+ // check valid measurement
+ if (ftr_sat->getObservation().P[0] < 1e-12) // TODO: move to GnssUtils::isValid(double, Combination)
+ continue;
+
// emplace a pseudo range factor
- FactorBase::emplace<FactorGnssPseudoRange>(ftr_sat,
- GnssUtils::Combination::CODE_L1,//TODO: from params
- 0.1,// TODO: compute from params and dt
- ftr_sat,
- sensor_gnss_,
- shared_from_this(),
- params_->apply_loss_function);
+ auto new_fac = FactorBase::emplace<FactorGnssPseudoRange>(ftr_sat,
+ GnssUtils::Combination::CODE_L1,//TODO: from params
+ 0.1,// TODO: compute from params and dt
+ ftr_sat,
+ sensor_gnss_,
+ shared_from_this(),
+ params_->apply_loss_function);
+
+ new_factors.push_back(new_fac);
WOLF_DEBUG("FactorGnssPseudoRange emplaced for satellite: ", ftr_sat->satNumber());
}
- if (origin_ptr_ == last_ptr_)
- return;
-
// TDCP FACTORS (tracked sats)
- // iterate over tracked features of last
-// for (auto ftr_k_pair : track_matrix_.snapshot(last_ptr_))
+// if (origin_ptr_ != last_ptr_)
// {
-// // current feature
-// auto ftr_k = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr_k_pair.second);
-// assert(ftr_k != nullptr);
-//
-// WOLF_DEBUG("FeatureGnssSatellite sat: ", ftr_k->satNumber(), " id: ", ftr_k->id());
-//
-// // emplace a tdcp factor from last to each KF
-// for (auto ftr_r_pair : track_matrix_.trackAtKeyframes(ftr_k_pair.first))
+// // iterate over tracked features of last
+// for (auto ftr_k_pair : track_matrix_.snapshot(last_ptr_))
// {
-// // reference feature
-// auto ftr_r = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr_r_pair.second);
-// assert(ftr_r != nullptr);
+// // current feature
+// auto ftr_k = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr_k_pair.second);
+// assert(ftr_k != nullptr);
+//
+// WOLF_DEBUG("FeatureGnssSatellite sat: ", ftr_k->satNumber(), " id: ", ftr_k->id());
//
-// if (ftr_k == ftr_r)
+// // emplace a tdcp factor from last to each KF
+// for (auto ftr_r_pair : track_matrix_.trackAtKeyframes(ftr_k_pair.first))
// {
-// WOLF_DEBUG("same KF and feature, skipping...");
-// break;
-// }
+// // reference feature
+// auto ftr_r = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr_r_pair.second);
+// assert(ftr_r != nullptr);
+//
+// // check valid measurement
+// if (ftr_sat->getObservation().L[0] < 1e-12) // TODO: move to GnssUtils::isValid(double, Combination)
+// continue;
+//
+// if (ftr_k == ftr_r)
+// {
+// WOLF_DEBUG("same KF and feature, skipping...");
+// break;
+// }
//
-// WOLF_DEBUG("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id());
+// WOLF_DEBUG("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id());
//
-// // emplace tdcp factor
-// FactorBase::emplace<FactorGnssTdcp>(ftr_k,
-// GnssUtils::Combination::CARRIER_L1,//TODO: from params
-// 0.1,// TODO: compute from params and dt
-// ftr_r,
-// ftr_k,
-// sensor_gnss_,
-// shared_from_this(),
-// params_->apply_loss_function);
+// // emplace tdcp factor
+// auto new_fac = FactorBase::emplace<FactorGnssTdcp>(ftr_k,
+// GnssUtils::Combination::CARRIER_L1,//TODO: from params
+// 0.1,// TODO: compute from params and dt
+// ftr_r,
+// ftr_k,
+// sensor_gnss_,
+// shared_from_this(),
+// params_->apply_loss_function);
+// new_factors.push_back(new_fac);
//
-// // WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
-// // " origin: " , feature_in_origin->id() ,
-// // " from last: " , feature_in_last->id() );
+// // WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
+// // " origin: " , feature_in_origin->id() ,
+// // " from last: " , feature_in_last->id() );
+// }
+// WOLF_DEBUG("All TDCP factors emplaced!");
// }
-// WOLF_DEBUG("All TDCP factors emplaced!");
// }
+
+ // remove outliers
+ removeOutliers(new_factors, last_ptr_);
}
void ProcessorTrackerGnss::advanceDerived()
@@ -194,6 +219,7 @@ void ProcessorTrackerGnss::advanceDerived()
ProcessorTrackerFeature::advanceDerived();
untracked_last_features_ = std::move(untracked_incoming_features_);
+ fix_last_ = fix_incoming_;
}
void ProcessorTrackerGnss::resetDerived()
@@ -201,6 +227,66 @@ void ProcessorTrackerGnss::resetDerived()
ProcessorTrackerFeature::resetDerived();
untracked_last_features_ = std::move(untracked_incoming_features_);
+ fix_last_ = fix_incoming_;
+}
+
+void ProcessorTrackerGnss::postProcess()
+{
+ getProblem()->print(4, 1, 1, 1);
+}
+
+void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap) const
+{
+ FactorBasePtrList remove_fac;
+
+ //WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
+ // copy states
+ Eigen::VectorXd x(cap->getFrame()->getP()->getState());
+ Eigen::VectorXd o(cap->getFrame()->getO()->getState());
+ Eigen::VectorXd clock_drift(cap->getStateBlock("T")->getState());
+ Eigen::VectorXd x_antena(sensor_gnss_->getP()->getState());
+ Eigen::VectorXd t_ENU_map(sensor_gnss_->getEnuMapTranslation()->getState());
+ Eigen::VectorXd roll_ENU_map(sensor_gnss_->getEnuMapRoll()->getState());
+ Eigen::VectorXd pitch_ENU_map(sensor_gnss_->getEnuMapPitch()->getState());
+ Eigen::VectorXd yaw_ENU_map(sensor_gnss_->getEnuMapYaw()->getState());
+
+ // create double* array of a copy of the state for pseudo range factors
+ double* parameters_pr[8] = {x.data(),
+ o.data(),
+ clock_drift.data(),
+ x_antena.data(),
+ t_ENU_map.data(),
+ roll_ENU_map.data(),
+ pitch_ENU_map.data(),
+ yaw_ENU_map.data()};
+ // create residuals
+ Eigen::VectorXd residual_pr(1);
+
+ for (auto fac : fac_list)
+ {
+ // PSEUDO RANGE FACTORS
+ auto fac_pr = std::dynamic_pointer_cast<FactorGnssPseudoRange>(fac);
+ if (fac_pr)
+ {
+ // evaluate the factor in this state
+ fac_pr->evaluate(parameters_pr, residual_pr.data(), nullptr);
+
+ WOLF_INFO("FactorGnssPseudoRange with residual = ", residual_pr);
+
+ // discard if residual too high evaluated at the current estimation
+ if (residual_pr(0) > 1e3)
+ {
+ WOLF_WARN("Discarding FactorGnssPseudoRange, considered OUTLIER");
+ WOLF_TRACE("Feature: ", fac->getMeasurement().transpose(),"\nError: ",(fac->getMeasurementSquareRootInformationUpper().inverse()*residual_pr).transpose());
+ remove_fac.push_back(fac_pr);
+ }
+ }
+ // TODO: TDCP FACTORS
+ }
+
+ // remove outliers
+ for (auto fac : remove_fac)
+ fac->remove();
}
} // namespace wolf
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index e8eeddfa846d0c5627ba6a429cddffc21afcdd83..3b3a63e879c147c01c6cb3cc8567876c941ddd9b 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -1,6 +1,6 @@
#include "gnss/sensor/sensor_gnss.h"
#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/utils/transformations.h"
@@ -22,9 +22,9 @@ SensorGnss::SensorGnss(const Eigen::VectorXd& _extrinsics,
assert(_extrinsics.size() == 3 && "Bad extrinsics size");
addStateBlock("t", std::make_shared<StateBlock>(3, params_->translation_fixed));
- addStateBlock("r", std::make_shared<StateBlock>(1, params_->roll_fixed));
- addStateBlock("p", std::make_shared<StateBlock>(1, params_->pitch_fixed));
- addStateBlock("y", std::make_shared<StateBlock>(1, params_->yaw_fixed));
+ addStateBlock("r", std::make_shared<StateAngle>(0.0, params_->roll_fixed));
+ addStateBlock("p", std::make_shared<StateAngle>(0.0, params_->pitch_fixed));
+ addStateBlock("y", std::make_shared<StateAngle>(0.0, params_->yaw_fixed));
if (params_->set_ENU)
setEcefEnu(params_->ENU_latlonalt, false);