11 #ifndef GPSVELSENSORCLASS_H
12 #define GPSVELSENSORCLASS_H
41 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
58 std::cout <<
"Created: [" << this->
name_ <<
"] Sensor" << std::endl;
108 std::cout <<
"Info: [" <<
name_ <<
"] Set External GPS Reference: \n" << gps_reference << std::endl;
112 std::cout <<
"Warning: [" <<
name_ <<
"] "
113 <<
"Trying to set GPS reference but reference was already set. Action has no effect." << std::endl;
118 std::shared_ptr<CoreType> latest_core_data)
130 std::cout <<
"Info: [" <<
name_ <<
"] Set Internal GPS Reference: \n" << gps_reference << std::endl;
134 std::string calibration_type;
138 calibration_type =
"Given";
147 calibration_type =
"Auto";
148 std::cout <<
"GPS calibration AUTO init not implemented yet" << std::endl;
153 BufferDataType result(std::make_shared<CoreType>(*latest_core_data.get()),
154 std::make_shared<GpsVelSensorData>(sensor_state));
158 std::cout <<
"Info: Initialized [" <<
name_ <<
"] with [" << calibration_type <<
"] Calibration at t=" << timestamp
161 std::cout <<
"Info: [" <<
name_ <<
"] Calibration(rounded):" << std::endl;
162 std::cout <<
"\tPosition[m]: [" << sensor_state.
state_.p_ig_.transpose() <<
" ]" << std::endl;
169 std::shared_ptr<void> latest_sensor_data,
const Eigen::MatrixXd& prior_cov,
178 Eigen::Vector3d v_meas = meas->
velocity_;
185 Eigen::MatrixXd R_meas_dyn;
192 R_meas_dyn = this->
R_.asDiagonal();
194 Eigen::MatrixXd R_meas(R_meas_dyn);
197 const int size_of_sensor_state = prior_sensor_state.
cov_size_;
198 const int size_of_full_error_state = size_of_core_state + size_of_sensor_state;
199 const Eigen::MatrixXd P = prior_cov;
200 assert(P.size() == size_of_full_error_state * size_of_full_error_state);
203 const Eigen::Matrix3d I_3 = Eigen::Matrix3d::Identity();
204 const Eigen::Matrix3d O_3 = Eigen::Matrix3d::Zero();
206 const Eigen::Vector3d omega_i = prior_core_state.
w_m_;
208 const Eigen::Vector3d P_wi = prior_core_state.
p_wi_;
209 const Eigen::Vector3d V_wi = prior_core_state.
v_wi_;
210 const Eigen::Vector3d b_w = prior_core_state.
b_w_;
211 const Eigen::Matrix3d R_wi = prior_core_state.
q_wi_.toRotationMatrix();
212 const Eigen::Vector3d P_ig = prior_sensor_state.
p_ig_;
214 const Eigen::Vector3d P_gw_w = prior_sensor_state.
p_gw_w_;
215 const Eigen::Matrix3d R_gw_w = prior_sensor_state.
q_gw_w_.toRotationMatrix();
218 const Eigen::Matrix3d Hp_pwi = R_gw_w;
219 const Eigen::Matrix3d Hp_vwi = O_3;
220 const Eigen::Matrix3d Hp_rwi = -R_gw_w * R_wi *
Utils::Skew(P_ig);
221 const Eigen::Matrix3d Hp_bw = O_3;
222 const Eigen::Matrix3d Hp_ba = O_3;
224 const Eigen::Matrix3d Hp_pig = R_gw_w * R_wi;
225 const Eigen::Matrix3d Hp_pgw_w = O_3;
226 const Eigen::Matrix3d Hp_rgw_w = O_3;
228 const int num_states =
static_cast<int>(Hp_pwi.cols() + Hp_vwi.cols() + Hp_rwi.cols() + Hp_bw.cols() +
229 Hp_ba.cols() + Hp_pig.cols() + Hp_pgw_w.cols() + Hp_rgw_w.cols());
232 Eigen::MatrixXd H_p(3, num_states);
233 H_p << Hp_pwi, Hp_vwi, Hp_rwi, Hp_bw, Hp_ba, Hp_pig, Hp_pgw_w, Hp_rgw_w;
236 Eigen::MatrixXd H_v(3, num_states);
237 Eigen::Vector3d v_est;
242 const Eigen::Vector3d mu = V_wi + R_wi *
Utils::Skew(omega_i - b_w) * P_ig;
243 const Eigen::Vector3d d_mu = mu / mu.norm();
246 const Eigen::Matrix3d Hv_pwi = O_3;
247 const Eigen::Matrix3d Hv_vwi = R_wi * alpha * d_mu.transpose();
248 const Eigen::Matrix3d Hv_rwi =
252 const Eigen::Matrix3d Hv_bw = O_3;
253 const Eigen::Matrix3d Hv_ba = O_3;
255 const Eigen::Matrix3d Hv_pig = R_wi * alpha * d_mu.transpose() * R_wi *
Utils::Skew(omega_i - b_w);
256 const Eigen::Matrix3d Hv_pgw_w = O_3;
257 const Eigen::Matrix3d Hv_rgw_w = O_3;
259 H_v << Hv_pwi, Hv_vwi, Hv_rwi, Hv_bw, Hv_ba, Hv_pig, Hv_pgw_w, Hv_rgw_w;
260 v_est = R_wi * alpha * (mu).norm();
264 const Eigen::Matrix3d Hv_pwi = O_3;
265 const Eigen::Matrix3d Hv_vwi = I_3;
267 const Eigen::Matrix3d Hv_bw = O_3;
268 const Eigen::Matrix3d Hv_ba = O_3;
270 const Eigen::Matrix3d Hv_pig = R_wi *
Utils::Skew(omega_i - b_w);
271 const Eigen::Matrix3d Hv_pgw_w = O_3;
272 const Eigen::Matrix3d Hv_rgw_w = O_3;
274 H_v << Hv_pwi, Hv_vwi, Hv_rwi, Hv_bw, Hv_ba, Hv_pig, Hv_pgw_w, Hv_rgw_w;
275 v_est = V_wi + R_wi *
Utils::Skew(omega_i - b_w) * P_ig;
279 Eigen::MatrixXd H(H_p.rows() + H_v.rows(), H_v.cols());
284 const Eigen::Vector3d p_est = P_gw_w + R_gw_w * (P_wi + R_wi * P_ig);
285 const Eigen::Vector3d res_p = p_meas - p_est;
288 const Eigen::Vector3d res_v = v_meas - v_est;
291 residual_ = Eigen::MatrixXd(res_p.rows() + res_v.rows(), 1);
297 assert(correction.size() == size_of_full_error_state * 1);
307 assert(P_updated.size() == size_of_full_error_state * size_of_full_error_state);
315 const Eigen::MatrixXd sensor_correction = correction.block(size_of_core_state, 0, size_of_sensor_state, 1);
322 core_data.
state_ = corrected_core_state;
325 std::shared_ptr<GpsVelSensorData> sensor_data(std::make_shared<GpsVelSensorData>());
326 sensor_data->set_cov(P_updated);
327 sensor_data->state_ = corrected_sensor_state;
329 BufferDataType state_entry(std::make_shared<CoreType>(core_data), sensor_data);
340 *new_state_data = state_entry;
346 const Eigen::MatrixXd& correction)
352 corrected_sensor_state.
p_ig_ = prior_sensor_state.
p_ig_ + correction.block(0, 0, 3, 1);
353 corrected_sensor_state.
p_gw_w_ = prior_sensor_state.
p_gw_w_ + correction.block(3, 0, 3, 1);
354 corrected_sensor_state.
q_gw_w_ =
356 return corrected_sensor_state;
bool has_meas_noise
Definition: measurement_base_class.h:23
bool get_meas_noise(Eigen::MatrixXd *meas_noise)
get the measurement noise associated with the current sensor measurement
Definition: measurement_base_class.h:25
int cov_size_
Definition: base_states.h:25
The BaseSensorData class binds the sensor state and covariance matrix.
Definition: bind_sensor_data.h:29
EIGEN_MAKE_ALIGNED_OPERATOR_NEW T state_
Definition: bind_sensor_data.h:30
Eigen::MatrixXd get_full_cov() const
get_full_cov builds the full covariance matrix
Definition: bind_sensor_data.h:63
Eigen::MatrixXd sensor_cov_
covariance of the sensor states
Definition: bind_sensor_data.h:37
The BufferDataType binds the core and sensor state in form of a shared void pointer.
Definition: buffer_data_type.h:36
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 set_dof(const int &value)
set_dof Set degree of freedom for the X2 distribution
void PrintReport(const std::string &name)
PrintReport Print a formated report e.g. if the test did not pass.
Definition: core_state_type.h:21
Eigen::Vector3d w_m_
Definition: core_state_type.h:34
static constexpr int size_error_
Definition: core_state_type.h:38
Eigen::Vector3d v_wi_
Definition: core_state_type.h:28
Eigen::Vector3d b_w_
Definition: core_state_type.h:30
Eigen::Vector3d p_wi_
Definition: core_state_type.h:27
Eigen::Quaternion< double > q_wi_
Definition: core_state_type.h:29
static CoreStateType ApplyCorrection(CoreStateType state_prior, Eigen::Matrix< double, CoreStateType::size_error_, 1 > correction)
ApplyCorrection.
Definition: core_state_type.h:46
Definition: core_type.h:19
CoreStateMatrix cov_
Definition: core_type.h:22
CoreStateType state_
Definition: core_type.h:21
Eigen::MatrixXd CalculateCovUpdate()
CalculateCovUpdate Updating the state covariance after the state update.
Eigen::MatrixXd CalculateCorrection()
Kalman gain.
The GpsConversion class.
Definition: gps_conversion.h:66
void set_gps_reference(mars::GpsCoordinates coordinates)
set_gps_reference
mars::GpsCoordinates get_gps_reference()
get_gps_reference
Eigen::Matrix< double, 3, 1 > get_enu(mars::GpsCoordinates coordinates)
get_enu get current GPS reference coordinates
Definition: gps_w_vel_measurement_type.h:21
GpsCoordinates coordinates_
Definition: gps_w_vel_measurement_type.h:23
Eigen::Vector3d velocity_
Definition: gps_w_vel_measurement_type.h:24
Definition: gps_w_vel_sensor_class.h:34
void set_gps_reference_coordinates(const mars::GpsCoordinates &gps_reference)
Definition: gps_w_vel_sensor_class.h:101
void set_vel_rot_thr(const double &value)
Definition: gps_w_vel_sensor_class.h:73
BufferDataType Initialize(const Time ×tamp, std::shared_ptr< void > sensor_data, std::shared_ptr< CoreType > latest_core_data)
Initialize the state of an individual sensor.
Definition: gps_w_vel_sensor_class.h:117
void set_initial_calib(std::shared_ptr< void > calibration)
set_initial_calib Sets the calibration of an individual sensor
Definition: gps_w_vel_sensor_class.h:90
GpsVelSensorClass(const std::string &name, std::shared_ptr< CoreState > core_states)
Definition: gps_w_vel_sensor_class.h:47
EIGEN_MAKE_ALIGNED_OPERATOR_NEW GpsConversion gps_conversion_
Definition: gps_w_vel_sensor_class.h:43
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.
Definition: gps_w_vel_sensor_class.h:168
GpsVelSensorStateType get_state(const std::shared_ptr< void > &sensor_data)
Definition: gps_w_vel_sensor_class.h:78
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...
Definition: gps_w_vel_sensor_class.h:84
GpsVelSensorStateType ApplyCorrection(const GpsVelSensorStateType &prior_sensor_state, const Eigen::MatrixXd &correction)
Definition: gps_w_vel_sensor_class.h:345
void set_use_vel_rot(const bool &value)
Definition: gps_w_vel_sensor_class.h:68
bool use_vel_rot_
Definition: gps_w_vel_sensor_class.h:37
void set_gps_reference_coordinates(const double &latitude, const double &longitude, const double &altitude)
Definition: gps_w_vel_sensor_class.h:96
Eigen::Vector3d v_rot_axis_
Definition: gps_w_vel_sensor_class.h:36
void set_v_rot_axis(const Eigen::Vector3d &vec)
Definition: gps_w_vel_sensor_class.h:63
bool using_external_gps_reference_
Definition: gps_w_vel_sensor_class.h:44
bool gps_reference_is_set_
Definition: gps_w_vel_sensor_class.h:45
double vel_rot_thr_
Definition: gps_w_vel_sensor_class.h:38
virtual ~GpsVelSensorClass()=default
Definition: gps_w_vel_sensor_state_type.h:20
Eigen::Vector3d p_gw_w_
Definition: gps_w_vel_sensor_state_type.h:25
Eigen::Quaterniond q_gw_w_
Definition: gps_w_vel_sensor_state_type.h:26
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d p_ig_
Definition: gps_w_vel_sensor_state_type.h:24
std::string name_
Name of the individual sensor instance.
Definition: sensor_abs_class.h:23
bool is_initialized_
True if the sensor has been initialized.
Definition: sensor_abs_class.h:24
bool const_ref_to_nav_
True if the reference should not be estimated.
Definition: sensor_abs_class.h:27
bool use_dynamic_meas_noise_
True if dynamic noise values from measurements should be used.
Definition: sensor_abs_class.h:29
Definition: update_sensor_abs_class.h:24
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
Eigen::VectorXd R_
Measurement noise "squared".
Definition: update_sensor_abs_class.h:32
std::shared_ptr< void > initial_calib_
Definition: update_sensor_abs_class.h:37
Chi2 chi2_
Definition: update_sensor_abs_class.h:40
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
static Eigen::Quaterniond ApplySmallAngleQuatCorr(const Eigen::Quaterniond &q_prior, const Eigen::Vector3d &correction)
ApplySmallAngleQuatCorr.
Eigen::Matrix< double, CoreStateType::size_error_, 1 > CoreStateVector
Definition: core_state_type.h:135
The GpsCoordinates struct.
Definition: gps_conversion.h:22
double longitude_
Definition: gps_conversion.h:29
double altitude_
Definition: gps_conversion.h:30
double latitude_
Definition: gps_conversion.h:28