AHRS
Dependencies: Eigen
EKF.cpp@20:1182bc29c195, 2019-10-09 (annotated)
- Committer:
- altb2
- Date:
- Wed Oct 09 13:45:36 2019 +0000
- Revision:
- 20:1182bc29c195
- Parent:
- 19:42ea6dd68185
Major changes for new structure of IndNav_QK3
Who changed what in which revision?
User | Revision | Line number | New 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 | 20:1182bc29c195 | 42 | x_km.a[0][0]= x.a[0][0]+Ts*(Z->a[0][0]-x.a[2][0]+tan(x.a[1][0])*my_sin(x.a[0][0])*(Z->a[1][0]-x.a[3][0])); |
altb2 | 20:1182bc29c195 | 43 | x_km.a[1][0]= x.a[1][0]+Ts*(my_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 | 20:1182bc29c195 | 46 | x_km.a[4][0]= x.a[4][0]+Ts*(-g*my_sin(x.a[1][0])-kdm*x.a[4][0]); |
altb2 | 20:1182bc29c195 | 47 | x_km.a[5][0]= x.a[5][0]+Ts*(g*my_cos(x.a[1][0])*my_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 | 20:1182bc29c195 | 50 | void EKF::update(float gx,float gy,float gz,float ax,float ay,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 | } |