Diff: ekf.cpp

Revision:

4:3c21fb0c9e84

Child:

5:eee47600b772

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ekf.cpp	Fri May 03 13:46:40 2019 +0000
@@ -0,0 +1,104 @@
+#include "ekf.h"
+
+using namespace std;
+
+ekf::ekf(float Ts) : x(6,1,0.0),x_km(6,1,0.0),P(6,0.01),Q(6),R(2,0.025),F(6,6,0.0),H(2,6,0.0),K(6,2,0.0),E(6)
+{
+    
+    g = 9.81;
+    tau_g = 10;
+    itau_g = 1/tau_g;
+    k1 = 0.25;
+    m = 1.1;
+    kdm = k1/m;
+    this->Ts = Ts;
+    
+    F.put_entry(1,3,-1);
+    F.put_entry(2,4,-1);
+    F.put_entry(3,3,-1/tau_g);
+    F.put_entry(4,4,-1/tau_g);
+    F.put_entry(5,2,-g);
+    F.put_entry(6,1,g);
+    F.put_entry(5,5,-k1/m);
+    F.put_entry(6,6,-k1/m);
+    H.put_entry(1,5,-kdm);
+    H.put_entry(2,6,-kdm);
+    F.scale(Ts);
+    Q.mcopy(&(F/=F));   // Q=A*A'
+    Q.scale(.1);
+    F.mcopy(&(F+E));
+    }
+    
+    
+ekf::~ekf(void) {
+    printf("EKF is being deleted\r\n");
+    }
+
+void ekf::dgl(matrix *Z)
+{
+    
+    x_km.a[0][0]= x.a[0][0]+Ts*(Z->a[0][0]-x.a[2][0]+tan(x.a[1][0])*sin(x.a[0][0])*(Z->a[1][0]-x.a[3][0]));
+    x_km.a[1][0]= x.a[1][0]+Ts*(cos(x.a[0][0])*(Z->a[1][0]-x.a[3][0]));
+    x_km.a[2][0]= x.a[2][0]+Ts*(-itau_g*x.a[2][0]);
+    x_km.a[3][0]= x.a[3][0]+Ts*(-itau_g*x.a[3][0]);
+    x_km.a[4][0]= x.a[4][0]+Ts*(-g*sin(x.a[1][0])-kdm*x.a[4][0]);
+    x_km.a[5][0]= x.a[5][0]+Ts*(g*cos(x.a[1][0])*sin(x.a[0][0])-kdm*x.a[5][0]);
+    }
+
+void ekf::loop(matrix *Z)
+{
+    // F_km1 is constant
+
+    dgl(Z);                          // here x_km is calculated
+    P.mcopy(&(((F*P)/=F)+Q));                        //P_km = F_km1*P_km*F_km1'+Q_k
+
+    matrix dum1(2);
+    dum1.mcopy(&(((H*P)/=H)+R));
+    K.mcopy(&((P/=H)*dum1.inv_2x2()));                   // K_k=(P_km*H_k')*inv(H_k*P_km*H_k'+R_k); % Kalman-Gain
+                                        // x_k = x_km + K_k*(Z(r,[4 5])'-copter3D_calc_h_ekf(x_km,kin)); 
+    matrix dum2(2,1,0.0);
+    dum2.put_entry(1,1,Z->a[3][0]+kdm*x_km.a[4][0]);
+    dum2.put_entry(2,1,Z->a[4][0]+kdm*x_km.a[5][0]);     // calculates Z-h h is -k/m*...
+    x_km += (K*dum2);
+    x.mcopy(&x_km);
+                                        // P_km = (eye(6)-K_k*H_k)*P_km; % Kovar
+    P.mcopy(&((E-(K*H))*P));
+  
+    }
+float ekf::get_est_state(uint8_t i)
+{
+    return x_km.a[i-1][0];
+    }
+
+// 
+void ekf::display_matrix(char c)
+{
+    switch(c)  {
+        case 'H':
+            printf("H=\r\n");
+            H.printout();
+                break;
+        case 'P':
+            printf("P=\r\n");
+            P.printout();
+                break;
+        case 'Q':
+            printf("Q=\r\n");
+            Q.printout();
+                break;
+        case 'R':
+            printf("R=\r\n");
+            R.printout();
+                break;
+        case 'F':
+            printf("F=\r\n");
+            F.printout();
+                break;
+        case 'K':
+            printf("K=\r\n");
+            K.printout();
+                break;
+        default:
+                break;
+         }
+    }    
\ No newline at end of file