Diff: EKF_RPY.cpp

Revision:

12:180e09c4ea26

Parent:

9:be2e1d2f683d

Child:

13:2e03d9c03409

diff -r 6a7c3795d8fb -r 180e09c4ea26 EKF_RPY.cpp
--- a/EKF_RPY.cpp	Mon Oct 21 09:20:25 2019 +0000
+++ b/EKF_RPY.cpp	Tue Oct 22 19:37:32 2019 +0000
@@ -6,6 +6,22 @@
 EKF_RPY::EKF_RPY(float Ts, float mx0, float my0, float mz0)
 {
     this->Ts = Ts;
+    set_para();
+    m0 << mx0, my0, mz0;
+    reset();
+}
+
+EKF_RPY::EKF_RPY(float Ts) 
+{
+    this->Ts = Ts;
+    set_para();
+    reset();
+}
+
+EKF_RPY::~EKF_RPY() {}
+
+void EKF_RPY::set_para()
+{   
     /* [n_gyro; n_v; n_b_g] */
     var_fx << 0.01f, 0.01f, 0.01f, 0.0001f, 0.0001f, 0.0001f, 0.0001f, 0.0001f;
     /* [n_acc; n_mag] */
@@ -13,12 +29,8 @@
     rho = 0.5f;
     kv = 0.5f;       /* k1/m */
     g = 9.81f;
-    m0 << mx0, my0, mz0;
-    reset();
 }
 
-EKF_RPY::~EKF_RPY() {}
-
 void EKF_RPY::reset()
 {   
     u.setZero();
@@ -34,6 +46,11 @@
     I.setIdentity();
 }
 
+void EKF_RPY::set_m0(float mx0, float my0, float mz0)
+{
+    m0 << mx0, my0, mz0;
+}
+
 float EKF_RPY::get_est_state(uint8_t i)
 {   
     /* x = [ang; v; b_g] = [0: phi