diff --git a/rtc/KalmanFilter/EKFilter.h b/rtc/KalmanFilter/EKFilter.h index a0895873de7..9fb882d6473 100644 --- 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 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 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 q(x[0], x[1], x[2], x[3]); rpy = hrp::rpyFromRot(q.toRotationMatrix());