summaryrefslogtreecommitdiff
path: root/src/state.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/state.cpp')
-rw-r--r--src/state.cpp88
1 files changed, 48 insertions, 40 deletions
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 ----- */