diff --git a/rtc/KalmanFilter/KalmanFilter.cpp b/rtc/KalmanFilter/KalmanFilter.cpp index faa6895acdc..3b6d644d186 100644 --- a/rtc/KalmanFilter/KalmanFilter.cpp +++ b/rtc/KalmanFilter/KalmanFilter.cpp @@ -15,6 +15,8 @@ #include #include +//#define USE_EKF + // Module specification // static const char* kalmanfilter_spec[] = @@ -116,6 +118,24 @@ RTC::ReturnCode_t KalmanFilter::onInitialize() << std::endl; } + r_filter.setF(1, -m_dt, 0, 1); + r_filter.setP(0, 0, 0, 0); + r_filter.setQ(0.001*m_dt, 0, 0, 0.003*m_dt); + r_filter.setR(0.03); + r_filter.setB(m_dt, 0); + + p_filter.setF(1, -m_dt, 0, 1); + p_filter.setP(0, 0, 0, 0); + p_filter.setQ(0.001*m_dt, 0, 0, 0.003*m_dt); + p_filter.setR(0.03); + p_filter.setB(m_dt, 0); + + y_filter.setF(1, -m_dt, 0, 1); + y_filter.setP(0, 0, 0, 0); + y_filter.setQ(0.001*m_dt, 0, 0, 0.003*m_dt); + y_filter.setR(0.03); + y_filter.setB(m_dt, 0); + m_rpy.data.r = 0; m_rpy.data.p = 0; m_rpy.data.y = 0; @@ -189,6 +209,7 @@ RTC::ReturnCode_t KalmanFilter::onExecute(RTC::UniqueId ec_id) if (m_accIn.isNew()){ m_accIn.read(); +#ifdef USE_EKF Eigen::Vector3d acc = m_sensorR * hrp::Vector3(m_acc.data.ax, m_acc.data.ay, m_acc.data.az); // transform to imaginary acc data Eigen::Vector3d gyro = m_sensorR * hrp::Vector3(m_rate.data.avx, m_rate.data.avy, m_rate.data.avz); // transform to imaginary rate data @@ -207,6 +228,92 @@ RTC::ReturnCode_t KalmanFilter::onExecute(RTC::UniqueId ec_id) m_rpy.data.y = eulerZYX(0); m_rpy.data.p = eulerZYX(1); m_rpy.data.r = eulerZYX(2); +#else // USE_EKF + //std::cerr << "rate : " << m_rate.data.avx << " " << m_rate.data.avy << " " << m_rate.data.avz << std::endl; // rad/sec (AngularVelocity3D) / gyro / stable, drift + //std::cerr << "acc : " << m_acc.data.ax << " " << m_acc.data.ay << " " << m_acc.data.az << std::endl; // m/sec (Acceleration3D) / accelerometer / unstable , no drift + // + // G = [ cosb, sinb sina, sinb cosa, + // 0, cosa, -sina, + // -sinb, cosb sina, cosb cosa] + // s = [sx, sy, sz]t ( accelerometer ) + // g = [ 0 0 -g]t + // s = G g = [g sinb, -g cosb sina, -g cosb cosa]t + double sx = m_acc.data.ax - sx_ref, sy = m_acc.data.ay - sy_ref, sz = m_acc.data.az - sz_ref; + + // hrp::RateGyroSensor* sensor = m_robot->sensor("gyrometer"); + hrp::Vector3 s = m_sensorR * hrp::Vector3(sx, sy, sz); + sx = s(0); sy = s(1); sz = s(2); // transform to imaginary acc data + double g = sqrt(sx * sx + sy * sy + sz * sz); + // atan2(y, x) = atan(y/x) + double a, b; +#if 0 + x/g = sinb + -y/g = cosb sina + -z/g = cosb cosa + y/g^2 = cosb^2 sina^2 + z/g^2 = cosb^2 cosa^2 + y/g^2 + z/g^2 = cosb^2 + y/g^2 = (1 - sinb^2) sina^2 + z/g^2 = (1 - sinb^2) cosa^2 + y/g^2 = (1 - x/g^2) sina^2 + z/g^2 = (1 - x/g^2) cosa^2 +#endif + b = atan2( - sx / g, sqrt( sy/g * sy/g + sz/g * sz/g ) ); + a = atan2( ( sy/g ), ( sz/g ) ); + //std::cerr << "a(roll) = " << a*180/M_PI << ", b(pitch) = " << b*180/M_PI << ", sx = " << sx << ", sy = " << sy << ", sz = " << sz << std::endl; + m_rpyRaw.data.r = a; + m_rpyRaw.data.p = b; + m_rpyRaw.data.y = 0; +#if 0 + m_rpy.data.r = m_rpyRaw.data.r; + m_rpy.data.p = m_rpyRaw.data.p; + m_rpy.data.y = m_rpyRaw.data.y; +#endif +#if 0 + // complementary filter + m_rpy.data.r = 0.98 *(m_rpy.data.r+m_rate.data.avx*m_dt) + 0.02*m_rpyRaw.data.r; + m_rpy.data.p = 0.98 *(m_rpy.data.p+m_rate.data.avy*m_dt) + 0.02*m_rpyRaw.data.p; + m_rpy.data.y = 0.98 *(m_rpy.data.y+m_rate.data.avz*m_dt) + 0.02*m_rpyRaw.data.y; +#endif + // kalman filter + // x[0] = m_rpyRaw.data.r; // angle (from m/sec Acceleration3D, unstable, no drift ) + // x[1] = m_rate.data.avx; // rate ( rad/sec, AngularVelocity, gyro, stable/drift ) + hrp::Vector3 av = m_sensorR * hrp::Vector3(m_rate.data.avx, m_rate.data.avy, m_rate.data.avz); + m_rate.data.avx = av(0); m_rate.data.avy = av(1); m_rate.data.avz = av(2); // transform to imaginary rate data + // use kalman filter with imaginary data + r_filter.update(m_rate.data.avx, m_rpyRaw.data.r); + p_filter.update(m_rate.data.avy, m_rpyRaw.data.p); + y_filter.update(m_rate.data.avz, m_rpyRaw.data.y); + + Eigen::AngleAxis aaZ(y_filter.getx()[0], Eigen::Vector3d::UnitZ()); + Eigen::AngleAxis aaY(p_filter.getx()[0], Eigen::Vector3d::UnitY()); + Eigen::AngleAxis aaX(r_filter.getx()[0], Eigen::Vector3d::UnitX()); + Eigen::Quaternion q = aaZ * aaY * aaX; + hrp::Matrix33 imaginaryRotationMatrix = q.toRotationMatrix(); + hrp::Matrix33 realRotationMatrix = imaginaryRotationMatrix * m_sensorR; // inverse transform to real data + hrp::Vector3 euler = realRotationMatrix.eulerAngles(2,1,0); + m_rpy.data.y = euler(0); + m_rpy.data.p = euler(1); + m_rpy.data.r = euler(2); +#if 0 + std::cout << m_acc.tm.sec + m_acc.tm.nsec/1000000000.0 << " " + << m_rpyRaw.data.r << " " + << m_rpyRaw.data.p << " " + << m_rpyRaw.data.y << " " + << m_rpy.data.r << " " + << m_rpy.data.p << " " + << m_rpy.data.y << " " + << (m_rpyRaw.data.r - m_rpyRaw_prev.data.r) << " " + << (m_rpyRaw.data.p - m_rpyRaw_prev.data.p) << " " + << (m_rpyRaw.data.y - m_rpyRaw_prev.data.y) << " " + << (m_rpy.data.r - m_rpy_prev.data.r) << " " + << (m_rpy.data.p - m_rpy_prev.data.p) << " " + << (m_rpy.data.y - m_rpy_prev.data.y) + << std::endl; + m_rpy_prev = m_rpy; + m_rpyRaw_prev = m_rpyRaw; +#endif +#endif m_rpyOut.write(); m_rpyRawOut.write(); @@ -250,7 +357,26 @@ RTC::ReturnCode_t KalmanFilter::onExecute(RTC::UniqueId ec_id) */ bool KalmanFilter::SetKalmanFilterParam(double Q_angle, double Q_rate, double R_angle) { - return true; + + r_filter.setF(1, -m_dt, 0, 1); + r_filter.setP(0, 0, 0, 0); + r_filter.setQ(Q_angle*m_dt, 0, 0, Q_rate*m_dt); + r_filter.setR(R_angle); + r_filter.setB(m_dt, 0); + + p_filter.setF(1, -m_dt, 0, 1); + p_filter.setP(0, 0, 0, 0); + p_filter.setQ(Q_angle*m_dt, 0, 0, Q_rate*m_dt); + p_filter.setR(R_angle); + p_filter.setB(m_dt, 0); + + y_filter.setF(1, -m_dt, 0, 1); + y_filter.setP(0, 0, 0, 0); + y_filter.setQ(Q_angle*m_dt, 0, 0, Q_rate*m_dt); + y_filter.setR(R_angle); + y_filter.setB(m_dt, 0); + + return true; } diff --git a/rtc/KalmanFilter/KalmanFilter.h b/rtc/KalmanFilter/KalmanFilter.h index 2fb35505339..c76eefeba43 100644 --- a/rtc/KalmanFilter/KalmanFilter.h +++ b/rtc/KalmanFilter/KalmanFilter.h @@ -28,6 +28,59 @@ namespace hrp{ class KFilter { public: KFilter() { + F(0,0) = 1; F(0,1) =-0.005; + F(1,0) = 0; F(1,1) = 1; + + P(0,0) = 0; P(0,1) = 0; + P(1,0) = 0; P(1,1) = 0; + + Q(0,0) = 0.001 * 0.005; Q(0,1) = 0.000 * 0.005; + Q(1,0) = 0.000 * 0.005; Q(1,1) = 0.003 * 0.005; + + R = 0.03; + + B(0) = 0.005; B(1) = 0; + + H(0,0) = 1; H(0,1) = 0; + + x(0) = 0; x(1) = 0; + + I = hrp::dmatrix::Identity(2,2); + } + void setF(double _f0, double _f1, double _f2, double _f3) { F(0,0) = _f0; F(0,1) = _f1; F(1,0) = _f2; F(1,1) = _f3;} + void setP(double _p0, double _p1, double _p2, double _p3) { P(0,0) = _p0; P(0,1) = _p1; P(1,0) = _p2; P(1,1) = _p3;} + void setQ(double _q0, double _q1, double _q2, double _q3) { Q(0,0) = _q0; Q(0,1) = _q1; Q(1,0) = _q2; Q(1,1) = _q3;} + void setR(double _R) { R = _R; } + void setB(double _b0, double _b1) { B[0] = _b0; B[1] = _b1; } + hrp::Vector2 &getx() { return x; } + void update(double u, double z) { + // Predicted (a priori) state estimate + x = F * x + B * u; + // Predicted (a priori) estimate covariance + P = F * P * F.transpose() + Q; + // + // Innovation or measurement residual + double y = z - H * x; + // Innovation (or residual) covariance + double S = H * P * H.transpose() + R; + // Optimal Kalman gain + K = P * H.transpose() / S; + // Updated (a posteriori) state estimate + x = x + K * y; + // Updated (a posteriori) estimate covariance + P = (I - K * H) * P; + } +private: + hrp::Matrix22 P, Q, I, F; + Eigen::Matrix H; + hrp::Vector2 B, K; + hrp::Vector2 x; + double R; +}; + +class EKFilter { +public: + EKFilter() { x << 1, 0, 0, 0, 0, 0, 0; P = Eigen::Matrix::Identity() * 5; Q = Eigen::Matrix::Identity(); @@ -334,7 +387,8 @@ class KalmanFilter private: double m_dt; - KFilter ekf_filter; + KFilter r_filter, p_filter, y_filter; + EKFilter ekf_filter; hrp::BodyPtr m_robot; hrp::Matrix33 m_sensorR; unsigned int m_debugLevel;