diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 9462efe7a6dd314ff2ab94cbf923ee79f5c4e21e..8880fc0a2cfb09a46712a502601a801a1f923a39 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -301,9 +301,9 @@ inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _sub_struct
     for (const char key : structure)
     {
         const auto& sb = getStateBlock(key);
-        if (!sb){
-            WOLF_ERROR("Stateblock key ", key, " not in the structure");
-        }
+
+        assert(sb != nullptr && "Requested StateBlock key not in the structure");
+
         state.segment(index,sb->getSize()) = sb->getState();
         index += sb->getSize();
     }
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index 44fdc98d588d46cbdf996ff94ec788f38bf7ce9c..0d3cafcc80c602a4de1bf59b9ce7c2ddd9123c9a 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -49,8 +49,6 @@ class VectorComposite : public std::unordered_map < std::string, Eigen::VectorXd
         VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
         VectorComposite(const StateStructure& _structure, const std::list<VectorXd>& _vectors);
 
-        unsigned int size(const StateStructure& _structure) const;
-
         Eigen::VectorXd vector(const StateStructure& _structure) const;
 
         /**
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 225e6f1ed184b3b51959ab4f125c1dbb9066eaec..18b850f102145bed7b2cf49fc42930ebc8a0822e 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -440,6 +440,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
                     state.insert(pair_key_vec);
             }
         }
+
         // check for empty blocks and fill them with zeros
         for (const auto& ckey : frame_structure_)
         {
@@ -483,6 +484,15 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
                 state.insert(pair_key_vec);
         }
     }
+
+    // check for empty blocks and fill them with zeros
+    for (const auto& ckey : frame_structure_)
+    {
+        const auto& key = string(1,ckey);
+        if (state.count(key) == 0)
+            state.emplace(key, stateZero(key).at(key));
+    }
+
     return state;
 }
 
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index 612b8e53ec55d7e5b787a5aa0c49312d21dfe4a9..90b7f47c6b5ffa2a28374b5673827d325224430f 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.cpp
@@ -62,18 +62,6 @@ VectorComposite::VectorComposite (const StateStructure& _structure, const std::l
 }
 
 
-unsigned int VectorComposite::size(const StateStructure &_structure) const
-{
-    unsigned int size = 0;
-    for (const auto& ckey : _structure)
-    {
-        std::string     key(1,ckey); // ckey is char
-        const VectorXd& v       =  this->at(key);
-        size                    +=  v.size();
-    }
-    return size;
-}
-
 Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const
 {
     // traverse once with unordered_map access
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
index 4d5a420119526c2fc1984aecfc22e01b40d0084b..56f705aa3b9c3689a076989fcd8e30896d80fe13 100644
--- a/test/gtest_state_composite.cpp
+++ b/test/gtest_state_composite.cpp
@@ -331,20 +331,6 @@ TEST(VectorComposite, unary_Minus)
     ASSERT_MATRIX_APPROX((-x).at("O"), Vector3d(-2,-2,-2), 1e-20);
 }
 
-TEST(VectorComposite, size)
-{
-    VectorComposite x;
-
-    x.emplace("P", Vector2d(1,1));
-    x.emplace("O", Vector3d(2,2,2));
-    x.emplace("V", Vector4d(3,3,3,3));
-
-    ASSERT_EQ(x.size("PO"), 5);
-    ASSERT_EQ(x.size("VO"), 7);
-    ASSERT_EQ(x.size("PVO"), 9);
-    ASSERT_EQ(x.size("OPV"), 9);
-}
-
 TEST(VectorComposite, stateVector)
 {
     VectorComposite x;