diff --git a/catkin_ws/src/smargon/src/nudge.cpp b/catkin_ws/src/smargon/src/nudge.cpp
index dd3a687..6f63324 100644
--- a/catkin_ws/src/smargon/src/nudge.cpp
+++ b/catkin_ws/src/smargon/src/nudge.cpp
@@ -29,8 +29,10 @@ double OMEGA, CHI, PHI;
 double OX, OY, OZ;
 
 double NX, NY, NZ;
+//Orientation tweak parameters:
 double offsetOMEGA = 0.0;
 double FPRX=0.0, FPRY=0.0, FPRZ=0.0;
+//double FPRX=-90.0, FPRY=0.0, FPRZ=13.0; //Vespa orientation
 
 // Commonly used functions ////////////////////////////////////////////////////
 int calculateSH () {
@@ -65,16 +67,27 @@ int calculateSH () {
 
 int calculateO () {
 
-	double oldOX, oldOY, oldOZ, omega;
+	double oldOX, oldOY, oldOZ, omega, offsetOmega, fprx, fpry, fprz;
 	double nudgeOX, nudgeOY, nudgeOZ;
 	//Get existing SH values:
 	oldOX = OX; oldOY = OY; oldOZ = OZ;
 
 	//calculate the nudge vector [NX,NY,NZ] in SH coordinates:
  	omega = OMEGA/180*M_PI;
-	nudgeOX = NY*cos(omega) - NZ*sin(omega);
-	nudgeOY = NY*sin(omega) + NZ*cos(omega);
-	nudgeOZ = NX;
+	offsetOmega = offsetOMEGA/180*M_PI;
+	fprx = FPRX/180*M_PI;
+	fpry = FPRY/180*M_PI;
+	fprz = FPRZ/180*M_PI;
+
+	//This is the simplified formula without fprx,fpry,frpz, offsetOmega
+	//nudgeOX = NY*cos(omega) - NZ*sin(omega);
+	//nudgeOY = NY*sin(omega) + NZ*cos(omega);
+	//nudgeOZ = NX;
+
+	//This is the formula that takes in account fprx,fpry,frpz and offsetOmega
+        nudgeOX = NX*(sin(omega + offsetOmega)*sin(fpry) + cos(omega + offsetOmega)*cos(fpry)*sin(fprz)) + NY*(cos(omega + offsetOmega)*cos(fprx)*cos(fprz) - sin(omega + offsetOmega)*cos(fpry)*sin(fprx) + cos(omega + offsetOmega)*sin(fprx)*sin(fpry)*sin(fprz))+ NZ*(cos(omega + offsetOmega)*cos(fprx)*sin(fpry)*sin(fprz) - cos(omega + offsetOmega)*cos(fprz)*sin(fprx) - sin(omega + offsetOmega)*cos(fprx)*cos(fpry));
+        nudgeOY = NX*(sin(omega + offsetOmega)*cos(fpry)*sin(fprz) - cos(omega + offsetOmega)*sin(fpry)) + NY*(cos(omega + offsetOmega)*cos(fpry)*sin(fprx) + sin(omega + offsetOmega)*cos(fprx)*cos(fprz) + sin(omega + offsetOmega)*sin(fprx)*sin(fpry)*sin(fprz))+ NZ*(cos(omega + offsetOmega)*cos(fprx)*cos(fpry) - sin(omega + offsetOmega)*cos(fprz)*sin(fprx) + sin(omega + offsetOmega)*cos(fprx)*sin(fpry)*sin(fprz));
+        nudgeOZ = NX*cos(fpry)*cos(fprz) + NY*(cos(fprz)*sin(fprx)*sin(fpry) - cos(fprx)*sin(fprz)) + NZ*(sin(fprx)*sin(fprz) + cos(fprx)*cos(fprz)*sin(fpry));
 
 	//add the nudge vecor to the existing SH position
 	OX = oldOX + nudgeOX;