File content as of revision 13:16bc19155f54:

#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 Ts_in):
                        alpha(alpha_in),
                        Ts(Ts_in),
                        lpfv_y_acce(3, Ts_in, 200.0), // Input filter for accelerometers
                        lpfv_y_mag(3, Ts_in, 200.0), // Input filter for magenetometers
                        lpfv_w(3, Ts_in, 200.0) // Input filter for gyroscope
{
    // Dimension
    n = 3;
    //
    init_flag = 0; // Uninitialized

    // Default: close the gyro-bias estimation
    enable_biasEst = false;
    one_over_gamma = 0.0;
    // Default: close the estimation for magenetic field
    enable_magEst = false;

    // Unit transformation
    pi = 3.1415926;
    deg2rad = pi/180.0;
    rad2deg = 180.0/pi;
    gravity = 9.81; //  m/s^2

    // 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 magMap_temp[] = {3,-1, 2}; // real z-axis is in the reverse direction
    int gyroMap_temp[] = {3,-1,-2};
    accMap_real2here.assign(accmap_temp, accmap_temp+n);
    magMap_real2here.assign(magMap_temp, magMap_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
    xg_est = unit_nx; // g is pointing downward
    xm_est = Get_VectorScalarMultiply(unit_nz, -1.0); // m is pointing forward
    gyroBias_est = zeros;
    omega = zeros;
    //
    y_acce = zeros; // Accelerometer outputs
    y_mag = zeros; // Magnetometer outputs

    // Linear acceleration estimation
    acce_sensorFrame = 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;
    //
    pitchRate = 0.0;
    rollRate = 0.0;
    yawRate = 0.0;

    // Gain matrix
    Set_L1_diag(alpha);

}
// Public methods
void ATTITUDE::Vectors_to_EulerAngle(const vector<float> &vg_in, const vector<float> &vm_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(-vg_in[1],vg_in[0]); // theta, roll
    pitch = atan2(cos(roll)*vg_in[2],vg_in[0]); // psi, pitch
    */

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

    // Calculate the yaw angle
    if (enable_magEst){
        float num = vm_in[1]*cos(pitch);
        float den = (vm_in[2]*cos(roll) - vm_in[1]*sin(pitch)*sin(roll));
        yaw = atan2(num, den);
    }else{
        yaw = 0.0; // phi, yaw
    }


    // Calculate the rate of each Eular angle
    if (abs(roll) > 1.2217 ){ // 75 deg, singular case
        yawRate = 0.0; // 0.0, approximation
        pitchRate = omega[1]; // w2, approximation
    }else{ // Normal case
        // yawRate = (w1*cos(psi) + w3*sin(psi))/cos(theta)
        yawRate = (omega[0]*cos(pitch) + omega[2]*sin(pitch))/cos(roll);
        // pitchRate = w2 + yawRate*sin(theta)
        pitchRate = omega[1] + yawRate*sin(roll);
    }
    // rollRate = w3*cos(psi) - w1*sin(psi)
    rollRate = (omega[2]*cos(pitch) - omega[0]*sin(pitch));

}
// Setting parameters
// Set L1, the diagonal matrix
void ATTITUDE::Set_L1_diag(float alpha_in) // set diagnal element of gain matrix
{
    alpha = alpha_in;
    L1_diag.assign(n,alpha_in);
}
void ATTITUDE::enable_gyroBiasEst(float gamma_in){ // Enable the gyro-bias estimation
    enable_biasEst = true;
    one_over_gamma = 1/gamma_in;
}
//
void ATTITUDE::Init(void) // Let x_est = ys
{
    // y_acce = y_in;
    // Normolization(xg_est,y_in); // xg_est be set as normalized y_in
    xg_est = y_acce;
    xm_est = y_mag;
    ++init_flag;
}
void ATTITUDE::iterateOnce(const vector<float> &y_acce_in, const vector<float> &omega_in) // Main alogorithm
{
    enable_magEst = false; // no magenetometers input
    // Input mapping
    InputMapping(y_acce, y_acce_in, accMap_real2here);
    // InputMapping(y_mag, y_mag_in, magMap_real2here);
    InputMapping(omega, omega_in, gyroMap_real2here);

    // Input filter
    // y_acce = lpfv_y_acce.filter(y_acce);
    // y_mag = lpfv_y_mag.filter(y_mag);
    // omega = lpfv_w.filter(omega);

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

    //
    if(init_flag < 3){
        Init();
    }
    else{
        // Estimation kernel process
        EstimationKernel(xg_est, y_acce, omega);
        // EstimationKernel(xm_est, y_mag, omega);

        // gyro-bias estimation
        if (enable_biasEst){
            updateGyroBiasEst();
        }
    }

    // Calculate the linear acceleration
    acce_sensorFrame = Get_VectorPlus(y_acce, xg_est, true); // minus
    
    //
    Vectors_to_EulerAngle(xg_est,xm_est);
}
void ATTITUDE::iterateOnce(const vector<float> &y_acce_in, const vector<float> &y_mag_in, const vector<float> &omega_in) // Main alogorithm
{
    enable_magEst = true; // with magenetometers input
    // Input mapping
    InputMapping(y_acce, y_acce_in, accMap_real2here);
    InputMapping(y_mag, y_mag_in, magMap_real2here);
    InputMapping(omega, omega_in, gyroMap_real2here);

    // Input filter
    // y_acce = lpfv_y_acce.filter(y_acce);
    // y_mag = lpfv_y_mag.filter(y_mag);
    // omega = lpfv_w.filter(omega);

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

    //
    if(init_flag < 3){
        Init();
    }
    else{
        // Estimation kernel process
        EstimationKernel(xg_est, y_acce, omega);
        EstimationKernel(xm_est, y_mag, omega);

        // gyro-bias estimation
        if (enable_biasEst){
            updateGyroBiasEst();
        }
    }

    // Calculate the linear acceleration
    acce_sensorFrame = Get_VectorPlus(y_acce, xg_est, true); // minus
    //
    Vectors_to_EulerAngle(xg_est,xm_est);
}
// transform the x_est into "real" coordinate
void ATTITUDE::getEstimation_realCoordinate(vector<float> &V_out){
    OutputMapping(V_out,xg_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
////////////////////////////////////
// Input/output coordinate transformations within the different definitions between the "real" one and the "here" one
// 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];
        }
    }
}
// The kernel of the estimation process
////////////////////////////////////////
void ATTITUDE::EstimationKernel(vector<float> &_x_est_, const vector<float> &_ys_, const vector<float> &_omega_){
    static vector<float> _w_cross_ys_;
    Get_CrossProduct3(_w_cross_ys_, _omega_, _ys_);
    for(size_t i = 0; i < n; ++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]);
    }

}
void ATTITUDE::updateGyroBiasEst(void){ // Update the gyro bias estimation
    if (one_over_gamma == 0.0){
        return;
    }
    //
    Get_CrossProduct3(ys_cross_x_ys, y_acce, Get_VectorPlus(xg_est,y_acce,true));
    //
    gyroBias_est = Get_VectorPlus(gyroBias_est, Get_VectorScalarMultiply(ys_cross_x_ys, (one_over_gamma)), true);
}

//////////////////////////////////////// end The kernal of the estimation process

// Utilities
// vector operation
void ATTITUDE::Get_CrossProduct3(vector<float> &v_c, const vector<float> &v_a, const vector<float> &v_b) // v_a X v_b
{
    // Check the size
    if (v_c.size() != n){
        v_c.resize(n);
    }
    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;
    }
}