diff options
| -rw-r--r-- | src/body.cpp | 6 | ||||
| -rw-r--r-- | src/main.cpp | 4 |
2 files changed, 5 insertions, 5 deletions
diff --git a/src/body.cpp b/src/body.cpp index e6bfb6c..2a907a1 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -84,7 +84,7 @@ Matrix<double,1,1> Body::R() { Matrix<double,1,1> R; - R << 1e-8; + R << 1e-3; return R; } @@ -104,8 +104,8 @@ Body::Q (double dt) Q.block<3,3>(3,0) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity(); Q.block<3,3>(0,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity(); Q.block<3,3>(3,3) = dt*dt*Matrix<double,3,3>::Identity(); - Q *= 800e-6; - Q.block<3,3>(6,6) = dt*dt*5e-3*Matrix<double,3,3>::Identity(); + Q *= 800e-5; + Q.block<3,3>(6,6) = dt*dt*5e-6*Matrix<double,3,3>::Identity(); return Q; } /* ----- end of method Body::q ----- */ diff --git a/src/main.cpp b/src/main.cpp index 6da1cb4..cde65f5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -249,13 +249,13 @@ while (scanf("%s", line)!=EOF) { case INSPVAS: pvaCallback(msg, mu, qbw); - if (seenutm && seencov && seenpva) - mu.unicsv(); break; case RAWIMUS: dt = update_dt(msg.stamp, &t_old); imuCallback(msg, mu, qbw, dt); + if (seenutm && seencov && seenpva) + mu.unicsv(); break; case INSCOVS: |