diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index b68b4ad7dd482f071faf04339656879635232dd2..33aa6fcb4b221ea1dc23f24b21f4a0fb12cbfc0d 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -142,6 +142,10 @@ public:
          **/
         void resetLocalParamUpdated();
 
+        /** \brief perturb state
+         */
+        void perturb(double amplitude = 0.1);
+
         /** \brief Add this state_block to the problem
          **/
         //void addToProblem(ProblemPtr _problem_ptr);
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index f582ae97787b4a3df8d1e98177385427580a90c0..783a56a5e8cb52fe0ae79e2ad14ca4b50976a48c 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -36,4 +36,22 @@ void StateBlock::setFixed(bool _fixed)
 //    _problem_ptr->removeStateBlock(shared_from_this());
 //}
 
+void StateBlock::perturb(double amplitude)
+{
+    using namespace Eigen;
+    VectorXd perturbation(VectorXd::Random(getLocalSize()) * amplitude);
+    if (local_param_ptr_ == nullptr)
+        state_ += perturbation;
+    else
+    {
+        VectorXd state_perturbed(getSize());
+        // Note: LocalParametrizationBase::plus() works with Eigen::Map only. Build all necessary maps:
+        Map<const VectorXd> state_map(state_.data(), getSize());
+        Map<const VectorXd> perturbation_map(perturbation.data(), getLocalSize());
+        Map<VectorXd> state_perturbed_map(state_perturbed.data(), getSize());
+        local_param_ptr_->plus(state_map, perturbation_map, state_perturbed_map);
+        state_ = state_perturbed;
+    }
+}
+
 }
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index bf13016352ac0399328d1fb93ac0892dca210a10..7a5ef929681177d8512cc780bb05e785108ee9d1 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -137,6 +137,10 @@ target_link_libraries(gtest_shared_from_this ${PROJECT_NAME})
 wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp)
 target_link_libraries(gtest_solver_manager ${PROJECT_NAME})
 
+# StateBlock class test
+wolf_add_gtest(gtest_state_block gtest_state_block.cpp)
+target_link_libraries(gtest_state_block ${PROJECT_NAME})
+
 # TimeStamp class test
 wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp)
 target_link_libraries(gtest_time_stamp ${PROJECT_NAME})
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..19d9e3cf63623ef275ee5f8fc8111f4e88ece4d1
--- /dev/null
+++ b/test/gtest_state_block.cpp
@@ -0,0 +1,69 @@
+/*
+ * gtest_state_block.cpp
+ *
+ *  Created on: Mar 31, 2020
+ *      Author: jsola
+ */
+
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
+
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
+
+#include <iostream>
+
+
+using namespace Eigen;
+using namespace std;
+using namespace wolf;
+
+TEST(StateBlock, block_perturb)
+{
+    Vector3d x(10,20,30);
+    StateBlock sb(x);
+
+    sb.perturb(0.5);
+
+    WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
+
+    ASSERT_NE(x.norm(), sb.getState().norm());
+    ASSERT_MATRIX_APPROX(x , sb.getState() , 0.5 * 4); // 4-sigma test...
+}
+
+TEST(StateBlock, angle_perturb)
+{
+    Vector1d x(1.0);
+    StateAngle sb(x(0));
+
+    sb.perturb(0.1);
+
+    WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
+
+    ASSERT_NE(x.norm(), sb.getState().norm());
+    ASSERT_MATRIX_APPROX(x , sb.getState() , 0.1 * 4); // 4-sigma test...
+}
+
+TEST(StateBlock, quaternion_perturb)
+{
+    Vector4d x(0.5,0.5,0.5,0.5);
+    StateQuaternion sb(x);
+
+    sb.perturb(0.01);
+
+    WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
+
+    ASSERT_LT((sb.getState().transpose() * x).norm() , 1.0); // quaternions are not parallel ==> not equal
+    ASSERT_MATRIX_APPROX(x , sb.getState() , 0.01 * 4); // 4-sigma test...
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
+
+
+