diff --git a/sw/airborne/fms/libeknav/basic_ins_qkf.cpp b/sw/airborne/fms/libeknav/basic_ins_qkf.cpp
index 28bed7af09..81cdaa259f 100644
--- a/sw/airborne/fms/libeknav/basic_ins_qkf.cpp
+++ b/sw/airborne/fms/libeknav/basic_ins_qkf.cpp
@@ -30,6 +30,7 @@ basic_ins_qkf::basic_ins_qkf(
 		const Vector3d& gyro_white_noise,
 		const Vector3d& gyro_stability_noise,
 		const Vector3d& accel_white_noise,
+		Quaterniond initial_orientation,		// this is new
 		const Vector3d& vel_estimate)
 	: gyro_stability_noise(gyro_stability_noise)
 	, gyro_white_noise(gyro_white_noise)
@@ -37,7 +38,8 @@ basic_ins_qkf::basic_ins_qkf(
 {
 	avg_state.position = estimate;
 	avg_state.gyro_bias = Vector3d::Zero();
-	avg_state.orientation = Quaterniond::Identity();
+	//avg_state.orientation = Quaterniond::Identity();
+	avg_state.orientation = initial_orientation;
 	avg_state.velocity = vel_estimate;
 
 	cov << Matrix3d::Identity()*bias_error*bias_error, Matrix<double, 3, 9>::Zero(),
diff --git a/sw/airborne/fms/libeknav/ins_qkf.hpp b/sw/airborne/fms/libeknav/ins_qkf.hpp
index 091880572e..09c5e866d3 100644
--- a/sw/airborne/fms/libeknav/ins_qkf.hpp
+++ b/sw/airborne/fms/libeknav/ins_qkf.hpp
@@ -20,7 +20,7 @@
  *  along with libeknav.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "sigma_points.hpp"
+//#include "sigma_points.hpp"
 #include "quaternions.hpp"
 #include <Eigen/StdVector>
 
diff --git a/sw/airborne/fms/libeknav/test_libeknav_2.cpp b/sw/airborne/fms/libeknav/test_libeknav_2.cpp
index b9305df183..1dad527784 100644
--- a/sw/airborne/fms/libeknav/test_libeknav_2.cpp
+++ b/sw/airborne/fms/libeknav/test_libeknav_2.cpp
@@ -20,7 +20,7 @@ int main(int, char *[]) {
   /* initial state */
   Vector3d pos_0_ecef(1017.67e3, -5079.282e3, 3709.041e3);
   Vector3d speed_0_ecef(0., 0., 0.);
-  //  Vector3d orientation(0., 0., 0.);
+    Quaterniond orientation(1., 0., 0., 0.);
   Vector3d bias_0(0., 0., 0.);
 
   /* initial covariance */
@@ -45,7 +45,7 @@ int main(int, char *[]) {
   Vector3d magnetometer = Vector3d::UnitZ();
 
   basic_ins_qkf ins(pos_0_ecef, pos_cov_0, bias_cov_0, speed_cov_0,
-		    gyro_white_noise, gyro_stability_noise, accel_white_noise);
+		    gyro_white_noise, gyro_stability_noise, accel_white_noise,orientation);
 
   const double dt = 1./512.; /* predict at 512Hz */
   for (int i=1; i<5000; i++) {