diff --git a/mrpt_pf_localization/CMakeLists.txt b/mrpt_pf_localization/CMakeLists.txt
index e540a82..bc033b5 100644
--- a/mrpt_pf_localization/CMakeLists.txt
+++ b/mrpt_pf_localization/CMakeLists.txt
@@ -40,10 +40,24 @@ ENDIF()
 ###########
 
 ## Declare a cpp executable
-add_executable(${PROJECT_NAME}_node
-    src/${PROJECT_NAME}_node.cpp
+add_library(${PROJECT_NAME}_core
     src/${PROJECT_NAME}/${PROJECT_NAME}_core.cpp
     include/${PROJECT_NAME}/${PROJECT_NAME}_core.h
+)
+
+target_include_directories(${PROJECT_NAME}_core
+    PUBLIC
+    include
+)
+
+target_link_libraries(${PROJECT_NAME}_core
+  mrpt::gui
+  mrpt::slam
+  mrpt::ros2bridge
+)
+
+add_executable(${PROJECT_NAME}_node
+    src/${PROJECT_NAME}_node.cpp
     include/${PROJECT_NAME}_node.h
 )
 
@@ -66,6 +80,7 @@ target_include_directories(${PROJECT_NAME}_node
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(${PROJECT_NAME}_node
+  ${PROJECT_NAME}_core
   mrpt::gui
   mrpt::slam
   mrpt::ros2bridge
@@ -77,6 +92,7 @@ target_link_libraries(${PROJECT_NAME}_node
 
 install(
   TARGETS
+    ${PROJECT_NAME}_core
     ${PROJECT_NAME}_node
   DESTINATION
     lib/${PROJECT_NAME}
@@ -88,12 +104,16 @@ install(
 )
 
 
-#find_package(ament_cmake_gtest REQUIRED)
-#ament_add_gtest(${PROJECT_NAME}-test test/test_pose_cov_ops.cpp)
-
 ament_export_dependencies()
 
 if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(
+    ${PROJECT_NAME}-test test/test_pf_localization.cpp
+    APPEND_ENV MRPT_LOCALIZATION_SOURCE_DIR=${CMAKE_SOURCE_DIR}
+  )
+  target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core)
+
   find_package(ament_lint_auto REQUIRED)
 
   # the following line skips the linter which checks for copyrights
diff --git a/mrpt_pf_localization/include/mrpt_pf_localization/mrpt_pf_localization_core.h b/mrpt_pf_localization/include/mrpt_pf_localization/mrpt_pf_localization_core.h
index 39c2af7..d0fffba 100644
--- a/mrpt_pf_localization/include/mrpt_pf_localization/mrpt_pf_localization_core.h
+++ b/mrpt_pf_localization/include/mrpt_pf_localization/mrpt_pf_localization_core.h
@@ -166,6 +166,7 @@ class PFLocalizationCore : public mrpt::system::COutputLogger
 		const std::string& simplemap_file);
 
 	// TODO: Getters
+	State getState() const { return state_.fsm_state; }
 
 	/** @} */
 
diff --git a/mrpt_pf_localization/test/test_pf_localization.cpp b/mrpt_pf_localization/test/test_pf_localization.cpp
new file mode 100644
index 0000000..5d85d75
--- /dev/null
+++ b/mrpt_pf_localization/test/test_pf_localization.cpp
@@ -0,0 +1,27 @@
+/* +------------------------------------------------------------------------+
+   |                             mrpt_navigation                            |
+   |                                                                        |
+   | Copyright (c) 2014-2023, Individual contributors, see commit authors   |
+   | See: https://github.com/mrpt-ros-pkg/mrpt_navigation                   |
+   | All rights reserved. Released under BSD 3-Clause license. See LICENSE  |
+   +------------------------------------------------------------------------+ */
+
+#include <gtest/gtest.h>
+#include <mrpt_pf_localization/mrpt_pf_localization_core.h>
+
+TEST(PF_Localization, InitState)
+{
+	PFLocalizationCore loc;
+
+	for (int i = 0; i < 10; i++)
+	{
+		EXPECT_EQ(loc.getState(), PFLocalizationCore::State::UNINITIALIZED);
+		loc.step();
+	}
+}
+
+int main(int argc, char** argv)
+{
+	::testing::InitGoogleTest(&argc, argv);
+	return RUN_ALL_TESTS();
+}