Chun Feng Huang / ATTITUDE_ESTIMATION_WIPV

Attitude estimation using IMU (3-DoF ver.)

Fork of ATTITUDE_ESTIMATION by LDSC_Robotics_TAs

ATTITUDE_ESTIMATION.h@3:7deaf89fbe33, 2016-05-03 (annotated)

Committer:
benson516
Date:
Tue May 03 23:05:49 2016 +0000
Revision:
3:7deaf89fbe33
Parent:
2:46d355d7abaa
Child:
5:01e322f4158f
Add public array x_est[3]

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 1:edc7ccfc5562 40 float _unit_nk[3]; // (-k) direction [0;0;-1]
benson516 1:edc7ccfc5562 41 float _zero3[3]; // Zero vector [0;0;0]
benson516 1:edc7ccfc5562 42 //
benson516 0:8126c86bac2a 43 int8_t _init_flag; // Flag for displaying initialization status
benson516 1:edc7ccfc5562 44 float _x_est[3]; // Estimated state
benson516 1:edc7ccfc5562 45 // float _omega_x[3][3]; // Skew symmetric matrix of omega
benson516 0:8126c86bac2a 46
benson516 1:edc7ccfc5562 47
benson516 1:edc7ccfc5562 48 float _L1_diag[3]; // Diagonal vector of gain matrix L1
benson516 1:edc7ccfc5562 49
benson516 0:8126c86bac2a 50
benson516 0:8126c86bac2a 51 // Method
benson516 1:edc7ccfc5562 52 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 53 void Set_L1_diag(float alpha); // set diagnal element of gain matrix
benson516 0:8126c86bac2a 54
benson516 0:8126c86bac2a 55 };
benson516 0:8126c86bac2a 56 #endif // _ATTITUDE_ESTIMATION_H_