diff --git a/trajectory_planner/include/trajectory_planner/LieEKF.h b/trajectory_planner/include/trajectory_planner/LieEKF.h
index a3d0ba1..d75640a 100644
--- a/trajectory_planner/include/trajectory_planner/LieEKF.h
+++ b/trajectory_planner/include/trajectory_planner/LieEKF.h
@@ -42,6 +42,9 @@ class LieEKF {
 
         void runFilter(Vector3d gyro, Vector3d acc, Vector3d lfpmeasured, Vector3d rfpmeasured, Matrix3d lfrot, Matrix3d rfrot, int* contact, bool update_enaled);
 
+        Vector3d getGBasePose();
+        Vector3d getGBaseVel();
+        Quaterniond getGBaseQuat();
     
     private:
         Vector3d GLeftFootPos_;
diff --git a/trajectory_planner/include/trajectory_planner/QuatEKF.h b/trajectory_planner/include/trajectory_planner/QuatEKF.h
index e76280d..e3f82a2 100644
--- a/trajectory_planner/include/trajectory_planner/QuatEKF.h
+++ b/trajectory_planner/include/trajectory_planner/QuatEKF.h
@@ -43,7 +43,10 @@ class QuatEKF {
 
         void runFilter(Vector3d gyro, Vector3d acc, Vector3d lfpmeasured, Vector3d rfpmeasured, Matrix3d lfrot, Matrix3d rfrot, int* contact, bool update_enaled);
 
-    
+        Vector3d getGBasePose();
+        Vector3d getGBaseVel();
+        Quaterniond getGBaseQuat();
+
     private:
         Vector3d GLeftFootPos_;
         Vector3d GRightFootPos_;
diff --git a/trajectory_planner/src/LieEKF.cpp b/trajectory_planner/src/LieEKF.cpp
index b5dc1de..ceae3e2 100644
--- a/trajectory_planner/src/LieEKF.cpp
+++ b/trajectory_planner/src/LieEKF.cpp
@@ -52,6 +52,19 @@ LieEKF::LieEKF(){
 
 LieEKF::~LieEKF(){}
 
+Vector3d LieEKF::getGBasePose(){
+    return GBasePos_;
+}
+
+Vector3d LieEKF::getGBaseVel(){
+    return GBaseVel_;
+}
+
+Quaterniond LieEKF::getGBaseQuat(){
+    Quaterniond q(GBaseRot_);
+    return q;
+}
+
 void LieEKF::initializeStates(Matrix3d R, Vector3d base_vel, Vector3d base_pos, Vector3d lf_pos, 
                               Vector3d rf_pos, Vector3d gyro_bias, Vector3d acc_bias){
     GBaseRot_ = R;
diff --git a/trajectory_planner/src/QuatEKF.cpp b/trajectory_planner/src/QuatEKF.cpp
index 15fb14c..4fbaa1f 100644
--- a/trajectory_planner/src/QuatEKF.cpp
+++ b/trajectory_planner/src/QuatEKF.cpp
@@ -66,6 +66,18 @@ QuatEKF::~QuatEKF() {
 
 }
 
+Vector3d QuatEKF::getGBasePose(){
+    return GBasePos_;
+}
+
+Vector3d QuatEKF::getGBaseVel(){
+    return GBaseVel_;
+}
+
+Quaterniond QuatEKF::getGBaseQuat(){
+    return GBaseQuat_;
+}
+
 void QuatEKF::initializeStates(Quaterniond q, Vector3d base_vel, Vector3d base_pos, Vector3d lf_pos, 
                                Vector3d rf_pos, Vector3d gyro_bias, Vector3d acc_bias){
     GBaseQuat_ = q;