File content as of revision 3:7deaf89fbe33:

#include "ATTITUDE_ESTIMATION.h"

ATTITUDE::ATTITUDE(float alpha_in, float Ts_in):alpha(alpha_in), Ts(Ts_in)
{
    //_unit_nk
    //_zero3
    float v1[3]={0.0,0.0,-1.0};
    float v2[3]={0.0,0.0,0.0};   
    Set_vector3(_unit_nk,v1);
    Set_vector3(_zero3,v2);
    
    
    Set_vector3(EulerAngle,_zero3);    
    Set_vector3(ys,_zero3);    
    Set_vector3(omega,_zero3);    
    
    _init_flag = 0; // Uninitialized
    Set_vector3(_x_est,_unit_nk);    
    Set_L1_diag(alpha);
    
    
}
// Public methods
void ATTITUDE::gVector_to_EulerAngle(float v_out[], float v_in[])
{
    //
    // This function should be customized according to the definition of coordinate system
    //
    
    // Here we follow the definition in bicycle paper
    v_out[0] = 0.0; // phi, yaw
    v_out[2] = atan2(-v_in[1],v_in[0]); // theta, roll
    v_out[1] = atan2(cos(v_out[2])*v_in[2],v_in[0]); // psi, pitch
}
void ATTITUDE::Get_CrossProduct3(float v_c[], float v_a[], float v_b[]) // v_a X v_b
{
    v_c[0] = (-v_a[2]*v_b[1]) + v_a[1]*v_b[2];
    v_c[1] = v_a[2]*v_b[0] - v_a[0]*v_b[2];
    v_c[2] = (-v_a[1]*v_b[0]) + v_a[0]*v_b[1]; 
}
float ATTITUDE::Get_Vector3Norm(float v_in[])
{
    float temp = 0.0;
    
    for(int i=0; i<3; i++)
        temp += v_in[i]*v_in[i];
    return sqrt(temp); // <- Should chech if this function is available (?)
}
void ATTITUDE::Init(float y_in[]) // Let _x_est = y_in
{
    Set_vector3(ys,y_in);
    Set_vector3(_x_est,ys); // _x_est be set as y_in
    ++_init_flag;    
}
void ATTITUDE::Run(float y_in[], float omega_in[]) // Main alogorithm
{
    static float x_est_plus[3];
    static float cross_pruduct[3];
    
    Set_vector3(ys,y_in);
    Set_vector3(omega,omega_in);
    
    if(_init_flag < 3)
        ATTITUDE::Init(ys);
    else
    {
        Get_CrossProduct3(cross_pruduct,omega,ys);
        for(int i=0; i<3; i++)
            x_est_plus[i] = _x_est[i] + Ts*(_L1_diag[i]*(ys[i] - _x_est[i]) - cross_pruduct[i]);
             
        Set_vector3(_x_est,x_est_plus);
    }
    //
    Get_Estimation(x_est);
    gVector_to_EulerAngle(EulerAngle,_x_est);
}
void ATTITUDE::Get_Estimation(float v[])
{
    Set_vector3(v,_x_est);
}
void ATTITUDE::Get_Attitude_Euler(float v[])
{
    Set_vector3(v,EulerAngle);
}

// Private methods
void ATTITUDE::Set_vector3(float v_a[], float v_b[]) // Vectors in R^3. Let the values of v_b be set to v_a   
{
    for(int i=0; i<3; i++ )
        v_a[i] = v_b[i];
}
void ATTITUDE::Set_L1_diag(float alpha) // set diagnal element of gain matrix
{
    for(int i=0; i<3; i++)
        _L1_diag[i] = alpha;
}