Index: /trunk/lib/FlairFilter/src/AhrsComplementaryFilter.cpp
===================================================================
--- /trunk/lib/FlairFilter/src/AhrsComplementaryFilter.cpp	(revision 196)
+++ /trunk/lib/FlairFilter/src/AhrsComplementaryFilter.cpp	(revision 197)
@@ -51,4 +51,8 @@
     kb[1]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"kb[1]:",0.,10,0.1,2,0.01);
     kb[2]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"kb[2]:",0.,10,0.1,2,0.01);
+    
+    km[0]=new DoubleSpinBox(parent->GetLayout()->NewRow(),"km[0]:",0.,10,0.1,2,0.01);
+    km[1]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"km[1]:",0.,10,0.1,2,0.01);
+    km[2]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"km[2]:",0.,10,0.1,2,0.01);
   
   SetIsReady(true);
@@ -72,13 +76,16 @@
     Vector3Df aBar,aHat,aTilde;
     Vector3Df mBar,mHat,mTilde;
-    Vector3Df alpha,dBHat,omegaHat;
+    Vector3Df alpha,dBHat,omegaHat,magRef;
     Quaternion dQHat;
 
     //float ka[3]={0.5,0.5,0};
     //float kb[3]={0.01,0.01,0.01};
-    float k_m[3]= {0,0,0};
+    //float k_m[3]= {0,0,0};
+magRef.x=20.8296;
+magRef.y=.2095;
+magRef.z=43.3333;
 
     if(isInit==true) {
-        // CORRECTION FORM ACCELEROMETER
+        // CORRECTION FROM ACCELEROMETER
         aBar = rawAcc;
 
@@ -97,20 +104,20 @@
         aTilde=CrossProduct(aHat, aBar);
 
-        // CORRECTION FROM FICTIOUS MAGNETOMETER (to avoid drift of yaw)
-        // estimation of IMU vector using QHat (estimated quaternion): mHat = Inv(QHat) * m_ref = Inv(QHat) * [1.0, 0.0, 0.0]'
+        
+        // CORRECTION FROM MAGNETOMETER
+        // estimation of IMU vector using QHat (estimated quaternion): mHat = Inv(QHat) * m_ref = Inv(QHat) * magRef
         /*
           Inv(QHat) = [q0 -q1 -q2 -q3]'
-          Inv(QHat)*g = [1-2(q2^2+q3^2)     2(q1q2+q0q3)     2(q1q3-q0q2)]     [1]
-                         [  2(q1q2-q0q3)   1-2(q1^2+q3^2))    2(q2q3+q0q1)]  *  [0]
-                         [  2(q1q3+q0q2)     2(q2q3-q0q1)   1-2(q1^2+q2^2)]     [0]
+          Inv(QHat)*g = [1-2(q2^2+q3^2)     2(q1q2+q0q3)     2(q1q3-q0q2)]     [magRef.x]
+                         [  2(q1q2-q0q3)   1-2(q1^2+q3^2))    2(q2q3+q0q1)]  *  [magRef.x]
+                         [  2(q1q3+q0q2)     2(q2q3-q0q1)   1-2(q1^2+q2^2)]     [magRef.x]
         */
-        mBar.x=1;
-        mBar.y=0;
-        mBar.z=0;
-
-        mHat.x = (1-2.0*(QHat.q2*QHat.q2 + QHat.q3*QHat.q3));
-        mHat.y = 2.0*(QHat.q1*QHat.q2 - QHat.q0*QHat.q3);
-        mHat.z = 2.0*(QHat.q1*QHat.q3 + QHat.q0*QHat.q2);
-
+        mBar=rawMag;
+        
+        mHat.x = (1-2.0*(QHat.q2*QHat.q2 + QHat.q3*QHat.q3))*magRef.x+2.0*(QHat.q1*QHat.q2+QHat.q0*QHat.q3)*magRef.y+2.0*(QHat.q1*QHat.q3-QHat.q0*QHat.q2)*magRef.z;
+        mHat.y = 2.0*(QHat.q1*QHat.q2 - QHat.q0*QHat.q3)*magRef.x+(1-2.0*(QHat.q1*QHat.q1 + QHat.q3*QHat.q3))*magRef.y+2.0*(QHat.q2*QHat.q3 + QHat.q0*QHat.q1)*magRef.z;
+        mHat.z = 2.0*(QHat.q1*QHat.q3+QHat.q0*QHat.q2)*magRef.x+2.0*(QHat.q2*QHat.q3 - QHat.q0*QHat.q1)*magRef.y+(1-2.0*(QHat.q1*QHat.q1 + QHat.q2*QHat.q2))*magRef.z;
+       
+        
         // compute the error between mHat and mTilde
         mTilde=CrossProduct(mHat, mBar);
@@ -120,7 +127,7 @@
 
         // calculate the correction to apply to the quaternion
-        alpha.x = (ka[0]->Value()*aTilde.x)/(G*G) + (k_m[0]*mTilde.x);
-        alpha.y = (ka[1]->Value()*aTilde.y)/(G*G) + (k_m[1]*mTilde.y);
-        alpha.z = (ka[2]->Value()*aTilde.z)/(G*G) + (k_m[2]*mTilde.z);
+        alpha.x = (ka[0]->Value()*aTilde.x)/(G*G) + (km[0]->Value()*mTilde.x)/(magRef.GetNorm()*magRef.GetNorm());
+        alpha.y = (ka[1]->Value()*aTilde.y)/(G*G) + (km[1]->Value()*mTilde.y)/(magRef.GetNorm()*magRef.GetNorm());
+        alpha.z = (ka[2]->Value()*aTilde.z)/(G*G) + (km[2]->Value()*mTilde.z)/(magRef.GetNorm()*magRef.GetNorm());
 
         // Bias derivative
Index: /trunk/lib/FlairFilter/src/AhrsComplementaryFilter.h
===================================================================
--- /trunk/lib/FlairFilter/src/AhrsComplementaryFilter.h	(revision 196)
+++ /trunk/lib/FlairFilter/src/AhrsComplementaryFilter.h	(revision 197)
@@ -69,4 +69,5 @@
             gui::DoubleSpinBox *ka[3];
             gui::DoubleSpinBox *kb[3];
+            gui::DoubleSpinBox *km[3];
     };
 } // end namespace filter
