From 9533fbcda07254b65a53a9109555662d9a09086c Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Tue, 28 Mar 2017 09:53:29 -0500 Subject: update --- Makefile | 2 +- src/body.cpp | 8 ++++---- src/feature.cpp | 19 +++++++++++++------ src/feature.h | 2 +- src/main.cpp | 32 +++++++++++++++++++++----------- src/main.h | 1 + src/state.cpp | 9 +++++---- src/state.h | 2 +- src/types.cpp | 29 +++++++++++++++++++++++++++++ src/types.h | 1 + 10 files changed, 77 insertions(+), 28 deletions(-) create mode 100644 src/types.cpp diff --git a/Makefile b/Makefile index f65de67..657ff46 100644 --- a/Makefile +++ b/Makefile @@ -1,4 +1,4 @@ -OBJECT=src/main.o src/body.o src/state.o src/feature.o src/camera.o src/ourerr.o +OBJECT=src/main.o src/body.o src/state.o src/feature.o src/camera.o src/ourerr.o src/types.o SRCS=$(patsubst %.o,%.cpp,$(OBJECT)) CXXFLAGS+=-O2 -march=native -std=c++11 -pedantic-errors CXXFLAGS+=-g diff --git a/src/body.cpp b/src/body.cpp index ff54e89..6985278 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -31,7 +31,7 @@ Body::dx ( const Matrix &del ) // Constrain the height if (X[2]<-1.) { X[2]=-1.; - } else if (X[2]>-0.2) { + } else if (X[2]>-0.3) { X[2]=-0.2; } return ; @@ -90,7 +90,7 @@ Matrix Body::R() { Matrix R; - R << 1e-2; + R << 1e-3; return R; } @@ -110,8 +110,8 @@ Body::Q (double dt) Q.block<3,3>(3,0) = 0.5*dt*dt*dt*Matrix::Identity(); Q.block<3,3>(0,3) = 0.5*dt*dt*dt*Matrix::Identity(); Q.block<3,3>(3,3) = dt*dt*Matrix::Identity(); - Q *= 800e-5; - Q.block<3,3>(6,6) = dt*dt*5e-7*Matrix::Identity(); + Q *= 800e-6; + Q.block<3,3>(6,6) = dt*dt*5e-6*Matrix::Identity(); return Q; } /* ----- end of method Body::q ----- */ diff --git a/src/feature.cpp b/src/feature.cpp index 5963ae5..ffce145 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -26,13 +26,20 @@ *-------------------------------------------------------------------------------------- */ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr, - const Vector3d &xbw, const Quaterniond &q) + const Vector3d &xbw, const Quaterniond &q, double z) { - double z = xbw[2]; _id = id; Vector3d xib; xib = findDepth(q,z,xs,xr); X = x2p(xib); + /* + Vector3d pib; + xib = xs/xs[0]; + pib[0] = xib[1]; + pib[1] = xib[2]; + pib[2] = .1; + X = pib; + */ X0 = X; xb0w = xbw; q0 = q; @@ -125,7 +132,7 @@ Feature::Q ( const double dt ) { Matrix Q; Q = Matrix::Identity(); - Q *= dt*dt*1e-3; + Q *= dt*dt;//*1e-1; return Q; } /* ----- end of method Feature::q ----- */ @@ -134,7 +141,7 @@ Feature::R ( ) { Matrix R; R = Matrix::Identity(); - R *= 1e-3; + R *= 1e-4; return R; } /* ----- end of method Feature::R ----- */ @@ -485,8 +492,8 @@ Matrix Feature::P0 ( ) { Matrix P; - double p0 = 1e-2; - double p1 = 1e-1; + double p0 = 1e-4; + double p1 = 1e-4; P << p0, 0., 0., 0., p0, 0., 0., 0., p1; diff --git a/src/feature.h b/src/feature.h index fc5b171..c8b835e 100644 --- a/src/feature.h +++ b/src/feature.h @@ -25,7 +25,7 @@ class Feature public: /* ==================== LIFECYCLE ======================================= */ Feature ( int id, const Vector3d &xs, const Vector3d &xr, - const Vector3d &xbw, const Quaterniond &q); + const Vector3d &xbw, const Quaterniond &q, double z); /* ==================== ACCESSORS ======================================= */ int id(); diff --git a/src/main.cpp b/src/main.cpp index 897a319..4e61484 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -50,10 +50,13 @@ covCallback(const message &msg, State &mu, const Quaterniond &q) // Rotation from ENU to NED Matrix3d Renuned; Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1; - mu.position_covariance(Renuned*msg.covariance.position*Renuned.transpose()); + Matrix3d pcov = Renuned*msg.covariance.position*Renuned.transpose(); + pcov(2,2) *= 10; + mu.position_covariance(pcov); mu.velocity_covariance(Rbw.transpose()*msg.covariance.velocity*Rbw); } + /* * === FUNCTION ====================================================================== * Name: imgCallback @@ -67,13 +70,9 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q) if (seenutm && seenpva && seencov) { std::vector z; - Vector3d tip; - tip << 0.6*3.43, 0, -0.34; - tip = q._transformVector(tip); - measurement_t height; height.z_type = HEIGHT; - height.height = tip[2]; + height.height = aboveWater(q); z.push_back(height); strcat(msg.image_names[0],".txt"); @@ -167,19 +166,30 @@ pvaCallback(const message &msg, State &mu, Quaterniond &q) void utmCallback(const message &msg, State &mu, const Quaterniond &q) { - Vector3d tip; - tip << 0.6*3.43, 0, -0.34; - tip = q._transformVector(tip); + static int i=0; if ((!seenutm || !seencov) && seenpva) { seenutm=true; UTM utm_water; utm_water.northing = msg.utm.northing; utm_water.easting = msg.utm.easting; - utm_water.up = -tip[2]; + utm_water.up = -aboveWater(q); utm_water.zone_i = msg.utm.zone_i; utm_water.zone_c = msg.utm.zone_c; mu.pos(utm_water); - } + } else { + /* + i+=1; + if (i%18==0) { + UTM utm_water; + utm_water.northing = msg.utm.northing; + utm_water.easting = msg.utm.easting; + utm_water.up = -aboveWater(q); + utm_water.zone_i = msg.utm.zone_i; + utm_water.zone_c = msg.utm.zone_c; + mu.pos(utm_water); + } + */ + } return; } diff --git a/src/main.h b/src/main.h index 0746a99..a78bab0 100644 --- a/src/main.h +++ b/src/main.h @@ -30,4 +30,5 @@ void imuCallback(const message &msg, State &mu, const Quaterniond &q, const time void pvaCallback(const message &msg, Matrix &X, Quaterniond &q); void utmCallback(const message &msg, State &mu, const Quaterniond &q); + #endif /* ----- #ifndef main_INC ----- */ diff --git a/src/state.cpp b/src/state.cpp index c33c053..aee0caa 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -21,7 +21,7 @@ State::State ( ) { body = new Body; P = Matrix::Zero(9,9); - P.block<3,3>(6,6) = 1e-9*Matrix3d::Identity(); + P.block<3,3>(6,6) = 1e-6*Matrix3d::Identity(); return ; } /* ----- end of method State::State ----- */ @@ -180,7 +180,7 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, { MatrixXd K; // P^T is implied here since P is symmetric - K = S.partialPivLu().solve(h*P).transpose(); + K = S.fullPivLu().solve(h*P).transpose(); // Compute P_k|k P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P; @@ -269,7 +269,7 @@ State::handle_measurements ( const std::vector &z, const Quaterni featuresToAdd.push_back(*i); } } - if (featuresToUpdate.size()>0) { + if (featuresToUpdate.size()>1) { #ifdef FULLS MatrixXd h; h = H(q,featuresToUpdate); @@ -289,11 +289,12 @@ void State::addFeatures ( std::vector &F, const Quaterniond &q) { // Add new features + double z = aboveWater(q); Vector3d pos = body->ned(); for (auto i=F.begin(); i!=F.end(); ++i) { if (features.size()>MAXFEATURES) break; // Create feature - Feature *f = new Feature(i->id, i->source, i->reflection, pos, q); + Feature *f = new Feature(i->id, i->source, i->reflection, pos, q, z); if (!f->sane()) { delete f; continue; diff --git a/src/state.h b/src/state.h index 4001216..89676f3 100644 --- a/src/state.h +++ b/src/state.h @@ -9,7 +9,7 @@ #include "feature.h" #include "types.h" -#define MAXFEATURES 50 +#define MAXFEATURES 30 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once #define FULLS // Comment out to treat each measurement independently. using Eigen::Dynamic; diff --git a/src/types.cpp b/src/types.cpp new file mode 100644 index 0000000..151fd63 --- /dev/null +++ b/src/types.cpp @@ -0,0 +1,29 @@ +/* + * ===================================================================================== + * + * Filename: types.cpp + * + * Description: + * + * Version: 1.0 + * Created: 03/27/2017 03:26:51 PM + * Revision: none + * Compiler: gcc + * + * Author: Martin Miller (MHM), miller7@illinois.edu + * Organization: Aerospace Robotics and Controls Lab (ARC) + * + * ===================================================================================== + */ + + +#include "types.h" + +double +aboveWater(const Quaterniond &q) +{ + Vector3d tip; + tip << 0.65*3.43, 0, -0.60; + tip = q._transformVector(tip); + return tip[2]; +} diff --git a/src/types.h b/src/types.h index c284d4a..f32dfea 100644 --- a/src/types.h +++ b/src/types.h @@ -79,4 +79,5 @@ typedef struct { velocity_t velocity; covariance_t covariance; } message; +double aboveWater(const Quaterniond &q); #endif /* ----- #ifndef types_INC ----- */ -- cgit v1.1