File content as of revision 13:16bc19155f54:

#ifndef _ATTITUDE_ESTIMATION_H_
#define _ATTITUDE_ESTIMATION_H_

#include "mbed.h"
#include "FILTER_LIB.h"
#include <vector>

using std::vector;

/*
class LPF_vector
{public:
    vector<float> output;

    LPF_vector(size_t dimension, float samplingTime, float cutOff_freq_Hz_in); // cutOff_freq_Hz_in is in "Hz"
    vector<float> filter(const vector<float> &v_in);
    void reset(const vector<float> &v_in);

private:
    size_t n;
    float Ts;
    float cutOff_freq_Hz; // Hz
    float alpha_Ts;
    float One_alpha_Ts;

    // Flag
    bool Flag_Init;

    //
    vector<float> zeros; // Zero vector [0;0;0]
};
*/

// Class
class ATTITUDE{
public:

    // Variables
    float alpha; // Convergent rate, rad/sec.
    float one_over_gamma; // 1/gamma, one_over_gamma == 0 means no estimation on gyro bias
    float Ts; // Sampling time, sec.
    bool enable_biasEst; // Enable the gyro-bias estimation capability
    bool enable_magEst; // Enable the estimation of magenetic field

    // 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;
    vector<int> magMap_real2here;
    vector<int> gyroMap_real2here;

    vector<float> xg_est; // Estimated g-vector
    vector<float> xm_est; // Estimated m-vector (magnetic field)
    // vector<float> x_est; // Estimated state
    vector<float> gyroBias_est; // The estimated gyro bias in each channel
    vector<float> omega; // Rotation speed in body-fixed frame
    //
    vector<float> y_acce; // Accelerometer outputs
    vector<float> y_mag; // Magnetometer outputs

    // Linear acceleration estimation
    vector<float> acce_sensorFrame; // The residual of acceleration, acce_sensorFrame = y_acce - xg_est

    // No use, may be deleted
    // vector<float> w_cross_ys; // omega X ys
    vector<float> ys_cross_x_ys; // ys X (x_est - ys)

    // Eular angles, in rad/s
    float pitch;
    float roll;
    float yaw;
    //
    float pitchRate;
    float rollRate;
    float yawRate;


    // Constructor:
    // Initialize the estimator
    ATTITUDE(float alpha_in, float Ts_in); // alpha in rad/sec., Ts in sec.

    // Methods
    // Get the estimation results in Eular angle
    // vector <--> Eular angle
    void Vectors_to_EulerAngle(const vector<float> &vg_in, const vector<float> &vm_in);
    //float* EulerAngle_to_gVector(float Eu_in[]);

    // Setting parameters
    // Set L1, the diagonal matrix
    void Set_L1_diag(float alpha_in); // set diagnal element of gain matrix
    void enable_gyroBiasEst(float gamma_in); // Enable the gyro-bias estimation

    // Estimator
    void Init(void); // Let x_est = ys
    // Two version of estimator, with/without magenetic field estimation
    void iterateOnce(const vector<float> &y_acce_in, const vector<float> &omega_in); // Main alogorithm
    void iterateOnce(const vector<float> &y_acce_in, const vector<float> &y_mag_in, const vector<float> &omega_in); // Main alogorithm
    // Get the results
    void getEstimation_realCoordinate(vector<float> &V_out);
    float pitch_deg(void);
    float roll_deg(void);
    float yaw_deg(void);

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

private:
    // Dimension
    size_t n;

    // Variables
    vector<float> unit_nx; // (-x) direction [-1;0;0]
    vector<float> unit_ny; // (-y) direction [0;-1;0]
    vector<float> unit_nz; // (-z) direction [0;0;-1]
    vector<float> zeros; // Zero vector [0;0;0]
    //
    size_t init_flag; // Flag for displaying initialization status
    // float _omega_x[3][3]; // Skew symmetric matrix of omega

    vector<float> L1_diag; // Diagonal vector of gain matrix L1

    //
    // Input/output coordinate transformations within the different definitions between the "real" one and the "here" one
    void InputMapping(vector<float> &v_hereDef, const vector<float> &v_realDef, const vector<int> &map_real2here);
    void OutputMapping(vector<float> &v_realDef, const vector<float> &v_hereDef, const vector<int> &map_real2here);


    // The kernel of the estimation process
    void EstimationKernel(vector<float> &_x_est_, const vector<float> &_ys_, const vector<float> &_omega_);
    void updateGyroBiasEst(void);

    // Utilities
    // vector operation
    //float* Vector_to_SkewSymmetry(float v_in[]);
    //float* SkewSymmetry_to_Vector(float M_in[3][]);
    void Get_CrossProduct3(vector<float> &v_c, const vector<float> &v_a, const vector<float> &v_b); // v_a X v_b
    vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b
    vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a
    float Get_Vector3Norm(const vector<float> &v_in);
    void Normolization(vector<float> &V_out, const vector<float> &V_in);

    // Low-pass filter, vector version
    LPF_vector lpfv_y_acce;
    LPF_vector lpfv_y_mag;
    LPF_vector lpfv_w;
};

#endif // _ATTITUDE_ESTIMATION_H_