p71924506
/
att_ekf
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

remove some debug code

This commit is contained in:
libing64 2016-02-29 13:01:56 +08:00
parent d4594c7311
commit ec15a57ffc
3 changed files with 30 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/att_ekf.cpp
View File

@ -25,6 +25,7 @@ Att_ekf::Att_ekf()
R_imu.block<3, 3>(0, 0) = R_acc;
R_imu.block<3, 3>(3, 3) = R_gyro;
P = MatrixXd::Identity(12, 12)*10;
}
Att_ekf::~Att_ekf()
{
@ -32,7 +33,9 @@ Att_ekf::~Att_ekf()
}
void Att_ekf::predict(double t)
{
cout << "before predict: " << x.transpose() << endl;
double dt = t - curr_t;
cout << "dt :" << dt << endl;
Vector3d w = x.head(3);
Vector3d wa = x.segment<3>(3);
Vector3d ra = x.segment<3>(6);
@ -56,7 +59,6 @@ void Att_ekf::predict(double t)
}
void Att_ekf::update_magnetic(Vector3d& mag, double t)
{
rm = mag;
if(!(mag_initialized && imu_initialized) )
{
x.tail(3) = mag;
@ -64,7 +66,12 @@ void Att_ekf::update_magnetic(Vector3d& mag, double t)
curr_t = t;
return;
}
if(t < curr_t) cout << "t is smaller than curr_t" << endl;
if(t < curr_t)
{
cout << "t is smaller than curr_t" << endl;
return;
}
predict(t);
MatrixXd H = MatrixXd::Zero(3, 12);
@ -73,14 +80,14 @@ void Att_ekf::update_magnetic(Vector3d& mag, double t)
MatrixXd K = P.inverse()*H.transpose()*(H*P.inverse()*H.transpose() + R_mag).inverse();
MatrixXd I = MatrixXd::Identity(12, 12);
x = x + K*(z - H*x);
P = (I- K*H)*P.inverse();
//cout << "update mag: " << x.transpose() << endl;
cout << "update mag: " << x.transpose() << endl;
}
void Att_ekf::update_imu(Vector3d &acc, Vector3d & gyro, double t)
{
ra = acc;
if(!(mag_initialized && imu_initialized))
{
x.head(3) = gyro;
@ -90,14 +97,17 @@ void Att_ekf::update_imu(Vector3d &acc, Vector3d & gyro, double t)
curr_t = t;
return;
}
if(t < curr_t) cout << "t is smaller than curr_t" << endl;
if(t < curr_t)
{
cout << "t is smaller than curr_t" << endl;
return;
}
predict(t);
MatrixXd H = MatrixXd::Zero(6, 12);
VectorXd z(6);
z.head(3) = gyro;
z.tail(3) = acc;
H.block<3, 3>(0, 0) = Matrix3d::Identity();
H.block<3, 3>(3, 6) = Matrix3d::Identity();
@ -105,7 +115,7 @@ void Att_ekf::update_imu(Vector3d &acc, Vector3d & gyro, double t)
MatrixXd I = MatrixXd::Identity(12, 12);
x = x + K*(z - H*x);
P = (I- K*H)*P.inverse();
//cout << "update imu: " << x.transpose() << endl;
cout << "update imu: " << x.transpose() << endl;
}
@ -113,18 +123,14 @@ Matrix3d Att_ekf::get_rotation_matrix()
{
if(!(mag_initialized && imu_initialized)) return Matrix3d::Identity();
Matrix3d Rbn;//ENU to body
Vector3d ra = x.segment<3>(6);
Vector3d rm = x.segment<3>(9);
cout << "rm: " << rm.transpose() << endl;
cout << "ra: " << ra.transpose() << endl;
Vector3d Iz = ra;//ENU coordinate
Vector3d Iy = skew_symmetric(Iz)*rm;//why rm in ENU coordinate is [1 0 z], it should be [0 1 x]
Vector3d Iy = skew_symmetric(Iz)*rm;//why rm in ENU coordinate is [1 0 x], it should be [0 1 x]
Vector3d Ix = skew_symmetric(Iy)*Iz;
Ix /= Ix.norm(); Iy /= Iy.norm(); Iz /= Iz.norm();
Rbn.col(0) = Ix; Rbn.col(1) = Iy; Rbn.col(2) = Iz;
return Rbn.transpose();
}

9
src/att_ekf.h
View File

@ -28,10 +28,7 @@ private:
bool imu_initialized;
bool mag_initialized;
const double acc_cov = 1e-2;
const double gyro_cov = 1e-4;
const double mag_cov = 1e-1;
//TODO
Vector3d rm, ra;//test
const double acc_cov = 0.5;
const double gyro_cov = 0.05;
const double mag_cov = 0.5;
};

14
src/att_ekf_node.cpp
View File

@ -35,8 +35,7 @@ void magCallback(const geometry_msgs::Vector3StampedConstPtr& msg)
mag(0) = msg->vector.x;
mag(1) = msg->vector.y;
mag(2) = msg->vector.z;
cout << "mag: " << mag.transpose() << endl;
//cout << "mag: " << mag.transpose() << endl;
}
@ -92,7 +91,7 @@ int main(int argc, char **argv)
if(imu_q.size() == 0 || mag_q.size() == 0) continue;
while(imu_q.back().first - imu_q.front().first > 0.15)//buffer 0.15s
while(imu_q.back().first - imu_q.front().first > 0.05)//buffer size
{
if(imu_q.front().first < mag_q.front().first)
{
@ -116,11 +115,12 @@ int main(int argc, char **argv)
att_ekf.update_magnetic(mag, t);
mag_q.pop_front();
}
if(imu_q.size() == 0 || mag_q.size() == 0) break;
}
cout << "t: " << att_ekf.get_time() << endl;
Quaterniond q = mat2quaternion(att_ekf.get_rotation_matrix());
cout << "q: " << q.w() << " " << q.vec().transpose() << endl;
cout << "q_gt: " << q_gt.w() << " " << q_gt.vec().transpose() << endl;
// cout << "t: " << att_ekf.get_time() << endl;
// Quaterniond q = mat2quaternion(att_ekf.get_rotation_matrix());
// cout << "q: " << q.w() << " " << q.vec().transpose() << endl;
// cout << "q_gt: " << q_gt.w() << " " << q_gt.vec().transpose() << endl;
publish_pose();
//Quaterniond dq = q.inverse()*q_gt;