diff --git a/include/signals/Integration.h b/include/signals/Integration.h
index 7105ea9..6e1666a 100644
--- a/include/signals/Integration.h
+++ b/include/signals/Integration.h
@@ -4,11 +4,11 @@
 template<typename IntegratorType>
 struct Integrator
 {
-    template<typename BaseSignalSpec, typename TangentSignalSpec>
-    static bool integrate(Signal<BaseSignalSpec, TangentSignalSpec>&          xInt,
-                          const Signal<TangentSignalSpec, TangentSignalSpec>& x,
-                          const double&                                       tf,
-                          const bool&                                         insertIntoHistory = false)
+    template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
+    static bool integrate(Signal<T, BaseSignalSpec, TangentSignalSpec>&          xInt,
+                          const Signal<T, TangentSignalSpec, TangentSignalSpec>& x,
+                          const double&                                          tf,
+                          const bool&                                            insertIntoHistory = false)
     {
         double t0 = xInt.t();
         double dt;
@@ -19,12 +19,12 @@ struct Integrator
         return IntegratorType::integrate(xInt, x, t0, tf, insertIntoHistory);
     }
 
-    template<typename BaseSignalSpec, typename TangentSignalSpec>
-    static bool integrate(Signal<BaseSignalSpec, TangentSignalSpec>&          xInt,
-                          const Signal<TangentSignalSpec, TangentSignalSpec>& x,
-                          const double&                                       tf,
-                          const double&                                       dt,
-                          const bool&                                         insertIntoHistory = false)
+    template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
+    static bool integrate(Signal<T, BaseSignalSpec, TangentSignalSpec>&          xInt,
+                          const Signal<T, TangentSignalSpec, TangentSignalSpec>& x,
+                          const double&                                          tf,
+                          const double&                                          dt,
+                          const bool&                                            insertIntoHistory = false)
     {
         double t_k     = xInt.t();
         bool   success = true;
@@ -48,12 +48,12 @@ struct Integrator
 
 struct EulerIntegratorSpec
 {
-    template<typename BaseSignalSpec, typename TangentSignalSpec>
-    static bool integrate(Signal<BaseSignalSpec, TangentSignalSpec>&          xInt,
-                          const Signal<TangentSignalSpec, TangentSignalSpec>& x,
-                          const double&                                       t0,
-                          const double&                                       tf,
-                          const bool&                                         insertIntoHistory)
+    template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
+    static bool integrate(Signal<T, BaseSignalSpec, TangentSignalSpec>&          xInt,
+                          const Signal<T, TangentSignalSpec, TangentSignalSpec>& x,
+                          const double&                                          t0,
+                          const double&                                          tf,
+                          const bool&                                            insertIntoHistory)
     {
         double dt = tf - t0;
         return xInt.update(tf, xInt() + x(tf) * dt, x(tf), insertIntoHistory);
@@ -62,12 +62,12 @@ struct EulerIntegratorSpec
 
 struct TrapezoidalIntegratorSpec
 {
-    template<typename BaseSignalSpec, typename TangentSignalSpec>
-    static bool integrate(Signal<BaseSignalSpec, TangentSignalSpec>&          xInt,
-                          const Signal<TangentSignalSpec, TangentSignalSpec>& x,
-                          const double&                                       t0,
-                          const double&                                       tf,
-                          const bool&                                         insertIntoHistory)
+    template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
+    static bool integrate(Signal<T, BaseSignalSpec, TangentSignalSpec>&          xInt,
+                          const Signal<T, TangentSignalSpec, TangentSignalSpec>& x,
+                          const double&                                          t0,
+                          const double&                                          tf,
+                          const bool&                                            insertIntoHistory)
     {
         double dt = tf - t0;
         return xInt.update(tf, xInt() + (xInt.dot() + x(tf)) * dt / 2.0, x(tf), insertIntoHistory);
diff --git a/include/signals/Signal.h b/include/signals/Signal.h
index 292b0ea..41738bc 100644
--- a/include/signals/Signal.h
+++ b/include/signals/Signal.h
@@ -29,7 +29,7 @@ enum DerivativeMethod
     FINITE_DIFF
 };
 
-template<typename BaseSignalSpec, typename TangentSignalSpec>
+template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
 class Signal
 {
 public:
@@ -75,9 +75,9 @@ class Signal
         this->needsSort_          = other.needsSort_;
     }
 
-    Signal<TangentSignalSpec, TangentSignalSpec> dotSignal()
+    Signal<T, TangentSignalSpec, TangentSignalSpec> dotSignal()
     {
-        Signal<TangentSignalSpec, TangentSignalSpec> signalDot;
+        Signal<T, TangentSignalSpec, TangentSignalSpec> signalDot;
         for (auto signalDP : signalHistory_)
         {
             signalDot.update(signalDP.t, signalDP.xdot, true);
@@ -86,17 +86,17 @@ class Signal
         return signalDot;
     }
 
-    template<typename BSS, typename TSS>
-    friend Signal<BSS, TSS> operator+(const Signal<BSS, TSS>& l, const Signal<TSS, TSS>& r);
+    template<typename S, typename BSS, typename TSS>
+    friend Signal<S, BSS, TSS> operator+(const Signal<S, BSS, TSS>& l, const Signal<S, TSS, TSS>& r);
 
-    template<typename BSS, typename TSS>
-    friend Signal<TSS, TSS> operator-(const Signal<BSS, TSS>& l, const Signal<BSS, TSS>& r);
+    template<typename S, typename BSS, typename TSS>
+    friend Signal<S, TSS, TSS> operator-(const Signal<S, BSS, TSS>& l, const Signal<S, BSS, TSS>& r);
 
-    template<typename BSS, typename TSS>
-    friend Signal<BSS, TSS> operator*(const double& l, const Signal<BSS, TSS>& r);
+    template<typename S, typename BSS, typename TSS>
+    friend Signal<S, BSS, TSS> operator*(const double& l, const Signal<S, BSS, TSS>& r);
 
-    template<typename BSS, typename TSS>
-    friend Signal<BSS, TSS> operator*(const Signal<BSS, TSS>& l, const double& r);
+    template<typename S, typename BSS, typename TSS>
+    friend Signal<S, BSS, TSS> operator*(const Signal<S, BSS, TSS>& l, const double& r);
 
     double t() const
     {
@@ -255,6 +255,26 @@ class Signal
         return true;
     }
 
+    static inline BaseType baseZero()
+    {
+        return BaseSignalSpec::ZeroType();
+    }
+
+    static inline TangentType tangentZero()
+    {
+        return TangentSignalSpec::ZeroType();
+    }
+
+    static inline T baseNorm(const BaseType& x)
+    {
+        return BaseSignalSpec::Norm(x);
+    }
+
+    static inline T tangentNorm(const TangentType& x)
+    {
+        return TangentSignalSpec::Norm(x);
+    }
+
 private:
     double                t_;
     BaseType              x_;
@@ -543,11 +563,11 @@ class Signal
     }
 };
 
-template<typename BaseSignalSpec, typename TangentSignalSpec>
-Signal<BaseSignalSpec, TangentSignalSpec> operator+(const Signal<BaseSignalSpec, TangentSignalSpec>&    l,
-                                                    const Signal<TangentSignalSpec, TangentSignalSpec>& r)
+template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
+Signal<T, BaseSignalSpec, TangentSignalSpec> operator+(const Signal<T, BaseSignalSpec, TangentSignalSpec>&    l,
+                                                       const Signal<T, TangentSignalSpec, TangentSignalSpec>& r)
 {
-    Signal<BaseSignalSpec, TangentSignalSpec> lpr = l;
+    Signal<T, BaseSignalSpec, TangentSignalSpec> lpr = l;
     lpr.x_ += r(l.t());
     lpr.xdot_ += r.dot(l.t());
     for (auto& signalDP : lpr.signalHistory_)
@@ -558,11 +578,11 @@ Signal<BaseSignalSpec, TangentSignalSpec> operator+(const Signal<BaseSignalSpec,
     return lpr;
 }
 
-template<typename BaseSignalSpec, typename TangentSignalSpec>
-Signal<TangentSignalSpec, TangentSignalSpec> operator-(const Signal<BaseSignalSpec, TangentSignalSpec>& l,
-                                                       const Signal<BaseSignalSpec, TangentSignalSpec>& r)
+template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
+Signal<T, TangentSignalSpec, TangentSignalSpec> operator-(const Signal<T, BaseSignalSpec, TangentSignalSpec>& l,
+                                                          const Signal<T, BaseSignalSpec, TangentSignalSpec>& r)
 {
-    Signal<TangentSignalSpec, TangentSignalSpec> lmr;
+    Signal<T, TangentSignalSpec, TangentSignalSpec> lmr;
     lmr.interpolationMethod = l.interpolationMethod;
     lmr.extrapolationMethod = l.extrapolationMethod;
     lmr.derivativeMethod    = l.derivativeMethod;
@@ -583,10 +603,11 @@ Signal<TangentSignalSpec, TangentSignalSpec> operator-(const Signal<BaseSignalSp
     return lmr;
 }
 
-template<typename BaseSignalSpec, typename TangentSignalSpec>
-Signal<BaseSignalSpec, TangentSignalSpec> operator*(const double& l, const Signal<BaseSignalSpec, TangentSignalSpec>& r)
+template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
+Signal<T, BaseSignalSpec, TangentSignalSpec> operator*(const double&                                       l,
+                                                       const Signal<T, BaseSignalSpec, TangentSignalSpec>& r)
 {
-    Signal<BaseSignalSpec, TangentSignalSpec> lr = r;
+    Signal<T, BaseSignalSpec, TangentSignalSpec> lr = r;
     lr.x_ *= l;
     lr.xdot_ *= l;
     for (auto& signalDP : lr.signalHistory_)
@@ -597,10 +618,11 @@ Signal<BaseSignalSpec, TangentSignalSpec> operator*(const double& l, const Signa
     return lr;
 }
 
-template<typename BaseSignalSpec, typename TangentSignalSpec>
-Signal<BaseSignalSpec, TangentSignalSpec> operator*(const Signal<BaseSignalSpec, TangentSignalSpec>& l, const double& r)
+template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
+Signal<T, BaseSignalSpec, TangentSignalSpec> operator*(const Signal<T, BaseSignalSpec, TangentSignalSpec>& l,
+                                                       const double&                                       r)
 {
-    Signal<BaseSignalSpec, TangentSignalSpec> lr = l;
+    Signal<T, BaseSignalSpec, TangentSignalSpec> lr = l;
     lr.x_ *= r;
     lr.xdot_ *= r;
     for (auto& signalDP : lr.signalHistory_)
@@ -623,6 +645,10 @@ struct ScalarSignalSpec
     {
         return (T)1. / 0.;
     }
+    static T Norm(const Type& a)
+    {
+        return a;
+    }
 };
 
 template<typename T, size_t d>
@@ -637,9 +663,13 @@ struct VectorSignalSpec
     {
         return Type::Zero(); // TODO fix
     }
+    static T Norm(const Type& a)
+    {
+        return a.norm();
+    }
 };
 
-template<typename ManifoldType>
+template<typename T, typename ManifoldType>
 struct ManifoldSignalSpec
 {
     using Type = ManifoldType;
@@ -651,10 +681,14 @@ struct ManifoldSignalSpec
     {
         return Type::identity(); // TODO fix
     }
+    static T Norm(const Type& a)
+    {
+        return ManifoldType::Log(a).norm();
+    }
 };
 
 template<typename T>
-using ScalarSignal = Signal<ScalarSignalSpec<T>, ScalarSignalSpec<T>>;
+using ScalarSignal = Signal<T, ScalarSignalSpec<T>, ScalarSignalSpec<T>>;
 
 template<typename T>
 inline std::ostream& operator<<(std::ostream& os, const ScalarSignal<T>& x)
@@ -664,7 +698,7 @@ inline std::ostream& operator<<(std::ostream& os, const ScalarSignal<T>& x)
 }
 
 template<typename T, size_t d>
-using VectorSignal = Signal<VectorSignalSpec<T, d>, VectorSignalSpec<T, d>>;
+using VectorSignal = Signal<T, VectorSignalSpec<T, d>, VectorSignalSpec<T, d>>;
 
 template<typename T, size_t d>
 inline std::ostream& operator<<(std::ostream& os, const VectorSignal<T, d>& x)
@@ -674,7 +708,7 @@ inline std::ostream& operator<<(std::ostream& os, const VectorSignal<T, d>& x)
 }
 
 template<typename T, typename ManifoldType, size_t d>
-using ManifoldSignal = Signal<ManifoldSignalSpec<ManifoldType>, VectorSignalSpec<T, d>>;
+using ManifoldSignal = Signal<T, ManifoldSignalSpec<T, ManifoldType>, VectorSignalSpec<T, d>>;
 
 template<typename T, typename ManifoldType, size_t d>
 inline std::ostream& operator<<(std::ostream& os, const ManifoldSignal<T, ManifoldType, d>& x)
diff --git a/include/signals/State.h b/include/signals/State.h
index 84008be..5569581 100644
--- a/include/signals/State.h
+++ b/include/signals/State.h
@@ -30,6 +30,13 @@ struct State
         return x;
     }
 
+    T norm() const
+    {
+        const T poseNorm  = PoseTypeSpec::Norm(pose);
+        const T twistNorm = TwistTypeSpec::Norm(twist);
+        return std::sqrt(poseNorm * poseNorm + twistNorm * twistNorm);
+    }
+
     State& operator*=(const double& s)
     {
         pose *= s;
@@ -121,7 +128,7 @@ VectorStateType<T, d> operator/(const VectorStateType<T, d>& l, const double& r)
 }
 
 template<typename T, typename ManifoldType, size_t PD, size_t TD>
-using ManifoldStateType = State<T, ManifoldSignalSpec<ManifoldType>, PD, VectorSignalSpec<T, TD>, TD>;
+using ManifoldStateType = State<T, ManifoldSignalSpec<T, ManifoldType>, PD, VectorSignalSpec<T, TD>, TD>;
 
 template<typename T, typename ManifoldType, size_t PD, size_t TD>
 inline std::ostream& operator<<(std::ostream& os, const ManifoldStateType<T, ManifoldType, PD, TD>& x)
@@ -173,6 +180,10 @@ struct ScalarStateSignalSpec
     {
         return Type::identity(); // TODO fix
     }
+    static T Norm(const Type& a)
+    {
+        return a.norm();
+    }
 };
 
 template<typename T, size_t d>
@@ -187,6 +198,10 @@ struct VectorStateSignalSpec
     {
         return Type::identity(); // TODO fix
     }
+    static T Norm(const Type& a)
+    {
+        return a.norm();
+    }
 };
 
 template<typename T, typename ManifoldType, size_t PD, size_t TD>
@@ -201,10 +216,14 @@ struct ManifoldStateSignalSpec
     {
         return Type::identity(); // TODO fix
     }
+    static T Norm(const Type& a)
+    {
+        return a.norm();
+    }
 };
 
 template<typename T>
-using ScalarStateSignal = Signal<ScalarStateSignalSpec<T>, ScalarStateSignalSpec<T>>;
+using ScalarStateSignal = Signal<T, ScalarStateSignalSpec<T>, ScalarStateSignalSpec<T>>;
 
 template<typename T>
 inline std::ostream& operator<<(std::ostream& os, const ScalarStateSignal<T>& x)
@@ -214,7 +233,7 @@ inline std::ostream& operator<<(std::ostream& os, const ScalarStateSignal<T>& x)
 }
 
 template<typename T, size_t d>
-using VectorStateSignal = Signal<VectorStateSignalSpec<T, d>, VectorStateSignalSpec<T, d>>;
+using VectorStateSignal = Signal<T, VectorStateSignalSpec<T, d>, VectorStateSignalSpec<T, d>>;
 
 template<typename T, size_t d>
 inline std::ostream& operator<<(std::ostream& os, const VectorStateSignal<T, d>& x)
@@ -225,7 +244,7 @@ inline std::ostream& operator<<(std::ostream& os, const VectorStateSignal<T, d>&
 }
 
 template<typename T, typename ManifoldType, size_t PD, size_t TD>
-using ManifoldStateSignal = Signal<ManifoldStateSignalSpec<T, ManifoldType, PD, TD>, VectorStateSignalSpec<T, TD>>;
+using ManifoldStateSignal = Signal<T, ManifoldStateSignalSpec<T, ManifoldType, PD, TD>, VectorStateSignalSpec<T, TD>>;
 
 template<typename T, typename ManifoldType, size_t PD, size_t TD>
 inline std::ostream& operator<<(std::ostream& os, const ManifoldStateSignal<T, ManifoldType, PD, TD>& x)
diff --git a/tests/SignalTest.cpp b/tests/SignalTest.cpp
index a63a03c..8a8b8c8 100644
--- a/tests/SignalTest.cpp
+++ b/tests/SignalTest.cpp
@@ -48,6 +48,14 @@ BOOST_AUTO_TEST_CASE(TestLinearInterpolation)
     BOOST_CHECK_CLOSE(q(-0.5).z(), 0., 1e-8);
 }
 
+BOOST_AUTO_TEST_CASE(TestNorms)
+{
+    SO3d     q = SO3d::identity();
+    Vector3d v(3., 4., 0.);
+    BOOST_CHECK_CLOSE(SO3dSignal::baseNorm(q), 0.0, 1e-8);
+    BOOST_CHECK_CLOSE(SO3dSignal::tangentNorm(v), 5.0, 1e-8);
+}
+
 BOOST_AUTO_TEST_CASE(TestDirtyDerivative)
 {
     Vector2dSignal v;