Add covariance info to Pose and Vel

Fix getMode bug

include/create/create.h

@@ -33,6 +33,7 @@ POSSIBILITY OF SUCH DAMAGE.
 #define CREATE_H
 
 #include <boost/shared_ptr.hpp>
+#include <boost/numeric/ublas/matrix.hpp>
 #include <string>
 #include <unistd.h>
 
@@ -44,6 +45,8 @@ POSSIBILITY OF SUCH DAMAGE.
 namespace create {
   class Create {
     private:
+      typedef boost::numeric::ublas::matrix<float> Matrix;
+
       enum CreateLED {
         LED_DEBRIS = 1,
         LED_SPOT = 2,
@@ -75,9 +78,13 @@ namespace create {
       bool firstOnData;
       util::timestamp_t prevOnDataTime;
 
+      Matrix poseCovar;
+
       void init();
-      bool updateLEDs();
+      // Add two matrices and handle overflow case
+      Matrix addMatrices(const Matrix &A, const Matrix &B) const;
       void onData();
+      bool updateLEDs();
 
     protected:
       boost::shared_ptr<create::Data> data;