=============== Calibration Utility 3: ==============
............... Magnetometer Calibration ..............
Discussing Phase-2 Calibration of the HW-351 MAG3110 3-axis magnetometer.
The magnetometer calibration comprises two Phases. The Magnetometer Data Recorder as discussed and presented in Section-1 Utility-2 is Phase 1. With this utility running on the Arduino platform 216 X,Y,Z (raw) Magnetometer data are recorded in a txt-file of the HW351 MAG3110 sensor.
An editor like Notepad is then used to reduce the file down to the start of the array formatted data. The remaining array formatted data of the file is stored in a file named "recorded-sensordata.txt" in the user created directory d:\Pelles C-SourceCode\sensordata\. A snippet of this file is shown below.
Sec2-Calib4-Fig.1: Snippet of Array Formatted Magnetometer Raw Measurements.
In this section the final Phase 2 of the magnetometer calibration will be discussed. For this, lests assume a magnetometer measurment in an ideal world, free of any magnetic disturbances and a perfect 3-axis magnetometer. For each measurement taken under an arbitrary sensor orientation the square root of the sum of the components X, Y, Z square results in the same constant value i.e.when plotting the X,Y,Z values the locus of these measurements is a sphere centered at the origin of the X,Y,Z coordinate frame.
Unfortunately the world is not ideal and the magnetometer afflicted with imperfections like differences in axis gain, cross axis interferences, errors in orthogonalities of sensor component axis etc. The result is that in this real world the locus of the X, Y, Z components of the measurements is an ellipsoid shifted from the origin of the X,Y,Z frame.
To get data that can be used for an accurate heading the raw magnetometer measurements need to be corrected i.e.calibrated.
The calibration effort comprises finding the shift and transformation that transforms the raw magnetometer measurements from the surface of the ellipsoid to the surface of a sphere centered at the origin. This mathematical operation removes the errors in the data allowing an accurate heading to be computed.
This all is well discussed in the excellent paper [Ref.3].
To process the array formatted datafile "recorded-rensordata.txt" the 64-bit Windows application "Pelles-ST-Rotated-Ellipsoid-Fitting-for-Sensor-Calibration-V13-8-21.c" has been developed. This application is developed with the freeware Pelles-C for Windows Version 9.00.9 compiler.
On good grounds [Ref.3] demonstrates that in the most general case the 216 raw data points are laying on the surface of a rotated ellipsoid. With the Least-Square method the shift vector that centers the ellipsoid at the origin of the coordinate system, and the ellipsoid fit-matrix that defines the centered but rotated ellipsoid are calculated. The formalism of this LS model is step by step presented in [Ref.2], and is here implemented up to, and including the calculation of the shift vector and shifted ellipsoid fit-matrix.
The calculation of the calibration matrix that transforms shifted magnetometer measurements from the surface of the rotated ellipsoid to the surface of a sphere centered both at the origin of the data's coordinate frame is based on the theory presented in [Ref.3]. It is mentioned here that the author of this reference calls the effect that causes the data shift a "Hard-Iron" effect, mathematically represented by a vector. Anomalies like difference in axis gains, induced cross-axis interferences, non-orthogonality and mis-alignment of the X,Y,Z sensors on the chip and PCB he calls "Soft-Iron" effects, all together represented by a 3x3 matrix. Finally, to enable the calculation of the calibration matrix the author of [Ref.3] assumes symmetry of the 3x3 "Soft-Iron" matrix.
As output the Pelles-C calibration program generates the following three .txt files and writes these to the PC/Lap-Top hard-disk:
- gnuplot-rawdata.txt. This file, as shown below can be displayed with the freeware Gnuplot application and the relevant script in Section-5 Ref-2 Software.
Sec2-Calib3-Fig.2: XY, XZ and YZ GNUPLOT of the 216 Raw Measurements.
- final-calib-results.txt, comprising the X,Y,Z components of the shift vector, and the final calibration matrix. The shift vector removes the hard-iron effects and the calibration matrix removes the soft-iron effects.
An example of the calibration analysis is shown in file "final-calib-results.txt" below.
Sec2-Calib3-Fig.3: Final Magnetometer Calibration Results.
- gnuplot-calibdata.txt. This file comprises the result of the 216 X,Y,Z raw measurements after application of the shift vector followed by the transformatiSn with the calibration matrix. This result is displayed below with the Gnuplot application and the relevant script in Section-5 Ref-2 Software.
Not part of this .txt file is the calculation of the square root of the sum of the calibrated X, Y, Z square. However this result is part of the printed output displayed on the screen. For a quality calibration this result should read for all rows a value close to unity.
Sec2-Calib3-Fig.4: GNUPLOT of the 216 Calibrated Measurements.
The implemented C-code of the Least Square (LS) method that
calculates the shift vector and the ellipsoid fit-matrix is followed by the calculation of the final calibration matrix using Eigen-Value and Eigen-Vector calculations. The final calibration matrix transforms the shifted magnetometer measurements from the surface of the shifted ellipsoid to the surface of a sphere both centered at the same origin. This latter transformation removes the soft-iron effects, allowing an accurate heading to be calculated.
The mathematical processing of the file "recorded-sensordata.txt" from Phase-1 to the calibration result of Phase-2 is step by step discussed in the following.
Pelles-C Magnetometer Calibration Source-Code discussed Step by Step.
Last update 24-8-21
The shift vector and the fit-matrix is calculated with the Least-Square (LS) method. The used LS formalism is presented in [Ref.2]. During development of the code it pointed out that some matrices were poorly conditioned and hard to invert. Because of this a scaling factor to the data is applied. Together with a 64-bit compiler and the data-type double this problem has been tackled.
Calcul-Step 1: The 216 raw x,y,z magnetometer measurements from file "recorded-sensordata.txt" are loaded in matrix D[216][9].
The raw data are then exported as a Gnulot compatible file
"GnuPlot-RawData.txt".
An example of a GnuPlot of the raw data is shown in above figures Sec2-Calib4-Fig.2.
Calcul-Step 2: Matrix inversion and LS (Least-Square) solution-vector calculation.
Matrix D is filled with the raw measurements as defined in [Ref.2]. A copy of the product matrix DTD is stored in matrix OrigDTD for inversion check.
Because of the poor condition of matrices D and DT, both matrices are down-scaled with an experimentally determined factor to make the inversion of matrix DTD[9][9] successfull. The Gauss-Jordan method Ref.[4] is used for inversion. The inversion result is stored in matrix DTD. The inverted matrix DTD is then upscaled and multiplied with matrix OrigDTD for inversion check.
Finally the LS solution vector:
Vec[1x9] = Vec[a,b,c,d,e,f,g,h,i] is calculated.
Calcul-Step 3: Calculation Ellipsoid Offset-Vector and Ellipsoid Fit-Matrix.
The LS solution vector defines the quadratic function F(X,Y,Z)=Vec[0]XX+Vec[1]YY+Vec[2]ZZ+Vec[3]2XY+Vec[4]2XZ+Vec[5]2YZ+Vec[6]2X+Vec[7]2Y+Vec[8]2Z-1, an ellipsoid being the locus of the raw magnetometer measurements. The PARTIAL derivatives dF/dX=0, dF/dY=0, dF/dZ=0
give 3 equations from which the coordinates of the center i.e. offset of the ellipsoid can be calculated. This is in [Ref.2] achieved with auxiliary matrix A3[3][3] and vector Vghi[3]. The presented formalism leads to the offset-vector VS[1x3]. Applying the coordinate transformation X = X'+ VS[0], Y = Y'+ VS[1], Z = Z'+ VS[2] based on the offset vector VS[1x3] to the quadratic function F(X,Y,Z), and taking the solutin of dF/dX=0, dF/dY=0, dF/dZ=0 into account, delivers the desired matrix B3[3x3]. To calculate Matrix B3[3x3] auxiliary matrices A4[4x4] and T[4x4] is used, all in line with the formalism as per [Ref.2].
Symmetric matrix B3[3x3] represents the fit matrix, i.e.the matrix that defines the rotated ellipsoid shifted with vector VS to the origin of the coordinate-system X',Y',Z'.
Sofar, all papers on magnetometer calibration are aligned. However, on the next steps to get to the solution of the calibration problem, the schools differ.
In this Site we apply the proposed solution of [Ref.3], assuming the inverse of the "Soft-Iron" matrix W**-1[3x3] is symmetric. Note: to align the following text with [Ref.3], the matrix B3[3x3] will be renamed to matrix A[3x3]!!.
The objective of the following Calcul-Steps is to calculate the inverse soft-iron matrix W**-1[3x3] equal the square root of the ellipsoid fit-matrix A[3x3].
Calcul-Step 4: Eigenvalue calculation of fit-matrix A[3x3].
- For later eigenvalue check calculate the determinant of fit matrix A[3x3]
- Upscale A[3x3] to matrix UpsA[3x3] for improved Jakobi eigenvalue calculation
- Calculate eigen-values and vectors of matrix UpsA[3x3] using Jakobi. Note: Discard the Jakobi eignvectors.
- Check the eigenvalues as follows: Downscale the eigenvalues and multiply the 3 JacoEiVals. Check if result is equivalent to above determinant of fit-matri A[3x3].
Calcul-Step 5: Eigenvector calculation of fit-matrix A[3x3].
- Calculate with Gauss elimination the eigenvectors of fhe Jakobi eigenvalues.
- Normalize the eigenvectors and construct the orthonomal eigenvalue matrix NormEivec
- Check orthogonality of eigenvectors
- Finally transforming the fit-matrix A[3x3] to its principal axis with the orthonormal matrix of eigenvectors NormEiVec. This results in the Jacobi eigenvalue matrix JacoEiVals of fit-matrix A[3x3].
Calcul-Step 6: Calculation of the inverse soft-iron matrix W**-1[3x3], i.e. the Calibration matrix.
- Calculate squareroot of jacobi eigenvalue matrix JacoEiVals, and store in diagonal matrix SQRTJacoEiVals
- Transform SQRTJacoEiVals with NormEiVec matrix to Wmin1, the calibration matrix W**-1 related to coordinate-system X',Y',Z'.
Calcul-Step 7: Verification of the calibration results.
- As verification of the succesful calibration the 216 raw X,Y,Z magnetometer measurements is achieved with the offset vector V and the soft iron matrix Wmin1. The result is shown in above Calib4-Fig.4: GNUPLOT of the 216 Calibrated Measurements.
En excellent check of the quality of the calibration is the calculation of the square root of the sum of the calibrated X, Y, Z values square. This result is part of the printed output displayed on the screen, and should for all rows be close to unity. A snippet of the calibrated file is shown in below figure.
Sec2-Calib4-Fig.5: Check Quality of Calibration.
Pelles-C Magnetometer Calibration Source Code
// Last Update: 28-12-22
// Change IDE Font: Tools=>Options=>Source=>Font
// Code to be used for full sensor calibration!!!!!
// The code is developed with: PELLES C for Windows, Version 9.00.9 as Win64 Console Program
// For accuracy reasons the code should be compiled, and excecuted in 64 bit.
// Pelles-ST-Rotated-Ellipsoid-Fitting-for-Sensor-Calibration
// MAGNETOMETER CALIBRATION using 216 measurements.
// GAUSS-JORDAN (GJ) is used for matrix inversion
// 1: Calculation of Calibration Shift Vector Vx, Vy, Vz.
// 2: Calculation of the symmetric ellipsoid fit-matrix that will deliver the eigenvalues and eigenvectors.
// Eigenvalues calculated with Jakobi Eigenvalue Code
// Improved Eigenvector calculation using Gauss-elimination method.
// Ref.2: ST Design Tip DT0059. Ellipsoid or sphere fitting for sensor calibration by Andrea Vitali.
// Ref.3: Freescale Semiconductor Application Note. Document Number: AN4246 Rev. 4.0, 11/2015
// Calibrating an eCompass in the Presence of Hard-and Soft-Iron Interference by Talat Ozyagcilar.
//........................................................................................
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//--( Start Main )--( Start Main )--( Start Main )--( Start Main )--( Start Main )----
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <stdio.h>
#include <math.h>
#include <conio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <stdint.h> //to define uint64_t
#define Type double //Note: The analysis will be performed in doubles
#define _R 216 // Number of Rows of the input data matrix
#define _C 9 // Number of Columns
#define M _C
#define N _C
#define _L 3
#define _n 3 // Enter the size of the square symmetric matrix
#define _row _n
#define _col _n
#define _m _n
#define _Gcol _m // Number of Colums for the Augmented Matrix of the Gauss elimination
int i, j, k, l, p, q, flag;
int z =_col;
int n= _row;
int R=_R;
int C1 =_C;
int L=_L;
// Def: matrix m x p (m=rows ,p=columns)
Type A[_row][_col], UpsA[_row][_col], d[_row][_col], s[_row][_col], s1[_row][_col], s1t[_row][_col];
Type aG[_row][_Gcol], x[_row]; //This matrix is used for Gauss elimination to calculate the eigenvectors.
Type C[_row][_col] , CTp[_col][_row]; // Matrix and transposed matrix of eigenvectors
Type sltt[_row][_col]; //This matrix is used to test the juctaposition of the eigenvectors.
//Type sT[_row][_col]; //This matrix is the transposed eigenvector matrix sT used to test the transformation
Type temp[_row][_col];
Type hmax, theta, ratio;
Type pi=3.141592654;
Type zero=0.00001; //0.00001 zero determines the accurcacy of the calculation
Type radius; // Vector length
Type DFileIn[_R][_L];
Type DCalib1[_R][_L];
Type DCalib2[_R][_L];
Type D[_R][_C]; //D = input matrix m x p (m=rows=24,p=columns=4)
//m rows is aantal metingen, bv 24 bij kubus onder 90 graden
//D[m][0] = bv gemeten X-waarde in mGauss
//D[m][1] = bv gemeten Y-waarde in mGauss
//D[m][2] = bv gemeten Z-waarde in mGauss
Type ReduD[_R][_C];
Type DT [_C][_R], ReduDT [_C][_R]; // DT= transposed input matrix p x m (p=rows=4,m=columns)
Type DTD[_C][_C], ReduDTD[_C][_C];
Type OrigDTD[_C][_C]; // auxiliary matrix used for checking matrix-inversion
Type DTDh[_C][_C];
Type Dh1[_C][_C]; // auxiliary matrix Dh1=(DTp x D)=>p x p=>9 x 9 matrix
Type NVec[_R]; // N-vector filled with 24x1
Type DTNVec[_C]; // Hulp Vector DTNVec[9x1]=DT.NVec
Type Vec[_C]; // "Solution" vector
Type Vghi[3]; // part of "Solution" vector
Type VS[3]; // Shift vector
Type VS1[3]; // Rotated Shift vector
Type VS2[3];
Type A3[3][3]; //matrix to calculate the shift vector
Type HA3[3][3]; //Auxiliary matrix
Type HHA3[3][3]; //Auxiliary matrix to check A3 Matrixinversion
Type B3[3][3]; //hulp matrix
Type A4[4][4]; //hulp matrix
Type B4[4][4]; //hulp matrix
Type HATemp[4][4]; //hulp matrix
Type T[4][4];
Type TT[4][4];
Type EiVec[3][3], EiVecTp[3][3];
Type hEiVec[3][3], hEiVecTp[3][3];
Type NormEiVec[3][3], NormEiVecTp[3][3];
Type JacoEiVals[3][3], TstJacoEiVals[3][3];
Type JacoEiVec[3][3], JacoEiVecTp[3][3];
Type DetVal;
Type Wmin1[3][3]; //Calibration matrix W**-1
void importInteger3ColDataTextFile(Type* A);
void exportRawMagdata2textfile(Type* A);
void exportCalibMagdata2textfile(Type* A);
//void exportFinalCalibResults2textfile(Type* A);
void exportFinalCalibResults2textfile(void);
void MatrixMathPrint(Type* A , int row , int col, int decimals, char *Stringlabel);
void MatrixMathSciePrint(Type* A, int row, int col, int decimals, char *Stringlabel);
void MatrixMultiplyConstant(Type* A, Type* B, int row, int col, Type constant);
void MatrixMathCopy(Type* A , int row , int col, Type* B);
void MatrixMathMultiply(Type* A, Type* B, int m, int p, int n, Type* C);
void MatrixMathMatrixVectorMultiply(Type* A, Type* U, int m, int p, Type* V);
void MatrixMathTranspose(Type* A, int row, int col, Type* C);
void MatrixMath_I_Matrix(Type* A, int row, int col);
int MatrixMathInvert(Type* A, int n);
void MatrixMathAddition (Type* A, Type* B, int row, int col, Type* C);
void MatrixMathSubtract (Type* A, Type* B, int row, int col, Type* C);
void GaussElimination(Type* A, int row, int col);
Type MatrixMathDeterminant(Type* _D, int _t);
Type determinant(Type _a[_row][_col], int _t);
void main()
{
// xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
// Calcul-Step 1: Importing the 216 x,y,z magnetometer measurements.
// FILENAAM is "d:\\Pelles C-SourceCode\\sensordata\\recorded-sensordata.txt";
importInteger3ColDataTextFile((Type*)DFileIn);
printf("The from file imported Data Matrix is: \n");
for (i=0;i < R;i++)
{
for (j=0;j < L;j++)
{
D[i][j] = DFileIn[i][j];
printf(" %.0f", D[i][j]);
printf("\t");
}
printf("\n");
}
// For quality control the raw measurements are exportd as GnuPlot compatible file.
exportRawMagdata2textfile((Type*) DFileIn);
// xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
//================= All 216 Magnetic Data Records have been loaded ======================
// Calcul-Step 2: Calculating the tilted ellipsoid as discussed in[Ref.2] =====
// This factor has experimentally been figured out to down-scale the poorly conditioned
// matrix for successfull inversion.
Type factor = 0.001;
Type factor1;
for (i=0; i < _R ; i++)
{
D[i][3] = 2*D[i][0]*D[i][1]; // 2XY
D[i][4] = 2*D[i][0]*D[i][2]; // 2XZ
D[i][5] = 2*D[i][1]*D[i][2]; // 2YZ
D[i][6] = 2*D[i][0]; // 2X
D[i][7] = 2*D[i][1]; // 2Y
D[i][8] = 2*D[i][2]; // 2Z
D[i][0] = D[i][0]*D[i][0]; // XX
D[i][1] = D[i][1]*D[i][1]; // YY
D[i][2] = D[i][2]*D[i][2]; // ZZ
}
// MatrixMathPrint((Type*)D,_R,_C,1,"It is assumed that all Bit-value measurements lay on a Rotated Ellipsoid.");
MatrixMathTranspose((Type*)D,_R,_C,(Type*)DT); //calculate the transposed matrix DT
// MatrixMathPrint((Type*)DT,_C,_R,1,"DT is the transposed D matrix");
// Auxilliary matrix OrigDTD stores the original product matrix DTD, later to be used for matrix inversion check.
MatrixMathMultiply((Type*)DT, (Type*)D, C1,R,C1, (Type*)OrigDTD);
// Fill NVec[216x1] vector with ones.
for (i=0; i < _R ; i++) NVec[i] = 1.0;
// _R=216 _C=9
MatrixMathMatrixVectorMultiply((Type*)DT,(Type*)NVec,_C,_R,(Type*)DTNVec);
// vector DTNVec[9x1] = DT[9x216].NVec[216x1]
// START matrix inversion. Prepare inversion by down-scaling matrix D and DT with factor.
// Because of poor matrix condition down-scaling the matrices is required for succesfull inversion.
// -------------------------------------------------------------------------------------
MatrixMultiplyConstant((Type*)D , (Type*)ReduD ,R,C1, factor); //Reduce Matrix D with factor
MatrixMultiplyConstant((Type*)DT, (Type*)ReduDT ,C1,R, factor); //Reduce Matrix DT with factor
MatrixMathMultiply((Type*)ReduDT, (Type*) ReduD, C1,R,C1, (Type*) ReduDTD); //DTD is here reduced
// Gauss-Jordan Matrix Inversion.
// ---------------------------------------
MatrixMathInvert((Type*)ReduDTD,C1); //calculate inverse of down-scaled matrix DTD and store in DTD
// ----------------------------------------
// Up-Scaling the inverted down-scaled matrix
factor1=factor*factor;
MatrixMultiplyConstant((Type*)ReduDTD, (Type*)DTD, C1,C1, factor1);
// DTD = INV(DT.D)[[9x9] matrix of the oroginal (DT.D)[[9x9] matrix.
MatrixMathSciePrint((Type*)DTD,C1,C1,8 ,"The up-scaled inverted Matrix in scientific notation.");
// Check the matrix inversion.
MatrixMathMultiply((Type*)DTD, (Type*)OrigDTD, C1,C1,C1, (Type*) Dh1);
MatrixMathPrint((Type*)Dh1,_C,_C,6,"The Matrixinversion is Checked. If unity matrix then OK.");
// Inverted DTD[9x9] matrix is Up-scaled with factor1, and the inversion has been checked.
// END of Gauss-Jordan matrix inversion.
// ---------------------------------------------------------------------------------------
// Calculate the Least-Square solution vector Vec[1x9]=Vec[a,b,c,d,e,f,g,h,i]
printf("\n");
for (i=0; i < C1; i++) printf("DTNVec= %.0f\n", DTNVec[i]); // DTNVec[9x1]
printf("\n");
// Calculation of LS solution vector Vec[9x1]
MatrixMathMatrixVectorMultiply((Type*)DTD,(Type*)DTNVec,_C,_C,(Type*)Vec);
for (i=0; i < C1; i++)
{
printf("The LS solution vector Vec= %6.4e\n", Vec[i]);
}
// a XX+ b YY+ c ZZ+ d 2XY+ e 2XZ+ f 2YZ+ g 2X+ h 2Y+ i 2Z =1
// Vec[0]XX+Vec[1]YY+Vec[2]ZZ+Vec[3]2XY+Vec[4]2XZ+Vec[5]2YZ+Vec[6]2X+Vec[7]2Y+Vec[8]2Z =1
//== With the LS-calculation the optimum ellipsoid, being the locus of the raw magnetometer measurements, is found. ==
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
// Calcul-Step 3: Calculating the offset vector of the ellipsoid as per [Ref.2]
// The PARTIAL derivatives dF/dX=0, dF/dY=0, dF/dZ=0 from the second degree function
// F(X,Y,Z)=Vec[0]XX+Vec[1]YY+Vec[2]ZZ+Vec[3]2XY+Vec[4]2XZ+Vec[5]2YZ+Vec[6]2X+Vec[7]2Y+Vec[8]2Z-1
// result in 3 equations from which the center i.e. offset of the ellipsoid is calculated.
// The formalism of [Ref.2] is equivalent and calculates the offset with auxiliary matrix A3[3][3]
// and vector Vghi[3].
A3[0][0]=Vec[0]; //a
A3[0][1]=Vec[3]; //d
A3[0][2]=Vec[4]; //e
A3[1][0]=Vec[3]; //d
A3[1][1]=Vec[1]; //b
A3[1][2]=Vec[5]; //f
A3[2][0]=Vec[4]; //e
A3[2][1]=Vec[5]; //f
A3[2][2]=Vec[2]; //c
MatrixMathCopy((Type*) A3, 3,3, (Type*) HA3); //Store A3 in auxiliary matrix HA3 for inversion check
MatrixMathInvert((Type*) A3,3); //calculate inverse van A3[3x3] matrix
// Check the inversion of matrix A3
MatrixMathMultiply((Type*) A3, (Type*) HA3, 3,3,3, (Type*) HHA3);
MatrixMathPrint((Type*)HHA3,3,3,6,"HHA3[3x3] stores the Matrixinversion Check. If HHA3 is unity matrix then OK.");
MatrixMultiplyConstant((Type*) A3, (Type*) A3, 3, 3, -1); // multiply matrix A3 with -1
Vghi[0]=Vec[6]; //g
Vghi[1]=Vec[7]; //h
Vghi[2]=Vec[8]; //i
// Calculation Offset vector VS[3]
printf("\n");
MatrixMathMatrixVectorMultiply((Type*) A3,(Type*)Vghi,3,3,(Type*)VS);
printf("Offset vector X component VSx= %.0f\n",VS[0]);
printf("Offset vector Y component VSy= %.0f\n",VS[1]);
printf("Offset vector Z component VSz= %.0f\n",VS[2]);
printf("\n");
printf("\n");
//========= The offset vector of the rotated ellipsoid is determined ===========
//------ Calculating the matrix that represents the rotated ellipsoid, translated
// with the offset vector to the origin. Calculation as per [Ref.2] ------
A4[0][0]=Vec[0]; //a
A4[0][1]=Vec[3]; //d
A4[0][2]=Vec[4]; //e
A4[0][3]=Vec[6]; //g
A4[1][0]=Vec[3]; //d
A4[1][1]=Vec[1]; //b
A4[1][2]=Vec[5]; //f
A4[1][3]=Vec[7]; //h
A4[2][0]=Vec[4]; //e
A4[2][1]=Vec[5]; //f
A4[2][2]=Vec[2]; //c
A4[2][3]=Vec[8]; //i
A4[3][0]=Vec[6]; //g
A4[3][1]=Vec[7]; //h
A4[3][2]=Vec[8]; //i
A4[3][3]=-1;
T[0][0]=T[1][1]=T[2][2]=T[3][3]=1;
T[0][1]=T[0][2]=T[0][3]=T[1][0]=T[1][2]=T[1][3]=T[2][0]=T[2][1]=T[2][3]=0;
T[3][0]=VS[0];
T[3][1]=VS[1];
T[3][2]=VS[2];
// Above constructed matrix T{4x4} reads:
// {1,0,0,0},
// {0,1,0,0},
// {0,0,1,0},
// {VS[0],VS[1],VS[2],1}};
// Calculate the transposed matrix TT[4x4] of matrix T[4x4]
MatrixMathTranspose((Type*)T,4,4,(Type*)TT);
MatrixMathMultiply((Type*) A4, (Type*)TT, 4,4,4, (Type*) HATemp);
MatrixMathMultiply((Type*)T, (Type*)HATemp, 4,4,4, (Type*) B4);
MatrixMathSciePrint((Type*)B4,4,4,8 ,"B4[4x4] stores T[4x4].A[4x4].TT[4x4].");
// Calculate the value of the ellipsoid function F(X,Y,Z) for the offset values X0, Y0, Z0
Type FX0Y0Z0= Vec[0]*VS[0]*VS[0] +Vec[1]*VS[1]*VS[1] +Vec[2]*VS[2]*VS[2] +
2*Vec[3]*VS[0]*VS[1] +2*Vec[4]*VS[0]*VS[2] +2*Vec[5]*VS[1]*VS[2] +
2*Vec[6]*VS[0] +2*Vec[7]*VS[1] +2*Vec[8]*VS[2] -1;
printf("\n");
// Note: B4[3][3]= g*X0+h*Y0+i*Z0-1 = Vec[6]*VS[0] +Vec[7]*VS[1] +Vec[8]*VS[2] -1;
// Voor afleiding van B4[3][3]=F(X0,Y0,Z0) zie mijn notes.
printf("Note: The value of B4[3][3] is equal to F(X0,Y0,Z0)= %.5f\n",FX0Y0Z0);
printf("\n");
// Extract the 3x3 matrix B3 from the 4x4 matrix B4
for (i=0; i < 3; i++)
{
for (j=0;j < 3;j++)
{
B3[i][j] = B4[i][j];
}
}
// MatrixMathSciePrint((Type*) B3,3,3,8 ,"B3[3x3].");
double val;
val = -1/ B4[3][3];
// printf("The value of val= %.5f\n",val);
MatrixMultiplyConstant((Type*) B3, (Type*) B3, 3, 3, val);
printf("\n");
MatrixMathSciePrint((Type*) B3,3,3,8 ,"B3[3x3] the fit-matrix of the (tilted) shifted ellipsoid is:");
printf("\n\n");
printf("Calculation of the Offset-Vector and fit-matrix of rotated Ellipsoid translated to
the origin has been completed.\n");
printf("\n");
// ---- Calculation of the (ellipsoid) Offset-Vector, and the rotated, translated ellipsoid fit-matrix
// as per [Ref.2] is completed --------------------------
printf("********************************************************************************************");
printf("\n");
printf("********************************************************************************************");
printf("\n");
// xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
// Calcul-STEP 4: Eigenvalue calculation of fit-matrix A[3x3].
MatrixMathCopy((Type*) B3,3,3, (Type*)A); //For convenience matrix B3 is renamed to A
// MatrixMathSciePrint((Type*)A,z,z,12,"The symmetric fit-matrix A to calculate eigen values
and vectors in scientific notation is: ");
// Calculate the determinant of fit-matrix A[3x3] for later check of eigenvalue product.
// DetVal=MatrixMathDeterminant((Type *) A, z);
DetVal=MatrixMathDeterminant((Type *) A, n);
printf("Determinant of Matrix A = ");
printf("%6.6e", DetVal);
printf("\n");
// To improve the Jakobi eigenvalue calculation the ellipsoid A[3][3] will be upscaled with upscalefactor .
Type upscalefactor = 10e+08;
Type downscalefactor; // is 1/upscalefactor
MatrixMultiplyConstant((Type*)A, (Type*)UpsA,z,z,upscalefactor);
MatrixMathSciePrint((Type*)UpsA,z,z,6,"The symmetric upscaled matrix UpsA[3][3] in scientific notation is: ");
// **********************************************************************************************
// **********************************************************************************************
// ******************** Jakobi Eigenvalue and Eigenvector Code Ref.5 ****************************
// **********************************************************************************************
// **********************************************************************************************
// Jakobi Step 1: Read the symmetric Matrix UpsA
// Jakobi Step 2: Initialize d=UpsA and S=I
MatrixMathCopy((Type*)UpsA, z, z, (Type*)d); //Initialize matrix d = matrix UpsA
MatrixMath_I_Matrix((Type*)s, z, z); //Initialize s as I-Matrix
// Check print matrix d
// MatrixMathPrint((Type*)d, z, z, 12, "The copy matrix of matrix A=d[3x3] is: ");
float flag=1;
while(flag>zero)
{
// Jakobi Step 3: Find the largest off-diagonal element in d=|d[ij]| and let it be d[ij]
i=1;
j=2;
hmax = fabs(d[i][j]);
for (p = 0; p < z ; p ++)
{
for (q = 0; q < z ; q ++)
{
if (p!=q)
if (hmax < fabs(d[p][q]))
{
hmax=fabs(d[p][q]);
i=p; //index i, used below, is determined here
j=q; //index j, used below, is determined here
}
}
}
// printf("\n");
printf("Max off-line element of matrix d=> d[ij]= %.12f\n", d[i][j]);
// printf("\n");
// Jakobi Step 4: Find the rotational angle Theta
if (d[i][i]==d[j][j])
{
if (d[i][j] > 0)
{
theta=pi/4;
}
else
{
theta=-pi/4;
}
}
else
{
theta=0.5*atan(2*d[i][j]/(d[i][i]-d[j][j]));
}
printf("\n");
printf("The rotational angle Theta= %.4f\n", theta);
// Serial.print(theta,6);
printf("\n");
// Jakobi Step 5: Computate the matrix s1=s[p][q]
// Note: s1t is the transposed of s1
MatrixMath_I_Matrix((Type*)s1, z, z); //Initialize S1 as I-Matrix
MatrixMath_I_Matrix((Type*)s1t, z, z); //Initialize S1T as I-Matrix
s1[i][i]= cos(theta);
s1[j][j]= s1[i][i];
s1[j][i]= sin(theta);
s1[i][j]=-s1[j][i];
s1t[i][i]=s1[i][i];
s1t[j][j]=s1[j][j];
s1t[i][j]=s1[j][i];
s1t[j][i]=s1[i][j];
MatrixMathPrint((Type*)s1 , z, z, 3, "The matrix s1 is: ");
MatrixMathPrint((Type*)s1t, z, z, 3, "The matrix s1t is: ");
// Jakobi Step 6: Calculate D= S1TxDxS1 and S=SxS1
MatrixMathMultiply((Type*)s1t, (Type*)d, z, z, z, (Type*)temp);
MatrixMathMultiply((Type*)temp, (Type*)s1, z, z, z, (Type*)d);
MatrixMathSciePrint((Type*)d, z, z,6, "The Jakobi eigenvalue matrix of the upscaled matrix UpsA is: ");
MatrixMathMultiply((Type*)s,(Type*)s1, z, z, z, (Type*)temp);
MatrixMathCopy((Type*)temp, z, z, (Type*)s);
MatrixMathSciePrint((Type*)s, z, z, 6,"The Jakobi eigenvector matrix of the upscaled matrix UpsA is: ");
flag=0;
for (i = 0; i < z ; i ++) //check the sum of non-diagonal terms
for (j = 0; j < z ; j ++)
{
if(i!=j)
{
if(fabs(d[i][j]) > zero ) // zero=0.0001;
{
flag=flag+fabs(d[i][j]);
}
}
}
printf("\n");
printf("Flag= %.6f\n", flag);
printf("\n\n");
}
// ****************** END of Jakobi Eigenvalue While Loop ******************
// ***************** Check Calculation of Jakobi Eigenvalues ***************
// DOWNSCALING the Jakobi eigenvalues with:
downscalefactor=1/upscalefactor;
//check effect of changing order of eigenvalues
Type a=d[0][0];
Type b=d[1][1];
Type c=d[2][2];
d[0][0]=c; //a,b,c b,c,a c,a,b
d[1][1]=a;
d[2][2]=b;
printf("Finally the Jakobi Eigenvalues are: ");
for (i = 0; i < n ; i ++)
{
JacoEiVals[i][i]=d[i][i]*downscalefactor;
printf("%.6e ", JacoEiVals[i][i]);
}
printf("\n\n");
// Check if the product of the Jakobi eigenvalues equal is to determinant
// of the matrix A. All downscaled.
float eivalproduct =1;
for (i = 0; i < n ; i ++)
{
eivalproduct *= JacoEiVals[i][i];
}
printf(" The product of the 3 Eigenvalues is: %0.6e", eivalproduct);
printf("\n");
printf(" Determinant of Matrix A[3x3]= ");
printf("%6.6e", DetVal);
printf("\n\n");
printf("\nAAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA \n");
printf("End Eigenvalue Calculs - End Eigenvalue Calculs - End Eigenvalue Calculs - End Eigenvalue Calculs\n");
printf("AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA \n");
// xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
// Calcul-Step 5: Eigenvector calculation of fit-matrix A[3x3].
printf("\nBBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB \n");
printf("Start Gauss-Elim Eigenvector Calculs - Start Gauss-Elim Eigenvector Calculs \n");
printf("BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB \n");
// Start of loop over the eigenvalues of matrix A[3][3] to calculate the eigenvectors with Gauss-Elimination
for (l=0; l < n; l++)
{
MatrixMath_I_Matrix((Type*) temp,n,n); //was z
printf("\n");
printf(" The selected Jakobi eigenvalue to calculate its eigenvector is: %.6e\n", JacoEiVals[l][l]);
MatrixMultiplyConstant((Type*) temp,(Type*) temp,n,n,JacoEiVals[l][l]);
// Selected eigenvector for eigenvector calc.
MatrixMathSubtract((Type*) A,(Type*) temp,n,n,(Type*) aG);
// aG is the ellipsoid fit-matrix reduced by the selected eigenvalue
// MatrixMathSciePrint((Type*) aG,n,n,6,"The ellipsoid fit-matrix reduced by the selected eigenvalue is: ");
printf("\n");
GaussElimination((Type*) aG,n,n); // Gauss elimination produces the Eigenvector Matrix EiVec[3][3]
}
printf("\n");
MatrixMathSciePrint((Type*)EiVec,n,n,6,"The Gauss eliminated Eigenvector Matrix is: ");
printf("\n");
// Normalizing the Gauss eliminated eigenvectors EiVec
for (j = 0; j < z ; j ++){
for (i = 0; i < z ; i ++){
hEiVec[i][j]=EiVec[i][j];
}
}
Type Nhs1;
Nhs1= sqrt(hEiVec[0][0]*hEiVec[0][0] + hEiVec[1][0]*hEiVec[1][0] + hEiVec[2][0]*hEiVec[2][0]);
NormEiVec[0][0]=hEiVec[0][0]/Nhs1;
NormEiVec[1][0]=hEiVec[1][0]/Nhs1;
NormEiVec[2][0]=hEiVec[2][0]/Nhs1;
Nhs1= sqrt(NormEiVec[0][0]*NormEiVec[0][0] + NormEiVec[1][0]*NormEiVec[1][0] + NormEiVec[2][0]*NormEiVec[2][0]);
printf("Length check of normalized Gauss-EiVec1 eigenvector Nhs1= %.6e",Nhs1);
printf("\n");
Type Nhs2;
Nhs2= sqrt(hEiVec[0][1]*hEiVec[0][1] + hEiVec[1][1]*hEiVec[1][1] + hEiVec[2][1]*hEiVec[2][1]);
NormEiVec[0][1]=hEiVec[0][1]/Nhs2;
NormEiVec[1][1]=hEiVec[1][1]/Nhs2;
NormEiVec[2][1]=hEiVec[2][1]/Nhs2;
Nhs2= sqrt(NormEiVec[0][1]*NormEiVec[0][1] + NormEiVec[1][1]*NormEiVec[1][1] + NormEiVec[2][1]*NormEiVec[2][1]);
printf("Length check of normalized Gauss-EiVec2 eigenvector Nhs2= %.6e",Nhs2);
printf("\n");
Type Nhs3;
Nhs3= sqrt(hEiVec[0][2]*hEiVec[0][2] + hEiVec[1][2]*hEiVec[1][2] + hEiVec[2][2]*hEiVec[2][2]);
NormEiVec[0][2]=hEiVec[0][2]/Nhs3;
NormEiVec[1][2]=hEiVec[1][2]/Nhs3;
NormEiVec[2][2]=hEiVec[2][2]/Nhs3;
Nhs3= sqrt(NormEiVec[0][2]*NormEiVec[0][2] + NormEiVec[1][2]*NormEiVec[1][2] + NormEiVec[2][2]*NormEiVec[2][2]);
printf("Length check of normalized Gauss-EiVec3 eigenvector Nhs3= %.6e",Nhs3);
printf("\n\n");
MatrixMathTranspose((Type*) NormEiVec,n,n, (Type*) NormEiVecTp);
MatrixMathMultiply((Type*) NormEiVecTp, (Type*) NormEiVec,n,n,n, (Type*) temp);
MatrixMathSciePrint((Type*)temp,n,n,6,"Checking orthogonality of the Gauss Eigenvector Matrix: ");
printf("\n");
// ============================================================================
MatrixMathSciePrint((Type*)NormEiVec, n, n, 6, "The orthonormal matrix of Gauss eigenvectors is: ");
printf("\n");
MatrixMathMultiply((Type*) A, (Type*) NormEiVec,n,n,n, (Type*) temp);
MatrixMathMultiply((Type*) NormEiVecTp, (Type*) temp,n,n,n, (Type*) TstJacoEiVals);
MatrixMathSciePrint((Type*)TstJacoEiVals, n, n, 6, "Checking the Jacobi Eigenvalues by transforming
ellipsoid fit-matrix A with the orthonormal eigenvector matrices:");
printf("\n");
printf("The Jakobi Eigenvalues are: ");
for (i = 0; i < n ; i ++)
{
printf("%.6e ", JacoEiVals[i][i]);
}
printf("\n\n");
printf("\nBBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB \n");
printf("BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB \n");
Type SQRTJacoEiVals[3][3]; //SQRT of Eigenvalue matrix JacoEiVals of fit-matrix A
// Type Wmin1[3][3]; //Calibration matrix W**-1
SQRTJacoEiVals[0][0]= sqrt(JacoEiVals[0][0]);
SQRTJacoEiVals[0][1]= 0.0;
SQRTJacoEiVals[0][2]= 0.0;
SQRTJacoEiVals[1][0]= 0.0;
SQRTJacoEiVals[1][1]= sqrt(JacoEiVals[1][1]);
SQRTJacoEiVals[1][2]= 0.0;
SQRTJacoEiVals[2][0]= 0.0;
SQRTJacoEiVals[2][1]= 0.0;
SQRTJacoEiVals[2][2]= sqrt(JacoEiVals[2][2]);
MatrixMathSciePrint((Type*)SQRTJacoEiVals, z, z, 6, "Square Root of the diagonal eigenvalue matrix JacoEiVals: ");
MatrixMathMultiply((Type*) NormEiVec, (Type*) SQRTJacoEiVals,n,n,n, (Type*) temp);
MatrixMathMultiply((Type*) temp, (Type*) NormEiVecTp,n,n,n, (Type*) Wmin1);
MatrixMathSciePrint((Type*)Wmin1, z, z, 6, "Calculation of W**-1=sqrt(A): ");
MatrixMathMultiply((Type*) Wmin1, (Type*) Wmin1,n,n,n, (Type*) temp);
MatrixMathSciePrint((Type*)temp, z, z, 6, "Check-Calculation of Wmin1xWmin1=A : ");
printf("\n");
// printf("1e Calibration Step: Shifting the raw ellipsoid data to origin coordinate system x',y',z'. \n");
for (i=0;i < R;i++) //R=216
{
for (j=0;j < L;j++) //L=3
{
// Apply the calculated shift to the 216 raw Bit-Value measurements.
DCalib1[i][j] = DFileIn[i][j]- VS[j];
}
}
int count=0;
while (count < R) //R=216
{
for (j=0;j < L;j++) //L=3
{
VS1[j] = DCalib1[count][j]; //Store the 3 values of each row in vector VS1
}
for (i=0;i < 3;i++)
{
VS2[i]=0;
for (j=0;j < 3;j++)
{
VS2[i]+= Wmin1[i][j]*VS1[j]; //multiply VS1 values with calibratie matrix
}
}
for (i=0;i < 3;i++)
{
DCalib2[count][i] = VS2[i];
}
count=count+1;
}
printf("\n The Calibrated measurements are: \n");
for (i=0;i < R;i++) //R=216 Rows
{
radius = sqrt(DCalib2[i][0]*DCalib2[i][0] + DCalib2[i][1]*DCalib2[i][1] + DCalib2[i][2]*DCalib2[i][2]);
for (j=0;j < L;j++) //L=3 Cols
{
printf(" %.3f", DCalib2[i][j]);
printf(" ");
}
printf(" %.3f", radius); //colmn 4
printf("\n");
}
printf("\n");
// For quality control, export the calibrated measurements as GnuPlot compatible file.
exportCalibMagdata2textfile((Type*) DCalib2);
printf("\n\n\nDisplay and export the Final Calibration Results: \n\n");
exportFinalCalibResults2textfile();
printf("\n");
i=10;
while (i < 100) //Endless loop to make sure .exe keeps results displaying on monitor
{
i++;
i--;
}
}
/*
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
*/
//§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
//§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
//++++++++++++++++++++++ Start of function Library ++++++++++++++++++++++++++++++
//§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
//***************** IMPORT INTEGER Matrix via Text File Routine ******************
//Last update 16-2-2019
void importInteger3ColDataTextFile(Type* A)
{
// read all characters from a file, outputting only the letter 'b's
// it finds in the file
FILE *fp;
int i,j,k, val;
Type val1;
char* filename = "d:\\Pelles C-SourceCode\\sensordata\\recorded-sensordata.txt";
char c;
int C=3;
char dest[10];
i=0;
j=0;
k=0;
fp = fopen(filename, "r");
if (fp == NULL)
{
printf("Could not open file %s",filename);
// return 1;
}
// this while-statement assigns into c, and then checks against EOF:
while((c = fgetc(fp)) != EOF)
{
if (c == '=')
{
if (k > 2) j++;
for(i=0;i < 10;i++)
{
dest[i]= 0;
}
i=0;
while((c = fgetc(fp)) != ';')
{
dest[i]= c;
i++;
}
val = atoi(dest);
val1 =(Type)val;
// printf("j = %d", j);
if (k>(C-1)) k=0;
// printf(" k = %d\n", k);
// printf("String value = %s\n", dest);
// printf("Int value = %d\n", val);
// printf("Type value = %.2f\n", val1);
// printf("\n");
A[C*j+k]=val1;
k++;
}
}
fclose(fp);
//-------------
/*
for (i=0;i < R;i++)
{
for (j=0;j < C;j++)
{
printf(" Matrix DF = %.4f\n", A[C*i+j]);
}
}
printf("\n\n");
// return 0;
*/
//------------
}
//**********************************************************************************
//******* Matrix-Vector Multiplication Routine **********************************
// V = A*U
void MatrixMathMatrixVectorMultiply(Type* A, Type* U, int m, int p, Type* V)
{
// A = input matrix A(rows x cols)
// U = input vector U(rows)
// V = output vector V(rows)= A(rows x cols)*U(rows)
// m = number of rows in A
// p = number of columns in A = number of rows in U
// V = output vector = A*U(m)
int i, j, k;
Type Ah[m][p]; //hulp array
Type Uh[p]; //hulp input vector
Type Vh[p];
for (i=0;i < m;i++)
{
for(j=0;j < p;j++)
{
Ah[i][j] = A[p*i+j];
}
}
for(j=0;j < p;j++)
{
Uh[j] = U[j];
// printf("\t%10.6f", Uh[j]);
// printf("\n");
}
for (i=0;i < m;i++)
{
V[i]=0;
for (k=0;k < p;k++)
{
V[i]+= Ah[i][k]*Uh[k];
}
}
/*
printf("\n");
for (i=0;i < p;i++)
{
printf("\t%.1f", V[i]);
printf("\n");
}
*/
}
//************* Matrix Inversion Routine *****************************************
// * 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(Type* 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
Type tmp; // used for finding max value and making column swaps}
/*
//printf("\n");
printf("The Matrix to be inverted with Gauss-Jordan is: \n");
for (i=0;i < n;i++)
{
for(j=0;j < n;j++)
{
printf("%.4f", A[n*i+j]);
printf(" ");
}
printf("\n");
}
*/
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;
//printf("%.4f\n", tmp);
}
}
// check for matrix singularity
if (A[pivrow*n+k] == 0.0f)
{
printf(" \n");
printf("Inversion failed due to singular matrix\n");
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;
//printf("%lld\n", A[pivrow*n+j]);
}
}
pivrows[k] = pivrow; // record row swap (even if no swap happened)
tmp = 1.0f/A[k*n+k]; // invert pivot element
// tmp = A[k*n+k];
A[k*n+k] = 1.0f; // This element of input matrix becomes result matrix
//printf("%lld\n", A[k*n+k]);
//printf("%.8f\n", tmp);
// Perform row reduction (divide every element by pivot)
for (j = 0; j < n; j++)
{
A[k*n+j] = A[k*n+j]*tmp;
}
//OK
// 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.0;
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 Gauss-Jordan Matrix Inversion Function ********************
//******************** Start Gauss Eliminatiom Function ************************
void GaussElimination(Type* _aG, int row, int col)
{
// n=3= row, m=4= col;
// _aG = augmented input matrix (row x col)
Type aGh[row][col]; //hulp array
int i,j,k;
printf(" The augmented input matrix is: \n");
for (i=0; i < row; i++)
{
for(j=0; j < col; j++)
{
aGh[i][j] = _aG[col*i+j];
printf(" aGh[%d][%d]= %.6e",i,j,aGh[i][j]);
}
printf("\n");
}
for(i=0;i < n-1;i++)
{
if(aGh[i][i] == 0.0)
{
printf("Mathematical Error! _aG[%d][%d]= %.12f\n",i,i, aGh[i][i]);
exit(0);
}
for(j=i+1; j < n; j++)
{
// printf("\n\n aGh[%d][%d]= %.6e",j,i, aGh[j][i]);
// printf(" aGh[%d][%d]= %.6e\n",i,i,aGh[i][i]);
ratio = aGh[j][i]/aGh[i][i];
for(k=0; k < n+1; k++)
{
// printf("\n i= %d", i);
// printf(" j= %d", j);
// printf(" k= %d\n", k);
aGh[j][k] = aGh[j][k] - ratio * aGh[i][k];
// printf(" aGh[%d][%d]= %.6e",j,k,aGh[j][k]);
}
}
}
printf("\n");
printf(" The triangulized augmented input matrix is: \n");
for (i=0; i < row; i++)
{
for(j=0; j < col; j++)
{
// aGh[i][j] = _aG[col*i+j];
printf(" aGh[%d][%d]= %.6e",i,j,aGh[i][j]);
}
printf("\n");
}
// printf("\n");
// MatrixMathSciePrint((Type*) aGh,n,n,6,"The triangulized augmented input Matrix is: ");
printf("\n");
// printf(" n=%d",n);
// Calculate of eigenvector matrix (the matrix columns are the eigenvectors)
if( (abs(aGh[n-1][n-1]) || n>3) > 0.00000000001)
{
printf("Eigenvector Error! aGh[%d][%d]= %.12f\n",n,n, aGh[i][i]);
exit(0);
}
EiVec[0][l]= aGh[1][1] * aGh[0][2]/(aGh[0][0]*aGh[1][2])-aGh[0][1]/aGh[0][0];
EiVec[1][l]= 1;
EiVec[2][l]=-aGh[1][1]/aGh[1][2];
printf("\n");
}
//*********************** End Gauss Elimination Function *************************
// ***************** Start V2-Matrix Determinant Calc-Routine *****************
// Last Update: Bruno Best 09-7-20, 13:00 hrs
Type MatrixMathDeterminant(Type * _D, int _t)
{
// A = input matrix AND result matrix
// n = number of rows = number of columns in A (n x n)
int i, j;
Type _b[_row][_col];
Type res;
// printf("\n\n");
// printf("The SQUARE matrix for which the determinant is calculated is: \n");
for (i=0;i < _row;i++)
{
for(j=0;j < _col;j++)
{
_b[i][j] = _D[_row*i+j]; //load the to inverted data matrix
// printf("%.12f", _b[i][j]);
// printf(" ");
}
// printf("\n");
}
res= determinant(_b, _t);
return(res);
}
//--------------------------------------------------------------------------------------------
// ******************** Function to calculating Determinant of the Matrix ********************
Type determinant(Type _a[_row][_col], int _t)
{
Type s=1,det=0;
Type _d[_row][_col];
int i,j,m,n,c;
if (_t==1)
{
return (_a[0][0]);
}
else
{
det=0;
for (c=0;c < _t;c++)
{
m=0;
n=0;
for (i=0;i < _row;i++)
{
for (j=0;j < _col;j++)
{
_d[i][j]=0;
if (i != 0 && j != c)
{
// printf("%.6f", a[i][j]);
_d[m][n]=_a[i][j];
if (n < (_col-2))
n++;
else
{
n=0;
m++;
}
}
}
}
det=det + s * (_a[0][c] * determinant(_d,_t-1));
s=-1 * s;
}
}
return (det);
}
//***************** Matrix Printing Routine ************************************
// Uses tabs to separate numbers under assumption printed Type width won't cause problems
//void MatrixMathPrint(Type* A, int m, int n, int decimals, String label){
void MatrixMathPrint(Type* A, int row, int col, int decimals, char *Stringlabel)
{
// A = input matrix (row x col)
int i,j;
Type hval;
printf(" \n");
// print the string
while(*Stringlabel != '\0') {
printf("%c", *Stringlabel);
// move the ptr pointer to the next memory location
Stringlabel++;
}
printf(" \n");
for (i=0; i<row; i++){
for (j=0;j<col;j++){
hval=A[col*i+j];
printf("%+.*f", decimals, hval);
printf("\t");
}
printf(" \n");
}
}
//***************** Matrix Scientific Printing Routine **********************************
// Uses tabs to separate numbers under assumption printed Type width won't cause problems
//void MatrixMathSciePrint(Type* A, int m, int n, int decimals, String label)
void MatrixMathSciePrint(Type* A, int row, int col, int decimals, char *Stringlabel)
{
// A = input matrix (rows x cols)
int i,j;
Type hval;
printf(" \n");
// print the string
while(*Stringlabel != '\0') {
printf("%c", *Stringlabel);
// move the ptr pointer to the next memory location
Stringlabel++;
}
printf(" \n");
for (i=0; i<row; i++){
for (j=0;j<col;j++){
hval=A[col*i+j];
printf("%+.*f", decimals, hval);
printf("\t");
}
printf(" \n");
}
}
//*********** Copy Matrix Type to Type *****************************************
void MatrixMathCopy(Type* A, int row, int col, Type* B)
{
int i, j, k;
printf("\n");
for (i=0;i<row;i++)
{
for(j=0;j<col;j++)
{
B[col*i+j] = A[col*i+j];
// printf("%.6f", A[col*i+j]);
// printf(" %.6f", B[col*i+j]);
// printf("\t");
}
// printf("\n");
}
}
//************ Generate a I-Matrix ***************************************************
void MatrixMath_I_Matrix(Type* A, int row, int col)
{
int i, j;
for (i=0;i<row;i++)
for (j=0;j<col;j ++)
{
A[row*i+j]=0;
}
for (i=0;i<row;i ++)
{
A[row*i+i]=1;
}
}
//***************** Multiply Matrix with a CONSTANT ************************************
// void MatrixMath::Print(float* A, int m, int n, String label){
void MatrixMultiplyConstant(Type* A, Type* B, int row, int col, Type constant)
{
// A = input matrix (row x col)
int i,j;
for (i=0;i<row; i++){
for (j=0;j<col;j++){
B[col*i+j]= constant * A[col*i+j];
}
}
}
//******* Matrix Multiplication Routine **********************************
// C = A*B
void MatrixMathMultiply(Type* A, Type* B, int _p, int _r, int _q, Type* 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<_p;i++)
{
for(j=0;j<_q;j++)
{
C[_q*i+j]=0;
for (k=0;k<_r;k++)
{
C[_q*i+j]= C[_q*i+j]+ A[_r*i+k]* B[_q*k+j];
}
}
}
}
//*********** Matrix Transpose Routine ****************************************
//
void MatrixMathTranspose(Type* A, int row, int col, Type* C)
{
// A = input matrix (row x col)
// C = output matrix = the transpose of A (row x col)
int i, j;
for (i=0;i<row;i++)
for(j=0;j<col;j++)
C[row*j+i]=A[col*i+j];
}
//*********** Matrix Addition Routine ****************************************
void MatrixMathAddition(Type* A, Type* B, int row, int col, Type* C)
{
// A = input matrix (row x col)
// B = input matrix (row x col)
// C = output matrix = A-B (row x col)
int i, j;
for (i=0;i<row;i++)
for(j=0;j<col;j++)
C[col*i+j]=A[col*i+j]+B[col*i+j];
}
//*********** Matrix Subtraction Routine ****************************************
void MatrixMathSubtract(Type* A, Type* B, int row, int col, Type* C)
{
// A = input matrix (row x col)
// B = input matrix (row x col)
// C = output matrix = A-B (row x col)
int i, j;
for (i=0;i<row;i++){
for(j=0;j<col;j++){
C[row*i+j]=A[row*i+j]-B[row*i+j];
}
}
}
//***************** Export RawMag-Data to a GnuPlot-RAWData Text File *********************************
void exportRawMagdata2textfile(Type* A)
{
FILE *fp1;
float val;
int i;
char* filename = "d:\\Pelles C-SourceCode\\sensordata\\GnuPlot-RawData.txt";
char dest[50]; //26
char desth[15]; //10
char destk[15]; //10
char destl[15]; //10
i=0;
fp1 = fopen(filename, "w");
if (fp1 == NULL)
{
printf("Could not open file %s",filename);
// return 1;
exit(EXIT_FAILURE);
}
while(i < 3*R)
{
strcpy(dest, ""); //empty the dest String
val = A[i];
sprintf(desth, "%f%s", val, " , ");
strcat(dest, desth); // ;
i++;
val= A[i];
sprintf(destk, "%f%s", val, " , ");
strcat(dest, destk);
i++;
val= A[i];
sprintf(destl, "%f%s", val, "");
strcat(dest, destl);
i++;
// printf("Raw-Gnuplot-Data are: %s\n", dest);
fprintf(fp1,"%s\n", dest); // Write data to file
strcpy(dest, ""); //empty the dest String
}
// Close file to save file data
fclose(fp1);
printf("\n");
// Success message
printf("Raw-Data Gnu-Plot File is created and saved successfully. \n");
}
//§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
//§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
//************** Export Calibrated Mag-Data to a GnuPlot-CALIBData Text File *******
void exportCalibMagdata2textfile(Type* A)
{
FILE *fp1;
float val;
int i;
char* filename = "d:\\Pelles C-SourceCode\\sensordata\\GnuPlot-CalibData.txt";
char dest[50]; //26
char desth[15]; //10
char destk[15]; //10
char destl[15]; //10
i=0;
fp1 = fopen(filename, "w");
if (fp1 == NULL)
{
printf("Could not open file %s",filename);
// return 1;
exit(EXIT_FAILURE);
}
while(i < 3*R)
{
strcpy(dest, ""); //empty the dest String
val = A[i];
sprintf(desth, "%f%s", val, " , ");
strcat(dest, desth); // ;
i++;
val= A[i];
sprintf(destk, "%f%s", val, " , ");
strcat(dest, destk);
i++;
val= A[i];
sprintf(destl, "%f%s", val, "");
strcat(dest, destl);
i++;
// printf("Calb-Gnuplot-Data are: %s\n", dest);
fprintf(fp1,"%s\n", dest); // Write data to file
strcpy(dest, ""); //empty the dest String
}
// Close file to save file data
fclose(fp1);
// printf("\n");
// Success message
printf("Gnu-Plot file of calibrated measurements is created and saved successfully. \n");
}
//§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
//§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
//************** Export Final Mag-Calibration Results to a Text File **************
//Last update 28-12-2022
// * C program to create a file and write data to file.
void exportFinalCalibResults2textfile(void)
{
FILE *fp;
int val;
float valf;
char* filename = "d:\\Pelles C-SourceCode\\sensordata\\Final-Calib-Results.txt";
char dest[100];
char desth[20];
char destk[20];
char destl[20];
// const char str1[] = " ";
//-----------Converting Calibration to Sensor Config Format -----------
const char strWmin20[] = "// Sensor Calibration Data in Config Format.";
const char strVSxyz[] = "// x, y, z-Component of Hard-Iron Shift vector Mag_Vsi in bit-values is: ";
const char strVSx[] = " Mag_VSx = ";
const char strVSy[] = " Mag_VSy = ";
const char strVSz[] = " Mag_VSz = ";
fp = fopen(filename, "w");
if (fp == NULL)
{
printf("Could not open file %s",filename);
// return 1;
exit(EXIT_FAILURE);
}
fprintf(fp,"%s\n", strWmin20);
// time function
printf("\n");
time_t t = time(NULL);
struct tm *tm = localtime(&t);
fprintf(fp,"%s", asctime(tm));
fprintf(fp,"\n\n");
printf("%s\n", strVSxyz);
fprintf(fp,"%s\n", strVSxyz);
for (i=0;i < 3;i++)
{
dest[0]='\0';
if (i==0) strcat(dest, strVSx);
if (i==1) strcat(dest, strVSy);
if (i==2) strcat(dest, strVSz);
val=(int)VS[i];
sprintf(desth, "%d%s", val, ";");
strcat(dest, desth);
printf("%s\n", dest);
fprintf(fp,"%s\n", dest);
// dest[0]='\0';
strcpy(dest, "");
}
printf("\n\n");
fprintf(fp,"\n\n");
//---------------------------------------------------
const char strWmin1[] = "// The Mag-Calibration Matrix Wmin1 or W**-1=sqrt(A[3x3]) is: !!!!!";
printf("%s\n", strWmin1);
fprintf(fp,"%s\n", strWmin1);
printf("\n");
fprintf(fp,"\n");
const char strWmin1_0[] = "// First row of Wmin1 MagCTp row=0";
printf("%s\n", strWmin1_0);
fprintf(fp,"%s\n", strWmin1_0);
const char strWmin1_0_0[] = " Wmin1[0][0] = ";
const char strWmin1_0_1[] = " Wmin1[0][1] = ";
const char strWmin1_0_2[] = " Wmin1[0][2] = ";
for (i=0;i < 3;i++)
{
dest[0]='\0';
if (i==0) strcat(dest, strWmin1_0_0);
if (i==1) strcat(dest, strWmin1_0_1);
if (i==2) strcat(dest, strWmin1_0_2);
valf=Wmin1[i][0];
sprintf(desth, "%e%s", valf, ";");
strcat(dest, desth);
printf("%s\n", dest);
fprintf(fp,"%s\n", dest);
// dest[0]='\0';
strcpy(dest, "");
}
printf("\n");
fprintf(fp,"\n");
const char strWmin1_1[] = "// Second row of Wmin1 MagCTp row=1";
printf("%s\n", strWmin1_1);
fprintf(fp,"%s\n", strWmin1_1);
const char strWmin1_1_0[] = " Wmin1[1][0] = ";
const char strWmin1_1_1[] = " Wmin1[1][1] = ";
const char strWmin1_1_2[] = " Wmin1[1][2] = ";
for (i=0;i < 3;i++)
{
dest[0]='\0';
if (i==0) strcat(dest, strWmin1_1_0);
if (i==1) strcat(dest, strWmin1_1_1);
if (i==2) strcat(dest, strWmin1_1_2);
valf=Wmin1[i][1];
sprintf(desth, "%e%s", valf, ";");
strcat(dest, desth);
printf("%s\n", dest);
fprintf(fp,"%s\n", dest);
// dest[0]='\0';
strcpy(dest, "");
}
printf("\n");
fprintf(fp,"\n");
const char strWmin1_2[] = "// Third row of Wmin1 MagCTp row=2";
printf("%s\n", strWmin1_2);
fprintf(fp,"%s\n", strWmin1_2);
const char strWmin1_2_0[] = " Wmin1[2][0] = ";
const char strWmin1_2_1[] = " Wmin1[2][1] = ";
const char strWmin1_2_2[] = " Wmin1[2][2] = ";
for (i=0;i < 3;i++)
{
dest[0]='\0';
if (i==0) strcat(dest, strWmin1_2_0);
if (i==1) strcat(dest, strWmin1_2_1);
if (i==2) strcat(dest, strWmin1_2_2);
valf=Wmin1[i][2];
sprintf(desth, "%e%s", valf, ";");
strcat(dest, desth);
printf("%s\n", dest);
fprintf(fp,"%s\n", dest);
// dest[0]='\0';
strcpy(dest, "");
}
printf("\n\n");
fprintf(fp,"\n\n");
// Close file to save file data
fclose(fp);
// Success message with Date and Time
printf("Final Calibration Results saved successfully to file 'Final-Calib-Results.txt' \n");
printf("%s\n", asctime(tm));
}
////**** End Sec2 Calib.Utility 4: Magnetometer Calibration ****
// Last Update: 28-12-22
// Change IDE Font: Tools=>Options=>Source=>Font
// Code to be used for full sensor calibration!!!!!
// The code is developed with: PELLES C for Windows, Version 9.00.9 as Win64 Console Program
// For accuracy reasons the code should be compiled, and excecuted in 64 bit.
// Pelles-ST-Rotated-Ellipsoid-Fitting-for-Sensor-Calibration
// MAGNETOMETER CALIBRATION using 216 measurements.
// GAUSS-JORDAN (GJ) is used for matrix inversion
// 1: Calculation of Calibration Shift Vector Vx, Vy, Vz.
// 2: Calculation of the symmetric ellipsoid fit-matrix that will deliver the eigenvalues and eigenvectors.
// Eigenvalues calculated with Jakobi Eigenvalue Code
// Improved Eigenvector calculation using Gauss-elimination method.
// Ref.2: ST Design Tip DT0059. Ellipsoid or sphere fitting for sensor calibration by Andrea Vitali.
// Ref.3: Freescale Semiconductor Application Note. Document Number: AN4246 Rev. 4.0, 11/2015
// Calibrating an eCompass in the Presence of Hard-and Soft-Iron Interference by Talat Ozyagcilar.
//........................................................................................
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//--( Start Main )--( Start Main )--( Start Main )--( Start Main )--( Start Main )----
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <stdio.h>
#include <math.h>
#include <conio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <stdint.h> //to define uint64_t
#define Type double //Note: The analysis will be performed in doubles
#define _R 216 // Number of Rows of the input data matrix
#define _C 9 // Number of Columns
#define M _C
#define N _C
#define _L 3
#define _n 3 // Enter the size of the square symmetric matrix
#define _row _n
#define _col _n
#define _m _n
#define _Gcol _m // Number of Colums for the Augmented Matrix of the Gauss elimination
int i, j, k, l, p, q, flag;
int z =_col;
int n= _row;
int R=_R;
int C1 =_C;
int L=_L;
// Def: matrix m x p (m=rows ,p=columns)
Type A[_row][_col], UpsA[_row][_col], d[_row][_col], s[_row][_col], s1[_row][_col], s1t[_row][_col];
Type aG[_row][_Gcol], x[_row]; //This matrix is used for Gauss elimination to calculate the eigenvectors.
Type C[_row][_col] , CTp[_col][_row]; // Matrix and transposed matrix of eigenvectors
Type sltt[_row][_col]; //This matrix is used to test the juctaposition of the eigenvectors.
//Type sT[_row][_col]; //This matrix is the transposed eigenvector matrix sT used to test the transformation
Type temp[_row][_col];
Type hmax, theta, ratio;
Type pi=3.141592654;
Type zero=0.00001; //0.00001 zero determines the accurcacy of the calculation
Type radius; // Vector length
Type DFileIn[_R][_L];
Type DCalib1[_R][_L];
Type DCalib2[_R][_L];
Type D[_R][_C]; //D = input matrix m x p (m=rows=24,p=columns=4)
//m rows is aantal metingen, bv 24 bij kubus onder 90 graden
//D[m][0] = bv gemeten X-waarde in mGauss
//D[m][1] = bv gemeten Y-waarde in mGauss
//D[m][2] = bv gemeten Z-waarde in mGauss
Type ReduD[_R][_C];
Type DT [_C][_R], ReduDT [_C][_R]; // DT= transposed input matrix p x m (p=rows=4,m=columns)
Type DTD[_C][_C], ReduDTD[_C][_C];
Type OrigDTD[_C][_C]; // auxiliary matrix used for checking matrix-inversion
Type DTDh[_C][_C];
Type Dh1[_C][_C]; // auxiliary matrix Dh1=(DTp x D)=>p x p=>9 x 9 matrix
Type NVec[_R]; // N-vector filled with 24x1
Type DTNVec[_C]; // Hulp Vector DTNVec[9x1]=DT.NVec
Type Vec[_C]; // "Solution" vector
Type Vghi[3]; // part of "Solution" vector
Type VS[3]; // Shift vector
Type VS1[3]; // Rotated Shift vector
Type VS2[3];
Type A3[3][3]; //matrix to calculate the shift vector
Type HA3[3][3]; //Auxiliary matrix
Type HHA3[3][3]; //Auxiliary matrix to check A3 Matrixinversion
Type B3[3][3]; //hulp matrix
Type A4[4][4]; //hulp matrix
Type B4[4][4]; //hulp matrix
Type HATemp[4][4]; //hulp matrix
Type T[4][4];
Type TT[4][4];
Type EiVec[3][3], EiVecTp[3][3];
Type hEiVec[3][3], hEiVecTp[3][3];
Type NormEiVec[3][3], NormEiVecTp[3][3];
Type JacoEiVals[3][3], TstJacoEiVals[3][3];
Type JacoEiVec[3][3], JacoEiVecTp[3][3];
Type DetVal;
Type Wmin1[3][3]; //Calibration matrix W**-1
void importInteger3ColDataTextFile(Type* A);
void exportRawMagdata2textfile(Type* A);
void exportCalibMagdata2textfile(Type* A);
//void exportFinalCalibResults2textfile(Type* A);
void exportFinalCalibResults2textfile(void);
void MatrixMathPrint(Type* A , int row , int col, int decimals, char *Stringlabel);
void MatrixMathSciePrint(Type* A, int row, int col, int decimals, char *Stringlabel);
void MatrixMultiplyConstant(Type* A, Type* B, int row, int col, Type constant);
void MatrixMathCopy(Type* A , int row , int col, Type* B);
void MatrixMathMultiply(Type* A, Type* B, int m, int p, int n, Type* C);
void MatrixMathMatrixVectorMultiply(Type* A, Type* U, int m, int p, Type* V);
void MatrixMathTranspose(Type* A, int row, int col, Type* C);
void MatrixMath_I_Matrix(Type* A, int row, int col);
int MatrixMathInvert(Type* A, int n);
void MatrixMathAddition (Type* A, Type* B, int row, int col, Type* C);
void MatrixMathSubtract (Type* A, Type* B, int row, int col, Type* C);
void GaussElimination(Type* A, int row, int col);
Type MatrixMathDeterminant(Type* _D, int _t);
Type determinant(Type _a[_row][_col], int _t);
void main()
{
// xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
// Calcul-Step 1: Importing the 216 x,y,z magnetometer measurements.
// FILENAAM is "d:\\Pelles C-SourceCode\\sensordata\\recorded-sensordata.txt";
importInteger3ColDataTextFile((Type*)DFileIn);
printf("The from file imported Data Matrix is: \n");
for (i=0;i < R;i++)
{
for (j=0;j < L;j++)
{
D[i][j] = DFileIn[i][j];
printf(" %.0f", D[i][j]);
printf("\t");
}
printf("\n");
}
// For quality control the raw measurements are exportd as GnuPlot compatible file.
exportRawMagdata2textfile((Type*) DFileIn);
// xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
//================= All 216 Magnetic Data Records have been loaded ======================
// Calcul-Step 2: Calculating the tilted ellipsoid as discussed in[Ref.2] =====
// This factor has experimentally been figured out to down-scale the poorly conditioned
// matrix for successfull inversion.
Type factor = 0.001;
Type factor1;
for (i=0; i < _R ; i++)
{
D[i][3] = 2*D[i][0]*D[i][1]; // 2XY
D[i][4] = 2*D[i][0]*D[i][2]; // 2XZ
D[i][5] = 2*D[i][1]*D[i][2]; // 2YZ
D[i][6] = 2*D[i][0]; // 2X
D[i][7] = 2*D[i][1]; // 2Y
D[i][8] = 2*D[i][2]; // 2Z
D[i][0] = D[i][0]*D[i][0]; // XX
D[i][1] = D[i][1]*D[i][1]; // YY
D[i][2] = D[i][2]*D[i][2]; // ZZ
}
// MatrixMathPrint((Type*)D,_R,_C,1,"It is assumed that all Bit-value measurements lay on a Rotated Ellipsoid.");
MatrixMathTranspose((Type*)D,_R,_C,(Type*)DT); //calculate the transposed matrix DT
// MatrixMathPrint((Type*)DT,_C,_R,1,"DT is the transposed D matrix");
// Auxilliary matrix OrigDTD stores the original product matrix DTD, later to be used for matrix inversion check.
MatrixMathMultiply((Type*)DT, (Type*)D, C1,R,C1, (Type*)OrigDTD);
// Fill NVec[216x1] vector with ones.
for (i=0; i < _R ; i++) NVec[i] = 1.0;
// _R=216 _C=9
MatrixMathMatrixVectorMultiply((Type*)DT,(Type*)NVec,_C,_R,(Type*)DTNVec);
// vector DTNVec[9x1] = DT[9x216].NVec[216x1]
// START matrix inversion. Prepare inversion by down-scaling matrix D and DT with factor.
// Because of poor matrix condition down-scaling the matrices is required for succesfull inversion.
// -------------------------------------------------------------------------------------
MatrixMultiplyConstant((Type*)D , (Type*)ReduD ,R,C1, factor); //Reduce Matrix D with factor
MatrixMultiplyConstant((Type*)DT, (Type*)ReduDT ,C1,R, factor); //Reduce Matrix DT with factor
MatrixMathMultiply((Type*)ReduDT, (Type*) ReduD, C1,R,C1, (Type*) ReduDTD); //DTD is here reduced
// Gauss-Jordan Matrix Inversion.
// ---------------------------------------
MatrixMathInvert((Type*)ReduDTD,C1); //calculate inverse of down-scaled matrix DTD and store in DTD
// ----------------------------------------
// Up-Scaling the inverted down-scaled matrix
factor1=factor*factor;
MatrixMultiplyConstant((Type*)ReduDTD, (Type*)DTD, C1,C1, factor1);
// DTD = INV(DT.D)[[9x9] matrix of the oroginal (DT.D)[[9x9] matrix.
MatrixMathSciePrint((Type*)DTD,C1,C1,8 ,"The up-scaled inverted Matrix in scientific notation.");
// Check the matrix inversion.
MatrixMathMultiply((Type*)DTD, (Type*)OrigDTD, C1,C1,C1, (Type*) Dh1);
MatrixMathPrint((Type*)Dh1,_C,_C,6,"The Matrixinversion is Checked. If unity matrix then OK.");
// Inverted DTD[9x9] matrix is Up-scaled with factor1, and the inversion has been checked.
// END of Gauss-Jordan matrix inversion.
// ---------------------------------------------------------------------------------------
// Calculate the Least-Square solution vector Vec[1x9]=Vec[a,b,c,d,e,f,g,h,i]
printf("\n");
for (i=0; i < C1; i++) printf("DTNVec= %.0f\n", DTNVec[i]); // DTNVec[9x1]
printf("\n");
// Calculation of LS solution vector Vec[9x1]
MatrixMathMatrixVectorMultiply((Type*)DTD,(Type*)DTNVec,_C,_C,(Type*)Vec);
for (i=0; i < C1; i++)
{
printf("The LS solution vector Vec= %6.4e\n", Vec[i]);
}
// a XX+ b YY+ c ZZ+ d 2XY+ e 2XZ+ f 2YZ+ g 2X+ h 2Y+ i 2Z =1
// Vec[0]XX+Vec[1]YY+Vec[2]ZZ+Vec[3]2XY+Vec[4]2XZ+Vec[5]2YZ+Vec[6]2X+Vec[7]2Y+Vec[8]2Z =1
//== With the LS-calculation the optimum ellipsoid, being the locus of the raw magnetometer measurements, is found. ==
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
// Calcul-Step 3: Calculating the offset vector of the ellipsoid as per [Ref.2]
// The PARTIAL derivatives dF/dX=0, dF/dY=0, dF/dZ=0 from the second degree function
// F(X,Y,Z)=Vec[0]XX+Vec[1]YY+Vec[2]ZZ+Vec[3]2XY+Vec[4]2XZ+Vec[5]2YZ+Vec[6]2X+Vec[7]2Y+Vec[8]2Z-1
// result in 3 equations from which the center i.e. offset of the ellipsoid is calculated.
// The formalism of [Ref.2] is equivalent and calculates the offset with auxiliary matrix A3[3][3]
// and vector Vghi[3].
A3[0][0]=Vec[0]; //a
A3[0][1]=Vec[3]; //d
A3[0][2]=Vec[4]; //e
A3[1][0]=Vec[3]; //d
A3[1][1]=Vec[1]; //b
A3[1][2]=Vec[5]; //f
A3[2][0]=Vec[4]; //e
A3[2][1]=Vec[5]; //f
A3[2][2]=Vec[2]; //c
MatrixMathCopy((Type*) A3, 3,3, (Type*) HA3); //Store A3 in auxiliary matrix HA3 for inversion check
MatrixMathInvert((Type*) A3,3); //calculate inverse van A3[3x3] matrix
// Check the inversion of matrix A3
MatrixMathMultiply((Type*) A3, (Type*) HA3, 3,3,3, (Type*) HHA3);
MatrixMathPrint((Type*)HHA3,3,3,6,"HHA3[3x3] stores the Matrixinversion Check. If HHA3 is unity matrix then OK.");
MatrixMultiplyConstant((Type*) A3, (Type*) A3, 3, 3, -1); // multiply matrix A3 with -1
Vghi[0]=Vec[6]; //g
Vghi[1]=Vec[7]; //h
Vghi[2]=Vec[8]; //i
// Calculation Offset vector VS[3]
printf("\n");
MatrixMathMatrixVectorMultiply((Type*) A3,(Type*)Vghi,3,3,(Type*)VS);
printf("Offset vector X component VSx= %.0f\n",VS[0]);
printf("Offset vector Y component VSy= %.0f\n",VS[1]);
printf("Offset vector Z component VSz= %.0f\n",VS[2]);
printf("\n");
printf("\n");
//========= The offset vector of the rotated ellipsoid is determined ===========
//------ Calculating the matrix that represents the rotated ellipsoid, translated
// with the offset vector to the origin. Calculation as per [Ref.2] ------
A4[0][0]=Vec[0]; //a
A4[0][1]=Vec[3]; //d
A4[0][2]=Vec[4]; //e
A4[0][3]=Vec[6]; //g
A4[1][0]=Vec[3]; //d
A4[1][1]=Vec[1]; //b
A4[1][2]=Vec[5]; //f
A4[1][3]=Vec[7]; //h
A4[2][0]=Vec[4]; //e
A4[2][1]=Vec[5]; //f
A4[2][2]=Vec[2]; //c
A4[2][3]=Vec[8]; //i
A4[3][0]=Vec[6]; //g
A4[3][1]=Vec[7]; //h
A4[3][2]=Vec[8]; //i
A4[3][3]=-1;
T[0][0]=T[1][1]=T[2][2]=T[3][3]=1;
T[0][1]=T[0][2]=T[0][3]=T[1][0]=T[1][2]=T[1][3]=T[2][0]=T[2][1]=T[2][3]=0;
T[3][0]=VS[0];
T[3][1]=VS[1];
T[3][2]=VS[2];
// Above constructed matrix T{4x4} reads:
// {1,0,0,0},
// {0,1,0,0},
// {0,0,1,0},
// {VS[0],VS[1],VS[2],1}};
// Calculate the transposed matrix TT[4x4] of matrix T[4x4]
MatrixMathTranspose((Type*)T,4,4,(Type*)TT);
MatrixMathMultiply((Type*) A4, (Type*)TT, 4,4,4, (Type*) HATemp);
MatrixMathMultiply((Type*)T, (Type*)HATemp, 4,4,4, (Type*) B4);
MatrixMathSciePrint((Type*)B4,4,4,8 ,"B4[4x4] stores T[4x4].A[4x4].TT[4x4].");
// Calculate the value of the ellipsoid function F(X,Y,Z) for the offset values X0, Y0, Z0
Type FX0Y0Z0= Vec[0]*VS[0]*VS[0] +Vec[1]*VS[1]*VS[1] +Vec[2]*VS[2]*VS[2] +
2*Vec[3]*VS[0]*VS[1] +2*Vec[4]*VS[0]*VS[2] +2*Vec[5]*VS[1]*VS[2] +
2*Vec[6]*VS[0] +2*Vec[7]*VS[1] +2*Vec[8]*VS[2] -1;
printf("\n");
// Note: B4[3][3]= g*X0+h*Y0+i*Z0-1 = Vec[6]*VS[0] +Vec[7]*VS[1] +Vec[8]*VS[2] -1;
// Voor afleiding van B4[3][3]=F(X0,Y0,Z0) zie mijn notes.
printf("Note: The value of B4[3][3] is equal to F(X0,Y0,Z0)= %.5f\n",FX0Y0Z0);
printf("\n");
// Extract the 3x3 matrix B3 from the 4x4 matrix B4
for (i=0; i < 3; i++)
{
for (j=0;j < 3;j++)
{
B3[i][j] = B4[i][j];
}
}
// MatrixMathSciePrint((Type*) B3,3,3,8 ,"B3[3x3].");
double val;
val = -1/ B4[3][3];
// printf("The value of val= %.5f\n",val);
MatrixMultiplyConstant((Type*) B3, (Type*) B3, 3, 3, val);
printf("\n");
MatrixMathSciePrint((Type*) B3,3,3,8 ,"B3[3x3] the fit-matrix of the (tilted) shifted ellipsoid is:");
printf("\n\n");
printf("Calculation of the Offset-Vector and fit-matrix of rotated Ellipsoid translated to
the origin has been completed.\n");
printf("\n");
// ---- Calculation of the (ellipsoid) Offset-Vector, and the rotated, translated ellipsoid fit-matrix
// as per [Ref.2] is completed --------------------------
printf("********************************************************************************************");
printf("\n");
printf("********************************************************************************************");
printf("\n");
// xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
// Calcul-STEP 4: Eigenvalue calculation of fit-matrix A[3x3].
MatrixMathCopy((Type*) B3,3,3, (Type*)A); //For convenience matrix B3 is renamed to A
// MatrixMathSciePrint((Type*)A,z,z,12,"The symmetric fit-matrix A to calculate eigen values
and vectors in scientific notation is: ");
// Calculate the determinant of fit-matrix A[3x3] for later check of eigenvalue product.
// DetVal=MatrixMathDeterminant((Type *) A, z);
DetVal=MatrixMathDeterminant((Type *) A, n);
printf("Determinant of Matrix A = ");
printf("%6.6e", DetVal);
printf("\n");
// To improve the Jakobi eigenvalue calculation the ellipsoid A[3][3] will be upscaled with upscalefactor .
Type upscalefactor = 10e+08;
Type downscalefactor; // is 1/upscalefactor
MatrixMultiplyConstant((Type*)A, (Type*)UpsA,z,z,upscalefactor);
MatrixMathSciePrint((Type*)UpsA,z,z,6,"The symmetric upscaled matrix UpsA[3][3] in scientific notation is: ");
// **********************************************************************************************
// **********************************************************************************************
// ******************** Jakobi Eigenvalue and Eigenvector Code Ref.5 ****************************
// **********************************************************************************************
// **********************************************************************************************
// Jakobi Step 1: Read the symmetric Matrix UpsA
// Jakobi Step 2: Initialize d=UpsA and S=I
MatrixMathCopy((Type*)UpsA, z, z, (Type*)d); //Initialize matrix d = matrix UpsA
MatrixMath_I_Matrix((Type*)s, z, z); //Initialize s as I-Matrix
// Check print matrix d
// MatrixMathPrint((Type*)d, z, z, 12, "The copy matrix of matrix A=d[3x3] is: ");
float flag=1;
while(flag>zero)
{
// Jakobi Step 3: Find the largest off-diagonal element in d=|d[ij]| and let it be d[ij]
i=1;
j=2;
hmax = fabs(d[i][j]);
for (p = 0; p < z ; p ++)
{
for (q = 0; q < z ; q ++)
{
if (p!=q)
if (hmax < fabs(d[p][q]))
{
hmax=fabs(d[p][q]);
i=p; //index i, used below, is determined here
j=q; //index j, used below, is determined here
}
}
}
// printf("\n");
printf("Max off-line element of matrix d=> d[ij]= %.12f\n", d[i][j]);
// printf("\n");
// Jakobi Step 4: Find the rotational angle Theta
if (d[i][i]==d[j][j])
{
if (d[i][j] > 0)
{
theta=pi/4;
}
else
{
theta=-pi/4;
}
}
else
{
theta=0.5*atan(2*d[i][j]/(d[i][i]-d[j][j]));
}
printf("\n");
printf("The rotational angle Theta= %.4f\n", theta);
// Serial.print(theta,6);
printf("\n");
// Jakobi Step 5: Computate the matrix s1=s[p][q]
// Note: s1t is the transposed of s1
MatrixMath_I_Matrix((Type*)s1, z, z); //Initialize S1 as I-Matrix
MatrixMath_I_Matrix((Type*)s1t, z, z); //Initialize S1T as I-Matrix
s1[i][i]= cos(theta);
s1[j][j]= s1[i][i];
s1[j][i]= sin(theta);
s1[i][j]=-s1[j][i];
s1t[i][i]=s1[i][i];
s1t[j][j]=s1[j][j];
s1t[i][j]=s1[j][i];
s1t[j][i]=s1[i][j];
MatrixMathPrint((Type*)s1 , z, z, 3, "The matrix s1 is: ");
MatrixMathPrint((Type*)s1t, z, z, 3, "The matrix s1t is: ");
// Jakobi Step 6: Calculate D= S1TxDxS1 and S=SxS1
MatrixMathMultiply((Type*)s1t, (Type*)d, z, z, z, (Type*)temp);
MatrixMathMultiply((Type*)temp, (Type*)s1, z, z, z, (Type*)d);
MatrixMathSciePrint((Type*)d, z, z,6, "The Jakobi eigenvalue matrix of the upscaled matrix UpsA is: ");
MatrixMathMultiply((Type*)s,(Type*)s1, z, z, z, (Type*)temp);
MatrixMathCopy((Type*)temp, z, z, (Type*)s);
MatrixMathSciePrint((Type*)s, z, z, 6,"The Jakobi eigenvector matrix of the upscaled matrix UpsA is: ");
flag=0;
for (i = 0; i < z ; i ++) //check the sum of non-diagonal terms
for (j = 0; j < z ; j ++)
{
if(i!=j)
{
if(fabs(d[i][j]) > zero ) // zero=0.0001;
{
flag=flag+fabs(d[i][j]);
}
}
}
printf("\n");
printf("Flag= %.6f\n", flag);
printf("\n\n");
}
// ****************** END of Jakobi Eigenvalue While Loop ******************
// ***************** Check Calculation of Jakobi Eigenvalues ***************
// DOWNSCALING the Jakobi eigenvalues with:
downscalefactor=1/upscalefactor;
//check effect of changing order of eigenvalues
Type a=d[0][0];
Type b=d[1][1];
Type c=d[2][2];
d[0][0]=c; //a,b,c b,c,a c,a,b
d[1][1]=a;
d[2][2]=b;
printf("Finally the Jakobi Eigenvalues are: ");
for (i = 0; i < n ; i ++)
{
JacoEiVals[i][i]=d[i][i]*downscalefactor;
printf("%.6e ", JacoEiVals[i][i]);
}
printf("\n\n");
// Check if the product of the Jakobi eigenvalues equal is to determinant
// of the matrix A. All downscaled.
float eivalproduct =1;
for (i = 0; i < n ; i ++)
{
eivalproduct *= JacoEiVals[i][i];
}
printf(" The product of the 3 Eigenvalues is: %0.6e", eivalproduct);
printf("\n");
printf(" Determinant of Matrix A[3x3]= ");
printf("%6.6e", DetVal);
printf("\n\n");
printf("\nAAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA \n");
printf("End Eigenvalue Calculs - End Eigenvalue Calculs - End Eigenvalue Calculs - End Eigenvalue Calculs\n");
printf("AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA AAAAAAAAAA \n");
// xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
// Calcul-Step 5: Eigenvector calculation of fit-matrix A[3x3].
printf("\nBBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB \n");
printf("Start Gauss-Elim Eigenvector Calculs - Start Gauss-Elim Eigenvector Calculs \n");
printf("BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB BBBBBBBBBB \n");
// Start of loop over the eigenvalues of matrix A[3][3] to calculate the eigenvectors with Gauss-Elimination
for (l=0; l < n; l++)
{
MatrixMath_I_Matrix((Type*) temp,n,n); //was z
printf("\n");
printf(" The selected Jakobi eigenvalue to calculate its eigenvector is: %.6e\n", JacoEiVals[l][l]);
MatrixMultiplyConstant((Type*) temp,(Type*) temp,n,n,JacoEiVals[l][l]);
// Selected eigenvector for eigenvector calc.
MatrixMathSubtract((Type*) A,(Type*) temp,n,n,(Type*) aG);
// aG is the ellipsoid fit-matrix reduced by the selected eigenvalue
// MatrixMathSciePrint((Type*) aG,n,n,6,"The ellipsoid fit-matrix reduced by the selected eigenvalue is: ");
printf("\n");
GaussElimination((Type*) aG,n,n); // Gauss elimination produces the Eigenvector Matrix EiVec[3][3]
}
printf("\n");
MatrixMathSciePrint((Type*)EiVec,n,n,6,"The Gauss eliminated Eigenvector Matrix is: ");
printf("\n");
// Normalizing the Gauss eliminated eigenvectors EiVec
for (j = 0; j < z ; j ++){
for (i = 0; i < z ; i ++){
hEiVec[i][j]=EiVec[i][j];
}
}
Type Nhs1;
Nhs1= sqrt(hEiVec[0][0]*hEiVec[0][0] + hEiVec[1][0]*hEiVec[1][0] + hEiVec[2][0]*hEiVec[2][0]);
NormEiVec[0][0]=hEiVec[0][0]/Nhs1;
NormEiVec[1][0]=hEiVec[1][0]/Nhs1;
NormEiVec[2][0]=hEiVec[2][0]/Nhs1;
Nhs1= sqrt(NormEiVec[0][0]*NormEiVec[0][0] + NormEiVec[1][0]*NormEiVec[1][0] + NormEiVec[2][0]*NormEiVec[2][0]);
printf("Length check of normalized Gauss-EiVec1 eigenvector Nhs1= %.6e",Nhs1);
printf("\n");
Type Nhs2;
Nhs2= sqrt(hEiVec[0][1]*hEiVec[0][1] + hEiVec[1][1]*hEiVec[1][1] + hEiVec[2][1]*hEiVec[2][1]);
NormEiVec[0][1]=hEiVec[0][1]/Nhs2;
NormEiVec[1][1]=hEiVec[1][1]/Nhs2;
NormEiVec[2][1]=hEiVec[2][1]/Nhs2;
Nhs2= sqrt(NormEiVec[0][1]*NormEiVec[0][1] + NormEiVec[1][1]*NormEiVec[1][1] + NormEiVec[2][1]*NormEiVec[2][1]);
printf("Length check of normalized Gauss-EiVec2 eigenvector Nhs2= %.6e",Nhs2);
printf("\n");
Type Nhs3;
Nhs3= sqrt(hEiVec[0][2]*hEiVec[0][2] + hEiVec[1][2]*hEiVec[1][2] + hEiVec[2][2]*hEiVec[2][2]);
NormEiVec[0][2]=hEiVec[0][2]/Nhs3;
NormEiVec[1][2]=hEiVec[1][2]/Nhs3;
NormEiVec[2][2]=hEiVec[2][2]/Nhs3;
Nhs3= sqrt(NormEiVec[0][2]*NormEiVec[0][2] + NormEiVec[1][2]*NormEiVec[1][2] + NormEiVec[2][2]*NormEiVec[2][2]);
printf("Length check of normalized Gauss-EiVec3 eigenvector Nhs3= %.6e",Nhs3);
printf("\n\n");
MatrixMathTranspose((Type*) NormEiVec,n,n, (Type*) NormEiVecTp);
MatrixMathMultiply((Type*) NormEiVecTp, (Type*) NormEiVec,n,n,n, (Type*) temp);
MatrixMathSciePrint((Type*)temp,n,n,6,"Checking orthogonality of the Gauss Eigenvector Matrix: ");
printf("\n");
// ============================================================================
MatrixMathSciePrint((Type*)NormEiVec, n, n, 6, "The orthonormal matrix of Gauss eigenvectors is: ");
printf("\n");
MatrixMathMultiply((Type*) A, (Type*) NormEiVec,n,n,n, (Type*) temp);
MatrixMathMultiply((Type*) NormEiVecTp, (Type*) temp,n,n,n, (Type*) TstJacoEiVals);
MatrixMathSciePrint((Type*)TstJacoEiVals, n, n, 6, "Checking the Jacobi Eigenvalues by transforming
ellipsoid fit-matrix A with the orthonormal eigenvector matrices:");
printf("\n");
printf("The Jakobi Eigenvalues are: ");
for (i = 0; i < n ; i ++)
{
printf("%.6e ", JacoEiVals[i][i]);
}
printf("\n\n");
printf("\nBBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB \n");
printf("BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB BBBBBBBBBBB \n");
Type SQRTJacoEiVals[3][3]; //SQRT of Eigenvalue matrix JacoEiVals of fit-matrix A
// Type Wmin1[3][3]; //Calibration matrix W**-1
SQRTJacoEiVals[0][0]= sqrt(JacoEiVals[0][0]);
SQRTJacoEiVals[0][1]= 0.0;
SQRTJacoEiVals[0][2]= 0.0;
SQRTJacoEiVals[1][0]= 0.0;
SQRTJacoEiVals[1][1]= sqrt(JacoEiVals[1][1]);
SQRTJacoEiVals[1][2]= 0.0;
SQRTJacoEiVals[2][0]= 0.0;
SQRTJacoEiVals[2][1]= 0.0;
SQRTJacoEiVals[2][2]= sqrt(JacoEiVals[2][2]);
MatrixMathSciePrint((Type*)SQRTJacoEiVals, z, z, 6, "Square Root of the diagonal eigenvalue matrix JacoEiVals: ");
MatrixMathMultiply((Type*) NormEiVec, (Type*) SQRTJacoEiVals,n,n,n, (Type*) temp);
MatrixMathMultiply((Type*) temp, (Type*) NormEiVecTp,n,n,n, (Type*) Wmin1);
MatrixMathSciePrint((Type*)Wmin1, z, z, 6, "Calculation of W**-1=sqrt(A): ");
MatrixMathMultiply((Type*) Wmin1, (Type*) Wmin1,n,n,n, (Type*) temp);
MatrixMathSciePrint((Type*)temp, z, z, 6, "Check-Calculation of Wmin1xWmin1=A : ");
printf("\n");
// printf("1e Calibration Step: Shifting the raw ellipsoid data to origin coordinate system x',y',z'. \n");
for (i=0;i < R;i++) //R=216
{
for (j=0;j < L;j++) //L=3
{
// Apply the calculated shift to the 216 raw Bit-Value measurements.
DCalib1[i][j] = DFileIn[i][j]- VS[j];
}
}
int count=0;
while (count < R) //R=216
{
for (j=0;j < L;j++) //L=3
{
VS1[j] = DCalib1[count][j]; //Store the 3 values of each row in vector VS1
}
for (i=0;i < 3;i++)
{
VS2[i]=0;
for (j=0;j < 3;j++)
{
VS2[i]+= Wmin1[i][j]*VS1[j]; //multiply VS1 values with calibratie matrix
}
}
for (i=0;i < 3;i++)
{
DCalib2[count][i] = VS2[i];
}
count=count+1;
}
printf("\n The Calibrated measurements are: \n");
for (i=0;i < R;i++) //R=216 Rows
{
radius = sqrt(DCalib2[i][0]*DCalib2[i][0] + DCalib2[i][1]*DCalib2[i][1] + DCalib2[i][2]*DCalib2[i][2]);
for (j=0;j < L;j++) //L=3 Cols
{
printf(" %.3f", DCalib2[i][j]);
printf(" ");
}
printf(" %.3f", radius); //colmn 4
printf("\n");
}
printf("\n");
// For quality control, export the calibrated measurements as GnuPlot compatible file.
exportCalibMagdata2textfile((Type*) DCalib2);
printf("\n\n\nDisplay and export the Final Calibration Results: \n\n");
exportFinalCalibResults2textfile();
printf("\n");
i=10;
while (i < 100) //Endless loop to make sure .exe keeps results displaying on monitor
{
i++;
i--;
}
}
/*
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß-End Main-ßßßß
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
*/
//§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
//§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
//++++++++++++++++++++++ Start of function Library ++++++++++++++++++++++++++++++
//§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
//***************** IMPORT INTEGER Matrix via Text File Routine ******************
//Last update 16-2-2019
void importInteger3ColDataTextFile(Type* A)
{
// read all characters from a file, outputting only the letter 'b's
// it finds in the file
FILE *fp;
int i,j,k, val;
Type val1;
char* filename = "d:\\Pelles C-SourceCode\\sensordata\\recorded-sensordata.txt";
char c;
int C=3;
char dest[10];
i=0;
j=0;
k=0;
fp = fopen(filename, "r");
if (fp == NULL)
{
printf("Could not open file %s",filename);
// return 1;
}
// this while-statement assigns into c, and then checks against EOF:
while((c = fgetc(fp)) != EOF)
{
if (c == '=')
{
if (k > 2) j++;
for(i=0;i < 10;i++)
{
dest[i]= 0;
}
i=0;
while((c = fgetc(fp)) != ';')
{
dest[i]= c;
i++;
}
val = atoi(dest);
val1 =(Type)val;
// printf("j = %d", j);
if (k>(C-1)) k=0;
// printf(" k = %d\n", k);
// printf("String value = %s\n", dest);
// printf("Int value = %d\n", val);
// printf("Type value = %.2f\n", val1);
// printf("\n");
A[C*j+k]=val1;
k++;
}
}
fclose(fp);
//-------------
/*
for (i=0;i < R;i++)
{
for (j=0;j < C;j++)
{
printf(" Matrix DF = %.4f\n", A[C*i+j]);
}
}
printf("\n\n");
// return 0;
*/
//------------
}
//**********************************************************************************
//******* Matrix-Vector Multiplication Routine **********************************
// V = A*U
void MatrixMathMatrixVectorMultiply(Type* A, Type* U, int m, int p, Type* V)
{
// A = input matrix A(rows x cols)
// U = input vector U(rows)
// V = output vector V(rows)= A(rows x cols)*U(rows)
// m = number of rows in A
// p = number of columns in A = number of rows in U
// V = output vector = A*U(m)
int i, j, k;
Type Ah[m][p]; //hulp array
Type Uh[p]; //hulp input vector
Type Vh[p];
for (i=0;i < m;i++)
{
for(j=0;j < p;j++)
{
Ah[i][j] = A[p*i+j];
}
}
for(j=0;j < p;j++)
{
Uh[j] = U[j];
// printf("\t%10.6f", Uh[j]);
// printf("\n");
}
for (i=0;i < m;i++)
{
V[i]=0;
for (k=0;k < p;k++)
{
V[i]+= Ah[i][k]*Uh[k];
}
}
/*
printf("\n");
for (i=0;i < p;i++)
{
printf("\t%.1f", V[i]);
printf("\n");
}
*/
}
//************* Matrix Inversion Routine *****************************************
// * 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(Type* 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
Type tmp; // used for finding max value and making column swaps}
/*
//printf("\n");
printf("The Matrix to be inverted with Gauss-Jordan is: \n");
for (i=0;i < n;i++)
{
for(j=0;j < n;j++)
{
printf("%.4f", A[n*i+j]);
printf(" ");
}
printf("\n");
}
*/
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;
//printf("%.4f\n", tmp);
}
}
// check for matrix singularity
if (A[pivrow*n+k] == 0.0f)
{
printf(" \n");
printf("Inversion failed due to singular matrix\n");
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;
//printf("%lld\n", A[pivrow*n+j]);
}
}
pivrows[k] = pivrow; // record row swap (even if no swap happened)
tmp = 1.0f/A[k*n+k]; // invert pivot element
// tmp = A[k*n+k];
A[k*n+k] = 1.0f; // This element of input matrix becomes result matrix
//printf("%lld\n", A[k*n+k]);
//printf("%.8f\n", tmp);
// Perform row reduction (divide every element by pivot)
for (j = 0; j < n; j++)
{
A[k*n+j] = A[k*n+j]*tmp;
}
//OK
// 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.0;
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 Gauss-Jordan Matrix Inversion Function ********************
//******************** Start Gauss Eliminatiom Function ************************
void GaussElimination(Type* _aG, int row, int col)
{
// n=3= row, m=4= col;
// _aG = augmented input matrix (row x col)
Type aGh[row][col]; //hulp array
int i,j,k;
printf(" The augmented input matrix is: \n");
for (i=0; i < row; i++)
{
for(j=0; j < col; j++)
{
aGh[i][j] = _aG[col*i+j];
printf(" aGh[%d][%d]= %.6e",i,j,aGh[i][j]);
}
printf("\n");
}
for(i=0;i < n-1;i++)
{
if(aGh[i][i] == 0.0)
{
printf("Mathematical Error! _aG[%d][%d]= %.12f\n",i,i, aGh[i][i]);
exit(0);
}
for(j=i+1; j < n; j++)
{
// printf("\n\n aGh[%d][%d]= %.6e",j,i, aGh[j][i]);
// printf(" aGh[%d][%d]= %.6e\n",i,i,aGh[i][i]);
ratio = aGh[j][i]/aGh[i][i];
for(k=0; k < n+1; k++)
{
// printf("\n i= %d", i);
// printf(" j= %d", j);
// printf(" k= %d\n", k);
aGh[j][k] = aGh[j][k] - ratio * aGh[i][k];
// printf(" aGh[%d][%d]= %.6e",j,k,aGh[j][k]);
}
}
}
printf("\n");
printf(" The triangulized augmented input matrix is: \n");
for (i=0; i < row; i++)
{
for(j=0; j < col; j++)
{
// aGh[i][j] = _aG[col*i+j];
printf(" aGh[%d][%d]= %.6e",i,j,aGh[i][j]);
}
printf("\n");
}
// printf("\n");
// MatrixMathSciePrint((Type*) aGh,n,n,6,"The triangulized augmented input Matrix is: ");
printf("\n");
// printf(" n=%d",n);
// Calculate of eigenvector matrix (the matrix columns are the eigenvectors)
if( (abs(aGh[n-1][n-1]) || n>3) > 0.00000000001)
{
printf("Eigenvector Error! aGh[%d][%d]= %.12f\n",n,n, aGh[i][i]);
exit(0);
}
EiVec[0][l]= aGh[1][1] * aGh[0][2]/(aGh[0][0]*aGh[1][2])-aGh[0][1]/aGh[0][0];
EiVec[1][l]= 1;
EiVec[2][l]=-aGh[1][1]/aGh[1][2];
printf("\n");
}
//*********************** End Gauss Elimination Function *************************
// ***************** Start V2-Matrix Determinant Calc-Routine *****************
// Last Update: Bruno Best 09-7-20, 13:00 hrs
Type MatrixMathDeterminant(Type * _D, int _t)
{
// A = input matrix AND result matrix
// n = number of rows = number of columns in A (n x n)
int i, j;
Type _b[_row][_col];
Type res;
// printf("\n\n");
// printf("The SQUARE matrix for which the determinant is calculated is: \n");
for (i=0;i < _row;i++)
{
for(j=0;j < _col;j++)
{
_b[i][j] = _D[_row*i+j]; //load the to inverted data matrix
// printf("%.12f", _b[i][j]);
// printf(" ");
}
// printf("\n");
}
res= determinant(_b, _t);
return(res);
}
//--------------------------------------------------------------------------------------------
// ******************** Function to calculating Determinant of the Matrix ********************
Type determinant(Type _a[_row][_col], int _t)
{
Type s=1,det=0;
Type _d[_row][_col];
int i,j,m,n,c;
if (_t==1)
{
return (_a[0][0]);
}
else
{
det=0;
for (c=0;c < _t;c++)
{
m=0;
n=0;
for (i=0;i < _row;i++)
{
for (j=0;j < _col;j++)
{
_d[i][j]=0;
if (i != 0 && j != c)
{
// printf("%.6f", a[i][j]);
_d[m][n]=_a[i][j];
if (n < (_col-2))
n++;
else
{
n=0;
m++;
}
}
}
}
det=det + s * (_a[0][c] * determinant(_d,_t-1));
s=-1 * s;
}
}
return (det);
}
//***************** Matrix Printing Routine ************************************
// Uses tabs to separate numbers under assumption printed Type width won't cause problems
//void MatrixMathPrint(Type* A, int m, int n, int decimals, String label){
void MatrixMathPrint(Type* A, int row, int col, int decimals, char *Stringlabel)
{
// A = input matrix (row x col)
int i,j;
Type hval;
printf(" \n");
// print the string
while(*Stringlabel != '\0') {
printf("%c", *Stringlabel);
// move the ptr pointer to the next memory location
Stringlabel++;
}
printf(" \n");
for (i=0; i<row; i++){
for (j=0;j<col;j++){
hval=A[col*i+j];
printf("%+.*f", decimals, hval);
printf("\t");
}
printf(" \n");
}
}
//***************** Matrix Scientific Printing Routine **********************************
// Uses tabs to separate numbers under assumption printed Type width won't cause problems
//void MatrixMathSciePrint(Type* A, int m, int n, int decimals, String label)
void MatrixMathSciePrint(Type* A, int row, int col, int decimals, char *Stringlabel)
{
// A = input matrix (rows x cols)
int i,j;
Type hval;
printf(" \n");
// print the string
while(*Stringlabel != '\0') {
printf("%c", *Stringlabel);
// move the ptr pointer to the next memory location
Stringlabel++;
}
printf(" \n");
for (i=0; i<row; i++){
for (j=0;j<col;j++){
hval=A[col*i+j];
printf("%+.*f", decimals, hval);
printf("\t");
}
printf(" \n");
}
}
//*********** Copy Matrix Type to Type *****************************************
void MatrixMathCopy(Type* A, int row, int col, Type* B)
{
int i, j, k;
printf("\n");
for (i=0;i<row;i++)
{
for(j=0;j<col;j++)
{
B[col*i+j] = A[col*i+j];
// printf("%.6f", A[col*i+j]);
// printf(" %.6f", B[col*i+j]);
// printf("\t");
}
// printf("\n");
}
}
//************ Generate a I-Matrix ***************************************************
void MatrixMath_I_Matrix(Type* A, int row, int col)
{
int i, j;
for (i=0;i<row;i++)
for (j=0;j<col;j ++)
{
A[row*i+j]=0;
}
for (i=0;i<row;i ++)
{
A[row*i+i]=1;
}
}
//***************** Multiply Matrix with a CONSTANT ************************************
// void MatrixMath::Print(float* A, int m, int n, String label){
void MatrixMultiplyConstant(Type* A, Type* B, int row, int col, Type constant)
{
// A = input matrix (row x col)
int i,j;
for (i=0;i<row; i++){
for (j=0;j<col;j++){
B[col*i+j]= constant * A[col*i+j];
}
}
}
//******* Matrix Multiplication Routine **********************************
// C = A*B
void MatrixMathMultiply(Type* A, Type* B, int _p, int _r, int _q, Type* 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<_p;i++)
{
for(j=0;j<_q;j++)
{
C[_q*i+j]=0;
for (k=0;k<_r;k++)
{
C[_q*i+j]= C[_q*i+j]+ A[_r*i+k]* B[_q*k+j];
}
}
}
}
//*********** Matrix Transpose Routine ****************************************
//
void MatrixMathTranspose(Type* A, int row, int col, Type* C)
{
// A = input matrix (row x col)
// C = output matrix = the transpose of A (row x col)
int i, j;
for (i=0;i<row;i++)
for(j=0;j<col;j++)
C[row*j+i]=A[col*i+j];
}
//*********** Matrix Addition Routine ****************************************
void MatrixMathAddition(Type* A, Type* B, int row, int col, Type* C)
{
// A = input matrix (row x col)
// B = input matrix (row x col)
// C = output matrix = A-B (row x col)
int i, j;
for (i=0;i<row;i++)
for(j=0;j<col;j++)
C[col*i+j]=A[col*i+j]+B[col*i+j];
}
//*********** Matrix Subtraction Routine ****************************************
void MatrixMathSubtract(Type* A, Type* B, int row, int col, Type* C)
{
// A = input matrix (row x col)
// B = input matrix (row x col)
// C = output matrix = A-B (row x col)
int i, j;
for (i=0;i<row;i++){
for(j=0;j<col;j++){
C[row*i+j]=A[row*i+j]-B[row*i+j];
}
}
}
//***************** Export RawMag-Data to a GnuPlot-RAWData Text File *********************************
void exportRawMagdata2textfile(Type* A)
{
FILE *fp1;
float val;
int i;
char* filename = "d:\\Pelles C-SourceCode\\sensordata\\GnuPlot-RawData.txt";
char dest[50]; //26
char desth[15]; //10
char destk[15]; //10
char destl[15]; //10
i=0;
fp1 = fopen(filename, "w");
if (fp1 == NULL)
{
printf("Could not open file %s",filename);
// return 1;
exit(EXIT_FAILURE);
}
while(i < 3*R)
{
strcpy(dest, ""); //empty the dest String
val = A[i];
sprintf(desth, "%f%s", val, " , ");
strcat(dest, desth); // ;
i++;
val= A[i];
sprintf(destk, "%f%s", val, " , ");
strcat(dest, destk);
i++;
val= A[i];
sprintf(destl, "%f%s", val, "");
strcat(dest, destl);
i++;
// printf("Raw-Gnuplot-Data are: %s\n", dest);
fprintf(fp1,"%s\n", dest); // Write data to file
strcpy(dest, ""); //empty the dest String
}
// Close file to save file data
fclose(fp1);
printf("\n");
// Success message
printf("Raw-Data Gnu-Plot File is created and saved successfully. \n");
}
//§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
//§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
//************** Export Calibrated Mag-Data to a GnuPlot-CALIBData Text File *******
void exportCalibMagdata2textfile(Type* A)
{
FILE *fp1;
float val;
int i;
char* filename = "d:\\Pelles C-SourceCode\\sensordata\\GnuPlot-CalibData.txt";
char dest[50]; //26
char desth[15]; //10
char destk[15]; //10
char destl[15]; //10
i=0;
fp1 = fopen(filename, "w");
if (fp1 == NULL)
{
printf("Could not open file %s",filename);
// return 1;
exit(EXIT_FAILURE);
}
while(i < 3*R)
{
strcpy(dest, ""); //empty the dest String
val = A[i];
sprintf(desth, "%f%s", val, " , ");
strcat(dest, desth); // ;
i++;
val= A[i];
sprintf(destk, "%f%s", val, " , ");
strcat(dest, destk);
i++;
val= A[i];
sprintf(destl, "%f%s", val, "");
strcat(dest, destl);
i++;
// printf("Calb-Gnuplot-Data are: %s\n", dest);
fprintf(fp1,"%s\n", dest); // Write data to file
strcpy(dest, ""); //empty the dest String
}
// Close file to save file data
fclose(fp1);
// printf("\n");
// Success message
printf("Gnu-Plot file of calibrated measurements is created and saved successfully. \n");
}
//§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
//§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
//************** Export Final Mag-Calibration Results to a Text File **************
//Last update 28-12-2022
// * C program to create a file and write data to file.
void exportFinalCalibResults2textfile(void)
{
FILE *fp;
int val;
float valf;
char* filename = "d:\\Pelles C-SourceCode\\sensordata\\Final-Calib-Results.txt";
char dest[100];
char desth[20];
char destk[20];
char destl[20];
// const char str1[] = " ";
//-----------Converting Calibration to Sensor Config Format -----------
const char strWmin20[] = "// Sensor Calibration Data in Config Format.";
const char strVSxyz[] = "// x, y, z-Component of Hard-Iron Shift vector Mag_Vsi in bit-values is: ";
const char strVSx[] = " Mag_VSx = ";
const char strVSy[] = " Mag_VSy = ";
const char strVSz[] = " Mag_VSz = ";
fp = fopen(filename, "w");
if (fp == NULL)
{
printf("Could not open file %s",filename);
// return 1;
exit(EXIT_FAILURE);
}
fprintf(fp,"%s\n", strWmin20);
// time function
printf("\n");
time_t t = time(NULL);
struct tm *tm = localtime(&t);
fprintf(fp,"%s", asctime(tm));
fprintf(fp,"\n\n");
printf("%s\n", strVSxyz);
fprintf(fp,"%s\n", strVSxyz);
for (i=0;i < 3;i++)
{
dest[0]='\0';
if (i==0) strcat(dest, strVSx);
if (i==1) strcat(dest, strVSy);
if (i==2) strcat(dest, strVSz);
val=(int)VS[i];
sprintf(desth, "%d%s", val, ";");
strcat(dest, desth);
printf("%s\n", dest);
fprintf(fp,"%s\n", dest);
// dest[0]='\0';
strcpy(dest, "");
}
printf("\n\n");
fprintf(fp,"\n\n");
//---------------------------------------------------
const char strWmin1[] = "// The Mag-Calibration Matrix Wmin1 or W**-1=sqrt(A[3x3]) is: !!!!!";
printf("%s\n", strWmin1);
fprintf(fp,"%s\n", strWmin1);
printf("\n");
fprintf(fp,"\n");
const char strWmin1_0[] = "// First row of Wmin1 MagCTp row=0";
printf("%s\n", strWmin1_0);
fprintf(fp,"%s\n", strWmin1_0);
const char strWmin1_0_0[] = " Wmin1[0][0] = ";
const char strWmin1_0_1[] = " Wmin1[0][1] = ";
const char strWmin1_0_2[] = " Wmin1[0][2] = ";
for (i=0;i < 3;i++)
{
dest[0]='\0';
if (i==0) strcat(dest, strWmin1_0_0);
if (i==1) strcat(dest, strWmin1_0_1);
if (i==2) strcat(dest, strWmin1_0_2);
valf=Wmin1[i][0];
sprintf(desth, "%e%s", valf, ";");
strcat(dest, desth);
printf("%s\n", dest);
fprintf(fp,"%s\n", dest);
// dest[0]='\0';
strcpy(dest, "");
}
printf("\n");
fprintf(fp,"\n");
const char strWmin1_1[] = "// Second row of Wmin1 MagCTp row=1";
printf("%s\n", strWmin1_1);
fprintf(fp,"%s\n", strWmin1_1);
const char strWmin1_1_0[] = " Wmin1[1][0] = ";
const char strWmin1_1_1[] = " Wmin1[1][1] = ";
const char strWmin1_1_2[] = " Wmin1[1][2] = ";
for (i=0;i < 3;i++)
{
dest[0]='\0';
if (i==0) strcat(dest, strWmin1_1_0);
if (i==1) strcat(dest, strWmin1_1_1);
if (i==2) strcat(dest, strWmin1_1_2);
valf=Wmin1[i][1];
sprintf(desth, "%e%s", valf, ";");
strcat(dest, desth);
printf("%s\n", dest);
fprintf(fp,"%s\n", dest);
// dest[0]='\0';
strcpy(dest, "");
}
printf("\n");
fprintf(fp,"\n");
const char strWmin1_2[] = "// Third row of Wmin1 MagCTp row=2";
printf("%s\n", strWmin1_2);
fprintf(fp,"%s\n", strWmin1_2);
const char strWmin1_2_0[] = " Wmin1[2][0] = ";
const char strWmin1_2_1[] = " Wmin1[2][1] = ";
const char strWmin1_2_2[] = " Wmin1[2][2] = ";
for (i=0;i < 3;i++)
{
dest[0]='\0';
if (i==0) strcat(dest, strWmin1_2_0);
if (i==1) strcat(dest, strWmin1_2_1);
if (i==2) strcat(dest, strWmin1_2_2);
valf=Wmin1[i][2];
sprintf(desth, "%e%s", valf, ";");
strcat(dest, desth);
printf("%s\n", dest);
fprintf(fp,"%s\n", dest);
// dest[0]='\0';
strcpy(dest, "");
}
printf("\n\n");
fprintf(fp,"\n\n");
// Close file to save file data
fclose(fp);
// Success message with Date and Time
printf("Final Calibration Results saved successfully to file 'Final-Calib-Results.txt' \n");
printf("%s\n", asctime(tm));
}
////**** End Sec2 Calib.Utility 4: Magnetometer Calibration ****
Free-Drones Company 2022