summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/body.cpp10
-rw-r--r--src/main.cpp2
-rw-r--r--src/state.cpp88
-rw-r--r--src/state.h7
4 files changed, 57 insertions, 50 deletions
diff --git a/src/body.cpp b/src/body.cpp
index e6afe8a..ff54e89 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -40,7 +40,7 @@ Body::dx ( const Matrix<double,9,1> &del )
void
Body::vel ( const Matrix<double,3,1> &v )
{
- X.segment(3,3) = v;
+ X.segment<3>(3) = v;
return ;
} /* ----- end of method Body::vel ----- */
@@ -127,7 +127,7 @@ Body::H ( )
{
Matrix<double,1,9> H;
H = Matrix<double,1,9>::Zero();
- H[0,2] = 1;
+ H(2) = 1.;
return H ;
} /* ----- end of method Body::H ----- */
@@ -192,9 +192,9 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond
Matrix<double,3,3> W;
W = skewSymmetric(ang);
A.block<3,3>(3,0) = -W;
- b.segment(3,3) = acc-bias+Rbw.transpose()*gravity_world;
+ b.segment<3>(3) = acc-bias+Rbw.transpose()*gravity_world;
- X.segment(0,6) += (A*X.segment(3,3)+b)*dt;
+ X.segment<6>(0) += (A*X.segment<3>(3)+b)*dt;
return ;
} /* ----- end of method Body::motionModel ----- */
@@ -226,7 +226,7 @@ Body::unicsv ( )
void
Body::accelerometer_bias ( const Vector3d &b )
{
- X.segment(6,3) = b;
+ X.segment<3>(6) = b;
return ;
} /* ----- end of method State::accelerometer_bias ----- */
diff --git a/src/main.cpp b/src/main.cpp
index bb2a3b3..897a319 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -98,7 +98,7 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q)
if (fclose(fin)==EOF) {
err_sys("fclose");
}
- mu.feature_update(z,q);
+ mu.handle_measurements(z,q);
}
return;
diff --git a/src/state.cpp b/src/state.cpp
index 1df68eb..c33c053 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(6,6,3,3) = 1e-9*Matrix3d::Identity();
+ P.block<3,3>(6,6) = 1e-9*Matrix3d::Identity();
return ;
} /* ----- end of method State::State ----- */
@@ -63,13 +63,14 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z )
int col;
case REFLECTION:
col = rowById(z[i].id);
- Feature *fi;
- fi = featureById(z[i].id);
if (col==-1) {
fprintf(stderr, "Feature %d not found, quitting.\n", z[i].id);
exit(1);
}
+ Feature *fi;
+ fi = featureById(z[i].id);
+
h.block<6,9>(row,0) = fi->Hx(pos,q);
h.block<6,3>(row,col) = fi->Hy(pos,q);
row += 6;
@@ -92,6 +93,14 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z )
return h;
} /* ----- end of method State::H ----- */
+/*
+ *--------------------------------------------------------------------------------------
+ * Class: State
+ * Method: State :: Hrows
+ * Description: Returns the number of rows in the measurement vector based on
+ * the number and types of measurements.
+ *--------------------------------------------------------------------------------------
+ */
int
State::Hrows ( const std::vector<measurement_t> &z)
{
@@ -117,6 +126,13 @@ State::Hrows ( const std::vector<measurement_t> &z)
return rows ;
} /* ----- end of method State::Hrows ----- */
+/*
+ *--------------------------------------------------------------------------------------
+ * Class: State
+ * Method: State :: R
+ * Description: Returns the measurement noise matrix.
+ *--------------------------------------------------------------------------------------
+ */
MatrixXd
State::R ( const std::vector<measurement_t> &z )
{
@@ -165,11 +181,16 @@ 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();
+
+ // Compute P_k|k
P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P;
P = 0.5*(P+P.transpose());
+ // Compute the innovation or error
Matrix<double,Dynamic,1> y;
y = innovation(z,q);
+
+ // Get the update
Matrix<double,Dynamic,1> dx;
dx = K*y;
body->dx(dx.segment<9>(0));
@@ -187,7 +208,7 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S,
*--------------------------------------------------------------------------------------
* Class: State
* Method: State :: innovation
- * Description: Returns the innocation vector z-hhat
+ * Description: Returns the innovation vector z-hhat
*--------------------------------------------------------------------------------------
*/
Matrix<double,Dynamic,1>
@@ -227,31 +248,15 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q)
return y;
}
-void
-State::update ( const Quaterniond &q, const std::vector<measurement_t> &z )
-{
-#ifdef FULLS
- MatrixXd h;
- h = H(q,z);
- MatrixXd S;
- S = h*P*h.transpose() + R(z);
- kalmanUpdate(h,S,z,q);
- cout << features.size() << endl;
-
-#else /* ----- not FULLS ----- */
-
-#endif /* ----- not FULLS ----- */
- /*
- Matrix<double,9,1> dx;
- Matrix<double,1,1> h;
- h = body->h();
- dx = K*(z-h);
- body->update(dx);
- */
-}
-
+/*
+ *--------------------------------------------------------------------------------------
+ * Class: State
+ * Method: State :: handle_measurements
+ * Description: Update EKF, and adds new features to state.
+ *--------------------------------------------------------------------------------------
+ */
void
-State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &q)
+State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterniond &q)
{
std::vector<measurement_t> featuresToAdd;
std::vector<measurement_t> featuresToUpdate;
@@ -265,7 +270,15 @@ State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &
}
}
if (featuresToUpdate.size()>0) {
- update(q, featuresToUpdate);
+#ifdef FULLS
+ MatrixXd h;
+ h = H(q,featuresToUpdate);
+ MatrixXd S;
+ S = h*P*h.transpose() + R(featuresToUpdate);
+ kalmanUpdate(h,S,featuresToUpdate,q);
+#else /* ----- not FULLS ----- */
+
+#endif /* ----- not FULLS ----- */
}
addFeatures( featuresToAdd, q);
@@ -276,11 +289,11 @@ void
State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q)
{
// Add new features
+ Vector3d pos = body->ned();
for (auto i=F.begin(); i!=F.end(); ++i) {
if (features.size()>MAXFEATURES) break;
// Create feature
- Vector3d pos = body->ned();
- Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, pos[2]);
+ Feature *f = new Feature(i->id, i->source, i->reflection, pos, q);
if (!f->sane()) {
delete f;
continue;
@@ -288,13 +301,7 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q)
features.push_back(f);
// Expand P
- expandP();
-
- // Initialize P values
- int j=features.size();
- Matrix<double,3,3> Pi;
- Pi = f->P0();
- initializePi(j,Pi);
+ expandP(f->P0());
}
return ;
} /* ----- end of method State::addFeatures ----- */
@@ -564,9 +571,10 @@ State::featureById ( int id )
*--------------------------------------------------------------------------------------
*/
void
-State::expandP ( )
+State::expandP ( const Matrix3d &Pi)
{
- P.conservativeResize(3+P.rows(),3+P.cols());
+ P.conservativeResizeLike(MatrixXd::Zero(3+P.cols(),3+P.cols()));
+ P.bottomRightCorner<3,3>() = Pi;
return ;
} /* ----- end of method State::expandP ----- */
diff --git a/src/state.h b/src/state.h
index ea8cfef..4001216 100644
--- a/src/state.h
+++ b/src/state.h
@@ -9,7 +9,7 @@
#include "feature.h"
#include "types.h"
-#define MAXFEATURES 30
+#define MAXFEATURES 50
//#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;
@@ -48,7 +48,7 @@ class State
/* ==================== MUTATORS ======================================= */
void accelerometer_bias(const Vector3d &b);
void pos(const UTM &utm);
- void feature_update( const std::vector<measurement_t> & z, const Quaterniond &q);
+ void handle_measurements( const std::vector<measurement_t> & z, const Quaterniond &q);
void initializePi(int i, const Matrix<double,3,3> &Pi);
void kalmanUpdate( const MatrixXd &h, const MatrixXd &S,
const std::vector<measurement_t> &z, const Quaterniond &q);
@@ -59,7 +59,6 @@ class State
std::list<Feature *>::iterator removeFeature(std::list<Feature *>::iterator &i, int j);
void velocity_covariance(const Matrix<double,3,3> &p);
void vel(const Matrix<double,3,1> &v);
- void update ( const Quaterniond &q, const std::vector<measurement_t> &z );
/* ==================== OPERATORS ======================================= */
Matrix<double,Dynamic,1> innovation( const std::vector<measurement_t> &z, const Quaterniond &q);
@@ -73,7 +72,7 @@ class State
private:
/* ==================== METHODS ======================================= */
void addFeatures(std::vector<measurement_t> &F, const Quaterniond &q);
- void expandP();
+ void expandP ( const Matrix3d &Pi);
void shrinkP( int i );
/* ==================== DATA MEMBERS ======================================= */