diff --git a/sw/airborne/fms/libeknav/ins_qkf.hpp b/sw/airborne/fms/libeknav/ins_qkf.hpp
index 9f1edec48d..091880572e 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>
 
@@ -155,13 +155,23 @@ struct basic_ins_qkf
 	 * @param gyro_stability_noise The diagonal matrix of gyro instability noise
 	 * @param accel_white_noise The diagonal matrix of accelerometer white noise
 	 */
+			//Old one without orientation_init()
+	basic_ins_qkf(const Vector3d& pos_estimate,
+			double pos_error, double bias_error, double v_error,
+			const Vector3d& gyro_white_noise,
+			const Vector3d& gyro_stability_noise,
+			const Vector3d& accel_white_noise,
+			Quaterniond initial_orientation = Quaterniond::Identity(),
+			const Vector3d& vel_estimate = Vector3d::Zero());
+	
+	/*		//Old one without orientation_init()
 	basic_ins_qkf(const Vector3d& pos_estimate,
 			double pos_error, double bias_error, double v_error,
 			const Vector3d& gyro_white_noise,
 			const Vector3d& gyro_stability_noise,
 			const Vector3d& accel_white_noise,
 			const Vector3d& vel_estimate = Vector3d::Zero());
-
+	*/
 	/**
 	 * Report an INS observation, to propagate the filter forward by one time
 	 * step. The coordinate system is maintained in ECEF coordinates.
@@ -237,7 +247,6 @@ struct basic_ins_qkf
 	 */
 	void obs_gyro_bias(const Vector3d& bias, const Vector3d& bias_error);
 
-
 	/**
 	 * Measure the total angular error between the filter's attitude estimate
 	 * and some other orientation.
diff --git a/sw/airborne/fms/libeknav/ins_qkf_predict.cpp b/sw/airborne/fms/libeknav/ins_qkf_predict.cpp
index 83b830dc50..307f766425 100644
--- a/sw/airborne/fms/libeknav/ins_qkf_predict.cpp
+++ b/sw/airborne/fms/libeknav/ins_qkf_predict.cpp
@@ -19,11 +19,18 @@
  *  along with libeknav.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+/**
+ * This is a modified version by Martin Dieblich
+ * The general changes are a more consequent naming
+ * 
+ */
+
+
 #include "ins_qkf.hpp"
 #include "assertions.hpp"
+#include "timer.hpp"
 
 #ifdef TIME_OPS
-#include "timer.hpp"
 # include <iostream>
 #endif
 
@@ -51,13 +58,13 @@ linear_predict(basic_ins_qkf& _this, const Vector3d& gyro_meas, const Vector3d&
 	// zero out the error vector in the z direction.
 	// accel_cov is the relationship between error vectors in the tangent space
 	// of the vehicle orientation and the translational reference frame.
-	Vector3d accel_body = _this.avg_state.orientation.conjugate()*accel_meas;		// accel_body is in ECEF!
-	std::cout << "   ACCEL_BODY(" << accel_body.transpose() << ")\n";
-	Vector3d accel_gravity = _this.avg_state.position.normalized()*9.81;
-	std::cout << "   ACCEL_GRAVITY(" << accel_gravity.transpose() << ")\n";
-	//Vector3d accel_resid = accel_body - accel_gravity;
-	Vector3d accel_resid = accel_body + accel_gravity;								// This is better!
-	std::cout << "   ACCEL_RESID(" << accel_resid.transpose() << ")\n";
+	
+	Matrix3d rot = _this.avg_state.orientation.conjugate().toRotationMatrix();
+	
+	Vector3d accel_ecef = rot*accel_meas;		// a_e = (q_e2b)^* x a_b = q_b2e x a_b
+	Vector3d accel_gravity = _this.avg_state.position.normalized()*(-9.81);         
+	Vector3d accel_resid = accel_ecef + accel_gravity;								// a = (xdd-g)+g = xdd;
+	
 #if 0
 	// This form works well with zero static acceleration.
 	Matrix<double, 3, 3> accel_cov =
@@ -65,12 +72,12 @@ linear_predict(basic_ins_qkf& _this, const Vector3d& gyro_meas, const Vector3d&
 		* axis_scale(_this.avg_state.position.normalized(), 0) * 9.81;
 #elif 1
 	Matrix<double, 3, 3> accel_cov =
-		Eigen::AngleAxisd(-M_PI*0.5, accel_body.normalized())
-		* axis_scale(accel_body.normalized(), 0) * accel_meas.norm();
+		Eigen::AngleAxisd(-M_PI*0.5, accel_ecef.normalized())
+		* axis_scale(accel_ecef.normalized(), 0) * accel_meas.norm();
 #else
 	// The following form ends up being identical to the simpler one
 	// above
-	Matrix<double, 3, 3> accel_cov = 
+	Matrix<double, 3, 3> accel_cov =
 		Eigen::AngleAxisd(-M_PI*0.5, _this.avg_state.position.normalized())
 		* axis_scale(_this.avg_state.position.normalized(), 0) * 9.81
 		+ Eigen::AngleAxisd(-M_PI*0.5, accel_resid.normalized())
@@ -134,9 +141,10 @@ linear_predict(basic_ins_qkf& _this, const Vector3d& gyro_meas, const Vector3d&
 
 	Quaterniond orientation = exp<double>((gyro_meas - _this.avg_state.gyro_bias) * dt)
 			* _this.avg_state.orientation;
-	Vector3d accel = accel_body - _this.avg_state.position.normalized() * 9.81;
-	Vector3d position = _this.avg_state.position + _this.avg_state.velocity * dt + 0.5*accel*dt*dt;
-	Vector3d velocity = _this.avg_state.velocity + accel*dt;
+	//Vector3d accel = accel_ecef - _this.avg_state.position.normalized() * 9.81;
+	//std::cout << "   ACCEL______(" << accel.transpose() << ")\n";
+	Vector3d position = _this.avg_state.position + _this.avg_state.velocity * dt + 0.5*accel_resid*dt*dt;
+	Vector3d velocity = _this.avg_state.velocity + accel_resid*dt;
 
 	_this.avg_state.position = position;
 	_this.avg_state.velocity = velocity;