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

update KalmanFilter. KF -> EKF and RPY -> Quaternion #296

Merged
merged 1 commit into from
Aug 10, 2014
Merged

update KalmanFilter. KF -> EKF and RPY -> Quaternion #296

Show file tree
Hide file tree
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
234 changes: 64 additions & 170 deletions rtc/KalmanFilter/KalmanFilter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ KalmanFilter::KalmanFilter(RTC::Manager* manager)
m_debugLevel(0),
dummy(0)
{
m_service0.kalman(this);
m_service0.kalman(this);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

趣味もあるけど，こういう変更は，結局何を変更したかわかりづらいので，
ー　中の振る舞いを変えるならその変更のコミットを
ー　インデントなどのフォーマットをかえるならそのコミットを
と分けるのが良いです．

}

KalmanFilter::~KalmanFilter()
Expand Down Expand Up @@ -93,9 +93,9 @@ RTC::ReturnCode_t KalmanFilter::onInitialize()
// Setup robot model
RTC::Properties& prop = getProperties();
if ( ! coil::stringTo(m_dt, prop["dt"].c_str()) ) {
std::cerr << "KalmanFilter failed to prop[dt] " << prop["dt"] << ""
<< std::endl;
return RTC::RTC_ERROR;
std::cerr << "KalmanFilter failed to prop[dt] " << prop["dt"] << ""
<< std::endl;
return RTC::RTC_ERROR;
}

m_robot = hrp::BodyPtr(new hrp::Body());
Expand All @@ -104,36 +104,17 @@ RTC::ReturnCode_t KalmanFilter::onInitialize()
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext())
)){
std::cerr << "failed to load model[" << prop["model"] << "]"
<< std::endl;
std::cerr << "failed to load model[" << prop["model"] << "]"
<< 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 All @@ -151,24 +132,24 @@ RTC::ReturnCode_t KalmanFilter::onInitialize()


/*
RTC::ReturnCode_t KalmanFilter::onFinalize()
{
RTC::ReturnCode_t KalmanFilter::onFinalize()
{
return RTC::RTC_OK;
}
}
*/

/*
RTC::ReturnCode_t KalmanFilter::onStartup(RTC::UniqueId ec_id)
{
RTC::ReturnCode_t KalmanFilter::onStartup(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
}
*/

/*
RTC::ReturnCode_t KalmanFilter::onShutdown(RTC::UniqueId ec_id)
{
RTC::ReturnCode_t KalmanFilter::onShutdown(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
}
*/

RTC::ReturnCode_t KalmanFilter::onActivated(RTC::UniqueId ec_id)
Expand All @@ -185,174 +166,87 @@ RTC::ReturnCode_t KalmanFilter::onDeactivated(RTC::UniqueId ec_id)

RTC::ReturnCode_t KalmanFilter::onExecute(RTC::UniqueId ec_id)
{
static int initialize = 0;
static int initialize = 0;
//std::cerr << m_profile.instance_name<< ": onExecute(" << ec_id << ") " << std::endl;
if (m_rpyIn.isNew() ) {
m_rpyIn.read();
m_rpy.data.r = m_rate.data.avx;
m_rpy.data.p = m_rate.data.avy;
m_rpy.data.y = m_rate.data.avz;
m_rpyOut.write();
return RTC::RTC_OK;
}
if (m_rpyIn.isNew() ) {
m_rpyIn.read();
m_rpy.data.r = m_rate.data.avx;
m_rpy.data.p = m_rate.data.avy;
m_rpy.data.y = m_rate.data.avz;
m_rpyOut.write();
return RTC::RTC_OK;
}
if (m_rateIn.isNew()){
m_rateIn.read();
}
double sx_ref = 0.0, sy_ref = 0.0, sz_ref = 0.0;
if (m_accRefIn.isNew()){
m_accRefIn.read();
sx_ref = m_accRef.data.ax, sy_ref = m_accRef.data.ay, sz_ref = m_accRef.data.az;
m_accRefIn.read();
sx_ref = m_accRef.data.ax, sy_ref = m_accRef.data.ay, sz_ref = m_accRef.data.az;
}
if (m_accIn.isNew()){
m_accIn.read();

//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

m_rpyOut.write();
m_rpyRawOut.write();
m_accIn.read();

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

std::cerr << "raw data acc : " << std::endl << acc << std::endl;
std::cerr << "raw data gyro : " << std::endl << gyro << std::endl;

ekf_filter.prediction(gyro, m_dt);
ekf_filter.correction(acc, Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0));
/* ekf_filter.printAll(); */

Eigen::Matrix<double, 7, 1> x = ekf_filter.getx();
Eigen::Quaternion<double> q = Eigen::Quaternion<double>(x[0], x[1], x[2], x[3]);
Eigen::Vector3d eulerZYX = q.toRotationMatrix().eulerAngles(2,1,0);
m_rpy.data.y = eulerZYX(0);
m_rpy.data.p = eulerZYX(1);
m_rpy.data.r = eulerZYX(2);

m_rpyOut.write();
m_rpyRawOut.write();
}
return RTC::RTC_OK;
}

/*
RTC::ReturnCode_t KalmanFilter::onAborting(RTC::UniqueId ec_id)
{
RTC::ReturnCode_t KalmanFilter::onAborting(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
}
*/

/*
RTC::ReturnCode_t KalmanFilter::onError(RTC::UniqueId ec_id)
{
RTC::ReturnCode_t KalmanFilter::onError(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
}
*/

/*
RTC::ReturnCode_t KalmanFilter::onReset(RTC::UniqueId ec_id)
{
RTC::ReturnCode_t KalmanFilter::onReset(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
}
*/

/*
RTC::ReturnCode_t KalmanFilter::onStateUpdate(RTC::UniqueId ec_id)
{
RTC::ReturnCode_t KalmanFilter::onStateUpdate(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
}
*/

/*
RTC::ReturnCode_t KalmanFilter::onRateChanged(RTC::UniqueId ec_id)
{
RTC::ReturnCode_t KalmanFilter::onRateChanged(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
}
*/

bool KalmanFilter::SetKalmanFilterParam(double Q_angle, double Q_rate, double R_angle) {

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;
return true;
}


Expand Down
Loading