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_formalisms_in_three_dimensions
https://en.wikipedia.org/wiki/Rotation_matrix
https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
https://en.wikipedia.org/wiki/Rotations_in_4-dimensional_Euclidean_space#The_Euler.E2.80.93Rodrigues_formula_for_3D_rotations

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

• Rodrigues.js

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

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

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

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

Comparison of theoretical and actual magnetometer readings

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.

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

• Rodrigues.js
• testRodrigues1.js
• TestRodrigues10Jan17.js

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)

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

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)

1 Attachment

• 8gauss.ods

Rework the results table:

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.2274842];

 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:

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

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: