diff options
| -rw-r--r-- | src/body.cpp | 6 | ||||
| -rw-r--r-- | src/body.h | 4 | ||||
| -rw-r--r-- | src/feature.cpp | 15 | ||||
| -rw-r--r-- | src/feature.h | 8 | ||||
| -rw-r--r-- | src/main.cpp | 7 | ||||
| -rw-r--r-- | src/main.h | 10 |
6 files changed, 36 insertions, 14 deletions
diff --git a/src/body.cpp b/src/body.cpp index 6985278..7ded581 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -90,7 +90,7 @@ Matrix<double,1,1> Body::R() { Matrix<double,1,1> R; - R << 1e-3; + R << R_HEIGHT; return R; } @@ -110,8 +110,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-6*Matrix<double,3,3>::Identity(); + Q *= IMU_NOISE; + Q.block<3,3>(6,6) = dt*dt*IMU_RANDOMWALK*Matrix<double,3,3>::Identity(); return Q; } /* ----- end of method Body::q ----- */ @@ -3,7 +3,9 @@ #include <Eigen/Dense> #include <iostream> #include "types.h" - +#define R_HEIGHT 1e-2 /* measurement noise of height measurement */ +#define IMU_NOISE 800e-6 /* IMU process noise */ +#define IMU_RANDOMWALK 5e-6 /* Bias process noise */ using Eigen::Matrix; using Eigen::Vector3d; using Eigen::Quaterniond; diff --git a/src/feature.cpp b/src/feature.cpp index 9dce1d1..fc08ee7 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -132,7 +132,7 @@ Feature::Q ( const double dt ) { Matrix<double,3,3> Q; Q = Matrix<double,3,3>::Identity(); - Q *= dt*dt;//*1e-1; + Q *= dt*dt*FEATURE_NOISE;//*1e-1; return Q; } /* ----- end of method Feature::q ----- */ @@ -141,7 +141,9 @@ Feature::R ( ) { Matrix<double,6,6> R; R = Matrix<double,6,6>::Identity(); - R *= 1e-4; + R.block<2,2>(0,0) *= VIEW_NOISE; + R.block<2,2>(2,2) *= INITIAL_VIEW_NOISE; + R.block<2,2>(4,4) *= REFLECTION_VIEW_NOISE; return R; } /* ----- end of method Feature::R ----- */ @@ -492,11 +494,12 @@ Matrix<double,3,3> Feature::P0 ( ) { Matrix<double,3,3> P; - double p0 = 1e-4; - double p1 = 1e-4; + double p0 = FEATURECOVX; + double p1 = FEATURECOVY; + double p2 = FEATURECOVRHO; P << p0, 0., 0., - 0., p0, 0., - 0., 0., p1; + 0., p1, 0., + 0., 0., p2; return P; } /* ----- end of method Feature::P0 ----- */ diff --git a/src/feature.h b/src/feature.h index a8af84e..bddb50a 100644 --- a/src/feature.h +++ b/src/feature.h @@ -4,6 +4,14 @@ #include <iostream> #include "types.h" +#define FEATURE_NOISE 1e-3 /* Feature process noise */ +#define VIEW_NOISE 1e-3 /* */ +#define INITIAL_VIEW_NOISE 1e-3 /* */ +#define REFLECTION_VIEW_NOISE 1e-2 /* */ +#define FEATURECOVX 1e-3 /* */ +#define FEATURECOVY 1e-3 /* */ +#define FEATURECOVRHO 1e-2 /* */ + using Eigen::Dynamic; using Eigen::Matrix; using Eigen::MatrixXd; diff --git a/src/main.cpp b/src/main.cpp index b1b1a5d..eb1cdc1 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -31,7 +31,7 @@ double aboveWater(const Quaterniond &q) { Vector3d tip; - tip << 0.65*3.43, 0, -0.60; + tip << CANOECENTER*3.43, 0, -0.60; tip = q._transformVector(tip); return tip[2]; } @@ -122,7 +122,7 @@ imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp Vector3d acc(msg.linear_acceleration.y,msg.linear_acceleration.x,-msg.linear_acceleration.z); Vector3d ang(msg.angular_velocity.y,msg.angular_velocity.x,-msg.angular_velocity.z); // WARNING: This is the bias for 1112-1 - Vector3d ang_bias(-2.795871394666666222e-03, 6.984255690000021506e-03, -1.418145565750002614e-03); + Vector3d ang_bias(ANGBIASX, ANGBIASY, ANGBIASZ); ang -= ang_bias; double dtf = dt.secs+dt.nsecs*1e-9; @@ -332,8 +332,7 @@ main(int argc, char **argv) Quaterniond qbw; // bias in FRD coordinates Vector3d acc_bias; - //acc_bias << -0.03713532, 1.125*0.01465135, -1*-0.00709229; - acc_bias << 0.95*-0.03713532, 0.65*0.01465135, -1*-0.00709229; + acc_bias << ACCBIASX, ACCBIASY, ACCBIASZ; mu.accelerometer_bias(acc_bias); timestamp dt{0,0}; timestamp t_old{0,0}; @@ -18,6 +18,16 @@ //#define ROS_PUBLISH /* Uncomment to publish ROS node */ //#define ROS_SUBSCRIBE /* Uncomment to subscribe to ROS */ +#define ANGBIASX -2.795871394666666222e-03 /* */ +#define ANGBIASY 6.984255690000021506e-03 +#define ANGBIASZ -1.418145565750002614e-03 + +#define ACCBIASX 0.95*-0.03713532 +#define ACCBIASY 0.65*0.01465135 +#define ACCBIASZ -1*-0.00709229 + +#define CANOECENTER 0.65 /* center of gravity of canoe */ + #if ROS_PUBLISH #include <ros/ros.h> #include <ros/geometry_msgs.h> |