Diff: EKF_RP.cpp

Revision:

21:aab1ac72095b

Parent:

20:0227c82a78c2

Child:

22:4148af9e3d81

diff -r 0227c82a78c2 -r aab1ac72095b EKF_RP.cpp
--- a/EKF_RP.cpp	Mon Oct 28 12:14:59 2019 +0000
+++ b/EKF_RP.cpp	Mon Jan 06 10:20:10 2020 +0000
@@ -13,7 +13,7 @@
     rho = 1.0f;
     kv = 0.5f;       /* k1/m */
     g = 9.81f;
-    scale_P0 = 1000000.0f;
+    scale_P0 = 1000.0f;
     reset();
 }
 
@@ -42,12 +42,12 @@
 
 float EKF_RP::get_est_state(uint8_t i)
 {   
-    /* x = [ang; v; b_g] = [0: phi
+    /* x = [ang; b_g; v] = [0: phi
                             1: theta
-                            2: vx
-                            3: vy
-                            4: b_gx
-                            5: b_gy] */
+                            2: b_gx
+                            3: b_gy
+                            4: vx
+                            5: vy] */
     return x(i);
 }