problem description
there is an electronic compass on the hardware. After being disturbed, I need to calibrate it so that it points north correctly. I used the ellipsoid fitting method, but the success rate is not high, so I can"t judge whether the calculated offset is correct?
related codes
-sharpinclude <stdio.h>
-sharpinclude <string.h>
-sharpinclude <math.h>
-sharpdefine MATRIX_SIZE 7
-sharpdefine u8 unsigned char
double m_matrix[MATRIX_SIZE][MATRIX_SIZE+1];
int m = MATRIX_SIZE;
int n = MATRIX_SIZE+1;
double m_result[MATRIX_SIZE];
@implementation CompassCalibration
void DispMatrix(void);
double Abs(double a)
{
return a<0 ? -a : a;
}
u8 Equal(double a,double b)
{
return Abs(a-b) < 1e-6;
}
void ResetMatrix(void)
{
int row , column;
for(row = 0 ; row<m ; rowPP){
for(column = 0 ; column<n ; columnPP)
m_matrix[row][column] = 0.0f;
}
}
void CalcData_Input(double x , double y , double z)
{
double V[MATRIX_SIZE];
int row , column;
V[0] = x*x;
V[1] = y*y;
V[2] = z*z;
V[3] = x;
V[4] = y;
V[5] = z;
V[6] = 1.0;
//VxVt(VtV)
for(row = 0 ; row<MATRIX_SIZE ; rowPP){
for(column = 0 ; column<MATRIX_SIZE ; columnPP){
m_matrix[row][column] += V[row]*V[column];
}
}
}
void SwapRow(int row1 , int row2)
{
int column;
double tmp;
for(column = 0 ; column<n ; columnPP){
tmp = m_matrix[row1][column];
m_matrix[row1][column] = m_matrix[row2][column];
m_matrix[row2][column] = tmp;
}
}
void MoveBiggestElement2Top(int s_row , int s_column)
{
int row,column;
for(row = s_row+1 ; row<m ; rowPP){
if( Abs(m_matrix[s_row][s_column])<Abs(m_matrix[row][s_column])){
SwapRow(s_row , row);
}
}
}
//
u8 Matrix_GaussElimination(void)
{
int row,column,i,j;
double tmp;
for(row = 0,column=0 ; row<m-1 && column<n-1 ; rowPP,columnPP){
//
MoveBiggestElement2Top(row , column);
//0
if(Equal(m_matrix[row][column],0.0f)){
printf("qiyi matrix:%d %d\r\n" , row , column);
//DispMatrix();
//return 0;
row--;
continue;
}
//
for(i = row+1 ; i<m ; iPP){
if(Equal(m_matrix[i][column],0.0f))
continue; //0
tmp = m_matrix[i][column]/m_matrix[row][column];
for(j = column ; j<n ; jPP){
m_matrix[i][j] -= m_matrix[row][j]*tmp;
}
}
DispMatrix();
printf("\r\n");
}
return 1;
}
//
int Matrix_RowSimplify(void)
{
int c = n;//(+1);
//
int row,column,k,s,t;
double tmp;
//
for(row=0,column=0;row<m && column<n;rowPP,columnPP)
{
if(Equal(m_matrix[row][column],0))//;
{
row--;
continue;
}
//
c--;//;
//
//a[i][j]1;
tmp = 1 / m_matrix[row][column];
for(k=column;k<n;kPP)//"0";
m_matrix[row][k] *= tmp;
//
//a[s][j]0
for(s=0;s<row;sPP)//0;
{
if(Equal(m_matrix[s][column],0))
continue;//0;
//
tmp = m_matrix[s][column] / m_matrix[row][column];
for(t=column;t<n;tPP)
m_matrix[s][t] -= m_matrix[row][t]*tmp;
//
}
}
//
return c;
}
void Matrix_Solve(double* C , double* sol)
{
int row,column,i;
int any_sol[MATRIX_SIZE];
//
memset(any_sol , 0 , MATRIX_SIZE);
for(row=0,column=0 ; row<m && column<n-1 ; rowPP,columnPP){
if(Equal(m_matrix[row][column] , 0.0f)){
any_sol[column] = 1; //
row--; //1
}
}
//
row = 0;
for(column = 0 ; column<n-1 ; columnPP){
if(any_sol[column] == 1){ //
sol[column] = C[column];
}else{
sol[column] = m_matrix[row][n-1];
//
for(i = column+1 ; i<n-1 ; iPP){
if(any_sol[i]==1 && !Equal(m_matrix[row][i],0.0f)){
sol[column] -= m_matrix[row][i]*C[i];
}
}
rowPP;
}
}
}
void DispMatrix(void)
{
int row,column;
for(row = 0 ; row<m ; rowPP){
for(column = 0 ; column<n ; columnPP){
printf("%.3f " , m_matrix[row][column]);
}
printf("\r\n");
}
}
-(NSArray * )Ellipsoid_fitting_Process:(NSMutableDictionary *)dic
{
double C[MATRIX_SIZE];
double Res[MATRIX_SIZE];
int i;
double k;
ResetMatrix();
//()(APM)
for (int i = 0; i<dic.count; iPP) {
NSArray * arr = [dic objectForKey:[NSString stringWithFormat:@"%d",i]];
short x =[arr[0] shortValue];
short y =[arr[1] shortValue];
short z =[arr[2] shortValue];
printf("*************:%d %d %d",x,y,z);
printf("\r\n");
CalcData_Input(x,y,z);
}
Matrix_GaussElimination();
Matrix_RowSimplify();
for(i = 0 ; i<MATRIX_SIZE ; iPP){
C[i] = 1000.0f;
}
Matrix_Solve(C , Res);
printf("a:%.2f b:%.2f c:%.2f d:%.2f e:%.2f f:%.2f g:%.2f\r\n" , Res[0],Res[1],Res[2],Res[3],Res[4],Res[5],Res[6]);
k = (Res[3]*Res[3]/Res[0]+Res[4]*Res[4]/Res[1]+Res[5]*Res[5]/Res[2] - 4*Res[6])/(4*2*2);
m_result[0] = sqrt(Res[0] / k);
m_result[1] = sqrt(Res[1] / k);
m_result[2] = sqrt(Res[2] / k);
m_result[3] = Res[3] / (2 * Res[0]);
m_result[4] = Res[4] / (2 * Res[1]);
m_result[5] = Res[5] / (2 * Res[2]);
double index = sqrt(m_result[3] * m_result[3] +m_result[4] * m_result[4] + m_result[5] * m_result[5]);
printf("Xo:%f Yo:%f Zo:%f Xg:%f Yg:%f Zg:%f C:%f index :%f\r\n" , m_result[3],m_result[4],m_result[5],m_result[0],m_result[1],m_result[2],k,index);
// if (index>=150) {
NSArray * arr = [NSArray arrayWithObjects:[NSNumber numberWithDouble:m_result[3]],[NSNumber numberWithDouble:m_result[4]],[NSNumber numberWithDouble:m_result[5]],nil];
return arr;
// }else{
// return nil;
// }
}
what result do you expect? What is the error message actually seen
I hope that the seniors who have come into contact with compass calibration will know how to improve the accuracy and accuracy judgment conditions.