1 files changed, 5 insertions, 3 deletions
diff --git a/src/state.cpp b/src/state.cpp
index cba336e..cbf0e15 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -215,7 +215,7 @@ State::partialUpdate( const MatrixXd &h, const MatrixXd &S,
 {
     MatrixXd K;
     // P^T is implied here since P is symmetric
-    K = S.fullPivLu().solve(h*P).transpose();
+    K = S.fullPivHouseholderQr().solve(h*P).transpose();
 
     // Compute the innovation or error
     Matrix<double,Dynamic,1> y;
@@ -1111,8 +1111,10 @@ State::featuresAsMeasurements ( vector<measurement_t> &yk, const Camera &cam,
         // Get ellipse
         Matrix<double,6,6> s;
         s = (*i)->S(Pxx(), Pxy((*i)->id()), Pyy((*i)->id()), body->ned(), q);
-        z.Ssrc << s(0,0), s(1,1);
-        z.Sref << s(4,4), s(5,5);
+	z.Ssrc[0] = s(0,0);
+	z.Ssrc[1] = s(1,1);
+	z.Sref[0] = s(4,4);
+	z.Sref[1] = s(5,5);
 
         yk.push_back(z);
     }