summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--Makefile2
-rw-r--r--src/body.cpp8
-rw-r--r--src/feature.cpp19
-rw-r--r--src/feature.h2
-rw-r--r--src/main.cpp32
-rw-r--r--src/main.h1
-rw-r--r--src/state.cpp9
-rw-r--r--src/state.h2
-rw-r--r--src/types.cpp29
-rw-r--r--src/types.h1
10 files changed, 77 insertions, 28 deletions
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<double,9,1> &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<double,1,1>
Body::R()
{
Matrix<double,1,1> 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<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-5;
- Q.block<3,3>(6,6) = dt*dt*5e-7*Matrix<double,3,3>::Identity();
+ Q *= 800e-6;
+ 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/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<double,3,3> Q;
Q = Matrix<double,3,3>::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<double,6,6> R;
R = Matrix<double,6,6>::Identity();
- R *= 1e-3;
+ R *= 1e-4;
return R;
} /* ----- end of method Feature::R ----- */
@@ -485,8 +492,8 @@ Matrix<double,3,3>
Feature::P0 ( )
{
Matrix<double,3,3> 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<measurement_t> 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<double,9,1> &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<double,Dynamic,Dynamic>::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<measurement_t> &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<measurement_t> &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 ----- */