altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

Diff: EKF.h

Revision:
20:1182bc29c195
Parent:
19:42ea6dd68185
Child:
21:31e01d3e0143
--- a/EKF.h	Wed Oct 02 15:30:15 2019 +0000
+++ b/EKF.h	Wed Oct 09 13:45:36 2019 +0000
@@ -5,18 +5,19 @@
 #include <mbed.h>
 #include "matrix.h"
 
-class ekf
+class EKF
 {
 public:
 
-    ekf(float);
-    virtual ~ekf();
+    EKF(float);
+    virtual ~EKF();
     float get_est_state(uint8_t);
     void loop(matrix *);
     void display_matrix(char);
     float getYaw(void){
         return 0;
         }
+    void update(float,float,float,float,float,float);
 private:
     matrix x;
     matrix x_km;
@@ -30,6 +31,8 @@
     float g,tau_g,m,itau_g,k1,kdm;
     float Ts;
     void dgl(matrix *);
+    float my_sin(float x){return x;}
+    float my_cos(float x){return 1.0f-0.5*x*x;}
 };
 
 #endif
\ No newline at end of file