altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

EKF.cpp@19:42ea6dd68185, 2019-10-02 (annotated)

Committer:
altb2
Date:
Wed Oct 02 15:30:15 2019 +0000
Revision:
19:42ea6dd68185
Child:
20:1182bc29c195
major changes for QK3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 19:42ea6dd68185 1 #include "EKF.h"
altb2 19:42ea6dd68185 2
altb2 19:42ea6dd68185 3 using namespace std;
altb2 19:42ea6dd68185 4
altb2 19:42ea6dd68185 5 EKF::EKF(float Ts) : x(6,1,0.0),x_km(6,1,0.0),P(6,0.001),Q(6),R(2,0.00045),F(6,6,0.0),H(2,6,0.0),K(6,2,0.0),E(6)
altb2 19:42ea6dd68185 6 {
altb2 19:42ea6dd68185 7
altb2 19:42ea6dd68185 8 g = -9.81; // -1 wege Drehung des z-Vektors (z nach oben)
altb2 19:42ea6dd68185 9 tau_g = 2.0;
altb2 19:42ea6dd68185 10 itau_g = 1.0f/tau_g;
altb2 19:42ea6dd68185 11 k1 = 0.55;
altb2 19:42ea6dd68185 12 m = 1.1;
altb2 19:42ea6dd68185 13 kdm = k1/m;
altb2 19:42ea6dd68185 14 this->Ts = Ts;
altb2 19:42ea6dd68185 15
altb2 19:42ea6dd68185 16 F.put_entry(0,2,-1);
altb2 19:42ea6dd68185 17 F.put_entry(1,3,-1);
altb2 19:42ea6dd68185 18 F.put_entry(2,2,-1/tau_g);
altb2 19:42ea6dd68185 19 F.put_entry(3,3,-1/tau_g);
altb2 19:42ea6dd68185 20 F.put_entry(4,1,-g);
altb2 19:42ea6dd68185 21 F.put_entry(5,0,g);
altb2 19:42ea6dd68185 22 F.put_entry(4,4,-k1/m);
altb2 19:42ea6dd68185 23 F.put_entry(5,5,-k1/m); // F is continuous Linearized!!
altb2 19:42ea6dd68185 24 H.put_entry(0,4,-kdm);
altb2 19:42ea6dd68185 25 H.put_entry(1,5,-kdm);
altb2 19:42ea6dd68185 26 F.scale(Ts);
altb2 19:42ea6dd68185 27 F.mcopy(&(F+E));
altb2 19:42ea6dd68185 28 Q.mcopy(&(F/=F)); // Q=A*A'
altb2 19:42ea6dd68185 29 Q.scale(1e-11);
altb2 19:42ea6dd68185 30 //H.printout();
altb2 19:42ea6dd68185 31 //P.printout();
altb2 19:42ea6dd68185 32 }
altb2 19:42ea6dd68185 33
altb2 19:42ea6dd68185 34
altb2 19:42ea6dd68185 35 EKF::~EKF(void) {
altb2 19:42ea6dd68185 36 printf("EKF is being deleted\r\n");
altb2 19:42ea6dd68185 37 }
altb2 19:42ea6dd68185 38
altb2 19:42ea6dd68185 39 void EKF::dgl(matrix *Z)
altb2 19:42ea6dd68185 40 {
altb2 19:42ea6dd68185 41
altb2 19:42ea6dd68185 42 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]));
altb2 19:42ea6dd68185 43 x_km.a[1][0]= x.a[1][0]+Ts*(cos(x.a[0][0])*(Z->a[1][0]-x.a[3][0]));
altb2 19:42ea6dd68185 44 x_km.a[2][0]= x.a[2][0]+Ts*(-itau_g*x.a[2][0]);
altb2 19:42ea6dd68185 45 x_km.a[3][0]= x.a[3][0]+Ts*(-itau_g*x.a[3][0]);
altb2 19:42ea6dd68185 46 x_km.a[4][0]= x.a[4][0]+Ts*(-g*sin(x.a[1][0])-kdm*x.a[4][0]);
altb2 19:42ea6dd68185 47 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]);
altb2 19:42ea6dd68185 48 }
altb2 19:42ea6dd68185 49 // Just map gx ... to measurement matrix (to be consistent with Mahony etc.
altb2 19:42ea6dd68185 50 void EKF::update(float gx,float gy,float gz,float ax,float ax,float az){
altb2 19:42ea6dd68185 51 matrix measurement(6, 1, 0.0f);
altb2 19:42ea6dd68185 52 measurement.put_entry(0, 0, gx);
altb2 19:42ea6dd68185 53 measurement.put_entry(1, 0, gy);
altb2 19:42ea6dd68185 54 measurement.put_entry(3, 0, ax);
altb2 19:42ea6dd68185 55 measurement.put_entry(4, 0, ay);
altb2 19:42ea6dd68185 56 loop(&measurement);
altb2 19:42ea6dd68185 57 }
altb2 19:42ea6dd68185 58 //
altb2 19:42ea6dd68185 59 void EKF::loop(matrix *Z)
altb2 19:42ea6dd68185 60 {
altb2 19:42ea6dd68185 61 // F_km1 is constant
altb2 19:42ea6dd68185 62
altb2 19:42ea6dd68185 63 dgl(Z); // here x_km is calculated
altb2 19:42ea6dd68185 64 P.mcopy(&(((F*P)/=F)+Q)); //P_km = F_km1*P_km*F_km1'+Q_k
altb2 19:42ea6dd68185 65
altb2 19:42ea6dd68185 66 matrix dum1(2);
altb2 19:42ea6dd68185 67 dum1.mcopy(&(((H*P)/=H)+R));
altb2 19:42ea6dd68185 68 K.mcopy(&((P/=H)*dum1.inv_2x2())); // K_k=(P_km*H_k')*inv(H_k*P_km*H_k'+R_k); % Kalman-Gain
altb2 19:42ea6dd68185 69 // x_k = x_km + K_k*(Z(r,[4 5])'-copter3D_calc_h_ekf(x_km,kin));
altb2 19:42ea6dd68185 70 matrix dum2(2,1,0.0);
altb2 19:42ea6dd68185 71 dum2.put_entry(0,0,Z->a[3][0]+kdm*x_km.a[4][0]);
altb2 19:42ea6dd68185 72 dum2.put_entry(1,0,Z->a[4][0]+kdm*x_km.a[5][0]); // calculates Z-h h is -k/m*...
altb2 19:42ea6dd68185 73 x_km += (K*dum2);
altb2 19:42ea6dd68185 74 x.mcopy(&x_km);
altb2 19:42ea6dd68185 75 // P_km = (eye(6)-K_k*H_k)*P_km; % Kovar
altb2 19:42ea6dd68185 76 P.mcopy(&((E-(K*H))*P));
altb2 19:42ea6dd68185 77
altb2 19:42ea6dd68185 78 }
altb2 19:42ea6dd68185 79 float EKF::get_est_state(uint8_t i)
altb2 19:42ea6dd68185 80 {
altb2 19:42ea6dd68185 81 return x_km.a[i][0];
altb2 19:42ea6dd68185 82 }