Diff: EKF_RPY.cpp

Revision:

22:4148af9e3d81

Parent:

21:aab1ac72095b

--- a/EKF_RPY.cpp	Mon Jan 06 10:20:10 2020 +0000
+++ b/EKF_RPY.cpp	Mon Jan 27 09:24:35 2020 +0000
@@ -27,7 +27,8 @@
     /* [n_acc; n_mag] */
     var_gy << 40.0f, 40.0f, 1.0f, 1.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;
 }
@@ -63,11 +64,11 @@
     /* x = [ang; v; b_g] = [0: phi
                             1: theta
                             2: psi
-                            3: vx
-                            4: vy
-                            5: b_gx
-                            6: b_gy
-                            7: b_gz] */
+                            3: b_gx
+                            4: b_gy
+                            5: b_gz
+                            6: vx
+                            7: vy] */
     return x(i);
 }