diff options
Diffstat (limited to 'src/main.cpp')
| -rw-r--r-- | src/main.cpp | 22 |
1 files changed, 14 insertions, 8 deletions
diff --git a/src/main.cpp b/src/main.cpp index a30ca6c..838bab8 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -23,6 +23,9 @@ bool seenutm=false; bool seenpva=false; Eigen::Matrix<double,9,9> P; +using std::cout; +using std::endl; +using std::cerr; #ifdef USE_ROS @@ -44,7 +47,9 @@ imgCallback(const message *msg) } void -imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, const Eigen::Quaternion<double> &q, const timestamp dt) +imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, + Eigen::Matrix<double,9,9> &P, const Eigen::Quaternion<double> &q, + const timestamp dt) { if (!seenutm || !seenpva || (dt.secs==0 && dt.nsecs==0)) return; using Eigen::Vector3d; @@ -53,9 +58,14 @@ imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, const Eigen::Quate // WARNING: This is the bias for 1112-1 Vector3d ang_bias(-2.795871394666666222e-03, 6.984255690000021506e-03, 1.418145565750002614e-03); ang-=ang_bias; - Body mu; + Body body; + State mu; double dtf = dt.secs+dt.nsecs*1e-9; - mu.motionModel(X,acc,ang,q,dtf); + + Eigen::Matrix<double,9,9> F; + F = body.F(X,ang,q); + mu.Pkk1(P,F,body.Q(dtf)); + body.motionModel(X,acc,ang,q,dtf); } void @@ -221,9 +231,6 @@ main(int argc, char **argv) #ifdef USE_ROS #else /* ----- not USE_ROS ----- */ -using std::cout; -using std::endl; -using std::cerr; printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East\n"); // Read sensors from file char line[MAXLINE]; @@ -251,12 +258,11 @@ while (scanf("%s", line)!=EOF) { case RAWIMUS: dt = update_dt(msg.stamp, &t_old); - imuCallback(msg, bodyStateVector, qbw, dt); + imuCallback(msg, bodyStateVector, P, qbw, dt); break; case INSCOVS: covCallback(msg, P, qbw); - cout << P << endl; default: break; |