mars_lib 0.1.0.2abe2576fe7f
Modular and Robust Sensor-Fusion
Loading...
Searching...
No Matches
Public Member Functions | Public Attributes | List of all members
mars::GpsSensorClass Class Reference

#include </home/runner/work/mars_lib/mars_lib/source/mars/include/mars/sensors/gps/gps_sensor_class.h>

+ Inheritance diagram for mars::GpsSensorClass:
+ Collaboration diagram for mars::GpsSensorClass:

Public Member Functions

 GpsSensorClass (const std::string &name, std::shared_ptr< CoreState > core_states)
 
virtual ~GpsSensorClass ()=default
 
GpsSensorStateType get_state (const std::shared_ptr< void > &sensor_data)
 
Eigen::MatrixXd get_covariance (const std::shared_ptr< void > &sensor_data)
 get_covariance Resolves a void pointer to the covariance matrix of the corresponding sensor type Each sensor is responsible to cast its own data type
 
void set_initial_calib (std::shared_ptr< void > calibration)
 set_initial_calib Sets the calibration of an individual sensor
 
void set_gps_reference_coordinates (const double &latitude, const double &longitude, const double &altitude)
 
void set_gps_reference_coordinates (const mars::GpsCoordinates &gps_reference)
 
BufferDataType Initialize (const Time &timestamp, std::shared_ptr< void > sensor_data, std::shared_ptr< CoreType > latest_core_data)
 Initialize the state of an individual sensor.
 
bool CalcUpdate (const Time &, std::shared_ptr< void > measurement, const CoreStateType &prior_core_state, std::shared_ptr< void > latest_sensor_data, const Eigen::MatrixXd &prior_cov, BufferDataType *new_state_data)
 CalcUpdate Calculates the update for an individual sensor definition.
 
GpsSensorStateType ApplyCorrection (const GpsSensorStateType &prior_sensor_state, const Eigen::MatrixXd &correction)
 
- Public Member Functions inherited from mars::SensorInterface
virtual EIGEN_MAKE_ALIGNED_OPERATOR_NEW ~SensorInterface ()=default
 

Public Attributes

EIGEN_MAKE_ALIGNED_OPERATOR_NEW GpsConversion gps_conversion_
 
bool using_external_gps_reference_
 
bool gps_reference_is_set_
 
- Public Attributes inherited from mars::UpdateSensorAbsClass
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int aux_states_
 
int aux_error_states_
 
int ref_to_nav_
 
Eigen::MatrixXd residual_
 
Eigen::VectorXd R_
 Measurement noise "squared".
 
Eigen::MatrixXd F_
 
Eigen::MatrixXd H_
 
Eigen::MatrixXd Q_
 
std::shared_ptr< void > initial_calib_ { nullptr }
 
bool initial_calib_provided_ { false }
 True if an initial calibration was provided.
 
Chi2 chi2_
 
std::shared_ptr< CoreStatecore_states_
 
- Public Attributes inherited from mars::SensorAbsClass
int id_ { -1 }
 
std::string name_
 Name of the individual sensor instance.
 
bool is_initialized_ { false }
 True if the sensor has been initialized.
 
bool do_update_ { true }
 True if updates should be performed with the sensor.
 
int type_ { -1 }
 Future feature, holds information such as position or orientation for highlevel decissions.
 
bool const_ref_to_nav_ { true }
 True if the reference should not be estimated.
 
bool ref_to_nav_given_ { false }
 True if the reference to the navigation frame is given by initial calibration.
 
bool use_dynamic_meas_noise_ { false }
 True if dynamic noise values from measurements should be used.
 

Constructor & Destructor Documentation

◆ GpsSensorClass()

mars::GpsSensorClass::GpsSensorClass ( const std::string &  name,
std::shared_ptr< CoreState core_states 
)
inline
42 {
43 name_ = name;
44 core_states_ = std::move(core_states);
45 const_ref_to_nav_ = false;
49
50 // chi2
51 chi2_.set_dof(3);
52
53 std::cout << "Created: [" << this->name_ << "] Sensor" << std::endl;
54 }
void set_dof(const int &value)
set_dof Set degree of freedom for the X2 distribution
bool gps_reference_is_set_
Definition gps_sensor_class.h:39
bool using_external_gps_reference_
Definition gps_sensor_class.h:38
std::string name_
Name of the individual sensor instance.
Definition sensor_abs_class.h:23
bool const_ref_to_nav_
True if the reference should not be estimated.
Definition sensor_abs_class.h:27
bool initial_calib_provided_
True if an initial calibration was provided.
Definition update_sensor_abs_class.h:38
std::shared_ptr< CoreState > core_states_
Definition update_sensor_abs_class.h:42
Chi2 chi2_
Definition update_sensor_abs_class.h:40
+ Here is the call graph for this function:

◆ ~GpsSensorClass()

virtual mars::GpsSensorClass::~GpsSensorClass ( )
virtualdefault

Member Function Documentation

◆ get_state()

GpsSensorStateType mars::GpsSensorClass::get_state ( const std::shared_ptr< void > &  sensor_data)
inline
59 {
60 GpsSensorData data = *static_cast<GpsSensorData*>(sensor_data.get());
61 return data.state_;
62 }
BindSensorData< GpsSensorStateType > GpsSensorData
Definition gps_sensor_class.h:30

◆ get_covariance()

Eigen::MatrixXd mars::GpsSensorClass::get_covariance ( const std::shared_ptr< void > &  sensor_data)
inlinevirtual

get_covariance Resolves a void pointer to the covariance matrix of the corresponding sensor type Each sensor is responsible to cast its own data type

Parameters
sensor_data
Returns
Covariance matrix contained in the sensor data struct

Implements mars::SensorInterface.

65 {
66 GpsSensorData data = *static_cast<GpsSensorData*>(sensor_data.get());
67 return data.get_full_cov();
68 }
+ Here is the call graph for this function:

◆ set_initial_calib()

void mars::GpsSensorClass::set_initial_calib ( std::shared_ptr< void >  calibration)
inlinevirtual

set_initial_calib Sets the calibration of an individual sensor

Parameters
calibration

Implements mars::SensorInterface.

71 {
72 initial_calib_ = calibration;
74 }
std::shared_ptr< void > initial_calib_
Definition update_sensor_abs_class.h:37

◆ set_gps_reference_coordinates() [1/2]

void mars::GpsSensorClass::set_gps_reference_coordinates ( const double &  latitude,
const double &  longitude,
const double &  altitude 
)
inline
77 {
78 set_gps_reference_coordinates(mars::GpsCoordinates(latitude, longitude, altitude));
79 }
void set_gps_reference_coordinates(const double &latitude, const double &longitude, const double &altitude)
Definition gps_sensor_class.h:76
The GpsCoordinates struct.
Definition gps_conversion.h:22
+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ set_gps_reference_coordinates() [2/2]

void mars::GpsSensorClass::set_gps_reference_coordinates ( const mars::GpsCoordinates gps_reference)
inline
82 {
84 {
88 std::cout << "Info: [" << name_ << "] Set External GPS Reference: \n" << gps_reference << std::endl;
89 }
90 else
91 {
92 std::cout << "Warning: [" << name_ << "] "
93 << "Trying to set GPS reference but reference was already set. Action has no effect." << std::endl;
94 }
95 }
void set_gps_reference(mars::GpsCoordinates coordinates)
set_gps_reference
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GpsConversion gps_conversion_
Definition gps_sensor_class.h:37
+ Here is the call graph for this function:

◆ Initialize()

BufferDataType mars::GpsSensorClass::Initialize ( const Time timestamp,
std::shared_ptr< void >  measurement,
std::shared_ptr< CoreType latest_core_data 
)
inlinevirtual

Initialize the state of an individual sensor.

Parameters
timestampcurrent timestamp
measurementcurrent sensor measurement
latest_core_data
Returns

Implements mars::SensorInterface.

99 {
100 GpsMeasurementType measurement = *static_cast<GpsMeasurementType*>(sensor_data.get());
101
103 {
104 GpsCoordinates gps_reference(measurement.coordinates_.latitude_, measurement.coordinates_.longitude_,
105 measurement.coordinates_.altitude_);
106
107 gps_conversion_.set_gps_reference(gps_reference);
109
110 std::cout << "Info: [" << name_ << "] Set Internal GPS Reference: \n" << gps_reference << std::endl;
111 }
112
113 GpsSensorData sensor_state;
114 std::string calibration_type;
115
116 if (this->initial_calib_provided_)
117 {
118 calibration_type = "Given";
119
120 GpsSensorData calib = *static_cast<GpsSensorData*>(initial_calib_.get());
121
122 sensor_state.state_ = calib.state_;
123 sensor_state.sensor_cov_ = calib.sensor_cov_;
124 }
125 else
126 {
127 calibration_type = "Auto";
128 std::cout << "GPS calibration AUTO init not implemented yet" << std::endl;
129 exit(EXIT_FAILURE);
130 }
131
132 // Bypass core state for the returned object
133 BufferDataType result(std::make_shared<CoreType>(*latest_core_data.get()),
134 std::make_shared<GpsSensorData>(sensor_state));
135
136 is_initialized_ = true;
137
138 std::cout << "Info: Initialized [" << name_ << "] with [" << calibration_type << "] Calibration at t=" << timestamp
139 << std::endl;
140
141 std::cout << "Info: [" << name_ << "] Calibration(rounded):" << std::endl;
142 std::cout << "\tPosition[m]: [" << sensor_state.state_.p_ig_.transpose() << " ]" << std::endl;
143 std::cout << "\tReference: \n" << gps_conversion_.get_gps_reference() << std::endl;
144
145 return result;
146 }
mars::GpsCoordinates get_gps_reference()
get_gps_reference
bool is_initialized_
True if the sensor has been initialized.
Definition sensor_abs_class.h:24
+ Here is the call graph for this function:

◆ CalcUpdate()

bool mars::GpsSensorClass::CalcUpdate ( const Time timestamp,
std::shared_ptr< void >  measurement,
const CoreStateType prior_core_state_data,
std::shared_ptr< void >  latest_sensor_data,
const Eigen::MatrixXd &  prior_cov,
BufferDataType new_state_data 
)
inlinevirtual

CalcUpdate Calculates the update for an individual sensor definition.

Parameters
timestampcurrent timestamp
measurementcurrent sensor measurement
prior_core_state_data
latest_sensor_data
prior_covPrior covariance containing core, sensor and sensor cross covariance
new_state_dataUpdated state data
Returns
True if the update was successful, false if the update was rejected

Implements mars::SensorInterface.

151 {
152 // Cast the sensor measurement and prior state information
153 GpsMeasurementType* meas = static_cast<GpsMeasurementType*>(measurement.get());
154 GpsSensorData* prior_sensor_data = static_cast<GpsSensorData*>(latest_sensor_data.get());
155
156 // Decompose sensor measurement
157 Eigen::Vector3d p_meas = gps_conversion_.get_enu(meas->coordinates_);
158
159 // Extract sensor state
160 GpsSensorStateType prior_sensor_state(prior_sensor_data->state_);
161
162 // Generate measurement noise matrix and check
163 // if noisevalues from the measurement object should be used
164 Eigen::MatrixXd R_meas_dyn;
165 if (meas->has_meas_noise && use_dynamic_meas_noise_)
166 {
167 meas->get_meas_noise(&R_meas_dyn);
168 }
169 else
170 {
171 R_meas_dyn = this->R_.asDiagonal();
172 }
173 const Eigen::Matrix<double, 3, 3> R_meas = R_meas_dyn;
174
175 const int size_of_core_state = CoreStateType::size_error_;
176 const int size_of_sensor_state = prior_sensor_state.cov_size_;
177 const int size_of_full_error_state = size_of_core_state + size_of_sensor_state;
178 const Eigen::MatrixXd P = prior_cov;
179 assert(P.size() == size_of_full_error_state * size_of_full_error_state);
180
181 // Calculate the measurement jacobian H
182 // const Eigen::Matrix3d I_3 = Eigen::Matrix3d::Identity();
183 const Eigen::Vector3d P_wi = prior_core_state.p_wi_;
184 const Eigen::Matrix3d R_wi = prior_core_state.q_wi_.toRotationMatrix();
185 const Eigen::Vector3d P_ig = prior_sensor_state.p_ig_;
186
187 const Eigen::Vector3d P_gw_w = prior_sensor_state.p_gw_w_;
188 const Eigen::Matrix3d R_gw_w = prior_sensor_state.q_gw_w_.toRotationMatrix();
189
190 // Position
191 const Eigen::Matrix3d Hp_pwi = R_gw_w;
192 const Eigen::Matrix3d Hp_vwi = Eigen::Matrix3d::Zero();
193 const Eigen::Matrix3d Hp_rwi = -R_gw_w * R_wi * Utils::Skew(P_ig);
194 const Eigen::Matrix3d Hp_bw = Eigen::Matrix3d::Zero();
195 const Eigen::Matrix3d Hp_ba = Eigen::Matrix3d::Zero();
196
197 const Eigen::Matrix3d Hp_pig = R_gw_w * R_wi;
198 const Eigen::Matrix3d Hp_pgw_w = Eigen::Matrix3d::Zero(); // I_3;
199 const Eigen::Matrix3d Hp_rgw_w = Eigen::Matrix3d::Zero(); // R_gw_w * Utils::Skew(P_wi + R_wi * P_ig);
200
201 // Assemble the jacobian for the position (horizontal)
202 // H_p = [Hp_pwi Hp_vwi Hp_rwi Hp_bw Hp_ba Hp_pig Hp_pgw_w Hp_rgw_w ];
203 Eigen::MatrixXd H(3, Hp_pwi.cols() + Hp_vwi.cols() + Hp_rwi.cols() + Hp_bw.cols() + Hp_ba.cols() + Hp_pig.cols() +
204 Hp_pgw_w.cols() + Hp_rgw_w.cols());
205
206 H << Hp_pwi, Hp_vwi, Hp_rwi, Hp_bw, Hp_ba, Hp_pig, Hp_pgw_w, Hp_rgw_w;
207
208 // Calculate the residual z = z~ - (estimate)
209 // Position
210 const Eigen::Vector3d p_est = P_gw_w + R_gw_w * (P_wi + R_wi * P_ig);
211 residual_ = Eigen::MatrixXd(p_est.rows(), 1);
212 residual_ = p_meas - p_est;
213
214 // Perform EKF calculations
215 mars::Ekf ekf(H, R_meas, residual_, P);
216 const Eigen::MatrixXd correction = ekf.CalculateCorrection(&chi2_);
217 assert(correction.size() == size_of_full_error_state * 1);
218
219 // Perform Chi2 test
220 if (!chi2_.passed_ && chi2_.do_test_)
221 {
223 return false;
224 }
225
226 Eigen::MatrixXd P_updated = ekf.CalculateCovUpdate();
227 assert(P_updated.size() == size_of_full_error_state * size_of_full_error_state);
228 P_updated = Utils::EnforceMatrixSymmetry(P_updated);
229
230 // Apply Core Correction
231 CoreStateVector core_correction = correction.block(0, 0, CoreStateType::size_error_, 1);
232 CoreStateType corrected_core_state = CoreStateType::ApplyCorrection(prior_core_state, core_correction);
233
234 // Apply Sensor Correction
235 const Eigen::MatrixXd sensor_correction = correction.block(size_of_core_state, 0, size_of_sensor_state, 1);
236 const GpsSensorStateType corrected_sensor_state = ApplyCorrection(prior_sensor_state, sensor_correction);
237
238 // Return Results
239 // CoreState data
240 CoreType core_data;
241 core_data.cov_ = P_updated.block(0, 0, CoreStateType::size_error_, CoreStateType::size_error_);
242 core_data.state_ = corrected_core_state;
243
244 // SensorState data
245 std::shared_ptr<GpsSensorData> sensor_data(std::make_shared<GpsSensorData>());
246 sensor_data->set_cov(P_updated);
247 sensor_data->state_ = corrected_sensor_state;
248
249 BufferDataType state_entry(std::make_shared<CoreType>(core_data), sensor_data);
250
252 {
253 // corrected_sensor_data.ref_to_nav = prior_ref_to_nav;
254 }
255 else
256 {
257 // TODO also estimate ref to nav
258 }
259
260 *new_state_data = state_entry;
261
262 return true;
263 }
bool passed_
Determine if the test is performed or not.
Definition ekf.h:84
bool do_test_
Upper critival value.
Definition ekf.h:83
void PrintReport(const std::string &name)
PrintReport Print a formated report e.g. if the test did not pass.
static constexpr int size_error_
Definition core_state_type.h:38
static CoreStateType ApplyCorrection(CoreStateType state_prior, Eigen::Matrix< double, CoreStateType::size_error_, 1 > correction)
ApplyCorrection.
Definition core_state_type.h:46
Definition ekf.h:92
Eigen::Matrix< double, 3, 1 > get_enu(mars::GpsCoordinates coordinates)
get_enu get current GPS reference coordinates
GpsSensorStateType ApplyCorrection(const GpsSensorStateType &prior_sensor_state, const Eigen::MatrixXd &correction)
Definition gps_sensor_class.h:265
bool use_dynamic_meas_noise_
True if dynamic noise values from measurements should be used.
Definition sensor_abs_class.h:29
Eigen::VectorXd R_
Measurement noise "squared".
Definition update_sensor_abs_class.h:32
Eigen::MatrixXd residual_
Definition update_sensor_abs_class.h:31
static Eigen::MatrixXd EnforceMatrixSymmetry(const Eigen::Ref< const Eigen::MatrixXd > &mat_in)
EnforceMatrixSymmetry.
static Eigen::Matrix3d Skew(const Eigen::Vector3d &v)
skew generate the skew symmetric matrix of v
Eigen::Matrix< double, CoreStateType::size_error_, 1 > CoreStateVector
Definition core_state_type.h:135
+ Here is the call graph for this function:

◆ ApplyCorrection()

GpsSensorStateType mars::GpsSensorClass::ApplyCorrection ( const GpsSensorStateType prior_sensor_state,
const Eigen::MatrixXd &  correction 
)
inline
266 {
267 // state + error state correction
268 // with quaternion from small angle approx -> new state
269
270 GpsSensorStateType corrected_sensor_state;
271 corrected_sensor_state.p_ig_ = prior_sensor_state.p_ig_ + correction.block(0, 0, 3, 1);
272 corrected_sensor_state.p_gw_w_ = prior_sensor_state.p_gw_w_ + correction.block(3, 0, 3, 1);
273 corrected_sensor_state.q_gw_w_ =
274 Utils::ApplySmallAngleQuatCorr(prior_sensor_state.q_gw_w_, correction.block(6, 0, 3, 1));
275 return corrected_sensor_state;
276 }
static Eigen::Quaterniond ApplySmallAngleQuatCorr(const Eigen::Quaterniond &q_prior, const Eigen::Vector3d &correction)
ApplySmallAngleQuatCorr.
+ Here is the call graph for this function:
+ Here is the caller graph for this function:

Member Data Documentation

◆ gps_conversion_

EIGEN_MAKE_ALIGNED_OPERATOR_NEW GpsConversion mars::GpsSensorClass::gps_conversion_

◆ using_external_gps_reference_

bool mars::GpsSensorClass::using_external_gps_reference_

◆ gps_reference_is_set_

bool mars::GpsSensorClass::gps_reference_is_set_

The documentation for this class was generated from the following file: