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.