Skip to content

Commit

Permalink
Merge pull request #317 from snozawa/revive_old_kf_codes
Browse files Browse the repository at this point in the history 
Revive old KalmanFilter code
  • Loading branch information
@fkanehiro
fkanehiro committed Sep 1, 2014
2 parents cb4f108 + 1501563 commit 1ecaac5
Show file tree
Hide file tree
Showing 2 changed files with 182 additions and 2 deletions.
128 changes: 127 additions & 1 deletion rtc/KalmanFilter/KalmanFilter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
#include <hrpModel/Link.h>
#include <hrpModel/Sensor.h>

//#define USE_EKF

// Module specification
// <rtc-template block="module_spec">
static const char* kalmanfilter_spec[] =
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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

Expand All @@ -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<hrp::RateGyroSensor>("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<double> aaZ(y_filter.getx()[0], Eigen::Vector3d::UnitZ());
Eigen::AngleAxis<double> aaY(p_filter.getx()[0], Eigen::Vector3d::UnitY());
Eigen::AngleAxis<double> aaX(r_filter.getx()[0], Eigen::Vector3d::UnitX());
Eigen::Quaternion<double> 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();
Expand Down Expand Up @@ -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;
}


Expand Down
56 changes: 55 additions & 1 deletion rtc/KalmanFilter/KalmanFilter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double, 1, 2> 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<double, 7, 7>::Identity() * 5;
Q = Eigen::Matrix<double, 7, 7>::Identity();
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 1ecaac5

Please sign in to comment.