summaryrefslogtreecommitdiff
path: root/src/state.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/state.cpp')
-rw-r--r--src/state.cpp97
1 files changed, 95 insertions, 2 deletions
diff --git a/src/state.cpp b/src/state.cpp
index 47364f1..4677595 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -186,7 +186,11 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S,
{
MatrixXd K;
// P^T is implied here since P is symmetric
+#ifdef BLOCKSI // Here S is S^-1
+ K = P*h.transpose()*S;
+#else
K = S.fullPivLu().solve(h*P).transpose();
+#endif
// Compute P_k|k
P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P;
@@ -308,8 +312,35 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
MatrixXd S;
S = h*P*h.transpose() + R(featuresToUpdate);
kalmanUpdate(h,S,featuresToUpdate,q);
-#else /* ----- not FULLS ----- */
-
+#endif
+#ifdef BLOCKSI
+ MatrixXd h;
+ h = H(q,featuresToUpdate);
+ MatrixXd S;
+ S = blockSI(featuresToUpdate,q);
+ cout << S << endl;
+ kalmanUpdate(h,S,featuresToUpdate,q);
+#endif
+#ifdef MEAS1
+ for (auto i=featuresToUpdate.begin();
+ i!=featuresToUpdate.end(); ++i) {
+ Matrix<double,Dynamic,Dynamic,0,6,6> S;
+ MatrixXd h;
+ std::vector<measurement_t> onez;
+ onez.push_back(*i);
+ h = H(q,onez);
+ if (i->z_type==HEIGHT) {
+ S = body->S(Pxx());
+ } else if (i->z_type==REFLECTION) {
+ Feature *f = featureById(i->id);
+ S = f->S(Pxx(), Pxy(i->id), Pyy(i->id), body->ned() ,q);
+ } else {
+ Feature *f = featureById(i->id);
+ S = f->S(Pxx(), Pxy(i->id), Pyy(i->id), body->ned() ,q);
+ S = S.block<4,4>(0,0);
+ }
+ kalmanUpdate(h,S,onez,q);
+ }
#endif /* ----- not FULLS ----- */
}
addFeatures( featuresToAdd, q, zmeas);
@@ -317,6 +348,68 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
return ;
} /* ----- end of method State::feature_update ----- */
+MatrixXd
+State::blockSI ( const std::vector<measurement_t> &z, const Quaterniond &q )
+{
+ int rows = Hrows(z);
+ MatrixXd SI;
+ SI = MatrixXd::Zero(rows,rows);
+ {
+ int row = 0;
+ for (auto i=z.begin(); i!=z.end(); ++i) {
+ if (i->z_type==HEIGHT) {
+ Matrix<double,1,1> s;
+ s = body->S(Pxx());
+ SI.block<1,1>(row,row) = s.inverse();
+ row += 1;
+ } else if (i->z_type==REFLECTION) {
+ Feature *f;
+ Matrix<double,6,6> s;
+ f = featureById(i->id);
+ s = f->S(Pxx(), Pxy(i->id), Pyy(i->id), body->ned() ,q);
+ SI.block<6,6>(row,row) = s.inverse();
+ row += 6;
+ } else if (i->z_type==MONO) {
+ /*
+ Feature *f;
+ Matrix<double,6,6> s;
+ f = featureById(i->id);
+ s = f->S(Pxx(), Pxy(i->id), Pyy(i->id), body->ned() ,q);
+ s = s.block<4,4>(0,0);
+ SI.block<4,4>(row,row) = s.inverse();
+ row += 4;
+ */
+ } else {
+ fprintf(stderr, "Unrecognized feature type, quitting\n");
+ exit(1);
+ }
+ }
+ }
+
+ return SI ;
+} /* ----- end of method State::blockSI ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ * Class: State
+ * Method: State :: Pxy
+ * Description: Return the covariance block for feature with the given id.
+ *--------------------------------------------------------------------------------------
+ */
+Matrix<double,9,3>
+State::Pxy ( int id )
+{
+ int col = rowById(id);
+ return P.block<9,3>(0,col) ;
+} /* ----- end of method State::Pxy ----- */
+
+Matrix<double,3,3>
+State::Pyy ( int id )
+{
+ int row = rowById(id);
+ return P.block<3,3>(row,row) ;
+} /* ----- end of method State::Pyy ----- */
+
void
State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double z)
{