![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
Diff: EKF_RP.cpp
- Revision:
- 17:1d98928f7681
- Parent:
- 16:c39e084f05ed
- Child:
- 18:f374ca9a3e75
diff -r c39e084f05ed -r 1d98928f7681 EKF_RP.cpp --- a/EKF_RP.cpp Thu Oct 24 06:43:17 2019 +0000 +++ b/EKF_RP.cpp Fri Oct 25 08:44:32 2019 +0000 @@ -59,16 +59,19 @@ P = F * P * F.transpose() + Q; e = y - gy(); - // K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse(); - // x = x + K * e; - // P = (I - K * H) * P; + /* inversion faster 184 mus < 207 mus recursion */ + K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse(); + x = x + K * e; + P = (I - K * H) * P; /* only valid if R is diagonal (uncorrelated noise) */ + /* for(uint8_t i = 0; i < 2; 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()