AHRS
Dependencies: Eigen
Diff: EKF_RPY.cpp
- Revision:
- 22:495a419e474c
- Parent:
- 21:31e01d3e0143
- Child:
- 23:71996bfe68eb
diff -r 31e01d3e0143 -r 495a419e474c EKF_RPY.cpp --- a/EKF_RPY.cpp Mon Oct 21 17:14:27 2019 +0000 +++ b/EKF_RPY.cpp Mon Oct 28 07:52:43 2019 +0000 @@ -6,33 +6,30 @@ EKF_RPY::EKF_RPY(float Ts, float mx0, float my0, float mz0) { this->Ts = Ts; - /* [n_gyro; n_v; n_b_g] */ - var_fx << 0.01f, 0.01f, 0.01f, 0.0001f, 0.0001f, 0.0001f, 0.0001f, 0.0001f; - /* [n_acc; n_mag] */ - var_gy << 45.0f, 45.0f, 0.1f, 0.1f; - rho = 0.5f; - kv = 0.5f; /* k1/m */ - g = 9.81f; + set_para(); m0 << mx0, my0, mz0; reset(); } -EKF_RPY::EKF_RPY(float Ts) // Constructor without m0, set m0 later!!! + +EKF_RPY::EKF_RPY(float Ts) { this->Ts = Ts; + set_para(); + reset(); +} + +EKF_RPY::~EKF_RPY() {} + +void EKF_RPY::set_para() +{ /* [n_gyro; n_v; n_b_g] */ - var_fx << 0.01f, 0.01f, 0.01f, 0.0001f, 0.0001f, 0.0001f, 0.0001f, 0.0001f; + var_fx << 0.1f, 0.1f, 0.1f, 0.002f, 0.002f, 0.002f, 0.002f, 0.002f; /* [n_acc; n_mag] */ - var_gy << 45.0f, 45.0f, 0.1f, 0.1f; - rho = 0.5f; + var_gy << 40.0f, 40.0f, 1.0f, 1.0f; + rho = 1.0f; kv = 0.5f; /* k1/m */ g = 9.81f; - reset(); } -void EKF_RPY::set_m0(float mx0, float my0, float mz0){ - m0 << mx0, my0, mz0; - } - -EKF_RPY::~EKF_RPY() {} void EKF_RPY::reset() { @@ -47,6 +44,12 @@ P = Q; K.setZero(); I.setIdentity(); + e.setZero(); +} + +void EKF_RPY::set_m0(float mx0, float my0, float mz0) +{ + m0 << mx0, my0, mz0; } float EKF_RPY::get_est_state(uint8_t i) @@ -74,11 +77,21 @@ x = fxd(); P = F * P * F.transpose() + Q; + e = y - gy(); + /* inversion faster 356 mus < 432 mus recursion */ K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse(); + x = x + K * e; + P = (I - K * H) * P; - 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_RPY::update_angles()