diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/state.cpp | 94 | ||||
| -rw-r--r-- | src/state.h | 8 |
2 files changed, 91 insertions, 11 deletions
diff --git a/src/state.cpp b/src/state.cpp index 243c34b..eac80d8 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -149,25 +149,100 @@ State::R ( const std::vector<measurement_t> &z ) return r ; } /* ----- end of method State::R ----- */ +/* + *-------------------------------------------------------------------------------------- + * Class: State + * Method: State :: kalmanUpdate + * Description: Performs the kalman update. + *-------------------------------------------------------------------------------------- + */ +void +State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, + const std::vector<measurement_t> &z, const Quaterniond &q) +{ + MatrixXd K; + K = P*h.transpose()*S; + // P^T is implied here since P is symmetric + K = S.partialPivLu().solve(h*P).transpose(); + P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P; + P = 0.5*(P+P.transpose()); + + Matrix<double,Dynamic,1> y; + y = innovation(z,q); + Matrix<double,Dynamic,1> dx; + dx = K*y; + body->update(dx.segment<9>(0)); + { + int row=9; + for (auto i=features.begin(); i!=features.end(); ++i, row+=3) { + (*i)->dx(dx.segment<3>(row)); + } + } + + return ; +} /* ----- end of method State::kalmanUpdate ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: State + * Method: State :: innovation + * Description: Returns the innocation vector z-hhat + *-------------------------------------------------------------------------------------- + */ +Matrix<double,Dynamic,1> +State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q) +{ + Matrix<double,Dynamic,1> y; + int rows = Hrows(z); + y = Matrix<double,Dynamic,1>::Zero(rows,1); + + for (int i=0,row=0; i<z.size(); ++i) { + measurement_type mt=z[i].z_type; + if (mt==REFLECTION) { + Feature *fi; + fi = featureById(z[i].id); + Matrix<double,6,1> hi, zi; + hi = fi->h(body->ned(), q); + zi.segment<2>(0) = z[i].source.segment<2>(0); + zi.segment<2>(2) = fi->initialView(); + zi.segment<2>(4) = z[i].reflection.segment<2>(0); + y.segment<6>(row) = zi-hi; + row += 6; + } else if (mt==MONO) { + fprintf(stderr, "mono points not supported.\n"); + exit(1); + } else if (mt==HEIGHT) { + Matrix<double,1,1> zi, hi; + zi[0] = z[i].height; + Vector3d xbw; + xbw = body->ned(); + hi[0] = xbw[2]; + + y.segment<1>(row) = zi-hi; + row += 1; + } else { + fprintf(stderr, "Unknown measurement type\n"); + } + } + + return y; +} + void State::update ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z ) { - MatrixXd h; - h = H(pos,q,z); #ifdef FULLS + MatrixXd h; + h = H(pos,q,z); MatrixXd S; S = h*P*h.transpose() + R(z); + kalmanUpdate(h,S,z,q); + #else /* ----- not FULLS ----- */ - <+ELSE PART+> + #endif /* ----- not FULLS ----- */ /* - Matrix<double,1,1> S; - S=body->S(Pxx()); - Matrix<double,9,1> K; - K = P*H.transpose()*S.inverse(); - P = (Matrix<double,9,9>::Identity()-K*H)*P; - P = 0.5*(P+P.transpose()); Matrix<double,9,1> dx; Matrix<double,1,1> h; h = body->h(); @@ -181,6 +256,7 @@ State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond & { std::vector<measurement_t> featuresToAdd; for (auto i=z.begin(); i!=z.end(); ++i) { + if (i->z_type==HEIGHT) continue; if (exists(i->id)) { ; } else { diff --git a/src/state.h b/src/state.h index 2a18e36..3a90217 100644 --- a/src/state.h +++ b/src/state.h @@ -12,11 +12,12 @@ #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; using Eigen::Matrix; using Eigen::MatrixXd; -using Eigen::Dynamic; -using Eigen::Vector3d; using Eigen::Quaterniond; +using Eigen::Vector2d; +using Eigen::Vector3d; using std::cout; using std::cerr; using std::endl; @@ -49,6 +50,8 @@ class State void enu(const UTM &utm); void feature_update( 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); void motionModel(const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt); void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt); @@ -59,6 +62,7 @@ class State void update ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z ); /* ==================== OPERATORS ======================================= */ + Matrix<double,Dynamic,1> innovation( const std::vector<measurement_t> &z, const Quaterniond &q); void unicsv(); protected: |