Changeset 197 in flair-src


Ignore:
Timestamp:
Sep 13, 2017, 4:51:39 PM (7 years ago)
Author:
Sanahuja Guillaume
Message:

use magneto

Location:
trunk/lib/FlairFilter/src
Files:
2 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairFilter/src/AhrsComplementaryFilter.cpp

    r167 r197  
    5151    kb[1]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"kb[1]:",0.,10,0.1,2,0.01);
    5252    kb[2]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"kb[2]:",0.,10,0.1,2,0.01);
     53   
     54    km[0]=new DoubleSpinBox(parent->GetLayout()->NewRow(),"km[0]:",0.,10,0.1,2,0.01);
     55    km[1]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"km[1]:",0.,10,0.1,2,0.01);
     56    km[2]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"km[2]:",0.,10,0.1,2,0.01);
    5357 
    5458  SetIsReady(true);
    … …  
    7276    Vector3Df aBar,aHat,aTilde;
    7377    Vector3Df mBar,mHat,mTilde;
    74     Vector3Df alpha,dBHat,omegaHat;
     78    Vector3Df alpha,dBHat,omegaHat,magRef;
    7579    Quaternion dQHat;
    7680
    7781    //float ka[3]={0.5,0.5,0};
    7882    //float kb[3]={0.01,0.01,0.01};
    79     float k_m[3]= {0,0,0};
     83    //float k_m[3]= {0,0,0};
     84magRef.x=20.8296;
     85magRef.y=.2095;
     86magRef.z=43.3333;
    8087
    8188    if(isInit==true) {
    82         // CORRECTION FORM ACCELEROMETER
     89        // CORRECTION FROM ACCELEROMETER
    8390        aBar = rawAcc;
    8491
    … …  
    97104        aTilde=CrossProduct(aHat, aBar);
    98105
    99         // CORRECTION FROM FICTIOUS MAGNETOMETER (to avoid drift of yaw)
    100         // estimation of IMU vector using QHat (estimated quaternion): mHat = Inv(QHat) * m_ref = Inv(QHat) * [1.0, 0.0, 0.0]'
     106       
     107        // CORRECTION FROM MAGNETOMETER
     108        // estimation of IMU vector using QHat (estimated quaternion): mHat = Inv(QHat) * m_ref = Inv(QHat) * magRef
    101109        /*
    102110          Inv(QHat) = [q0 -q1 -q2 -q3]'
    103           Inv(QHat)*g = [1-2(q2^2+q3^2)     2(q1q2+q0q3)     2(q1q3-q0q2)]     [1]
    104                          [  2(q1q2-q0q3)   1-2(q1^2+q3^2))    2(q2q3+q0q1)]  *  [0]
    105                          [  2(q1q3+q0q2)     2(q2q3-q0q1)   1-2(q1^2+q2^2)]     [0]
     111          Inv(QHat)*g = [1-2(q2^2+q3^2)     2(q1q2+q0q3)     2(q1q3-q0q2)]     [magRef.x]
     112                         [  2(q1q2-q0q3)   1-2(q1^2+q3^2))    2(q2q3+q0q1)]  *  [magRef.x]
     113                         [  2(q1q3+q0q2)     2(q2q3-q0q1)   1-2(q1^2+q2^2)]     [magRef.x]
    106114        */
    107         mBar.x=1;
    108         mBar.y=0;
    109         mBar.z=0;
    110 
    111         mHat.x = (1-2.0*(QHat.q2*QHat.q2 + QHat.q3*QHat.q3));
    112         mHat.y = 2.0*(QHat.q1*QHat.q2 - QHat.q0*QHat.q3);
    113         mHat.z = 2.0*(QHat.q1*QHat.q3 + QHat.q0*QHat.q2);
    114 
     115        mBar=rawMag;
     116       
     117        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;
     118        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;
     119        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;
     120       
     121       
    115122        // compute the error between mHat and mTilde
    116123        mTilde=CrossProduct(mHat, mBar);
    … …  
    120127
    121128        // calculate the correction to apply to the quaternion
    122         alpha.x = (ka[0]->Value()*aTilde.x)/(G*G) + (k_m[0]*mTilde.x);
    123         alpha.y = (ka[1]->Value()*aTilde.y)/(G*G) + (k_m[1]*mTilde.y);
    124         alpha.z = (ka[2]->Value()*aTilde.z)/(G*G) + (k_m[2]*mTilde.z);
     129        alpha.x = (ka[0]->Value()*aTilde.x)/(G*G) + (km[0]->Value()*mTilde.x)/(magRef.GetNorm()*magRef.GetNorm());
     130        alpha.y = (ka[1]->Value()*aTilde.y)/(G*G) + (km[1]->Value()*mTilde.y)/(magRef.GetNorm()*magRef.GetNorm());
     131        alpha.z = (ka[2]->Value()*aTilde.z)/(G*G) + (km[2]->Value()*mTilde.z)/(magRef.GetNorm()*magRef.GetNorm());
    125132
    126133        // Bias derivative

  • trunk/lib/FlairFilter/src/AhrsComplementaryFilter.h

    r167 r197  
    6969            gui::DoubleSpinBox *ka[3];
    7070            gui::DoubleSpinBox *kb[3];
     71            gui::DoubleSpinBox *km[3];
    7172    };
    7273} // end namespace filter
Note: See TracChangeset for help on using the changeset viewer.