diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp
index de4a2959f..324b054a9 100644
--- a/gtsam/linear/KalmanFilter.cpp
+++ b/gtsam/linear/KalmanFilter.cpp
@@ -32,7 +32,7 @@ namespace gtsam {
 	GaussianConditional* solve(GaussianFactorGraph& factorGraph) {
 
 		// Solve the factor graph
-		const bool useQR = true; // make sure we use QR (numerically stable)
+		const bool useQR = false; // make sure we use QR (numerically stable)
 		GaussianSequentialSolver solver(factorGraph, useQR);
 		GaussianBayesNet::shared_ptr bayesNet = solver.eliminate();
 
@@ -124,32 +124,19 @@ namespace gtsam {
 		// 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
 		// See documentation in HessianFactor, we have A1 = -F,  A2 = I_, b = B*u:
-		Vector b = B*u;
-		Matrix M = inverse(Q);
-		Matrix Ft = trans(F);
-		Matrix G12 = -Ft*M;
-		Matrix G11 = -G12*F;
-		Matrix G22 = M;
-		Vector g2 = M*b;
-		Vector g1 = -Ft*g2;
+		// TODO: starts to seem more elaborate than straight-up KF equations?
+		Matrix M = inverse(Q), Ft = trans(F);
+		Matrix G12 = -Ft*M, G11 = -G12*F, G22 = M;
+		Vector b = B*u, g2 = M*b, g1 = -Ft*g2;
 		double f = dot(b,g2);
 		HessianFactor::shared_ptr factor(new HessianFactor(0, 1, G11, G12, g1, G22, g2, f));
-
-//#define DEBUG_PREDICTQ
-#ifdef DEBUG_PREDICTQ
-		gtsam::print(b);
-		gtsam::print(M);
-		gtsam::print(G11);
-		gtsam::print(G12);
-		gtsam::print(G22);
-		gtsam::print(g1);
-		gtsam::print(g2);
-		cout << f << endl;
-		factor->print("factor");
-#endif
-
 		factorGraph.push_back(factor);
 
+#ifdef DEBUG_PREDICTQ
+    Matrix AbtAb = factorGraph.denseHessian();
+		gtsam::print(AbtAb);
+#endif
+
 		// Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
 		density_.reset(solve(factorGraph));
 	}
@@ -165,13 +152,10 @@ namespace gtsam {
 		// However, now the factor related to the motion model is defined as
 		// f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2
 		factorGraph.add(0, A0, 1, A1, b, model);
-#ifdef DEBUG_PREDICTQ
-		gtsam::print(Matrix(trans(A0)*A0));
-		gtsam::print(Matrix(trans(A0)*A1));
-		gtsam::print(Matrix(trans(A1)*A1));
-		gtsam::print(Matrix(trans(A0)*b));
-		gtsam::print(Matrix(trans(A1)*b));
-		cout << dot(b,b) << endl;
+
+		#ifdef DEBUG_PREDICTQ
+    Matrix AbtAb = factorGraph.denseHessian();
+		gtsam::print(AbtAb);
 #endif
 
 		density_.reset(solve(factorGraph));
diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp
index 281c8ac7e..db4464d32 100644
--- a/gtsam/linear/tests/testKalmanFilter.cpp
+++ b/gtsam/linear/tests/testKalmanFilter.cpp
@@ -104,8 +104,8 @@ TEST( KalmanFilter, linear1 ) {
 	EXPECT(assert_equal(expected1,kalmanFilter.mean()));
 	EXPECT(assert_equal(I11,kalmanFilter.information()));
 
-	// Run iteration 2
-	kalmanFilter.predict(F, B, u, modelQ);
+	// Run iteration 2 (with full covariance)
+	kalmanFilter.predictQ(F, B, u, Q);
 	EXPECT(assert_equal(expected2,kalmanFilter.mean()));
 	kalmanFilter.update(H,z2,modelR);
 	EXPECT(assert_equal(expected2,kalmanFilter.mean()));
@@ -117,6 +117,36 @@ TEST( KalmanFilter, linear1 ) {
 	EXPECT(assert_equal(expected3,kalmanFilter.mean()));
 }
 
+/* ************************************************************************* */
+TEST( KalmanFilter, predict ) {
+
+	// Create dynamics model
+	Matrix F = Matrix_(2,2, 1.0,0.1, 0.2,1.1);
+	Matrix B = Matrix_(2,3, 1.0,0.1,0.2, 1.1,1.2,0.8);
+	Vector u = Vector_(3, 1.0, 0.0, 2.0);
+	Matrix R = Matrix_(2,2, 1.0,0.0, 0.0,3.0);
+	Matrix M = trans(R)*R;
+	Matrix Q = inverse(M);
+
+	// Create the Kalman Filter initialization point
+	State x_initial(0.0,0.0);
+	SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2,1);
+
+	// Create two KalmanFilter objects
+	KalmanFilter kalmanFilter1(x_initial, P_initial);
+	KalmanFilter kalmanFilter2(x_initial, P_initial);
+
+	// Ensure predictQ and predict2 give same answer for non-trivial inputs
+	kalmanFilter1.predictQ(F, B, u, Q);
+	// We have A1 = -F,  A2 = I_, b = B*u:
+	Matrix A1 = -R*F, A2 = R;
+	Vector b = R*B*u;
+	SharedDiagonal nop = noiseModel::Isotropic::Sigma(2, 1.0);
+	kalmanFilter2.predict2(A1,A2,b,nop);
+	EXPECT(assert_equal(kalmanFilter1.mean(),kalmanFilter2.mean()));
+	EXPECT(assert_equal(kalmanFilter1.covariance(),kalmanFilter2.covariance()));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;