diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
index bd5c58bee5aec1229ab8e33fc707495deb6dbcc3..d0ecac38ce590cf222f6149a0fd5715555e1e649 100644
--- a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
@@ -31,21 +31,31 @@ SimpleJsonLoggerEntry::SimpleJsonLoggerEntry()
 {
 }
 
-void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, Eigen::Vector3f vec)
+void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::Vector3f &vec)
+{
+    obj->add(key, ToArr((Eigen::VectorXf)vec));
+}
+
+void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::VectorXf &vec)
 {
     obj->add(key, ToArr(vec));
 }
 
-void SimpleJsonLoggerEntry::AddAsObj(const std::string& key, Eigen::Vector3f vec)
+void SimpleJsonLoggerEntry::AddAsObj(const std::string& key, const Eigen::Vector3f &vec)
 {
     obj->add(key, ToObj(vec));
 }
 
-void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, Eigen::Matrix4f mat)
+void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::Matrix4f &mat)
 {
     obj->add(key, ToArr(mat));
 }
 
+void SimpleJsonLoggerEntry::AddMatrix(const std::string& key, const Eigen::MatrixXf& mat)
+{
+    obj->add(key, MatrixToArr(mat));
+}
+
 void SimpleJsonLoggerEntry::Add(const std::string& key, const std::string& value)
 {
     obj->add(key, JsonValue::Create(value));
@@ -56,13 +66,29 @@ void SimpleJsonLoggerEntry::Add(const std::string& key, float value)
     obj->add(key, JsonValue::Create(value));
 }
 
+void SimpleJsonLoggerEntry::Add(const std::string& key, const std::vector<float>& value)
+{
+    obj->add(key, ToArr(value));
+}
 
-JsonArrayPtr SimpleJsonLoggerEntry::ToArr(Eigen::Vector3f vec)
+
+JsonArrayPtr SimpleJsonLoggerEntry::ToArr(const Eigen::VectorXf& vec)
 {
     JsonArrayPtr arr(new JsonArray);
-    arr->add(JsonValue::Create(vec(0)));
-    arr->add(JsonValue::Create(vec(1)));
-    arr->add(JsonValue::Create(vec(2)));
+    for(int i = 0; i < vec.rows(); i++)
+    {
+        arr->add(JsonValue::Create(vec(i)));
+    }
+    return arr;
+}
+
+JsonArrayPtr SimpleJsonLoggerEntry::ToArr(const std::vector<float>& vec)
+{
+    JsonArrayPtr arr(new JsonArray);
+    for(float v : vec)
+    {
+        arr->add(JsonValue::Create(v));
+    }
     return arr;
 }
 
@@ -111,6 +137,21 @@ JsonArrayPtr SimpleJsonLoggerEntry::ToArr(Eigen::Matrix4f mat)
     return arr;
 }
 
+JsonArrayPtr SimpleJsonLoggerEntry::MatrixToArr(Eigen::MatrixXf mat)
+{
+    JsonArrayPtr arr(new JsonArray);
+    for(int i = 0; i < mat.rows(); i++)
+    {
+        JsonArrayPtr row(new JsonArray);
+        for(int j = 0; j < mat.cols(); j++)
+        {
+            row->add(JsonValue::Create(mat(i, j)));
+        }
+        arr->add(row);
+    }
+    return arr;
+}
+
 void SimpleJsonLoggerEntry::AddTimestamp()
 {
     IceUtil::Time now = IceUtil::Time::now();
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h
index 0072eafe151eb74b5219ea7d6dd6b49f277dc643..3c2a25ff7a1cf00d94224c2cb46d0f7b7cf19d4d 100644
--- a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h
@@ -38,16 +38,21 @@ namespace armarx
     {
     public:
         SimpleJsonLoggerEntry();
-        void AddAsArr(const std::string& key, Eigen::Vector3f vec);
-        void AddAsObj(const std::string& key, Eigen::Vector3f vec);
-        void AddAsArr(const std::string& key, Eigen::Matrix4f mat);
+        void AddAsArr(const std::string& key, const Eigen::Vector3f &vec);
+        void AddAsArr(const std::string& key, const Eigen::VectorXf &vec);
+        void AddAsObj(const std::string& key, const Eigen::Vector3f &vec);
+        void AddAsArr(const std::string& key, const Eigen::Matrix4f &mat);
+        void AddMatrix(const std::string& key, const Eigen::MatrixXf &mat);
 
         void Add(const std::string& key, const std::string& value);
         void Add(const std::string& key, float value);
+        void Add(const std::string& key, const std::vector<float>& value);
 
-        static JsonArrayPtr ToArr(Eigen::Vector3f vec);
+        static JsonArrayPtr ToArr(const Eigen::VectorXf &vec);
+        static JsonArrayPtr ToArr(const std::vector<float>& vec);
         static JsonObjectPtr ToObj(Eigen::Vector3f vec);
         static JsonArrayPtr ToArr(Eigen::Matrix4f mat);
+        static JsonArrayPtr MatrixToArr(Eigen::MatrixXf mat);
 
         void AddTimestamp();