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