Diff: EKF_RP.cpp

Revision:

22:4148af9e3d81

Parent:

21:aab1ac72095b

Child:

24:e5188a2d72ca

diff -r aab1ac72095b -r 4148af9e3d81 EKF_RP.cpp
--- a/EKF_RP.cpp	Mon Jan 06 10:20:10 2020 +0000
+++ b/EKF_RP.cpp	Mon Jan 27 09:24:35 2020 +0000
@@ -11,7 +11,8 @@
     /* [n_acc] */
     var_gy << 40.0f, 40.0f;
     rho = 1.0f;
-    kv = 0.5f;       /* k1/m */
+    // kv = 0.5f;       /* QK3 k1/m */
+    kv = 0.26f;       /* QK4 k1/m */
     g = 9.81f;
     scale_P0 = 1000.0f;
     reset();
@@ -42,12 +43,12 @@
 
 float EKF_RP::get_est_state(uint8_t i)
 {   
-    /* x = [ang; b_g; v] = [0: phi
+    /* x = [ang; v; b_g] = [0: phi
                             1: theta
-                            2: b_gx
-                            3: b_gy
-                            4: vx
-                            5: vy] */
+                            2: vx
+                            3: vy
+                            4: b_gx
+                            5: b_gy] */
     return x(i);
 }