Chun Feng Huang / ATTITUDE_ESTIMATION_WIPV

Attitude estimation using IMU (3-DoF ver.)

Fork of ATTITUDE_ESTIMATION by LDSC_Robotics_TAs

ATTITUDE_ESTIMATION.h@5:01e322f4158f, 2016-05-04 (annotated)

Committer:
benson516
Date:
Wed May 04 10:47:37 2016 +0000
Revision:
5:01e322f4158f
Parent:
3:7deaf89fbe33
Child:
6:c362ed165c39
_unit_nk is changed to _unit_nz

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:8126c86bac2a 1 #ifndef _ATTITUDE_ESTIMATION_H_
benson516 0:8126c86bac2a 2 #define _ATTITUDE_ESTIMATION_H_
benson516 0:8126c86bac2a 3 #include "mbed.h"
benson516 0:8126c86bac2a 4
benson516 0:8126c86bac2a 5 // Class
benson516 0:8126c86bac2a 6 class ATTITUDE{
benson516 0:8126c86bac2a 7 public:
benson516 0:8126c86bac2a 8
benson516 0:8126c86bac2a 9 // Variables
benson516 1:edc7ccfc5562 10 float alpha; // Convergent rate, rad/sec.
benson516 1:edc7ccfc5562 11 float Ts; // Sampling time, sec.
benson516 0:8126c86bac2a 12
benson516 3:7deaf89fbe33 13 float x_est[3]; // Estimated state
benson516 1:edc7ccfc5562 14 float EulerAngle[3];
benson516 1:edc7ccfc5562 15 float ys[3]; // Sensor output
benson516 1:edc7ccfc5562 16 float omega[3]; // Rotation speed in body-fixed frame
benson516 0:8126c86bac2a 17
benson516 0:8126c86bac2a 18 // Constructor:
benson516 1:edc7ccfc5562 19 // Initialize the estimator
benson516 1:edc7ccfc5562 20 ATTITUDE(float alpha_in, float Ts_in); // alpha in rad/sec., Ts in sec.
benson516 0:8126c86bac2a 21
benson516 0:8126c86bac2a 22 // Methods
benson516 0:8126c86bac2a 23 // Transformations
benson516 1:edc7ccfc5562 24 //float* Vector_to_SkewSymmetry(float v_in[]);
benson516 1:edc7ccfc5562 25 //float* SkewSymmetry_to_Vector(float M_in[3][]);
benson516 1:edc7ccfc5562 26 void gVector_to_EulerAngle(float v_out[],float v_in[]);
benson516 1:edc7ccfc5562 27 //float* EulerAngle_to_gVector(float Eu_in[]);
benson516 1:edc7ccfc5562 28
benson516 1:edc7ccfc5562 29 // vector operation
benson516 1:edc7ccfc5562 30 void Get_CrossProduct3(float v_c[],float v_a[], float v_b[]); // v_c = v_a X v_b
benson516 1:edc7ccfc5562 31 float Get_Vector3Norm(float v_in[]);
benson516 1:edc7ccfc5562 32
benson516 0:8126c86bac2a 33 // Estimator
benson516 2:46d355d7abaa 34 void Init(float y_in[]); // Let _x_est = y_in
benson516 2:46d355d7abaa 35 void Run(float y_in[], float omega_in[]); // Main alogorithm
benson516 1:edc7ccfc5562 36 void Get_Estimation(float v[]);
benson516 1:edc7ccfc5562 37 void Get_Attitude_Euler(float v[]);
benson516 0:8126c86bac2a 38
benson516 0:8126c86bac2a 39 private:
benson516 5:01e322f4158f 40 // Variables
benson516 5:01e322f4158f 41 float _unit_nz[3]; // (-z) direction [0;0;-1]
benson516 1:edc7ccfc5562 42 float _zero3[3]; // Zero vector [0;0;0]
benson516 1:edc7ccfc5562 43 //
benson516 0:8126c86bac2a 44 int8_t _init_flag; // Flag for displaying initialization status
benson516 1:edc7ccfc5562 45 float _x_est[3]; // Estimated state
benson516 1:edc7ccfc5562 46 // float _omega_x[3][3]; // Skew symmetric matrix of omega
benson516 0:8126c86bac2a 47
benson516 1:edc7ccfc5562 48
benson516 1:edc7ccfc5562 49 float _L1_diag[3]; // Diagonal vector of gain matrix L1
benson516 1:edc7ccfc5562 50
benson516 0:8126c86bac2a 51
benson516 5:01e322f4158f 52 // Methods
benson516 1:edc7ccfc5562 53 void Set_vector3(float v_a[], float v_b[]); // Vectors in R^3. Let the values of v_b be set to v_a
benson516 1:edc7ccfc5562 54 void Set_L1_diag(float alpha); // set diagnal element of gain matrix
benson516 0:8126c86bac2a 55
benson516 0:8126c86bac2a 56 };
benson516 0:8126c86bac2a 57 #endif // _ATTITUDE_ESTIMATION_H_