diff --git a/src/stateestimation/DroneKalmanFilter.cpp b/src/stateestimation/DroneKalmanFilter.cpp
index e5eb7d3..6c99e6d 100644
--- a/src/stateestimation/DroneKalmanFilter.cpp
+++ b/src/stateestimation/DroneKalmanFilter.cpp
@@ -363,13 +363,13 @@ void DroneKalmanFilter::observeIMU_RPY(const ardrone_autonomy::Navdata* nav)
 
 	if(!baselinesYValid)	// only for initialization.
 	{
-		baselineY_IMU = nav->rotZ;
+		baselineY_IMU = nav->rotZ - node->initialYaw;
 		baselineY_Filter = yaw.state[0];
 		baselinesYValid = true;
 		timestampYawBaselineFrom = getMS(nav->header.stamp);
 	}
 
-	double imuYawDiff = (nav->rotZ - baselineY_IMU );
+	double imuYawDiff = (nav->rotZ - node->initialYaw - baselineY_IMU );
 	double observedYaw = baselineY_Filter + imuYawDiff;
 
 	yaw.state[0] =  angleFromTo(yaw.state[0],-180,180);
diff --git a/src/stateestimation/EstimationNode.cpp b/src/stateestimation/EstimationNode.cpp
index fcce315..15a1af6 100644
--- a/src/stateestimation/EstimationNode.cpp
+++ b/src/stateestimation/EstimationNode.cpp
@@ -104,6 +104,7 @@ EstimationNode::EstimationNode()
 	arDroneVersion = 0;
 	//memset(&lastNavdataReceived,0,sizeof(ardrone_autonomy::Navdata));
 
+    initialYaw = 0;
 
 }
 
@@ -239,8 +240,19 @@ void EstimationNode::comCb(const std_msgs::StringConstPtr str)
 		this->toogleLogging();
 	}
 
+	// hack to handle the setinitialstate command
 
+		// only handle commands with prefix c
+	if(str->data.length() > 2 && str->data.substr(0,2) == "c ")
+	{
 
+		float parameter;
+		// setInitialYaw
+		if(sscanf(str->data.c_str(),"c setInitialYaw %f",&parameter) == 1)
+		{
+			initialYaw = parameter;
+		}
+	}
 
 
 	int a, b;
diff --git a/src/stateestimation/EstimationNode.h b/src/stateestimation/EstimationNode.h
index d3eb741..ae22693 100644
--- a/src/stateestimation/EstimationNode.h
+++ b/src/stateestimation/EstimationNode.h
@@ -20,7 +20,7 @@
  */
 #ifndef __ESTIMATIONNODE_H
 #define __ESTIMATIONNODE_H
- 
+
 
 #include "ros/ros.h"
 #include "geometry_msgs/Twist.h"
@@ -134,7 +134,7 @@ struct EstimationNode
 	void toogleLogging();	// switches logging on or off.
 	std::string calibFile;
 	int arDroneVersion;
-
+    float initialYaw;
 
 };
 #endif /* __ESTIMATIONNODE_H */