EKFilter.h
Go to the documentation of this file.
00001 // -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; -*-
00002 #ifndef EKFILTER_H
00003 #define EKFILTER_H
00004 
00005 #include <hrpUtil/EigenTypes.h>
00006 #include <iostream>
00007 #include "hrpsys/util/Hrpsys.h"
00008 
00009 namespace hrp{
00010   typedef Eigen::Matrix<double, 7, 7> Matrix77;
00011   typedef Eigen::Matrix<double, 7, 1> Vector7;
00012 };
00013 
00014 class EKFilter {
00015 public:
00016   EKFilter()
00017     : P(hrp::Matrix77::Identity() * 0.1),
00018       Q(Eigen::Matrix3d::Identity() * 0.001),
00019       R(Eigen::Matrix3d::Identity() * 0.03),
00020       g_vec(Eigen::Vector3d(0.0, 0.0, 9.80665)),
00021       z_k(Eigen::Vector3d(0.0, 0.0, 9.80665)),
00022       min_mag_thre_acc(0.005), max_mag_thre_acc(0.05),
00023       min_mag_thre_gyro(0.0075), max_mag_thre_gyro(0.035)
00024   {
00025     x << 1, 0, 0, 0, 0, 0, 0;
00026   }
00027 
00028   hrp::Vector7 getx() const { return x; }
00029 
00030   void calcOmega(Eigen::Matrix4d& omega, const Eigen::Vector3d& w) const {
00031     /* \dot{q} = \frac{1}{2} omega q */
00032     omega <<
00033       0, -w[0], -w[1], -w[2],
00034       w[0],     0,  w[2], -w[1],
00035       w[1], -w[2],     0,  w[0],
00036       w[2],  w[1], -w[0],     0;
00037   }
00038 
00039   void calcPredictedState(hrp::Vector7& _x_a_priori,
00040                           const Eigen::Vector4d& q,
00041                           const Eigen::Vector3d& gyro,
00042                           const Eigen::Vector3d& drift) const {
00043     /* x_a_priori = f(x, u) */
00044     Eigen::Vector4d q_a_priori;
00045     Eigen::Vector3d gyro_compensated = gyro - drift;
00046     Eigen::Matrix4d omega;
00047     calcOmega(omega, gyro_compensated);
00048     q_a_priori = q + dt / 2 * omega * q;
00049     _x_a_priori.head<4>() = q_a_priori.normalized();
00050     _x_a_priori.tail<3>() = drift;
00051   }
00052 
00053   void calcF(hrp::Matrix77& F,
00054              const Eigen::Vector4d& q,
00055              const Eigen::Vector3d& gyro,
00056              const Eigen::Vector3d& drift) const {
00057     F = hrp::Matrix77::Identity();
00058     Eigen::Vector3d gyro_compensated = gyro - drift;
00059     Eigen::Matrix4d omega;
00060     calcOmega(omega, gyro_compensated);
00061     F.block<4, 4>(0, 0) += dt / 2 * omega;
00062     F.block<4, 3>(0, 4) <<
00063       + q[1], + q[2], + q[3],
00064       - q[0], + q[3], - q[2],
00065       - q[3], - q[0], + q[1],
00066       + q[2], - q[1], - q[0];
00067     F.block<4, 3>(0, 4) *= dt / 2;
00068   }
00069 
00070   void calcPredictedCovariance(hrp::Matrix77& _P_a_priori,
00071                                const hrp::Matrix77& F,
00072                                const Eigen::Vector4d& q) const {
00073     /* P_a_priori = F P F^T + V Q V^T */
00074     Eigen::Matrix<double, 4, 3> V_upper;
00075     V_upper <<
00076       - q[1], - q[2], - q[3],
00077       + q[0], - q[3], + q[2],
00078       + q[3], + q[0], - q[1],
00079       - q[2], + q[1], + q[0];
00080     V_upper *= dt / 2;
00081     hrp::Matrix77 VQVt = hrp::Matrix77::Zero();
00082     VQVt.block<4, 4>(0, 0) = V_upper * Q * V_upper.transpose();
00083     _P_a_priori = F * P * F.transpose() + VQVt;
00084   }
00085 
00086   Eigen::Vector3d calcAcc(const Eigen::Vector4d& q) const {
00087     Eigen::Quaternion<double> q_tmp(q[0], q[1], q[2], q[3]);
00088     Eigen::Vector3d acc = q_tmp.conjugate()._transformVector(g_vec);
00089     return acc;
00090   }
00091 
00092   void calcH(Eigen::Matrix<double, 3, 7>& H, const Eigen::Vector4d& q) const {
00093     double w = q[0], x = q[1], y = q[2], z = q[3];
00094     H <<
00095       -y, +z, -w, +x, 0, 0, 0,
00096       +x, +w, +z, +y, 0, 0, 0,
00097       +w, -x, -y, +z, 0, 0, 0;
00098     H *= 2 * g_vec[2];
00099   }
00100 
00101   Eigen::Vector3d calcMeasurementResidual(const Eigen::Vector3d& acc_measured,
00102                                           const Eigen::Vector4d& q) const {
00103     /* y = z - h(x) */
00104     Eigen::Vector3d y = acc_measured - calcAcc(q);
00105     return y;
00106   }
00107 
00108 
00109   void prediction(const Eigen::Vector3d& u) {
00110     Eigen::Vector4d q = x.head<4>();
00111     Eigen::Vector3d drift = x.tail<3>();
00112     hrp::Matrix77 F;
00113     calcF(F, q, u, drift);
00114     hrp::Vector7 x_tmp;
00115     calcPredictedState(x_tmp, q, u, drift);
00116     x_a_priori = x_tmp;
00117     hrp::Matrix77 P_tmp;
00118     calcPredictedCovariance(P_tmp, F, q);
00119     P_a_priori = P_tmp;
00120   }
00121 
00122   void correction(const Eigen::Vector3d& z, const Eigen::Matrix3d& fuzzyR) {
00123     Eigen::Vector4d q_a_priori = x_a_priori.head<4>();
00124     Eigen::Matrix<double, 3, 7> H;
00125     z_k = z;
00126     Eigen::Vector3d y = calcMeasurementResidual(z, q_a_priori);
00127     calcH(H, q_a_priori);
00128     Eigen::Matrix3d S = H * P_a_priori * H.transpose() + fuzzyR;
00129     Eigen::Matrix<double, 7, 3> K = P_a_priori * H.transpose() * S.inverse();
00130     hrp::Vector7 x_tmp = x_a_priori + K * y;
00131     x.head<4>() = x_tmp.head<4>().normalized(); /* quaternion */
00132     x.tail<3>() = x_tmp.tail<3>(); /* bias */
00133     P = (hrp::Matrix77::Identity() - K * H) * P_a_priori;
00134   }
00135 
00136   void printAll() const {
00137     std::cerr << "x" << std::endl << x << std::endl;
00138     std::cerr << "x_a_priori" << std::endl << x_a_priori << std::endl;
00139     std::cerr << "P" << std::endl << P << std::endl << std::endl;
00140     std::cerr << "P_a_priori" << std::endl << P_a_priori << std::endl << std::endl;
00141   }
00142 
00143 
00144   // Basically Equation (23), (24) and (25) in the paper [1]
00145   // [1] Chul Woo Kang and Chan Gook Park. Attitude estimation with accelerometers and gyros using fuzzy tuned Kalman filter.
00146   //     In European Control Conference, 2009.
00147   void calcRWithFuzzyRule(Eigen::Matrix3d& fuzzyR, const hrp::Vector3& acc, const hrp::Vector3& gyro) const {
00148     double alpha = std::min(std::fabs(acc.norm() - g_vec.norm()) / g_vec.norm(), 0.1);
00149     double beta = std::min(gyro.norm(), 0.05);
00150     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);
00151     double large_mu_gyro = std::max(std::min((beta - min_mag_thre_gyro) / (max_mag_thre_gyro - min_mag_thre_gyro), 1.0), 0.0);
00152     double w1, w2, w3, w4;
00153     w1 = (1.0 - large_mu_acc) * (1.0 - large_mu_gyro);
00154     w2 = (1.0 - large_mu_acc) * large_mu_gyro;
00155     w3 = large_mu_acc * (1.0 - large_mu_gyro);
00156     w4 = large_mu_acc * large_mu_gyro;
00157     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);
00158     double k1 = 400;
00159     fuzzyR = R + k1 * z * z * Eigen::Matrix3d::Identity();
00160   };
00161 
00162   void main_one (hrp::Vector3& rpy, hrp::Vector3& rpyRaw, const hrp::Vector3& acc, const hrp::Vector3& gyro)
00163   {
00164     Eigen::Matrix3d fuzzyR;
00165     calcRWithFuzzyRule(fuzzyR, acc, gyro);
00166     prediction(gyro);
00167     correction(acc, fuzzyR);
00168     /* ekf_filter.printAll(); */
00169     Eigen::Quaternion<double> q(x[0], x[1], x[2], x[3]);
00170     rpy = hrp::rpyFromRot(q.toRotationMatrix());
00171   };
00172 
00173   void setdt (const double _dt) { dt = _dt;};
00174   void resetKalmanFilterState() {
00175     Eigen::Quaternion<double> tmp_q;
00176     tmp_q.setFromTwoVectors(z_k, g_vec);
00177     x << tmp_q.w(), tmp_q.x(), tmp_q.y(), tmp_q.z(), 0, 0, 0;
00178   };
00179 private:
00180   hrp::Vector7 x, x_a_priori;
00181   hrp::Matrix77 P, P_a_priori;
00182   Eigen::Matrix3d Q, R;
00183   Eigen::Vector3d g_vec, z_k;
00184   double dt;
00185   double min_mag_thre_acc, max_mag_thre_acc, min_mag_thre_gyro, max_mag_thre_gyro;
00186 };
00187 
00188 #endif /* EKFILTER_H */


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:54