near complete detection_lidar_localizer_ekf.

src/detection/detection_lidar_localizer_ekf/ekf_localizer.cpp

@@ -781,6 +781,9 @@ void EKFLocalizer::UpdateGPS(const char *strdata, const unsigned int nSize, cons
 
     if(nSize<1)return;
 
+    static bool bFirst;
+
+
     iv::lidar::ndtpos xndtpos;
     if(!xndtpos.ParseFromArray(strdata,nSize))
     {
@@ -798,6 +801,29 @@ void EKFLocalizer::UpdateGPS(const char *strdata, const unsigned int nSize, cons
     mbCurPoseUpdate = true;
     mMutexCurPose.unlock();
 
+    if(bFirst == true)
+    {
+          Eigen::MatrixXd X(dim_x_, 1);
+          Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_);
+
+          X(IDX::X) = mCurPose.x;
+          X(IDX::Y) = mCurPose.y;
+          X(IDX::YAW) = mCurPose.yaw;
+          X(IDX::YAWB) = 0.0;
+          X(IDX::VX) = 0.0;
+          X(IDX::WZ) = 0.0;
+
+          P(IDX::X, IDX::X) = 0;
+          P(IDX::Y, IDX::Y) = 0;
+          P(IDX::YAW, IDX::YAW) = 0;
+          P(IDX::YAWB, IDX::YAWB) = 0.0001;
+          P(IDX::VX, IDX::VX) = 0.01;
+          P(IDX::WZ, IDX::WZ) = 0.01;
+
+          ekf_.init(X, P, extend_state_step_);
+        bFirst = false;
+    }
+
 
 }
 
@@ -959,7 +985,27 @@ void EKFLocalizer::timerproc()
 
     //  /* set current pose, twist */
     //  setCurrentResult();
+    getcurrentpose();
 
     //  /* publish ekf result */
     //  publishEstimateResult();
 }
+
+void EKFLocalizer::getcurrentpose()
+{
+
+      mekfpose.x = ekf_.getXelement(IDX::X);
+      mekfpose.y = ekf_.getXelement(IDX::Y);
+
+      mekfpose.z = 0;
+      mekfpose.roll = 0;
+      mekfpose.pitch = 0;
+
+      double yaw = ekf_.getXelement(IDX::YAW) + ekf_.getXelement(IDX::YAWB);
+
+      mekfpose.yaw = yaw;
+      mekfpose.roll = mCurPose.roll;
+      mekfpose.pitch = mCurPose.pitch;
+
+
+}

src/detection/detection_lidar_localizer_ekf/ekf_localizer.h

@@ -250,7 +250,11 @@ private:
   bool mbCurPoseUpdate = false;
   QMutex mMutexCurPose;
 
+  iv::pose mekfpose;
+
   void measurementUpdatePose();
 
   void timerproc();
+
+  void getcurrentpose();
 };