=============== Calibration Utility 1 ==============
............. Accelerometer Calibration ...........
Calibration of the GY-521 MPU6050 3-axis accelerometer.
Sec2 Calibration Utility 4 of this Web-site discusses the calibration of the magnetometer. The calibration of this sensor is based on the assumption that in the most general case the raw measurements of this sensor are laying on the surface of a tilted ellipsoid. In [Ref.3] this is extensively discussed. The reason for selecting this (complicated) method for calibration lies in the fact that the magnetic field strength and its direction changes from place to place. The gravity field demonstrates a "better" behavior. It is everywhere always vertical, and it's field-strength is in general well represented by the constant value 9.81 m/sec2. Rather than using the ellipsoid method to calibrate the accelerometer the "simpler"12 parameter calibration as discussed in Sec5 [Ref.1] will be used.
Calibration of the MPU6050 3-axis gyro is discussed in Sec-2_Calib-3.
The 45deg.wooden blok as shown in below figure plays a key role in the accelerometer calibration.
sec2-util1-Fig.1: Wooden cube with sensor and 45 deg.calibration block.
The MPU6050 is attached to the wooden cube in Fig.1. The sensor coordinate system is copied to the sides of the cube. The Test Bench from Sec-1_Util-1 Fig.1 will be used for the calibration process of this sensor.
The calibration set-up with the 45 deg.block allows for 6x4=24 different orientations of the wooden cube i.e. 24 different X,Y,Z measurements at 45 deg. Because of the precise 45 degree orientations, the exact values of two components of the gravity vector are known and having the value of 0.707107g, however with equal or different signs. The third component is zero.
The Arduino based and Teensy compatible sketch "T-Calib-MPU6050_accel-at_45deg.ino", has been developed to calculate from these 24 measurements the calibration parameters. The calculation is based on the Least Square (LS) method as presented in [Ref.1] Chapters 6.
After variables are declared, and the sensor has been configured in the Set-Up section, the Loop Section of the sketch runs one time top down recording the measurements.
Before a measurement is taken the sketch displays on the monitor to which defined position the cube has to be placed on the 45 deg.block.
Two examples of monitor readings are:
"1 of 24: Set Cube in X_45deg Up, Y_Up, Z=0 position and Type any key when ready...".
"15 of 24: Set Cube in Z-45deg-Down, X-Up, Y=0 position and Type any key when ready...");
etc.etc.
The corresponding set-up is shown in below Fig.2
sec2-util1-Fig.2: Orientation of wooden sensor-cube on 45 deg.calibration block.
Not only the measured gravity X,Y,Z components, but also for each cube position are the 3 pre-calculated true gravity vector components recorded. To increase the data stability, each recorded X,Y,Z measurement is the average of 50 measurements. Once all 24 measurements are completed, the recorded measurements together with the real gravity components are listed on the monitor. When OK'ed by the user, based on LS the calculation of the shift vector and calibration matrix is executed. As a quality measure of the calibration is for each channel the fit error calculatedand. This is done as per [Ref.1].
A user who wants to record the complete calibration process in a text file is advised to run next to the Arduino sketch the freeware CoolTerm serial-port terminal application. For details see Sec5_refs2-software.
It should be noted that both the calibration matrix and shift vector are valid for the used MPU6050 sensor only!
MPU6050 Accelerometer Calibration Source Code.
// Last Update: 23-06-2021
// MPU-6050 Accelerometer Calibration 24 positions at 45 degrees. Based on [Ref.1] Chapter 7: "12 parameter calibration".
#include
#define I2C Wire // Teensy
#define MPU_addr (0x68) // I2C address of the MPU-6050
// Variables used for data-logging
int i, m;
int acc[7]; // MPU-6050
int Sensitivity; // LSB/g
float X[24][4]; // X = input matrix m x p (m=rows=24, p=columns=4)
// m=24 defines number of measurements
//X[m][0] = stores measured value acc.X-component
//X[m][1] = stores measured value acc.Y-component
//X[m][2] = stores measured value acc.Z-component
//X[m][3] = 1, this 1 is stored for the Least-Square calculation
float Yx[24]; //True X-component of the m'th gravitational field measurement.
float Yy[24]; //True Y-component of the m'th gravitational field measurement
float Yz[24]; //True Z-component of the m'th gravitational field measurement
float pi=M_PI; //pi=3.14159;
float Check_X_Y[24][6];
// The following variables are used for the calibration analysis:
float XT[4][24]; // XT is used for storing transposed input matrix p x m (p=rows=4,m=columns=24)
float Xh1[4][4]; // auxiliary matrix Xh1=(XT x X)=>p x p=>4 x 4 matrix
float Xh2[4][4]; // auxiliary matrix Xh2 for temp.copy of Xh1 =>p x p =>4 x 4 matrix
float Xh3[4][4]; // auxiliary matrix used for inversion check =>p x p =>4 x 4 matrix
float Xh4[4][24]; // auxiliary matrix
float Wx[4]; // vector always 4 components
float Wy[4]; // vector always 4 components
float Wz[4]; // vector always 4 components
float CaliMatrix[3][3]; //Calibration Matrix is a 3 x 3 matrix
float CaliVec[3]; //shift vector always 3 components
float Yvec[3]; //Vector stores x.y.z Euler components
float Mvec[3]; //Vector with raw x,y,z measurements
float P[3]; // Residual vector always 3 components
float Ph[3]; // Residual help-vector always 3 components
float r[3]; // Residual vector always 3 components
float Residu[3]; // Residu x,y,z values
void setup(void)
{
I2C.begin(); // join i2c bus (address optional for master)
Serial.begin(9600); // start serial for output. Make sure you set your Serial Monitor to the same!
//++++++++++++ Configure MPU-6050 Accel en Gyro +++++++++++++++
set_MPU_6050_registers();
Serial.println("MPU-6050 Accelerometer Calibration");
Serial.println("");
}
void loop()
{
// 24 steps of data-aquisition.
//========================== 1: X-45deg Up, Y-Up, Z=0 =====================================
Serial.println("1 of 24: Set Cube in X-45deg Up, Y-Up, Z=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m=0;
Yx[m]=-0.707107;
Yy[m]=-0.707107;
Yz[m]= 0;
// Fill Row 1 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(1);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("X_45deg UP, Y_UP, Z=0 is done...");
Serial.println();
//=========== 2: X-45deg-Up, Y-Down, Z=0 ======================
Serial.println("2 of 24: Set Cube in X-45deg-Up, Y-Down, Z=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= -0.707107;
Yy[m]= +0.707107;
Yz[m]= 0;
// Fill Row 2 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(2);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("X-45deg-Up, Y-Down, Z=0 is done...");
Serial.println();
//============= 3: X-45deg-Down, Y-Up, Z=0 ====================
Serial.println("3 of 24: Set Cube in X-45deg-Down, Y-Up, Z=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= +0.707107;
Yy[m]= -0.707107;
Yz[m]= 0;
// Fill Row 3 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(3);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("X-45deg-Down, Y-Up, Z=0 is done...");
Serial.println();
//============= 4: X-45deg-Down, Y-Down, Z=0 ==================
Serial.println("4 of 24: Set Cube in X-45deg-Down, Y-Down, Z=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= +0.707107;
Yy[m]= +0.707107;
Yz[m]= 0;
// Fill Row 4 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(4);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("X-45deg-Down, Y-Down, Z=0 is done...");
Serial.println();
//================ 5: Y-45deg-Up, X-Up, Z=0 ===================
Serial.println("5 of 24: Set Cube in Y-45deg-Up, X-Up, Z=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]=-0.707107;
Yy[m]=-0.707107;
Yz[m]= 0;
// Fill Row 5 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(5);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Y-45deg-Up, X-Up, Z=0 is done...");
Serial.println();
//============== 6: Y-45deg-Up, X-Down, Z=0 ==================
Serial.println("6 of 24: Set Cube in Y-45deg-Up, X-Down, Z=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= +0.707107;
Yy[m]= -0.707107;
Yz[m]= 0;
// Fill Row 6 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(6);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Y-45deg-Up, X-Down, Z=0 is done...");
Serial.println();
//============== 7: Y_45deg Down, X_Up, Z=0 ==================
Serial.println("7 of 24: Set Cube in Y_45deg Down, X_Up, Z=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= -0.707107;
Yy[m]= +0.707107;
Yz[m]= 0;
// Fill Row 7 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(7);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Y_45deg Down, X_Up, Z=0 is done...");
Serial.println();
//============= 8: Y-45deg-Down, X-Down, Z=0 ==================
Serial.println("8 of 24: Set Cube in Y-45deg-Down, X-Down, Z=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= +0.707107;
Yy[m]= +0.707107;
Yz[m]= 0;
// Fill Row 8 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(8);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Y-45deg-Down, X-Down, Z=0 is done...");
Serial.println();
//============ 9: X-45deg-Up, Z-Up, Y=0 =======================
Serial.println("9 of 24: Set Cube in X-45deg-Up, Z-Up, Y=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= -0.707107;
Yy[m]= 0;
Yz[m]= -0.707107;
// Fill Row 9 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(9);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("X-45deg-Up, Z-Up, Y=0 is done...");
Serial.println();
//============= 10: X-45deg-Up, Z-Down, Y=0 ==================
Serial.println("10 of 24: Set Cube in X-45deg-Up, Z-Down, Y=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= -0.707107;
Yy[m]= 0;
Yz[m]= +0.707107;
// Fill Row 10 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(10);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("X-45deg-Up, Z-Down, Y=0 is done...");
Serial.println();
//============= 11: X-45deg-Down, Z-Up, Y=0 ===================
Serial.println("11 of 24: Set Cube in X-45deg-Down, Z-Up, Y=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= +0.707107;
Yy[m]= 0;
Yz[m]= -0.707107;
// Fill Row 11 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(11);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("X-45deg-Down, Z-Up, Y=0 is done...");
Serial.println();
//============= 12: X-45deg-Down, Z-Down, Y=0 =================
Serial.println("12 of 24: Set Cube in X-45deg-Down, Z-Down, Y=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= +0.707107;
Yy[m]= 0;
Yz[m]= +0.707107;
// Fill Row 12 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(12);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("X-45deg-Down, Z-Down, Y=0 is done...");
Serial.println();
//============= 13: Z-45deg-Up, X-Up, Y=0 =====================
Serial.println("13 of 24: Set Cube in Z-45deg-Up, X-Up, Y=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= -0.707107;
Yy[m]= 0;
Yz[m]=-0.707107;
// Fill Row 13 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(13);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Z-45deg-Up, X-Up, Y=0 is done...");
Serial.println();
//============= 14: Z-45deg-Up, X-Down, Y=0 ===================
Serial.println("14 of 24: Set Cube in Z-45deg-Up, X-Down, Y=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= +0.707107;
Yy[m]= 0;
Yz[m]= -0.707107;
// Fill Row 14 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(14);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Z-45deg-Up, X-Down, Y=0 is done...");
Serial.println();
//============ 15: Z-45deg-Down, X-Up, Y=0 ====================
Serial.println("15 of 24: Set Cube in Z-45deg-Down, X-Up, Y=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= -0.707107;
Yy[m]= 0;
Yz[m]= +0.707107;
// Fill Row 15 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(15);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Z-45deg-Down, X-Up, Y=0 is done...");
Serial.println();
//========== 16: Z-45deg-Down, X-Down, Y=0 ===================
Serial.println("16 of 24: Set Cube in Z-45deg-Down, X-Down, Y=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= +0.707107;
Yy[m]= 0;
Yz[m]= +0.707107;
// Fill Row 16 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(16);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Z-45deg-Down, X-Down, Y=0 is done...");
Serial.println();
//============ 17: Y-45deg-Up, Z-Up, X=0 ======================
Serial.println("17 of 24: Set Cube in Y-45deg-Up, Z-Up, X=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= 0;
Yy[m]= -0.707107;
Yz[m]= -0.707107;
// Fill Row 17 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(17);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Y-45deg-Up, Z-Up, X=0 is done...");
Serial.println();
//============ 18: Y-45deg-Up, Z-Down, X=0 ====================
Serial.println("18 of 24: Set Cube in Y-45deg-Up, Z-Down, X=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= 0;
Yy[m]= -0.707107;
Yz[m]= +0.707107;
// Fill Row 18 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(18);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Y-45deg-Up, Z-Down, X=0 is done...");
Serial.println();
//============= 19: Y-45deg-Down, Z-Up, X=0 ===================
Serial.println("19 of 24: Set Cube in Y-45deg-Down, Z-Up, X=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= 0;
Yy[m]= +0.707107;
Yz[m]= -0.707107;
// Fill Row 19 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(19);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Y-45deg-Down, Z-Up, X=0 is done...");
Serial.println();
//=========== 20: Y-45deg-Down, Z-Down, X=0 ==================
Serial.println("20 of 24: Set Cube in Y-45deg-Down, Z-Down, X=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= 0; //-sin(90*pi/180); //Euler hoeken in radians
Yy[m]= +0.707107; // sin(0*pi/180)*cos(0*pi/180); //Euler hoeken in radians
Yz[m]= +0.707107; // cos(0*pi/180)*cos(0*pi/180); //Euler hoeken in radians
// Fill Row =20 van input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(20);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Y-45deg-Down, Z-Down, X=0 is done...");
Serial.println();
//============ 21: Z-45deg-Up, Y-Up, X=0 =====================
Serial.println("21 of 24: Set Cube in Z-45deg-Up, Y-Up, X=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= 0;
Yy[m]= -0.707107;
Yz[m]= -0.707107;
// Fill Row 21 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(21);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Z-45deg-Up, Y-Up, X=0 is done...");
Serial.println();
//=============== 22: Z-45deg-Up, Y-Down, X=0 =================
Serial.println("22 of 24: Set Cube in Z-45deg-Up, Y-Down, X=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= 0;
Yy[m]= +0.707107;
Yz[m]= -0.707107;
// Fill Row 22 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(22);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Z-45deg-Up, Y-Down, X=0 is done...");
Serial.println();
//============== 23: Z-45deg-Down, Y-Up, X=0 ==================
Serial.println("23 of 24: Set Cube in Z-45deg-Down, Y-Up, X=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= 0;
Yy[m]= -0.707107;
Yz[m]= +0.707107;
// Fill Row 23 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(23);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Z-45deg-Down, Y-Up, X=0 is done...");
Serial.println();
//============ 24: Z-45deg-Down, Y-Down, X=0 ==================
Serial.println("24 of 24: Set Cube in Z-45deg-Down, Y-Down, X=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= 0;
Yy[m]= +0.707107;
Yz[m]= +0.707107;
// Fill Row 24 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(24);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Z-45deg-Down, Y-Down, X=0 is done...");
Serial.println();
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
//==================== All 24 Cube positions are recorded ===============================
Serial.println();
Serial.println("All 24 Cube positions done. Recorded Data are printed for consistency check");
Serial.println();
MatrixMathPrint((float*)Check_X_Y,24,6,"Check the consistency of input data.");
Serial.println();
Serial.println("Type any key to Start Calibration...");
while (!Serial.available()){} // wait for a character
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
//======================== Start of Calibration Analysis ================================
MatrixMathTranspose((float*)X,24,4,(float*)XT); //XT is transposed Data Matrix X
// MatrixMathPrint((float*)XT,4,24,"XT is the transposed Data matrix X");
MatrixMathMultiply((float*)XT,(float*)X,4,24,4,(float*)Xh1); //calculate (XT x X) and store in Xh1
// MatrixMathPrint((float*)Xh1,4,4,"XT x X");
MatrixMathCopy((float*)Xh1,4,4,(float*)Xh2); //create a temp.copy Xh2 of matrix Xh1=(XT x X)
Serial.println();
MatrixMathInvert((float*)Xh1,4); //calculate inverse van matrix Xh1=(XT x X)
// MatrixMathPrint((float*)Xh1,4,4,"Xh1 = (XT x X)**-1 inverted matrix");
// Check the matrix inversion using the temp.copy Xh2 and store the resut in Xh3
MatrixMathMultiply((float*)Xh2,(float*)Xh1,4,4,4,(float*)Xh3);
MatrixMathPrint((float*)Xh3,4,4,"Matrix inversion Check");
MatrixMathMultiply((float*)Xh1,(float*)XT,4,4,24,(float*)Xh4); //calculate ((XT x X)**-1) x XT and store result in Xh4
//MatrixMathPrint((float*)Xh4,4,6,"Xh4=((XT x X)**-1) x XT");
MatrixMathMultiply((float*)Xh4,(float*)Yx,4,24,1,(float*)Wx); //calculate Wx-Row
// MatrixMathPrint((float*)Wx,4,1, "Wx-Row");
MatrixMathMultiply((float*)Xh4,(float*)Yy,4,24,1,(float*)Wy); //calculate Wy-Row
// MatrixMathPrint((float*)Wy,4,1, "Wy-Row");
MatrixMathMultiply((float*)Xh4,(float*)Yz,4,24,1,(float*)Wz); //calculate Wz-Row
// MatrixMathPrint((float*)Wz,4,1, "Wz-Row");
for (i=0;i < 3;i++)
{
CaliMatrix[0][i]=Wx[i];
}
for (i=0;i < 3;i++)
{
CaliMatrix[1][i]=Wy[i];
}
for (i=0;i < 3;i++)
{
CaliMatrix[2][i]=Wz[i];
}
MatrixMathPrint((float*)CaliMatrix,3,3,"The Calibration Matrix");
CaliVec[0]=Wx[3];
CaliVec[1]=Wy[3];
CaliVec[2]=Wz[3];
MatrixMathPrint((float*)CaliVec,3,1, "The Calibration shift Vector");
//Quality check of the results.
Serial.println();
Serial.println("Calculation of the x,y,z residu components as Quality Check");
Residu[0]=0;
Residu[1]=0;
Residu[2]=0;
for (i=0;i < 24;i++)
{
Mvec[0]=X[i][0];
Mvec[1]=X[i][1];
Mvec[2]=X[i][2];
MatrixMathMultiply((float*)CaliMatrix,(float*)Mvec,3,3,1,(float*)Ph);
r[0]=Yx[i]-Ph[0]-CaliVec[0];
r[1]=Yy[i]-Ph[1]-CaliVec[1];
r[2]=Yz[i]-Ph[2]-CaliVec[2];
Residu[0]=Residu[0]+r[0]*r[0];
Residu[1]=Residu[1]+r[1]*r[1];
Residu[2]=Residu[2]+r[2]*r[2];
}
MatrixMathPrint((float*)Residu,3,1, "Square of residu-vector x,y,z as quality measure of calibration");
Serial.println();
Serial.println();
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++ Start Supporting Functions +++++++++++++++++++++++++++++++
void read_Raw_Accel(int Row)
{
delay(500); // only read every 0,5 seconds
int xsum=0;
int ysum=0;
int zsum=0;
float Gx,Gy,Gz;
int j;
for (j=0; j < 50 ; j++)
{
readAccel(acc); // read the raw globals x/y/z
xsum=xsum+acc[0];
ysum=ysum+acc[1];
zsum=zsum+acc[2];
delay(5);
}
Gx=(xsum/j) ; //average raw value in bitvalues
Gy=(ysum/j) ; //average raw value in bitvalues
Gz=(zsum/j) ; //average raw value in bitvalues
//Convert the accelerometer value to G's.
//Sensitivity=4096 LSB/g bij +/-8g full Scale range
// X[m][p] = input matrix m x p (m=rows=24,p=columns=4)
Row=Row-1;
X[Row][0] = (Gx / Sensitivity); //raw averaged value in g
X[Row][1] = (Gy / Sensitivity); //raw averaged value in g
X[Row][2] = (Gz / Sensitivity); //raw averaged value in g
X[Row][3] = 1;
Serial.print("Yx = ");
Serial.print(Yx[Row],4);
Serial.print(", ");
Serial.print("Yy = ");
Serial.print(Yy[Row],4);
Serial.print(", ");
Serial.print("Yz = ");
Serial.print(Yz[Row],4);
Serial.println(" ");
Serial.print("X-Column 1: ");
Serial.print(X[Row][0],2);
Serial.print(", ");
Serial.print("X-Column 2: ");
Serial.print(X[Row][1],2 );
Serial.print(", ");
Serial.print("X-Column 3: ");
Serial.print(X[Row][2],2 );
Serial.print(", ");
Serial.print("X-Column 4: ");
Serial.println(X[Row][3]);
}
//===================================================================================
void readAccel(int * result)
{
// Note: Bij DUE is int 4 byes and short is 2 bytes
// Read from MPU-6050 Accelerometer, process 6 values, dicard remaining 8
int regAddress = 0x3B; //first axis-acceleration-data register on the ADXL345
byte howManyBytesToRead = 14; //uint8_t means: give me an unsigned int of exactly 8 bits.
byte _buff[14];
short readings[7]; //short is 16bits=2bytes
readFrom(MPU_addr, regAddress, howManyBytesToRead, _buff); //read the acceleration data from the sensor
// MPU6050 reading
readings[0] = ((_buff[0] < < 8) | _buff[1]); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
readings[1] = ((_buff[2] < < 8) | _buff[3]); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
readings[2] = ((_buff[4] < < 8) | _buff[5]); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
result[0] = -(int)readings[0]; //bit-values, minus to get the g-sign right
result[1] = -(int)readings[1]; //bit-values, minus to get the g-sign right
result[2] = -(int)readings[2]; //bit-values, minus to get the g-sign right
}
//===================================================================================
// Reads num bytes starting from address register on MPU_addr in to _buff array
void readFrom(int DEVICE, byte address, int num, byte _buff[]) {
I2C.beginTransmission(DEVICE); // start transmission to device
I2C.write(address); // sends address to read from
I2C.endTransmission(); // end transmission
I2C.beginTransmission(DEVICE); // start transmission to device
I2C.requestFrom(DEVICE, num); // request 6 bytes from device
int i = 0;
while(I2C.available()) // device may send less than requested (abnormal)
{
_buff[i] = I2C.read(); // receive a byte
i++;
}
I2C.endTransmission(); // end transmission
}
//*******************************************************************************
//********* Various Matrix Functions **************************
//***************** Matrix Printing ************************************
// Uses tabs to separate numbers under assumption printed float width won't cause problems
// void MatrixMath::Print(float* A, int m, int n, String label){
void MatrixMathPrint(float* A, int m, int n, String label){
// A = input matrix (m x p)
// m = number of rows in A
// p = number of columns in A
int i,j;
Serial.println();
Serial.println(label);
for (i=0; i < m; i++){
for (j=0;j < n;j++){
Serial.print(A[n*i+j],6);
Serial.print("\t");
}
Serial.println();
}
}
//*********** Matrix Copy **********************************************************
void MatrixMathCopy(float* A, int n, int m, float* B)
{
int i, j, k;
for (i=0;i < m;i++)
for(j=0;j < n;j++)
{
B[n*i+j] = A[n*i+j];
}
}
//************* Matrix Addition ***********************************
void MatrixMathAdd(float* A, float* B, int m, int n, float* C)
{
// A = input matrix (m x n)
// B = input matrix (m x n)
// m = number of rows in A = number of rows in B
// n = number of columns in A = number of columns in B
// C = output matrix = A+B (m x n)
int i, j;
for (i=0;i < m;i++)
for(j=0;j < n;j++)
C[n*i+j]=A[n*i+j]+B[n*i+j];
}
//************* Matrix Subtraction ***********************************
void MatrixMathSubtract(float* A, float* B, int m, int n, float* C)
{
// A = input matrix (m x n)
// B = input matrix (m x n)
// m = number of rows in A = number of rows in B
// n = number of columns in A = number of columns in B
// C = output matrix = A-B (m x n)
int i, j;
for (i=0;i < m;i++)
for(j=0;j < n;j++)
C[n*i+j]=A[n*i+j]-B[n*i+j];
}
//******* Matrix Multiplication C=AxB ********************************
void MatrixMathMultiply(float* A, float* B, int m, int p, int n, float* C)
{
// A = input matrix (m x p)
// B = input matrix (p x n)
// m = number of rows in A
// p = number of columns in A = number of rows in B
// n = number of columns in B
// C = output matrix = A*B (m x n)
int i, j, k;
for (i=0;i < m;i++)
for(j=0;j < n;j++)
{
C[n*i+j]=0;
for (k=0;k < p;k++)
C[n*i+j]= C[n*i+j]+A[p*i+k]*B[n*k+j];
}
}
//*********** Matrix Transpose ****************************************
void MatrixMathTranspose(float* A, int m, int n, float* C)
{
// A = input matrix (m x n)
// m = number of rows in A
// n = number of columns in A
// C = output matrix = the transpose of A (n x m)
int i, j;
for (i=0;i < m;i++)
for(j=0;j < n;j++)
C[m*j+i]=A[n*i+j];
}
//************* Matrix Inversion *****************************************
// * This function inverts a matrix based on the Gauss Jordan method.
// * Specifically, it uses partial pivoting to improve numeric stability.
// * The algorithm is drawn from those presented in
// NUMERICAL RECIPES: The Art of Scientific Computing.
// * The function returns 1 on success, 0 on failure.
// * NOTE: The argument is ALSO the result matrix, meaning the input matrix is REPLACED
int MatrixMathInvert(float* A, int n)
{
// A = input matrix AND result matrix
// n = number of rows = number of columns in A (n x n)
int pivrow; // keeps track of current pivot row
int k,i,j; // k: overall index along diagonal; i: row index; j: col index
int pivrows[n]; // keeps track of rows swaps to undo at end
float tmp; // used for finding max value and making column swaps
for (k = 0; k < n; k++)
{
// find pivot row, the row with biggest entry in current column
tmp = 0;
for (i = k; i < n; i++)
{
if (abs(A[i*n+k]) >= tmp) // 'Avoid using other functions inside abs()?'
{
tmp = abs(A[i*n+k]);
pivrow = i;
}
}
// check for singular matrix
if (A[pivrow*n+k] == 0.0f)
{
Serial.println("Inversion failed due to singular matrix");
return 0;
}
// Execute pivot (row swap) if needed
if (pivrow != k)
{
// swap row k with pivrow
for (j = 0; j < n; j++)
{
tmp = A[k*n+j];
A[k*n+j] = A[pivrow*n+j];
A[pivrow*n+j] = tmp;
}
}
pivrows[k] = pivrow; // record row swap (even if no swap happened)
tmp = 1.0f/A[k*n+k]; // invert pivot element
A[k*n+k] = 1.0f; // This element of input matrix becomes result matrix
// Perform row reduction (divide every element by pivot)
for (j = 0; j < n; j++)
{
A[k*n+j] = A[k*n+j]*tmp;
}
// Now eliminate all other entries in this column
for (i = 0; i < n; i++)
{
if (i != k)
{
tmp = A[i*n+k];
A[i*n+k] = 0.0f; // The other place where in matrix becomes result mat
for (j = 0; j < n; j++)
{
A[i*n+j] = A[i*n+j] - A[k*n+j]*tmp;
}
}
}
}
// Done, now need to undo pivot row swaps by doing column swaps in reverse order
for (k = n-1; k >= 0; k--)
{
if (pivrows[k] != k)
{
for (i = 0; i < n; i++)
{
tmp = A[i*n+k];
A[i*n+k] = A[i*n+pivrows[k]];
A[i*n+pivrows[k]] = tmp;
}
}
}
return 1;
}
//**** End Sec2 Calib.Utility 1: Accelerometer Calibration ****
// Last Update: 23-06-2021
// MPU-6050 Accelerometer Calibration 24 positions at 45 degrees. Based on [Ref.1] Chapter 7: "12 parameter calibration".
#include
#define I2C Wire // Teensy
#define MPU_addr (0x68) // I2C address of the MPU-6050
// Variables used for data-logging
int i, m;
int acc[7]; // MPU-6050
int Sensitivity; // LSB/g
float X[24][4]; // X = input matrix m x p (m=rows=24, p=columns=4)
// m=24 defines number of measurements
//X[m][0] = stores measured value acc.X-component
//X[m][1] = stores measured value acc.Y-component
//X[m][2] = stores measured value acc.Z-component
//X[m][3] = 1, this 1 is stored for the Least-Square calculation
float Yx[24]; //True X-component of the m'th gravitational field measurement.
float Yy[24]; //True Y-component of the m'th gravitational field measurement
float Yz[24]; //True Z-component of the m'th gravitational field measurement
float pi=M_PI; //pi=3.14159;
float Check_X_Y[24][6];
// The following variables are used for the calibration analysis:
float XT[4][24]; // XT is used for storing transposed input matrix p x m (p=rows=4,m=columns=24)
float Xh1[4][4]; // auxiliary matrix Xh1=(XT x X)=>p x p=>4 x 4 matrix
float Xh2[4][4]; // auxiliary matrix Xh2 for temp.copy of Xh1 =>p x p =>4 x 4 matrix
float Xh3[4][4]; // auxiliary matrix used for inversion check =>p x p =>4 x 4 matrix
float Xh4[4][24]; // auxiliary matrix
float Wx[4]; // vector always 4 components
float Wy[4]; // vector always 4 components
float Wz[4]; // vector always 4 components
float CaliMatrix[3][3]; //Calibration Matrix is a 3 x 3 matrix
float CaliVec[3]; //shift vector always 3 components
float Yvec[3]; //Vector stores x.y.z Euler components
float Mvec[3]; //Vector with raw x,y,z measurements
float P[3]; // Residual vector always 3 components
float Ph[3]; // Residual help-vector always 3 components
float r[3]; // Residual vector always 3 components
float Residu[3]; // Residu x,y,z values
void setup(void)
{
I2C.begin(); // join i2c bus (address optional for master)
Serial.begin(9600); // start serial for output. Make sure you set your Serial Monitor to the same!
//++++++++++++ Configure MPU-6050 Accel en Gyro +++++++++++++++
set_MPU_6050_registers();
Serial.println("MPU-6050 Accelerometer Calibration");
Serial.println("");
}
void loop()
{
// 24 steps of data-aquisition.
//========================== 1: X-45deg Up, Y-Up, Z=0 =====================================
Serial.println("1 of 24: Set Cube in X-45deg Up, Y-Up, Z=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m=0;
Yx[m]=-0.707107;
Yy[m]=-0.707107;
Yz[m]= 0;
// Fill Row 1 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(1);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("X_45deg UP, Y_UP, Z=0 is done...");
Serial.println();
//=========== 2: X-45deg-Up, Y-Down, Z=0 ======================
Serial.println("2 of 24: Set Cube in X-45deg-Up, Y-Down, Z=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= -0.707107;
Yy[m]= +0.707107;
Yz[m]= 0;
// Fill Row 2 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(2);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("X-45deg-Up, Y-Down, Z=0 is done...");
Serial.println();
//============= 3: X-45deg-Down, Y-Up, Z=0 ====================
Serial.println("3 of 24: Set Cube in X-45deg-Down, Y-Up, Z=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= +0.707107;
Yy[m]= -0.707107;
Yz[m]= 0;
// Fill Row 3 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(3);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("X-45deg-Down, Y-Up, Z=0 is done...");
Serial.println();
//============= 4: X-45deg-Down, Y-Down, Z=0 ==================
Serial.println("4 of 24: Set Cube in X-45deg-Down, Y-Down, Z=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= +0.707107;
Yy[m]= +0.707107;
Yz[m]= 0;
// Fill Row 4 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(4);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("X-45deg-Down, Y-Down, Z=0 is done...");
Serial.println();
//================ 5: Y-45deg-Up, X-Up, Z=0 ===================
Serial.println("5 of 24: Set Cube in Y-45deg-Up, X-Up, Z=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]=-0.707107;
Yy[m]=-0.707107;
Yz[m]= 0;
// Fill Row 5 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(5);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Y-45deg-Up, X-Up, Z=0 is done...");
Serial.println();
//============== 6: Y-45deg-Up, X-Down, Z=0 ==================
Serial.println("6 of 24: Set Cube in Y-45deg-Up, X-Down, Z=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= +0.707107;
Yy[m]= -0.707107;
Yz[m]= 0;
// Fill Row 6 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(6);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Y-45deg-Up, X-Down, Z=0 is done...");
Serial.println();
//============== 7: Y_45deg Down, X_Up, Z=0 ==================
Serial.println("7 of 24: Set Cube in Y_45deg Down, X_Up, Z=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= -0.707107;
Yy[m]= +0.707107;
Yz[m]= 0;
// Fill Row 7 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(7);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Y_45deg Down, X_Up, Z=0 is done...");
Serial.println();
//============= 8: Y-45deg-Down, X-Down, Z=0 ==================
Serial.println("8 of 24: Set Cube in Y-45deg-Down, X-Down, Z=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= +0.707107;
Yy[m]= +0.707107;
Yz[m]= 0;
// Fill Row 8 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(8);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Y-45deg-Down, X-Down, Z=0 is done...");
Serial.println();
//============ 9: X-45deg-Up, Z-Up, Y=0 =======================
Serial.println("9 of 24: Set Cube in X-45deg-Up, Z-Up, Y=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= -0.707107;
Yy[m]= 0;
Yz[m]= -0.707107;
// Fill Row 9 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(9);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("X-45deg-Up, Z-Up, Y=0 is done...");
Serial.println();
//============= 10: X-45deg-Up, Z-Down, Y=0 ==================
Serial.println("10 of 24: Set Cube in X-45deg-Up, Z-Down, Y=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= -0.707107;
Yy[m]= 0;
Yz[m]= +0.707107;
// Fill Row 10 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(10);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("X-45deg-Up, Z-Down, Y=0 is done...");
Serial.println();
//============= 11: X-45deg-Down, Z-Up, Y=0 ===================
Serial.println("11 of 24: Set Cube in X-45deg-Down, Z-Up, Y=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= +0.707107;
Yy[m]= 0;
Yz[m]= -0.707107;
// Fill Row 11 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(11);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("X-45deg-Down, Z-Up, Y=0 is done...");
Serial.println();
//============= 12: X-45deg-Down, Z-Down, Y=0 =================
Serial.println("12 of 24: Set Cube in X-45deg-Down, Z-Down, Y=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= +0.707107;
Yy[m]= 0;
Yz[m]= +0.707107;
// Fill Row 12 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(12);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("X-45deg-Down, Z-Down, Y=0 is done...");
Serial.println();
//============= 13: Z-45deg-Up, X-Up, Y=0 =====================
Serial.println("13 of 24: Set Cube in Z-45deg-Up, X-Up, Y=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= -0.707107;
Yy[m]= 0;
Yz[m]=-0.707107;
// Fill Row 13 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(13);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Z-45deg-Up, X-Up, Y=0 is done...");
Serial.println();
//============= 14: Z-45deg-Up, X-Down, Y=0 ===================
Serial.println("14 of 24: Set Cube in Z-45deg-Up, X-Down, Y=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= +0.707107;
Yy[m]= 0;
Yz[m]= -0.707107;
// Fill Row 14 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(14);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Z-45deg-Up, X-Down, Y=0 is done...");
Serial.println();
//============ 15: Z-45deg-Down, X-Up, Y=0 ====================
Serial.println("15 of 24: Set Cube in Z-45deg-Down, X-Up, Y=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= -0.707107;
Yy[m]= 0;
Yz[m]= +0.707107;
// Fill Row 15 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(15);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Z-45deg-Down, X-Up, Y=0 is done...");
Serial.println();
//========== 16: Z-45deg-Down, X-Down, Y=0 ===================
Serial.println("16 of 24: Set Cube in Z-45deg-Down, X-Down, Y=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= +0.707107;
Yy[m]= 0;
Yz[m]= +0.707107;
// Fill Row 16 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(16);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Z-45deg-Down, X-Down, Y=0 is done...");
Serial.println();
//============ 17: Y-45deg-Up, Z-Up, X=0 ======================
Serial.println("17 of 24: Set Cube in Y-45deg-Up, Z-Up, X=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= 0;
Yy[m]= -0.707107;
Yz[m]= -0.707107;
// Fill Row 17 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(17);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Y-45deg-Up, Z-Up, X=0 is done...");
Serial.println();
//============ 18: Y-45deg-Up, Z-Down, X=0 ====================
Serial.println("18 of 24: Set Cube in Y-45deg-Up, Z-Down, X=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= 0;
Yy[m]= -0.707107;
Yz[m]= +0.707107;
// Fill Row 18 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(18);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Y-45deg-Up, Z-Down, X=0 is done...");
Serial.println();
//============= 19: Y-45deg-Down, Z-Up, X=0 ===================
Serial.println("19 of 24: Set Cube in Y-45deg-Down, Z-Up, X=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= 0;
Yy[m]= +0.707107;
Yz[m]= -0.707107;
// Fill Row 19 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(19);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Y-45deg-Down, Z-Up, X=0 is done...");
Serial.println();
//=========== 20: Y-45deg-Down, Z-Down, X=0 ==================
Serial.println("20 of 24: Set Cube in Y-45deg-Down, Z-Down, X=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= 0; //-sin(90*pi/180); //Euler hoeken in radians
Yy[m]= +0.707107; // sin(0*pi/180)*cos(0*pi/180); //Euler hoeken in radians
Yz[m]= +0.707107; // cos(0*pi/180)*cos(0*pi/180); //Euler hoeken in radians
// Fill Row =20 van input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(20);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Y-45deg-Down, Z-Down, X=0 is done...");
Serial.println();
//============ 21: Z-45deg-Up, Y-Up, X=0 =====================
Serial.println("21 of 24: Set Cube in Z-45deg-Up, Y-Up, X=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= 0;
Yy[m]= -0.707107;
Yz[m]= -0.707107;
// Fill Row 21 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(21);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Z-45deg-Up, Y-Up, X=0 is done...");
Serial.println();
//=============== 22: Z-45deg-Up, Y-Down, X=0 =================
Serial.println("22 of 24: Set Cube in Z-45deg-Up, Y-Down, X=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= 0;
Yy[m]= +0.707107;
Yz[m]= -0.707107;
// Fill Row 22 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(22);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Z-45deg-Up, Y-Down, X=0 is done...");
Serial.println();
//============== 23: Z-45deg-Down, Y-Up, X=0 ==================
Serial.println("23 of 24: Set Cube in Z-45deg-Down, Y-Up, X=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= 0;
Yy[m]= -0.707107;
Yz[m]= +0.707107;
// Fill Row 23 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(23);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Z-45deg-Down, Y-Up, X=0 is done...");
Serial.println();
//============ 24: Z-45deg-Down, Y-Down, X=0 ==================
Serial.println("24 of 24: Set Cube in Z-45deg-Down, Y-Down, X=0 position and Type any key when ready...");
while (!Serial.available()){} // wait for a character
m++;
Yx[m]= 0;
Yy[m]= +0.707107;
Yz[m]= +0.707107;
// Fill Row 24 of input matrix X[m x p] (m=rows=24,p=columns=4)
read_Raw_Accel(24);
Check_X_Y[m][0]=X[m][0];
Check_X_Y[m][1]=X[m][1];
Check_X_Y[m][2]=X[m][2];
Check_X_Y[m][3]=Yx[m];
Check_X_Y[m][4]=Yy[m];
Check_X_Y[m][5]=Yz[m];
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
Serial.println("Z-45deg-Down, Y-Down, X=0 is done...");
Serial.println();
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
//==================== All 24 Cube positions are recorded ===============================
Serial.println();
Serial.println("All 24 Cube positions done. Recorded Data are printed for consistency check");
Serial.println();
MatrixMathPrint((float*)Check_X_Y,24,6,"Check the consistency of input data.");
Serial.println();
Serial.println("Type any key to Start Calibration...");
while (!Serial.available()){} // wait for a character
while (Serial.available())
{
Serial.read(); // clear the input buffer
}
//======================== Start of Calibration Analysis ================================
MatrixMathTranspose((float*)X,24,4,(float*)XT); //XT is transposed Data Matrix X
// MatrixMathPrint((float*)XT,4,24,"XT is the transposed Data matrix X");
MatrixMathMultiply((float*)XT,(float*)X,4,24,4,(float*)Xh1); //calculate (XT x X) and store in Xh1
// MatrixMathPrint((float*)Xh1,4,4,"XT x X");
MatrixMathCopy((float*)Xh1,4,4,(float*)Xh2); //create a temp.copy Xh2 of matrix Xh1=(XT x X)
Serial.println();
MatrixMathInvert((float*)Xh1,4); //calculate inverse van matrix Xh1=(XT x X)
// MatrixMathPrint((float*)Xh1,4,4,"Xh1 = (XT x X)**-1 inverted matrix");
// Check the matrix inversion using the temp.copy Xh2 and store the resut in Xh3
MatrixMathMultiply((float*)Xh2,(float*)Xh1,4,4,4,(float*)Xh3);
MatrixMathPrint((float*)Xh3,4,4,"Matrix inversion Check");
MatrixMathMultiply((float*)Xh1,(float*)XT,4,4,24,(float*)Xh4); //calculate ((XT x X)**-1) x XT and store result in Xh4
//MatrixMathPrint((float*)Xh4,4,6,"Xh4=((XT x X)**-1) x XT");
MatrixMathMultiply((float*)Xh4,(float*)Yx,4,24,1,(float*)Wx); //calculate Wx-Row
// MatrixMathPrint((float*)Wx,4,1, "Wx-Row");
MatrixMathMultiply((float*)Xh4,(float*)Yy,4,24,1,(float*)Wy); //calculate Wy-Row
// MatrixMathPrint((float*)Wy,4,1, "Wy-Row");
MatrixMathMultiply((float*)Xh4,(float*)Yz,4,24,1,(float*)Wz); //calculate Wz-Row
// MatrixMathPrint((float*)Wz,4,1, "Wz-Row");
for (i=0;i < 3;i++)
{
CaliMatrix[0][i]=Wx[i];
}
for (i=0;i < 3;i++)
{
CaliMatrix[1][i]=Wy[i];
}
for (i=0;i < 3;i++)
{
CaliMatrix[2][i]=Wz[i];
}
MatrixMathPrint((float*)CaliMatrix,3,3,"The Calibration Matrix");
CaliVec[0]=Wx[3];
CaliVec[1]=Wy[3];
CaliVec[2]=Wz[3];
MatrixMathPrint((float*)CaliVec,3,1, "The Calibration shift Vector");
//Quality check of the results.
Serial.println();
Serial.println("Calculation of the x,y,z residu components as Quality Check");
Residu[0]=0;
Residu[1]=0;
Residu[2]=0;
for (i=0;i < 24;i++)
{
Mvec[0]=X[i][0];
Mvec[1]=X[i][1];
Mvec[2]=X[i][2];
MatrixMathMultiply((float*)CaliMatrix,(float*)Mvec,3,3,1,(float*)Ph);
r[0]=Yx[i]-Ph[0]-CaliVec[0];
r[1]=Yy[i]-Ph[1]-CaliVec[1];
r[2]=Yz[i]-Ph[2]-CaliVec[2];
Residu[0]=Residu[0]+r[0]*r[0];
Residu[1]=Residu[1]+r[1]*r[1];
Residu[2]=Residu[2]+r[2]*r[2];
}
MatrixMathPrint((float*)Residu,3,1, "Square of residu-vector x,y,z as quality measure of calibration");
Serial.println();
Serial.println();
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++ Start Supporting Functions +++++++++++++++++++++++++++++++
void read_Raw_Accel(int Row)
{
delay(500); // only read every 0,5 seconds
int xsum=0;
int ysum=0;
int zsum=0;
float Gx,Gy,Gz;
int j;
for (j=0; j < 50 ; j++)
{
readAccel(acc); // read the raw globals x/y/z
xsum=xsum+acc[0];
ysum=ysum+acc[1];
zsum=zsum+acc[2];
delay(5);
}
Gx=(xsum/j) ; //average raw value in bitvalues
Gy=(ysum/j) ; //average raw value in bitvalues
Gz=(zsum/j) ; //average raw value in bitvalues
//Convert the accelerometer value to G's.
//Sensitivity=4096 LSB/g bij +/-8g full Scale range
// X[m][p] = input matrix m x p (m=rows=24,p=columns=4)
Row=Row-1;
X[Row][0] = (Gx / Sensitivity); //raw averaged value in g
X[Row][1] = (Gy / Sensitivity); //raw averaged value in g
X[Row][2] = (Gz / Sensitivity); //raw averaged value in g
X[Row][3] = 1;
Serial.print("Yx = ");
Serial.print(Yx[Row],4);
Serial.print(", ");
Serial.print("Yy = ");
Serial.print(Yy[Row],4);
Serial.print(", ");
Serial.print("Yz = ");
Serial.print(Yz[Row],4);
Serial.println(" ");
Serial.print("X-Column 1: ");
Serial.print(X[Row][0],2);
Serial.print(", ");
Serial.print("X-Column 2: ");
Serial.print(X[Row][1],2 );
Serial.print(", ");
Serial.print("X-Column 3: ");
Serial.print(X[Row][2],2 );
Serial.print(", ");
Serial.print("X-Column 4: ");
Serial.println(X[Row][3]);
}
//===================================================================================
void readAccel(int * result)
{
// Note: Bij DUE is int 4 byes and short is 2 bytes
// Read from MPU-6050 Accelerometer, process 6 values, dicard remaining 8
int regAddress = 0x3B; //first axis-acceleration-data register on the ADXL345
byte howManyBytesToRead = 14; //uint8_t means: give me an unsigned int of exactly 8 bits.
byte _buff[14];
short readings[7]; //short is 16bits=2bytes
readFrom(MPU_addr, regAddress, howManyBytesToRead, _buff); //read the acceleration data from the sensor
// MPU6050 reading
readings[0] = ((_buff[0] < < 8) | _buff[1]); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
readings[1] = ((_buff[2] < < 8) | _buff[3]); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
readings[2] = ((_buff[4] < < 8) | _buff[5]); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
result[0] = -(int)readings[0]; //bit-values, minus to get the g-sign right
result[1] = -(int)readings[1]; //bit-values, minus to get the g-sign right
result[2] = -(int)readings[2]; //bit-values, minus to get the g-sign right
}
//===================================================================================
// Reads num bytes starting from address register on MPU_addr in to _buff array
void readFrom(int DEVICE, byte address, int num, byte _buff[]) {
I2C.beginTransmission(DEVICE); // start transmission to device
I2C.write(address); // sends address to read from
I2C.endTransmission(); // end transmission
I2C.beginTransmission(DEVICE); // start transmission to device
I2C.requestFrom(DEVICE, num); // request 6 bytes from device
int i = 0;
while(I2C.available()) // device may send less than requested (abnormal)
{
_buff[i] = I2C.read(); // receive a byte
i++;
}
I2C.endTransmission(); // end transmission
}
//*******************************************************************************
//********* Various Matrix Functions **************************
//***************** Matrix Printing ************************************
// Uses tabs to separate numbers under assumption printed float width won't cause problems
// void MatrixMath::Print(float* A, int m, int n, String label){
void MatrixMathPrint(float* A, int m, int n, String label){
// A = input matrix (m x p)
// m = number of rows in A
// p = number of columns in A
int i,j;
Serial.println();
Serial.println(label);
for (i=0; i < m; i++){
for (j=0;j < n;j++){
Serial.print(A[n*i+j],6);
Serial.print("\t");
}
Serial.println();
}
}
//*********** Matrix Copy **********************************************************
void MatrixMathCopy(float* A, int n, int m, float* B)
{
int i, j, k;
for (i=0;i < m;i++)
for(j=0;j < n;j++)
{
B[n*i+j] = A[n*i+j];
}
}
//************* Matrix Addition ***********************************
void MatrixMathAdd(float* A, float* B, int m, int n, float* C)
{
// A = input matrix (m x n)
// B = input matrix (m x n)
// m = number of rows in A = number of rows in B
// n = number of columns in A = number of columns in B
// C = output matrix = A+B (m x n)
int i, j;
for (i=0;i < m;i++)
for(j=0;j < n;j++)
C[n*i+j]=A[n*i+j]+B[n*i+j];
}
//************* Matrix Subtraction ***********************************
void MatrixMathSubtract(float* A, float* B, int m, int n, float* C)
{
// A = input matrix (m x n)
// B = input matrix (m x n)
// m = number of rows in A = number of rows in B
// n = number of columns in A = number of columns in B
// C = output matrix = A-B (m x n)
int i, j;
for (i=0;i < m;i++)
for(j=0;j < n;j++)
C[n*i+j]=A[n*i+j]-B[n*i+j];
}
//******* Matrix Multiplication C=AxB ********************************
void MatrixMathMultiply(float* A, float* B, int m, int p, int n, float* C)
{
// A = input matrix (m x p)
// B = input matrix (p x n)
// m = number of rows in A
// p = number of columns in A = number of rows in B
// n = number of columns in B
// C = output matrix = A*B (m x n)
int i, j, k;
for (i=0;i < m;i++)
for(j=0;j < n;j++)
{
C[n*i+j]=0;
for (k=0;k < p;k++)
C[n*i+j]= C[n*i+j]+A[p*i+k]*B[n*k+j];
}
}
//*********** Matrix Transpose ****************************************
void MatrixMathTranspose(float* A, int m, int n, float* C)
{
// A = input matrix (m x n)
// m = number of rows in A
// n = number of columns in A
// C = output matrix = the transpose of A (n x m)
int i, j;
for (i=0;i < m;i++)
for(j=0;j < n;j++)
C[m*j+i]=A[n*i+j];
}
//************* Matrix Inversion *****************************************
// * This function inverts a matrix based on the Gauss Jordan method.
// * Specifically, it uses partial pivoting to improve numeric stability.
// * The algorithm is drawn from those presented in
// NUMERICAL RECIPES: The Art of Scientific Computing.
// * The function returns 1 on success, 0 on failure.
// * NOTE: The argument is ALSO the result matrix, meaning the input matrix is REPLACED
int MatrixMathInvert(float* A, int n)
{
// A = input matrix AND result matrix
// n = number of rows = number of columns in A (n x n)
int pivrow; // keeps track of current pivot row
int k,i,j; // k: overall index along diagonal; i: row index; j: col index
int pivrows[n]; // keeps track of rows swaps to undo at end
float tmp; // used for finding max value and making column swaps
for (k = 0; k < n; k++)
{
// find pivot row, the row with biggest entry in current column
tmp = 0;
for (i = k; i < n; i++)
{
if (abs(A[i*n+k]) >= tmp) // 'Avoid using other functions inside abs()?'
{
tmp = abs(A[i*n+k]);
pivrow = i;
}
}
// check for singular matrix
if (A[pivrow*n+k] == 0.0f)
{
Serial.println("Inversion failed due to singular matrix");
return 0;
}
// Execute pivot (row swap) if needed
if (pivrow != k)
{
// swap row k with pivrow
for (j = 0; j < n; j++)
{
tmp = A[k*n+j];
A[k*n+j] = A[pivrow*n+j];
A[pivrow*n+j] = tmp;
}
}
pivrows[k] = pivrow; // record row swap (even if no swap happened)
tmp = 1.0f/A[k*n+k]; // invert pivot element
A[k*n+k] = 1.0f; // This element of input matrix becomes result matrix
// Perform row reduction (divide every element by pivot)
for (j = 0; j < n; j++)
{
A[k*n+j] = A[k*n+j]*tmp;
}
// Now eliminate all other entries in this column
for (i = 0; i < n; i++)
{
if (i != k)
{
tmp = A[i*n+k];
A[i*n+k] = 0.0f; // The other place where in matrix becomes result mat
for (j = 0; j < n; j++)
{
A[i*n+j] = A[i*n+j] - A[k*n+j]*tmp;
}
}
}
}
// Done, now need to undo pivot row swaps by doing column swaps in reverse order
for (k = n-1; k >= 0; k--)
{
if (pivrows[k] != k)
{
for (i = 0; i < n; i++)
{
tmp = A[i*n+k];
A[i*n+k] = A[i*n+pivrows[k]];
A[i*n+pivrows[k]] = tmp;
}
}
}
return 1;
}
//**** End Sec2 Calib.Utility 1: Accelerometer Calibration ****
Free-Drones Company 2022