From 229d97ad8e81b104ea6abc0024f055c186be94b6 Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Sat, 25 Mar 2017 20:44:04 -0500 Subject: Add State methods for kalman update and innovation. State::kalmanUpdate should be general enough to work with both the full and independent update methods. The State::innovation method calculates the innovation vector directly rather than building z and hhat individually. --- src/state.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) (limited to 'src/state.h') 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 & z, const Quaterniond &q); void initializePi(int i, const Matrix &Pi); + void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, + const std::vector &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 &z ); /* ==================== OPERATORS ======================================= */ + Matrix innovation( const std::vector &z, const Quaterniond &q); void unicsv(); protected: -- cgit v1.1