[KalmanFilter]modify Q, add drift decay, and enable to set EKFilter p…

…arams by service

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;

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,14 +55,21 @@ 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,
              const Eigen::Vector4d& q,
              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<double> 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;
 };
 

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);