use corrected imu from SINS

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<double> quaternion(orientation.qw(), orientation.qx(),
@@ -170,11 +174,7 @@ void LocalizationIntegImpl::ImuProcessImpl(const ImuData& imu_data) {
       static_cast<Eigen::Vector3d>(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<Eigen::Vector3d>(
       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<int>(state)),

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();

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<bool> keep_running_;
   std::queue<MeasureData> measure_data_queue_;
   int measure_data_queue_size_ = 150;