a

Dependencies:   mbed mbed-rtos

Files at this revision

API Documentation at this revision

Comitter:
alexpirciu
Date:
Thu Mar 28 07:44:42 2019 +0000
Parent:
0:c3e774091195
Commit message:
assa

Changed in this revision

include/BezierCurve/beziercurve.hpp Show annotated file Show diff for this revision Revisions of this file
include/BezierCurve/beziercurve.inl Show annotated file Show diff for this revision Revisions of this file
include/BezierCurve/polynomialfunction.hpp Show annotated file Show diff for this revision Revisions of this file
include/BezierCurve/polynomialfunction.inl Show annotated file Show diff for this revision Revisions of this file
include/BezierMotionPlanner/beziermotionplanner.hpp Show annotated file Show diff for this revision Revisions of this file
include/CommandInterpreter/commandinterpreter.hpp Show annotated file Show diff for this revision Revisions of this file
include/Controllers/controller.hpp Show annotated file Show diff for this revision Revisions of this file
include/Controllers/converters.hpp Show annotated file Show diff for this revision Revisions of this file
include/Controllers/converters.inl Show annotated file Show diff for this revision Revisions of this file
include/Controllers/pidcontroller.hpp Show annotated file Show diff for this revision Revisions of this file
include/Controllers/pidcontroller.inl Show annotated file Show diff for this revision Revisions of this file
include/Drivers/servo.hpp Show annotated file Show diff for this revision Revisions of this file
include/Drivers/vnh.hpp Show annotated file Show diff for this revision Revisions of this file
include/Encoders/encoder.hpp Show annotated file Show diff for this revision Revisions of this file
include/Encoders/encoderfilter.hpp Show annotated file Show diff for this revision Revisions of this file
include/Encoders/encoderinterface.hpp Show annotated file Show diff for this revision Revisions of this file
include/Encoders/quadratureencoder.hpp Show annotated file Show diff for this revision Revisions of this file
include/Encoders/quadratureencodertask.hpp Show annotated file Show diff for this revision Revisions of this file
include/Examples/Filter/ekf_am.hpp Show annotated file Show diff for this revision Revisions of this file
include/Examples/Sensors/sensorpublisher.hpp Show annotated file Show diff for this revision Revisions of this file
include/Examples/Sensors/sensorpublisher.inl Show annotated file Show diff for this revision Revisions of this file
include/Examples/Sensors/sensortask.hpp Show annotated file Show diff for this revision Revisions of this file
include/Examples/SystemModels/ackermannmodel.hpp Show annotated file Show diff for this revision Revisions of this file
include/Examples/SystemModels/ackermanntypes.hpp Show annotated file Show diff for this revision Revisions of this file
include/Examples/blinker.hpp Show annotated file Show diff for this revision Revisions of this file
include/Examples/echoer.hpp Show annotated file Show diff for this revision Revisions of this file
include/Filter/filter.hpp Show annotated file Show diff for this revision Revisions of this file
include/Filter/filter.inl Show annotated file Show diff for this revision Revisions of this file
include/Linalg/linalg.h Show annotated file Show diff for this revision Revisions of this file
include/Linalg/linalg.inl Show annotated file Show diff for this revision Revisions of this file
include/MotionController/motioncontroller.hpp Show annotated file Show diff for this revision Revisions of this file
include/Move/move.hpp Show annotated file Show diff for this revision Revisions of this file
include/Queue/queue.hpp Show annotated file Show diff for this revision Revisions of this file
include/Queue/queue.inl Show annotated file Show diff for this revision Revisions of this file
include/SHARP_IR_distance/DistanceSensors/distancesensors.hpp Show annotated file Show diff for this revision Revisions of this file
include/SHARP_IR_distance/DistanceSensors/distancesensors.inl Show annotated file Show diff for this revision Revisions of this file
include/SHARP_IR_distance/IR_DistanceSensorDriver/ir_distancesensordriver.hpp Show annotated file Show diff for this revision Revisions of this file
include/SHARP_IR_distance/drivertask.hpp Show annotated file Show diff for this revision Revisions of this file
include/SafetyStop/safetystopfunction.hpp Show annotated file Show diff for this revision Revisions of this file
include/SerialMonitor/serialmonitor.hpp Show annotated file Show diff for this revision Revisions of this file
include/SystemModels/systemmodels.hpp Show annotated file Show diff for this revision Revisions of this file
include/SystemModels/systemmodels.inl Show annotated file Show diff for this revision Revisions of this file
include/TaskManager/taskmanager.hpp Show annotated file Show diff for this revision Revisions of this file
include/Timer/timer.hpp Show annotated file Show diff for this revision Revisions of this file
include/Timer/timer.inl Show annotated file Show diff for this revision Revisions of this file
src/BezierCurve/beziercurve.cpp Show annotated file Show diff for this revision Revisions of this file
src/BezierCurve/polynomialfunction.cpp Show annotated file Show diff for this revision Revisions of this file
src/BezierMotionPlanner/beziermotionplanner.cpp Show annotated file Show diff for this revision Revisions of this file
src/CommandInterpreter/commandinterpreter.cpp Show annotated file Show diff for this revision Revisions of this file
src/Controllers/controller.cpp Show annotated file Show diff for this revision Revisions of this file
src/Controllers/converters.cpp Show annotated file Show diff for this revision Revisions of this file
src/Controllers/pidcontroller.cpp Show annotated file Show diff for this revision Revisions of this file
src/Drivers/servo.cpp Show annotated file Show diff for this revision Revisions of this file
src/Drivers/vnh.cpp Show annotated file Show diff for this revision Revisions of this file
src/Encoders/encoder.cpp Show annotated file Show diff for this revision Revisions of this file
src/Encoders/encoderFilter.cpp Show annotated file Show diff for this revision Revisions of this file
src/Encoders/quadratureencoder.cpp Show annotated file Show diff for this revision Revisions of this file
src/Encoders/quadratureencodertask.cpp Show annotated file Show diff for this revision Revisions of this file
src/Examples/Filter/ekf_am.cpp Show annotated file Show diff for this revision Revisions of this file
src/Examples/Sensors/sensorpublisher.cpp Show annotated file Show diff for this revision Revisions of this file
src/Examples/Sensors/sensortask.cpp Show annotated file Show diff for this revision Revisions of this file
src/Examples/SystemModels/ackermannmodel.cpp Show annotated file Show diff for this revision Revisions of this file
src/Examples/blinker.cpp Show annotated file Show diff for this revision Revisions of this file
src/Examples/echoer.cpp Show annotated file Show diff for this revision Revisions of this file
src/Filter/filter.cpp Show annotated file Show diff for this revision Revisions of this file
src/Linalg/linalg.cpp Show annotated file Show diff for this revision Revisions of this file
src/MotionController/motioncontroller.cpp Show annotated file Show diff for this revision Revisions of this file
src/Move/move.cpp Show annotated file Show diff for this revision Revisions of this file
src/Queue/queue.cpp Show annotated file Show diff for this revision Revisions of this file
src/SHARP_IR_distance/DistanceSensors/distancesensors.cpp Show annotated file Show diff for this revision Revisions of this file
src/SHARP_IR_distance/IR_DistanceSensorDriver/ir_distancesensordriver.cpp Show annotated file Show diff for this revision Revisions of this file
src/SafetyStop/safetystopfunction.cpp Show annotated file Show diff for this revision Revisions of this file
src/SerialMonitor/serialmonitor.cpp Show annotated file Show diff for this revision Revisions of this file
src/SystemModels/systemmodels.cpp Show annotated file Show diff for this revision Revisions of this file
src/TaskManager/taskmanager.cpp Show annotated file Show diff for this revision Revisions of this file
src/Timer/timer.cpp Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/BezierCurve/beziercurve.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,68 @@
+/**
+  ******************************************************************************
+  * @file    BezierCurve.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class declaration for the Bezier Curve
+  *          methods.
+  ******************************************************************************
+ */
+
+/* include guard */
+#ifndef BEZIERCURVE_HPP
+#define BEZIERCURVE_HPP
+
+/* complex numbers in cartesian form and several functions and overloads to operate with them */
+#include <complex>
+#include <BezierCurve/PolynomialFunction.hpp>
+
+#define BEZIER_ORDER 3
+
+namespace math{
+   /**
+    * @brief It implements the functionality of a Bezier curve. The points on the curve are represented by complex numbers and the input value of the curve have to belong to the interval [0,1].
+    * 
+    * @tparam T The type of the Bezier curve. 
+    */
+    template<class T>
+    class BezierCurve
+    {
+        public:
+            /* Constructors */
+            BezierCurve();
+            BezierCurve(std::complex<T> points[BEZIER_ORDER+1]);
+            BezierCurve(std::complex<T> a,std::complex<T> b,std::complex<T> c,std::complex<T> d);
+            /* Destructor */
+            virtual ~BezierCurve();
+            /* Set curve */
+            void setBezierCurve(std::complex<T> a,std::complex<T> b,std::complex<T> c,std::complex<T> d);
+            /* Get value */
+            std::complex<T> getValue(float input_value);
+            /* First order derivate value */
+            std::complex<T> get_FO_DerivateValue(float input_value);
+            /* Second order derivate value */
+            std::complex<T> get_SO_DerivateValue(float input_value);
+
+            math::PolynomialFunction<std::complex<T>,BEZIER_ORDER > getBezierCurve();
+            math::PolynomialFunction<std::complex<T>,BEZIER_ORDER-1> getFODerivate();
+            math::PolynomialFunction<std::complex<T>,BEZIER_ORDER-2> getSODerivate();
+
+        protected:
+
+        private:
+    //        PRIVATE FUNCTIONS
+    //      Convert the complex points to polynomial function
+            math::PolynomialFunction<std::complex<float>,BEZIER_ORDER> CP2PF();
+
+    //        PRIVATE PARAMETERS
+            std::complex<T> points[BEZIER_ORDER+1];
+            math::PolynomialFunction<std::complex<T>,BEZIER_ORDER> bezierCurve;
+            math::PolynomialFunction<std::complex<T>,BEZIER_ORDER-1> FOder_bezierCurve;
+            math::PolynomialFunction<std::complex<T>,BEZIER_ORDER-2> SOder_bezierCurve;
+    };
+};//namespace math
+#include "beziercurve.inl"
+
+#endif // BEZIERCURVE_HPP
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/BezierCurve/beziercurve.inl	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,167 @@
+
+namespace math{
+
+    /**
+     * @brief Construct a new Bezier Curve< T>:: Bezier Curve object
+     * 
+     */
+    template<class T>
+    BezierCurve<T>::BezierCurve(){
+    }
+
+    /**
+     * @brief Construct a new Bezier Curve< T>:: Bezier Curve object
+     * 
+     * @param a point A
+     * @param b point B
+     * @param c point C
+     * @param d point D
+     */
+    template<class T>
+    BezierCurve<T>::BezierCurve(std::complex<T> a,std::complex<T> b,std::complex<T> c,std::complex<T> d){
+        this->points[0]=a;
+        this->points[1]=b;
+        this->points[2]=c;
+        this->points[3]=d;
+
+        this->bezierCurve=this->CP2PF();
+        this->FOder_bezierCurve=this->bezierCurve.derivateFO();
+        this->SOder_bezierCurve=this->FOder_bezierCurve.derivateFO();
+    }
+
+    /**
+     * @brief Construct a new Bezier Curve< T>:: Bezier Curve object
+     * 
+     * @param points Array of points 
+     */
+    template<class T>
+    BezierCurve<T>::BezierCurve(std::complex<T> points[BEZIER_ORDER+1])
+    {
+        for(int32_t i=0;i<=BEZIER_ORDER;++i){
+            this->points[i]=points[i];
+        }
+        this->bezierCurve=this->CP2PF();
+        this->FOder_bezierCurve=this->bezierCurve.derivateFO();
+        this->SOder_bezierCurve=this->FOder_bezierCurve.derivateFO();
+    }
+
+    /**
+     * @brief Destroy the Bezier Curve< T>:: Bezier Curve object
+     * 
+     */
+    template<class T>
+    BezierCurve<T>::~BezierCurve()
+    {
+        //dtor
+    }
+
+    /**
+     * @brief This metohd create a new polynomial function based on the complex points.
+     * 
+     * @return The result polynomial function represent the Bezier curve.
+     */
+    template<class T>
+    math::PolynomialFunction<std::complex<float>,BEZIER_ORDER> BezierCurve<T>::CP2PF(){
+        math::PolynomialFunction<std::complex<float>,BEZIER_ORDER> pf;
+
+        const std::complex<T> temp_cst_3(3,0);
+        const std::complex<T> temp_cst_2(2,0);
+
+        std::complex<T> coef1=temp_cst_3*(this->points[1]-this->points[0]);
+        std::complex<T> coef2=temp_cst_3*(this->points[0]-temp_cst_2*this->points[1]+this->points[2]);
+        std::complex<T> coef3=temp_cst_3*(this->points[1]-this->points[2])+this->points[3]-this->points[0];
+        pf.setCoefficientValue(0,this->points[0]);
+        pf.setCoefficientValue(1,coef1);
+        pf.setCoefficientValue(2,coef2);
+        pf.setCoefficientValue(3,coef3);
+        return pf;
+    }
+
+    /**
+     * @brief Get the point on the bezier curve. 
+     * 
+     * @param input_value  The input value, it must belong to interval [0,1]. 
+     * @return The point as complex number
+     */
+    template<class T>
+    std::complex<T> BezierCurve<T>::getValue(float input_value){
+        T input_value_T=static_cast<T>(input_value);
+        return this->bezierCurve.calculateValue(input_value);
+    }
+
+    template<class T>
+    /**
+     * @brief Get the value of the first order derivative of Bezier curve
+     * 
+     * @param input_value The input value [0,1]
+     * @return The point as a complex number.
+     * 
+     */
+    std::complex<T> BezierCurve<T>::get_FO_DerivateValue(float input_value){
+        T input_value_T=static_cast<T>(input_value);
+        return this->FOder_bezierCurve.calculateValue(input_value_T);
+    }
+
+    /**
+     * @brief Get the value of the second order derivative of Bezier Curve.
+     * 
+     * @param input_value The input value of the function have to belong to interval [0,1]
+     * @return The resulted value as a complex number.
+     */
+    template<class T>
+    std::complex<T> BezierCurve<T>::get_SO_DerivateValue(float input_value){
+        T input_value_T=static_cast<T>(input_value);
+
+        return this->SOder_bezierCurve.calculateValue(input_value_T);
+    }
+
+    /**
+     * @brief Get the polynomial function, which respresents the Bezier curve.
+     * 
+     */
+    template<class T>
+    math::PolynomialFunction<std::complex<T>,BEZIER_ORDER> BezierCurve<T>::getBezierCurve(){
+        return this->bezierCurve;
+    }
+
+    /**
+     * @brief Get resulted polynomial function of the first order derivative
+     * 
+     * @return math::PolynomialFunction<std::complex<T>,BEZIER_ORDER-1> 
+     */
+    template<class T>
+    math::PolynomialFunction<std::complex<T>,BEZIER_ORDER-1> BezierCurve<T>::getFODerivate(){
+        return this->FOder_bezierCurve;
+    }
+
+    template<class T>
+    /**
+     * @brief Get resulted polynomial function of the second order derivative
+     * 
+     * @return math::PolynomialFunction<std::complex<T>,BEZIER_ORDER-2> 
+     */
+    math::PolynomialFunction<std::complex<T>,BEZIER_ORDER-2> BezierCurve<T>::getSODerivate(){
+        return this->SOder_bezierCurve;
+    }
+
+    template<class T>
+    /**
+     * @brief Set the points for creating a new Bezier curve
+     * 
+     * @param a point A
+     * @param b point B
+     * @param c point C
+     * @param d point D
+     */
+    void  BezierCurve<T>::setBezierCurve(std::complex<T> a,std::complex<T> b,std::complex<T> c,std::complex<T> d){
+        this->points[0]=a;
+        this->points[1]=b;
+        this->points[2]=c;
+        this->points[3]=d;
+
+        this->bezierCurve=this->CP2PF();
+        this->FOder_bezierCurve=this->bezierCurve.derivateFO();
+        this->SOder_bezierCurve=this->FOder_bezierCurve.derivateFO();
+    }
+
+}; // namespace math
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/BezierCurve/polynomialfunction.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,67 @@
+/**
+  ******************************************************************************
+  * @file    PolynomialFunction.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class declaration for the Plynomial Function
+  *          methods.
+  ******************************************************************************
+ */
+
+/* include guard */
+#ifndef POLYNOMIALFUNCTION_HPP
+#define POLYNOMIALFUNCTION_HPP
+
+#include <stdint.h>
+#include <math.h>
+#include <iostream>
+
+
+namespace math{
+   /**
+    * @brief It represents a one dimensional polynomial function
+    * 
+    * @tparam T The type of the Polynomial function
+    * @tparam N The degree of the polynom. 
+    */
+    template <class T,int32_t N>
+    class PolynomialFunction
+    {
+        public:
+            /* Constructors  */
+            PolynomialFunction();
+            PolynomialFunction(T coefficients[N+1]);
+            /* Destructors  */
+            virtual ~PolynomialFunction();
+
+    //        template<int32_t N2> void add(PolynomialFunction<T,N2> poli);
+            /* Add */
+            template<int32_t N2> math::PolynomialFunction<T,(N2<N?N:N2)> add(PolynomialFunction<T,N2> b);
+            /* Multiply */
+            template<int32_t N2> math::PolynomialFunction<T,(N2+N)> multip(PolynomialFunction<T,N2> b);
+            /* Calculate value */
+            T calculateValue(T input_value);
+            /* First order derivate */
+            math::PolynomialFunction<T,N-1> derivateFO();
+            /* Get degree */
+            int32_t getDegree();
+            /* Get coefficient value */
+            T getCoefficientValue(int32_t index);
+            /* Set coefficient value */
+            void setCoefficientValue(int32_t index,T value );
+        private:
+            /* Coefficients array */
+            /**
+             * @brief The list of the coeffients.
+             * 
+             */
+            T coefficients[N+1];
+    };
+
+}; // namepace math
+
+#include "polynomialfunction.inl"
+
+#endif // POLYNOMIALFUNCTION_HPP
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/BezierCurve/polynomialfunction.inl	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,144 @@
+
+namespace math{
+
+/**
+ * @brief Construct a new Polynomial Function< T, N>:: Polynomial Function object
+ * 
+ */
+template<class T,int32_t N>
+PolynomialFunction<T,N>::PolynomialFunction()
+{
+    for(int i=0;i<=N;++i){
+        this->coefficients[i]=static_cast<T>(0);
+    }
+}
+
+/**
+ * @brief Construct a new Polynomial Function< T, N>:: Polynomial Function object
+ * 
+ * @param coefficients The list of the coefficients for the polynomial function.
+ */
+template<class T,int32_t N>
+PolynomialFunction<T,N>::PolynomialFunction(T coefficients[N+1])
+{
+    for(int i=0;i<=N;++i){
+        this->coefficients[i]=coefficients[i];
+    }
+}
+
+/**
+ * @brief Destroy the Polynomial Function< T, N>:: Polynomial Function object
+ * 
+ */
+template<class T,int32_t N>
+PolynomialFunction<T,N>::~PolynomialFunction()
+{
+}
+
+/**
+ * @brief Get the degree of the polynomial function
+ * 
+ * @return The degree of the polynom
+ */
+template<class T,int32_t N>
+int32_t PolynomialFunction<T,N>::getDegree()
+{
+    return N;
+}
+
+/**
+ * @brief Get coefficient value
+ * 
+ * @param index         The index of the coefficient
+ * @return The value of the coefficient
+ */
+template<class T,int32_t N>
+T PolynomialFunction<T,N>::getCoefficientValue(int32_t index)
+{
+    if(index>N || index<0) return static_cast<T>(0);
+    return this->coefficients[index];
+}
+
+/**
+ * @brief Set the value of the coefficient
+ * 
+ * @param index     The index of the coefficient
+ * @param value     The new value of the coefficient
+ */
+template<class T,int32_t N>
+void PolynomialFunction<T,N>::setCoefficientValue(int32_t index, T value ){
+    if(index<=N){
+        this->coefficients[index]=value;
+    }
+}
+
+/**
+ * @brief It sums the polynom with the input polynom and return a result in the new polynom.
+ * 
+ * @param b         The input polynom.
+ * @return The result polynom.
+ */
+template<class T,int32_t N>
+template<int32_t N2>
+math::PolynomialFunction<T,(N2<N?N:N2)> PolynomialFunction<T,N>::add(PolynomialFunction<T,N2> b){
+    math::PolynomialFunction<T,N2<N?N:N2> p;
+    for(int i=0;i<=N2 || i<=N;++i){
+        p.setCoefficientValue(i,this->getCoefficientValue(i)+b.getCoefficientValue(i));
+    }
+    return p;
+}
+
+
+template<class T,int32_t N>
+template<int32_t N2>
+/**
+ * @brief It multiply the polynom with the given polynom and return the result polynom.
+ *  
+ * @param b     The input polynom
+ * @return The result polynom
+ */
+math::PolynomialFunction<T,(N2+N)> PolynomialFunction<T,N>::multip(PolynomialFunction<T,N2> b){
+    math::PolynomialFunction<T,(N2+N)> result;
+    for(int32_t i=0;i<=N2+N;++i){
+        T sum=static_cast<T>(0);
+        for(int32_t j=0;j<=i;++j){
+            sum+=this->getCoefficientValue(j)*b.getCoefficientValue(i-j);
+        }
+        result.setCoefficientValue(i,sum);
+    }
+
+    return result;
+}
+
+template<class T,int32_t N>
+/**
+ * @brief Calculate the polynom value 
+ * 
+ * @param input_value   The given value 
+ * @return The result value of the polynom. 
+ */
+T PolynomialFunction<T,N>::calculateValue(T input_value){
+    T output_value=static_cast<T>(0);
+    for(int32_t i=0;i<=N;++i){
+        output_value+=this->coefficients[i]*static_cast<T>(pow(input_value,i));
+    }
+//    std::cout<<"OOO"<<output_value<<std::endl;
+    return output_value;
+}
+
+template<class T, int32_t N>
+/**
+ * @brief It calcutes the first derivate of the polinom and return it by creating a new polynomial function object.
+ * 
+ * @return The result polynom.
+ */
+math::PolynomialFunction<T,N-1> PolynomialFunction<T,N>::derivateFO(){
+    math::PolynomialFunction<T,N-1> derivate;
+    for(int32_t i=0;i<N;++i){
+        T coeff=static_cast<T>(i+1);
+        derivate.setCoefficientValue(i,coeff*this->getCoefficientValue(i+1));
+    }
+    return derivate;
+};
+
+};
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/BezierMotionPlanner/beziermotionplanner.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,86 @@
+/**
+  ******************************************************************************
+  * @file    BezierMotionPlanner.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class declaration for the Bezier Motion
+  *          Planner methods.
+  ******************************************************************************
+ */
+
+/* include guard */
+#ifndef MOTIONPLANNER_HPP
+#define MOTIONPLANNER_HPP
+
+#include <complex>
+#include <utility>
+#include <math.h>
+#include <BezierCurve/BezierCurve.hpp>
+
+/* Module defines */
+#define WHEELBASE 0.265
+
+namespace planner{
+
+       /** CBezierMotionPlanner class. 
+        * @brief This class implements the functionality of planner to keep the robot on the given Bezier curve.
+        * 
+        */
+        class CBezierMotionPlanner
+        {
+        public:
+                /* Constructor */
+                CBezierMotionPlanner();
+                /* Constructor */
+                CBezierMotionPlanner(bool              isForward,
+                                std::complex<float>     a,
+                                std::complex<float>     b,
+                                std::complex<float>     c,
+                                std::complex<float>     d,
+                                float                   motion_duration_i,
+                                float                   timestep_i);
+                /* Set parameters */
+                void setMotionPlannerParameters(bool                    isForward,
+                                                std::complex<float>     a,
+                                                std::complex<float>     b,
+                                                std::complex<float>     c,
+                                                std::complex<float>     d,
+                                                float                   motion_duration_i,
+                                                float                   timestep_i);
+                /* Destructor */
+                virtual ~CBezierMotionPlanner();
+                /* Get Bezier Curve */
+                math::BezierCurve<float> getBezierCurve();
+        //      Velocity longitudinal and angular
+                /* Get next velocity value */
+                std::pair<float,float> getNextVelocity();
+        //      Velocity longitudinal and angular, Input value [0,1]
+                /* Get velocity value */
+                std::pair<float,float> getVelocity(float input_value);
+                /* Next value valid */
+                bool hasValidValue();
+                /* Is movement forward */
+                bool getForward();
+
+        private:
+                /* forward movement indicator */
+                bool isForward;
+                /* Bezier Curve object */
+                math::BezierCurve<float> bezierCurve;
+                /* motion duration */
+                float motion_duration;
+                /* time step */
+                float time_step;
+                /* Bezier value input step */
+                float bezierValueInput_step;
+                /* next Bezier value */
+                float next_bezierValueInput;
+                /* intialization indicator */
+                bool isInitialized;
+        };
+
+}; // namespace planner
+
+#endif // CMotionPlanner_HPP
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/CommandInterpreter/commandinterpreter.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,52 @@
+/**
+  ******************************************************************************
+  * @file    CommandInterpreter.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class declaration for the command interpreter
+  *          functionality.
+  ******************************************************************************
+ */
+
+/* Inclusion guard */
+#ifndef COMMAND_INTERPRETER_HPP
+#define COMMAND_INTERPRETER_HPP
+
+/* The mbed library */
+#include <mbed.h>
+#include <Move/move.hpp>
+#include <Queue/queue.hpp>
+
+//!  CTimer class. 
+/*! 
+ *  It is used for implementing the interpreter of commands received by Nucleo.
+ * */
+class CCommandInterpreter
+{
+public:
+    /* Constuctor */
+    CCommandInterpreter(Move& f_car);
+    /* Interpret character */
+    inline void interpretChar(unsigned char f_c);
+    /* Execute command */
+    inline void executeCommand();
+private:
+    /* Reset */
+    inline void reset();
+    /* Interpret command */
+    inline void intepretCommand();
+    /* buffer */
+    CQueue<unsigned char, 7> m_buffer;
+    /* reference to MOVE object */
+    Move& m_car;
+    unsigned char m_commandID;
+    /* command value */
+    float m_commandValue;
+    /* speed value */
+    float m_speedValue;
+    /* angle value */
+    float m_angleValue;
+};
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Controllers/controller.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,83 @@
+/**
+  ******************************************************************************
+  * @file    Controller.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class declaration for the controller
+  *          functionality.
+  ******************************************************************************
+ */
+
+/* Include guard */
+
+#ifndef CONTROLLER_HPP
+#define CONTROLLER_HPP
+#include<cmath>
+#include<Controllers/pidcontroller.hpp>
+
+#include <Encoders/encoderinterface.hpp>
+#include <Controllers/converters.hpp>
+
+#include <mbed.h>
+
+namespace controllers
+{
+
+   /** CControllerSiso class
+    * @brief It implements a controller with a single input and a single output. It needs an encoder getter interface to get the measured values, a controller to calculate the control signal. It can be completed with a converter to convert the measaurment unit of the control signal. 
+    * 
+    */
+    class CControllerSiso
+    {
+        /* PID controller declaration*/
+        template <class T>
+        using  ControllerType=siso::IController<T>;
+
+        public:
+            /* Construnctor */
+            CControllerSiso(encoders::IEncoderGetter&               f_encoder
+                            ,ControllerType<double>&                f_pid
+                            ,controllers::IConverter*                   f_converter=NULL);
+            /* Set controller reference value */
+            void setRef(double                       f_RefRps);
+            /* Get controller reference value */
+            double getRef();
+            /* Get control value */
+            double get();
+            /* Get error */
+            double getError();
+            /* Clear PID parameters */
+            void clear();
+            /* Control action */
+            bool control();
+
+        private:
+            /* PWM onverter */
+            double converter(double f_u);
+
+            /* Enconder object reference */
+            encoders::IEncoderGetter&               m_encoder;
+            /* PID object reference */
+            ControllerType<double>&                 m_pid;
+            /* Controller reference */
+            double                                  m_RefRps;
+            /* Control value */
+            double                                  m_u;
+            /* Error */
+            double                                  m_error;
+            /* Converter */
+            controllers::IConverter*                m_converter;
+            /* Scaled PWM control signal limits */
+            const float                                     m_control_sup;
+            const float                                     m_control_inf;
+            /* Inferior limit of absolut reference */
+            const float                                     m_ref_abs_inf;
+            /* Inferior and superior limits of absolute speed measurment for standing state */
+            const float                                     m_mes_abs_inf;
+            const float                                     m_mes_abs_sup;
+
+    };
+}; // namespace controllers
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Controllers/converters.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,74 @@
+/**
+******************************************************************************
+* @file    converters.hpp
+* @author  RBRO/PJ-IU
+* @version V1.0.0
+* @date    2018-10-29
+* @brief   
+******************************************************************************
+*/
+#ifndef CONVERTERS_H
+#define CONVERTERS_H
+
+#include<cmath>
+#include<stdint.h>
+#include<array>
+
+namespace controllers{
+    /**
+     * @brief Converter interface with single input and single output
+     * 
+     */
+    class IConverter{
+      public:
+        virtual float operator()(float) = 0;
+    };
+
+    /**
+     * @brief Polynomial converter.
+     * 
+     * @tparam NOrd The degree of the polynomial converter
+     */
+    template<uint8_t NOrd>
+    class CConverterPolynom:public IConverter
+    {
+      public:
+        CConverterPolynom(std::array<float,NOrd+1>);
+        float operator()(float);
+
+      private:
+        std::array<float,NOrd+1> m_coeff;
+    };
+
+    /**
+     * @brief A converter based on the set of break point and the multiple polynomial function.
+     * 
+     * @tparam NrBreak Number of the break.
+     * @tparam NOrd    Degree of the polynomial converter. 
+     */
+    template<uint8_t NrBreak,uint8_t NOrd>
+    class CConverterSpline:public IConverter
+    {
+      public:
+        using CCoeffContainerType = std::array<float,NOrd+1>;
+        using CSplineContainerType = std::array<CCoeffContainerType,NrBreak+1>;
+        using CBreakContainerType = std::array<float,NrBreak>;
+
+
+        CConverterSpline(CBreakContainerType f_breaks,CSplineContainerType f_splines);
+        float operator()(float);
+      private:
+        float splineValue( CCoeffContainerType,float);
+
+        CBreakContainerType     m_breaks;
+        CSplineContainerType    m_splines;
+        
+    };
+
+    #include "converters.inl"
+}; //namespace controllers
+
+
+
+#endif // CONVERTERS_H
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Controllers/converters.inl	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,77 @@
+
+
+/**
+ * @brief Construct a new CConverterPolynom<NOrd>::CConverterPolynom object
+ * 
+ * @param f_coeff The list of the coeffience.
+ */
+template<uint8_t NOrd>
+CConverterPolynom<NOrd>::CConverterPolynom(std::array<float,NOrd+1> f_coeff)
+:m_coeff(f_coeff)
+{
+}
+
+
+/**
+ * @brief Convert the input value. 
+ * 
+ */
+template<uint8_t NOrd>
+float CConverterPolynom<NOrd>::operator()(float f_v){
+    float l_res = m_coeff[NOrd];
+    for (uint8_t i = 0;i<NOrd;++i){
+        l_res += std::pow(f_v, NOrd-i)*m_coeff[i];
+    }
+    return l_res;
+}
+
+
+/**
+ * @brief Construct a new CConverterSpline<NrBreak, NOrd>::CConverterSpline object
+ * 
+ * @tparam NrBreak 
+ * @tparam NOrd 
+ * @param f_breaks The list of the break points.
+ * @param f_splines The list of the polynomial function.
+ */
+template <uint8_t NrBreak, uint8_t NOrd>
+CConverterSpline<NrBreak, NOrd>::CConverterSpline(CConverterSpline<NrBreak, NOrd>::CBreakContainerType f_breaks,CConverterSpline<NrBreak, NOrd>::CSplineContainerType f_splines)
+:m_breaks(f_breaks)
+,m_splines(f_splines)
+{
+}
+
+/**
+ * @brief Convert the input value.
+ * 
+ */
+template <uint8_t NrBreak, uint8_t NOrd>
+float CConverterSpline<NrBreak, NOrd>::operator()(float f_value){
+    typename CConverterSpline<NrBreak, NOrd>::CBreakContainerType::iterator it_breaks0=NULL,it_breaks1=NULL;
+    typename CConverterSpline<NrBreak, NOrd>::CSplineContainerType::iterator it_spline = m_splines.begin();
+    for (it_breaks1 = m_breaks.begin();it_breaks1 != m_breaks.end();){
+        if((it_breaks0==NULL || (it_breaks0!=NULL && (*it_breaks0)<f_value)) && f_value<=(*it_breaks1)){
+            break;
+        }
+        it_breaks0 = it_breaks1;
+        ++it_breaks1;
+        ++it_spline;
+    }
+    return this->splineValue(*it_spline, f_value);
+}
+
+/**
+ * @brief Convert the given value, by using the given polynom.
+ * 
+ * @param f_coeff The coeffiences of the polynom
+ * @param f_value The input value.
+ */
+template <uint8_t NrBreak, uint8_t NOrd>
+float CConverterSpline<NrBreak, NOrd>::splineValue( CCoeffContainerType f_coeff,float f_value){
+    float l_res = f_coeff[NOrd];
+    for (uint8_t i = 0;i<NOrd;++i)
+    {
+        l_res+=std::pow(f_value,NOrd-i)*f_coeff[i];
+    }
+    return l_res;
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Controllers/pidcontroller.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,82 @@
+#ifndef PID_CONTROLLER_H
+#define PID_CONTROLLER_H
+
+#include <cstdio>
+#include <Linalg/linalg.h>
+#include <SystemModels/systemmodels.hpp>
+#include <mbed.h>
+
+namespace controllers
+{
+    namespace siso
+    {
+        
+        /**
+         * @brief General interface class for the controller
+         * 
+         * @tparam T type of the variables (float, double)
+         */
+        template<class T>
+        class IController{
+            public:
+                virtual T calculateControl(const T&)=0;
+                virtual void clear()=0;
+        };
+
+        /**
+         * @brief It generates a discrete transferfunction for realizing a proportional–integral–derivative controller, which is discretized by the Euler’s method.
+         * 
+         * @tparam T type of the variables (float, double) 
+         */
+        template<class T>
+        class CPidController:public IController<T>
+        {
+            public:
+                /* Discrete transfer function type */
+                using CPidSystemmodelType = systemmodels::lti::siso::CDiscreteTransferFucntion<T,3,3>;
+                
+                /* Constructor */
+                CPidController(T              f_kp
+                              ,T              f_ki
+                              ,T              f_kd
+                              ,T              f_tf
+                              ,T              f_dt);
+                CPidController(CPidSystemmodelType f_pid,T f_dt);
+
+                /* Overloaded operator */
+                T calculateControl(const T& f_input);
+                /* Serial callback */
+                static void staticSerialCallback(void* obj,char const * a, char * b);
+                /* Initialization */
+                void clear();
+            private:
+                #ifdef CONTROL_TEST_HPP
+                    FRIEND_TEST(PidControlTest,Initialization);
+                    FRIEND_TEST(PidControlTest,Setter);
+                #endif 
+                /* Controller setting */
+                void setController(T              f_kp
+                                  ,T              f_ki
+                                  ,T              f_kd
+                                  ,T              f_tf);
+                /* Serial callback implementation */
+                void serialCallback(char const * a, char * b);
+
+                
+
+                /* Discrete transfer function */
+                CPidSystemmodelType     m_pidTf;
+
+
+                /* delta-time term */
+                T                 m_dt;
+
+        };
+        /* Include function definitions */
+        #include "PidController.inl"
+    }; // namespace siso
+}; // namespace controllers   
+
+
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Controllers/pidcontroller.inl	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,114 @@
+/** @brief  Class constructor
+  *
+  * Constructor method
+  *
+  * @param f_kp                proportional factor
+  * @param f_ki                integral factor
+  * @param f_kd                derivative factor
+  * @param f_tf                derivative time filter constant
+  * @param f_dt                sample time
+  */
+template<class T>
+CPidController<T>::CPidController(T              f_kp
+                                                   ,T              f_ki
+                                                   ,T              f_kd
+                                                   ,T              f_tf
+                                                   ,T              f_dt)
+    :m_pidTf()
+    ,m_dt(f_dt)
+{    
+    linalg::CMatrix<T,1,3> l_numPid({ (f_kd+f_tf*f_kp)/f_tf , (f_tf*f_dt*f_ki+f_dt*f_kp-2*f_tf*f_kp-2*f_kd)/f_tf , (f_kd+f_tf*f_kp+f_dt*f_dt*f_ki-f_dt*f_tf*f_ki-f_dt*f_kp)/f_tf});
+    linalg::CMatrix<T,1,3> l_denPid({ 1.0,-(2*f_tf-f_dt)/f_tf,(f_tf-f_dt)/f_tf });
+
+    m_pidTf.setNum(l_numPid.transpose());
+    m_pidTf.setDen(l_denPid.transpose());
+}
+
+// template<class T>
+// CPidController<T>::CPidController(  CPidSystemmodelType             f_pid
+//                                     ,T                              f_dt)
+//                                     :m_pidTf(f_pid)
+//                                     ,m_dt(f_dt){
+
+// }
+
+
+/** @brief  Operator
+  *
+  * @param f_input             input function
+  * \return                    control value
+  */
+template<class T>
+T CPidController<T>::calculateControl(const T& f_input)
+{
+    return m_pidTf(f_input);
+}
+
+/** @brief  Serial callback method
+  *
+  * Serial callback attaching serial callback to controller object
+  *
+  * @param  obj                 PID controller object
+  * @param  a                   string to read data from
+  * @param b                    string to write data to
+  * \return None
+  */
+template<class T>
+void CPidController<T>::staticSerialCallback(void* obj,char const * a, char * b)
+{
+    CPidController* self = static_cast<CPidController*>(obj);
+    self->serialCallback(a,b);
+}
+
+/** @brief  Reset to zero all memory of the controller.
+  *
+  */
+template<class T>
+void CPidController<T>::clear()
+{
+    m_pidTf.clearMemmory();
+}
+
+/** @brief  Set the parameter of the controller
+  *
+  *
+  * @param f_kp                proportional factor
+  * @param f_ki                integral factor
+  * @param f_kd                derivative factor
+  * @param f_tf                derivative time filter constant
+  */
+template<class T>
+void CPidController<T>::setController(
+    T              f_kp,
+    T              f_ki,
+    T              f_kd,
+    T              f_tf)
+{
+    linalg::CMatrix<T,1,3> l_numPid({ (f_kd+f_tf*f_kp)/f_tf , (f_tf*m_dt*f_ki+m_dt*f_kp-2*f_tf*f_kp-2*f_kd)/f_tf , (f_kd+f_tf*f_kp+m_dt*m_dt*f_ki-m_dt*f_tf*f_ki-m_dt*f_kp)/f_tf });
+    linalg::CMatrix<T,1,3> l_denPid({ 1,-(2*f_tf-m_dt)/f_tf,(f_tf-m_dt)/f_tf });
+    
+    m_pidTf.setNum(l_numPid.transpose());
+    m_pidTf.setDen(l_denPid.transpose());
+}
+
+/** @brief  Serial callback method setting controller to values received
+  *
+  *
+  * @param  a                   string to read data from
+  * @param b                    string to write data to
+  */
+template<class T>
+void CPidController<T>::serialCallback(char const * a, char * b)
+{
+    float l_kp,l_ki,l_kd,l_tf;
+    uint32_t l_res = sscanf(a,"%f;%f;%f;%f;",&l_kp,&l_ki,&l_kd,&l_tf);
+    if (4 == l_res)
+    {
+        setController(l_kp,l_ki,l_kd,l_tf);
+        sprintf(b,"ack;;%2.5f;%2.5f;%2.5f;%2.5f;",l_kp,l_ki,l_kd,l_tf);
+    }
+    else
+    {
+        sprintf(b,"sintax error;;");
+    }
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Drivers/servo.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,44 @@
+/**
+  ******************************************************************************
+  * @file    SERVO.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class declaration for the steering SERVO
+  *          functionality.
+  ******************************************************************************
+ */
+
+/* Include guard */
+#ifndef SERVO_HPP
+#define SERVO_HPP
+
+#include <mbed.h>
+
+
+namespace drivers{
+    /**
+     * @brief It is used for implementing SERVO functionality.
+     * 
+     */
+    class SERVO
+    {
+    public:
+        /* Constructor */
+        SERVO(PinName _pwm);
+        /* Destructor */
+        ~SERVO();
+        /* Set angle */
+        void SetAngle(float angle); //-25 to 25 degr
+    private:
+        /* convert angle to duty cycle */
+        float conversion(float angle); //angle to duty cycle
+        /* PWM value */
+        PwmOut pwm;
+        /* Current angle */
+        float current_angle;
+    };
+}; // namespace drivers
+
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Drivers/vnh.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,82 @@
+/**
+  ******************************************************************************
+  * @file    VNH.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class declaration for the VNH Bridge Driver
+  *          functionality.
+  ******************************************************************************
+ */
+
+/* Inclusion guard */
+#ifndef VNH_HPP
+#define VNH_HPP
+
+/* The mbed library */
+#include <mbed.h>
+/* Functions to compute common mathematical operations and transformations */
+#include <cmath>
+
+namespace drivers{
+    // ! Current getter interface
+    class ICurrentGetter{
+        public:
+            virtual float GetCurrent(void)=0;
+    };
+
+    /**
+     * @brief Command setter interface.
+     * 
+     */
+    class ICommandSetter{
+        public:
+            virtual void Run(float pwm);
+    };
+
+    //!  VNH class. 
+    /*! 
+    *  It is used for implementing motor bridge control.
+    * */
+    class VNH:public ICurrentGetter, public ICommandSetter
+    {
+    public:
+        /* Constructor */
+        VNH(PinName, PinName, PinName, PinName);
+        /* Destructor */
+        ~VNH();
+        /* Start */
+        void Start(void);
+        /* Stop */
+        void Stop(void);
+        /* Run */
+        void Run(float speed);
+        /* Brake */
+        void Brake();
+        /* Inverse */
+        void Inverse(float f_speed);
+        /* Get current */
+        float GetCurrent(void);
+        
+    private:
+        /* Go current */
+        void Go(float speed);
+        /* increment member */
+        float increment;
+        /* current speed member */
+        float current_speed;
+        /* PWM */
+        PwmOut      pwm;
+        /* pin A */
+        DigitalOut  ina;
+        /* pin B */
+        DigitalOut  inb;
+        /* current value */
+        AnalogIn    current;
+    };
+
+
+}; // namespace drivers
+
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Encoders/encoder.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,107 @@
+/**
+  ******************************************************************************
+  * @file    Encoder.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class declaration for the encoder
+  *          functionality.
+  ******************************************************************************
+ */
+
+/* Include guard */
+#ifndef MAG_ENCODER_HPP
+#define MAG_ENCODER_HPP
+
+#include <mbed.h>
+#include<TaskManager/taskmanager.hpp>
+#include<Encoders/encoderinterface.hpp>
+
+//!  CCounter class. 
+/*!
+ * It is used for counting encoder steps.
+ */
+class CCounter
+{
+    public:
+        /* Constructor */
+        CCounter(PinName f_pin);
+        /* Reset method */
+        void reset();
+        /* Increment method */
+        void increment();
+        /* Get counts method */
+        int32_t getCount();
+        /* Increment */
+        static void staticIncrement(void* obj);
+    private:
+        /* Interrupt object */
+        InterruptIn m_interrupt;
+        /* Counts member */
+        volatile int32_t m_count;
+};
+
+//!  CEncoder class. 
+/*!
+ * It inherits class task::CTask. 
+ * It is used for computing revolustions per second.
+ */
+class CEncoder: public task::CTask, public:: encoders::IEncoderGetter
+{
+    public:
+        /* Constructor */
+        CEncoder(uint32_t    f_period
+                ,float      f_period_sec
+                ,uint32_t   f_cpr
+                ,PinName    f_pinName);
+        /* Get rotations per second */
+        virtual float getSpeedRps();
+        virtual int16_t  getCount();
+        virtual bool isAbs(){return true;}
+
+    protected:
+        /* Run method */
+        virtual void _run();
+
+        /* Counter values */
+        CCounter        m_counter;
+        /* Period in seconds */
+        const float     m_period_sec;
+        /* Value of counts per revolution */
+        const uint32_t  m_cpr;//count per revolution (rise and fall edges)
+        /* Value of rotation per second */
+        float           m_rps;
+};
+
+//!  CMagEncoderTime class. 
+/*!
+ * It is used for computing high/low periods.
+ */
+class CMagEncoderTime
+{
+    public:
+        /* Constructor */
+        CMagEncoderTime(PinName f_pin);
+        /* Callback for rising edge interrupt */
+        void riseCallback();
+        /* Callback for falling edge interrupt */
+        void fallCallback();
+        /* Callback for rising edge interrupt attached to pbject */
+        static void staticRise(void* obj);
+        /* Callback for falling edge interrupt attached to pbject */
+        static void staticFall(void* obj);
+        /* Get high time */
+        float getHighTime();
+        /* Get low time */
+        float getLowTime();
+    private:
+        /* General purpose timer */
+        Timer       m_Timer,m_fallTimer;
+        /* Digital interrupt input */
+        InterruptIn m_interrupt;
+        /* High time */
+        volatile float      m_highTime;
+        /* Low time */
+        volatile float      m_lowTime;
+};
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Encoders/encoderfilter.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,49 @@
+/**
+  ******************************************************************************
+  * @file    EncoderFilter.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class declaration for the encoder filter
+  *          functionality.
+  ******************************************************************************
+ */
+
+/* Include guard */
+#ifndef ENCODER_FILTER_HPP
+#define ENCODER_FILTER_HPP
+#include <Encoders/encoder.hpp>
+#include <Filter/filter.hpp>
+
+//!  CEncoderFilter class. 
+/*!
+ * It inherits class CEncoder. 
+ * It is used for obtaining filtered encoder output.
+ */
+class CEncoderFilter:public CEncoder
+{
+    /* Filter object */
+    template <class T>
+    using CFilterFunction = filter::CFilterFunction<T>;
+    public:
+        /* Constructor */
+        CEncoderFilter  (uint32_t                       f_period
+                        ,float                          f_period_sec
+                        ,uint32_t                       f_cpr
+                        ,PinName                        f_pinName
+                        ,CFilterFunction<float>&        f_filter);
+        
+        virtual float getSpeedRps();
+        virtual int16_t  getCount() = 0;
+    protected:
+        /* Run method */
+        virtual void _run();
+        
+    private:
+        /* Filter member */
+        CFilterFunction<float>&    m_filter;
+        /* Value of filtered rotations per second */
+        float                      m_filteredRps;
+};
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Encoders/encoderinterface.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,43 @@
+/**
+ * @file encoderinterface.hpp
+ * @author RBRO/PJ-IU
+ * @brief 
+ * @version 0.1
+ * @date 2018-10-24
+ * 
+ * @copyright Copyright (c) 2018
+ * 
+ */
+#ifndef ENDOCER_INTERFACE_HPP
+#define ENDOCER_INTERFACE_HPP
+
+#include <stdint.h>
+namespace encoders{
+    /**
+     * @brief Rotary encoder interface class
+     *
+     */
+    class IEncoderGetter{
+        public:
+        /**
+         * @brief Get the Count value
+         * 
+         * @return int16_t 
+         */
+        virtual int16_t  getCount() = 0;
+        /**
+         * @brief Get the Speed Rps 
+         * 
+         * @return float 
+         */
+        virtual float getSpeedRps() = 0;
+        /**
+         * @brief Get the encoder capability. If it's true, than the encoder can give the oriantation, else it returns the absolute value of the rotation without the oriantation. 
+         * 
+         * @return bool 
+         */
+        virtual bool isAbs() = 0;
+    };
+}; // namepsace encoders;
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Encoders/quadratureencoder.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,63 @@
+/**
+ * @file quadratureencoder.hpp
+ * @author RBRO/PJ-IU
+ * @brief 
+ * @version 0.1
+ * @date 2018-10-23
+ * 
+ * @copyright Copyright (c) 2018
+ * 
+ */
+
+#ifndef Quadrature_ENCODER_HPP
+#define Quadrature_ENCODER_HPP
+
+#include <mbed.h>
+
+namespace encoders{
+/**
+ * @brief  It an interface for accessing the encoder position. It can be used to get and to reset the position of the encoder.
+ * 
+ */
+class CQuadratureEncoder_TIMX{
+    public:
+      virtual int16_t getCount() = 0;
+      virtual void reset() = 0;
+};
+
+/**
+ * @brief It's a singleton class for receiving and decoding the Quadrature signal by using the timer TIM4. It can get the direction of the rotation.
+ * 
+ */
+class CQuadratureEncoder_TIM4:public CQuadratureEncoder_TIMX{
+    /**
+     * @brief It uses to destroy the singleton class.
+     * 
+     */
+    class CQuadratureEncoder_TIM4_Destroyer{
+    public:
+        CQuadratureEncoder_TIM4_Destroyer(){}
+        ~CQuadratureEncoder_TIM4_Destroyer();
+        void SetSingleton(CQuadratureEncoder_TIM4* s);
+    private:
+         CQuadratureEncoder_TIM4* m_singleton;
+    };
+
+  public:
+    static CQuadratureEncoder_TIM4 *Instance();
+    int16_t getCount();
+    void reset();
+    virtual ~CQuadratureEncoder_TIM4() {}
+  protected:
+    CQuadratureEncoder_TIM4() {}
+   
+  private:
+    void initialize();
+    static CQuadratureEncoder_TIM4* m_instance;
+    static CQuadratureEncoder_TIM4_Destroyer m_destroyer;
+};
+
+};// namepsace drivers
+
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Encoders/quadratureencodertask.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,75 @@
+/**
+ * @file quadratureencodertask.hpp
+ * @author  RBRO/PJ-IU
+ * @brief 
+ * @version 0.1
+ * @date 2018-10-23
+ * 
+ * @copyright Copyright (c) 2018
+ * 
+ */
+#ifndef QUADRATURE_ENCODER_TASK_HPP
+#define QUADRATURE_ENCODER_TASK_HPP
+
+#include <Encoders/encoderinterface.hpp>
+#include <Encoders/QuadratureEncoder.hpp>
+#include <Filter/Filter.hpp>
+
+#include <rtos.h>
+
+namespace encoders{
+
+/**
+ * @brief Rotary encoder with filter interface class
+ * 
+ */
+class CQuadratureEncoderNonFilteredGetterInterface{
+    public:
+      virtual int16_t  getNonFilteredCount()=0;
+      virtual float getNonFilteredSpeedRps() = 0;
+};
+
+/**
+ * @brief It implements a periodic task, which get the value from the counter and reset it to zero.
+ * 
+ */
+class CQuadratureEncoderTask:public IEncoderGetter{
+    public:
+      CQuadratureEncoderTask(float,encoders::CQuadratureEncoder_TIMX*,uint16_t);
+      static void static_callback(void* obj);
+
+      void startTimer();
+  protected:
+    virtual void _run();
+    virtual int16_t getCount();
+    virtual float getSpeedRps();
+    virtual bool isAbs(){return false;}
+  protected:
+      encoders::CQuadratureEncoder_TIMX *m_Quadratureencoder;
+      int16_t           m_encoderCnt;
+      const float       m_taskperiod_s;
+      const uint16_t    m_resolution;
+      RtosTimer         m_timer;
+};
+
+/**
+ * @brief It implements the same functionality than CQuadratureEncoderTask class, but in additional it can filter the values. 
+ * 
+ */
+class CQuadratureEncoderWithFilterTask: public CQuadratureEncoderTask, public CQuadratureEncoderNonFilteredGetterInterface{
+    public:
+      CQuadratureEncoderWithFilterTask(float,encoders::CQuadratureEncoder_TIMX *, uint16_t,filter::CFilterFunction<float>&);
+      virtual void _run();
+      virtual int16_t getCount();
+      virtual float getSpeedRps();
+      virtual int16_t  getNonFilteredCount();
+      virtual float getNonFilteredSpeedRps();
+    protected:
+      double m_encoderCntFiltered;
+      filter::CFilterFunction<float>& m_filter;
+
+};
+
+}; // namespace encoders
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Examples/Filter/ekf_am.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,48 @@
+/**
+  ******************************************************************************
+  * @file    Ekf_am.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class declaration for the Ackermann model example
+  *          functionality.
+  ******************************************************************************
+ */
+
+/* Include guard */
+#ifndef EKF_AM_HPP
+#define EKF_AM_HPP
+
+#include <Filter/filter.hpp>
+#include <Examples/SystemModels/ackermannmodel.hpp>
+
+namespace examples
+{
+    namespace filter
+    {
+        /* Extended Kalman Filter */
+        template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+        using CEKF  =   ::filter::ltv::mimo::CEKF<T,NA,NB,NC>;
+        //!  CEKF_AM class. 
+        /*!
+        * It inherits class CEKF. 
+        * Extended Kalman Filter aplied on the Ackermann Model.
+        */
+        class CEKF_AM:public CEKF<double,2,10,5>
+        {
+            private:
+                /* Ackermann model*/
+                using CAckermannModel = ::examples::systemmodels::ackermannmodel::CAckermannModel;
+                /* Ackermann model Jacobian Matrix */
+                using CJMAckermannModel = ::examples::systemmodels::ackermannmodel::CJMAckermannModel;
+            public:
+                /* Constructor */
+                CEKF_AM(CAckermannModel&              f_ackermannModel
+                        ,CJMAckermannModel             f_jacobianMatrixCalc
+                        ,const CJMTransitionType&      f_Q
+                        ,const CObservationNoiseType&  f_R);
+        };
+    };
+};
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Examples/Sensors/sensorpublisher.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,61 @@
+/**
+  ******************************************************************************
+  * @file    SensorPublisher.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class declaration for the sensor publisher
+  *          functionality. Because template classes are used, the code was
+  *          placed in a corresponding .inl file and not in a .cpp one.
+  ******************************************************************************
+ */
+
+/* Include guard */
+#ifndef SENSOR_PUBLISHER_HPP
+#define SENSOR_PUBLISHER_HPP
+
+/* The mbed library */
+#include <mbed.h>
+/* Container that encapsulates fixed size arrays */
+#include <array>
+/* String types, character traits and a set of converting functions */
+#include <string>
+#include<TaskManager/taskmanager.hpp>
+
+
+namespace examples{
+    namespace sensors
+    {
+        //!  ProximityPublisher class. 
+        /*!
+        * It inherits class task::CTask. 
+        * It is used for publishing proximity sensor values.
+        */
+        template <class C_Sensor,uint Nr_Senrsor> 
+        class ProximityPublisher:public task::CTask
+        {
+            public:
+                /* Sensor array */
+                using SensorArrayT = std::array<C_Sensor*,Nr_Senrsor>;
+                /* Constructor */
+                ProximityPublisher(uint32_t,SensorArrayT,Serial&);
+                /* Serial callback attached to object */
+                static void staticSerialCallback(void* obj,char const * a, char * b);
+            protected:
+                /* Run method */
+                void _run();
+                /* Serial callback */
+                void serialCallback(char const * a, char * b);
+            private:
+                /* Sensor array */
+                SensorArrayT            m_sensors;
+                /* Reference to serial object */
+                Serial&                 m_serial;
+                /* Active flag */
+                bool                    m_isActivate;
+        };
+    }; // namespace sensors
+}; // namespace examples
+
+#include "sensorpublisher.inl"
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Examples/Sensors/sensorpublisher.inl	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,78 @@
+namespace examples{
+    namespace sensors{
+        /** \brief  Constructor for the CEncoderFilter class
+         *
+         *  Constructor method
+         *
+         *  @param f_period       period value 
+         *  @param f_sensors      sensor array 
+         *  @param f_serial       reference to the serial object
+         */
+        template <class C_Sensor,uint Nr_Senrsor> 
+        ProximityPublisher<C_Sensor,Nr_Senrsor>::ProximityPublisher(uint32_t        f_period
+                                                                ,SensorArrayT    f_sensors
+                                                                ,Serial&         f_serial)
+            :CTask(f_period)
+            ,m_sensors(f_sensors)
+            ,m_serial(f_serial)
+            ,m_isActivate(false)
+        {
+        }
+
+        /** \brief  Serial callback method
+         *
+         * Serial callback attaching serial callback to controller object
+         *
+         * @param  obj                 PID controller object
+         * @param  a                   string to read data from
+         * @param b                   string to write data to
+         * 
+         */
+        template <class C_Sensor,uint Nr_Senrsor> 
+        void ProximityPublisher<C_Sensor,Nr_Senrsor>::staticSerialCallback(void* obj,char const * a, char * b)
+        {
+            ProximityPublisher* self = static_cast<ProximityPublisher*>(obj);
+            self->serialCallback(a,b);
+        }
+
+        /** \brief  Serial callback actions 
+         *
+         * Serial callback
+         *
+         * @param  a                   string to read data from
+         * @param b                   string to write data to
+         * 
+         */
+        template <class C_Sensor,uint Nr_Senrsor> 
+        void ProximityPublisher<C_Sensor,Nr_Senrsor>::serialCallback(char const * a, char * b){
+            int l_isActivate=0;
+            uint32_t l_res = sscanf(a,"%d",&l_isActivate);
+            if(l_res==1){
+                m_isActivate=(l_isActivate>=1);
+                sprintf(b,"ack;;");
+            }else{
+                sprintf(b,"sintax error;;");
+            }
+        }
+
+        /** \brief  Method called each f_period
+         * 
+         *  
+         *  
+         */
+        template <class C_Sensor,uint Nr_Senrsor> 
+        void ProximityPublisher<C_Sensor,Nr_Senrsor>::_run(){
+            if(!m_isActivate) return;
+            char l_buf[100];
+            sprintf(l_buf,"@DSPB:");
+            for(uint8_t i=0;i<Nr_Senrsor;++i){
+                
+                sprintf(l_buf,"%s%2.2f;",l_buf,m_sensors[i]->getValue());
+            }
+            sprintf(l_buf,"%s;\n\r",l_buf);
+            m_serial.printf("%s",l_buf);
+
+        }
+    }; // namespace sensors
+}; // namespace examples
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Examples/Sensors/sensortask.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,85 @@
+/**
+  ******************************************************************************
+  * @file    SensorTask.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class declaration for the sensor task
+  *          functionality.
+  ******************************************************************************
+ */
+
+/* Include guard */
+#ifndef SENSOR_TASK_HPP
+#define SENSOR_TASK_HPP
+
+/* Standard C library for basic mathematical operations */
+#include <math.h>
+#include<TaskManager/taskmanager.hpp>
+#include <Drivers/vnh.hpp>
+#include <SerialMonitor/serialmonitor.hpp>
+#include <Controllers/controller.hpp>
+#include <SystemModels/systemmodels.hpp>
+
+#include <Encoders/encoderinterface.hpp>
+
+namespace examples
+{
+
+    namespace sensors{
+
+    
+        //!  CDISTFTest class. 
+        /*!
+        * It inherits class task::CTask. 
+        * It is used for testing distance sensors operation.
+        */
+        class CDISTFTest:public task::CTask
+        {
+            public:
+                /* Constructor */
+                CDISTFTest(uint32_t        f_period
+                        ,Serial&         f_serial);
+                
+            private:
+                /* Run method */
+                void _run();
+                
+                /* Reference to serial object */
+                Serial&                                                       m_serial;
+                /* Transfer function */
+                systemmodels::lti::siso::CDiscreteTransferFucntion<float,3,3> m_tf;
+        };
+
+        //!  CEncoderSender class. 
+        /*!
+        * It inherits class task::CTask. 
+        * It is used for sending acquired data.
+        */
+        class CEncoderSender:public task::CTask
+        {
+            public:
+                /* Constructor */
+                CEncoderSender(uint32_t            f_period
+                            ,encoders::IEncoderGetter&    f_encoder
+                            ,Serial&            f_serial);
+                /* Serial callback */
+                static void staticSerialCallback(void* obj,char const * a, char * b);
+            private:
+                /* Serial callback implementation */
+                void serialCallback(char const * a, char * b);
+                /* Run method */
+                void _run();
+
+                /* Active flag */
+                bool                m_isActived;
+                /*  Encoder value getter */
+                encoders::IEncoderGetter&     m_encoder;
+                /* Serial communication member*/
+                Serial&             m_serial;
+        };
+    }; // namespace sensors
+}; // namespace examples
+
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Examples/SystemModels/ackermannmodel.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,182 @@
+
+/**
+  ******************************************************************************
+  * @file    AckermannModel.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class declaration for the Ackermann model.
+  ******************************************************************************
+ */
+
+/* Include guard */
+#ifndef ACKERMANN_MODEL_HPP
+#define ACKERMANN_MODEL_HPP
+
+#include <math.h>
+#include <SystemModels/systemmodels.hpp>
+#include <Examples/SystemModels/ackermanntypes.hpp>
+
+/* Definition of PI value */
+#ifndef M_PI
+    #define M_PI 3.14159265358979323846
+#endif
+
+/* Definition of degrees to radians transformation */
+#ifndef DEG2RAD 
+    #define DEG2RAD M_PI/180.0
+#endif
+
+namespace examples
+{
+    namespace systemmodels
+    {
+        namespace ackermannmodel
+        {
+            /* System model type */
+            template<class T,uint32_t NA,uint32_t NB,uint32_t NC>
+            using CSystemModelType      = ::systemmodels::nlti::mimo::CDiscreteTimeSystemModel<T,NA,NB,NC>;
+            /* Jacobian matrix*/
+            template<class T,uint32_t NA,uint32_t NB,uint32_t NC>
+            using CJacobianMatricesType = ::systemmodels::nlti::mimo::CJacobianMatrices<T,NA,NB,NC>;
+            //!  CAckermannModel class. 
+            /*!
+            * It inherits class CSystemModelType. 
+            */
+            class CAckermannModel:public CSystemModelType<double,2,10,5>
+            {
+                public:
+                    /* Constructor */
+                    CAckermannModel(const double       f_dt     
+                                    ,const double      f_gamma
+                                    ,const double      f_wb
+                                    ,const double      f_b
+                                    ,const double      f_J
+                                    ,const double      f_K
+                                    ,const double      f_R
+                                    ,const double      f_L);
+                    /* Constructor */
+                    CAckermannModel(const CStatesType&      f_states
+                                    ,const double            f_dt     
+                                    ,const double            f_gamma
+                                    ,const double            f_wb
+                                    ,const double            f_b
+                                    ,const double            f_J
+                                    ,const double            f_K
+                                    ,const double            f_R
+                                    ,const double            f_L);
+                    /* Update method */
+                    CStatesType update(const CInputType&       f_input);
+                    
+                    /* Calculate output method */
+                    COutputType calculateOutput(const CInputType&  f_input);
+                private:
+                    /* gamma=Meter/Rotation */
+                    const double m_gamma;
+                    /* Wheel base distance in meter */
+                    const double m_wb;
+                    /* Motor Constants */
+                    const double m_bJ,m_KJ,m_KL,m_RL,m_L;      
+            };
+
+            //!  CJMAckermannModel class. 
+            /*!
+            * It inherits class CJacobianMatricesType. 
+            */
+            class CJMAckermannModel:public CJacobianMatricesType<double,2,10,5>
+            {
+                public:
+                    CJMAckermannModel   (double          f_dt
+                                        ,double          f_gamma
+                                        ,double          f_wb
+                                        ,double          f_b
+                                        ,double          f_J
+                                        ,double          f_K
+                                        ,double          f_R
+                                        ,double          f_L)
+                                        :m_dt(f_dt)
+                                        ,m_gamma(f_gamma)
+                                        ,m_wb(f_wb)
+                                        ,m_bJ(f_b/f_J)
+                                        ,m_KJ(f_K/f_J)
+                                        ,m_KL(f_K/f_L)
+                                        ,m_RL(f_R/f_L)
+                    {
+                        m_ObservationMatrix=initObservationMatrix();
+                    }
+                
+
+
+                    CJMObservationType getJMObservation(    const CStatesType&       f_states
+                                                            ,const CInputType&       f_input){
+                        return m_ObservationMatrix;
+                    }
+                
+                private:
+                    linalg::CMatrix<double,5,10> initObservationMatrix(){
+                        linalg::CMatrix<double,5,10> l_data;
+                        l_data[0][2]=l_data[1][3]=1.f/m_dt;
+                        l_data[0][4]=l_data[1][5]=-1.f/m_dt;
+                        l_data[2][7]=1.f;
+                        l_data[3][8]=m_gamma;
+                        l_data[4][9]=1.f;
+                        return l_data;
+                    }
+
+                    inline void  setJacobianStateMatrixWithOne(CJMTransitionType& f_matrix){
+                        f_matrix[0][0]=f_matrix[1][1]=1;
+                        f_matrix[4][2]=f_matrix[5][3]=1;
+                        f_matrix[6][6]=1;
+                        return;
+                    }
+
+                public:
+                    CJMTransitionType getJMTransition(       const CStatesType&       f_states
+                                                            ,const CInputType&       f_input){
+
+                        CJMTransitionType l_data(CJMTransitionType::zeros());
+                        setJacobianStateMatrixWithOne(l_data);
+                        CState l_states(f_states);
+                        CInput l_input(f_input);
+
+                        //Setting values in the jacobian matrix
+                        l_data[0][2]=m_dt;
+                        l_data[1][3]=m_dt;
+                        
+                        l_data[2][6]=-m_gamma*l_states.omega()*sin(l_states.teta_rad());
+                        l_data[2][8]=m_gamma*cos(l_states.teta_rad());
+
+                        l_data[3][6]=m_gamma*l_states.omega()*cos(l_states.teta_rad());
+                        l_data[3][8]=m_gamma*sin(l_states.teta_rad());
+
+                        double l_alpha_rad=l_input.alpha()*DEG2RAD;
+                        l_data[7][8]=l_data[6][8]=m_dt*m_gamma*tan(l_alpha_rad)/m_wb;
+                        //l_data[7][11]=l_data[6][11]=m_dt*m_gamma*f_state.omega/(m_wb*pow(cos(l_alpha_rad),2));
+
+                        l_data[8][8]=(1-m_dt*m_bJ);
+                        l_data[8][9]=m_dt*m_KJ;
+                        l_data[9][8]=m_dt*m_KL;
+                        l_data[9][9]=(1-m_dt*m_RL);
+                        //l_data[9][10]=m_dt;
+                        return l_data;
+                    }
+
+                private:
+                    /* Constant values  */
+                    /* Time step  */
+                    const double m_dt;
+                    /* gamma=Meter/Rotation  */
+                    const double m_gamma;
+                    /* Wheel base distance in meter  */
+                    const double m_wb;
+                    /* Motor Constants  */
+                    const double m_bJ,m_KJ,m_KL,m_RL;
+                    /* Please leave this matrix to be the last member in this class, 
+                    as it will be initialized  */
+                    linalg::CMatrix<double,5,10> m_ObservationMatrix;    
+            };
+        };
+    };
+};
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Examples/SystemModels/ackermanntypes.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,109 @@
+/**
+  ******************************************************************************
+  * @file    AckermannTypes.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class declaration and implementation for the
+             Ackermann types.
+  ******************************************************************************
+ */
+
+/* Include guard */
+#ifndef ACKERMANN_TYPES_HPP
+#define ACKERMANN_TYPES_HPP
+
+#include <Linalg/linalg.h>
+
+namespace examples
+{
+    namespace systemmodels
+    {
+        namespace ackermannmodel
+        {
+            class CState: public linalg::CMatrix<double,10,1>
+            {
+                public:
+                    CState():linalg::CMatrix<double,10,1>(){}
+                    CState(const linalg::CMatrix<double,10,1>& f_matrix):linalg::CMatrix<double,10,1>(f_matrix){}
+                    double& x(){return m_data[0][0];}
+                    const double& x() const {return m_data[0][0];}
+                    
+                    double& y(){return m_data[1][0];}
+                    const double& y() const {return m_data[1][0];}
+                    
+                    double& x_dot(){return m_data[2][0];}
+                    const double& x_dot() const {return m_data[2][0];}
+
+                    double& y_dot(){return m_data[3][0];}
+                    const double& y_dot() const{return m_data[3][0];}
+
+                    double& x_dot_prev(){return m_data[4][0];}
+                    const double& x_dot_prev() const{return m_data[4][0];}
+
+                    double& y_dot_prev(){return m_data[5][0];}
+                    const double& y_dot_prev() const{return m_data[5][0];}
+
+                    double& teta_rad(){return m_data[6][0];}
+                    const double& teta_rad() const{return m_data[6][0];}
+
+                    double& teta_rad_dot(){return m_data[7][0];}
+                    const double& teta_rad_dot() const{return m_data[7][0];}
+                    
+                    double& omega(){ return m_data[8][0];}
+                    const double& omega() const{return m_data[8][0];}
+
+                    double& i(){return m_data[9][0];}
+                    const double& i() const{return m_data[9][0];}
+
+                    // CState& operator=(const linalg::CMatrix<double,10,1>& f_matrix){
+                    //     for (uint32_t l_row = 0; l_row < 10; ++l_row)
+                    //     {
+                    //         for (uint32_t l_col = 0; l_col < 1; ++l_col)
+                    //         {
+                    //             this->m_data[l_row][l_col] = f_matrix[l_row][l_col];
+                    //         }
+                    //     }
+                    //     return *this;
+                    // }
+            };
+
+            class CInput:public linalg::CMatrix<double,2,1>
+            {
+                public:
+                    CInput():linalg::CMatrix<double,2,1>(){}
+                    CInput(const linalg::CMatrix<double,2,1>& f_matrix):linalg::CMatrix<double,2,1>(f_matrix){} 
+                    double& v(){return m_data[0][0];}
+                    const double& v()const{return m_data[0][0];}
+                    double& alpha(){return m_data[1][0];}
+                    const double& alpha()const{return m_data[1][0];}
+            };
+
+            class COutput:public linalg::CMatrix<double,5,1>
+            {
+                public:
+                    COutput():linalg::CMatrix<double,5,1>(){}
+                    COutput(const linalg::CMatrix<double,5,1>& f_matrix):linalg::CMatrix<double,5,1>(f_matrix){}
+                    double& x_ddot(){return m_data[0][0];}
+                    const double& x_ddot() const{return m_data[0][0];}
+                    double& y_ddot(){return m_data[1][0];}
+                    const double& y_ddot()const {return m_data[1][0];}
+                    double& teta_rad_dot(){return m_data[2][0];}
+                    const double& teta_rad_dot()const{return m_data[2][0];}
+                    double& speed(){return m_data[3][0];}
+                    const double& speed()const {return m_data[3][0];}
+                    
+                    // double& alpha(){
+                    //     return m_data[4][0];
+                    // }
+                    // const double& alpha()const{
+                    //     return m_data[4][0];
+                    // }
+                    double& i(){return m_data[4][0];}
+                    const double& i()const{return m_data[4][0];}
+            };
+        };
+    };
+};
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Examples/blinker.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,40 @@
+/**
+  ******************************************************************************
+  * @file    Blinker.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class definition for the blinker
+  *          functionality.
+  ******************************************************************************
+ */
+
+/* Include guard */
+#ifndef BLINKER_HPP
+#define BLINKER_HPP
+
+/* The mbed library */
+#include <mbed.h>
+#include <TaskManager/taskmanager.hpp>
+
+
+namespace examples{
+
+   /**
+    * @brief It is used for toggling an LED.
+    * 
+    */
+    class CBlinker : public task::CTask
+    {
+        public:
+            /* Construnctor */
+            CBlinker(uint32_t f_period, DigitalOut f_led);
+        private:
+            /* Run method */
+            virtual void _run();
+            /* Digital output line connected to a LED */
+            DigitalOut m_led;    
+    };
+}; // namespace examples
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Examples/echoer.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,41 @@
+/**
+  ******************************************************************************
+  * @file    Echoer.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class definition for the serial echoer
+  *          functionality.
+  ******************************************************************************
+ */
+
+/* Include guard */
+#ifndef ECHOER_HPP
+#define ECHOER_HPP
+
+/* The mbed library */
+#include <mbed.h>
+#include <TaskManager/taskmanager.hpp>
+
+
+namespace examples{
+   /**
+    * @brief It is used for echoing messages over UART.
+    * 
+    */
+    class CEchoer : public task::CTask
+    {
+        public:
+            /* Construnctor */
+            CEchoer(uint32_t f_period, Serial& f_serialPort);
+        private:
+            /* Run method */
+            virtual void _run();
+
+            /* Serial communication member*/
+            Serial& m_serialPort; 
+    };
+
+}; // namespace examples
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Filter/filter.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,318 @@
+/**
+  ******************************************************************************
+  * @file    Filter.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class declaration for the filter
+  *          functionality.
+  ******************************************************************************
+ */
+
+/* Include guard */
+#ifndef FILTER_HPP
+#define FILTER_HPP
+
+#include <Linalg/linalg.h>
+#include <SystemModels/systemmodels.hpp>
+
+namespace filter
+{
+    /**
+     * @brief The filter interface
+     * 
+     * @tparam T The type of the input and output signal
+     */
+    template<class T>
+    class CFilterFunction
+    {
+    public:
+        virtual T operator()(T&)=0;
+    };
+
+    namespace lti
+    {
+        namespace siso
+        {
+            /**
+             * @brief Infinite impulse response (IIR) discrete-time filter.
+             * 
+             */
+            template <class T, uint32_t NA, uint32_t NB>
+            class CIIRFilter:public CFilterFunction<T>
+            {
+            public:
+                /* Constructor */
+                CIIRFilter(const linalg::CRowVector<T,NA>& f_A,const linalg::CRowVector<T,NB>& f_B);
+                /* Operator */
+                T operator()(T& f_u);
+            private:
+                CIIRFilter(){}
+                linalg::CRowVector<T,NA> m_A;
+                linalg::CRowVector<T,NB> m_B;
+                linalg::CColVector<T,NA> m_Y;
+                linalg::CColVector<T,NB> m_U;
+            };
+
+            /**
+             * @brief Finite impulse response (FIR) discrete-time filter.
+             * 
+             */
+            template <class T, uint32_t NB>
+            class CFIRFilter:public CFilterFunction<T>
+            {
+            public:
+                /* Constructor */
+                CFIRFilter(const linalg::CRowVector<T,NB>& f_B);
+                /* Operator */
+                T operator()(T& f_u);
+            private:
+                CFIRFilter() {}
+                linalg::CRowVector<T,NB> m_B;
+                linalg::CColVector<T,NB> m_U;
+            };
+
+            /**
+             * @brief Mean filter
+             * 
+             * @tparam T 
+             * @tparam NB 
+             */
+            template <class T, uint32_t NB>
+            class CMeanFilter:public CFilterFunction<T>
+            {
+            public:
+                /* Constructor */
+                CMeanFilter();
+                /* Operator */
+                virtual T operator()(T& f_u);
+            private:
+                T m_B;
+                linalg::CColVector<T,NB> m_U;
+            };
+        };
+
+        namespace mimo
+        {
+            //!  CSSModel class. 
+            /*!
+             *  
+             * */
+            template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+            class CSSModel
+            {
+            public:
+                using CStateType = linalg::CColVector<T,NA>;
+                using CStateTransitionType = linalg::CMatrix<T,NA,NA>;
+                using CInputType = linalg::CColVector<T,NB>;
+                using CMeasurementType = linalg::CColVector<T,NC>;
+                using CInputMatrixType = linalg::CMatrix<T,NA,NB>;
+                using CMeasurementMatrixType = linalg::CMatrix<T,NC,NA>;
+                using CDirectTransferMatrixType = linalg::CMatrix<T,NC,NB>;
+                /* Constructor */
+                CSSModel(
+                    const CStateTransitionType& f_stateTransitionMatrix,
+                    const CInputMatrixType& f_inputMatrix,
+                    const CMeasurementMatrixType& f_measurementMatrix);
+                /* Constructor */
+                CSSModel(
+                    const CStateTransitionType& f_stateTransitionMatrix,
+                    const CInputMatrixType& f_inputMatrix,
+                    const CMeasurementMatrixType& f_measurementMatrix,
+                    const CDirectTransferMatrixType& f_directTransferMatrix);
+                /* Constructor */
+                CSSModel(
+                    const CStateTransitionType& f_stateTransitionMatrix,
+                    const CInputMatrixType& f_inputMatrix,
+                    const CMeasurementMatrixType& f_measurementMatrix,
+                    const CDirectTransferMatrixType& f_directTransferMatrix,
+                    const CStateType& f_state);
+
+                /* Get state method */
+                const CStateType& state() const {return m_stateVector;} 
+                CStateType& state() {return m_stateVector;} 
+
+                /* Operator */
+                CMeasurementType operator()(const CInputType& f_inputVector);
+                /* Update state */
+                void updateState(const CInputType& f_inputVector);
+                /* Get output */
+                CMeasurementType getOutput(const CInputType& f_inputVector);
+
+            private:
+                CSSModel() {}
+                /* state vector */
+                CStateType m_stateVector;
+                /* state transition matrix */
+                CStateTransitionType m_stateTransitionMatrix;
+                /* input matrix */
+                CInputMatrixType m_inputMatrix;
+                /* measurement matrix */
+                CMeasurementMatrixType m_measurementMatrix;
+                /* direct transfer matrix */
+                CDirectTransferMatrixType m_directTransferMatrix;
+            };
+
+            //!  CKalmanFilter class. 
+            /*!
+             *  
+             * */
+            template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+            class CKalmanFilter
+            {
+            public:
+                using CStateType                 = linalg::CVector<T, NA>;
+                using CInputVectorType           = linalg::CVector<T, NB>;
+                using COutputVectorType          = linalg::CVector<T, NC>;
+                using CInputType                 = linalg::CMatrix<T, NA, NB>;
+                using CControInputType           = linalg::CMatrix<T, NA, NC>;
+                using CModelCovarianceType       = linalg::CMatrix<T, NA, NA>;
+                using CMeasurementCovarianceType = linalg::CMatrix<T, NC, NC>;
+                using CStateTransType            = linalg::CMatrix<T, NA, NA>;
+                using CMeasurementType           = linalg::CMatrix<T, NC, NA>;
+                using CKalmanGainType            = linalg::CMatrix<T, NA, NC>;
+
+                /* Constructor */
+                CKalmanFilter(
+                    const CStateTransType&            f_stateTransitionModel,
+                    const CControInputType&           f_controlInput,
+                    const CMeasurementType&           f_observationModel,
+                    const CModelCovarianceType&       f_covarianceProcessNoise,
+                    const CMeasurementCovarianceType& f_observationNoiseCovariance);
+
+                /* Get state method */
+                const CStateType& state() const
+                {
+                    return m_posterioriState;
+                }
+                /* Get state method */
+                CStateType& state()
+                {
+                    return m_posterioriState;
+                }
+                /* Operator */
+                CMeasurementType operator()(const CInputVectorType& f_input);
+                /* Operator */
+                CMeasurementType operator()();
+                /* Predict */
+                void predict();
+                /* Predict */
+                void predict(const CInputVectorType& f_input);
+                /* Update */
+                const CMeasurementType& update(void);
+
+                private:
+                CKalmanFilter() {}
+
+                CStateType                 m_previousState;              // previous state
+                CStateType                 m_prioriState;                // priori state
+                CStateType                 m_posterioriState;            // posteriori state
+                CControInputType           m_controlInput;               // control input modelþ
+                CModelCovarianceType       m_previousCovariance;         // previous covariance estimate         // <-
+                CModelCovarianceType       m_prioriCovariance;           // priori covariance estimate           // <-
+                CModelCovarianceType       m_posterioriCovariance;       // posteriori covariance estimate       // <-
+                CStateTransType            m_stateTransitionModel;       // state transition model
+                CModelCovarianceType       m_covarianceProcessNoise;     // covariance of process noise          // done
+                CMeasurementType           m_measurementResidual;        // innovation or measurement residual
+                CMeasurementType           m_measurement;                // observation (or measurement)
+                CMeasurementType           m_observationModel;           // observation model
+                CMeasurementCovarianceType m_residualCovariance;         // innovation or residual covariance    // <-
+                CMeasurementCovarianceType m_observationNoiseCovariance; // covariance of observation noise      // <-
+                CKalmanGainType            m_kalmanGain;                 // optimal kalman gain
+            };
+
+        };
+    };
+    namespace ltv
+    {
+        namespace mimo
+        {
+            // Extended Kalman Filter
+            // Template parameters:
+            //              - T     variable type
+            //              - NA    no. of inputs
+            //              - NB    no. of states
+            //              - NC    no. of outputs
+            // Input parameters
+            template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+            class CEKF
+            {
+                public:
+                    using CSystemModelType              =   systemmodels::nlti::mimo::CDiscreteTimeSystemModel<T,NA,NB,NC>;
+                    using CJacobianMatricesType         =   systemmodels::nlti::mimo::CJacobianMatrices<T,NA,NB,NC>;
+                    using CStatesType                   =   linalg::CMatrix<T,NB,1>;
+                    using CInputType                    =   linalg::CMatrix<T,NA,1>;
+                    using COutputType                   =   linalg::CMatrix<T,NC,1>;
+                    using CJMTransitionType             =   linalg::CMatrix<T,NB,NB>;
+                    using CJMObservationType            =   linalg::CMatrix<T,NC,NB>;
+                    using CObservationNoiseType         =   linalg::CMatrix<T,NC,NC>;
+                    using CKalmanGainType               =   linalg::CMatrix<T,NC,NB>;
+                    
+                    /* Constructor */
+                    CEKF(CSystemModelType&                          f_systemModel
+                        ,CJacobianMatricesType&                     f_jbMatrices
+                        ,const CJMTransitionType&                   f_Q
+                        ,const CObservationNoiseType&               f_R);
+                    /* Predict */
+                    void predict    (const CInputType&         f_input);
+                    /* Update */
+                    void update     (const CInputType&         f_input
+                                    ,const COutputType&        f_measurement);
+                protected:
+                private:
+                    CSystemModelType&                    m_systemModel;
+                    CJacobianMatricesType&               m_jbMatrices;
+                    //Predicted covariance matrix
+                    CJMTransitionType                   m_covarianceMatrix;
+                    //The covariance of the process noise
+                    CJMTransitionType                   m_Q;
+                    //The covariance of the observation noise
+                    CObservationNoiseType               m_R;
+
+            };
+        };
+    };
+
+    namespace nonlinear
+    {
+        namespace siso
+        {   
+            
+            /**
+             * @brief Median filter
+             * 
+             */
+            template <class T, uint32_t N>
+            class CMedianFilter:public CFilterFunction<T>
+            {
+            public:
+                /* Constructor */
+                CMedianFilter();
+                inline T addNewValue(T& f_val);
+                inline T getMedian();
+                inline T operator()(T& f_v);
+
+            private:
+                struct my_structure
+                {
+                    T info;
+                      my_structure *next;
+                      my_structure *prev;
+                    
+                };
+
+                my_structure *m_median;    //pointer to the loist element that represents the median
+                my_structure *m_smallest;  //pionter to the smallest element of the list (the first)
+                uint32_t m_new;		     //index to tyhe newest element in the list
+                uint32_t m_size;		     //the size of the list/queue
+
+                my_structure m_queue[N];
+            };
+        }
+    }
+};
+
+#include "filter.inl"
+
+#endif // FILTER_H
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Filter/filter.inl	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,558 @@
+/**
+  ******************************************************************************
+  * @file    Filter.inl
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class implementations for the filter
+  *          functionality.
+  ******************************************************************************
+ */
+
+template <class T, uint32_t NC>
+using CMeasurementType = linalg::CColVector<T,NC>;
+
+/******************************************************************************/
+/** @brief  CIIRFilter Class constructor
+ *
+ *  Constructor method
+ *
+ *  @param f_A   the feedback filter coefficients 
+ *  @param f_B   the feedforward filter coefficients
+ */
+template <class T, uint32_t NA, uint32_t NB>
+filter::lti::siso::CIIRFilter<T,NA,NB>::CIIRFilter(const linalg::CRowVector<T,NA>& f_A,const linalg::CRowVector<T,NB>& f_B) 
+    : m_A(f_A)
+    , m_B(f_B)
+    , m_Y()
+    , m_U() 
+{
+}
+
+/** @brief  Operator
+  *
+  * @param f_u                 reference to input data
+  * @return                        the filtered output data
+  */
+template <class T, uint32_t NA, uint32_t NB>
+T filter::lti::siso::CIIRFilter<T,NA,NB>::operator()(T& f_u)
+{
+    for (uint32_t l_idx = NB-1; l_idx > 0; --l_idx)
+    {
+        m_U[l_idx] = m_U[l_idx-1];
+    }
+    m_U[0][0] = f_u;
+
+    linalg::CMatrix<T,1,1> l_y = m_B*m_U - m_A*m_Y;
+    // T l_y = m_B*m_U - m_A*m_Y;
+
+    for (uint32_t l_idx = NA-1; l_idx > 0 ; --l_idx)
+    {
+        m_Y[l_idx] = m_Y[l_idx-1];
+    }
+    m_Y[0][0] = l_y[0][0];
+
+    return m_Y[0][0];
+}
+
+/******************************************************************************/
+/** @brief  CFIRFilter Class constructor
+ *
+ *  Constructor method
+ *
+ *  @param f_B   the feedforward filter coefficients
+ */
+template <class T, uint32_t NB>
+filter::lti::siso::CFIRFilter<T,NB>::CFIRFilter(const linalg::CRowVector<T,NB>& f_B) 
+    : m_B(f_B), m_U() 
+{
+}
+
+/** @brief  Operator
+  *
+  * @param f_u                 reference to the input data
+  * @return                        the filtered output data
+  */
+template <class T, uint32_t NB>
+T filter::lti::siso::CFIRFilter<T,NB>::operator()(T& f_u)
+{
+    for (uint32_t l_idx = NB-1; l_idx > 0; --l_idx)
+    {
+        m_U[l_idx] = m_U[l_idx-1];
+    }
+    m_U[0] = f_u;
+
+    linalg::CMatrix<T,1,1> l_y = m_B*m_U;
+
+    return l_y[0][0];
+}
+
+/******************************************************************************/
+/** @brief  MeanFilter Class constructor
+ *
+ *  Constructor method
+ *
+ *  
+ */
+template <class T, uint32_t NB> 
+filter::lti::siso::CMeanFilter<T,NB>::CMeanFilter() 
+    : m_B(1./NB)
+    , m_U() 
+{
+}
+
+/** @brief  Operator
+  *
+  * @param f_u                 reference to the input data
+  * @return                        the filtered output data 
+  */
+template <class T, uint32_t NB> 
+T filter::lti::siso::CMeanFilter<T,NB>::operator()(T& f_u)
+{
+    T l_y =0;
+
+    for (uint32_t l_idx = 1; l_idx < NB; ++l_idx)
+    {
+        m_U[l_idx][0] = m_U[l_idx-1][0];
+        l_y += m_U[l_idx][0];
+    }
+    m_U[0][0] = f_u;
+    l_y += m_U[0][0];
+
+    return m_B*l_y;
+}
+
+/******************************************************************************/
+/** @brief  CSSModel Class constructor
+ *
+ *  Constructor method
+ *
+ *  @param f_stateTransitionMatrix
+ *  @param f_inputMatrix
+ *  @param f_measurementMatrix
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+filter::lti::mimo::CSSModel<T,NA,NB,NC>::CSSModel(
+        const CStateTransitionType& f_stateTransitionMatrix,
+        const CInputMatrixType& f_inputMatrix,
+        const CMeasurementMatrixType& f_measurementMatrix) 
+    : m_stateVector()
+    , m_stateTransitionMatrix(f_stateTransitionMatrix)
+    , m_inputMatrix(f_inputMatrix)
+    , m_measurementMatrix(f_measurementMatrix)
+    , m_directTransferMatrix()
+{
+    // do nothing
+}
+
+/** @brief  CSSModel Class constructor
+ *
+ *  Constructor method
+ *
+ *  @param f_stateTransitionMatrix
+ *  @param f_inputMatrix
+ *  @param f_measurementMatrix
+ *  @param f_directTransferMatrix
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+filter::lti::mimo::CSSModel<T,NA,NB,NC>::CSSModel(
+        const CStateTransitionType& f_stateTransitionMatrix,
+        const CInputMatrixType& f_inputMatrix,
+        const CMeasurementMatrixType& f_measurementMatrix,
+        const CDirectTransferMatrixType& f_directTransferMatrix) 
+    : m_stateVector()
+    , m_stateTransitionMatrix(f_stateTransitionMatrix)
+    , m_inputMatrix(f_inputMatrix)
+    , m_measurementMatrix(f_measurementMatrix)
+    , m_directTransferMatrix(f_directTransferMatrix)
+{
+    // do nothing
+}
+
+/** @brief  CSSModel Class constructor
+ *
+ *  Constructor method
+ *
+ *  @param f_stateTransitionMatrix
+ *  @param f_inputMatrix
+ *  @param f_measurementMatrix
+ *  @param f_directTransferMatrix
+ *  @param f_state
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+filter::lti::mimo::CSSModel<T,NA,NB,NC>::CSSModel(
+        const CStateTransitionType& f_stateTransitionMatrix,
+        const CInputMatrixType& f_inputMatrix,
+        const CMeasurementMatrixType& f_measurementMatrix,
+        const CDirectTransferMatrixType& f_directTransferMatrix,
+        const CStateType& f_state) 
+    : m_stateVector(f_state)
+    , m_stateTransitionMatrix(f_stateTransitionMatrix)
+    , m_inputMatrix(f_inputMatrix)
+    , m_measurementMatrix(f_measurementMatrix)
+    , m_directTransferMatrix(f_directTransferMatrix)
+{
+    // do nothing
+}
+
+/** @brief  Operator
+  *
+  * @param f_inputVector        reference to input vector
+  * @return                         the output data from the system model (output vector)
+  */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+CMeasurementType<T,NC> filter::lti::mimo::CSSModel<T,NA,NB,NC>::operator()(const CInputType& f_inputVector)
+{
+    updateState(f_inputVector);
+
+    return getOutput(f_inputVector);
+}
+
+/** @brief  Update state method
+  *
+  * @param f_inputVector        reference to input vector
+  * @return                         None
+  */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+void filter::lti::mimo::CSSModel<T,NA,NB,NC>::updateState(const CInputType& f_inputVector)
+{
+    m_stateVector = m_stateTransitionMatrix * m_stateVector + m_inputMatrix * f_inputVector;
+}
+
+/** @brief  Get output
+  *
+  * @param f_inputVector        reference to input vector
+  * @return                         the output data from the system model (output vector)
+  */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+CMeasurementType<T,NC> filter::lti::mimo::CSSModel<T,NA,NB,NC>::getOutput(const CInputType& f_inputVector)
+{
+    return m_measurementMatrix * m_stateVector + m_directTransferMatrix * f_inputVector;
+}
+
+/******************************************************************************/
+/** @brief  CKalmanFilter Class constructor
+ *
+ *  Constructor method
+ *
+ *  @param f_stateTransitionModel
+ *  @param f_controlInput
+ *  @param f_observationModel
+ *  @param f_covarianceProcessNoise
+ *  @param f_observationNoiseCovariance
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::CKalmanFilter(
+        const CStateTransType&            f_stateTransitionModel,
+        const CControInputType&           f_controlInput,
+        const CMeasurementType&           f_observationModel,
+        const CModelCovarianceType&       f_covarianceProcessNoise,
+        const CMeasurementCovarianceType& f_observationNoiseCovariance)
+    : m_stateTransitionModel(f_stateTransitionModel)
+    , m_controlInput(f_controlInput)
+    , m_observationModel(f_observationModel)
+    , m_covarianceProcessNoise(f_covarianceProcessNoise)
+    , m_observationNoiseCovariance(f_observationNoiseCovariance)
+{
+}
+
+/** @brief  Operator
+  *
+  * @param f_input              reference to input vector
+  * @return                         Updated (a posteriori) state estimate
+  */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+linalg::CMatrix<T, NC, NA> filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::operator()(const CInputVectorType& f_input)
+{
+    predict(f_input);
+    return update();
+}
+
+/** @brief  Operator
+  *
+  * 
+  * @return     Updated (a posteriori) state estimate
+  */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+linalg::CMatrix<T, NC, NA> filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::operator()()
+{
+    predict();
+    return update();
+}
+
+/** @brief  Predict
+  *
+  * 
+  * 
+  */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+void filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::predict()
+{
+    m_previousState      = m_prioriState;
+    m_previousCovariance = m_prioriCovariance;
+    m_prioriState        = m_stateTransitionModel * m_previousState;
+    m_prioriCovariance   = m_stateTransitionModel * m_previousCovariance * transpose(m_stateTransitionModel) + m_covarianceProcessNoise;
+}
+
+/** @brief  Predict
+  *
+  * @param f_input    vector input
+  * 
+  */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+void filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::predict(const CInputVectorType& f_input)
+{
+    m_previousState      = m_prioriState;
+    m_previousCovariance = m_prioriCovariance;
+    m_prioriState        = m_stateTransitionModel * m_previousState + m_controlInput * f_input;
+    m_prioriCovariance   = m_stateTransitionModel * m_previousCovariance * transpose(m_stateTransitionModel) + m_covarianceProcessNoise;
+}
+
+/** @brief  Update
+  *
+  * 
+  * @return    Measurement residual
+  */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+const linalg::CMatrix<T, NC, NA>& filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::update(void)
+{
+    m_measurementResidual  = m_measurement - m_observationModel * m_prioriState;
+    m_measurement          = m_observationModel * m_posterioriState;
+    m_residualCovariance   = m_observationModel * m_prioriCovariance * transpose(m_observationModel) + m_observationNoiseCovariance;
+    m_kalmanGain           = m_prioriCovariance * transpose(m_observationModel) * m_residualCovariance.inv();
+    m_posterioriState      = m_prioriState + m_kalmanGain * m_measurementResidual;
+    m_posterioriCovariance = ( CStateTransType::eye() - m_kalmanGain * m_observationModel ) * m_prioriCovariance;
+    return m_posterioriState;
+}
+
+/******************************************************************************/
+/** @brief  CEKF Class constructor
+ *
+ *  Constructor method
+ *
+ *  @param f_systemModel
+ *  @param f_jbMatrices
+ *  @param f_Q
+ *  @param f_R
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+filter::ltv::mimo::CEKF<T,NA,NB,NC>::CEKF(
+        CSystemModelType&                           f_systemModel
+        ,CJacobianMatricesType&                     f_jbMatrices
+        ,const CJMTransitionType&                   f_Q
+        ,const CObservationNoiseType&               f_R)
+    :m_systemModel(f_systemModel)
+    ,m_jbMatrices(f_jbMatrices)
+    ,m_covarianceMatrix(linalg::CMatrix<T,NB,NB>::ones())
+    ,m_Q(f_Q)
+    ,m_R(f_R)
+{
+}
+
+/** @brief  Predict
+  *
+  * @param f_input    vector input
+  * 
+  */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+void filter::ltv::mimo::CEKF<T,NA,NB,NC>::predict(const CInputType&   f_input)
+{
+    //Previous updated state
+    CStatesType l_prev_states=m_systemModel.getStates();
+    CJMTransitionType l_JF=m_jbMatrices.getJMTransition(l_prev_states,f_input);
+    //Predicted state estimate X_{k|k-1}
+    CStatesType l_pred_states=m_systemModel.update(f_input);
+    //Predicted covariance estimate
+    m_covarianceMatrix=l_JF*m_covarianceMatrix*l_JF.transpose()+m_Q;
+}
+
+/** @brief  Update
+  *
+  * @param f_input          vector input
+  * @param f_measurement    vector input
+  * 
+  */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+void filter::ltv::mimo::CEKF<T,NA,NB,NC>::update(const CInputType&   f_input
+                                                ,const COutputType&  f_measurement)
+{
+    // Estimated system output
+    COutputType l_y_est=m_systemModel.calculateOutput(f_input);
+    // Innovation or measurement residual
+    COutputType l_mes_res=f_measurement-l_y_est;
+    // Innovation (or residual) covariance
+    CStatesType l_states=m_systemModel.getStates();
+    CJMObservationType l_JH=m_jbMatrices.getJMObservation(f_input,l_states);
+    CObservationNoiseType l_s=l_JH*m_covarianceMatrix*l_JH.transpose()+m_R;
+    //Near-optimal Kalman gain
+    CKalmanGainType l_K=m_covarianceMatrix*l_JH.transpose()*l_s.inv();
+    //Updated state estimate
+    CStatesType l_updated_states=l_states+l_K*l_mes_res;
+    m_systemModel.setStates(l_updated_states);
+    //Updated covariance estimate
+    m_covarianceMatrix=(CJMTransitionType::eye()-l_K*l_JH)*m_covarianceMatrix;
+}
+
+/******************************************************************************/
+/** @brief  CMedianFilter Class constructor
+ *
+ *  Constructor method
+ *
+ *  
+ */
+template <class T, uint32_t N>
+filter::nonlinear::siso::CMedianFilter<T,N>::CMedianFilter()
+    :m_median()
+    ,m_smallest()
+    ,m_new(0)
+    ,m_size(N)
+    ,m_queue()
+{
+    // for (unsigned int l_idx = 0; l_idx < N; l_idx++)
+    // {
+    //     m_queue[l_idx] = new structura;
+        
+    // }
+
+    for (unsigned int l_idx = 0; l_idx < N ; l_idx++)
+    {
+        m_queue[l_idx].info = 0;
+        m_queue[l_idx].next = &(m_queue[(l_idx + 1) % N]);
+        m_queue[l_idx].prev = &(m_queue[(N + l_idx - 1) % N]);
+    }
+
+    m_new = N - 1;
+    m_size = N;
+    m_median = &(m_queue[m_size / 2]);
+    m_smallest =&(m_queue[0]);
+}
+
+/** @brief  Operator
+  *
+  * @param f_val            the input data
+  * @return                     filted the data 
+  */
+template <class T, uint32_t N>
+T filter::nonlinear::siso::CMedianFilter<T,N>::operator()(T& f_val)
+{
+    m_new = (m_new + 1) % m_size;		 //shift new index	 //inca e valoarea veche
+    /* // varianta pentru a decide valoarea mediana eficient-----eficient daca filtrul are dimensiuni mari
+    //	->V2 start	  remy_structurere          EXISTA CAUZE CARE NU SUNT TRATATE CORECT SAU NETRATATE COMPLET!!!!!!!!!!!
+    if ((m_queue[m_new]->info > m_median->info) && (f_val <= m_median->info))
+    {
+        if (f_val > m_median->prev->info)
+        {
+            m_median = m_queue[m_new];		   //med=new
+        }
+        else
+        {
+            m_median = m_median->prev;		   // <-
+        }
+    }
+    else if ((m_queue[m_new]->info < m_median->info) && (f_val > m_median->info))
+    {
+        if (f_val > m_median->next->info)
+        {
+            m_median = m_median->next;		  // ->
+        }
+        else
+        {   
+            m_median = m_queue[m_new];		  //med=new
+        }
+    }
+    else if ((m_queue[m_new]->info == m_median->info))
+    {
+        if ((f_val < m_median->info)&&(f_val <= m_median->prev->info))
+        {
+            m_median = m_median->prev;		   // <-
+        }
+        else if (f_val > m_median->info)
+            {
+                if (f_val <= m_median->next->info)
+                {
+                    m_median = m_queue[m_new];		  //med=new
+                }
+                else
+                {
+                    m_median = m_median->next;		  // ->
+                }
+            }
+            else
+            {
+                m_median = m_queue[m_new];		  //med=new
+            }
+    }
+    */
+    m_queue[m_new ].info = f_val;		 //suprascrie cea mai veche valoare
+
+    //ordonare dupa valoare
+
+        //elementul new se "scoate" din lista
+    m_queue[m_new].prev->next = m_queue[m_new].next;			 //5.
+    m_queue[m_new].next->prev = m_queue[m_new].prev;			 //6.
+
+        //update smallest value
+    my_structure* l_i = m_smallest;
+    if (&(m_queue[m_new]) == m_smallest)
+    {
+        if (m_queue[m_new].info > m_smallest->next->info)
+        {
+            m_smallest = m_smallest->next;
+            l_i = m_smallest;
+        }
+    }
+    else if (m_queue[m_new].info <= m_smallest->info)
+    {
+        l_i = m_smallest;
+        m_smallest = &m_queue[m_new];
+    }
+
+        //cautarea locului unde trebuie sa se amplaseze noul element in lista
+    unsigned int l_cnt = 1;
+    if (&(m_queue[m_new]) == l_i)
+    {
+        l_i = l_i->next;
+    }
+
+    while ((l_i->info < m_queue[m_new].info) && (l_cnt <= m_size - 1))
+    {
+        l_i = l_i->next;
+        l_cnt++;
+    }
+
+        //inserarea elemntului new la locul potrivit
+    l_i->prev->next = &m_queue[m_new];							 //1.
+    m_queue[m_new].next = l_i;									 //2.
+    m_queue[m_new].prev = l_i->prev;							 //3.
+    l_i->prev = &m_queue[m_new];									 //4.
+
+    //varianta ineficienta pentru aflarea medianului  cand filtrul are dimensiuni mari
+    m_median=m_smallest;
+    for(uint8_t iddx=0; iddx < m_size/2; ++iddx)
+    {
+        m_median=m_median->next;
+    }
+
+    return getMedian();
+}
+
+/*  */
+/** @brief  OperGet medianator
+  *
+  * 
+  * @return    median value
+  */
+template <class T, uint32_t N>
+T filter::nonlinear::siso::CMedianFilter<T, N>::getMedian()
+{
+    T ret_val;
+    if (1 == m_size % 2)		// daca filtrul are lungime impara
+    {
+        ret_val = m_median->info;
+    }
+    else						// daca filtrul are lungime para
+    {
+        ret_val = 0.5*(m_median->info + m_median->prev->info);
+    }
+    return ret_val;
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Linalg/linalg.h	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,473 @@
+#ifndef LINALG_H
+#define LINALG_H
+
+// #include <mbed.h>
+#include <stdint.h>
+#include <array>
+
+namespace linalg
+{
+    template <class T, uint32_t M, uint32_t N>
+    class CMatrix
+    {
+    public:
+        using CThisType = CMatrix<T,M,N>;
+        using CTransposeType = CMatrix<T,N,M>;
+        using CContainerType = std::array<std::array<T,N>,M>;
+        using CDataType =T;
+
+        template <uint32_t P>
+        using CLeftMultipliableType = CMatrix<T,P,M>;
+
+        template <uint32_t P>
+        using CRightMultipliableType = CMatrix<T,N,P>;
+
+        template <uint32_t P>
+        using CLeftMultiplicationResultType = CMatrix<T,P,N>;
+
+        template <uint32_t P>
+        using CRightMultiplicationResultType = CMatrix<T,M,P>;
+
+        friend class CMatrix<T,N,M>; 
+
+        CMatrix() : m_data() {}
+        CMatrix(const CThisType& f_matrix) : m_data(f_matrix.m_data) {}
+        CMatrix(const CThisType&& f_matrix) : m_data(f_matrix.m_data) {}
+        CMatrix(const CContainerType& f_data) : m_data(f_data) {}
+        CMatrix(const CContainerType&& f_data) : m_data(f_data) {}
+
+        CThisType& operator=(const CThisType& f_matrix)
+        {
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    this->m_data[l_row][l_col] = f_matrix.m_data[l_row][l_col];
+                }
+            }
+            return *this;
+        }
+        CThisType& operator=(const CThisType&& f_matrix)
+        {
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    this->m_data[l_row][l_col] = f_matrix.m_data[l_row][l_col];
+                }
+            }
+            return *this;
+        }
+
+        std::array<T,N>& operator[](uint32_t f_row)
+        {
+            return m_data[f_row];
+        }
+
+        CDataType& operator()(uint32_t f_row, uint32_t f_col)
+        {
+            return m_data[f_row][f_col];
+        }
+
+        const std::array<T,N>& operator[](uint32_t f_row) const
+        {
+            return m_data[f_row];
+        }
+
+        const CDataType& operator()(uint32_t f_row, uint32_t f_col) const
+        {
+            return m_data[f_row][f_col];
+        }
+
+        // template<>
+        // CMatrix<T,1,1>::operator CDataType()
+        // {
+        //     return m_data[0][0];
+        // }
+
+        CThisType& operator+() {return *this;}
+        CThisType operator-()
+        {
+            CThisType l_matrix;
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_matrix.m_data[l_row][l_col] = -this->m_data[l_row][l_col];
+                }
+            }
+            return l_matrix;
+        }
+        CThisType operator+(const CThisType& f_matrix)
+        {
+            CThisType l_matrix;
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] + f_matrix.m_data[l_row][l_col];
+                }
+            }
+            return l_matrix;
+        }
+        CThisType operator-(const CThisType& f_matrix)
+        {
+            CThisType l_matrix;
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] - f_matrix.m_data[l_row][l_col];
+                }
+            }
+            return l_matrix;
+        }
+        CThisType& operator+=(const CThisType& f_matrix)
+        {
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    this->m_data[l_row][l_col] += f_matrix.m_data[l_row][l_col];
+                }
+            }
+            return *this;
+        }
+        CThisType& operator-=(const CThisType& f_matrix)
+        {
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    this->m_data[l_row][l_col] -= f_matrix.m_data[l_row][l_col];
+                }
+            }
+            return *this;
+        }
+
+        CThisType operator+(const CDataType& f_val)
+        {
+            CThisType l_matrix;
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] + f_val;
+                }
+            }
+            return l_matrix;
+        }
+        CThisType operator-(const CDataType& f_val)
+        {
+            CThisType l_matrix;
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] - f_val;
+                }
+            }
+            return l_matrix;
+        }
+        CThisType& operator+=(const CDataType& f_val)
+        {
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    this->m_data[l_row][l_col] += f_val;
+                }
+            }
+            return *this;
+        }
+        CThisType& operator-=(const CDataType& f_val)
+        {
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    this->m_data[l_row][l_col] -= f_val;
+                }
+            }
+            return *this;
+        }
+        CThisType& operator*=(const CDataType& f_val)
+        {
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    this->m_data[l_row][l_col] *= f_val;
+                }
+            }
+            return *this;
+        }
+        CThisType& operator*=(const CThisType& f_val)
+        {
+            CThisType& l_thisRef(*this);
+            l_thisRef = l_thisRef * f_val;
+            return l_thisRef;
+        }
+        CThisType& operator/=(const CDataType& f_val)
+        {
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    this->m_data[l_row][l_col] /= f_val;
+                }
+            }
+            return *this;
+        }
+        CThisType operator*(const CDataType& f_val)
+        {
+            CThisType l_matrix;
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] * f_val;
+                }
+            }
+            return l_matrix;
+        }
+        template <uint32_t P>
+        CRightMultiplicationResultType<P> operator*(const CRightMultipliableType<P>& f_matrix)
+        {
+            CRightMultiplicationResultType<P> l_matrix;
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < P; ++l_col)
+                {
+                    l_matrix[l_row][l_col] = 0;
+                    for (uint32_t l_idx = 0; l_idx < N; ++l_idx)
+                    {
+                        l_matrix[l_row][l_col] += this->m_data[l_row][l_idx] * f_matrix[l_idx][l_col];
+                    }
+                }
+            }
+            return l_matrix;
+        }
+        CThisType inv();
+
+        CTransposeType transpose()
+        {
+            CTransposeType l_trsp;
+
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_trsp.m_data[l_col][l_row] = this->m_data[l_row][l_col];
+                }
+            }
+
+            return l_trsp;
+        }
+
+        template <uint32_t P>
+        CRightMultiplicationResultType<P> solve(const CRightMultipliableType<P>& f_B)
+        {
+            return CLUDecomposition(*this).solve();
+        }
+
+        static CThisType zeros()
+        {
+            CThisType l_res;
+
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_res[l_row][l_col] = 0;
+                }
+            }
+
+            return l_res;
+        }
+
+        static CThisType ones()
+        {
+            CThisType l_res;
+
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_res[l_row][l_col] = 1;
+                }
+            }
+
+            return l_res;
+        }
+
+        static CThisType eye()
+        {
+            CThisType l_res(CThisType::zeros());
+            uint32_t l_minDim = std::min(M,N);
+
+            for (uint32_t l_row = 0; l_row < l_minDim; ++l_row)
+            {
+                l_res[l_row][l_row] = 1;
+            }
+
+            return l_res;
+        }
+
+    protected:
+        std::array<std::array<T,N>,M> m_data;
+    };
+
+    template<class T, uint32_t N>
+    class CLUDecomposition
+    {
+    public:
+        using CThisType = CLUDecomposition<T,N>;
+        using COriginalType = CMatrix<T,N,N>;
+        using CDataType =T;
+
+        template <uint32_t P>
+        using CLeftMultipliableType = CMatrix<T,P,N>;
+
+        template <uint32_t P>
+        using CRightMultipliableType = CMatrix<T,N,P>;
+
+        template <uint32_t P>
+        using CLeftMultiplicationResultType = CMatrix<T,P,N>;
+
+        template <uint32_t P>
+        using CRightMultiplicationResultType = CMatrix<T,N,P>;
+
+        CLUDecomposition(const CThisType& f_decomposition) : m_L(f_decomposition.m_L), m_U(f_decomposition.m_U), m_P(f_decomposition.m_P) {}
+        CLUDecomposition(const CThisType&& f_decomposition) : m_L(f_decomposition.m_L), m_U(f_decomposition.m_U), m_P(f_decomposition.m_P) {}
+        CLUDecomposition(const COriginalType& f_matrix) : m_L(), m_U(), m_P() {decompose(f_matrix);}
+        CLUDecomposition(const COriginalType&& f_matrix) : m_L(), m_U(), m_P() {decompose(f_matrix);}
+
+        operator COriginalType()
+        {
+            return m_L * m_U;
+        }
+
+        COriginalType inv()
+        {
+            return triUInv() * triLInv();
+        }
+
+        COriginalType triLInv()
+        {
+            COriginalType l_invL(m_L);
+
+            for (uint32_t l_jdx = 0; l_jdx < N; ++l_jdx)
+            {
+                l_invL[l_jdx][l_jdx] = 1/l_invL[l_jdx][l_jdx];
+
+                for (uint32_t l_idx = l_jdx+1; l_idx < N; ++l_idx)
+                {
+                    l_invL[l_idx][l_jdx] = 0;
+
+                    for (uint32_t l_kdx = l_jdx; l_kdx < l_idx; ++l_kdx)
+                    {
+                        l_invL[l_idx][l_jdx] -= m_L[l_idx][l_kdx]*l_invL[l_kdx][l_jdx];
+                    }
+
+                    l_invL[l_idx][l_jdx] /= l_invL[l_idx][l_idx];
+                }
+            }
+
+            return l_invL;
+        }
+
+        COriginalType triUInv()
+        {
+            COriginalType l_invU(m_U);
+
+            for (int32_t l_jdx = (N-1); l_jdx >= 0; --l_jdx)
+            {
+                l_invU[l_jdx][l_jdx] = 1/l_invU[l_jdx][l_jdx];
+
+                for (int32_t l_idx = (l_jdx-1); l_idx >= 0; --l_idx)
+                {
+                    l_invU[l_idx][l_jdx] = 0;
+
+                    for (int32_t l_kdx = (l_idx+1); l_kdx < (l_jdx+1); ++l_kdx)
+                    {
+                        l_invU[l_idx][l_jdx] -= m_U[l_idx][l_kdx]*l_invU[l_kdx][l_jdx];
+                    }
+
+                    l_invU[l_idx][l_jdx] /= l_invU[l_idx][l_idx];
+                }
+            }
+
+            return l_invU;
+        }
+
+        template <uint32_t P>
+        CRightMultiplicationResultType<P> solve(const CRightMultipliableType<P>& f_B)
+        {
+            CRightMultipliableType<P> l_B;
+
+            for (uint32_t l_idx = 0; l_idx < N; ++l_idx)
+            {
+                for (uint32_t l_jdx = 0; l_jdx < P; ++l_jdx)
+                {
+                    l_B[l_idx][l_jdx] = f_B[m_P[l_idx]][l_jdx];
+                }
+            }
+
+            return sTriL(sTriU(l_B));
+        }
+
+        template <uint32_t P>
+        CRightMultiplicationResultType<P> sTriU(const CRightMultipliableType<P>& f_B)
+        {
+            return triUInv()*f_B;
+        }
+
+        template <uint32_t P>
+        CRightMultiplicationResultType<P> sTriL(const CRightMultipliableType<P>& f_B)
+        {
+            // return triLInv()*f_B;
+        }
+
+
+    // private:
+        void decompose(const CMatrix<T,N,N>& f_matrix)
+                {
+            m_U = f_matrix;
+            for (uint32_t l_kdx = 0; l_kdx < (N-1); ++l_kdx)
+            {
+                m_L[l_kdx][l_kdx] = 1;
+                m_P[l_kdx] = l_kdx;
+
+                for(uint32_t l_idx = l_kdx+1; l_idx < N; ++l_idx)
+                {
+                    m_L[l_idx][l_kdx] = m_U[l_idx][l_kdx]/m_U[l_kdx][l_kdx];
+                    m_U[l_idx][l_kdx] = 0;
+                    for(uint32_t l_jdx = l_kdx+1; l_jdx < N; ++l_jdx)
+                    {
+                        m_U[l_idx][l_jdx] = m_U[l_idx][l_jdx] - m_L[l_idx][l_kdx]*m_U[l_kdx][l_jdx];
+                    }
+                }
+            }
+            m_L[N-1][N-1] = 1;
+            m_P[N-1] = N-1;
+        }
+        CMatrix<T,N,N> m_L;
+        CMatrix<T,N,N> m_U;
+        std::array<uint32_t,N> m_P;
+    };
+
+    template <class T, uint32_t N>
+    using CColVector = CMatrix<T,N,1>;
+
+    template <class T, uint32_t N>
+    using CVector = CColVector<T,N>;
+
+    template <class T, uint32_t N>
+    using CRowVector = CMatrix<T,1,N>;
+};
+
+#include "linalg.inl"
+
+#endif // LINALG_H
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Linalg/linalg.inl	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,6 @@
+template<class T, uint32_t N,uint32_t M>
+typename linalg::CMatrix<T,N,M>::CThisType linalg::CMatrix<T,N,M>::inv()
+{
+    CThisType l_inv(linalg::CLUDecomposition<T,N>(*this).inv());
+    return l_inv;
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/MotionController/motioncontroller.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,136 @@
+/**
+  ******************************************************************************
+  * @file    MotionController.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class declaration for the motion controller
+  *          functionality.
+  ******************************************************************************
+ */
+
+#ifndef MOTION_CONTROLLER_HPP
+#define MOTION_CONTROLLER_HPP
+
+/* The mbed library */
+#include <mbed.h>
+#include <TaskManager/taskmanager.hpp>
+#include <Move/move.hpp>
+#include <BezierMotionPlanner/bezierMotionplanner.hpp>
+#include <Controllers/controller.hpp>
+#include <SafetyStop/safetystopfunction.hpp>
+
+#include <rtos.h>
+
+/**
+ * @brief Limit of the pwm command [percentage]
+ * 
+ */
+#define MCTL_PWM_COMMAND_LIMIT 75
+/**
+ * @brief Limit of the velocity command [Centimeter per second]
+ * 
+ */
+#define MCTL_SPEED_COMMAND_LIMIT 100
+
+//!  CMotionController class. 
+/*!
+ *  It inherits class task::CTask. 
+ *  It is used for executing move commands.
+ * */
+class CMotionController 
+{
+public:
+    CMotionController(
+        float f_period_sec,
+        Serial& f_serialPort, 
+        Move& f_car,
+        CSafetyStopFunction* f_safetyStop,
+        controllers::CControllerSiso* f_control = NULL);
+    CMotionController(
+        float f_period_sec, 
+        Serial& f_serialPort, 
+        Move& f_car,
+        controllers::CControllerSiso*           f_control = NULL);
+    
+
+    /* Serial callback method for Move command */
+    static void staticSerialCallbackMove(void* obj,char const * a, char * b);
+    /* Serial callback method for BRAKE command */
+    static void staticSerialCallbackBrake(void* obj,char const * a, char * b);
+    /* Serial callback method for hard BRAKE command */
+    static void staticSerialCallbackHardBrake(void* obj,char const * a, char * b) ;
+    /* Serial callback method for PID activation command */
+    static void staticSerialCallbackPID(void* obj,char const * a, char * b);
+    /* Static serial callback for spline command */
+    static void staticSerialCallbackSpline(void* obj,char const * a,char * b );
+    /* Static callback function for run method */
+    static void staticCallbackRun(void* obj);
+    /* Start the Rtos timer for applying run */
+    void startRtosTimer();
+
+    /* Reset method */
+    void reset();
+    /* Get speed method */
+    float getSpeed();
+    /* Get angle method */
+    float getAngle();
+    /* BRAKE callback method */
+    void BrakeCallback();
+    /* Set state method */
+    void setState(int f_state);
+private:
+    /* Run method */
+    virtual void _run();
+    /* Serial callback implementation */ 
+    void serialCallbackMove(char const * a, char * b);
+    /* BRAKE serial callback */
+    void serialCallbackBrake(char const * a, char * b);
+    /* Hard BRAKE serial callback */
+    void serialCallbackHardBrake(char const * a, char * b);
+    /* PID serial callback */
+    void serialCallbackPID(char const * a, char * b);
+    /* Spline serial callback */
+    void serialCallbackSpline(char const * a, char * b);
+    
+    
+    /* Static function to convert from linear velocity ( centimeter per second ) of robot to angular velocity ( rotation per second ) of motor */
+    static float Mps2Rps(float f_vel_cmps);
+    // /* Velocity to PWM function */
+    // static float VEL2PWM(float vel);
+
+private:
+    /* reference to Serial object */
+    Serial& m_serialPort;
+    /* reference to MOVE object */
+    Move& m_car;
+    /* Speed */
+    float m_speed;
+    /* Angle */
+    float m_angle;
+    /* PEriod i nseconds */
+    float   m_period_sec;
+    /* Spline activation state */
+    bool    m_isSplineActivated;
+    /* State machine state */
+    uint8_t m_state;
+    /* PID activation state */
+    bool    m_ispidActivated;
+    // 0-none
+    // 1-normal
+    // 2-brake regeneration
+    
+    /* motion planner object */
+    planner::CBezierMotionPlanner           m_motionPlanner;
+    /* Timeout */
+    Timeout                                 m_hbTimeOut;
+    /* Reference to control object */
+    controllers::CControllerSiso*           m_control;
+    /* Reference to safety stop function */
+    CSafetyStopFunction*                    m_safetyStop;
+    /* Rtos  timer for periodically applying */
+    RtosTimer                               m_timer;
+};
+
+#endif // MOTION_CONTROLLER_H 
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Move/move.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,53 @@
+/**
+  ******************************************************************************
+  * @file    MOVE.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class declaration for the moving
+  *          functionality.
+  ******************************************************************************
+ */
+
+/* Include guard */
+#ifndef MOVE_HPP
+#define MOVE_HPP
+
+#include <Drivers/servo.hpp>
+#include <Drivers/vnh.hpp>
+
+/**
+ * @brief  Move class
+ * It is used for executing move commands.
+ * 
+ */
+class Move
+{
+public:
+    /* Constructor */
+    Move(PinName, PinName, PinName, PinName, PinName);//A0
+    /* Destructor */
+    ~Move();
+    /* Steer */
+    void Steer(float angle);// -25 to + 25 degrees, - (left), + (right)
+    /* Speed */
+    void Speed(float speed);//-100 to + 100 -(back), + (front)   
+    /* Brake */ 
+    void Brake();
+    /* Inverse */
+    void Inverse(float f_speed);
+    /* Test */
+    void TestCar();
+    /* Reset */
+    void ResetCar();
+    /* Return bridge object */
+    drivers::VNH& getVNH();
+            
+private:
+    /* Servo pbject */
+    drivers::SERVO       servo;
+    /* Bridge object */
+    drivers::VNH         vnh;
+};
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Queue/queue.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,61 @@
+/**
+  ******************************************************************************
+  * @file    Queue.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class declaration for the queue
+  *          functionality.
+  ******************************************************************************
+ */
+
+/* Include guard */
+#ifndef QUEUE_HPP
+#define QUEUE_HPP
+
+
+/**
+ * @brief It is used for executing queue container.
+ * 
+ * @tparam T The type of the varieble
+ * @tparam N The size of the queue
+ */
+template <class T, unsigned int N>
+class CQueue
+{
+public:
+    /* Constructor */
+    CQueue();
+    /* Destructor */
+    virtual ~CQueue();
+    /* Is full method */
+    inline bool isFull();
+    /* Is full method */
+    inline bool isEmpty();
+    /* Peek */
+    inline T peek();
+    /* Pop */
+    inline T pop();
+    /* Get queue size */
+    inline unsigned int getSize();
+    /* Push single element */
+    inline void push(T& f_char);
+    /* Push multiple elements */
+    inline void push(T *f_char, unsigned int f_len);
+    /* Empty queue */ 
+    inline void empty();
+private:
+    /* buffer */
+    volatile T m_buffer[N];
+    /* start */
+    volatile unsigned int  m_start;
+    /* end */
+    volatile unsigned int  m_end;
+    /* size */
+    volatile unsigned int  m_size;
+};
+
+#include "queue.inl"
+
+#endif
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Queue/queue.inl	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,153 @@
+/**
+  ******************************************************************************
+  * @file    Queue.inl
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class definition for the queue
+  *          functionality. Inlin eimplementation.
+  ******************************************************************************
+ */
+
+/** @brief  Queue Class constructor
+ *
+ *  Constructor method
+ *
+ *  
+ */
+template <class T, unsigned int N>
+CQueue<T,N>::CQueue()
+    : m_buffer()
+    , m_start(0)
+    , m_end(N-1)
+    , m_size(0)
+{
+}
+
+/** @brief  Queue Class destructor
+ *
+ *  Destructor method
+ *
+ *  
+ */
+template <class T, unsigned int N>
+CQueue<T,N>::~CQueue()
+{
+}
+
+/** @brief  Is full method
+ *
+ *  
+ *  @return    True if Queue FULL
+ */
+template <class T, unsigned int N>
+bool CQueue<T,N>::isFull()
+{
+    return (m_start + 1)%N == m_end;
+}
+
+/** @brief  Is empty method
+ *
+ *  
+ *  @return    True if Queue EMPTY
+ */
+template <class T, unsigned int N>
+bool CQueue<T,N>::isEmpty()
+{
+    return (m_end + 1)%N == m_start;
+}
+
+/** @brief  Peek method
+ *
+ *  
+ *  @return    Value on top of queue
+ */
+template <class T, unsigned int N>
+T CQueue<T,N>::peek()
+{
+    return m_buffer[(m_end+1) % N];
+}
+
+/** @brief  Pop method
+ * 
+ *  Method for removing the item on top of the queue and returning it
+ *
+ *  
+ *  @return    Element at top of the queue
+ */
+template <class T, unsigned int N>
+T CQueue<T,N>::pop()
+{
+    if(!isEmpty())
+    {
+        m_end = (m_end+1) % N;
+        T l_char = m_buffer[m_end];
+        m_size--;
+        return l_char;
+    }
+    else
+    {
+        return 0;
+    }
+}
+
+/** @brief  Push method
+ * 
+ *  Method for inserting an item on top of the queue
+ *
+ *  @param f_char    element to be added to the queue
+ *  @return    None
+ */
+template <class T, unsigned int N>
+void CQueue<T,N>::push(T& f_char)
+{
+    if (!isFull())
+    {
+        m_buffer[m_start] = f_char;
+        m_start = (m_start+1) % N; 
+        m_size++;           
+    }
+}
+
+/** @brief  Push method
+ * 
+ *  Method for inserting a sequence of items on top of the queue
+ *
+ *  @param f_char    pointer to element to be added to the queue
+ *  @param f_len     number of elements to be added
+ *  @return    None
+ */
+template <class T, unsigned int N>
+inline void CQueue<T,N>::push(T *f_char, unsigned int f_len)
+{
+    for ( unsigned int l_idx = 0; l_idx < f_len; ++l_idx)
+    {
+        push(f_char[l_idx]);
+    }
+}
+
+/** @brief  Get size method
+ * 
+ *  Returns the size of the queue
+ *
+ *  
+ *  @return    Queue size
+ */
+template <class T, unsigned int N>
+unsigned int CQueue<T,N>::getSize()
+{
+    return m_size;
+}
+
+/** @brief  Empty queue method
+ *
+ *  
+ *  @return    None
+ */
+template <class T, unsigned int N>
+inline void CQueue<T,N>::empty()
+{
+    m_start = 0;
+    m_end = N-1;
+    m_size = 0;
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/SHARP_IR_distance/DistanceSensors/distancesensors.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,59 @@
+/**
+  ******************************************************************************
+  * @file    DistanceSensors.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    08-January-2018
+  * @brief   This file contains the class definition for the distance sensors read
+  *          methods.
+  ******************************************************************************
+ */
+
+/* Include guard */
+#ifndef DISTANCESENSORS_HPP
+#define DISTANCESENSORS_HPP
+
+#include <mbed.h>
+#include <array>
+#include <string>
+#include<TaskManager/taskmanager.hpp>
+#include <SHARP_IR_distance\DistanceSensors\distancesensors.hpp>
+using namespace std;
+
+/**
+ * @brief It is used for reading the attached distance sensors.
+ * 
+ */
+template<class T>
+class CDistanceSensors: public task::CTask
+{
+private:
+    /* The sensors list */
+    T                               m_sensor_list;
+    /* Index of current sensor */
+    uint8_t                         m_index;
+    /* The maximum number of sensors that are used */
+    const uint8_t                   m_N;
+    /* Flags for enabloing/disabling timeout */
+    Timeout                         m_disableTimeOut;
+    Timeout                         m_enableTimeOut;
+    /* Message that contains the read values */
+    string                          m_message;
+
+public:
+    /* Construnctor */
+    CDistanceSensors(uint32_t f_period,
+                     float f_baseTick,
+                     T&  f_sensor_list);
+    /* Destructor */
+    ~CDistanceSensors();
+    /* Callback for reading sensor value */
+    void ReadCallback(void);
+    /* Run method */
+    void _run(void);
+};
+
+#include "DistanceSensors.inl"
+
+/* Include guard */
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/SHARP_IR_distance/DistanceSensors/distancesensors.inl	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,79 @@
+/** @brief  Class constructor
+
+
+    @param f_period       sensor reading period
+    @param f_baseTick     base tick period, in seconds
+    @param f_sensor_list  the list of sensor objects
+ */
+template<class T >
+CDistanceSensors<T>::CDistanceSensors(const  uint32_t f_period,
+                            const float f_baseTick,
+                            T& f_sensor_list)
+    : task::CTask(f_period)
+    , m_sensor_list(f_sensor_list)
+    , m_index(0)
+    , m_N(f_sensor_list.size())
+    , m_disableTimeOut()
+    , m_enableTimeOut()
+    , m_message("@IRDS:")
+    // , m_buf("")
+{
+    //static_assert(f_sensor_list.size() * 0.03 < f_period*f_baseTick,
+        //"IR sensor reding period is too small! It must be grater than: number of sensors in \"conflict\" *0.3seconds!");
+}
+
+//static_assert(m_N * 0.03 < f_period*f_baseTick,
+//        "IR sensor reding period is too small! It must be grater than: number of sensors in \"conflict\" *0.3seconds!");
+
+/** @brief  Class destructor
+
+    Destructor method
+ */
+template<class T >
+CDistanceSensors<T>::~CDistanceSensors(){}
+
+/** @brief  Read callback method
+
+    Callback for reading the value of one distance sensor
+
+ */
+template<class  T >
+void CDistanceSensors<T>::ReadCallback(){
+    // // buffer for holding message data
+    // char buf [10];
+    // read distance form sensor at position m_index in the sensor list
+    m_sensor_list[m_index]->ReadDistance();
+    // disable sensor at position m_index in the sensor list
+    m_sensor_list[m_index]->disable();
+    //sprintf(buf,"-%d-%3.2f;",m_index, m_sensor_list[m_index]->getValue());/// cu m_index
+    // format value so it can be appended to message
+    // sprintf(buf,"%3.2f;", m_sensor_list[m_index]->getValue()); // standard formatting
+    // // append value from sensor to message
+    // m_message.append(buf);
+    // if the last sensor is read, send message on UART and exit method
+    if(m_index == m_N-1){
+        // m_message=m_message+";\r\n";
+        // m_serial.printf(m_message.c_str());
+        // m_message="@IRDS:";
+        return;
+    }
+    // go to next sensor
+    m_index = m_index+1;
+    // enable next sensor
+    m_sensor_list[m_index]->enable();
+    // disable sensor timeout, read next sensor after 0.03 seconds
+    m_disableTimeOut.attach(callback(this, &CDistanceSensors<T>::ReadCallback), 0.03);
+}
+
+/** @brief  Method called each f_period
+ * 
+ */
+template<class T >
+void CDistanceSensors<T>::_run(){
+    // start from first sensor
+    m_index=0;
+    // enable first sensor
+    m_sensor_list[m_index]->enable();
+    // read sensor after 0.03 seconds
+    m_disableTimeOut.attach(callback(this, &CDistanceSensors<T>::ReadCallback), 0.03);
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/SHARP_IR_distance/IR_DistanceSensorDriver/ir_distancesensordriver.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,85 @@
+/**
+  ******************************************************************************
+  * @file    IR_DistanceSensorDriver.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    08-January-2018
+  * @brief   This file contains the class definition for the SHARP IR sensor read
+  *          methods (driver).
+  ******************************************************************************
+ */
+
+/* Include guard */
+#ifndef IR_DISTANCESENSORDRIVER_HPP
+#define IR_DISTANCESENSORDRIVER_HPP
+
+#include <mbed.h>
+#include <math.h>
+
+/* Module defines */
+#define C_IR_DistanceSensorDriver_A -19057.99103709
+#define C_IR_DistanceSensorDriver_B 42562.72078701
+#define C_IR_DistanceSensorDriver_C -37447.6482552
+#define C_IR_DistanceSensorDriver_D 16482.62217927
+#define C_IR_DistanceSensorDriver_E -3758.54454044
+#define C_IR_DistanceSensorDriver_F 395.72452643
+
+/* //scaling values, variant 1
+a=733.06912872;
+b=-2221.23062859;
+c=2502.20535578;
+d=-1273.63067295;
+e=269.64209866;
+*/
+
+/*
+//scaling values, variant 2, used for obtaining more accurate values 
+a=-6144.18274059;
+b=19775.78355498;
+c=-25056.00451678;
+d=15739.05258272;
+e=-4986.65539646;
+f=673.27540402;
+*/
+/*scaling values, variant 3 (media benzii)
+a=-19057.99103709;
+b=42562.72078701;
+c=-37447.6482552;
+d=16482.62217927;
+e=-3758.54454044;
+f=395.72452643;
+*/
+
+/**
+ * @brief It is used for reading SHARP IR distance sensors.
+ * 
+ */
+class C_IR_DistanceSensorDriver
+{
+public:
+    /* Construnctor */
+    C_IR_DistanceSensorDriver(AnalogIn ain_pin, DigitalOut en_pin);
+    /* Destrunctor */
+    ~C_IR_DistanceSensorDriver();
+    /* Method for reading distance */
+    float ReadDistance(void);
+    /* Method for enabling sensor */
+    void enable(void);
+    /* Method for disabling sensor */
+    void disable(void);
+    //void setValue(float);
+    /* Method for obtaining read value */ 
+    float getValue(void);
+
+private:
+
+    /* the analog pin, giving the proportional voltage value */
+    AnalogIn ain_pin;
+    /* enable pin */
+    DigitalOut en_pin;
+    /* distance value */
+    float value;
+};
+
+/* Include guard */
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/SHARP_IR_distance/drivertask.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,53 @@
+#ifndef DRIVER_TASK_H
+#define DRIVER_TASK_H
+
+#include <TaskManager.h>
+#include <DistanceDriver.h>
+#include <filter.h>
+
+
+
+
+template<class T>
+class CDriverTask: public task::CTask
+{
+    public:
+        using FilterFunc=filter::CFilterFunction<T>;
+        CDriverTask( uint32_t f_period
+                    ,PinName f_pin
+                    ,FilterFunc& f_filter
+                    ,Serial& f_serial)
+                    :CTask(f_period)
+                    ,m_filter(f_filter)
+                    ,m_distanceDriver(f_pin)
+                    ,m_val_brut(0.0)
+                    ,m_val_filtrat(0.0)
+                    ,m_serial(f_serial)
+                    ,idx(0)
+        {
+        }
+    
+        T getDistanceFiltered(){
+            return m_val_filtrat;
+        }
+
+        T getDistanceMeasured(){
+            return m_val_brut;
+        }
+
+        
+    
+    private:
+        virtual void _run(){
+            m_val_brut=m_distanceDriver.ReadDistance();
+            m_val_filtrat=m_filter(m_val_brut);
+        }
+
+        FilterFunc&     m_filter;
+        DistanceDriver  m_distanceDriver;
+        T               m_val_brut;
+        T               m_val_filtrat;
+        Serial& m_serial;
+        int idx;
+};
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/SafetyStop/safetystopfunction.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,75 @@
+/**
+  ******************************************************************************
+  * @file    SafetyStopFunction.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class declaration for the safety stop activation
+  *          methods.
+  ******************************************************************************
+ */
+
+/* Include guard */
+#ifndef SAFETY_STOP_FUNCTION_HPP
+#define SAFETY_STOP_FUNCTION_HPP
+
+/* Required for the use of C_IR_DistanceSensorDriver */
+#include <SHARP_IR_distance\IR_DistanceSensorDriver\ir_distancesensordriver.hpp>
+
+
+/**
+ * @brief It is used for deciding whether to activate safetyu stop depending on
+ * distance sensors reading.
+ * 
+ */
+class CSafetyStopFunction{
+public:
+    /* Construnctor */
+    CSafetyStopFunction(C_IR_DistanceSensorDriver& f_senLeftFront
+                        , C_IR_DistanceSensorDriver& f_senMidLeftFront
+                        , C_IR_DistanceSensorDriver& f_senCenterFront
+                        , C_IR_DistanceSensorDriver& f_senMidRightFront
+                        , C_IR_DistanceSensorDriver& f_senRightFront
+                        // , C_IR_DistanceSensorDriver& f_senLeftBach
+                        // , C_IR_DistanceSensorDriver& f_senRightBach
+                        );
+
+    /* Destructor */
+    ~CSafetyStopFunction();
+
+    /* Function for activating safety stop */
+    bool isSafetyStopActive(float f_speed, float f_angle);
+    /* Serial callback method */
+    static void staticSerialCallback(void* obj,char const * a, char * b);
+private:
+    /* Serial callback actions */
+    void serialCallback(char const * a, char * b);
+    /* The functionality active state*/
+    bool  m_active;
+    /* Current car speed */
+    float m_speed;
+    /* Current steering angle */
+    float m_angle;
+    /* Front sensors */
+    C_IR_DistanceSensorDriver& m_senLeftFront;
+    C_IR_DistanceSensorDriver& m_senMidLeftFront;
+    C_IR_DistanceSensorDriver& m_senCenterFront;
+    C_IR_DistanceSensorDriver& m_senMidRightFront;
+    C_IR_DistanceSensorDriver& m_senRightFront;
+    /* Back sensors */
+    // C_IR_DistanceSensorDriver& m_senLeftBach;
+    // C_IR_DistanceSensorDriver& m_senRightBach;
+    /* Serial communication member */
+    /* USED FOR DEBUG */
+};
+
+/* Include guard */
+#endif
+
+
+
+
+
+
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/SerialMonitor/serialmonitor.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,70 @@
+/**
+  ******************************************************************************
+  * @file    SerialMonitor.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class declaration for the serial communication
+  *          functionality.
+  ******************************************************************************
+ */
+
+/* Inclusion guard */
+#ifndef SERIAL_MONITOR_HPP
+#define SERIAL_MONITOR_HPP
+
+/* The mbed library */
+#include <mbed.h>
+#include <map>
+#include <array>
+/* Function objects */
+#include <functional>
+#include<TaskManager/taskmanager.hpp>
+#include <Queue/queue.hpp>
+
+
+namespace serial{
+
+   /**
+    * @brief It is used for implementing serial communciation.
+    * 
+    */
+    class CSerialMonitor : public task::CTask
+    {
+    public:
+        typedef mbed::Callback<void(char const *, char *)> FCallback;
+        typedef std::map<string,FCallback> CSerialSubscriberMap;
+
+        /* Constructor */
+        CSerialMonitor(Serial& f_serialPort
+                    ,CSerialSubscriberMap f_serialSubscriberMap);
+    private:
+        /* Rx callback */
+        static void RxCallback(void *thisPointer);
+        /* Tx callback */
+        static void TxCallback(void *thisPointer);
+        /* Rx callback actions */
+        void serialRxCallback();
+        /* Tx callback actions */
+        void serialTxCallback();
+        /* Run method */
+        virtual void _run();
+
+        /* Reference to serial object */
+        Serial& m_serialPort;
+        /* Rx buffer */
+        CQueue<char,255> m_RxBuffer;
+        /* Tx buffer */
+        CQueue<char,255> m_TxBuffer;
+        /* Data buffer */
+        array<char,256> m_parseBuffer;
+        /* Parse iterator */
+        array<char,256>::iterator m_parseIt;
+        /* Serial subscriber */
+        CSerialSubscriberMap m_serialSubscriberMap;
+    };
+
+}; // namespace serial
+
+#endif // SERIAL_MONITOR_HPP
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/SystemModels/systemmodels.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,155 @@
+/**
+  ******************************************************************************
+  * @file    SystemModels/systemmodels.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class declarations for system model.
+  ******************************************************************************
+ */
+
+/* Inclusion guard */
+#ifndef SYSTEM_MODELS_HPP
+#define SYSTEM_MODELS_HPP
+
+#include <Linalg/linalg.h>
+
+// Discrete System Models
+namespace systemmodels{
+    //Linear time variant models
+    namespace lti{
+        namespace siso{
+           /**
+            * @brief Discrete transfer function
+            * 
+            * @tparam T The type of the variable 
+            * @tparam NNum The order of the polynomial
+            * @tparam NDen The order of the polynomial
+            */
+            template <class T,uint32_t NNum,uint32_t NDen>
+            class CDiscreteTransferFucntion{
+                public:
+                    using CDenType          =   linalg::CMatrix<T,NDen,1>;
+                    using CDenModType       =   linalg::CMatrix<T,NDen-1,1>;
+                    using CNumType          =   linalg::CMatrix<T,NNum,1>;
+                    // using CNumModType       =   linalg::CMatrix<T,NNum-1,1>;
+                    using CInputMem         =   linalg::CMatrix<T,1,NNum>;
+                    using COutputMem        =   linalg::CMatrix<T,1,NDen-1>;
+                    /* Constructor */
+                    CDiscreteTransferFucntion();
+
+                    CDiscreteTransferFucntion(const CNumType& f_num,const CDenType& f_den);
+
+                    /* Clear memory */
+                    void clearMemmory();
+                    /* Shift memory */
+                    template<uint32_t N>
+                    void shiftMemory(linalg::CMatrix<T,1,N>& f_mem);
+                    /* Operator */
+                    T operator()(const T& f_input);
+                    /* Set num */
+                    void setNum(const CNumType& f_num);
+                    /* Set den */
+                    void setDen(const CDenType& f_den);
+
+                    const CNumType& getNum();
+                    const CDenModType& getDen();
+                    float getDenCurrent();
+                    /* Get output */
+                    T getOutput();
+
+                private:
+                
+                    #ifdef SYSTEMMODEL_TEST_HPP
+                        FRIEND_TEST(CDiscreteTransferFucntionTest,Reset);
+                    #endif
+
+                    /* nominator type */
+                    CNumType    m_num;
+                    // CDenType       m_den;
+                    /* denominator type */
+                    CDenModType    m_den;
+                    T              m_denCoef;
+                    /* input memory */
+                    CInputMem      m_memInput;
+                    /* output memory */
+                    COutputMem     m_memOutput;
+            };
+        }
+
+    };
+    // Nonlinea time invariant models
+    namespace nlti{
+        //Multi-input and multi-output 
+        namespace mimo{
+            // Discrete time system model of a system nonlinear time invariant with multi-input and multi-output
+            //  T       -variable type
+            //  NA      -number of inputs
+            //  NB      -number of states
+            //  NC      -number of outputs
+            template <class T,uint32_t NA, uint32_t NB,uint32_t NC>
+            class CDiscreteTimeSystemModel{
+                public:
+                    using CStatesType           =   linalg::CMatrix<T,NB,1>;
+                    using CInputType            =   linalg::CMatrix<T,NA,1>;
+                    using COutputType           =   linalg::CMatrix<T,NC,1>;
+                    /* Constructor */
+                    CDiscreteTimeSystemModel(const double           f_dt);//:m_states(),m_dt(f_dt){}
+                    /* Constructor */
+                    CDiscreteTimeSystemModel(const CStatesType&     f_states
+                                            ,const double           f_dt);
+                    //State transition model
+                    //Calculate the system state depending on input, after calculation the relevant class members need to be syncronized.
+                    virtual CStatesType update(const CInputType&       f_input)=0;
+                    //State observation model
+                    //Calculate the system output depending on input, after calculation the relevant class members need to be syncronized.
+                    virtual COutputType calculateOutput(const CInputType&  f_input)=0;
+                    // GETTERS
+                    // The method returns the current system states
+                    CStatesType         getStates();//{return m_states;}
+                    // The method returns the current system output
+                    COutputType         getOutput(){return m_outputs;}
+                    float               getTimeStep(){return m_dt;}
+                    // SETTERS
+                    void                setStates(const CStatesType& f_states){
+                        m_states=f_states;
+                    }
+                protected:
+                    // States
+                    CStatesType                         m_states;
+                    // Output
+                    COutputType                         m_outputs;
+                    // Time step
+                    const double                        m_dt;
+                private:        
+            };
+
+            // the state transition and observation matrices defined by Jacobians
+            //  T       -variable type
+            //  NA      -number of inputs
+            //  NB      -number of states
+            //  NC      -number of outputs
+            template <class T,uint32_t NA, uint32_t NB,uint32_t NC>
+            class CJacobianMatrices{
+                public:
+                    using CStatesType           =   linalg::CMatrix<T,NB,1>;
+                    using CInputType            =   linalg::CMatrix<T,NA,1>;
+                    // using COutputType           =   linalg::CMatrix<T,NC,1>;
+                    using CJMTransitionType     =   linalg::CMatrix<T,NB,NB>;
+                    using CJMObservationType    =   linalg::CMatrix<T,NC,NB>;
+
+                    //The method calculates and returns  the states transition matrix 
+                    virtual CJMTransitionType getJMTransition(const CStatesType&     f_states
+                                                             ,const CInputType&      f_inputs)=0;
+                    // The method calculates and returns  the states observation matrix
+                    virtual CJMObservationType getJMObservation(const CStatesType&      f_states
+                                                               ,const CInputType&       f_input)=0;
+                private:
+            };
+        };
+    };
+};
+
+#include "systemmodels.inl"
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/SystemModels/systemmodels.inl	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,171 @@
+/******************************************************************************/
+/** \brief  CDiscreteTransferFucntion class constructor
+ *
+ *  Constructor method
+ */
+template <class T,uint32_t NNum,uint32_t NDen>
+systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::CDiscreteTransferFucntion()
+    :m_num(CNumType::zeros())
+    ,m_den(CDenModType::zeros())
+    ,m_denCoef(1)
+    ,m_memInput()
+    ,m_memOutput()
+{
+}
+
+
+template <class T,uint32_t NNum,uint32_t NDen>
+systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::CDiscreteTransferFucntion(const CNumType& f_num,const CDenType& f_den)
+    :m_num(CNumType::zeros())
+    ,m_den(CDenModType::zeros())
+    ,m_denCoef(1)
+    ,m_memInput()
+    ,m_memOutput()
+{
+    this->setNum(f_num);
+    this->setDen(f_den);
+}
+
+
+/** \brief  Clear memory
+ *
+ *  
+ *  
+ */
+template <class T,uint32_t NNum,uint32_t NDen>
+void systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::clearMemmory()
+{
+    m_memInput=CInputMem::zeros();
+    m_memOutput=COutputMem::zeros();
+}
+
+/** \brief  Clear memory
+ *
+ *  @param f_input      reference to input
+ *  @return    l_output
+ */
+template <class T,uint32_t NNum,uint32_t NDen>
+T systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::operator()(const T& f_input)
+{
+    shiftMemory<NNum>(m_memInput);
+    m_memInput[0][0]=f_input;
+    T l_output=((m_memInput*m_num-m_memOutput*m_den)[0][0])/m_denCoef;
+    shiftMemory<NDen-1>(m_memOutput);
+    m_memOutput[0][0]=l_output;
+    return l_output;
+}
+
+/** \brief  Set num 
+ *
+ *  @param f_num      reference to nominator
+ *  
+ */
+template <class T,uint32_t NNum,uint32_t NDen>
+void systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::setNum(const CNumType& f_num)
+{
+    for(uint32_t i=0;i<NNum;++i)
+    {
+        m_num[i][0]=f_num[i][0];
+    }
+}
+
+/** \brief  Set den 
+ *
+ *  @param f_den      reference to denominator
+ *  
+ */
+template <class T,uint32_t NNum,uint32_t NDen>
+void systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::setDen(const CDenType& f_den)
+{
+    for(uint32_t i=1;i<NDen;++i)
+    {
+        m_den[i-1][0]=f_den[i][0];
+    }
+    m_denCoef = f_den[0][0];
+}
+
+/** \brief  Get output 
+ *
+ *  
+ *  @return    Output memory
+ */
+template <class T,uint32_t NNum,uint32_t NDen>
+T systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::getOutput()
+{
+    return m_memOutput[0][0];
+}
+
+/** \brief  Shift memory
+ *
+ *  @param f_mem           reference to memory matrix
+ *  @return    none
+ */
+template <class T,uint32_t NNum,uint32_t NDen>
+template<uint32_t N>
+void systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::shiftMemory(linalg::CMatrix<T,1,N>& f_mem)
+{
+    for(uint32_t i=N-1;i>0;--i)
+    {
+        f_mem[0][i]=f_mem[0][i-1];
+    }
+}
+
+/******************************************************************************/
+/** \brief  CDiscreteTimeSystemModel class constructor
+ *
+ *  Constructor method
+ *
+ *  @param f_dt      Time step
+ */
+template <class T,uint32_t NA, uint32_t NB,uint32_t NC>
+systemmodels::nlti::mimo::CDiscreteTimeSystemModel<T,NA,NB,NC>::CDiscreteTimeSystemModel(const double f_dt)
+    : m_states()
+    , m_dt(f_dt)
+{
+}
+
+/** \brief  CDiscreteTimeSystemModel class constructor
+ *
+ *  Constructor method
+ *
+ *  @param f_states  States
+ *  @param f_dt      Time step
+ */
+template <class T,uint32_t NA, uint32_t NB,uint32_t NC>
+systemmodels::nlti::mimo::CDiscreteTimeSystemModel<T,NA,NB,NC>::CDiscreteTimeSystemModel(
+        const CStatesType&     f_states
+       ,const double           f_dt)
+    : m_states(f_states)
+    , m_dt(f_dt)
+{
+}
+
+
+
+/** \brief  Getter numitor 
+ *
+ *  @return    const reference to numitor
+ */
+template <class T,uint32_t NNum,uint32_t NDen>
+const typename systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::CNumType& systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::getNum(){
+    return m_num;
+}
+
+/** \brief  Getter denomitor
+ * Return the denomitor without the first coefficeint.
+ * 
+ *  @return    const reference to denomitor
+ */
+template <class T,uint32_t NNum,uint32_t NDen>
+const typename systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::CDenModType& systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::getDen(){
+    return m_den;
+}  
+
+/** \brief  Getter denomitor first coefficeint
+ *
+ *  @return    value of the denomitor first coefficeint
+ */
+template <class T,uint32_t NNum,uint32_t NDen>
+float systemmodels::lti::siso::CDiscreteTransferFucntion<T,NNum,NDen>::getDenCurrent(){
+    return m_denCoef;
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/TaskManager/taskmanager.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,97 @@
+/**
+  ******************************************************************************
+  * @file    TaskManager.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class declaration for task manager.
+  ******************************************************************************
+ */
+
+/* Inclusion guard */
+#ifndef TASK_MANAGER_HPP
+#define TASK_MANAGER_HPP
+
+#include <mbed.h>
+
+namespace task{
+   /**
+    * @brief It is used for implementing the task functionality.
+    * 
+    */
+    class CTask
+    {
+    public:
+        /* Constructor */
+        CTask(uint32_t f_period);
+        /* Destructor */
+        virtual ~CTask();
+        /* Run method */
+        virtual void run();
+        /* Timer callback */
+        void timerCallback()
+        {
+            m_ticks++;
+            if (m_ticks >= m_period)
+            {
+                m_ticks = 0;
+                Trigger();
+            }
+        }
+        /* Trigger */
+        void Trigger()
+        {
+            m_triggered = true;
+        }
+    protected:
+        /* _run methof */
+        virtual void _run() = 0;
+        /* period */
+        const uint32_t m_period;
+        /* ticks */
+        uint32_t m_ticks;
+        /* trigger flag */
+        bool m_triggered;
+    };
+
+   /**
+    * @brief It is used for implementing the task manager functionality.
+    * 
+    */
+    class CTaskManager
+    {
+    public:
+        /* Constructor */
+        CTaskManager(CTask** f_taskList, uint32_t f_taskCount, float f_baseFreq);
+        /* Destructor */
+        virtual ~CTaskManager();
+        /* Main callback */
+        inline void mainCallback()
+        {
+            for(uint32_t i = 0; i < m_taskCount; i++)
+            {
+                m_taskList[i]->run();
+            }
+        }
+        /* Timer callback */
+        inline void timerCallback()
+        {
+            for(uint32_t i = 0; i < m_taskCount; i++)
+            {
+                m_taskList[i]->timerCallback();
+            }
+        }
+    private:
+        /* Callback */
+        static void callback(void *thisPointer);
+        /* task list */
+        CTask** m_taskList;
+        /* task count */
+        uint32_t m_taskCount;
+        /* ticker */ /* A Ticker is used to call a function at a recurring interval */
+        Ticker m_ticker;
+    };
+
+}; // namespace task
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Timer/timer.hpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,58 @@
+/**
+  ******************************************************************************
+  * @file    Timer.hpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class declaration for the timer functionality.
+  ******************************************************************************
+ */
+
+/* Inclusion guard */
+#ifndef TIMER_HPP
+#define TIMER_HPP
+
+#include <mbed.h>
+
+/**
+ * @brief It is used for implementing timers.
+ * 
+ * @tparam N 
+ * @tparam 1 
+ */
+template <unsigned int N, unsigned int D = 1>
+class CTimer
+{
+public:
+    /* Constructor */
+    CTimer();
+    /* Destructor */
+    virtual ~CTimer();
+    /* Start timer */
+    inline void start();
+    /* Stop timer */
+    inline void stop() ;
+    /* Get timer */
+    inline uint32_t get();
+private:
+    /* Timer callback */
+    static void callback(void *thisPointer);
+    /* Increase milliseconds value */
+    void millisTicker();
+    /* Ticker member */
+    Ticker m_ticker;
+    /* Milliseconds value */
+    volatile uint32_t m_millisValue;
+    /* Flag indicating timer started */
+    bool m_started;
+};
+
+typedef CTimer<1,1000> CTimer_ms;
+typedef CTimer<1,1000000> CTimer_us;
+typedef CTimer<1,10000> CTimer_100us;
+typedef CTimer<60> CTimer_min;
+
+/* Inline implementation */
+#include "timer.inl"
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Timer/timer.inl	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,95 @@
+/**
+  ******************************************************************************
+  * @file    Timer.inl
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class definition for the timer functionality.
+  *          Inline implementation.
+  ******************************************************************************
+ */
+
+/** @brief  CTimer class constructor
+ *
+ *  Constructor method
+ *
+ *  
+ */
+template <unsigned int N, unsigned int D>
+CTimer<N,D>::CTimer()
+    : m_ticker()
+    , m_millisValue(0)
+    , m_started(0)
+{
+}
+
+/** @brief  CTimer class destructor
+ *
+ *  Destructor method
+ *
+ *  
+ */
+template <unsigned int N, unsigned int D>
+CTimer<N,D>::~CTimer() 
+{ 
+    if (m_started) 
+        stop ();
+}
+
+/** @brief  Start timer
+ *
+ *  
+ *  
+ */
+template <unsigned int N, unsigned int D>
+void CTimer<N,D>::start () 
+{
+    m_started = 1;
+    m_ticker.attach (mbed::callback(&CTimer::callback, this), 1.f*N/D);  
+}
+    
+/** @brief  Stop timer
+ *
+ *  
+ *  
+ */
+template <unsigned int N, unsigned int D>
+void CTimer<N,D>::stop() 
+{
+    m_started = 0;
+    m_ticker.detach ();
+}
+
+/** @brief  Get timer
+ *
+ *  
+ *  @return    Milliseconds value
+ */
+template <unsigned int N, unsigned int D>
+uint32_t CTimer<N,D>::get()
+{
+    return m_millisValue;
+}
+
+/** @brief  Timer callback
+ *
+ *  @param thisPointer   The object pointer.
+ *  
+ */
+template <unsigned int N, unsigned int D>
+void CTimer<N,D>::callback(void *thisPointer)
+{
+    CTimer<N,D>* self = static_cast<CTimer<N,D>*>(thisPointer);
+    self->millisTicker(); 
+}
+
+/** @brief Increase milliseconds value
+ *
+ *  
+ *  
+ */
+template <unsigned int N, unsigned int D>
+void CTimer<N,D>::millisTicker()
+{
+    m_millisValue ++;
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/BezierCurve/beziercurve.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,12 @@
+/**
+  ******************************************************************************
+  * @file    BezierCurve.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class implementation for the Bezier Curve
+  *          methods.
+  ******************************************************************************
+ */
+
+#include <BezierCurve/BezierCurve.hpp>
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/BezierCurve/polynomialfunction.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,1 @@
+#include <BezierCurve/PolynomialFunction.hpp>
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/BezierMotionPlanner/beziermotionplanner.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,168 @@
+/**
+  ******************************************************************************
+  * @file    BezierMotionPlanner.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class definition for the Bezier Motion
+  *          Planner methods.
+  ******************************************************************************
+ */
+
+#include <iostream>
+#include <BezierMotionPlanner/bezierMotionplanner.hpp>
+
+namespace planner{
+    
+    /**
+     * @brief Construct a new CBezierMotionPlanner::CBezierMotionPlanner object
+     * 
+     */
+    CBezierMotionPlanner::CBezierMotionPlanner()
+    {
+        this->isInitialized=false;
+    }
+
+    /**
+     * @brief Construct a new CBezierMotionPlanner::CBezierMotionPlanner object
+     * 
+     * @param isForward             Forward movement flag
+     * @param a                     Point A
+     * @param b                     Point B
+     * @param c                     Point C
+     * @param d                     Point D
+     * @param motion_duration_i     The motion duration in second. 
+     * @param timestep_i            The base period of the planner. (Sample time) 
+     */
+    CBezierMotionPlanner::CBezierMotionPlanner(bool                    isForward,
+                                            std::complex<float>     a,
+                                            std::complex<float>     b,
+                                            std::complex<float>     c,
+                                            std::complex<float>     d,
+                                            float                   motion_duration_i,
+                                            float                   timestep_i)
+        : isForward(isForward)
+        , bezierCurve(a,b,c,d)
+        , motion_duration(motion_duration_i)
+        , time_step(timestep_i)
+    {
+        this->bezierValueInput_step=1.0/(int)(this->motion_duration/this->time_step);
+        this->next_bezierValueInput=0.0;
+        this->isInitialized=true;
+    }
+
+
+    /**
+     * @brief Set motion planner parameters
+     * 
+     * @param isForward             Forward movement flag
+     * @param a                     Point A
+     * @param b                     Point B
+     * @param c                     Point C
+     * @param d                     Point D
+     * @param motion_duration_i     The motion duration in second. 
+     * @param timestep_i            The base period of the planner. (Sample time) 
+     */
+    void CBezierMotionPlanner::setMotionPlannerParameters(bool                    isForward,
+                                                        std::complex<float>     a,
+                                                        std::complex<float>     b,
+                                                        std::complex<float>     c,
+                                                        std::complex<float>     d,
+                                                        float                   motion_duration_i,
+                                                        float                   timestep_i)
+    {
+        this->isForward=isForward;
+        this->motion_duration=motion_duration_i;
+        this->time_step=timestep_i;
+        this->bezierCurve.setBezierCurve(a,b,c,d);
+        this->bezierValueInput_step=1.0/(int)(this->motion_duration/this->time_step);
+        this->next_bezierValueInput=0.0;
+        this->isInitialized=true;                        
+    }
+
+    /**
+     * @brief Destroy the CBezierMotionPlanner::CBezierMotionPlanner object
+     * 
+     */
+    CBezierMotionPlanner::~CBezierMotionPlanner()
+    {
+    }
+
+
+    /**
+     * @brief Get the Bezier curve.
+     * 
+     */
+    math::BezierCurve<float> CBezierMotionPlanner::getBezierCurve()
+    {
+        return this->bezierCurve;
+    }
+
+    /**
+     * @brief Get the next control parameters. It calculates the velocity and angle, and increase input value. 
+     * 
+     * @return It returns the next forward velocity and direction angle. 
+     */
+    std::pair<float,float> CBezierMotionPlanner::getNextVelocity(){
+        std::pair<float,float> commands=this->getVelocity(next_bezierValueInput);
+        this->next_bezierValueInput+=this->bezierValueInput_step;
+        return commands;
+    }
+
+    /**
+     * @brief Get the forward velocity and steering angle, base on the given input value.
+     * 
+     * @param input_value The input value have to belong to interval [0,1].
+     * @return It returns a pair of number, where the fist variable and the second contains the forward velocity and the steering angular, respectively.
+     */
+    std::pair<float,float> CBezierMotionPlanner::getVelocity(float input_value)
+    {
+        if(!this->isInitialized) return std::pair<float,float>(0,0);
+        
+        std::complex<float> dS=this->bezierCurve.get_FO_DerivateValue(input_value);
+
+        float dl_absolute=sqrt((dS*std::conj(dS)).real());//[0,1]//Length of the vector
+        float dl_real=dl_absolute/this->motion_duration;
+
+        std::complex<float> ddS=this->bezierCurve.get_SO_DerivateValue(input_value);
+
+        std::complex<float> correctorValue(0,-2);
+        std::complex<float> temp1=(dS*std::conj(ddS)-std::conj(dS)*ddS)/correctorValue;
+        float num=temp1.real();
+        if(dl_absolute==0)
+        {
+            return std::pair<float,float>(dl_real,0);
+        }
+
+        float k=num/pow(dl_absolute,3);
+
+        float angle_rad=atan(k*WHEELBASE);
+        float angle_deg=(180.f/M_PI)*angle_rad;
+
+        std::pair<float,float> commands(dl_real,angle_deg);
+        return commands;
+    }
+
+    /**
+     * @brief Get the state of the planner.
+     * 
+     * @return true The planner has valid value. 
+     * @return false The planner finished the last given curve, it cannnot get correct parameters. 
+     */
+    bool CBezierMotionPlanner::hasValidValue()
+    {
+        return (next_bezierValueInput>=0 && next_bezierValueInput<=1);
+    }
+
+    /** @brief  Get the direction of the motion
+     *
+     *  
+     * @return true The motion direction is forward. 
+     * @return false The motion direction is backward. 
+     */
+    bool CBezierMotionPlanner::getForward()
+    {
+        return this->isForward;
+    }
+
+}; // namespace planner
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/CommandInterpreter/commandinterpreter.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,180 @@
+/**
+  ******************************************************************************
+  * @file    CommandInterpreter.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class definition for the command interpreter
+  *          functionality.
+  ******************************************************************************
+ */
+
+#include <CommandInterpreter/commandinterpreter.hpp>
+
+/** \brief  CCommandInterpreter Class constructor
+ *
+ *  Constructor method
+ *
+ *  \param[in] f_car     reference to MOVE object
+ */
+CCommandInterpreter::CCommandInterpreter(Move& f_car) 
+    : m_buffer()
+    , m_car(f_car)
+    , m_commandValue()
+{
+}
+
+/** \brief  Interpret character
+ *
+ *  \param[in] f_c     character value
+ */
+void CCommandInterpreter::interpretChar(unsigned char f_c)
+{
+    if ((f_c == 'A') || (f_c == 'S'))
+    {
+//            led1 = 1;
+        reset();
+        m_buffer.push(f_c);
+        m_commandID = f_c;
+//            char s[100];
+//            unsigned int  l = sprintf(s,"storing command starting with %c, %c stored",f_c, m_buffer.peek());
+//            g_rpiWriteBuffer.push(s,l);
+    }
+    else
+    {
+        m_buffer.push(f_c);
+//            char s[100];
+//            unsigned int  l = sprintf(s,"peeked %c, command_ID is %c, size is %d",m_buffer.peek(),m_commandID,m_buffer.getSize());
+//            g_rpiWriteBuffer.push(s,l);
+        if (m_buffer.getSize() == 5)
+        {
+//                char s[100];
+//                unsigned int  l = sprintf(s,"interpreting command starting with %c",m_commandID);
+//                g_rpiWriteBuffer.push(s,l);
+            intepretCommand();
+            reset();
+        }
+    }
+}
+
+/** \brief  Execute command
+ *
+ */
+void CCommandInterpreter::executeCommand()
+{
+    m_car.Steer(m_angleValue);
+    m_car.Speed(m_speedValue);
+//        reset();
+}
+
+/** \brief  Reset
+ *
+ *  
+ *  
+ */
+void CCommandInterpreter::reset()
+{
+    m_commandID = 0;
+    m_buffer.empty();
+}
+
+/** \brief  Interpret command
+ *
+ *  
+ *  
+ */
+void CCommandInterpreter::intepretCommand()
+{
+    unsigned char test_char = m_buffer.pop();
+    if (test_char != m_commandID)
+    {
+        reset();
+//            led1 = 0;
+//            char s[100];
+//            unsigned int  l = sprintf(s,"interpretation failed at ID. Expected: %c, received: %c",m_commandID,test_char);
+//            g_rpiWriteBuffer.push(s,l);
+    }
+    else
+    {
+        float l_sign = 0;
+        if (m_buffer.peek() == '0')
+        {
+            l_sign = 1;
+        }
+        else if (m_buffer.peek() == '1')
+        {
+            l_sign = -1;
+        }
+        else
+        {
+            reset();
+//                led1 = 0;
+//                char s[100];
+//                unsigned int  l = sprintf(s,"interpretation failed a sign");
+//                g_rpiWriteBuffer.push(s,l);
+            return;
+        }
+        m_buffer.pop();
+        if ((m_buffer.peek() < '0') || (m_buffer.peek() > '9'))
+        {
+            reset();
+//                led1 = 0;
+//                char s[100];
+//                unsigned int  l = sprintf(s,"interpretation failed at val 1");
+//                g_rpiWriteBuffer.push(s,l);
+            return;
+        }
+        else
+        {
+            m_commandValue = m_buffer.pop() - '0';
+        }
+        if ((m_buffer.peek() < '0') || (m_buffer.peek() > '9'))
+        {
+            reset();
+//                led1 = 0;
+//                char s[100];
+//                unsigned int  l = sprintf(s,"interpretation failed at val 2, value is %c",m_buffer.peek());
+//                g_rpiWriteBuffer.push(s,l);
+            return;
+        }
+        else
+        {
+            m_commandValue *= 10;
+            m_commandValue += m_buffer.pop() - '0';
+            m_commandValue *= l_sign;
+        }
+        if (m_buffer.pop() != ';')
+        {
+            reset();
+//                led1 = 0;
+//                char s[100];
+//                unsigned int  l = sprintf(s,"interpretation failed at terminator");
+//                g_rpiWriteBuffer.push(s,l);
+            return;
+        }      
+        else
+        {
+            if(m_commandID == 'A')
+            {
+                m_angleValue = m_commandValue;
+//                    char s[100];
+//                    unsigned int  l = sprintf(s,"set angle to %f\n", m_angleValue);
+//                    g_rpiWriteBuffer.push(s,l);
+//                    led1 = 1;
+                reset();
+                return;
+            }
+            if(m_commandID == 'S')
+            {
+                m_speedValue = m_commandValue;
+//                    char s[100];
+//                    unsigned int  l = sprintf(s,"set speed to %f\n", m_speedValue);
+//                    g_rpiWriteBuffer.push(s,l);
+//                    led1 = 1;
+                reset();
+                return;
+            }
+            reset();
+        }      
+    }
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Controllers/controller.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,153 @@
+/**
+  ******************************************************************************
+  * @file    Controller.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class implementation for the controller
+  *          functionality.
+  ******************************************************************************
+ */
+
+#include <Controllers/controller.hpp>
+
+namespace controllers{
+    /**
+     * @brief Construct a new CControllerSiso::CControllerSiso object
+     * 
+     * @param f_encoder Reference to the encoder getter interface.
+     * @param f_pid     Reference to the controller interface.
+     * @param f_converter [Optional] Pointer to the converter interface. 
+     */
+    CControllerSiso::CControllerSiso(encoders::IEncoderGetter&          f_encoder
+                            ,ControllerType<double>&                    f_pid
+                            ,controllers::IConverter*                       f_converter)
+        :m_encoder(f_encoder)
+        ,m_pid(f_pid)
+        ,m_converter(f_converter)
+        ,m_control_sup(0.5)
+        ,m_control_inf(-0.5)
+        ,m_ref_abs_inf(10.0)
+        ,m_mes_abs_inf(30.0)
+        ,m_mes_abs_sup(300.0)
+    {
+    }
+
+    /**
+     * @brief Set the reference signal value.
+     * 
+     * @param f_RefRps The value of the reference signal
+     */
+    void CControllerSiso::setRef(double f_RefRps)
+    {
+        m_RefRps=f_RefRps;
+    }
+
+    /** \brief  Get the value of reference signal.
+     *
+     */
+    double CControllerSiso::getRef()
+    {
+        return m_RefRps;
+    }
+
+    /** @brief  Get the value of the control signal calculated last time.
+     * 
+     */
+    double CControllerSiso::get()
+    {
+        return m_u;
+    }
+
+    /** @brief  Get the value of the error between the measured and reference signal. 
+     *
+     */
+    double CControllerSiso::getError()
+    {
+        return m_error;
+    }
+
+    /** @brief  Clear the memory of the controller. 
+     *
+     */
+    void CControllerSiso::clear()
+    {
+        m_pid.clear();
+    }
+
+
+    /**
+     * @brief It calculates the next value of the control signal, by utilizing the given interfaces.
+     * 
+     * @return true control works fine
+     * @return false appeared an error
+     */
+    bool CControllerSiso::control()
+    {
+        // Mesurment speed value
+        float  l_MesRps = m_encoder.getSpeedRps();
+        bool   l_isAbs = m_encoder.isAbs();
+        float  l_ref;
+
+        // Check the measured value and the superior limit for avoid over control state.
+        // In this case deactivete the controller. 
+        if(std::abs(l_MesRps) > m_mes_abs_sup){
+            m_RefRps = 0.0;
+            m_u = 0.0;
+            error("@PIDA:CControllerSiso: To high speed!;;\r\n");
+            return false;
+        }
+        // Check the inferior limits of reference signal and measured signal for standing state.
+        // Inactivate the controller
+        if(std::abs(m_RefRps) < m_ref_abs_inf && std::abs(l_MesRps) < m_mes_abs_inf ){
+            m_u = 0.0;
+            m_error = 0.0;
+            return true; 
+        }
+
+        // Check measured value is oriantated or absolute
+        if ( l_isAbs ){
+            l_ref = std::abs(m_RefRps);
+        } else{ 
+            l_ref = m_RefRps;
+        }
+        float l_error=l_ref-l_MesRps;
+        float l_v_control = m_pid.calculateControl(l_error);
+        float l_pwm_control = converter(l_v_control);
+        
+        m_u=l_pwm_control;
+        m_error=l_error;
+        
+        // Check measured value is oriantated or absolute.
+        if(m_RefRps<0 && l_isAbs)
+        {
+            m_u=m_u*-1.0;
+        }
+        return true;
+    }
+
+    /** @brief  
+     *
+     * Apply the converter interface to change the measurment unit.
+     *
+     * @param f_u                  Input control signal
+     * @return                     Converted control signal
+     */
+    double CControllerSiso::converter(double f_u)
+    {
+        double l_pwm=f_u;
+        // Convert the control signal from V to PWM
+        if(m_converter!=NULL){
+            l_pwm = (*m_converter)(f_u);
+        }
+
+        // Check the pwm control signal and limits
+        if( l_pwm < m_control_inf ){
+            l_pwm  = m_control_inf;
+        } else if( l_pwm > m_control_sup ){
+            l_pwm  = m_control_sup;
+        }
+        return l_pwm;
+    }
+
+}; //  namespace controllers
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Controllers/converters.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,2 @@
+#include <Controllers\converters.hpp>
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Controllers/pidcontroller.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,13 @@
+/**
+  ******************************************************************************
+  * @file    PidController.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the classes implementation for the PID controller
+  *          functionality. Because template classes are used, the code was
+  *          placed in a corresponding .inl file
+  ******************************************************************************
+ */
+
+#include <Controllers/pidcontroller.hpp>
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Drivers/servo.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,63 @@
+/**
+  ******************************************************************************
+  * @file    SERVO.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class definition for the steering SERVO
+  *          functionality.
+  ******************************************************************************
+ */
+
+#include <Drivers/servo.hpp>
+
+
+namespace drivers{
+    /** @brief  SERVO class constructor
+     *
+     *  Constructor method
+     *
+     *  @param _pwm               pin connected to servo input
+     */
+    SERVO::SERVO(PinName _pwm)
+        :pwm(_pwm)
+    {
+        pwm.period_ms(20);    
+        // pwm.write(0.075);
+        pwm.write(0.07525);
+    };
+
+    /** @brief  SERVO class destructor
+     *
+     *  Destructor method
+     *
+     *   
+     */
+    SERVO::~SERVO()
+    {
+    };
+
+    /** @brief  Set angle method
+     *
+     *  @param angle      angle
+     */
+    void SERVO::SetAngle(float angle)
+    {
+        pwm.write(conversion(angle));
+    };
+
+    /** @brief  Conversion method
+     *
+     *  @param angle      angle
+     *  \return    duty cycle
+     */
+    float SERVO::conversion(float angle)
+    {
+        //rc-servo-4519
+        //return (0.0010321101 * angle + 0.0725);
+        //fs-5106B
+        // return (0.035 +0.0272+ (angle + 25.f) / 1953.135);
+        //RS2 MG/BB
+        return (0.0009505 * angle + 0.07525);
+    };
+}; // namespace drivers
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Drivers/vnh.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,135 @@
+/**
+  ******************************************************************************
+  * @file    VNH.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class defoinition for the VNH Bridge Driver
+  *          functionality.
+  ******************************************************************************
+ */
+
+#include <Drivers/vnh.hpp>
+
+
+namespace drivers{
+
+
+
+    /**  @brief  VNH class constructor
+     *
+     *  Constructor method
+     *
+     *  @param _pwm      PWM pin
+     *  @param _ina      pin A
+     *  @param _inb      pin B
+     *  @param _current  current analog pin
+     */
+    VNH::VNH(PinName _pwm, PinName _ina, PinName _inb, PinName _current)
+        :pwm(_pwm)
+        ,ina(_ina)
+        ,inb(_inb)
+        ,current(_current)
+    {  
+        pwm.period_us(200);   
+        increment = 0.01;
+        current_speed = 0;
+    }
+
+    /**  @brief  VNH class destructor
+     *
+     *  Destructor method
+     *
+     *   
+     */
+    VNH::~VNH()
+    {
+    }
+
+    /**  @brief  Start method
+     *
+     *   
+     *   
+     */
+    void VNH::Start(void)
+    {
+        ina = !inb;
+        pwm.write(0);
+    }
+
+    /**  @brief  Stop method
+     *
+     *   
+     *   
+     */
+    void VNH::Stop(void)
+    {
+        ina = inb;
+    }
+
+    /**  @brief  Run method
+     *
+     *  @param speed    speed value
+     *   
+     */
+    void VNH::Run(float speed)
+    {
+        Go(speed);
+    }
+
+    /**  @brief  Get current method
+     *
+     *   
+     *  \return    Current value
+     */
+    float VNH::GetCurrent(void)
+    {
+        return (current.read()) * 5 / 0.14;
+    }
+
+    /**  @brief  Go method
+     *
+     *  @param speed    speed value
+     *   
+     */
+    void VNH::Go(float speed)
+    {
+        //pwm.write(abs(speed));
+        if (speed < 0)
+        {
+            ina = 0;
+            inb = 1;
+        }
+        else
+        {
+            ina = 1;
+            inb = 0;
+        } 
+        pwm =std::abs(speed);
+    }
+
+    /**  @brief  Inverse method
+     *
+     *  @param f_speed  speed value
+     *   
+     */
+    void VNH::Inverse(float f_speed)
+    {
+        ina=!ina;
+        inb=!inb;
+        pwm =std::abs(f_speed);
+    }
+
+    /**  @brief  Brake method
+     *
+     *   
+     *   
+     */
+    void VNH::Brake()
+    {
+        ina.write(0);
+        inb.write(0);
+        pwm.write(100.0);
+    }
+
+}; // namespace drivers
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Encoders/encoder.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,204 @@
+/**
+  ******************************************************************************
+  * @file    Encoder.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class implementations for the encoder
+  *          functionality.
+  ******************************************************************************
+ */
+
+#include <Encoders/encoder.hpp>
+
+/** \brief  Constructor for the CCounter class
+ *
+ *  Constructor method
+ *
+ *  @param f_pin       digital pin connected to encoder output    
+ */
+CCounter::CCounter(PinName f_pin)
+    :m_interrupt(f_pin)
+    ,m_count(0)
+{
+    m_interrupt.rise(mbed::callback(CCounter::staticIncrement,this));
+    m_interrupt.fall(mbed::callback(CCounter::staticIncrement,this));
+}
+
+/** \brief  Counter reset method
+ * 
+ *  
+ *  
+ */
+void CCounter::reset(){
+    m_count=0;
+}
+
+/** \brief  Counter increment method
+ * 
+ *  
+ *  
+ */
+void CCounter::increment(){
+    m_count++;
+}
+
+/** \brief  Get counts method
+ * 
+ *  
+ *  @return    counts
+ */
+int32_t CCounter::getCount(){
+    return m_count;
+}
+
+/** \brief  Get counts method
+ * 
+ *  Method attaches the increment method to an object.
+ * 
+ *  @param obj            object
+ *  
+ */
+void CCounter::staticIncrement(void* obj){
+    CCounter* self=static_cast<CCounter*>(obj);
+    self->increment();
+}
+
+/** \brief  Constructor for the CEncoder class
+ *
+ *  Constructor method
+ *
+ *  @param f_period       period value 
+ *  @param f_period_sec   period value in seconds   
+ *  @param f_cpr          counts per revolution (rise and fall edges)
+ *  @param f_pinName      digital pin connected to encoder output
+ */
+CEncoder::CEncoder(uint32_t   f_period
+                  ,float      f_period_sec
+                  ,uint32_t   f_cpr
+                  ,PinName    f_pinName)
+    :task::CTask(f_period)
+    ,m_counter(f_pinName)
+    ,m_period_sec(f_period_sec)
+    ,m_cpr(f_cpr)
+    ,m_rps(0)
+{
+}
+
+/** \brief  Get rps method
+ *
+ *  Returns the value of the rotations per second value
+ *
+ *  
+ *  @return     rps   
+ */
+float CEncoder::getSpeedRps()
+{
+    return m_rps;
+}
+
+/** \brief  Get count
+ *
+ *  Returns the value of the count
+ *
+ *  
+ *  @return     rps   
+ */
+int16_t CEncoder::getCount()
+{
+    return m_rps*m_period_sec*m_cpr;
+}
+
+
+
+/** \brief  Run method
+ *
+ *  Method executed at predefined time intervals. 
+ *  Method called each f_period
+ *
+ *  
+ */
+void CEncoder::_run()
+{
+    float l_count=m_counter.getCount();
+    m_counter.reset();
+    m_rps=l_count/m_period_sec/m_cpr;
+}
+
+/** \brief  Constructor for the CMagEncoderTime class
+ *
+ *  Constructor method
+ *
+ *  @param f_pin       digital pin connected to encoder output    
+ */
+CMagEncoderTime::CMagEncoderTime(PinName f_pin)
+    :m_interrupt(f_pin)
+{
+    m_interrupt.rise(mbed::callback(CMagEncoderTime::staticRise,this));
+    // m_interrupt.fall(mbed::callback(CMagEncoderTime::staticFall,this));
+    m_Timer.start();
+}
+
+/** \brief  Callback method  for rising edge interrupt
+ *
+ *  
+ */
+void CMagEncoderTime::riseCallback()
+{
+    m_Timer.stop();
+    m_lowTime=m_Timer.read();
+    m_Timer.reset();
+    m_Timer.start();
+}
+
+/** \brief  Callback method for rising edge interrupt
+ *
+ *  
+ */
+void CMagEncoderTime::fallCallback()
+{
+    m_Timer.stop();
+    m_highTime=m_Timer.read();
+    m_Timer.reset();
+    m_Timer.start();
+}
+
+/** \brief  Callback method for rising edge interrupt attached to pbject
+ *
+ *  @param obj        object
+ */
+void CMagEncoderTime::staticRise(void* obj)
+{
+    CMagEncoderTime* self=static_cast<CMagEncoderTime*>(obj);
+    self->riseCallback();
+}
+
+/** \brief  Callback method for rising edge interrupt attached to pbject
+ *
+ *  @param obj        object
+ *  @return     None   
+ */
+void CMagEncoderTime::staticFall(void* obj)
+{
+    CMagEncoderTime* self=static_cast<CMagEncoderTime*>(obj);
+    self->fallCallback();
+}
+
+/** \brief  Callback method for rising edge interrupt
+ *
+ *  
+ *  @return     High period value   
+ */
+float CMagEncoderTime::getHighTime()
+{
+    return m_highTime;
+}
+
+/** \brief  Callback method for rising edge interrupt
+ *
+ *  @return     Low period value   
+ */
+float CMagEncoderTime::getLowTime()
+{
+    return m_lowTime;
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Encoders/encoderFilter.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,66 @@
+/**
+  ******************************************************************************
+  * @file    EncoderFilter.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class implementation for the encoder filter
+  *          functionality.
+  ******************************************************************************
+ */
+
+#include <Encoders/encoderFilter.hpp>
+
+/** \brief  Constructor for the CEncoderFilter class
+ *
+ *  Constructor method
+ *
+ *  \param[in] f_period       period value 
+ *  \param[in] f_period_sec   period value in seconds   
+ *  \param[in] f_cpr          counts per revolution (rise and fall edges)
+ *  \param[in] f_pinName      digital pin connected to encoder output
+ *  \param[in] f_filter       reference to filter object
+ */
+CEncoderFilter::CEncoderFilter(uint32_t                       f_period
+                              ,float                          f_period_sec
+                              ,uint32_t                       f_cpr
+                              ,PinName                        f_pinName
+                              ,CFilterFunction<float>&        f_filter)
+    :CEncoder(f_period,f_period_sec,f_cpr,f_pinName)
+    ,m_filter(f_filter)
+{
+}
+
+/** \brief  Get filtered rps value
+ *
+ *  Method for getting filtererd rotations per second value
+ *
+ *  \return filtered rps value
+ */
+float CEncoderFilter::getSpeedRps(){
+    return m_filteredRps;
+}
+
+/**
+ * @brief Getter filtered count value
+ * 
+ * @return int16_t 
+ */
+int16_t  CEncoderFilter::getCount(){
+     return m_filteredRps*m_period_sec*m_cpr;
+}
+
+/** \brief  Run method
+ *
+ *  Method executed at predefined time intervals. 
+ *  Method called each f_period
+ * 
+ */
+void CEncoderFilter::_run()
+{
+    float l_count=m_counter.getCount();
+    m_counter.reset();
+    m_rps=static_cast<float>(l_count)/m_period_sec/m_cpr;
+    m_filteredRps=m_filter(m_rps);
+}
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Encoders/quadratureencoder.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,111 @@
+/**
+ * @file quadratureencoder.cpp
+ * @author RBRO/PJ-IU
+ * @brief 
+ * @version 0.1
+ * @date 2018-10-23
+ * 
+ * @copyright Copyright (c) 2018
+ * 
+ */
+#include <Encoders/Quadratureencoder.hpp>
+
+
+namespace encoders{
+
+
+
+CQuadratureEncoder_TIM4 *CQuadratureEncoder_TIM4::m_instance = 0;
+CQuadratureEncoder_TIM4::CQuadratureEncoder_TIM4_Destroyer CQuadratureEncoder_TIM4::m_destroyer;
+
+
+/**
+ * @brief Setter function.
+ * 
+ * @param s singleton object address
+ */
+void CQuadratureEncoder_TIM4::CQuadratureEncoder_TIM4_Destroyer::SetSingleton(CQuadratureEncoder_TIM4* s){
+    m_singleton = s;
+}  
+
+/**
+ * @brief Destroy the cQuadratureencoder tim4::cQuadratureencoder tim4 destroyer::cQuadratureencoder tim4 destroyer object
+ * 
+ */
+CQuadratureEncoder_TIM4::CQuadratureEncoder_TIM4_Destroyer::~CQuadratureEncoder_TIM4_Destroyer(){
+    delete m_singleton;
+}
+
+
+/**
+ * @brief Constructor function. It verifies the existance of the singleton object and creates it.
+ * 
+ * @return The address of the singleton object
+ */
+CQuadratureEncoder_TIM4* CQuadratureEncoder_TIM4::Instance(){
+    if(!CQuadratureEncoder_TIM4::m_instance){
+        CQuadratureEncoder_TIM4::m_instance = new CQuadratureEncoder_TIM4;
+        m_instance->initialize();
+        CQuadratureEncoder_TIM4::m_destroyer.SetSingleton(m_instance);
+    }
+    return CQuadratureEncoder_TIM4::m_instance;
+}
+
+
+/**
+ * @brief Initialize the parameter of the object.
+ * 
+ */
+void CQuadratureEncoder_TIM4::initialize(){
+    //PB6 PB7 aka D10 MORPHO_PB7
+    // Enable clock for GPIOA
+   RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
+
+   //stm32f4xx.h
+   GPIOB->MODER |= GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1;         //PB6 & PB7 as Alternate Function   /*!< GPIO port mode register,               Address offset: 0x00      */
+   GPIOB->OTYPER |= GPIO_OTYPER_OT_6 | GPIO_OTYPER_OT_7;              //PB6 & PB7 as Inputs               /*!< GPIO port output type register,        Address offset: 0x04      */
+   GPIOB->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7; //Low speed                         /*!< GPIO port output speed register,       Address offset: 0x08      */
+   GPIOB->PUPDR |= GPIO_PUPDR_PUPDR6_1 | GPIO_PUPDR_PUPDR7_1;         //Pull Down                         /*!< GPIO port pull-up/pull-down register,  Address offset: 0x0C      */
+   GPIOB->AFR[0] |= 0x22000000;                                       //AF02 for PB6 & PB7                /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
+   GPIOB->AFR[1] |= 0x00000000;                                       //nibbles here refer to gpio8..15   /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
+
+   // configure TIM4 as Encoder input
+   // Enable clock for TIM4
+   // __TIM4_CLK_ENABLE();
+   RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;
+
+   TIM4->CR1 = 0x0001;                // CEN(Counter ENable)='1'     < TIM control register 1
+   TIM4->SMCR = TIM_ENCODERMODE_TI12; //               < TIM slave mode control register
+   //TIM_ENCODERMODE_TI1 input 1 edges trigger count
+   //TIM_ENCODERMODE_TI2 input 2 edges trigger count
+   //TIM_ENCODERMODE_TI12 all edges trigger count
+   TIM4->CCMR1 = 0xF1F1; // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1
+   //0xF nibble sets up filter
+   TIM4->CCMR2 = 0x0000;                       //                             < TIM capture/compare mode register 2
+   TIM4->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E; //     < TIM capture/compare enable register
+   TIM4->PSC = 0x0000;                         // Prescaler = (0+1)           < TIM prescaler
+   TIM4->ARR = 0xffff;                         // reload at 0xfffffff         < TIM auto-reload register
+
+   TIM4->CNT = 0x0000; //reset the counter before we use it
+}
+/**
+ * @brief Get the position of encoder. 
+ * 
+ */
+int16_t CQuadratureEncoder_TIM4::getCount(){
+    if(m_instance){
+        return TIM4->CNT;
+    }
+    return 0xffff;
+}
+/**
+ * @brief Reset the value of the counter to zero value.
+ */
+void CQuadratureEncoder_TIM4::reset(){
+    if(m_instance){
+        TIM4->CNT = 0;
+    }
+}
+
+
+}; // namespace drivers
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Encoders/quadratureencodertask.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,145 @@
+/**
+ * @file quadratureencodertask.cpp
+ * @author RBRO/PJ-IU
+ * @brief 
+ * @version 0.1
+ * @date 2018-10-23
+ * 
+ * @copyright Copyright (c) 2018
+ * 
+ */
+#include <Encoders/Quadratureencodertask.hpp>
+
+
+namespace encoders{
+
+
+
+/**
+ * @brief Construct a new CQuadratureEncoderTask::CQuadratureEncoderTask object
+ * 
+ * @param f_period_sec Period of the task
+ * @param f_Quadratureencoder The counter object
+ * @param f_resolution The resolution of the rotation encoder. (Cpr count per revolution)
+ */
+CQuadratureEncoderTask::CQuadratureEncoderTask(   float                           f_period_sec
+                                                ,encoders::CQuadratureEncoder_TIMX*        f_Quadratureencoder
+                                                ,uint16_t                        f_resolution)
+                                                :m_Quadratureencoder(f_Quadratureencoder)
+                                                ,m_taskperiod_s(f_period_sec)
+                                                ,m_resolution(f_resolution)
+                                                ,m_timer(mbed::callback(CQuadratureEncoderTask::static_callback,this))
+{
+}
+
+
+/**
+ * @brief Start the RosTimer to periodically apply the _run function. 
+ * 
+ */
+void CQuadratureEncoderTask::startTimer(){
+    m_timer.start(static_cast<int>(m_taskperiod_s*1000));
+}
+
+
+/**
+ * @brief The private run function, which will be applied periodically. 
+ * 
+ */
+void CQuadratureEncoderTask::_run(){
+    m_encoderCnt = m_Quadratureencoder->getCount();
+    m_Quadratureencoder->reset();
+}
+/**
+ * @brief Static callback function for applying the run function. 
+ * 
+ * @param obj CQuadratureEncoderTask object
+ */
+void CQuadratureEncoderTask::static_callback(void* obj){
+    CQuadratureEncoderTask* self = static_cast<CQuadratureEncoderTask*>(obj);
+    self->_run();
+}
+
+/**
+ * @brief Getter function for counted value.
+ * 
+ * @return int16_t counted impulse
+ */
+int16_t CQuadratureEncoderTask::getCount(){
+    return m_encoderCnt;
+}
+/**
+ * @brief Getter function for the rotation per second
+ * 
+ * @return rotation speed (rotation per second)
+ */
+
+float CQuadratureEncoderTask::getSpeedRps(){
+    return static_cast<float>(m_encoderCnt)/ m_resolution / m_taskperiod_s;
+}
+
+/**
+ * @brief Construct a new CQuadratureEncoderWithFilterTask::CQuadratureEncoderWithFilterTask object
+ * 
+ * @param f_period_sec Period of the task
+ * @param f_Quadratureencoder The counter object
+ * @param f_resolution The resolution of the rotation encoder. (Cpr count per revolution)
+ * @param f_filter The reference to the filter. 
+ */
+CQuadratureEncoderWithFilterTask::CQuadratureEncoderWithFilterTask(   float                           f_period_sec
+                                                                    ,encoders::CQuadratureEncoder_TIMX*        f_Quadratureencoder
+                                                                    ,uint16_t                       f_resolution
+                                                                    ,filter::CFilterFunction<float>&       f_filter)
+                                                                    :CQuadratureEncoderTask(f_period_sec,f_Quadratureencoder,f_resolution)
+                                                                    ,m_filter(f_filter)
+                                                                    {
+}
+
+/**
+ * @brief Private run method for getting the value from the counter, reseting it.  In the last step it filters the readed values. 
+ * 
+ */
+void CQuadratureEncoderWithFilterTask::_run(){
+    m_encoderCnt = m_Quadratureencoder->getCount();
+    m_Quadratureencoder->reset();
+    float temp = m_encoderCnt;
+    m_encoderCntFiltered = static_cast<int16_t>(m_filter(temp));
+
+}
+
+/**
+ * @brief Getter function for the last filtered value. 
+ * 
+ */
+int16_t CQuadratureEncoderWithFilterTask::getCount(){
+    return m_encoderCntFiltered;
+}
+
+/**
+ * @brief Getter function for the last filtered rotation speed. 
+ * 
+ */
+float CQuadratureEncoderWithFilterTask::getSpeedRps(){
+    return static_cast<double>(m_encoderCntFiltered)/m_resolution / m_taskperiod_s;
+
+}
+
+/**
+ * @brief Getter function for the last non-filtered value. 
+ * 
+ */
+int16_t CQuadratureEncoderWithFilterTask::getNonFilteredCount(){
+    return m_encoderCntFiltered;
+}
+
+/**
+ * @brief Getter function for the last non-filtered rotation speed. 
+ * 
+ */
+float CQuadratureEncoderWithFilterTask::getNonFilteredSpeedRps(){
+    return static_cast<double>(m_encoderCntFiltered)/m_resolution / m_taskperiod_s;
+
+}
+
+
+}; // namespace encoders 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Examples/Filter/ekf_am.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,30 @@
+/**
+  ******************************************************************************
+  * @file    Ekf_am.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class implementation for the Ackermann model example
+  *          functionality.
+  ******************************************************************************
+ */
+
+#include <Examples/Filter/ekf_am.hpp>
+
+/** \brief  Constructor for the CEKF_AM class
+ *
+ *  Constructor method
+ *
+ *  \param[in] f_ackermannModel       reference to Ackermann model object    
+ *  \param[in] f_jacobianMatrixCalc   Ackermann model Jacobian matrix
+ *  \param[in] f_Q                    reference to transition type
+ *  \param[in] f_R                    reference to observation noise type
+ */
+examples::filter::CEKF_AM::CEKF_AM(CAckermannModel&              f_ackermannModel
+                                  ,CJMAckermannModel             f_jacobianMatrixCalc
+                                  ,const CJMTransitionType&      f_Q
+                                  ,const CObservationNoiseType&  f_R)
+    :CEKF<double,2,10,5>(f_ackermannModel,f_jacobianMatrixCalc,f_Q,f_R)
+{
+    
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Examples/Sensors/sensorpublisher.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,15 @@
+/**
+  ******************************************************************************
+  * @file    SensorPublisher.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class implementation for the sensor publisher
+  *          functionality. Because template classes are used, the code was
+  *          placed in a corresponding .inl file.
+  ******************************************************************************
+ */
+
+#include <Examples/Sensors/sensorpublisher.hpp>
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Examples/Sensors/sensortask.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,115 @@
+/**
+  ******************************************************************************
+  * @file    SensorTask.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class implementation for the sensor task
+  *          functionality.
+  ******************************************************************************
+ */
+#include <Examples/Sensors/sensortask.hpp>
+
+namespace examples
+{
+
+    namespace sensors{
+        
+
+        /** \brief  Constructor for the CCounter class
+         *
+         *  Constructor method
+         *
+         *  @param f_period       period value
+         *  @param f_serial       reference to the serial object
+         */
+        CDISTFTest::CDISTFTest(uint32_t        f_period
+                                        ,Serial&         f_serial)
+            :task::CTask(f_period)
+            ,m_serial(f_serial)
+            ,m_tf()
+        {
+            std::array<std::array<float,1>,3> l_num({std::array<float,1>({3}),std::array<float,1>({-5.897}),std::array<float,1>({2.9})});
+            std::array<std::array<float,1>,3> l_den({std::array<float,1>({1}),std::array<float,1>({-1.949}),std::array<float,1>({0.9512})});
+            m_tf.setNum(l_num);
+            m_tf.setDen(l_den);
+        }
+
+        /** \brief  Method called each f_period
+         * 
+         *  
+         *  
+         */
+        void CDISTFTest::_run()
+        {
+            float l_input=1.0;
+            float l_output=m_tf(l_input);
+            m_serial.printf("%.4f\n",l_output);
+        }
+
+        /** \brief  Constructor for the CCounter class
+         *
+         *  Constructor method
+         *
+         *  @param f_period       period value
+         *  @param f_encoder      reference to encoder object
+         *  @param f_serial       reference to the serial object
+         */
+        CEncoderSender::CEncoderSender(uint32_t            f_period
+                                                ,encoders::IEncoderGetter&     f_encoder
+                                                ,Serial&             f_serial)
+            :task::CTask(f_period)
+            ,m_isActived(false)
+            ,m_encoder(f_encoder)
+            ,m_serial(f_serial)
+        {
+        }
+
+        /** \brief  Serial callback method
+         *
+         * Serial callback attaching serial callback to controller object
+         *
+         * @param  obj                 PID controller object
+         * @param  a                   string to read data from
+         * @param b                   string to write data to
+         * 
+         */
+        void CEncoderSender::staticSerialCallback(void* obj,char const * a, char * b){
+            examples::sensors::CEncoderSender* self = static_cast<CEncoderSender*>(obj);
+            self->serialCallback(a,b);
+        }
+
+        /** \brief  Serial callback actions
+         *
+         * Serial callback method setting controller to values received
+         *
+         * @param  a                   string to read data from
+         * @param b                   string to write data to
+         * 
+         */
+        void CEncoderSender::serialCallback(char const * a, char * b){
+            int l_isActivate=0;
+            uint32_t l_res = sscanf(a,"%d",&l_isActivate);
+            if(l_res==1){
+                m_isActived=(l_isActivate>=1);
+                sprintf(b,"ack;;");
+            }else{
+                sprintf(b,"sintax error;;");
+            }
+        }
+
+        /** \brief  Method called each f_period
+         * 
+         *  
+         *  
+         */
+        void CEncoderSender::_run()
+        {
+            if(!m_isActived) return;
+            float l_filtRps=m_encoder.getSpeedRps();
+            m_serial.printf("@ENPB:%.2f;;\r\n",l_filtRps);  
+        }                        
+
+    }; // namespace sensors
+}; // namespace examples
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Examples/SystemModels/ackermannmodel.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,141 @@
+/**
+  ******************************************************************************
+  * @file    AckermannModel.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class implementation for the Ackermann model.
+  ******************************************************************************
+ */
+
+#include <Examples/SystemModels/ackermannmodel.hpp>
+
+/* Specify model type */
+using ackermannmodeltype = systemmodels::nlti::mimo::CDiscreteTimeSystemModel<double,2,10,5>;
+
+/** \brief  Constructor for the CAckermannModel class
+ *
+ *  Constructor method
+ *
+ *  \param[in] f_states       reference to initial system states 
+ *  \param[in] f_dt           sample time
+ *  \param[in] f_gamma        reduction factor from motor to wheel
+ *  \param[in] f_wb           wheel base
+ *  \param[in] f_b            motor viscous friction constant
+ *  \param[in] f_J            moment of inertia of the rotor
+ *  \param[in] f_K            motor torque constant
+ *  \param[in] f_R            electric resistance 
+ *  \param[in] f_L            electric inductance
+ * 
+ */
+examples::systemmodels::ackermannmodel::CAckermannModel::CAckermannModel(
+                                     const CStatesType&     f_states
+                                    ,const double            f_dt     
+                                    ,const double            f_gamma
+                                    ,const double            f_wb
+                                    ,const double            f_b
+                                    ,const double            f_J
+                                    ,const double            f_K
+                                    ,const double            f_R
+                                    ,const double            f_L)
+    :CSystemModelType<double,2,10,5>(f_states,f_dt)
+    ,m_gamma(f_gamma)
+    ,m_wb(f_wb)
+    ,m_bJ(f_b/f_J)
+    ,m_KJ(f_K/f_J)
+    ,m_KL(f_K/f_L)
+    ,m_RL(f_R/f_L)
+    ,m_L(f_L)
+{
+}
+
+/** \brief  Constructor for the CAckermannModel class
+ *
+ *  Constructor method
+ *
+ *  \param[in] f_dt           sample time
+ *  \param[in] f_gamma        reduction factor from motor to wheel
+ *  \param[in] f_wb           wheel base
+ *  \param[in] f_b            motor viscous friction constant
+ *  \param[in] f_J            moment of inertia of the rotor
+ *  \param[in] f_K            motor torque constant
+ *  \param[in] f_R            electric resistance 
+ *  \param[in] f_L            electric inductance
+ * 
+ */
+examples::systemmodels::ackermannmodel::CAckermannModel::CAckermannModel(
+                                     const double      f_dt     
+                                    ,const double      f_gamma
+                                    ,const double      f_wb
+                                    ,const double      f_b
+                                    ,const double      f_J
+                                    ,const double      f_K
+                                    ,const double      f_R
+                                    ,const double      f_L)
+    :CSystemModelType<double,2,10,5>(f_dt)
+    ,m_gamma(f_gamma)
+    ,m_wb(f_wb)
+    ,m_bJ(f_b/f_J)
+    ,m_KJ(f_K/f_J)
+    ,m_KL(f_K/f_L)
+    ,m_RL(f_R/f_L)
+    ,m_L(f_L)
+{
+}
+
+/** \brief  Update method
+ *
+ *  Method for updating system states.
+ *
+ *  \param[in] f_input        reference to input type object
+ *  \return    system states
+ */
+ackermannmodeltype::CStatesType 
+    examples::systemmodels::ackermannmodel::CAckermannModel::update(
+      const CInputType&       f_input)
+{
+    CState l_states=m_states;
+    CInput l_input=f_input;
+    l_states.x()+=m_dt*l_states.x_dot();
+    l_states.y()+=m_dt*l_states.y_dot();
+
+    l_states.x_dot_prev()=l_states.x_dot();
+    l_states.y_dot_prev()=l_states.y_dot();
+
+    l_states.x_dot()=m_gamma*l_states.omega()*cos(l_states.teta_rad());
+    l_states.y_dot()=m_gamma*l_states.omega()*sin(l_states.teta_rad());
+
+    double l_alpha_rad=l_input.alpha()*DEG2RAD;
+    l_states.teta_rad_dot()=m_gamma*l_states.omega()*tan(l_alpha_rad)/m_wb;
+    l_states.teta_rad()+=m_dt*l_states.teta_rad_dot();
+
+    double omega_k_1=(1-m_dt*m_bJ)*l_states.omega()+m_dt*m_KJ*l_states.i();//next state of the motor's rotation speed
+    l_states.i()=-1*m_dt*m_KL*l_states.omega()+(1-m_dt*m_RL)*l_states.i()+m_dt*l_input.v()/m_L;
+    l_states.omega()=omega_k_1;
+    m_states=l_states;
+    return l_states;
+}
+
+/** \brief  Calculate output method
+ *
+ *  Method for calculating output depending on input.
+ *
+ *  \param[in] f_input        reference to input type object
+ *  \return    systemoutputs
+ */
+ackermannmodeltype::COutputType 
+    examples::systemmodels::ackermannmodel::CAckermannModel::calculateOutput(
+      const CInputType&  f_input)
+{
+    CState l_states=m_states;
+    // CInput l_input=f_input;
+    COutput l_outputs(COutputType::zeros());
+    l_outputs.x_ddot()=l_states.x_dot()-l_states.x_dot_prev();
+    l_outputs.y_ddot()=l_states.y_dot()-l_states.y_dot_prev();
+    l_outputs.teta_rad_dot()=l_states.teta_rad_dot();
+    l_outputs.speed()=l_states.omega()*m_gamma;
+    // output.alpha()=f_input.alpha();
+    l_outputs.i()=l_states.i();
+    m_outputs=l_outputs;
+    return l_outputs;
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Examples/blinker.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,38 @@
+/**
+  ******************************************************************************
+  * @file    Blinker.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class implementation for the blinker
+  *          functionality.
+  ******************************************************************************
+ */
+
+#include <Examples/blinker.hpp>
+
+
+namespace examples{
+    /** \brief  Class constructor
+     *
+     *  Constructor method
+     *
+     *  \param f_period       LED toggling reading period
+     *  \param f_led          Digital output line to which the LED is connected     
+     */
+    CBlinker::CBlinker(uint32_t f_period, DigitalOut f_led) 
+        : task::CTask(f_period)
+        , m_led(f_led) 
+    {
+        m_led = 1;
+    }
+
+    /** \brief  Method called each f_period
+     * 
+     */
+    void CBlinker::_run()
+    {
+        m_led = !m_led;
+    }
+
+}; // namespace examples
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Examples/echoer.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,36 @@
+/**
+  ******************************************************************************
+  * @file    Echoer.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class implementation for the serial echoer
+  *          functionality.
+  ******************************************************************************
+ */
+
+#include <Examples/echoer.hpp>
+
+namespace examples{
+    /** \brief  Class constructor
+     *
+     *  Constructor method
+     *
+     *  \param f_period       echoer execution period
+     *  \param f_serialPort   Serial communication object
+     */
+    CEchoer::CEchoer(uint32_t f_period, Serial& f_serialPort) 
+        : task::CTask(f_period)
+        , m_serialPort(f_serialPort)
+    {
+    }
+
+    /** \brief  Method called each f_period
+     * 
+     */
+    void CEchoer::_run()
+    {
+        m_serialPort.printf(".\n\r");
+    }
+
+}; // namespace examples
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Filter/filter.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,14 @@
+/**
+  ******************************************************************************
+  * @file    Filter.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class implementations for the filter
+  *          functionality. Because tmeplates are used, an .inl file contains the
+  *          actual implementation.
+  ******************************************************************************
+ */
+
+#include <Filter/filter.hpp>
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Linalg/linalg.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,2 @@
+#include <Linalg/linalg.h>
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/MotionController/motioncontroller.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,455 @@
+/**
+  ******************************************************************************
+  * @file    MotionController.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class definition for the motion controller
+  *          functionality.
+  ******************************************************************************
+ */
+
+#include <MotionController/motioncontroller.hpp>
+
+
+/** \brief  CMotionController Class constructor
+ *
+ *  Constructor method
+ *
+ *  @param f_period_sec          period for controller execution in seconds
+ *  @param f_serialPort          reference to serial communication object
+ *  @param f_car                 reference to MOVE object
+ *  @param f_safetyStop          reference to spline safery stop object
+ *  @param f_control             reference to controller object
+ */
+CMotionController::CMotionController(
+        float f_period_sec,
+        Serial& f_serialPort,
+        Move& f_car,
+        CSafetyStopFunction* f_safetyStop,
+        controllers::CControllerSiso* f_control) 
+    : m_serialPort(f_serialPort)
+    , m_car(f_car)
+    , m_speed()
+    , m_angle()
+    , m_period_sec(f_period_sec)
+    , m_isSplineActivated(false)
+    , m_ispidActivated(false)
+    , m_motionPlanner()
+    , m_hbTimeOut()
+    , m_control(f_control)
+    , m_safetyStop(f_safetyStop)
+    , m_timer(mbed::callback(CMotionController::staticCallbackRun,this))
+{
+}
+
+
+/** \brief  CMotionController Class constructor
+ *
+ *  Constructor method
+ *
+ *  @param f_period_sec          period for controller execution in seconds
+ *  @param f_serialPort          reference to serial communication object
+ *  @param f_car                 reference to MOVE object
+ *  @param f_control             reference to controller object
+ */
+CMotionController::CMotionController(
+        float f_period_sec,
+        Serial& f_serialPort,
+        Move& f_car,
+        controllers::CControllerSiso*           f_control) 
+    : m_serialPort(f_serialPort)
+    , m_car(f_car)
+    , m_speed()
+    , m_angle()
+    , m_period_sec(f_period_sec)
+    , m_isSplineActivated(false)
+    , m_ispidActivated(false)
+    , m_motionPlanner()
+    , m_hbTimeOut()
+    , m_control(f_control)
+    , m_safetyStop(NULL)
+    , m_timer(mbed::callback(CMotionController::staticCallbackRun,this))
+{
+}
+
+/** \brief  Serial callback method
+  *
+  * Serial callback attaching serial callback to controller object
+  *
+  * @param  obj                 PID controller object
+  * @param  a                   string to read data from
+  * @param b                   string to write data to
+  * 
+  */
+void CMotionController::staticSerialCallbackMove(void* obj,char const * a, char * b)
+{
+    CMotionController* self = static_cast<CMotionController*>(obj);
+    self->serialCallbackMove(a,b);
+}
+
+/** \brief  Serial callback method for BRAKE command
+  *
+  * Serial callback attaching serial callback to controller object
+  *
+  * @param  obj                 PID controller object
+  * @param  a                   string to read data from
+  * @param b                   string to write data to
+  * 
+  */
+void CMotionController::staticSerialCallbackBrake(void* obj,char const * a, char * b)
+{
+    CMotionController* self = static_cast<CMotionController*>(obj);
+    self->serialCallbackBrake(a,b);
+}
+
+/** \brief  Serial callback method for hard BRAKE command
+  *
+  * Serial callback attaching serial callback to controller object
+  *
+  * @param  obj                 PID controller object
+  * @param  a                   string to read data from
+  * @param b                   string to write data to
+  * 
+  */
+void CMotionController::staticSerialCallbackHardBrake(void* obj,char const * a, char * b) 
+{
+    CMotionController* self = static_cast<CMotionController*>(obj);
+    self->serialCallbackHardBrake(a,b);
+}
+
+/** \brief  Serial callback method for PID activation command
+  *
+  * Serial callback attaching serial callback to controller object
+  *
+  * @param  obj                 PID controller object
+  * @param  a                   string to read data from
+  * @param b                   string to write data to
+  * 
+  */
+void CMotionController::staticSerialCallbackPID(void* obj,char const * a, char * b) 
+{
+    CMotionController* self = static_cast<CMotionController*>(obj);
+    self->serialCallbackPID(a,b);
+}
+
+/**
+ * @brief Static seriel callback for spline command
+ * 
+ * @param obj                      object with the serial callback method
+ * @param a                        string to read data from
+ * @param b                        string to write data to
+ */
+void CMotionController::staticSerialCallbackSpline(void* obj, char const * a,char * b){
+    CMotionController* self = static_cast<CMotionController*>(obj);
+    self->serialCallbackSpline(a,b);
+}
+
+/** \brief  Reset method
+ * 
+ *  
+ *  
+ */
+void CMotionController::reset()
+{   
+    m_speed = 0;
+    m_angle = 0;
+}
+
+/** \brief  Get speed method
+ * 
+ *  
+ *  \return     Speed
+ */
+float CMotionController::getSpeed() 
+{
+    return m_speed;
+}
+
+/** \brief  Get angle method
+ * 
+ *  
+ *  \return     Angle
+ */
+float CMotionController::getAngle() 
+{
+    return m_angle;
+}
+
+/** \brief  BrakeCallback method
+ * 
+ *  
+ *  
+ */
+void CMotionController::BrakeCallback(){
+    m_state=2;
+}
+
+/** \brief  Set state method
+ * 
+ *  @param  f_state
+ *  
+ */
+void CMotionController::setState(int f_state){
+    m_state = f_state;
+}
+
+/** \brief  Method called each f_period
+ * 
+ *  
+ *  
+ */
+void CMotionController::_run()
+{
+    if(m_isSplineActivated) 
+    {
+        if(m_motionPlanner.hasValidValue())
+        {
+            std::pair<float,float> motion=m_motionPlanner.getNextVelocity();  
+            float l_dir=m_motionPlanner.getForward()?1:-1;
+            if(m_ispidActivated)
+            {
+                m_speed=motion.first*l_dir;
+                m_angle=motion.second;
+            }
+            else
+            {
+                //Pid isn't activated. It have to controllered with the robot speed value in meter per second.  
+                m_serialPort.printf("@SPLN:Err1;;\r\n");
+                m_state=2;
+                m_isSplineActivated=false;
+                m_speed=0.0;
+            }
+        }
+        else
+        {
+            m_serialPort.printf("@SPLN:Stop;;\r\n");
+            m_speed=0.0;
+            m_state=2;
+            m_isSplineActivated=false;
+        }
+    }
+    
+    //safety stop function
+    /**
+     * @brief It's a part of safety stop functionilty. Check if it's actived or created.
+     * 
+     */
+    if ((m_state!=2)&&(m_safetyStop!=NULL && m_safetyStop->isSafetyStopActive(m_speed,m_angle)==true))
+    {
+        m_state = 2;
+    }
+
+    switch(m_state)
+    {
+        // Move
+        case 1:
+            m_car.Steer(m_angle);
+            if(m_ispidActivated && m_control!=NULL)
+            {
+                m_control->setRef(CMotionController::Mps2Rps( m_speed ));
+                m_serialPort.printf("%f",CMotionController::Mps2Rps( m_speed ));
+                // Calculate control signal
+                bool l_isCorrect = m_control->control(); 
+                // Check the correct working of the control method
+                if( !l_isCorrect ){
+                    m_car.Brake();
+                    m_control->clear();
+                    m_state = 2;
+                }
+                m_car.Speed(m_control->get()*100.0);//Y
+            }
+            else
+            {
+                m_car.Speed(m_speed);
+            }
+            break;
+        // Brake
+        case 2:
+            m_car.Steer(m_angle);
+            m_car.Brake();
+            if( m_control!=NULL){
+                m_control->clear();
+            }
+            break;
+    }
+    
+}
+
+/** \brief  Serial callback actions for MOVE command
+  *
+  * Serial callback method setting controller to values received
+  *
+  * @param  a                   string to read data from
+  * @param b                   string to write data to
+  * 
+  */
+void CMotionController::serialCallbackMove(char const * a, char * b)
+{
+    float l_speed;
+    float l_angle;
+    uint32_t l_res = sscanf(a,"%f;%f",&l_speed,&l_angle);
+    if (2 == l_res)
+    {
+        if( !m_ispidActivated && std::abs(l_speed) > MCTL_PWM_COMMAND_LIMIT ){
+            sprintf(b,"Command is too high;;");
+            return;
+        }if( m_ispidActivated && std::abs(l_speed) > MCTL_SPEED_COMMAND_LIMIT ){
+            sprintf(b,"Command is too high;;");
+            return;
+        }
+
+        m_speed = l_speed;
+        m_angle = l_angle; 
+        m_isSplineActivated=false;
+        m_state=1;
+        sprintf(b,"ack;;");
+    }
+    else
+    {
+        sprintf(b,"sintax error;;");
+    }
+}
+
+/** \brief  Serial callback actions for BRAKE command
+  *
+  * Serial callback method setting controller to values received
+  *
+  * @param  a                   string to read data from
+  * @param b                   string to write data to
+  * 
+  */
+void CMotionController::serialCallbackBrake(char const * a, char * b)
+{
+    float l_angle;
+    uint32_t l_res = sscanf(a,"%f",&l_angle);
+    if(1 == l_res)
+    {
+        m_speed = 0;
+        m_angle = l_angle; 
+        m_state = 2;
+        if( m_control!=NULL){
+            m_control->setRef(0);
+        }
+        
+        sprintf(b,"ack;;");           
+    }
+    else
+    {
+        sprintf(b,"sintax error;;");
+    }
+}
+
+/** \brief  Serial callback actions for hard BRAKE command
+  *
+  * Serial callback method setting controller to values received
+  *
+  * @param  a                   string to read data from
+  * @param b                   string to write data to
+  * 
+  */
+void CMotionController::serialCallbackHardBrake(char const * a, char * b)
+{
+    float l_brake,l_angle;
+    uint32_t l_res = sscanf(a,"%f;%f",&l_brake,&l_angle);
+    if(2 == l_res && m_state!=0)
+    {
+        m_speed=0;
+        m_angle = l_angle; 
+        m_car.Inverse(l_brake);
+        m_hbTimeOut.attach(callback(this,&CMotionController::BrakeCallback),0.04);
+        m_state = 0;
+        sprintf(b,"ack;;");           
+    }
+    else
+    {
+        sprintf(b,"sintax error;;");
+    }
+}
+
+/** \brief  Serial callback actions for hard PID activation command
+  *
+  * Serial callback method setting controller to values received
+  *
+  * @param  a                   string to read data from
+  * @param b                   string to write data to
+  * 
+  */
+void CMotionController::serialCallbackPID(char const * a, char * b)
+{
+    int l_isActivate=0;
+    uint32_t l_res = sscanf(a,"%d",&l_isActivate);
+    if(l_res==1)
+    {   
+        if(m_control==NULL){
+            sprintf(b,"Control object wans't instances. Cannot be activate pid controller;;");
+        }else{
+            m_ispidActivated=(l_isActivate>=1);
+            m_state = 2;
+            sprintf(b,"ack;;");
+        }
+        
+    }else
+    {
+        sprintf(b,"sintax error;;");
+    }
+}
+
+/**
+ * @brief Serial callback actions for bezier spline command
+ * 
+ * @param a                   string to read data from
+ * @param b                   string to write data to
+ */
+void CMotionController::serialCallbackSpline(char const * a, char * b){
+    float a_x,a_y,b_x,b_y,c_x,c_y,d_x,d_y,duration_sec;
+    int isForward=1;
+    int32_t nrData=sscanf(a,"%d;%f;%f;%f;%f;%f;%f;%f;%f;%f",
+                                    &isForward,
+                                    &a_x,
+                                    &a_y,
+                                    &b_x,
+                                    &b_y,
+                                    &c_x,
+                                    &c_y,
+                                    &d_x,
+                                    &d_y,
+                                    &duration_sec);
+    if(10==nrData && duration_sec>0 && (isForward==1 || isForward==0)){
+        m_motionPlanner.setMotionPlannerParameters(static_cast<bool>(isForward), std::complex<float>(a_x,a_y),std::complex<float>(b_x,b_y),std::complex<float>(c_x,c_y),std::complex<float>(d_x,d_y),duration_sec,m_period_sec);
+        m_isSplineActivated=true;
+        m_state=1;
+        sprintf(b,"ack;;");
+    }else{
+        sprintf(b,"sintax error;;");
+    }
+}
+
+
+/**
+ * @brief Function to convert from linear velocity ( centimeter per second ) of robot to angular velocity ( rotation per second ) of motor.
+ * 
+ * @param f_vel_mps linear velocity
+ * @return float angular velocity
+ */
+float CMotionController::Mps2Rps(float f_vel_mps){
+    return f_vel_mps * 150.0;
+}
+
+/**
+ * @brief Static callback function for run method
+ * 
+ * @param obj pointer for the object
+ */
+void CMotionController::staticCallbackRun(void* obj){
+    CMotionController* self = static_cast<CMotionController*>(obj);
+    self->_run();
+}  
+
+/**
+ * @brief Start RtosTimer, which periodically apply the run method. The period of the task is defined in the contructor.
+ * 
+ */
+void CMotionController::startRtosTimer(){
+    this->m_timer.start(static_cast<int>(m_period_sec*1000));
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Move/move.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,134 @@
+/**
+  ******************************************************************************
+  * @file    Move.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class definition for the moving
+  *          functionality.
+  ******************************************************************************
+ */
+
+#include "mbed.h"
+#include <Move/move.hpp>
+//#include <array.h>
+
+/** \brief  Move Class constructor
+ *
+ *  Constructor method
+ *
+ *  @param _pwm_servo
+ *  @param _pwm_driver
+ *  @param _ina_driver
+ *  @param _inb_driver
+ *  @param _current_driver
+ */
+Move::Move(
+        PinName _pwm_servo,
+        PinName _pwm_driver, 
+        PinName _ina_driver, 
+        PinName _inb_driver, 
+        PinName _current_driver)
+    :servo(_pwm_servo),
+    vnh(_pwm_driver,_ina_driver,_inb_driver,_current_driver)
+{
+};
+
+/** \brief  Move Class destructor
+ *
+ *  Destructor method
+ *
+ *  
+ */
+Move::~Move()
+{
+};
+
+/** \brief  Steer method
+ *
+ *  @param angle
+ *  
+ */
+void Move::Steer(float angle)
+{
+    if ((angle <= 23) && (angle >= -23))
+        servo.SetAngle(angle);
+};
+
+/** \brief  Move method
+ *
+ *  @param speed The Pwm value, must belong to [0,100].
+ *  
+ */
+void Move::Speed(float speed)
+{
+    speed /=100;
+    vnh.Run(speed);
+};
+
+/** \brief  Brake method
+ *
+ *  
+ *  
+ */
+void Move::Brake()
+{
+    vnh.Brake();
+}
+
+/** \brief  Hard brake method
+ *
+ *  @param f_speed The Pwm value, must belong to [0,100].
+ *  
+ */
+void Move::Inverse(float f_speed)
+{
+    f_speed /=100;
+    vnh.Inverse(f_speed);
+}
+
+/** \brief  Car testing method
+ *
+ *  
+ *  
+ */
+void Move::TestCar()
+{
+    Steer(20);
+    wait(1);
+    Steer(0);
+    wait(1);
+    Steer(-20);
+    wait(1);
+    ResetCar();
+    wait(1);
+    Speed(25);
+    wait(1);
+    Speed(0);
+    wait(1);
+    Speed(-25);
+    wait(1);
+    ResetCar();
+    wait(1);
+};
+
+/** \brief  Reset speed and steer method
+ *
+ *  
+ *  
+ */
+void Move::ResetCar()
+{
+    Steer(0);
+    Speed(0);
+};
+
+/** \brief  Get bridge (driver) object reference method
+ *
+ *  
+ *  @return    reference to VNH object
+ */
+drivers::VNH& Move::getVNH() 
+{ 
+    return vnh; 
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Queue/queue.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,12 @@
+/**
+  ******************************************************************************
+  * @file    Queue.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-year
+  * @brief   This file contains the class definition for the queue
+  *          functionality.
+  ******************************************************************************
+ */
+
+#include <Queue/queue.hpp>
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/SHARP_IR_distance/DistanceSensors/distancesensors.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,13 @@
+/**
+  ******************************************************************************
+  * @file    DistanceSensors.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    08-January-2018
+  * @brief   This file contains the class implementation for the distance sensors
+  *          read methods.
+  ******************************************************************************
+ */
+
+#include <SHARP_IR_distance\DistanceSensors\distancesensors.hpp>
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/SHARP_IR_distance/IR_DistanceSensorDriver/ir_distancesensordriver.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,91 @@
+/**
+  ******************************************************************************
+  * @file    IR_DistanceSensorDriver.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    08-January-2018
+  * @brief   This file contains the class implementation for the SHARP IR sensor read
+  *          methods.
+  ******************************************************************************
+ */
+
+#include <SHARP_IR_distance\IR_DistanceSensorDriver\ir_distancesensordriver.hpp>
+
+/**
+ * @brief Construct a new c ir distancesensordriver::c ir distancesensordriver object
+ * 
+ * @param _ain_pin analog entrance pin
+ * @param _en_pin digital enable pin
+ */
+C_IR_DistanceSensorDriver::C_IR_DistanceSensorDriver(AnalogIn _ain_pin, DigitalOut _en_pin)
+    : ain_pin(_ain_pin)
+    , en_pin(_en_pin)
+    , value(0)
+{
+};
+
+/** @brief  Class destructor
+
+    Destructor method
+ */
+C_IR_DistanceSensorDriver::~C_IR_DistanceSensorDriver()
+{
+};
+
+/** @brief  Method for enabling sensor
+
+    
+    
+ */
+void  C_IR_DistanceSensorDriver::enable(){
+    en_pin=1;
+}
+
+/** @brief  Method for disabling sensor
+
+    
+    
+ */
+void  C_IR_DistanceSensorDriver::disable(){
+    en_pin=0;
+}
+/*
+void  C_IR_DistanceSensorDriver::setValue(float val){
+    this->value=val;
+}
+*/
+
+/** @brief  Method for getting value read from sensor
+
+    
+    @return     value read from sensor
+ */
+float  C_IR_DistanceSensorDriver::getValue(){
+    return this->value;
+}
+
+/** @brief  Method for getting scaled value read from sensor
+
+    
+    @return     scaled value read from sensor
+ */
+float C_IR_DistanceSensorDriver::ReadDistance()
+{
+    // read only if sensor enabled
+    if (this->en_pin == 1){
+        //read analog pin
+        float current_value=ain_pin.read();
+        //-scaling
+        float scaled_value =   C_IR_DistanceSensorDriver_A * pow(current_value,5)
+                                + C_IR_DistanceSensorDriver_B * pow(current_value,4)
+                                + C_IR_DistanceSensorDriver_C * pow(current_value,3)
+                                + C_IR_DistanceSensorDriver_D * pow(current_value,2)
+                                + C_IR_DistanceSensorDriver_E * current_value
+                                + C_IR_DistanceSensorDriver_F;
+                                
+        //float scaled_value=current_value; // without scaling, gross signal
+        this->value=scaled_value;
+        return this->value;
+    }
+    else return 0;
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/SafetyStop/safetystopfunction.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,135 @@
+/**
+  ******************************************************************************
+  * @file    SafetyStopFunction.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class definition for the safety stop activation
+  *          methods.
+  ******************************************************************************
+ */
+
+#include <SafetyStop/safetystopfunction.hpp>
+
+
+/**
+ * @brief Construct a new CSafetyStopFunction::CSafetyStopFunction object
+ * 
+ * @param f_senLeftFront sensor mounteed on left front part
+ * @param f_senMidLeftFront sensor mounteed on mid left front part
+ * @param f_senCenterFront sensor mounted on middle front part
+ * @param f_senMidRightFront sensor mounteed on mid right front part
+ * @param f_senRightFront sensor mounteed on right front part
+ */
+CSafetyStopFunction::CSafetyStopFunction(
+                      C_IR_DistanceSensorDriver& f_senLeftFront
+                    , C_IR_DistanceSensorDriver& f_senMidLeftFront
+                    , C_IR_DistanceSensorDriver& f_senCenterFront
+                    , C_IR_DistanceSensorDriver& f_senMidRightFront
+                    , C_IR_DistanceSensorDriver& f_senRightFront
+                    // , C_IR_DistanceSensorDriver& f_senLeftBack
+                    // , C_IR_DistanceSensorDriver& f_senRightBack
+                    )
+    : m_active(false) 
+    ,m_speed(0)
+    , m_angle(0)
+    , m_senLeftFront(f_senLeftFront)
+    , m_senMidLeftFront(f_senMidLeftFront)
+    , m_senCenterFront(f_senCenterFront)
+    , m_senMidRightFront(f_senMidRightFront)
+    , m_senRightFront(f_senRightFront)
+    // , m_senLeftBach(f_senLeftBack)
+    // , m_senRightBach(f_senRightBack)
+{
+}
+
+/** @brief  Class destructor
+ *
+ *  Destructor method
+ */
+CSafetyStopFunction::~CSafetyStopFunction()
+{}
+
+/** @brief  Function for activating safety stop
+ * 
+ *  @param  f_speed
+ *  @param f_angle
+ */
+bool CSafetyStopFunction::isSafetyStopActive(float f_speed, float f_angle){
+    // Verification the state of the functionality
+    if(!this->m_active){
+        // Inactive -> no need to brake
+        return false;
+    }
+    // Get current speed
+    m_speed = f_speed;
+    // Get current angle
+    m_angle = f_angle;
+    // Value indicating whether brake should be activated
+    bool ret_val  =false;
+    // dyunamic threshold for activating brake
+    float threshold;
+    // If moving forward take into account only front sensors
+    if (m_speed > 0)
+    {
+        threshold = m_speed*5-(1/m_speed)*10;
+        if ((m_senLeftFront.getValue() <= threshold) ||
+            (m_senMidLeftFront.getValue() <= threshold) ||
+            (m_senCenterFront.getValue() <= threshold) || 
+            (m_senMidRightFront.getValue() <= threshold) || 
+            (m_senRightFront.getValue() <= threshold)  )
+        {
+            ret_val = true;
+        }
+    }
+    // If moving forward take into account only front sensors
+    // Uncomment next if clause only if back sensors are being mounted
+    // else if (m_speed < 0)
+    //         {
+    //             threshold = m_speed*8+(1/m_speed)*10;
+    //             if ( (m_senLeftBach.getDistance() <= threshold*(-1)) || (m_senRightBach.getDistance() <= threshold*(-1))  )
+    //             {
+    //                 ret_val = true;
+    //             }
+    //         }
+
+    // Return value indicating whether brake should be activated
+    return ret_val;
+}
+
+/** @brief  Serial callback method
+  *
+  * Serial callback attaching serial callback to CSafetyStopFunction object
+  *
+  * @param  obj                 CSafetyStopFunction controller object
+  * @param  a                   string to read data from
+  * @param b                   string to write data to
+  * 
+  */
+void CSafetyStopFunction::staticSerialCallback(void* obj,char const * a, char * b)
+{
+    CSafetyStopFunction* self = static_cast<CSafetyStopFunction*>(obj);
+    self->serialCallback(a,b);
+}
+
+/** @brief  Serial callback actions
+  *
+  * Serial callback method setting controller to values received
+  *
+  * @param  a                   string to read data from
+  * @param b                   string to write data to
+  * 
+  */
+void CSafetyStopFunction::serialCallback(char const * a, char * b){
+        int l_isActivate=0;
+        uint32_t l_res= sscanf(a,"%d",&l_isActivate);
+        if(l_res==1){
+            m_active=(l_isActivate>=1);
+            sprintf(b,"ack;;");
+        }else{
+            sprintf(b,"sintax error;;");
+        }
+    }
+
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/SerialMonitor/serialmonitor.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,139 @@
+/**
+  ******************************************************************************
+  * @file    SerialMonitor.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class definition for the serial communication
+  *          functionality.
+  ******************************************************************************
+ */
+
+#include <SerialMonitor/serialmonitor.hpp>
+
+
+namespace serial{
+
+    /** @brief  CSerialMonitor class constructor
+     *
+     *
+     *  @param f_serialPort               reference to serial object
+     *  @param f_serialSubscriberMap      sensor mounteed on left front part
+     */
+    CSerialMonitor::CSerialMonitor(Serial& f_serialPort
+                    ,CSerialSubscriberMap f_serialSubscriberMap)
+            :task::CTask(0)
+            , m_serialPort(f_serialPort)
+            , m_RxBuffer()
+            , m_TxBuffer()
+            , m_parseBuffer()
+            , m_parseIt(m_parseBuffer.begin())
+            , m_serialSubscriberMap(f_serialSubscriberMap) 
+            {
+                m_serialPort.attach(mbed::callback(&CSerialMonitor::RxCallback, this), Serial::RxIrq);
+                m_serialPort.attach(mbed::callback(&CSerialMonitor::TxCallback, this), Serial::TxIrq);
+            }
+
+    /** @brief  Rx callback
+     *
+     *  @param thisPointer         the object pointer
+     *  
+     */
+    void CSerialMonitor::RxCallback(void *thisPointer)
+    {
+        CSerialMonitor* self = static_cast<CSerialMonitor*>(thisPointer);
+        self->serialRxCallback(); 
+    }
+
+    /** @brief  Tx callback
+     *
+     *  @param thisPointer         the object pointer
+     *  
+     */
+    void CSerialMonitor::TxCallback(void *thisPointer)
+    {
+        CSerialMonitor* self = static_cast<CSerialMonitor*>(thisPointer);
+        self->serialTxCallback(); 
+    }
+
+    /** @brief  Rx callback actions
+     *
+     *  @param None
+     *  
+     */
+    void CSerialMonitor::serialRxCallback()
+    {
+        __disable_irq();
+        while ((m_serialPort.readable()) && (!m_RxBuffer.isFull())) {
+            char l_c = m_serialPort.getc();
+            m_RxBuffer.push(l_c);
+        }
+        __enable_irq();
+        return;
+    }
+
+    /** @brief  Tx callback actions
+     *
+     *  @param None
+     *  
+     */
+    void CSerialMonitor::serialTxCallback()
+    {
+        __disable_irq();
+        while ((m_serialPort.writeable()) && (!m_TxBuffer.isEmpty())) {
+            m_serialPort.putc(m_TxBuffer.pop());
+        }
+        __enable_irq();
+        return;
+    }
+
+    /** @brief  Run method
+     * 
+     *  @param  None
+     *  @param None
+     */
+    void CSerialMonitor::_run()
+    {
+        if ((!m_RxBuffer.isEmpty()))
+        {
+            char l_c = m_RxBuffer.pop();
+            // m_serialPort.printf("%c",l_c);
+            if ('#' == l_c)
+            {
+                m_parseIt = m_parseBuffer.begin();
+                m_parseIt[0] = l_c;
+                m_parseIt++;
+                return;
+            }
+            if (m_parseIt != m_parseBuffer.end())
+            {
+                if (l_c == '\n')
+                {
+                    if ((';' == m_parseIt[-3]) && (';' == m_parseIt[-2]) && ('\r' == m_parseIt[-1]))
+                    {
+                        char l_msgID[5];
+                        char l_msg[256];
+
+                        uint32_t res = sscanf(m_parseBuffer.data(),"#%4s:%s;;",l_msgID,l_msg);
+                        if (res == 2)
+                        {
+                            auto l_pair = m_serialSubscriberMap.find(l_msgID);
+                            if (l_pair != m_serialSubscriberMap.end())
+                            {
+                                char l_resp[256] = "no response given";
+                                string s(l_resp);
+                                l_pair->second(l_msg,l_resp);
+                                m_serialPort.printf("@%s:%s\r\n",l_msgID,l_resp);
+                            }
+                        }
+                        m_parseIt = m_parseBuffer.begin();
+                    }
+                }
+                m_parseIt[0] = l_c;
+                m_parseIt++;
+                return;
+            }
+        }
+    }
+
+}; // namespace serial
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/SystemModels/systemmodels.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,11 @@
+/**
+  ******************************************************************************
+  * @file    SystemModels.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class definitions for system model.
+  ******************************************************************************
+ */
+
+#include <SystemModels/systemmodels.hpp>
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/TaskManager/taskmanager.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,92 @@
+/**
+  ******************************************************************************
+  * @file    TaskManager.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class definition for task manager.
+  ******************************************************************************
+ */
+
+#include <TaskManager/taskmanager.hpp>
+
+namespace task{
+
+    /******************************************************************************/
+    /** \brief  CTask class constructor
+     *
+     *  Constructor method
+     *
+     *  @param f_period      execution period
+     */
+    CTask::CTask(uint32_t f_period) 
+        : m_period(f_period)
+        , m_ticks(0)
+        , m_triggered(false) 
+    {
+    }
+
+    /** \brief  CTask class destructor
+     *
+     *  Destructor method
+     *
+     *  
+     */
+    CTask::~CTask() 
+    {
+    }
+
+    /** \brief  Run method
+     *
+     *  Destructor method
+     *
+     *  
+     *  
+     */
+    void CTask::run()
+    {
+        if (m_triggered)
+        {
+            m_triggered = false;
+            _run();
+        }
+    }
+
+    /******************************************************************************/
+    /** \brief  CTaskManager class constructor
+     *
+     *  Constructor method
+     *
+     *  @param f_taskList      task list
+     *  @param f_taskCount     task count
+     *  @param f_baseFreq      base frequency
+     */
+    CTaskManager::CTaskManager(task::CTask** f_taskList, uint32_t f_taskCount, float f_baseFreq)
+        : m_taskList(f_taskList)
+        , m_taskCount(f_taskCount) 
+    {
+        m_ticker.attach(mbed::callback(&task::CTaskManager::callback, this), f_baseFreq);
+    }
+
+    /** \brief  CTaskManager class destructor
+     *
+     *  Destructor method
+     *
+     *  
+     */
+    CTaskManager::~CTaskManager() 
+    {
+        m_ticker.detach();
+    }
+
+    /** \brief  Callback method
+     *
+     *  @param thisPointer       The object pointer
+     */
+    void CTaskManager::callback(void *thisPointer)
+    {
+        task::CTaskManager* self = static_cast<task::CTaskManager*>(thisPointer);
+        self->timerCallback(); 
+    }
+
+}; // namespace task
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/Timer/timer.cpp	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,11 @@
+/**
+  ******************************************************************************
+  * @file    Timer.cpp
+  * @author  RBRO/PJ-IU
+  * @version V1.0.0
+  * @date    day-month-2017
+  * @brief   This file contains the class definition for the timer functionality.
+  ******************************************************************************
+ */
+
+#include <Timer/timer.hpp>