Chun Feng Huang / ATTITUDE_ESTIMATION_WIPV

Attitude estimation using IMU (3-DoF ver.)

Fork of ATTITUDE_ESTIMATION by LDSC_Robotics_TAs

ATTITUDE_ESTIMATION.h@2:46d355d7abaa, 2016-05-03 (annotated)

Committer:
benson516
Date:
Tue May 03 22:15:07 2016 +0000
Revision:
2:46d355d7abaa
Parent:
1:edc7ccfc5562
Child:
3:7deaf89fbe33
Shorten the function names to "Init" and "Run"

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 1:edc7ccfc5562 13 float EulerAngle[3];
benson516 1:edc7ccfc5562 14 float ys[3]; // Sensor output
benson516 1:edc7ccfc5562 15 float omega[3]; // Rotation speed in body-fixed frame
benson516 0:8126c86bac2a 16
benson516 0:8126c86bac2a 17 // Constructor:
benson516 1:edc7ccfc5562 18 // Initialize the estimator
benson516 1:edc7ccfc5562 19 ATTITUDE(float alpha_in, float Ts_in); // alpha in rad/sec., Ts in sec.
benson516 0:8126c86bac2a 20
benson516 0:8126c86bac2a 21 // Methods
benson516 0:8126c86bac2a 22 // Transformations
benson516 1:edc7ccfc5562 23 //float* Vector_to_SkewSymmetry(float v_in[]);
benson516 1:edc7ccfc5562 24 //float* SkewSymmetry_to_Vector(float M_in[3][]);
benson516 1:edc7ccfc5562 25 void gVector_to_EulerAngle(float v_out[],float v_in[]);
benson516 1:edc7ccfc5562 26 //float* EulerAngle_to_gVector(float Eu_in[]);
benson516 1:edc7ccfc5562 27
benson516 1:edc7ccfc5562 28 // vector operation
benson516 1:edc7ccfc5562 29 void Get_CrossProduct3(float v_c[],float v_a[], float v_b[]); // v_c = v_a X v_b
benson516 1:edc7ccfc5562 30 float Get_Vector3Norm(float v_in[]);
benson516 1:edc7ccfc5562 31
benson516 0:8126c86bac2a 32 // Estimator
benson516 2:46d355d7abaa 33 void Init(float y_in[]); // Let _x_est = y_in
benson516 2:46d355d7abaa 34 void Run(float y_in[], float omega_in[]); // Main alogorithm
benson516 1:edc7ccfc5562 35 void Get_Estimation(float v[]);
benson516 1:edc7ccfc5562 36 void Get_Attitude_Euler(float v[]);
benson516 0:8126c86bac2a 37
benson516 0:8126c86bac2a 38 private:
benson516 1:edc7ccfc5562 39 float _unit_nk[3]; // (-k) direction [0;0;-1]
benson516 1:edc7ccfc5562 40 float _zero3[3]; // Zero vector [0;0;0]
benson516 1:edc7ccfc5562 41 //
benson516 0:8126c86bac2a 42 int8_t _init_flag; // Flag for displaying initialization status
benson516 1:edc7ccfc5562 43 float _x_est[3]; // Estimated state
benson516 1:edc7ccfc5562 44 // float _omega_x[3][3]; // Skew symmetric matrix of omega
benson516 0:8126c86bac2a 45
benson516 1:edc7ccfc5562 46
benson516 1:edc7ccfc5562 47 float _L1_diag[3]; // Diagonal vector of gain matrix L1
benson516 1:edc7ccfc5562 48
benson516 0:8126c86bac2a 49
benson516 0:8126c86bac2a 50 // Method
benson516 1:edc7ccfc5562 51 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 52 void Set_L1_diag(float alpha); // set diagnal element of gain matrix
benson516 0:8126c86bac2a 53
benson516 0:8126c86bac2a 54 };
benson516 0:8126c86bac2a 55 #endif // _ATTITUDE_ESTIMATION_H_