Diff: ATTITUDE_ESTIMATION.h

Revision:

0:8126c86bac2a

Child:

1:edc7ccfc5562

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ATTITUDE_ESTIMATION.h	Thu Apr 28 09:38:58 2016 +0000
@@ -0,0 +1,52 @@
+#ifndef _ATTITUDE_ESTIMATION_H_
+#define _ATTITUDE_ESTIMATION_H_
+#include "mbed.h"
+
+typedef struct{
+    float e[3];
+    } VECTOR3;
+typedef struct{
+    float e[3][3];
+    } MATRIX33;
+
+// Class
+class ATTITUDE{
+public:
+        
+    // Variables
+    VECTOR3 x_est;
+    VECTOR3 EulerAngle;
+    
+    
+    // Constructor:
+    // Initialize the estimator, 
+    ATTITUDE();
+    
+    // Methods
+    // Transformations
+    MATRIX33 Vector_to_SkewSymmetry(VECTOR3);
+    VECTOR3 SkewSymmetry_to_Vector(MATRIX33);
+    VECTOR3 gVector_to_EulerAngle(VECTOR3);
+    VECTOR3 EulerAngle_to_gVector(VECTOR3);   
+    // Estimator
+    void Init_Estimator(VECTOR3);
+    void Reset_Estimator(VECTOR3);
+    void Run_Estimator(VECTOR3);
+
+private:
+    int8_t _init_flag; // Flag for displaying initialization status 
+    VECTOR3 _x_est; // Estimated state
+    VECTOR3 _y; // Sensor output
+    VECTOR3 _omega; // Rotation speed in body-fixed frame
+    // MATRIX33 _Omega_cross; // Skew-symetric matrix of rotaion speed
+    
+    MATRIX33 _L1; // Gain matrix
+    float _Ts; // Sampling time
+    
+    // Method
+    void Set_x_est(VECTOR3);
+    void Set_measurements(float[],float[]); // array -> VECTOR33; y, omega
+    void Set_L1(MATRIX33); // set gain matrix
+
+};
+#endif // _ATTITUDE_ESTIMATION_H_
\ No newline at end of file