![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
Diff: EKF_RP.cpp
- Revision:
- 13:2e03d9c03409
- Parent:
- 9:be2e1d2f683d
- Child:
- 15:53485bd1ff28
diff -r 180e09c4ea26 -r 2e03d9c03409 EKF_RP.cpp --- a/EKF_RP.cpp Tue Oct 22 19:37:32 2019 +0000 +++ b/EKF_RP.cpp Tue Oct 22 20:21:04 2019 +0000 @@ -7,10 +7,10 @@ { this->Ts = Ts; /* [n_gyro; n_b_g; n_v] */ - var_fx << 0.01f, 0.01f, 0.0001f, 0.0001f, 0.0001f, 0.0001f; + var_fx << 0.1f, 0.1f, 0.002f, 0.002f, 0.002f, 0.002f; /* [n_acc] */ - var_gy << 45.0f, 45.0f; - rho = 0.5f; + var_gy << 40.0f, 40.0f; + rho = 1.0f; kv = 0.5f; /* k1/m */ g = 9.81f; reset(); @@ -31,6 +31,7 @@ P = Q; K.setZero(); I.setIdentity(); + e.setZero(); } float EKF_RP::get_est_state(uint8_t i) @@ -56,11 +57,14 @@ x = fxd(); P = F * P * F.transpose() + Q; - - K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse(); + e = y - gy(); - x = x + K * (y - gy()); - P = (I - K * H) * P; + /* only valid if R is diagonal (uncorrelated noise) */ + for(uint8_t i = 0; i < 4; i++) { + K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) ); + x = x + K.col(i) * e(i); + P = (I - K.col(i)*H.row(i)) * P; + } } void EKF_RP::update_angles()