From 56827af4a56dc2b68e4c7051bffcc4ef8a4c9071 Mon Sep 17 00:00:00 2001 From: zhouyao Date: Wed, 26 Jun 2019 20:07:40 -0700 Subject: [PATCH] use corrected imu from SINS --- .../local_integ/localization_integ_impl.cc | 25 ++++++++++--------- .../local_integ/localization_integ_process.cc | 19 ++++++++++++++ .../local_integ/localization_integ_process.h | 5 ++++ 3 files changed, 37 insertions(+), 12 deletions(-) diff --git a/modules/localization/msf/local_integ/localization_integ_impl.cc b/modules/localization/msf/local_integ/localization_integ_impl.cc index 3993e1d8e6..7412f6f5c2 100644 --- a/modules/localization/msf/local_integ/localization_integ_impl.cc +++ b/modules/localization/msf/local_integ/localization_integ_impl.cc @@ -144,6 +144,10 @@ void LocalizationIntegImpl::ImuProcessImpl(const ImuData& imu_data) { IntegState state; LocalizationEstimate integ_localization; integ_process_->GetResult(&state, &integ_localization); + ImuData corrected_imu; + integ_process_->GetRemoveBiasImu(&corrected_imu); + InertialParameter earth_param; + integ_process_->GetEarthParameter(&earth_param); // check msf running status and set msf_status in integ_localization LocalizationIntegStatus integ_status; @@ -160,8 +164,8 @@ void LocalizationIntegImpl::ImuProcessImpl(const ImuData& imu_data) { } // set linear acceleration - Eigen::Vector3d orig_acceleration(imu_data.fb[0], imu_data.fb[1], - imu_data.fb[2]); + Eigen::Vector3d orig_acceleration(corrected_imu.fb[0], corrected_imu.fb[1], + corrected_imu.fb[2]); const apollo::common::Quaternion& orientation = integ_localization.pose().orientation(); Eigen::Quaternion quaternion(orientation.qw(), orientation.qx(), @@ -170,11 +174,7 @@ void LocalizationIntegImpl::ImuProcessImpl(const ImuData& imu_data) { static_cast(quaternion * orig_acceleration); // Remove gravity. - // Gravity on the Earth's surface varies by around 0.7%. - // From 9.7639 m/s2 on the Nevado Huascaran mountain in Peru - // to 9.8337 m/s2 at the surface of the Arctic Ocean - // Here we simply subtract a standard gravity, by definition, 9.80665. - vec_acceleration(2) -= 9.80665; + vec_acceleration(2) -= earth_param.g; apollo::common::Point3D* linear_acceleration = posepb_loc->mutable_linear_acceleration(); @@ -192,8 +192,9 @@ void LocalizationIntegImpl::ImuProcessImpl(const ImuData& imu_data) { linear_acceleration_vrf->set_z(vec_acceleration_vrf(2)); // set angular velocity - Eigen::Vector3d orig_angular_velocity(imu_data.wibb[0], imu_data.wibb[1], - imu_data.wibb[2]); + Eigen::Vector3d orig_angular_velocity(corrected_imu.wibb[0], + corrected_imu.wibb[1], + corrected_imu.wibb[2]); Eigen::Vector3d vec_angular_velocity = static_cast( quaternion.toRotationMatrix() * orig_angular_velocity); apollo::common::Point3D* angular_velocity = @@ -204,9 +205,9 @@ void LocalizationIntegImpl::ImuProcessImpl(const ImuData& imu_data) { apollo::common::Point3D* angular_velocity_vrf = posepb_loc->mutable_angular_velocity_vrf(); - angular_velocity_vrf->set_x(imu_data.wibb[0]); - angular_velocity_vrf->set_y(imu_data.wibb[1]); - angular_velocity_vrf->set_z(imu_data.wibb[2]); + angular_velocity_vrf->set_x(corrected_imu.wibb[0]); + angular_velocity_vrf->set_y(corrected_imu.wibb[1]); + angular_velocity_vrf->set_z(corrected_imu.wibb[2]); lastest_integ_localization_ = LocalizationResult(LocalizationMeasureState(static_cast(state)), diff --git a/modules/localization/msf/local_integ/localization_integ_process.cc b/modules/localization/msf/local_integ/localization_integ_process.cc index a160fc88b2..2a83fb9b41 100644 --- a/modules/localization/msf/local_integ/localization_integ_process.cc +++ b/modules/localization/msf/local_integ/localization_integ_process.cc @@ -35,6 +35,8 @@ LocalizationIntegProcess::LocalizationIntegProcess() integ_state_(IntegState::NOT_INIT), ins_pva_(), pva_covariance_{0.0}, + corrected_imu_(), + earth_param_(), keep_running_(false), measure_data_queue_size_(150), delay_output_counter_(0) {} @@ -102,6 +104,8 @@ void LocalizationIntegProcess::RawImuProcess(const ImuData &imu_msg) { // add imu msg and get current predict pose sins_->AddImu(imu_msg); sins_->GetPose(&ins_pva_, pva_covariance_); + sins_->GetRemoveBiasImu(&corrected_imu_); + sins_->GetEarthParameter(&earth_param_); if (sins_->IsSinsAligned()) { integ_state_ = IntegState::NOT_STABLE; @@ -241,6 +245,21 @@ void LocalizationIntegProcess::GetResult(IntegState *state, InsPva *sins_pva, return; } +void LocalizationIntegProcess::GetRemoveBiasImu(ImuData *imu_data) { + CHECK_NOTNULL(imu_data); + + *imu_data = corrected_imu_; + return; +} + +void LocalizationIntegProcess::GetEarthParameter( + InertialParameter *earth_param) { + CHECK_NOTNULL(earth_param); + + *earth_param = earth_param_; + return; +} + void LocalizationIntegProcess::MeasureDataProcess( const MeasureData &measure_msg) { measure_data_queue_mutex_.lock(); diff --git a/modules/localization/msf/local_integ/localization_integ_process.h b/modules/localization/msf/local_integ/localization_integ_process.h index 0aba6564a8..82d2be4835 100644 --- a/modules/localization/msf/local_integ/localization_integ_process.h +++ b/modules/localization/msf/local_integ/localization_integ_process.h @@ -60,6 +60,8 @@ class LocalizationIntegProcess { void GetResult(IntegState *state, LocalizationEstimate *localization); void GetResult(IntegState *state, InsPva *sins_pva, double pva_covariance[9][9]); + void GetRemoveBiasImu(ImuData *imu_data); + void GetEarthParameter(InertialParameter *earth_param); // itegration measure data process void MeasureDataProcess(const MeasureData &measure_msg); @@ -89,6 +91,9 @@ class LocalizationIntegProcess { InsPva ins_pva_; double pva_covariance_[9][9]; + ImuData corrected_imu_; + InertialParameter earth_param_; + std::atomic keep_running_; std::queue measure_data_queue_; int measure_data_queue_size_ = 150; -- GitLab