diff --git a/msf_core/CMakeLists.txt b/msf_core/CMakeLists.txt
index d120b52f..f748408c 100644
--- a/msf_core/CMakeLists.txt
+++ b/msf_core/CMakeLists.txt
@@ -9,8 +9,11 @@ project(msf_core)
 #  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
 set(CMAKE_BUILD_TYPE Release)
 
-find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs dynamic_reconfigure msf_timing tf)
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake/)
+
+find_package(catkin REQUIRED COMPONENTS sensor_msgs dynamic_reconfigure msf_timing tf)
 find_package(Eigen REQUIRED)
+find_package(GLog REQUIRED)
 
 include_directories(include ${Eigen_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
 
@@ -27,15 +30,16 @@ add_definitions (-DENABLE_MSF_TIMING)
 generate_dynamic_reconfigure_options(cfg/MSF_Core.cfg)
 
 catkin_package(
-    DEPENDS eigen
+    DEPENDS eigen GLog
     CATKIN_DEPENDS roscpp sensor_msgs dynamic_reconfigure msf_timing tf
     INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS}
     LIBRARIES msf_core similaritytransform
-    CFG_EXTRAS export_flags.cmake
+    CFG_EXTRAS export_flags.cmake FindGLog.cmake
 )
 
-add_library(msf_core src/lib/msf_tools.cc src/lib/falsecolor.cc src/lib/gps_conversion.cc)
-target_link_libraries(msf_core ${catkin_LIBRARIES})
+add_library(${PROJECT_NAME} src/lib/msf_tools.cc src/lib/falsecolor.cc 
+                     src/lib/gps_conversion.cc)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GLOG_LIBRARY})
 add_dependencies(msf_core ${PROJECT_NAME}_gencfg) 
 
 add_library(similaritytransform src/similaritytransform.cc)
@@ -44,5 +48,6 @@ target_link_libraries(similaritytransform ${catkin_LIBRARIES})
 add_executable(test_similaritytransform src/test/test_similaritytransform.cc)
 target_link_libraries(test_similaritytransform similaritytransform)
 
-# in case anything here depends on the dynreconf file
-# add_dependencies(example_node ${${PROJECT_NAME}_EXPORTED_TARGETS})
+catkin_add_gtest(test_static_statelist src/test/test_staticstatelist.cc)
+target_link_libraries(test_static_statelist pthread ${PROJECT_NAME} ${GLOG_LIBRARY})
+
diff --git a/msf_core/cmake/FindGLog.cmake b/msf_core/cmake/FindGLog.cmake
new file mode 100644
index 00000000..037f9163
--- /dev/null
+++ b/msf_core/cmake/FindGLog.cmake
@@ -0,0 +1,104 @@
+# - Try to find the Google Glog library
+#
+#  This module defines the following variables
+#
+#  GLOG_FOUND - Was Glog found
+#  GLOG_INCLUDE_DIRS - the Glog include directories
+#  GLOG_LIBRARIES - Link to this
+#
+#  This module accepts the following variables
+#
+#  GLOG_ROOT - Can be set to Glog install path or Windows build path
+#
+#=============================================================================
+#  FindGlog.cmake, adapted from FindBullet.cmake which has the following
+#  copyright -
+#-----------------------------------------------------------------------------
+# Copyright 2009 Kitware, Inc.
+# Copyright 2009 Philip Lowman <philip@yhbt.com>
+#
+# Distributed under the OSI-approved BSD License (the "License");
+# see accompanying file Copyright.txt for details.
+#
+# This software is distributed WITHOUT ANY WARRANTY; without even the
+# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+# See the License for more information.
+#=============================================================================
+# (To distribute this file outside of CMake, substitute the full
+#  License text for the above reference.)
+
+if (NOT DEFINED GLOG_ROOT)
+    set (GLOG_ROOT /usr /usr/local)
+endif (NOT DEFINED GLOG_ROOT)
+
+if(MSVC)
+set(LIB_PATHS ${GLOG_ROOT} ${GLOG_ROOT}/Release)
+else(MSVC)
+set (LIB_PATHS ${GLOG_ROOT} ${GLOG_ROOT}/lib)
+endif(MSVC)
+
+macro(_FIND_GLOG_LIBRARY _var)
+  find_library(${_var}
+     NAMES 
+        ${ARGN}
+     PATHS
+		${LIB_PATHS}
+     PATH_SUFFIXES lib
+  )
+  mark_as_advanced(${_var})
+endmacro()
+
+macro(_GLOG_APPEND_LIBRARIES _list _release)
+   set(_debug ${_release}_DEBUG)
+   if(${_debug})
+      set(${_list} ${${_list}} optimized ${${_release}} debug ${${_debug}})
+   else()
+      set(${_list} ${${_list}} ${${_release}})
+   endif()
+endmacro()
+
+if(MSVC)
+    find_path(GLOG_INCLUDE_DIR NAMES raw_logging.h
+        PATHS
+		  ${GLOG_ROOT}/src/windows
+          ${GLOG_ROOT}/src/windows/glog
+		  )
+else(MSVC)
+	# Linux/OS X builds
+    find_path(GLOG_INCLUDE_DIR NAMES raw_logging.h
+        PATHS
+          ${GLOG_ROOT}/include/glog
+		  )
+endif(MSVC)
+
+# Find the libraries
+if(MSVC)
+    _FIND_GLOG_LIBRARY(GLOG_LIBRARY                   libglog.lib)
+else(MSVC)
+	# Linux/OS X builds
+    _FIND_GLOG_LIBRARY(GLOG_LIBRARY                   libglog.so)
+endif(MSVC)
+
+message("glog library = " ${GLOG_LIBRARY})
+
+# handle the QUIETLY and REQUIRED arguments and set GLOG_FOUND to TRUE if 
+# all listed variables are TRUE
+include("${CMAKE_ROOT}/Modules/FindPackageHandleStandardArgs.cmake")
+FIND_PACKAGE_HANDLE_STANDARD_ARGS(Glog DEFAULT_MSG
+    GLOG_LIBRARY)
+
+if(MSVC)
+    string(REGEX REPLACE "/glog$" "" VAR_WITHOUT ${GLOG_INCLUDE_DIR})
+    string(REGEX REPLACE "/windows$" "" VAR_WITHOUT ${VAR_WITHOUT})
+    set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIRS} "${VAR_WITHOUT}")
+    string(REGEX REPLACE "/libglog.lib" "" GLOG_LIBRARY_DIR ${GLOG_LIBRARY})
+else(MSVC)
+	# Linux/OS X builds
+    set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR})
+    string(REGEX REPLACE "/libglog.so" "" GLOG_LIBRARY_DIR ${GLOG_LIBRARY})
+endif(MSVC)
+
+if(GLOG_FOUND)
+   _GLOG_APPEND_LIBRARIES(GLOG GLOG_LIBRARY)
+endif()
+
diff --git a/msf_core/src/test/test_msf_core.cc b/msf_core/include/msf_core/testing_entrypoint.h
similarity index 55%
rename from msf_core/src/test/test_msf_core.cc
rename to msf_core/include/msf_core/testing_entrypoint.h
index 2baa764a..8f30aed4 100644
--- a/msf_core/src/test/test_msf_core.cc
+++ b/msf_core/include/msf_core/testing_entrypoint.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2012-2013 Simon Lynen, ASL, ETH Zurich, Switzerland
+ * Copyright (C) 2014 Simon Lynen, ASL, ETH Zurich, Switzerland
  * You can contact the author at <slynen at ethz dot ch>
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -14,7 +14,23 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
-#include <ros/ros.h>
-#include <msf_core/msf_core.h>
-int main(int argc, char** argv) { }
 
+#ifndef TESTING_ENTRYPOINT_H_
+#define TESTING_ENTRYPOINT_H_
+
+#include <gtest/gtest.h>
+#include <glog/logging.h>
+
+// Let the Eclipse parser see the macro.
+#ifndef TEST
+#define TEST(a, b) int Test_##a##_##b()
+#endif
+
+#define MSF_UNITTEST_ENTRYPOINT\
+  int main(int argc, char** argv) {\
+ ::testing::InitGoogleTest(&argc, argv);\
+  google::InitGoogleLogging(argv[0]);\
+  return RUN_ALL_TESTS();\
+}
+
+#endif   // TESTING_ENTRYPOINT_H_
diff --git a/msf_core/package.xml b/msf_core/package.xml
index 17b66fa3..abb4a4d2 100644
--- a/msf_core/package.xml
+++ b/msf_core/package.xml
@@ -19,11 +19,11 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <!-- Dependencies needed to compile this package. -->
-  <build_depend>roscpp</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>msf_timing</build_depend>
   <build_depend>tf</build_depend>
+  <build_depend>GLog</build_depend>
 
   <!-- Dependencies needed after this package is compiled. -->
   <run_depend>roscpp</run_depend>
@@ -31,4 +31,5 @@
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>msf_timing</run_depend>
   <run_depend>tf</run_depend>
+  <run_depend>GLog</run_depend>
 </package>
diff --git a/msf_core/src/test/test_staticstatelist.cc b/msf_core/src/test/test_staticstatelist.cc
index 0dd2bb29..b3c45db9 100644
--- a/msf_core/src/test/test_staticstatelist.cc
+++ b/msf_core/src/test/test_staticstatelist.cc
@@ -15,16 +15,13 @@
  * limitations under the License.
  */
 
-#include <msf_core/msf_core.hpp>
-#include <gtest/gtest.h>
+#include <msf_core/msf_core.h>
+#include <msf_core/testing_entrypoint.h>
 
-#define WITHTESTS 1
-
-#if (WITHTESTS == 1)
 // Test calculated sizes.
-TEST(CompileTimeComputation, stateSizeCalculation) {
+TEST(MSF_Core, CompileTimeComputation_StateSizeCalculation) {
   using namespace msf_core;
-  enum {
+  enum StateDefinition {
     a,
     b,
     c,
@@ -35,29 +32,30 @@ TEST(CompileTimeComputation, stateSizeCalculation) {
 
   // Setup some state type.
   typedef boost::fusion::vector<
-  StateVar_T<Eigen::Matrix<double, vectorlength1, 1>, a >,
-  StateVar_T<Eigen::Quaterniond, b >,
-  StateVar_T<Eigen::Matrix<double, 1, 1>, c >,
-  StateVar_T<Eigen::Matrix<double, vectorlength2, 1>, d >
+      msf_core::StateVar_T<Eigen::Matrix<double, vectorlength1, 1>, a>,
+      msf_core::StateVar_T<Eigen::Quaterniond, b>,
+      msf_core::StateVar_T<Eigen::Matrix<double, 1, 1>, c>,
+      msf_core::StateVar_T<Eigen::Matrix<double, vectorlength2, 1>, d>
   > fullState_T;
-  typedef GenericState_T<fullState_T> EKFState;
+  typedef msf_core::GenericState_T<fullState_T, StateDefinition> EKFState;
 
-  EKFState somestate;
-  ASSERT_EQ(somestate.get<0>().sizeInCorrection_, vectorlength1);
-  ASSERT_EQ(somestate.get<1>().sizeInCorrection_, 3);
-  ASSERT_EQ(somestate.get<2>().sizeInCorrection_, 1);
-  ASSERT_EQ(somestate.get<3>().sizeInCorrection_, vectorlength2);
-
-  ASSERT_EQ(somestate.get<0>().sizeInState_, vectorlength1);
-  ASSERT_EQ(somestate.get<1>().sizeInState_, 4);
-  ASSERT_EQ(somestate.get<2>().sizeInState_, 1);
-  ASSERT_EQ(somestate.get<3>().sizeInState_, vectorlength2);
+  const EKFState somestate;
+
+  EXPECT_EQ(somestate.GetStateVariable<0>().sizeInCorrection_, vectorlength1);
+  EXPECT_EQ(somestate.GetStateVariable<1>().sizeInCorrection_, 3);
+  EXPECT_EQ(somestate.GetStateVariable<2>().sizeInCorrection_, 1);
+  EXPECT_EQ(somestate.GetStateVariable<3>().sizeInCorrection_, vectorlength2);
+
+  EXPECT_EQ(somestate.GetStateVariable<0>().sizeInState_, vectorlength1);
+  EXPECT_EQ(somestate.GetStateVariable<1>().sizeInState_, 4);
+  EXPECT_EQ(somestate.GetStateVariable<2>().sizeInState_, 1);
+  EXPECT_EQ(somestate.GetStateVariable<3>().sizeInState_, vectorlength2);
 }
 
 // Test indices of statevars in state vectors.
-TEST(CompileTimeComputation, stateIndexCalculation) {
+TEST(MSF_Core, CompileTimeComputation_StateIndexCalculation) {
   using namespace msf_core;
-  enum {
+  enum StateDefinition {
     a,
     b,
     c,
@@ -68,55 +66,55 @@ TEST(CompileTimeComputation, stateIndexCalculation) {
 
   // Setup some state type.
   typedef boost::fusion::vector<
-  StateVar_T<Eigen::Matrix<double, vectorlength1, 1>, a >,
-  StateVar_T<Eigen::Quaterniond, b >,
-  StateVar_T<Eigen::Matrix<double, 1, 1>, c >,
-  StateVar_T<Eigen::Matrix<double, vectorlength2, 1>, d >
+      StateVar_T<Eigen::Matrix<double, vectorlength1, 1>, a>,
+      StateVar_T<Eigen::Quaterniond, b>,
+      StateVar_T<Eigen::Matrix<double, 1, 1>, c>,
+      StateVar_T<Eigen::Matrix<double, vectorlength2, 1>, d>
   > fullState_T;
-  typedef GenericState_T<fullState_T> EKFState;
+  typedef GenericState_T<fullState_T, StateDefinition> EKFState;
 
   EKFState somestate;
   static const int idxstartcorr1 = msf_tmp::GetStartIndex<fullState_T,
-      StateVar_T<Eigen::Matrix<double, vectorlength1, 1>, a >,
+      StateVar_T<Eigen::Matrix<double, vectorlength1, 1>, a>,
       msf_tmp::CorrectionStateLengthForType>::value;
   static const int idxstartstate1 = msf_tmp::GetStartIndex<fullState_T,
-      StateVar_T<Eigen::Matrix<double, vectorlength1, 1>, a >,
+      StateVar_T<Eigen::Matrix<double, vectorlength1, 1>, a>,
       msf_tmp::StateLengthForType>::value;
-  ASSERT_EQ(idxstartcorr1, 0);
-  ASSERT_EQ(idxstartstate1, 0);
+  EXPECT_EQ(idxstartcorr1, 0);
+  EXPECT_EQ(idxstartstate1, 0);
 
   static const int idxstartcorr2 = msf_tmp::GetStartIndex<fullState_T,
-      StateVar_T<Eigen::Quaterniond, b >,
+      StateVar_T<Eigen::Quaterniond, b>,
       msf_tmp::CorrectionStateLengthForType>::value;
   static const int idxstartstate2 = msf_tmp::GetStartIndex<fullState_T,
-      StateVar_T<Eigen::Quaterniond, b >, msf_tmp::StateLengthForType>::value;
-  ASSERT_EQ(idxstartcorr2, vectorlength1);
-  ASSERT_EQ(idxstartstate2, vectorlength1);
+      StateVar_T<Eigen::Quaterniond, b>, msf_tmp::StateLengthForType>::value;
+  EXPECT_EQ(idxstartcorr2, vectorlength1);
+  EXPECT_EQ(idxstartstate2, vectorlength1);
 
   static const int idxstartcorr3 = msf_tmp::GetStartIndex<fullState_T,
-      StateVar_T<Eigen::Matrix<double, 1, 1>, c >,
+      StateVar_T<Eigen::Matrix<double, 1, 1>, c>,
       msf_tmp::CorrectionStateLengthForType>::value;
   static const int idxstartstate3 = msf_tmp::GetStartIndex<fullState_T,
-      StateVar_T<Eigen::Matrix<double, 1, 1>, c >,
+      StateVar_T<Eigen::Matrix<double, 1, 1>, c>,
       msf_tmp::StateLengthForType>::value;
-  ASSERT_EQ(idxstartcorr3, vectorlength1 + 3);
-  ASSERT_EQ(idxstartstate3, vectorlength1 + 4);
+  EXPECT_EQ(idxstartcorr3, vectorlength1 + 3);
+  EXPECT_EQ(idxstartstate3, vectorlength1 + 4);
 
   static const int idxstartcorr4 = msf_tmp::GetStartIndex<fullState_T,
-      StateVar_T<Eigen::Matrix<double, vectorlength2, 1>, d >,
+      StateVar_T<Eigen::Matrix<double, vectorlength2, 1>, d>,
       msf_tmp::CorrectionStateLengthForType>::value;
   static const int idxstartstate4 = msf_tmp::GetStartIndex<fullState_T,
-      StateVar_T<Eigen::Matrix<double, vectorlength2, 1>, d >,
+      StateVar_T<Eigen::Matrix<double, vectorlength2, 1>, d>,
       msf_tmp::StateLengthForType>::value;
-  ASSERT_EQ(idxstartcorr4, vectorlength1 + 3 + 1);
-  ASSERT_EQ(idxstartstate4, vectorlength1 + 4 + 1);
+  EXPECT_EQ(idxstartcorr4, vectorlength1 + 3 + 1);
+  EXPECT_EQ(idxstartstate4, vectorlength1 + 4 + 1);
 
 }
 
 // Tests compile time computed values for the state.
-TEST(CompileTimeComputation, stateLengthCalculation) {
+TEST(MSF_Core, CompileTimeComputation_StateLengthCalculation) {
   using namespace msf_core;
-  enum {
+  enum StateDefinition {
     a,
     b,
     c,
@@ -127,20 +125,22 @@ TEST(CompileTimeComputation, stateLengthCalculation) {
 
   //setup some state type
   typedef boost::fusion::vector<
-  StateVar_T<Eigen::Matrix<double, vectorlength1, 1>, a >,
-  StateVar_T<Eigen::Quaterniond, b >,
-  StateVar_T<Eigen::Matrix<double, 1, 1>, c >,
-  StateVar_T<Eigen::Matrix<double, vectorlength2, 1>, d >
+      StateVar_T<Eigen::Matrix<double, vectorlength1, 1>, a>,
+      StateVar_T<Eigen::Quaterniond, b>,
+      StateVar_T<Eigen::Matrix<double, 1, 1>, c>,
+      StateVar_T<Eigen::Matrix<double, vectorlength2, 1>, d>
   > fullState_T;
-  typedef GenericState_T<fullState_T> EKFState;
+  typedef GenericState_T<fullState_T, StateDefinition> EKFState;
 
-  ASSERT_EQ(EKFState::nStateVarsAtCompileTime, 4);
-  ASSERT_EQ(EKFState::nStatesAtCompileTime, vectorlength1 + vectorlength2 + 4 + 1);
-  ASSERT_EQ(EKFState::nErrorStatesAtCompileTime, vectorlength1 + vectorlength2 + 3 +1);
+  EXPECT_EQ(EKFState::nStateVarsAtCompileTime, 4);
+  EXPECT_EQ(EKFState::nStatesAtCompileTime,
+            vectorlength1 + vectorlength2 + 4 + 1);
+  EXPECT_EQ(EKFState::nErrorStatesAtCompileTime,
+            vectorlength1 + vectorlength2 + 3 + 1);
 }
 
-TEST(RuntimeTimeComputation, copyForNonPropagationStates) {
-  enum {
+TEST(MSF_Core, RuntimeTimeComputation_CopyForNonPropagationStates) {
+  enum StateDefinition {
     p_,
     v_,
     q_,
@@ -153,94 +153,60 @@ TEST(RuntimeTimeComputation, copyForNonPropagationStates) {
   };
 
   typedef boost::fusion::vector<
-  msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, p_, true>,
-  msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, v_, true>,
-  msf_core::StateVar_T<Eigen::Quaternion<double>, q_, true>,
-  msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, b_w_, true>,
-  msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, b_a_, true>,
-
-  msf_core::StateVar_T<Eigen::Matrix<double, 1, 1>, L_>,
-  msf_core::StateVar_T<Eigen::Quaternion<double>, q_wv_>,
-  msf_core::StateVar_T<Eigen::Quaternion<double>, q_ci_>,
-  msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, p_ci_>
+      msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, p_, true>,
+      msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, v_, true>,
+      msf_core::StateVar_T<Eigen::Quaternion<double>, q_, true>,
+      msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, b_w_, true>,
+      msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, b_a_, true>,
+
+      msf_core::StateVar_T<Eigen::Matrix<double, 1, 1>, L_>,
+      msf_core::StateVar_T<Eigen::Quaternion<double>, q_wv_>,
+      msf_core::StateVar_T<Eigen::Quaternion<double>, q_ci_>,
+      msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, p_ci_>
   > fullState_T;
 
-  typedef msf_core::GenericState_T<fullState_T> EKFState;
+  typedef msf_core::GenericState_T<fullState_T, StateDefinition> EKFState;
 
   EKFState first_state;
   EKFState second_state;
 
-  first_state.reset();
-  second_state.reset();
+  first_state.Reset();
+  second_state.Reset();
 
-  first_state.get<msf_core::p_>().state_.setRandom();
+  Eigen::Matrix<double, 3, 1> p;
+  p.setRandom();
+  first_state.Set<p_>(p);
 
-  first_state.get<msf_core::q_>().state_.w() = 0.4;
-  first_state.get<msf_core::q_>().state_.x() = 0.1;
-  first_state.get<msf_core::q_>().state_.y() = 0.2;
-  first_state.get<msf_core::q_>().state_.z() = 0.7;
-  first_state.get<msf_core::q_>().state_.normalize();
+  Eigen::Quaternion<double> q;
+  q.w() = 0.4;
+  q.x() = 0.1;
+  q.y() = 0.2;
+  q.z() = 0.7;
+  q.normalize();
+  first_state.Set<q_>(q);
 
   boost::fusion::for_each(
-      first_state.statevars_,
-      msf_tmp::CopyNonPropagationStates<EKFState>(second_state)
-  );
-
-  ASSERT_DOUBLE_EQ(first_state.get<msf_core::p_>().state_(0),
-                   second_state.get<msf_core::p_>().state_(0));
-  ASSERT_DOUBLE_EQ(first_state.get<msf_core::p_>().state_(1),
-                   second_state.get<msf_core::p_>().state_(1));
-  ASSERT_DOUBLE_EQ(first_state.get<msf_core::p_>().state_(2),
-                   second_state.get<msf_core::p_>().state_(2));
-
-  ASSERT_DOUBLE_EQ(first_state.get<msf_core::q_>().state_.w(),
-                   second_state.get<msf_core::q_>().state_.w());
-  ASSERT_DOUBLE_EQ(first_state.get<msf_core::q_>().state_.x(),
-                   second_state.get<msf_core::q_>().state_.x());
-  ASSERT_DOUBLE_EQ(first_state.get<msf_core::q_>().state_.y(),
-                   second_state.get<msf_core::q_>().state_.y());
-  ASSERT_DOUBLE_EQ(first_state.get<msf_core::q_>().state_.z(),
-                   second_state.get<msf_core::q_>().state_.z());
+      first_state.statevars,
+      msf_tmp::CopyNonPropagationStates<EKFState>(second_state));
+
+  const EKFState& const_ref_first_state = first_state;
+  const EKFState& const_ref_second_state = second_state;
+
+  EXPECT_DOUBLE_EQ(const_ref_first_state.Get<p_>()(0),
+                   const_ref_second_state.Get<p_>()(0));
+  EXPECT_DOUBLE_EQ(const_ref_first_state.Get<p_>()(1),
+                   const_ref_second_state.Get<p_>()(1));
+  EXPECT_DOUBLE_EQ(const_ref_first_state.Get<p_>()(2),
+                   const_ref_second_state.Get<p_>()(2));
+
+  EXPECT_DOUBLE_EQ(const_ref_first_state.Get<q_>().w(),
+                   const_ref_second_state.Get<q_>().w());
+  EXPECT_DOUBLE_EQ(const_ref_first_state.Get<q_>().x(),
+                   const_ref_second_state.Get<q_>().x());
+  EXPECT_DOUBLE_EQ(const_ref_first_state.Get<q_>().y(),
+                   const_ref_second_state.Get<q_>().y());
+  EXPECT_DOUBLE_EQ(const_ref_first_state.Get<q_>().z(),
+                   const_ref_second_state.Get<q_>().z());
 }
 
-#endif
-
-int main(int argc, char** argv) {
-
-  // An instantiation of a state.
-  msf_core::EKFState somestate;
-  msf_core::EKFState otherstate;
-
-  std::cout << "otherstate init: " << otherstate.get<0>().initialized
-      << std::endl;
-  somestate.get<0>().initialized = true;
-  std::cout << "somestate init: " << somestate.get<0>().initialized
-      << std::endl;
-  std::cout << "otherstate init: " << otherstate.get<0>().initialized
-      << std::endl;
-
-  std::cout << "name " << somestate.get<0>().name_ << std::endl;
-
-  // Number of state variables.
-  std::cout << "nstatevars: " << msf_core::EKFState::nStatesAtCompileTime
-      << std::endl;
-
-  // Number of states.
-  std::cout << "nstates: " << msf_core::EKFState::nStatesAtCompileTime
-      << std::endl;
-
-  // Number of correction states.
-  std::cout << "nerrortates: " << msf_core::EKFState::nErrorStatesAtCompileTime
-      << std::endl;
-
-  // Apply correction to all states.
-  Eigen::Matrix<double, msf_core::EKFState::nErrorStatesAtCompileTime, 1> correction;
-  std::cout << "passing random correction vector" << std::endl;
-  correction.setRandom();
-  somestate.Correct(correction);
-
-#if (WITHTESTS == 1)
-  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-#endif
-}
+MSF_UNITTEST_ENTRYPOINT
diff --git a/msf_core/src/test/test_tmptestbed.cc b/msf_core/src/test/test_tmptestbed.cc
index 7c4c2019..24eedf64 100644
--- a/msf_core/src/test/test_tmptestbed.cc
+++ b/msf_core/src/test/test_tmptestbed.cc
@@ -15,7 +15,8 @@
  * limitations under the License.
  */
 
-#include <msf_core/msf_state.hpp>
+#include <msf_core/msf_state.h>
+
 int main(int argc, char** argv) {
 
   msf_core::EKFState somestate;