Software Gimbals for your compass

Posted on
  • Software Gimbals for your compass

    Introduction

    Hardware gimbals have been used to compensate for the rockling of a boat any many other applications. Here is a brief glimpse:
    https://en.wikipedia.org/wiki/Gimbal
    The idea for this project is to use the three axis readings from an accelerometer to provide a software gimbal for a three axis magnetometer.
    Several math techniques can be used to perform this task, such as rotation matrices, quaternions, and the one implemented here the Rodrigues Rotation Formula.
    https://en.wikipedia.org/wiki/Rotation_f­ormalisms_in_three_dimensions
    https://en.wikipedia.org/wiki/Rotation_m­atrix
    https://en.wikipedia.org/wiki/Quaternion­s_and_spatial_rotation
    https://en.wikipedia.org/wiki/Rodrigues%­27_rotation_formula
    https://en.wikipedia.org/wiki/Rotations_­in_4-dimensional_Euclidean_space#The_Eul­er.E2.80.93Rodrigues_formula_for_3D_rota­tions

    The module Rodrigues.js

    The module Rodrigues creates an instance containing the following parameters:

    /** Define the vector rotation object*/
    function Rodrigues(){
    //magnitudes of vectors
     this.Ra=0;
     this.Rb=0;
     this.Rk=0;
    //unit vectors
     this.An=[0,0,0];
     this.Bn=[0,0,0];
     this.Kn=[0,0,0];//the rotation axis
    // vectors
     this.K=[0,0,0];
    //Scalars
     this.theta=0;//rotation angle in degrees
     this.ct=0;// cos(theta)
     this.st=0;//sin(theta)
    //Result vectors
     this.F=[0,0,0]; // a unit vector
     this.FF=[0,0,0];// scaled vector
    }
    

    Functions in Rodrigues:

    There are two functions in the Rodrigues module.
    • Setup(A,B)
    The vectors A and B are the arguments and the function performs the following task.

    1. Calculate the magnitudes and unit vectors from vectors A and B, giving
      Ra,Rb, An and Bn.
    2. Calculate K the vector cross product of A and B, and the magnitude Rk and the unit vector Kn
    3. Determines the angle theta between vectors A and B, the sin st and cos cs.
    4. Rotates vector A so it points the same direction as vector B
    5. Returns the norm of the rotated vector A, the scaled rotated vector is contained in FF
      • Having called setup to relate the two frames of reference, the function D=FF.RotVector(C) can be called to rotate vector C about the axis Kn by angle theta.

      Usage.

      The following is an example

    var pirate=180/Math.PI;
    var FF;
    var A,An,Kn,Theta,B,C ,D,E,F;
    function test(){
     FF=require("Rodrigues").connect();
    //A=[-0.70710678118,-0.70710678118,0];
    A=[0.70710678118,0.70710678118,0];
    B=[0,0,1];
    console.log("A= ",',',A);
    console.log("B= ",',',B);
    C=FF.Setup(A,B);
    console.log("C= ",',',C,',',FF.theta);
    console.log("Kn= ",',',FF.Kn);
    console.log("ct= ",',',FF.ct,','," st= ",',',FF.st);
    D=FF.RotVector(C);
    console.log("D= ",',',D);  
    E=FF.RotVector(D);
    console.log("E= ",',',E);  
    F=FF.RotVector(E);
    console.log("F= ",',',F);  
    }
    

    First attempts to gimbal a magnetometer

    //testRodrigues1.js
    // 3 Jan 2017
    /** Use the Rodrigues Formula and data to level a compass */
    //The data are from a LSM9DS1 IMU chip
    //The Y axis is pointing roughly North
    //Readings with the XY plane level, X down and then X up
    //The output shows the accelerometer data Z pointing down
    //and the magnetometer data leveled
    
    /*
    https://en.wikipedia.org/wiki/Rodrigues%­27_rotation_formula
    */
    var pirate=180/Math.PI;
    var FF;
    function test(){
     FF=require("Rodrigues").connect();
     var i;
    // negate X axis for fun
     for(i=0;i<Adata.length;i++){
      Adata[i][0]=-Adata[i][0];
      Mdata[i][0]=-Mdata[i][0];
     }
    for(i=0;i<Adata.length;i++){
    console.log(i,',',FF.Setup(Adata[i],[0,0­,1]),',',FF.theta,',',FF.RotVector(Mdata­[i]),',',FF.Kn);
    }//next i
    }
    
    setTimeout(function () {
      test();
    }, 1000);
    
    //Accelerometer data
    var Adata=[
    [-0.049159472,  -0.001571133 ,  0.944969535  , ], 
    [-0.035622872,  -0.009391533 ,  0.968284963  , ], 
    [-0.039314672,  -0.006627933 ,  0.981773731  , ], 
    [-0.034509472,  -0.005157933 ,  0.977928516  , ], 
    ];
    
    //Magnetometer data
    var Mdata=[
    [-0.004785037,0.214759059,-0.35996205, ], 
    [-0.002195637,0.217274343,-0.35459097, ], 
    [-0.006668237,0.216994867,-0.36106068, ], 
    [-0.012317837,0.218112771,-0.35825307, ], 
    ];
    

    Poor results more is needed

    One issue is the inversion problem, which can be demonstrated using paper, pen and paper clip. On the paper write North at the top, South at the bottom, East on the right and West on the left. Turn the paper over left to right (not top to bottom). On the paper write North at the top, South at the bottom, East on the left and West on the right.
    Clip the paper clip to one corner. This represents the North magnetic pole position relative to our paper compass. Now flip the paper over, left to right or top to bottom and note that the paperclip heading changes.
    The software needs to level the compass and then detect if the compass is inverted and then flip the readings by 180 degrees along the rotation axis Kn.
    Results to follow.


    1 Attachment

  • Software Gimbals

    Numerical Gotchas

    The code doesn't like zero magnitude vectors. The current module version detects these and returns a zero vector [0,0,0]. These can occur when the arguments to the functions are zero vectors.

    An additional case occurs when the K vector is zero. This occurs when the cross product of the arguments to the Setup function is a zero vector.
    Let A= [0,0,1] and B= [0,0,1] Setup(A,B) returns [0,0,0].

    I'm considering a flag that if true would cause the zero K vector to be detected and a small dither values to each axis of on of the arguments and start the function over again.

    Sorting Out Issues Theory vs Application

    My first approach was to collect data from an IMU and apply the Rodrigues rotation formula. Some difficulty getting this to work has led to a different approach.

    • Find a small cardboard box and draw the various axis on the sides of the box with a magic marker, along with positive direction arrows.
    • For a given heading and dip angle calculate theoretical values for the magnetometer readings. Use the cardboard box on each of it's six sides to apply the theroretical values to the axis fot that orientation. The direction of the axis detemine the sign of the value.
    • Assuming positive results using the theoretical values, obtain data from an IMU and compare it to the theoretical values and take appropriate action to produce positive results.
    • First results of this proceedure:

    //Accelerometer data
    var Adata=[
    [0.00001,0,1],
    [-1,0,0.00001],
    [0.00001,0,-1],
    [1,0,0.00001],
    
    [0,0.00001,1],
    [0,-1,0.00001],
    [0,0.00001,-1],
    [0,1,0.00001],
    ];
    

    Note use of 0.00001 to avoid the zero value K vector problem previously disscussed
    and

    //Magnetometer data
    var Mdata=[
    
    [0.866,0.5,1.732 ],
    [-1.732,0.5,0.866],  
    [-0.866,0.5,-1.732 ],
    [1.732,0.5,-0.866],  
    
    [0.5,0.866,1.732 ],
    [0.5,-1.732,0.866],  
    [0.5,-0.866,-1.732 ],
    [0.5,1.732,-0.866],  
    
    ];
    

    The Rodrigues' Rotation Formula was applied to this data and produced:
    N= sample
    A= accelerometer vector
    K= roatation axis vctor

    N Aroll Apitch Ayaw Kroll Kpitch Kroll
    0.00 0.00 0.00 1.00 0.00 -1.00 0.00
    1.00 -1.00 0.00 0.00 0.00 1.00 0.00
    2.00 0.00 0.00 -1.00 0.00 -1.00 0.00
    3.00 1.00 0.00 0.00 0.00 -1.00 0.00
    4.00 0.00 0.00 1.00 1.00 0.00 0.00
    5.00 0.00 -1.00 0.00 -1.00 0.00 0.00
    6.00 0.00 0.00 -1.00 1.00 0.00 0.00
    7.00 0.00 1.00 0.00 1.00 0.00 0.00

    and
    Normed vectors
    M= test magnetometer data
    M1= transformed magnetometer data
    and Scalars
    Tilt degrees
    Heading degrees
    Dip degrees

    Mroll Mpitch Myaw M1roll M1pitch M1yaw Tiltdeg Heading Dip
    1.0.43 0.25 0.87 0.43 0.25 0.87 0.00 330.00 60.00
    -0.87 0.25 0.43 0.43 0.25 0.87 90.00 330.00 60.00
    -0.43 0.25 -0.87 0.43 0.25 0.87 0.00 330.00 60.00
    0.87 0.25 -0.43 0.43 0.25 0.87 90.00 330.00 60.00
    0.25 0.43 0.87 0.25 0.43 0.87 0.00 300.00 60.00
    0.25 -0.87 0.43 0.25 0.43 0.87 90.00 300.00 60.00
    0.25 -0.43 -0.87 0.25 0.43 0.87 0.00 300.00 60.00
    0.25 0.87 -0.43 0.25 0.43 0.87 90.00 300.00 60.00

    A sucessful test so far!
    The code follows:

    function test(){
     FF=require("Rodrigues").connect();
     var i;
     for(i=0;i<Adata.length;i++){
    // for(i=0;i<4;i++){
    
     A=FF.Setup(Adata[i],[0,0,1]);
       Theta=FF.theta;
       Kn=FF.Kn;
     A=FF.An;
     B=FF.RotVector(Mdata[i]);
       An=FF.An;
     heading=pirate*Math.atan2(-B[1],B[0]);
     if(heading<0)heading+=360;
     dip=pirate*Math.atan(B[2]/Math.sqrt(B[0]­*B[0]+B[1]*B[1]));
    console.log(i,',',A,',',Kn,',',An,',',B,­',',
     Theta,',',heading,',',dip);
    }//next i
    }
    
  • Software Gimbals

    Comparison of theoretical and actual accelerometer readings

    N Troll Tpitch Tyaw Aroll Apitch Ayaw
    0 0.00001 0 1 -0.003021269 -0.041954586 0.999114951
    1 -1 0 0.00001 -0.997628186 -0.025433701 0.063961937
    2 0.00001 0 -1 -0.045201791 0.000406194 -0.998977794
    3 1 0.00001 1 0.993574107 -0.033400851 -0.108142854

    T = Theory, A= Accelerometer data
    Notice the signs match.

    Comparison of theoretical and actual magnetometer readings

    N Troll Tpitch Tyaw Mroll Mpitch Myaw
    0 0.866 0.5 1.732 0.167571285 0.470716976 -0.866224793
    1 -1.732 0.5 0.866 -0.865231458 0.404036655 -0.296865129
    2 -0.866 0.5 -1.732 -0.163571745 0.52435959 0.835638262
    3 1.732 0.5 -0.866 0.735023669 0.362766964 0.572835348

    T=Theory, M= Magnetometer data
    Notice that the Z axis signs don’t’match.

    Results using the sign change on the Z-axis data on data.

    Data are from a LSM9DS1 IMU.

    N Aroll Apitch Ayaw Kroll Kpitch Kyaw Mroll Mpitch Myaw M1roll M1pitch M1yaw tilt heading dip
    0 -0.003 -0.042 0.999 -0.997 0.072 0.000 0.168 0.471 0.866 0.170 0.507 0.845 2.411 288.565 57.694
    1 -0.998 -0.025 0.064 -0.025 1.000 0.000 -0.865 0.404 0.297 0.231 0.432 0.872 86.333 298.100 60.679
    2 -0.045 0.000 -0.999 0.009 1.000 0.000 -0.164 0.524 -0.836 0.135 0.522 0.842 -2.591 284.511 57.393
    3 0.994 -0.033 -0.108 -0.034 -0.999 0.000 0.735 0.363 -0.573 0.504 0.371 0.780 -83.792 323.682 51.273

    A= Accelerometer Data
    K= Rotation Axis from Rodrigues Rotation Formula
    M= Magnetometer Data
    M1= Transformed Magnetometer Data
    Tilt= angle from Rodrigues
    Heading and Dip angles in degrees calculated from transformed magnetometer data.

    Conclusion

    This data set shows the transformation of real world accelerometer and magnetometer data to achieve the target of a software gimbaled compass. It is a limited data set and needs further testing to confirm the results. (This stuff is like a Rubic’s cube!)

  • Rodrigues Rotation Formula Module Numerical Issues

    The origin of the issues is a divide by zero problem

    Resolution of these issues must apply to both the Setup() and RotVector() functions.
    The divide by zero problem arises when the Norm of a vector is computed.
    Given vector V the Norm(V) is given by V/ sqrt(V dot V). If the dot product is zero then a division by zero will occur. The Norm is also called the unit vector and the components of the unit vector are the direction cosines.

    The Rodrigues’ formula requires four different vectors to be Normed:
    • The vector A an argument to the Setup(A,B) function,
    • The vector B an argument to the Setup(A,B) function,
    • The vector C an argument to the RotVector(C) function, and
    • The vector K used in the Setup(A,B) function.

      What should the Rodrigues formula return in each case?
    • Zero vectors as arguments.
      A zero vector has the components [0, 0, 0] and its dot product is zero.
      If vectors A and/or B are zero then Setup(A,B) should return the Norm of vector A. The RotVector(C) should return the Norm of vector C.

    • The K vector is the cross product of vectors A and B. K is a zero vector if

    • Vector A and/or B is a zero vector. Return the Norm of A.

    • Vectors A and B point in the same direction. Return the Norm of A.

    • Vectors A and B point in opposite directions. Return the -Norm of A.

    These modifications have been implemented in the attached Rodrigues.js file.
    This update functions with the previous test program testRodrigues1.js and with a new test file TestRodrigues10jan17.js

    Files attached:
    //testRodrigues1.js
    //TestRodrigues10jan17.js
    //Rodrigues10jan17.js as module //Rodrigues.js


    3 Attachments

  • Software Gimbaled Compass

    The data were acquired using the 12 element calibration of a LSM9DS1 IMU

    The 12 element calibration was obtained using a collection of raw data points taken at many different orientations, and processed using am Excel spreadsheet and the solver function to fit the data to a spherical surface. This method is discussed further in the following conversation link. (note a long cable was used to minimize the IMU from the magnetic fields of the table and computer)
    http://forum.espruino.com/conversations/­298287/
    The following code was used to apply the calibrations:

    //Use 12 element calibration on raw count magnetometer data
    function cal8G(S){
     var i;
     var mm=[0,0,0];
     var scale=8;
     var mres=[
         [0.970972836,0.171154738,-0.000344029],
         [-0.095223453,0.956653319,-0.000125418],­
         [0.017179224,0.005190024,1],
         ];
     var moff=[1057.176483,120.7727841,549.551664­];
     for(i=0;i<3;i++) S.m[i] -=moff[i];
     for(i=0;i<3;i++)
      mm[i]=S.m[0]*mres[i][0]+S.m[1]*mres[i][1­]+S.m[2]*mres[i][2];
      for(i=0;i<3;i++)S.m[i]=mm[i]*scale/32768­;
    }//end cal8G(S)
    function readall(W){
      var heading,dip;
      var pirate=180/Math.PI;
    if(W.magAvailable()){
      W.readMag();
      W.readAccel();
      cal8G(W);
      W.m[2]=-W.m[2];
    heading=pirate*Math.atan2(W.m[0],-W.m[1]­);
    if(heading<0)heading +=360;
    dip=pirate*Math.atan(W.m[2]/Math.sqrt(W.­m[0]*W.m[0]+W.m[1]*W.m[1]));
    console.log(W.a,',',W.m,',',heading,',',­dip);
    }//endif
    }//end readall
    

    The dataset

    The following dataset was obtained using the above calibration with the IMU approximately level starting at 0 and proceeding by approximate 45 degree steps.
    (360, 315, 270 ,225 ,180 ,135 ,90 ,45)

    ax ay az mx my mz heading dip
    0.005221328 0.038883267 0.965721486 -0.017482972 -0.152165638 0.420466182 353.4457781 69.98441706
    0.053742128 0.060580467 0.998802538 0.008982245 -0.163318019 0.411716107 3.148009986 68.333274
    0.045889728 0.061932867 1.00154912 0.004620515 -0.160749845 0.421310071 1.646428435 69.10788548
    0.086792528 0.069459267 0.973839161 0.009863154 -0.162452582 0.419266816 3.474393718 68.78469698
    0.065286328 0.019067667 0.963829397 -0.073864097 -0.105995351 0.43284815 325.1288376 73.38107377
    0.122655728 0.030180867 0.982750293 -0.064172881 -0.109797951 0.430729882 329.6952783 73.5503905
    0.090484328 0.042881667 0.982628223 -0.064828213 -0.112110436 0.429526044 329.9612166 73.22163288
    0.113982928 0.052818867 0.973839161 -0.061649354 -0.118602017 0.424600681 332.5345796 72.5255674
    0.088491928 0.053877267 0.991661425 -0.11020607 -0.019251924 0.432085705 279.9090234 75.48383928
    0.075717128 0.041294067 0.982750293 -0.108910915 -0.016052224 0.429125695 278.3843782 75.61167923
    0.088609128 0.061932867 0.987999316 -0.112078203 -0.016217852 0.423323378 278.2336196 75.02319545
    0.073373128 0.057581667 0.966453908 -0.105814424 -0.010888918 0.434674039 275.875387 76.24886152
    0.110994328 0.038648067 0.951927543 -0.033200414 0.097015611 0.435331124 198.8918302 76.74596353
    0.092652528 0.022419267 0.965721486 -0.040103186 0.098880808 0.435206472 202.0760467 76.22399812
    0.073490328 0.031886067 0.982994434 -0.038122585 0.091081115 0.432258614 202.7120608 77.13311516
    0.090367128 0.019302867 0.952232718 -0.042207576 0.093858834 0.4357436 204.2130729 76.71158587
    -0.023316872 0.080160867 0.985924121 -0.015551534 0.110731677 0.43034586 187.994526 75.43470381
    -0.021148672 0.067577667 0.980675098 -0.015549808 0.106691785 0.433040365 188.2921962 76.01869954
    -0.038845872 0.075339267 0.973656056 -0.01500844 0.109727365 0.427652799 187.7885496 75.48110246
    -0.045819272 0.058287267 0.963463186 -0.021450093 0.110121715 0.429476292 191.0223588 75.35988506
    0.124589528 0.022713267 0.963829397 0.145392752 0.077359058 0.413377119 118.0160836 68.27734928
    0.138946528 0.005249667 0.934837701 0.160771714 0.068957421 0.404331212 113.2152255 66.60400467
    0.133613928 -0.020328333 0.927330377 0.16788097 0.076340165 0.401990206 114.4526189 65.35553348
    0.188404928 -0.017505933 0.954918265 0.177739414 0.073471827 0.40011101 112.4586468 64.32722696
    -0.098676472 -0.003393933 1.005028123 0.093650207 0.005196927 0.424718089 93.17625478 77.54674095
    -0.021090072 -0.030912333 0.975548146 0.103647753 0.004453889 0.423075824 92.46056617 76.22222377
    -0.038787272 -0.009861933 0.991112109 0.100384753 -0.005683938 0.417053226 86.75928569 76.4454667
    -0.064454072 -0.001747533 0.942222954 0.100278956 -0.021119951 0.424901951 78.10664105 76.44022583
    -0.021617472 -0.017564733 1.003990526 0.020143978 -0.104996777 0.423349642 10.86041872 75.8270026
    -0.021793272 -0.080892333 0.974083302 0.030669343 -0.092007227 0.423620563 18.43509874 77.10486344
    0.019871328 -0.071307933 0.95082891 0.039384256 -0.090485263 0.423949378 23.52137433 76.89631367
    0.003053128 -0.116231133 0.946007133 0.051640347 -0.086696446 0.42372145 30.77995151 76.60433025

    Using the Dip Angle Statistic to Determine Axis Signs

    The data were then processed using the Rodrigues.js module into datasets where various combinations of the signs of the X, Y , and Z axis were manipulated. The following table shows the statistics of the Dip angle as the signs of the X, Y amd Z data were changed. Compare the Dip Range

    Statistic Raw x,y,z x,y,-z x,-y,-z
    Mean 73.68802652 -73.39285194 73.27452467 72.29268725
    Standard Error 0.683692412 1.227276358 0.898702904 0.23150967
    Median 75.48247087 -75.00020203 73.83858428 72.20170648
    Mode #N/A #N/A #N/A #N/A
    Standard Deviation 3.867548323 6.94252348 5.083831342 1.309616458
    Sample Variance 14.95793003 48.19863227 25.84534112 1.715095267
    Kurtosis 0.028460278 1.561952363 -0.906235772 -0.460864552
    Skewness -1.151317713 1.323796746 0.029464285 0.276014742
    Range 13.21951399 29.10003759 17.46765933 4.930872778
    Minimum 64.32722696 -83.15527605 64.49625552 70.01846639
    Maximum 77.54674095 -54.05523846 81.96391485 74.94933917
    Sum 2358.016849 -2348.571262 2344.784789 2313.365992
    Count 32 32 32 32

    Code used to apply the Rodrigues.js module

    The follwing code was used to apply the Rodrigues.js module.

    function test(){
     FF=require("Rodrigues").connect();
     var i;
     for(i=0;i<Adata.length;i++){
    // for(i=0;i<4;i++){
    
     A=FF.Setup(Adata[i],[0,0,1]);
       Theta=FF.theta;
       Kn=FF.Kn;
     A=FF.An;
    // Mdata[i][0]=-Mdata[i][0];
    // Positive x, negate y and z to minimize dip range
    // z was negated in acquisition program
     Mdata[i][1]=-Mdata[i][1];
     B=FF.RotVector(Mdata[i]);
       An=FF.An;
    heading=pirate*Math.atan2(B[1],B[0]);
    if(heading<0)heading +=360;
    dip=pirate*Math.atan(B[2]/Math.sqrt(B[0]­*B[0]+B[1]*B[1]));
    console.log(i,',',A,',',Kn,',',An,',',B,­',',
     Theta,',',heading,',',dip);
    }//next i
    }
    

    The Results

    The final output using positive X and negative Y and Z follows:
    ( ais accelerometer, k is rotation axis, m raw data, m1 is processed data; vectors have been Normed to unit vectors)

    N ax ay az kx ky kz mx my mz m1x m1y m1z Tilt Heading Dip
    0 0.005 0.040 0.999 0.991 -0.133 0.000 -0.039 0.340 0.940 -0.044 0.302 0.952 2.326 351.676 72.231
    1 0.054 0.060 0.997 0.748 -0.664 0.000 0.020 0.369 0.929 -0.030 0.312 0.950 4.635 354.468 71.747
    2 0.046 0.062 0.997 0.803 -0.595 0.000 0.010 0.356 0.934 -0.033 0.298 0.954 4.401 353.694 72.543
    3 0.089 0.071 0.994 0.625 -0.781 0.000 0.022 0.361 0.932 -0.062 0.294 0.954 6.512 348.128 72.507
    4 0.068 0.020 0.998 0.280 -0.960 0.000 -0.164 0.235 0.958 -0.228 0.216 0.949 4.036 313.420 71.701
    5 0.124 0.030 0.992 0.239 -0.971 0.000 -0.143 0.244 0.959 -0.261 0.215 0.941 7.324 309.538 70.221
    6 0.092 0.043 0.995 0.428 -0.904 0.000 -0.145 0.250 0.957 -0.232 0.208 0.950 5.818 311.918 71.825
    7 0.116 0.054 0.992 0.420 -0.907 0.000 -0.138 0.266 0.954 -0.249 0.215 0.944 7.351 310.819 70.781
    8 0.089 0.054 0.995 0.520 -0.854 0.000 -0.247 0.043 0.968 -0.332 -0.009 0.943 5.964 268.508 70.605
    9 0.077 0.042 0.996 0.479 -0.878 0.000 -0.246 0.036 0.969 -0.320 -0.004 0.948 5.015 269.292 71.365
    10 0.089 0.062 0.994 0.573 -0.820 0.000 -0.256 0.037 0.966 -0.341 -0.023 0.940 6.245 266.217 70.018
    11 0.076 0.059 0.995 0.617 -0.787 0.000 -0.236 0.024 0.971 -0.309 -0.033 0.950 5.512 263.948 71.882
    12 0.116 0.040 0.992 0.329 -0.944 0.000 -0.074 -0.217 0.973 -0.186 -0.256 0.949 7.038 216.003 71.568
    13 0.095 0.023 0.995 0.235 -0.972 0.000 -0.089 -0.221 0.971 -0.182 -0.243 0.953 5.637 216.774 72.344
    14 0.075 0.032 0.997 0.398 -0.917 0.000 -0.086 -0.205 0.975 -0.158 -0.237 0.959 4.659 213.743 73.460
    15 0.094 0.020 0.995 0.209 -0.978 0.000 -0.094 -0.210 0.973 -0.186 -0.229 0.956 5.543 219.004 72.851
    16 -0.024 0.081 0.996 0.960 0.279 0.000 -0.035 -0.249 0.968 -0.012 -0.327 0.945 4.840 182.173 70.919
    17 -0.022 0.069 0.997 0.954 0.299 0.000 -0.035 -0.239 0.970 -0.014 -0.305 0.952 4.130 182.653 72.208
    18 -0.040 0.077 0.996 0.889 0.458 0.000 -0.034 -0.248 0.968 0.004 -0.322 0.947 4.976 179.263 71.195
    19 -0.047 0.060 0.997 0.786 0.618 0.000 -0.048 -0.248 0.968 -0.003 -0.306 0.952 4.400 180.514 72.177
    20 0.128 0.023 0.991 0.179 -0.984 0.000 0.327 -0.174 0.929 0.205 -0.196 0.959 7.486 133.680 73.513
    21 0.147 0.006 0.989 0.038 -0.999 0.000 0.365 -0.157 0.918 0.226 -0.162 0.961 8.460 125.583 73.858
    22 0.143 -0.022 0.990 -0.150 -0.989 0.000 0.380 -0.173 0.909 0.246 -0.152 0.957 8.292 121.771 73.191
    23 0.194 -0.018 0.981 -0.093 -0.996 0.000 0.400 -0.166 0.901 0.218 -0.149 0.965 11.208 124.264 74.700
    24 -0.098 -0.003 0.995 -0.034 0.999 0.000 0.215 -0.012 0.976 0.310 -0.009 0.951 5.611 91.609 71.952
    25 -0.022 -0.032 0.999 -0.826 0.564 0.000 0.238 -0.010 0.971 0.259 0.020 0.966 2.197 85.483 74.949
    26 -0.039 -0.010 0.999 -0.246 0.969 0.000 0.234 0.013 0.972 0.272 0.023 0.962 2.312 85.191 74.170
    27 -0.068 -0.002 0.998 -0.027 1.000 0.000 0.229 0.048 0.972 0.295 0.050 0.954 3.915 80.368 72.575
    28 -0.022 -0.017 1.000 -0.631 0.776 0.000 0.046 0.240 0.970 0.067 0.257 0.964 1.589 14.580 74.577
    29 -0.022 -0.083 0.996 -0.966 0.260 0.000 0.071 0.212 0.975 0.092 0.292 0.952 4.916 17.528 72.195
    30 0.021 -0.075 0.997 -0.963 -0.268 0.000 0.090 0.208 0.974 0.070 0.280 0.957 4.452 14.090 73.209
    31 0.003 -0.122 0.993 -1.000 -0.026 0.000 0.119 0.199 0.973 0.115 0.316 0.942 7.007 20.062 70.328

    1 Attachment

  • Rework the results table:

    ax ay az mx my mz m1x m1y m1z Tilt Heading Dip
    0.005 0.04 0.999 -0.039 0.34 0.94 -0.044 0.302 0.952 2.326 351.676 72.231
    0.054 0.06 0.997 0.02 0.369 0.929 -0.03 0.312 0.95 4.635 354.468 71.747
    0.046 0.062 0.997 0.01 0.356 0.934 -0.033 0.298 0.954 4.401 353.694 72.543
    0.089 0.071 0.994 0.022 0.361 0.932 -0.062 0.294 0.954 6.512 348.128 72.507
    0.068 0.02 0.998 -0.164 0.235 0.958 -0.228 0.216 0.949 4.036 313.42 71.701
    0.124 0.03 0.992 -0.143 0.244 0.959 -0.261 0.215 0.941 7.324 309.538 70.221
    0.092 0.043 0.995 -0.145 0.25 0.957 -0.232 0.208 0.95 5.818 311.918 71.825
    0.116 0.054 0.992 -0.138 0.266 0.954 -0.249 0.215 0.944 7.351 310.819 70.781
    0.089 0.054 0.995 -0.247 0.043 0.968 -0.332 -0.009 0.943 5.964 268.508 70.605
    0.077 0.042 0.996 -0.246 0.036 0.969 -0.32 -0.004 0.948 5.015 269.292 71.365
    0.089 0.062 0.994 -0.256 0.037 0.966 -0.341 -0.023 0.94 6.245 266.217 70.018
    0.076 0.059 0.995 -0.236 0.024 0.971 -0.309 -0.033 0.95 5.512 263.948 71.882
    0.116 0.04 0.992 -0.074 -0.217 0.973 -0.186 -0.256 0.949 7.038 216.003 71.568
    0.095 0.023 0.995 -0.089 -0.221 0.971 -0.182 -0.243 0.953 5.637 216.774 72.344
    0.075 0.032 0.997 -0.086 -0.205 0.975 -0.158 -0.237 0.959 4.659 213.743 73.46
    0.094 0.02 0.995 -0.094 -0.21 0.973 -0.186 -0.229 0.956 5.543 219.004 72.851
    -0.024 0.081 0.996 -0.035 -0.249 0.968 -0.012 -0.327 0.945 4.84 182.173 70.919
    -0.022 0.069 0.997 -0.035 -0.239 0.97 -0.014 -0.305 0.952 4.13 182.653 72.208
    -0.04 0.077 0.996 -0.034 -0.248 0.968 0.004 -0.322 0.947 4.976 179.263 71.195
    -0.047 0.06 0.997 -0.048 -0.248 0.968 -0.003 -0.306 0.952 4.4 180.514 72.177
    0.128 0.023 0.991 0.327 -0.174 0.929 0.205 -0.196 0.959 7.486 133.68 73.513
    0.147 0.006 0.989 0.365 -0.157 0.918 0.226 -0.162 0.961 8.46 125.583 73.858
    0.143 -0.022 0.99 0.38 -0.173 0.909 0.246 -0.152 0.957 8.292 121.771 73.191
    0.194 -0.018 0.981 0.4 -0.166 0.901 0.218 -0.149 0.965 11.208 124.264 74.7
    -0.098 -0.003 0.995 0.215 -0.012 0.976 0.31 -0.009 0.951 5.611 91.609 71.952
    -0.022 -0.032 0.999 0.238 -0.01 0.971 0.259 0.02 0.966 2.197 85.483 74.949
    -0.039 -0.01 0.999 0.234 0.013 0.972 0.272 0.023 0.962 2.312 85.191 74.17
    -0.068 -0.002 0.998 0.229 0.048 0.972 0.295 0.05 0.954 3.915 80.368 72.575
    -0.022 -0.017 1 0.046 0.24 0.97 0.067 0.257 0.964 1.589 14.58 74.577
    -0.022 -0.083 0.996 0.071 0.212 0.975 0.092 0.292 0.952 4.916 17.528 72.195
    0.021 -0.075 0.997 0.09 0.208 0.974 0.07 0.28 0.957 4.452 14.09 73.209
    0.003 -0.122 0.993 0.119 0.199 0.973 0.115 0.316 0.942 7.007 20.062 70.328
  • If 12 factor calibration is used on both accelerometer and magnetometer data, and the Rodrigues formula is applied do the magnetic dip statistics improve?

    Code to apply 12 factor calibrations ( Using a LSM9DS1 IMU )

    //Use 12 element calibration on raw count magnetometer data
    function cal8G(S){
     var i;
     var mm=[0,0,0];
     var scale=8;
     var mres=[
         [0.970972836,0.171154738,-0.000344029],
         [-0.095223453,0.956653319,-0.000125418],­
         [0.017179224,0.005190024,1],
         ];
     var moff=[1057.176483,120.7727841,549.551664­];
     for(i=0;i<3;i++) S.m[i] -=moff[i];
     for(i=0;i<3;i++)
      mm[i]=S.m[0]*mres[i][0]+S.m[1]*mres[i][1­]+S.m[2]*mres[i][2];
      for(i=0;i<3;i++)S.m[i]=mm[i]*scale/32768­;
    }//end cal8G(S)
    //Use 12 element calibration on raw count accelerometer data
    function calA2(S){
     var i;
     var mm=[0,0,0];
     var scale=2;
     var mres=[
       [1.005708319,-0.007635345,-1.55429E-05],­
       [0.001530038,1.006717582,-9.04159E-05],
       [0.017299441,0.097632586,1]
       ];
     var moff=[-126.0318135,-350.2409885,730.2274­842];
    
     for(i=0;i<3;i++) S.a[i] -=moff[i];
     for(i=0;i<3;i++)
      mm[i]=S.a[0]*mres[i][0]+S.a[1]*mres[i][1­]+S.a[2]*mres[i][2];
      for(i=0;i<3;i++)S.a[i]=mm[i]*scale/32768­;
    }//end cal8G(S)
    ////////////////////
    function readall(W){
    if(W.magAvailable()){
      W.readMag();
      W.readAccel();
      cal8G(W);
     calA2(W);
    console.log(W.a,',',W.m);
    //console.log(W.a);
    }//endif
    }//end readall
    
    

    Data were collected and pasted into the following code that is used to determine the signs of the magnetometer axis that produce the best magnetic dip angle statistics.

    function test(){
     FF=require("Rodrigues").connect();
     var i,j,k,l;
     var d="";
      var t=[0,0,0];
      console.log("mmm,mmp,mpm,mpp,pmm,pmp,ppm­,ppp");
     for(i=0;i<Adata.length;i++){
      d=" "+i;
     A=FF.Setup(Adata[i],[0,0,1]);
     Theta=FF.theta;
     Kn=FF.Kn;
     A=FF.An;
    for(j=-1;j<2;j=j+2){
     for(k=-1;k<2;k=k+2){
      for(l=-1;l<2;l=l+2){
       t[0]=Mdata[i][0]*j;
       t[1]=Mdata[i][1]*k;
       t[2]=Mdata[i][2]*l;
       B=FF.RotVector(t);
       An=FF.An;
     dip=pirate*Math.atan(B[2]/Math.sqrt(B[0]­*B[0]+B[1]*B[1]));
         d=d+","+dip;
      }//nextl
     }//nextk
    }//nextj
    console.log(d);
    }//next i
    }
    

    This produces output that looks like the following:

    N mmm mmp mpm mpp pmm pmp ppm ppp
    0 66.84 -65.86 63.19 -69.97 69.97 -63.19 65.86 -66.84
    1 65.84 -67.25 63.43 -70.09 70.09 -63.43 67.25 -65.84
    2 66.60 -64.70 61.16 -71.07 71.07 -61.16 64.70 -66.60

    This output is copied into a CSV file and the CSV file is opened using Excel. Descriptive Statistics are then applied to the columns of data

    Statistic mmm mmp mpm mpp pmm pmp ppm ppp
    Mean 36.62 -58.66 22.56 -72.11 72.11 -22.56 58.66 -36.62
    Standard Error 7 4.84 8.39 0.39 0.39 8.39 4.84 7
    Median 65.64 -72.26 65.17 -72.21 72.21 -65.17 72.26 -65.64
    Mode #N/A #N/A #N/A #N/A #N/A #N/A #N/A #N/A
    Standard Deviation 58.16 40.16 69.67 3.27 3.27 69.67 40.16 58.16
    Sample Variance 3382.36 1613.19 4853.89 10.67 10.67 4853.89 1613.19 3382.36
    Kurtosis -0.42 3.95 -1.6 0.78 0.78 -1.6 3.95 -0.42
    Skewness -1.16 2.31 -0.59 0.28 -0.28 0.59 -2.31 1.16
    Range 177.14 150.05 169.77 14.85 14.85 169.77 150.05 177.14
    Minimum -88.87 -87.5 -81.07 -79.4 64.56 -88.7 -62.55 -88.27
    Maximum 88.27 62.55 88.7 -64.56 79.4 81.07 87.5 88.87
    Sum 2526.48 -4047.26 1556.98 -4975.74 4975.74 -1556.98 4047.26 -2526.48
    Count 69 69 69 69 69 69 69 69
    COV 159.00% -68.00% 309.00% -5.00% 5.00% -309.00% 68.00% -159.00%

    The mpp and pmm columns produce the smallest coffoecoent of variation (COV) values. The following code uses the +X, -Y, -Z (pmm) combination.

    function test(){
     FF=require("Rodrigues").connect();
     var i;
     for(i=0;i<Adata.length;i++){
     A=FF.Setup(Adata[i],[0,0,1]);
       Theta=FF.theta;
       Kn=FF.Kn;
     A=FF.An;
       t[0]=Mdata[i][0]*1;
       t[1]=Mdata[i][1]*-1;
       t[2]=Mdata[i][2]*-1;
      B=FF.RotVector(t);
       An=FF.An;
      heading=pirate*Math.atan2(-B[1],B[0]);
     if(heading<0)heading+=360;
     dip=pirate*Math.atan(B[2]/Math.sqrt(B[0]­*B[0]+B[1]*B[1]));
    console.log(i,',',A,',',An,',',B,',',
     Theta,',',heading,',',dip);
    }//next i
    }
    

    This produces the following:

    N ax ay az mx my mz m1x m1y m1z Tilt Heading Dip
    0 0.12 0.04 0.99 0.08 0.37 0.92 -0.03 0.34 0.94 7.34 264.53 69.97
    1 0.13 0.02 0.99 0.11 0.36 0.93 -0.02 0.34 0.94 7.66 267.24 70.09
    2 0.13 0.06 0.99 0.11 0.38 0.92 -0.01 0.32 0.95 7.97 268.51 71.07
    3 0.16 0.06 0.99 0.11 0.39 0.92 -0.04 0.33 0.94 9.72 263.55 70.79
    4 0.16 0.08 0.98 -0.09 0.34 0.94 -0.24 0.26 0.94 10.39 227.72 69.28
    5 0.06 0.07 1.00 -0.10 0.32 0.94 -0.16 0.25 0.96 5.34 237.84 72.82
    6 0.08 0.05 1.00 -0.12 0.30 0.95 -0.19 0.25 0.95 5.16 232.69 71.77
    7 0.08 0.06 0.99 -0.12 0.30 0.95 -0.20 0.25 0.95 5.79 231.24 71.55
    8 0.12 -0.01 0.99 -0.17 -0.01 0.99 -0.28 0.01 0.96 6.68 181.35 73.45
    9 0.13 -0.01 0.99 -0.18 0.00 0.98 -0.31 0.01 0.95 7.25 180.95 72.20
    10 0.10 -0.02 0.99 -0.18 -0.01 0.98 -0.28 0.01 0.96 6.00 182.72 73.58
    11 0.12 -0.01 0.99 -0.19 0.00 0.98 -0.30 0.01 0.95 6.82 181.88 72.27
    12 0.09 0.05 0.99 -0.04 -0.21 0.98 -0.12 -0.26 0.96 5.94 114.97 73.10
    13 0.07 0.05 1.00 -0.04 -0.23 0.97 -0.11 -0.28 0.95 4.89 110.49 72.52
    14 0.08 0.05 1.00 -0.03 -0.23 0.97 -0.10 -0.28 0.95 5.31 110.53 72.62
    15 0.09 0.05 1.00 -0.02 -0.23 0.97 -0.11 -0.28 0.95 5.68 111.32 72.63
    16 0.08 0.09 0.99 0.08 -0.19 0.98 0.01 -0.28 0.96 7.09 88.93 73.76
    17 0.11 0.09 0.99 0.10 -0.19 0.98 -0.01 -0.27 0.96 8.00 91.34 74.09
    18 0.10 0.08 0.99 0.11 -0.20 0.97 0.01 -0.28 0.96 7.25 87.14 73.69
    19 0.13 0.08 0.99 0.10 -0.20 0.97 -0.02 -0.28 0.96 8.80 94.39 73.48
    20 0.12 0.14 0.98 0.31 -0.08 0.95 0.20 -0.21 0.96 10.39 46.51 73.30
    21 0.09 0.12 0.99 0.30 -0.09 0.95 0.21 -0.20 0.96 8.76 44.28 73.03
    22 0.09 0.12 0.99 0.30 -0.09 0.95 0.21 -0.21 0.95 8.76 45.22 72.50
    23 0.08 0.11 0.99 0.30 -0.08 0.95 0.22 -0.19 0.96 7.84 40.87 73.14
    24 -0.11 0.02 0.99 0.19 0.08 0.98 0.30 0.06 0.95 6.49 349.05 72.11
    25 -0.15 0.00 0.99 0.17 0.05 0.98 0.32 0.05 0.95 8.75 350.49 71.26
    26 -0.16 0.02 0.99 0.16 0.05 0.99 0.32 0.03 0.95 9.48 354.59 71.02
    27 -0.17 -0.03 0.99 0.15 0.03 0.99 0.31 0.06 0.95 9.79 349.45 71.54
    28 -0.03 -0.05 1.00 0.13 0.23 0.97 0.15 0.27 0.95 3.14 299.10 71.81
    29 -0.04 -0.03 1.00 0.11 0.23 0.97 0.15 0.26 0.95 2.85 299.20 72.50
    30 -0.03 -0.04 1.00 0.12 0.23 0.97 0.15 0.26 0.95 2.87 300.37 72.21
    31 -0.01 -0.05 1.00 0.12 0.22 0.97 0.13 0.27 0.96 2.70 295.80 72.84
    32 -0.15 -0.12 0.98 -0.15 0.17 0.97 -0.01 0.28 0.96 10.81 268.65 73.89
    33 -0.14 -0.13 0.98 -0.16 0.17 0.97 -0.03 0.29 0.96 10.96 264.77 72.81
    34 -0.14 -0.11 0.98 -0.17 0.16 0.97 -0.03 0.27 0.96 10.31 263.52 74.28
    35 -0.09 -0.13 0.99 -0.14 0.15 0.98 -0.05 0.27 0.96 9.17 259.69 73.83
    36 0.95 -0.10 0.31 0.90 0.32 0.28 0.04 0.41 0.91 72.21 275.70 65.49
    37 0.94 -0.10 0.34 0.89 0.33 0.31 0.04 0.42 0.91 70.14 274.89 65.04
    38 0.94 -0.10 0.32 0.88 0.33 0.33 0.01 0.43 0.90 71.15 270.85 64.56
    39 0.94 -0.11 0.33 0.88 0.32 0.34 0.00 0.43 0.90 70.81 270.42 64.75
    40 0.08 -0.04 -1.00 -0.02 0.26 -0.97 0.31 0.08 0.95 -5.33 345.27 71.59
    41 0.09 -0.06 -0.99 0.01 0.25 -0.97 0.30 0.07 0.95 -6.19 347.94 71.84
    42 0.06 -0.08 -1.00 0.02 0.26 -0.97 0.31 -0.12 0.94 -5.42 21.98 70.65
    43 0.12 -0.07 -0.99 0.04 0.24 -0.97 0.31 0.08 0.95 -8.02 345.75 71.41
    44 -0.92 -0.36 -0.18 -0.97 -0.23 -0.02 0.10 0.18 0.98 -79.40 298.09 77.90
    45 -0.92 -0.37 -0.16 -0.97 -0.23 -0.05 0.05 0.18 0.98 -80.68 286.11 79.40
    46 -0.90 -0.40 -0.18 -0.96 -0.26 -0.03 0.07 0.20 0.98 -79.86 289.55 77.59
    47 -0.90 -0.40 -0.17 -0.96 -0.28 -0.03 0.07 0.18 0.98 -80.38 291.45 78.69
    48 -0.90 -0.42 -0.15 -0.95 -0.30 0.01 0.08 0.18 0.98 -81.59 294.28 78.29
    49 -0.97 -0.07 0.21 -0.98 -0.20 -0.04 -0.24 -0.15 0.96 77.69 147.66 73.67
    50 -0.95 -0.08 0.31 -0.98 -0.21 0.00 -0.29 -0.15 0.94 71.91 152.86 70.74
    51 -0.95 -0.09 0.31 -0.98 -0.21 0.05 -0.25 -0.15 0.96 72.06 149.36 73.18
    52 -0.94 -0.08 0.33 -0.98 -0.20 0.05 -0.27 -0.15 0.95 70.50 151.66 72.02
    53 0.07 -0.06 -1.00 0.39 0.00 -0.92 0.04 0.33 0.94 -5.19 276.97 70.45
    54 0.09 -0.08 -0.99 0.39 0.00 -0.92 0.02 0.31 0.95 -6.85 274.35 71.97
    55 0.06 -0.10 -0.99 0.38 -0.01 -0.92 0.21 0.26 0.94 -6.54 309.08 70.25
    56 0.14 -0.05 -0.99 0.43 -0.01 -0.90 -0.20 0.22 0.96 -8.75 227.03 72.81
    57 0.99 -0.09 -0.15 0.98 0.09 0.17 -0.29 0.20 0.94 -81.59 215.17 69.31
    58 0.99 -0.08 -0.09 0.98 0.09 0.19 -0.26 0.19 0.95 -84.78 215.22 71.22
    59 0.99 -0.08 -0.08 0.98 0.10 0.18 -0.24 0.20 0.95 -85.65 220.13 71.89
    60 0.99 -0.10 -0.08 0.98 0.08 0.17 -0.23 0.20 0.95 -85.15 220.17 72.19
    61 -0.16 -0.98 -0.13 -0.31 -0.95 0.06 -0.13 0.21 0.97 -82.70 238.86 75.94
    62 -0.15 -0.98 -0.10 -0.32 -0.95 0.03 -0.15 0.15 0.98 -84.48 224.53 77.45
    63 -0.17 -0.98 -0.13 -0.33 -0.94 0.06 -0.13 0.21 0.97 -82.56 239.09 75.75
    64 -0.18 -0.98 -0.08 -0.33 -0.94 0.05 -0.12 0.16 0.98 -85.15 231.68 78.45
    65 -0.17 0.97 0.18 -0.57 0.81 0.17 -0.41 -0.09 0.91 79.85 167.84 65.39
    66 -0.23 0.95 0.21 -0.59 0.79 0.17 -0.38 -0.06 0.92 77.75 170.57 67.09
    67 -0.24 0.94 0.23 -0.60 0.78 0.19 -0.39 -0.08 0.92 76.95 168.65 66.85
    68 -0.22 0.94 0.27 -0.61 0.77 0.20 -0.42 -0.04 0.91 74.08 174.05 65.15
  • Post a reply
    • Bold
    • Italics
    • Link
    • Image
    • List
    • Quote
    • code
    • Preview
About

Software Gimbals for your compass

Posted by Avatar for ClearMemory041063 @ClearMemory041063

Actions