Merge pull request #958 from eisoku9618/patch-2

[EKFilter.h] fix typo : fussy -> fuzzy
fkanehiro · Feb 29, 2016 · 2d5592c · 2d5592c
2 parents bccae27 + b4a7f2a
commit 2d5592c
Showing 1 changed file with 7 additions and 7 deletions.
diff --git a/rtc/KalmanFilter/EKFilter.h b/rtc/KalmanFilter/EKFilter.h
@@ -119,13 +119,13 @@ class EKFilter {
     P_a_priori = P_tmp;
   }
 
-  void correction(const Eigen::Vector3d& z, const Eigen::Matrix3d& fussyR) {
+  void correction(const Eigen::Vector3d& z, const Eigen::Matrix3d& fuzzyR) {
     Eigen::Vector4d q_a_priori = x_a_priori.head<4>();
     Eigen::Matrix<double, 3, 7> H;
     z_k = z;
     Eigen::Vector3d y = calcMeasurementResidual(z, q_a_priori);
     calcH(H, q_a_priori);
-    Eigen::Matrix3d S = H * P_a_priori * H.transpose() + fussyR;
+    Eigen::Matrix3d S = H * P_a_priori * H.transpose() + fuzzyR;
     Eigen::Matrix<double, 7, 3> K = P_a_priori * H.transpose() * S.inverse();
     hrp::Vector7 x_tmp = x_a_priori + K * y;
     x.head<4>() = x_tmp.head<4>().normalized(); /* quaternion */
@@ -144,7 +144,7 @@ class EKFilter {
   // Basically Equation (23), (24) and (25) in the paper [1]
   // [1] Chul Woo Kang and Chan Gook Park. Attitude estimation with accelerometers and gyros using fuzzy tuned Kalman filter.
   //     In European Control Conference, 2009.
-  void calcRWithFuzzyRule(Eigen::Matrix3d& fussyR, const hrp::Vector3& acc, const hrp::Vector3& gyro) const {
+  void calcRWithFuzzyRule(Eigen::Matrix3d& fuzzyR, const hrp::Vector3& acc, const hrp::Vector3& gyro) const {
     double alpha = std::min(std::fabs(acc.norm() - g_vec.norm()) / g_vec.norm(), 0.1);
     double beta = std::min(gyro.norm(), 0.05);
     double large_mu_acc = std::max(std::min((alpha - min_mag_thre_acc) / (max_mag_thre_acc - min_mag_thre_acc), 1.0), 0.0);
@@ -156,15 +156,15 @@ class EKFilter {
     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;
-    fussyR = R + k1 * z * z * Eigen::Matrix3d::Identity();
+    fuzzyR = R + k1 * z * z * Eigen::Matrix3d::Identity();
   };
 
   void main_one (hrp::Vector3& rpy, hrp::Vector3& rpyRaw, const hrp::Vector3& acc, const hrp::Vector3& gyro)
   {
-    Eigen::Matrix3d fussyR;
-    calcRWithFuzzyRule(fussyR, acc, gyro);
+    Eigen::Matrix3d fuzzyR;
+    calcRWithFuzzyRule(fuzzyR, acc, gyro);
     prediction(gyro);
-    correction(acc, fussyR);
+    correction(acc, fuzzyR);
     /* ekf_filter.printAll(); */
     Eigen::Quaternion<double> q(x[0], x[1], x[2], x[3]);
     rpy = hrp::rpyFromRot(q.toRotationMatrix());