altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

Files at this revision

API Documentation at this revision

Revision 19:ccb6fc8bf872, committed 2019-10-28

Comitter:
pmic
Date:
Mon Oct 28 12:08:15 2019 +0000
Parent:
18:f374ca9a3e75
Child:
20:0227c82a78c2
Commit message:
Add increase_diag_P function so diag(P) can be increased while arming copter.

Changed in this revision

EKF_RP.cpp Show annotated file Show diff for this revision Revisions of this file
EKF_RP.h Show annotated file Show diff for this revision Revisions of this file
EKF_RPY.cpp Show annotated file Show diff for this revision Revisions of this file
EKF_RPY.h Show annotated file Show diff for this revision Revisions of this file 
--- a/EKF_RP.cpp	Mon Oct 28 11:31:48 2019 +0000
+++ b/EKF_RP.cpp	Mon Oct 28 12:08:15 2019 +0000
@@ -13,6 +13,7 @@
     rho = 1.0f;
     kv = 0.5f;       /* k1/m */
     g = 9.81f;
+    scale_P0 = 1000000.0f;
     reset();
 }
 
@@ -28,10 +29,16 @@
     calc_H();
     initialize_Q();
     initialize_R();
-    P = 1000000.0f * ( F * P * F.transpose() + Q );
     K.setZero();
     I.setIdentity();
     e.setZero();
+    P.setZero();
+    increase_diag_P();
+}
+
+void EKF_RP::increase_diag_P()
+{
+    P = P + scale_P0 * I;      
 }
 
 float EKF_RP::get_est_state(uint8_t i)

--- a/EKF_RP.h	Mon Oct 28 11:31:48 2019 +0000
+++ b/EKF_RP.h	Mon Oct 28 12:08:15 2019 +0000
@@ -15,6 +15,7 @@
     virtual ~EKF_RP();
     
     void reset();
+    void increase_diag_P();
     float get_est_state(uint8_t i);
     void update(float gyro_x, float gyro_y, float accel_x, float accel_y);
     
@@ -25,6 +26,7 @@
     float s2;
     float c2;
     
+    float scale_P0;
     float g;
     float kv;
     float Ts;

--- a/EKF_RPY.cpp	Mon Oct 28 11:31:48 2019 +0000
+++ b/EKF_RPY.cpp	Mon Oct 28 12:08:15 2019 +0000
@@ -29,6 +29,7 @@
     rho = 1.0f;
     kv = 0.5f;       /* k1/m */
     g = 9.81f;
+    scale_P0 = 1000000.0f;
 }
 
 void EKF_RPY::reset()
@@ -41,10 +42,16 @@
     calc_H();
     initialize_Q();
     initialize_R();
-    P = 1000000.0f * ( F * P * F.transpose() + Q );
     K.setZero();
     I.setIdentity();
     e.setZero();
+    P.setZero();
+    increase_diag_P();
+}
+
+void EKF_RPY::increase_diag_P()
+{
+    P = P + scale_P0 * I;
 }
 
 void EKF_RPY::set_m0(float mx0, float my0, float mz0)

--- a/EKF_RPY.h	Mon Oct 28 11:31:48 2019 +0000
+++ b/EKF_RPY.h	Mon Oct 28 12:08:15 2019 +0000
@@ -17,6 +17,7 @@
     
     void set_para();
     void reset();
+    void increase_diag_P();
     void set_m0(float mx0, float my0, float mz0);
     float get_est_state(uint8_t i);
     void update(float gyro_x, float gyro_y, float gyro_z, float accel_x, float accel_y, float magnet_x, float magnet_y);
@@ -32,6 +33,7 @@
     float s3;
     float c3;
     
+    float scale_P0;
     float g;
     float kv;
     float Ts;