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.

1 files changed, 6 insertions, 2 deletions

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: