altb_pmic
/
Test_ekf
Testing ekf implementation for Quadro_1.
Diff: EKF_RPY.cpp
- Revision:
- 15:53485bd1ff28
- Parent:
- 13:2e03d9c03409
- Child:
- 16:c39e084f05ed
diff -r c4bc8ba29c2f -r 53485bd1ff28 EKF_RPY.cpp --- a/EKF_RPY.cpp Wed Oct 23 09:59:59 2019 +0000 +++ b/EKF_RPY.cpp Thu Oct 24 06:32:00 2019 +0000 @@ -79,12 +79,16 @@ 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; + /* 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; - } + // 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_RPY::update_angles()