diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 99e15bd20351c1dcb61f4d373a842b7ea492d5d6..aa1bfaa4e87fc450c33407e67edc19103beb5f2d 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -146,6 +146,9 @@ public:
          **/
         void resetLocalParamUpdated();
 
+        virtual void setIdentity(bool _notify = true);
+        virtual void setZero    (bool _notify = true);
+
         /** \brief perturb state
          */
         void perturb(double amplitude = 0.1);
@@ -286,6 +289,17 @@ inline double* StateBlock::getStateData()
     return state_.data();
 }
 
+inline void StateBlock::setIdentity(bool _notify)
+{
+    setState( VectorXd::Zero(state_size_), _notify );
+}
+
+inline void StateBlock::setZero(bool _notify)
+{
+    setIdentity(_notify);
+}
+
+
 }// namespace wolf
 
 #endif
diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
index 4760a52b7faa2bf188b520534b8512722d209f63..547e849005666a97fe36e6761b95dca94cf06182 100644
--- a/include/core/state_block/state_homogeneous_3d.h
+++ b/include/core/state_block/state_homogeneous_3d.h
@@ -19,6 +19,7 @@ class StateHomogeneous3d : public StateBlock
         StateHomogeneous3d(bool _fixed = false);
         StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false);
         virtual ~StateHomogeneous3d();
+        virtual void setIdentity(bool _notify = true) override;
 };
 
 inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed) :
@@ -40,6 +41,13 @@ inline StateHomogeneous3d::~StateHomogeneous3d()
     // The local_param_ptr_ pointer is already removed by the base class
 }
 
+inline void StateHomogeneous3d::setIdentity(bool _notify)
+{
+    setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
+}
+
+
+
 } // namespace wolf
 
 #endif /* SRC_STATE_HOMOGENEOUS_3d_H_ */
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index 3732201633ca04eeac5815b9948c8e9d7261f852..a7f460b81324c44fa7d319a88e3569a3f2e796b6 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -20,6 +20,7 @@ class StateQuaternion : public StateBlock
         StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false);
         StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false);
         virtual ~StateQuaternion();
+        virtual void setIdentity(bool _notify = true) override;
 };
 
 inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed) :
@@ -45,6 +46,11 @@ inline StateQuaternion::~StateQuaternion()
     // The local_param_ptr_ pointer is already removed by the base class
 }
 
+inline void StateQuaternion::setIdentity(bool _notify)
+{
+    setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
+}
+
 } // namespace wolf
 
 #endif /* SRC_STATE_QUATERNION_H_ */
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
index d2a1398a8e21daf05603f7810b031f98528b009e..a148d4b0681bd9440bd2bd7488432cc2b94b294a 100644
--- a/test/gtest_state_block.cpp
+++ b/test/gtest_state_block.cpp
@@ -19,6 +19,69 @@ using namespace Eigen;
 using namespace std;
 using namespace wolf;
 
+TEST(StateBlock, identity)
+{
+    Vector3d x(10,20,30);
+    StateBlock sb(x);
+
+    sb.setIdentity();
+
+    ASSERT_MATRIX_APPROX(sb.getState(), Vector3d::Zero() , 1e-20);
+}
+
+TEST(StateAngle, identity)
+{
+    StateAngle sb(1.0);
+
+    sb.setIdentity();
+
+    ASSERT_MATRIX_APPROX(Vector1d::Zero() , sb.getState() , 1e-20);
+}
+
+TEST(StateQuaternion, identity)
+{
+    Vector4d x(0.5,0.5,0.5,0.5);
+    StateQuaternion sb(x);
+
+    sb.setIdentity();
+
+    Vector4d q1(0 ,0,0,1);
+
+    ASSERT_MATRIX_APPROX(q1 , sb.getState() , 1e-20);
+}
+
+TEST(StateBlock, zero)
+{
+    Vector3d x(10,20,30);
+    StateBlock sb(x);
+
+    sb.setZero();
+
+    ASSERT_MATRIX_APPROX(sb.getState(), Vector3d::Zero() , 1e-20);
+}
+
+TEST(StateAngle, zero)
+{
+    StateAngle sb(1.0);
+
+    sb.setZero();
+
+    ASSERT_MATRIX_APPROX(Vector1d::Zero() , sb.getState() , 1e-20);
+}
+
+TEST(StateQuaternion, zero)
+{
+    Vector4d x(0.5,0.5,0.5,0.5);
+    StateQuaternion sb(x);
+
+    sb.setZero();
+
+    Vector4d q1(0 ,0,0,1);
+
+    ASSERT_MATRIX_APPROX(q1 , sb.getState() , 1e-20);
+}
+
+
 TEST(StateBlock, plus)
 {
     Vector3d x(10,20,30);
@@ -33,7 +96,7 @@ TEST(StateBlock, plus)
     ASSERT_MATRIX_APPROX(x + dx , sb.getState() , 1e-20);
 }
 
-TEST(StateBlock, angle_plus)
+TEST(StateAngle, plus)
 {
     StateAngle sb(1.0);
 
@@ -46,7 +109,7 @@ TEST(StateBlock, angle_plus)
     ASSERT_MATRIX_APPROX(Vector1d(1.0) + dx , sb.getState() , 1e-20);
 }
 
-TEST(StateBlock, angle_plus_past_pi)
+TEST(StateAngle, plus_past_pi)
 {
     StateAngle sb(3.14);
 
@@ -59,7 +122,7 @@ TEST(StateBlock, angle_plus_past_pi)
     ASSERT_MATRIX_APPROX(Vector1d(3.14 - 2.0*M_PI) + dx , sb.getState() , 1e-20);
 }
 
-TEST(StateBlock, quaternion_plus_norm)
+TEST(StateQuaternion, plus_norm)
 {
     Vector4d x(0.5,0.5,0.5,0.5);
     StateQuaternion sb(x);
@@ -72,7 +135,7 @@ TEST(StateBlock, quaternion_plus_norm)
     ASSERT_LT((sb.getState().transpose() * x).norm() , 1.0); // quaternions are not parallel ==> not equal
 }
 
-TEST(StateBlock, quaternion_plus_trivial)
+TEST(StateQuaternion, plus_trivial)
 {
     Vector4d x(0,0,0,1);   // identity (0,0,0,1)
     StateQuaternion sb(x);
@@ -86,7 +149,7 @@ TEST(StateBlock, quaternion_plus_trivial)
 }
 
 
-TEST(StateBlock, block_perturb)
+TEST(StateBlock, perturb)
 {
     Vector3d x(10,20,30);
     StateBlock sb(x);
@@ -99,7 +162,7 @@ TEST(StateBlock, block_perturb)
     ASSERT_MATRIX_APPROX(x , sb.getState() , 0.5 * 4); // 4-sigma test...
 }
 
-TEST(StateBlock, angle_perturb)
+TEST(StateAngle, perturb)
 {
     Vector1d x(1.0);
     StateAngle sb(x(0));
@@ -112,7 +175,7 @@ TEST(StateBlock, angle_perturb)
     ASSERT_MATRIX_APPROX(x , sb.getState() , 0.1 * 4); // 4-sigma test...
 }
 
-TEST(StateBlock, quaternion_perturb)
+TEST(StateQuaternion, perturb)
 {
     Vector4d x(0.5,0.5,0.5,0.5);
     StateQuaternion sb(x);