File content as of revision 6:c362ed165c39:

#include "ATTITUDE_ESTIMATION.h"

//=====================LPF ====================//
LPF_vector::LPF_vector(size_t dimension, float samplingTime, float cutOff_freq_Hz_in)
{
    n = dimension;
    Ts = samplingTime;
    cutOff_freq_Hz = cutOff_freq_Hz_in;
    alpha_Ts = (2*3.1415926)*cutOff_freq_Hz*Ts;
    One_alpha_Ts = 1.0 - alpha_Ts;

    zeros.assign(n, 0.0);

    output = zeros;

    //
    Flag_Init = false;
}

vector<float> LPF_vector::filter(const vector<float> &input)
{
    // Initialization
    if (!Flag_Init){
        reset(input);
        Flag_Init = true;
        return output;
    }

    for (size_t i = 0; i < n; ++i){
        // output = One_alpha_Ts*output + alpha_Ts*input;
        output[i] += alpha_Ts*(input[i] - output[i]);
    }

    return output;
}
void LPF_vector::reset(const vector<float> &input)
{
    // output = (1.0 - alpha_Ts)*output + alpha_Ts*input;
    output = input;
    return;
}

//-------------------------------------------------------//
ATTITUDE::ATTITUDE(float alpha_in, float one_over_gamma_in, float Ts_in):
                        alpha(alpha_in),
                        one_over_gamma(one_over_gamma_in),
                        Ts(Ts_in),
                        lpfv_ys(3, Ts_in, 10.0), // Input filter
                        lpfv_w(3, Ts_in, 200.0) // Input filter
{
    // Dimension
    n = 3;
    //
    init_flag = 0; // Uninitialized

    // Default: close the gyro-bias estimation
    enable_biasEst = false;

    // Unit transformation
    pi = 3.1415926;
    deg2rad = pi/180.0;
    rad2deg = 180.0/pi;

    // The map from "real" coordinate to "here" coordinate
    // eg. accMap_real2here = [3,-1,-2];
    // means: real  ->  here
    //     1   x         z   3
    //     2   y        -x  -1
    //     3   z        -y  -2
    // int accmap_temp[] = {3,-1,-2};
    int accmap_temp[] = {-3,1,2}; // Reverse: The direction of accelerometer is defined based on the direction of the acceleration of the sensor, not the g-direction
    // int accmap_temp[] = {1, 2, 3};
    //
    int gyroMap_temp[] = {3,-1,-2};
    accMap_real2here.assign(accmap_temp, accmap_temp+n);
    gyroMap_real2here.assign(gyroMap_temp, gyroMap_temp+n);

    // zeros
    zeros.assign(n,0.0);
    // unit_nz
    unit_nx = zeros;
    unit_ny = zeros;
    unit_nz = zeros;
    unit_nx[0] = -1; // negative x
    unit_ny[1] = -1; // negative y
    unit_nz[2] = -1; // negative z

    // States
    x_est = unit_nx; // x is pointing downward
    gyroBias_est = zeros;
    omega = zeros;
    ys = zeros;
    w_cross_ys = zeros; // omega X ys
    ys_cross_x_ys = zeros; // ys X (x_est - ys)

    // Eular angles, in rad/s
    pitch = 0.0;
    roll = 0.0;
    yaw = 0.0;


    // Gain matrix
    Set_L1_diag(alpha);

}

// real -> here
void ATTITUDE::InputMapping(vector<float> &v_hereDef, const vector<float> &v_realDef, const vector<int> &map_real2here){
    // The map from "real" coordinate to "here" coordinate
    // eg. accMap_real2here = [3,-1,-2];
    // means: real  ->  here
    //     1   x         z   3
    //     2   y        -x  -1
    //     3   z        -y  -2
    // vector<int> accMap_real2here = {3,-1,-2};
    // vector<int> gyroMap_real2here = {3,-1,-2};

    // Iterate through "real" coordinates
    int idx_here = 1;
    for (size_t i = 0; i < n; ++i){
        idx_here = map_real2here[i];
        if (idx_here > 0){
            v_hereDef[idx_here-1] = v_realDef[i];
        }else{
            v_hereDef[-idx_here-1] = -1*v_realDef[i];
        }
    }
}
// here -> real
void ATTITUDE::OutputMapping(vector<float> &v_realDef, const vector<float> &v_hereDef, const vector<int> &map_real2here){
    // This is the inverse mapping of the InputMapping

    // The map from "real" coordinate to "here" coordinate
    // eg. accMap_real2here = [3,-1,-2];
    // means: real  ->  here
    //     1   x         z   3
    //     2   y        -x  -1
    //     3   z        -y  -2
    // vector<int> accMap_real2here = {3,-1,-2};
    // vector<int> gyroMap_real2here = {3,-1,-2};

    // Iterate through "real" coordinates
    int idx_here = 1;
    for (size_t i = 0; i < n; ++i){
        idx_here = map_real2here[i];
        if (idx_here > 0){
            v_realDef[i] = v_hereDef[idx_here-1];
        }else{
            v_realDef[i] = -1*v_hereDef[-idx_here-1];
        }
    }
}

// Public methods
void ATTITUDE::gVector_to_EulerAngle(const vector<float> &v_in)
{
    //
    // This function should be customized according to the definition of coordinate system
    //

    /*
    // Here we follow the definition in bicycle paper
    yaw = 0.0; // phi, yaw
    roll = atan2(-v_in[1],v_in[0]); // theta, roll
    pitch = atan2(cos(roll)*v_in[2],v_in[0]); // psi, pitch
    */

    // Eular angle: 1-3-2, zs is pointing forwasd
    yaw = 0.0; // phi, yaw
    pitch = atan2(-v_in[2],-v_in[0]); // psi, pitch
    if (abs(pitch) <  0.7854){ // pi/4
        roll = atan2(cos(pitch)*v_in[1],-v_in[0]); // theta, roll
    }else{
        if (pitch >= 0.0)
            roll = atan2(sin(pitch)*v_in[1],-v_in[2]); // theta, roll
        else
            roll = atan2(-sin(pitch)*v_in[1],v_in[2]); // theta, roll
    }



}
void ATTITUDE::Get_CrossProduct3(vector<float> &v_c, const vector<float> &v_a, const vector<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];
}
vector<float> ATTITUDE::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
{
    static vector<float> v_c(n);
    for (size_t i = 0; i < n; ++i){
        if (is_minus){
            v_c[i] = v_a[i] - v_b[i];
        }else{
            v_c[i] = v_a[i] + v_b[i];
        }
    }
    return v_c;
}
vector<float> ATTITUDE::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
{
    static vector<float> v_c(n);
    for (size_t i = 0; i < n; ++i){
        v_c[i] = scale*v_a[i];

    }
    return v_c;
}
float ATTITUDE::Get_Vector3Norm(const vector<float> &v_in)
{
    float temp = 0.0;

    for (size_t i = 0; i < n; ++i)
        temp += v_in[i]*v_in[i];
    return sqrt(temp); // <- Should check if this function is available (?)
}
void ATTITUDE::Normolization(vector<float> &V_out, const vector<float> &V_in){
    float norm = Get_Vector3Norm(V_in);
    for (size_t i = 0; i < n; ++i){
        V_out[i] = V_in[i]/norm;
    }
}
void ATTITUDE::Init(const vector<float> &y_in) // Let _x_est = y_in
{
    // ys = y_in;
    // Normolization(x_est,y_in); // x_est be set as normalized y_in
    x_est = y_in;
    ++init_flag;
}
void ATTITUDE::iterateOnce(const vector<float> &y_in, const vector<float> &omega_in) // Main alogorithm
{
    InputMapping(ys, y_in, accMap_real2here);
    InputMapping(omega, omega_in, gyroMap_real2here);

    // Input filter
    ys = lpfv_ys.filter(ys);
    // omega = lpfv_w.filter(omega);

    // gyro-bias estimation
    if (enable_biasEst){
        omega = Get_VectorPlus(omega,gyroBias_est, true); // omega - gyroBias_est
    }

    //
    if(init_flag < 3){
        Init(ys);
    }
    else{
        //
        Get_CrossProduct3(w_cross_ys,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]) - w_cross_ys[i]);
            x_est[i] += Ts*( L1_diag[i]*(ys[i] - x_est[i]) - w_cross_ys[i]);
        }

        // gyro-bias estimation
        if (enable_biasEst){
            updateGyroBiasEst();
        }
    }
    //
    gVector_to_EulerAngle(x_est);
}
void ATTITUDE::updateGyroBiasEst(void){ // Update the gyro bias estimation
    if (one_over_gamma == 0.0){
        return;
    }
    //
    Get_CrossProduct3(ys_cross_x_ys, ys, Get_VectorPlus(x_est,ys,true));
    //
    gyroBias_est = Get_VectorPlus(gyroBias_est, Get_VectorScalarMultiply(ys_cross_x_ys, (one_over_gamma)), true);
}
// transform the x_est into "real" coordinate
void ATTITUDE::getEstimation_realCoordinate(vector<float> &V_out){
    OutputMapping(V_out,x_est,accMap_real2here);
}
// Get Eular angles
float ATTITUDE::pitch_deg(void){
    return (rad2deg*pitch);
}
float ATTITUDE::roll_deg(void){
    return (rad2deg*roll);
}
float ATTITUDE::yaw_deg(void){
    return (rad2deg*yaw);
}

// Private methods
void ATTITUDE::Set_L1_diag(float alpha_in) // set diagnal element of gain matrix
{
    alpha = alpha_in;
    L1_diag.assign(n,alpha_in);
}