diff --git a/include/kalman/SquareRootExtendedKalmanFilter.hpp b/include/kalman/SquareRootExtendedKalmanFilter.hpp
index 2f876af..a05df2a 100644
--- a/include/kalman/SquareRootExtendedKalmanFilter.hpp
+++ b/include/kalman/SquareRootExtendedKalmanFilter.hpp
@@ -208,7 +208,7 @@ namespace Kalman {
         template<class Type>
         bool computePredictedCovarianceSquareRoot(  const Jacobian<Type, State>& A, const CovarianceSquareRoot<State>& S,
                                                     const Jacobian<Type, Type>& B,  const CovarianceSquareRoot<Type>& R,
-                                                    CovarianceSquareRoot<Type>& S_pred)
+                                                    CovarianceSquareRoot<Type>& S_pred) const
         {
             // Compute QR decomposition of (transposed) augmented matrix
             Matrix<T, State::RowsAtCompileTime + Type::RowsAtCompileTime, Type::RowsAtCompileTime> tmp;
@@ -251,7 +251,7 @@ namespace Kalman {
         template<class Measurement>
         bool computeKalmanGain( const Jacobian<Measurement, State>& H,
                                 const CovarianceSquareRoot<Measurement>& S_y,
-                                KalmanGain<Measurement>& K)
+                                KalmanGain<Measurement>& K) const
         {
             // Solve using backsubstitution
             // AX=B with B = HSS^T and X = K^T and A = S_yS_y^T
diff --git a/include/kalman/SquareRootUnscentedKalmanFilter.hpp b/include/kalman/SquareRootUnscentedKalmanFilter.hpp
index f236267..7b61d04 100644
--- a/include/kalman/SquareRootUnscentedKalmanFilter.hpp
+++ b/include/kalman/SquareRootUnscentedKalmanFilter.hpp
@@ -221,7 +221,7 @@ namespace Kalman {
          */
         template<class Type>
         bool computeCovarianceSquareRootFromSigmaPoints(const Type& mean, const SigmaPoints<Type>& sigmaPoints, 
-                                                        const CovarianceSquareRoot<Type>& noiseCov, CovarianceSquareRoot<Type>& cov)
+                                                        const CovarianceSquareRoot<Type>& noiseCov, CovarianceSquareRoot<Type>& cov) const
         {
             // Compute QR decomposition of (transposed) augmented matrix
             Matrix<T, 2*State::RowsAtCompileTime + Type::RowsAtCompileTime, Type::RowsAtCompileTime> tmp;
@@ -272,7 +272,7 @@ namespace Kalman {
         bool computeKalmanGain( const Measurement& y,
                                 const SigmaPoints<Measurement>& sigmaMeasurementPoints,
                                 const CovarianceSquareRoot<Measurement>& S_y,
-                                KalmanGain<Measurement>& K)
+                                KalmanGain<Measurement>& K) const
         {
             // Note: The intermediate eval() is needed here (for now) due to a bug in Eigen that occurs
             // when Measurement::RowsAtCompileTime == 1 AND State::RowsAtCompileTime >= 8
diff --git a/include/kalman/UnscentedKalmanFilter.hpp b/include/kalman/UnscentedKalmanFilter.hpp
index cd13a34..39612dd 100644
--- a/include/kalman/UnscentedKalmanFilter.hpp
+++ b/include/kalman/UnscentedKalmanFilter.hpp
@@ -218,7 +218,7 @@ namespace Kalman {
          */
         template<class Type>
         bool computeCovarianceFromSigmaPoints(  const Type& mean, const SigmaPoints<Type>& sigmaPoints, 
-                                                const Covariance<Type>& noiseCov, Covariance<Type>& cov)
+                                                const Covariance<Type>& noiseCov, Covariance<Type>& cov) const
         {
             decltype(sigmaPoints) W = this->sigmaWeights_c.transpose().template replicate<Type::RowsAtCompileTime,1>();
             decltype(sigmaPoints) tmp = (sigmaPoints.colwise() - mean);
@@ -241,7 +241,7 @@ namespace Kalman {
         bool computeKalmanGain( const Measurement& y,
                                 const SigmaPoints<Measurement>& sigmaMeasurementPoints,
                                 const Covariance<Measurement>& P_yy,
-                                KalmanGain<Measurement>& K)
+                                KalmanGain<Measurement>& K) const
         {
             // Note: The intermediate eval() is needed here (for now) due to a bug in Eigen that occurs
             // when Measurement::RowsAtCompileTime == 1 AND State::RowsAtCompileTime >= 8
diff --git a/include/kalman/UnscentedKalmanFilterBase.hpp b/include/kalman/UnscentedKalmanFilterBase.hpp
index 25e7fb2..51c65c6 100644
--- a/include/kalman/UnscentedKalmanFilterBase.hpp
+++ b/include/kalman/UnscentedKalmanFilterBase.hpp
@@ -137,7 +137,8 @@ namespace Kalman {
          * @return The predicted measurement
          */
         template<class Measurement, template<class> class CovarianceBase>
-        Measurement computeMeasurementPrediction(const MeasurementModelType<Measurement, CovarianceBase>& m, SigmaPoints<Measurement>& sigmaMeasurementPoints)
+        Measurement computeMeasurementPrediction(const MeasurementModelType<Measurement, CovarianceBase>& m,
+                                                 SigmaPoints<Measurement>& sigmaMeasurementPoints) const
         {
             // Predict measurements for each sigma point
             computeSigmaPointMeasurements<Measurement>(m, sigmaMeasurementPoints);
@@ -204,7 +205,8 @@ namespace Kalman {
          * @param [out] sigmaMeasurementPoints The struct of expected sigma measurements to be computed
          */
         template<class Measurement, template<class> class CovarianceBase>
-        void computeSigmaPointMeasurements(const MeasurementModelType<Measurement, CovarianceBase>& m, SigmaPoints<Measurement>& sigmaMeasurementPoints)
+        void computeSigmaPointMeasurements(const MeasurementModelType<Measurement, CovarianceBase>& m,
+                                           SigmaPoints<Measurement>& sigmaMeasurementPoints) const
         {
             for( int i = 0; i < SigmaPointCount; ++i )
             {
@@ -221,7 +223,7 @@ namespace Kalman {
          * @return The prediction
          */
         template<class Type>
-        Type computePredictionFromSigmaPoints(const SigmaPoints<Type>& sigmaPoints)
+        Type computePredictionFromSigmaPoints(const SigmaPoints<Type>& sigmaPoints) const
         {
             // Use efficient matrix x vector computation
             return sigmaPoints * sigmaWeights_m;