diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp
index 6e413f846..68e6a00f1 100644
--- a/gtsam/linear/KalmanFilter.cpp
+++ b/gtsam/linear/KalmanFilter.cpp
@@ -53,7 +53,7 @@ KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {
 /* ************************************************************************* */
 // Auxiliary function to create a small graph for predict or update and solve
 KalmanFilter::State //
-KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) {
+KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const {
 
   // Create a factor graph
   GaussianFactorGraph factorGraph;
@@ -65,7 +65,7 @@ KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) {
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::init(const Vector& x0,
-    const SharedDiagonal& P0) {
+    const SharedDiagonal& P0) const {
 
   // Create a factor graph f(x0), eliminate it into P(x0)
   GaussianFactorGraph factorGraph;
@@ -74,7 +74,7 @@ KalmanFilter::State KalmanFilter::init(const Vector& x0,
 }
 
 /* ************************************************************************* */
-KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) {
+KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) const {
 
   // Create a factor graph f(x0), eliminate it into P(x0)
   GaussianFactorGraph factorGraph;
@@ -89,7 +89,7 @@ void KalmanFilter::print(const string& s) const {
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F,
-    const Matrix& B, const Vector& u, const SharedDiagonal& model) {
+    const Matrix& B, const Vector& u, const SharedDiagonal& model) const {
 
   // The factor related to the motion model is defined as
   // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
@@ -100,7 +100,7 @@ KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F,
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
-    const Matrix& B, const Vector& u, const Matrix& Q) {
+    const Matrix& B, const Vector& u, const Matrix& Q) const {
 
 #ifndef NDEBUG
   DenseIndex n = F.cols();
@@ -126,7 +126,7 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0,
-    const Matrix& A1, const Vector& b, const SharedDiagonal& model) {
+    const Matrix& A1, const Vector& b, const SharedDiagonal& model) const {
   // Nhe factor related to the motion model is defined as
   // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2
   Key k = step(p);
@@ -135,7 +135,7 @@ KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0,
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
-    const Vector& z, const SharedDiagonal& model) {
+    const Vector& z, const SharedDiagonal& model) const {
   // The factor related to the measurements would be defined as
   // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T
   //    = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T
@@ -145,7 +145,7 @@ KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
-    const Vector& z, const Matrix& Q) {
+    const Vector& z, const Matrix& Q) const {
   Key k = step(p);
   Matrix M = inverse(Q), Ht = trans(H);
   Matrix G = Ht * M * H;
diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h
index 7c738d639..63271401c 100644
--- a/gtsam/linear/KalmanFilter.h
+++ b/gtsam/linear/KalmanFilter.h
@@ -62,7 +62,7 @@ private:
   const GaussianFactorGraph::Eliminate function_; /** algorithm */
 
   State solve(const GaussianFactorGraph& factorGraph) const;
-  State fuse(const State& p, GaussianFactor::shared_ptr newFactor);
+  State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const;
 
 public:
 
@@ -80,10 +80,10 @@ public:
    * @param x0 estimate at time 0
    * @param P0 covariance at time 0, given as a diagonal Gaussian 'model'
    */
-  State init(const Vector& x0, const SharedDiagonal& P0);
+  State init(const Vector& x0, const SharedDiagonal& P0) const;
 
   /// version of init with a full covariance matrix
-  State init(const Vector& x0, const Matrix& P0);
+  State init(const Vector& x0, const Matrix& P0) const;
 
   /// print
   void print(const std::string& s = "") const;
@@ -102,7 +102,7 @@ public:
    *   and w is zero-mean, Gaussian white noise with covariance Q.
    */
   State predict(const State& p, const Matrix& F, const Matrix& B,
-      const Vector& u, const SharedDiagonal& modelQ);
+      const Vector& u, const SharedDiagonal& modelQ) const;
 
   /*
    *  Version of predict with full covariance
@@ -111,7 +111,7 @@ public:
    *  This version allows more realistic models than a diagonal covariance matrix.
    */
   State predictQ(const State& p, const Matrix& F, const Matrix& B,
-      const Vector& u, const Matrix& Q);
+      const Vector& u, const Matrix& Q) const;
 
   /**
    * Predict the state P(x_{t+1}|Z^t)
@@ -122,7 +122,7 @@ public:
    *   with an optional noise model.
    */
   State predict2(const State& p, const Matrix& A0, const Matrix& A1,
-      const Vector& b, const SharedDiagonal& model);
+      const Vector& b, const SharedDiagonal& model) const;
 
   /**
    * Update Kalman filter with a measurement
@@ -133,7 +133,7 @@ public:
    * In this version, R is restricted to diagonal Gaussians (model parameter)
    */
   State update(const State& p, const Matrix& H, const Vector& z,
-      const SharedDiagonal& model);
+      const SharedDiagonal& model) const;
 
   /*
    *  Version of update with full covariance
@@ -142,7 +142,7 @@ public:
    *  This version allows more realistic models than a diagonal covariance matrix.
    */
   State updateQ(const State& p, const Matrix& H, const Vector& z,
-      const Matrix& Q);
+      const Matrix& Q) const;
 };
 
 } // \namespace gtsam