Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[EKFilter.h] fix typo : fussy -> fuzzy #958

Merged
merged 1 commit into from
Feb 29, 2016
Merged

[EKFilter.h] fix typo : fussy -> fuzzy #958

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 7 additions & 7 deletions rtc/KalmanFilter/EKFilter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand All @@ -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);
Expand All @@ -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());
Expand Down