diff --git a/CMakeLists.txt b/CMakeLists.txt
index 14a8e7602..44015de3e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -114,13 +114,13 @@ endif()
 
 option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
 # Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library)
-set(GTSAM_BOOST_LIBRARIES ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY})
+set(GTSAM_BOOST_LIBRARIES ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY})
 if (GTSAM_DISABLE_NEW_TIMERS)
     message("WARNING:  GTSAM timing instrumentation manually disabled")
     add_definitions(-DGTSAM_DISABLE_NEW_TIMERS)
 else()
     if(Boost_TIMER_LIBRARY)
-      list(APPEND GTSAM_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES} ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY})
+      list(APPEND GTSAM_BOOST_LIBRARIES ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY})
     else()
       message("WARNING:  Boost older than 1.48 was found, GTSAM timing instrumentation will use the older, less accurate, Boost timer library.")
     endif()
@@ -174,16 +174,19 @@ endforeach()
 ###############################################################################
 # Global compile options
 
-# General build settings
+# Include boost - use 'BEFORE' so that a specific boost specified to CMake
+# takes precedence over a system-installed one.
+include_directories(BEFORE ${TBB_INCLUDE_DIRS} ${Boost_INCLUDE_DIR})
+
+# Add includes for source directories 'BEFORE' boost and any system include
+# paths so that the compiler uses GTSAM headers in our source directory instead
+# of any previously installed GTSAM headers.
 include_directories(BEFORE
   gtsam/3rdparty/UFconfig 
   gtsam/3rdparty/CCOLAMD/Include
   ${CMAKE_SOURCE_DIR}
   ${CMAKE_BINARY_DIR} # So we can include generated config header files
-  CppUnitLite
-  ${TBB_INCLUDE_DIRS}
-  ${Boost_INCLUDE_DIR})
-link_directories(${Boost_LIBRARY_DIRS})
+  CppUnitLite)
 
 if(MSVC)
 	add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS)
@@ -291,9 +294,9 @@ print_config_flag(${GTSAM_BUILD_TESTS}                 "Build Tests
 if (DOXYGEN_FOUND)
     print_config_flag(${GTSAM_BUILD_DOCS}                  "Build Docs                     ")
 endif()
+print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY}        "Build shared GTSAM Library     ")
+print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY}        "Build static GTSAM Library     ")
 if(NOT MSVC)
-	print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY}        "Build shared GTSAM Library     ")
-	print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY}        "Build static GTSAM Library     ")
 	print_config_flag(${GTSAM_BUILD_CONVENIENCE_LIBRARIES} "Build Convenience Libraries    ")
 endif()
 print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES}        "Put build-type in library name ")
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 6e283ae72..384c55a77 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -71,8 +71,6 @@ foreach(subdir ${gtsam_subdirs})
     add_subdirectory(${subdir})
 endforeach(subdir)
  
-message(STATUS "navigation_srcs: [${navigation_srcs}]")
- 
 # To add additional sources to gtsam when building the full library (static or shared)
 # Add the subfolder with _srcs appended to the end to this list
 set(gtsam_srcs
diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp
index c32621ca2..4a4730e60 100644
--- a/gtsam/linear/KalmanFilter.cpp
+++ b/gtsam/linear/KalmanFilter.cpp
@@ -24,6 +24,7 @@
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/KalmanFilter.h>
 #include <gtsam/linear/HessianFactor.h>
+#include <gtsam/inference/Permutation.h>
 #include <gtsam/base/Testable.h>
 
 #include <boost/make_shared.hpp>
@@ -33,16 +34,31 @@ namespace gtsam {
   using namespace std;
 
   /// Auxiliary function to solve factor graph and return pointer to root conditional
-  KalmanFilter::State solve(const GaussianFactorGraph& factorGraph,
-      bool useQR) {
+  KalmanFilter::State solve(const GaussianFactorGraph& factorGraph, bool useQR)
+  {
+    // Start indices at zero
+    Index nVars = 0;
+    internal::Reduction remapping;
+    BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, factorGraph)
+      BOOST_FOREACH(Index j, *factor)
+        if(remapping.insert(make_pair(j, nVars)).second)
+          ++ nVars;
+    Permutation inverseRemapping = remapping.inverse();
+    GaussianFactorGraph factorGraphOrdered(factorGraph); // NOTE this shares the factors with the original!!
+    factorGraphOrdered.reduceWithInverse(remapping);
 
     // Solve the factor graph
-    GaussianSequentialSolver solver(factorGraph, useQR);
+    GaussianSequentialSolver solver(factorGraphOrdered, useQR);
     GaussianBayesNet::shared_ptr bayesNet = solver.eliminate();
 
     // As this is a filter, all we need is the posterior P(x_t),
     // so we just keep the root of the Bayes net
     GaussianConditional::shared_ptr conditional = bayesNet->back();
+
+    // Undo the remapping
+    factorGraphOrdered.permuteWithInverse(inverseRemapping);
+    conditional->permuteWithInverse(inverseRemapping);
+
     // TODO: awful ! A copy constructor followed by ANOTHER copy constructor in make_shared?
     return boost::make_shared<GaussianDensity>(*conditional);
   }
diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt
index 39fcaccc1..f59208ef6 100644
--- a/gtsam_unstable/CMakeLists.txt
+++ b/gtsam_unstable/CMakeLists.txt
@@ -10,7 +10,7 @@ set (gtsam_unstable_subdirs
     slam
 )
 
-set(GTSAM_UNSTABLE_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES} ${Boost_THREAD_LIBRARY})
+set(GTSAM_UNSTABLE_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES})
 
 add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)