diff options
| -rw-r--r-- | src/body.cpp | 6 | ||||
| -rw-r--r-- | src/body.h | 2 | ||||
| -rw-r--r-- | src/feature.cpp | 2 | ||||
| -rw-r--r-- | src/feature.h | 14 | ||||
| -rw-r--r-- | src/main.cpp | 7 | ||||
| -rw-r--r-- | src/main.h | 8 | ||||
| -rw-r--r-- | src/state.cpp | 5 | ||||
| -rw-r--r-- | src/state.h | 2 |
8 files changed, 30 insertions, 16 deletions
diff --git a/src/body.cpp b/src/body.cpp index 691a209..041d971 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -29,10 +29,10 @@ Body::dx ( const Matrix<double,9,1> &del ) { X += del; // Constrain the height - if (X[2]<-1.) { + if (X[2]<-1.5) { X[2]=-1.; } else if (X[2]>-0.3) { - X[2]=-0.2; + X[2]=-0.3; } return ; } /* ----- end of method Body::dx ----- */ @@ -195,6 +195,8 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond b.segment<3>(3) = acc-bias+Rbw.transpose()*gravity_world; X.segment<6>(0) += (A*X.segment<3>(3)+b)*dt; + if (X[2]>-0.3) X[2]=-0.3; + if (X[2]<-1.3) X[2]=-1.3; return ; } /* ----- end of method Body::motionModel ----- */ @@ -5,7 +5,7 @@ #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 */ +#define IMU_RANDOMWALK 50e-6 /* Bias process noise */ using Eigen::Matrix; using Eigen::Vector3d; using Eigen::Quaterniond; diff --git a/src/feature.cpp b/src/feature.cpp index 6f0fac5..8b1ffe6 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -146,7 +146,7 @@ Feature::Q ( const double dt ) { Matrix<double,3,3> Q; Q = Matrix<double,3,3>::Identity(); - Q *= dt*dt*FEATURE_NOISE;//*1e-1; + Q *= dt*dt*dt*dt*FEATURE_NOISE;//*1e-1; return Q; } /* ----- end of method Feature::q ----- */ diff --git a/src/feature.h b/src/feature.h index 07fe5ef..ce42d9f 100644 --- a/src/feature.h +++ b/src/feature.h @@ -4,13 +4,13 @@ #include <iostream> #include "types.h" -#define FEATURE_NOISE 1e-3 /* Feature process noise */ -#define VIEW_NOISE 0.35 /* */ -#define INITIAL_VIEW_NOISE 1. /* */ -#define REFLECTION_VIEW_NOISE .35 /* */ -#define FEATURECOVX 1. /* */ -#define FEATURECOVY 1. /* */ -#define FEATURECOVRHO 5e-3 /* */ +#define FEATURE_NOISE 1 /* Feature process noise */ +#define VIEW_NOISE .33 /* */ +#define INITIAL_VIEW_NOISE .33 /* */ +#define REFLECTION_VIEW_NOISE 1.1 /* */ +#define FEATURECOVX .33 /* */ +#define FEATURECOVY .33 /* */ +#define FEATURECOVRHO 5e-2 /* */ #define FEATURECOVRHO_MONO 0.5 /* */ #define RHO_0 1./10. /* */ diff --git a/src/main.cpp b/src/main.cpp index fc08e7a..9de28ff 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -31,7 +31,7 @@ double aboveWater(const Quaterniond &q) { Vector3d tip; - tip << CANOECENTER*3.43, 0, -0.60; + tip << CANOECENTER*3.43, 0, -0.65; tip = q._transformVector(tip); return tip[2]; } @@ -187,9 +187,9 @@ utmCallback(const message &msg, State &mu, const Quaterniond &q) utm_water.zone_c = msg.utm.zone_c; mu.pos(utm_water); } else { - /* i+=1; - if (i%12==0) { + //if ( i==20) { + if (false) { UTM utm_water; utm_water.northing = msg.utm.northing; utm_water.easting = msg.utm.easting; @@ -198,7 +198,6 @@ utmCallback(const message &msg, State &mu, const Quaterniond &q) utm_water.zone_c = msg.utm.zone_c; mu.pos(utm_water); } - */ } return; } @@ -26,6 +26,14 @@ #define ACCBIASY 0.65*0.01465135 #define ACCBIASZ -1*-0.00709229 +//#define ACCBIASX 0.99*-0.03713532 +//#define ACCBIASY 0.2*0.01465135 +//#define ACCBIASZ -1*-0.00709229 + +//#define ACCBIASX -0.03713532 +//#define ACCBIASY 0.01465135 +//#define ACCBIASZ -1*-0.00709229 + #define CANOECENTER 0.65 /* center of gravity of canoe */ #if ROS_PUBLISH diff --git a/src/state.cpp b/src/state.cpp index 94e8a5c..93a1585 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -198,7 +198,12 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, // Compute the innovation or error Matrix<double,Dynamic,1> y; + Eigen::ArrayXd ya; y = innovation(z,q); + ya = y; + + if ( (ya < -0.1).any() ) return; + if ( (ya > 0.1).any() ) return; // Get the update Matrix<double,Dynamic,1> dx; diff --git a/src/state.h b/src/state.h index 92a83ab..d223d0f 100644 --- a/src/state.h +++ b/src/state.h @@ -10,7 +10,7 @@ #include "types.h" #define MAXFEATURES 50 -#define COVBIAS 1e-7 +#define COVBIAS 2e-5 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once #define FULLS // Comment out to treat each measurement independently. //#define BLOCKSI |