diff --git a/idl/KalmanFilterService.idl b/idl/KalmanFilterService.idl index 5cbf4944139..4c57269f262 100644 --- a/idl/KalmanFilterService.idl +++ b/idl/KalmanFilterService.idl @@ -27,6 +27,13 @@ module OpenHRP double Q_angle; double Q_rate; double R_angle; + // EKFilter + double EKF_Q_quot; + double EKF_Q_rate; + double EKF_Q_gyro; + double EKF_R_k1; + double EKF_R_k2; + double EKF_drift_T; // Common KFAlgorithm kf_algorithm; DblArray3 acc_offset; diff --git a/rtc/KalmanFilter/EKFilter.h b/rtc/KalmanFilter/EKFilter.h index a6b34c80c85..55828090536 100644 --- a/rtc/KalmanFilter/EKFilter.h +++ b/rtc/KalmanFilter/EKFilter.h @@ -15,12 +15,20 @@ class EKFilter { public: EKFilter() : P(hrp::Matrix77::Identity() * 0.1), - Q(Eigen::Matrix3d::Identity() * 0.001), + Qg(Eigen::Matrix3d::Identity() * 0.001), + Q(hrp::Matrix77::Identity() * 0.001 * 0.005), R(Eigen::Matrix3d::Identity() * 0.03), g_vec(Eigen::Vector3d(0.0, 0.0, 9.80665)), z_k(Eigen::Vector3d(0.0, 0.0, 9.80665)), min_mag_thre_acc(0.005), max_mag_thre_acc(0.05), - min_mag_thre_gyro(0.0075), max_mag_thre_gyro(0.035) + min_mag_thre_gyro(0.0075), max_mag_thre_gyro(0.035), + dt(0.005), + Q_quot(0.001), + Q_rate(0.001), + Q_gyro(0.001), + R_k1(400), + R_k2(0.03), + drift_T(3.0) { x << 1, 0, 0, 0, 0, 0, 0; } @@ -47,7 +55,11 @@ class EKFilter { calcOmega(omega, gyro_compensated); q_a_priori = q + dt / 2 * omega * q; _x_a_priori.head<4>() = q_a_priori.normalized(); - _x_a_priori.tail<3>() = drift; + if(drift_T!=0.0){ + _x_a_priori.tail<3>() = drift * (1.0-dt/drift_T); + }else{ + _x_a_priori.tail<3>() = drift; + } } void calcF(hrp::Matrix77& F, @@ -55,6 +67,9 @@ class EKFilter { const Eigen::Vector3d& gyro, const Eigen::Vector3d& drift) const { F = hrp::Matrix77::Identity(); + if(drift_T!=0.0){ + for(int i=4;i<7;i++) F(i,i)=1.0-dt/drift_T; + } Eigen::Vector3d gyro_compensated = gyro - drift; Eigen::Matrix4d omega; calcOmega(omega, gyro_compensated); @@ -78,9 +93,9 @@ class EKFilter { + q[3], + q[0], - q[1], - q[2], + q[1], + q[0]; V_upper *= dt / 2; - hrp::Matrix77 VQVt = hrp::Matrix77::Zero(); - VQVt.block<4, 4>(0, 0) = V_upper * Q * V_upper.transpose(); - _P_a_priori = F * P * F.transpose() + VQVt; + hrp::Matrix77 VQgVt = hrp::Matrix77::Zero(); + VQgVt.block<4, 4>(0, 0) = V_upper * Qg * V_upper.transpose(); + _P_a_priori = F * P * F.transpose() + VQgVt + Q; } Eigen::Vector3d calcAcc(const Eigen::Vector4d& q) const { @@ -155,8 +170,7 @@ class EKFilter { w3 = large_mu_acc * (1.0 - large_mu_gyro); w4 = large_mu_acc * large_mu_gyro; double z = (w1 * 0.0 + w2 * (3.5 * alpha + 8.0 * beta + 0.5) + w3 * (3.5 * alpha + 8.0 * beta + 0.5) + w4 * 1.0) / (w1 + w2 + w3 + w4); - double k1 = 400; - fuzzyR = R + k1 * z * z * Eigen::Matrix3d::Identity(); + fuzzyR = R + R_k1 * z * z * Eigen::Matrix3d::Identity(); }; void main_one (hrp::Vector3& rpy, hrp::Vector3& rpyRaw, const hrp::Vector3& acc, const hrp::Vector3& gyro) @@ -171,17 +185,45 @@ class EKFilter { }; void setdt (const double _dt) { dt = _dt;}; + + void setParam (const double _dt, const double _Q_quot, const double _Q_rate, const double _Q_gyro, const double _R_k1, const double _R_k2, const double _drift_T, const std::string print_str = "") + { + setdt(_dt); + Q_quot = _Q_quot; + Q_rate = _Q_rate; + Q_gyro = _Q_gyro; + R_k1 = _R_k1; + R_k2 = _R_k2; + drift_T = _drift_T; + + for(int i=0;i<3;i++) Qg(i,i)=Q_gyro; + for(int i=0;i<4;i++) Q(i,i)=Q_quot*dt; + for(int i=4;i<7;i++) Q(i,i)=Q_rate*dt; + for(int i=0;i<3;i++) R(i,i)=R_k2; + + std::cerr << "[" << print_str << "] Q_quot=" << Q_quot << ", Q_rate=" << Q_rate << ", Q_gyro= " << Q_gyro << ", R_k1=" << R_k1 << ", R_k2=" << R_k2 << ", drift_T=" << drift_T << std::endl; + }; + void resetKalmanFilterState() { Eigen::Quaternion tmp_q; tmp_q.setFromTwoVectors(z_k, g_vec); x << tmp_q.w(), tmp_q.x(), tmp_q.y(), tmp_q.z(), 0, 0, 0; }; + double getQquot () const {return Q_quot;}; + double getQrate () const {return Q_rate;}; + double getQgyro () const {return Q_gyro;}; + double getR_k1 () const {return R_k1;}; + double getR_k2 () const {return R_k2;}; + double getdrift_T () const {return drift_T;}; + private: hrp::Vector7 x, x_a_priori; hrp::Matrix77 P, P_a_priori; - Eigen::Matrix3d Q, R; + Eigen::Matrix3d Qg, R; + hrp::Matrix77 Q; + double Q_quot, Q_rate, Q_gyro ,R_k1, R_k2; Eigen::Vector3d g_vec, z_k; - double dt; + double dt, drift_T; double min_mag_thre_acc, max_mag_thre_acc, min_mag_thre_gyro, max_mag_thre_gyro; }; diff --git a/rtc/KalmanFilter/KalmanFilter.cpp b/rtc/KalmanFilter/KalmanFilter.cpp index 873fd47b35d..61124399631 100644 --- a/rtc/KalmanFilter/KalmanFilter.cpp +++ b/rtc/KalmanFilter/KalmanFilter.cpp @@ -132,7 +132,7 @@ RTC::ReturnCode_t KalmanFilter::onInitialize() } rpy_kf.setParam(m_dt, 0.001, 0.003, 1000, std::string(m_profile.instance_name)); rpy_kf.setSensorR(m_sensorR); - ekf_filter.setdt(m_dt); + ekf_filter.setParam(m_dt, 0.001, 0.003, 0.001, 400, 0.03, 3.0, std::string(m_profile.instance_name)); kf_algorithm = OpenHRP::KalmanFilterService::RPYKalmanFilter; m_qCurrent.data.length(m_robot->numJoints()); acc_offset = hrp::Vector3::Zero(); @@ -292,6 +292,7 @@ bool KalmanFilter::setKalmanFilterParam(const OpenHRP::KalmanFilterService::Kalm { std::cerr << "[" << m_profile.instance_name << "] setKalmanFilterParam" << std::endl; rpy_kf.setParam(m_dt, i_param.Q_angle, i_param.Q_rate, i_param.R_angle, std::string(m_profile.instance_name)); + ekf_filter.setParam(m_dt, i_param.EKF_Q_quot, i_param.EKF_Q_rate, i_param.EKF_Q_gyro, i_param.EKF_R_k1, i_param.EKF_R_k2, i_param.EKF_drift_T, std::string(m_profile.instance_name)); kf_algorithm = i_param.kf_algorithm; for (size_t i = 0; i < 3; i++) { acc_offset(i) = i_param.acc_offset[i]; @@ -318,6 +319,13 @@ bool KalmanFilter::getKalmanFilterParam(OpenHRP::KalmanFilterService::KalmanFilt i_param.Q_angle = rpy_kf.getQangle(); i_param.Q_rate = rpy_kf.getQrate(); i_param.R_angle = rpy_kf.getRangle(); + i_param.EKF_Q_quot = ekf_filter.getQquot(); + i_param.EKF_Q_rate = ekf_filter.getQrate(); + i_param.EKF_Q_gyro = ekf_filter.getQgyro(); + i_param.EKF_R_k1 = ekf_filter.getR_k1(); + i_param.EKF_R_k2 = ekf_filter.getR_k2(); + i_param.EKF_drift_T = ekf_filter.getdrift_T(); + i_param.kf_algorithm = kf_algorithm; for (size_t i = 0; i < 3; i++) { i_param.acc_offset[i] = acc_offset(i);