RPYKalmanFilter.h
Go to the documentation of this file.
00001 #ifndef RPYKALMANFILTER_H
00002 #define RPYKALMANFILTER_H
00003 
00004 #include <hrpUtil/EigenTypes.h>
00005 #include <hrpUtil/Eigen3d.h>
00006 #include "hrpsys/util/Hrpsys.h"
00007 namespace hrp{
00008   typedef Eigen::Vector2d Vector2;
00009   typedef Eigen::Matrix2d Matrix22;
00010 }
00011 #include <iostream>
00012 
00013 class KFilter {
00014 public:
00015   KFilter() {
00016       F(0,0) = 1; F(0,1) =-0.005;
00017       F(1,0) = 0; F(1,1) =    1;
00018 
00019       P(0,0) = 0; P(0,1) =    0;
00020       P(1,0) = 0; P(1,1) =    0;
00021 
00022       Q(0,0) = 0.001 * 0.005; Q(0,1) = 0.000 * 0.005;
00023       Q(1,0) = 0.000 * 0.005; Q(1,1) = 0.003 * 0.005;
00024 
00025       R = 0.03;
00026 
00027       B(0) = 0.005; B(1) = 0;
00028 
00029       H(0,0) = 1; H(0,1) = 0;
00030 
00031       x(0) = 0; x(1) = 0;
00032       z = 0;
00033 
00034       I = hrp::dmatrix::Identity(2,2);
00035   }
00036   void setF(const double _f0, const double _f1, const double _f2, const double _f3) { F << _f0, _f1, _f2, _f3; };
00037   void setP(const double _p0, const double _p1, const double _p2, const double _p3) { P << _p0, _p1, _p2, _p3; };
00038   void setQ(const double _q0, const double _q1, const double _q2, const double _q3) { Q << _q0, _q1, _q2, _q3; };
00039   void setR(const double _R) { R = _R; }
00040   void setB(const double _b0, const double _b1) { B << _b0, _b1; };
00041   const hrp::Vector2 &getx() { return x; }
00042   void update(const double u, const double _z) {
00043       // Predicted (a priori) state estimate
00044       x = F * x + B * u;
00045       // Predicted (a priori) estimate covariance
00046       P = F * P * F.transpose() + Q;
00047       //
00048       // Innovation or measurement residual
00049       z = _z;
00050       double y = z - H * x;
00051       // Innovation (or residual) covariance
00052       double S = H * P * H.transpose() + R;
00053       // Optimal Kalman gain
00054       K = P * H.transpose() / S;
00055       // Updated (a posteriori) state estimate
00056       x = x + K * y;
00057       // Updated (a posteriori) estimate covariance
00058       P = (I - K * H) * P;
00059   }
00060   void resetStateByObservation() {
00061       x << z ,0;
00062   };
00063 private:
00064   hrp::Matrix22 P, Q, I, F;
00065   Eigen::Matrix<double, 1, 2> H;
00066   hrp::Vector2 B, K;
00067   hrp::Vector2 x;
00068   double R, z;
00069 };
00070 
00071 class RPYKalmanFilter {
00072 public:
00073     RPYKalmanFilter() : m_sensorR(hrp::Matrix33::Identity()) {};
00074     void main_one (hrp::Vector3& rpy, hrp::Vector3& rpyRaw, hrp::Vector3& baseRpyCurrent, const hrp::Vector3& acc, const hrp::Vector3& gyro, const double& sl_y, const hrp::Matrix33& BtoS)
00075     {
00076       //
00077       // G = [ cosb, sinb sina, sinb cosa,
00078       //          0,      cosa,     -sina,
00079       //      -sinb, cosb sina, cosb cosa]
00080       // s = [sx, sy, sz]t ( accelerometer )
00081       // g = [ 0 0 -g]t
00082       // s = G g = [g sinb, -g cosb sina, -g cosb cosa]t
00083       // hrp::RateGyroSensor* sensor = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
00084       double g = sqrt(acc(0) * acc(0) + acc(1) * acc(1) + acc(2) * acc(2));
00085       // atan2(y, x) = atan(y/x)
00086       double a, b;
00087       b = atan2( - acc(0) / g, sqrt( acc(1)/g * acc(1)/g + acc(2)/g * acc(2)/g ) );
00088       a = atan2( ( acc(1)/g ), ( acc(2)/g ) );
00089       rpyRaw = hrp::Vector3(a,b,sl_y);
00090       // #if 0
00091       //       // complementary filter
00092       //       m_rpy.data.r = 0.98 *(m_rpy.data.r+m_rate.data.avx*m_dt) + 0.02*m_rpyRaw.data.r;
00093       //       m_rpy.data.p = 0.98 *(m_rpy.data.p+m_rate.data.avy*m_dt) + 0.02*m_rpyRaw.data.p;
00094       //       m_rpy.data.y = 0.98 *(m_rpy.data.y+m_rate.data.avz*m_dt) + 0.02*m_rpyRaw.data.y;
00095       // #endif
00096       // kalman filter
00097       // x[0] = m_rpyRaw.data.r; // angle (from m/sec Acceleration3D, unstable, no drift )
00098       // x[1] = m_rate.data.avx; // rate ( rad/sec, AngularVelocity, gyro, stable/drift )
00099       // use kalman filter with imaginary data
00100       hrp::Vector3 gyro2 = gyro;
00101 #if 1
00102       double roll = r_filter.getx()[0];
00103       double pitch = p_filter.getx()[0];
00104       double yaw = y_filter.getx()[0];
00105       hrp::Matrix33 tmpMat;
00106       tmpMat << 1.0, sin(roll)*sin(pitch)/cos(pitch), cos(roll)*sin(pitch)/cos(pitch),
00107                 0,   cos(roll),                       -sin(roll),
00108                 0,   sin(roll)/cos(pitch),            cos(roll)/cos(pitch);
00109       gyro2 = tmpMat * gyro;
00110 #endif
00111       r_filter.update(gyro2(0), rpyRaw(0));
00112       p_filter.update(gyro2(1), rpyRaw(1));
00113       y_filter.update(gyro2(2), rpyRaw(2));
00114 
00115       hrp::Matrix33 imaginaryRotationMatrix = hrp::rotFromRpy(r_filter.getx()[0], p_filter.getx()[0], y_filter.getx()[0]); // the imaginary frame relative to a world reference frame
00116       hrp::Matrix33 realRotationMatrix = imaginaryRotationMatrix * m_sensorR; // the imu frame relative to a world reference frame
00117       hrp::Vector3 euler = hrp::rpyFromRot(realRotationMatrix);
00118       rpy(0) = euler(0);
00119       rpy(1) = euler(1);
00120       rpy(2) = euler(2);
00121       hrp::Matrix33 WtoB = realRotationMatrix * BtoS.transpose(); // the body frame relative to a world reference frame
00122       hrp::Vector3 euler2 = hrp::rpyFromRot(WtoB);
00123       baseRpyCurrent(0) = euler2(0);
00124       baseRpyCurrent(1) = euler2(1);
00125       baseRpyCurrent(2) = euler2(2);
00126     };
00127     void setParam (const double _dt, const double _Q_angle, const double _Q_rate, const double _R_angle, const std::string print_str = "")
00128     {
00129         Q_angle = _Q_angle;
00130         Q_rate = _Q_rate;
00131         R_angle = _R_angle;
00132         r_filter.setF(1, -_dt, 0, 1);
00133         r_filter.setP(0, 0, 0, 0);
00134         r_filter.setQ(Q_angle*_dt, 0, 0, Q_rate*_dt);
00135         r_filter.setR(R_angle);
00136         r_filter.setB(_dt, 0);
00137 
00138         p_filter.setF(1, -_dt, 0, 1);
00139         p_filter.setP(0, 0, 0, 0);
00140         p_filter.setQ(Q_angle*_dt, 0, 0, Q_rate*_dt);
00141         p_filter.setR(R_angle);
00142         p_filter.setB(_dt, 0);
00143 
00144         y_filter.setF(1, -_dt, 0, 1);
00145         y_filter.setP(0, 0, 0, 0);
00146         y_filter.setQ(Q_angle*_dt, 0, 0, Q_rate*_dt);
00147         y_filter.setR(R_angle);
00148         y_filter.setB(_dt, 0);
00149         std::cerr << "[" << print_str << "]   Q_angle=" << Q_angle << ", Q_rate=" << Q_rate << ", R_angle=" << R_angle << std::endl;
00150     };
00151     void resetKalmanFilterState()
00152     {
00153         r_filter.resetStateByObservation();
00154         p_filter.resetStateByObservation();
00155         y_filter.resetStateByObservation();
00156     };
00157     void setSensorR (const hrp::Matrix33& sr) { m_sensorR = sr;};
00158     double getQangle () const { return Q_angle;};
00159     double getQrate () const { return Q_rate;};
00160     double getRangle () const { return R_angle;};
00161 private:
00162     KFilter r_filter, p_filter, y_filter;
00163     double Q_angle, Q_rate, R_angle;
00164     hrp::Matrix33 m_sensorR;
00165 };
00166 
00167 #endif /* RPYKALMANFILTER_H */


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