From d9254650bd6b8998dec87695ae4875e9f0a4b2da Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Wed, 29 Mar 2017 15:17:19 -0500
Subject: Add new update methods.

But they don't really work.
---
 src/state.h | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

(limited to 'src/state.h')

diff --git a/src/state.h b/src/state.h
index 0e1fe90..11bfa46 100644
--- a/src/state.h
+++ b/src/state.h
@@ -9,9 +9,11 @@
 #include "feature.h"
 #include "types.h"
 
-#define MAXFEATURES 30
+#define MAXFEATURES 50
 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once
 #define FULLS // Comment out to treat each measurement independently.
+//#define BLOCKSI
+//#define MEAS1
 using Eigen::Dynamic;
 using Eigen::Matrix;
 using Eigen::MatrixXd;
@@ -42,7 +44,10 @@ class State
         int Hrows( const std::vector<measurement_t> &z );
         MatrixXd Q(double dt);
         MatrixXd R( const std::vector<measurement_t> &z );
+        MatrixXd blockSI( const std::vector<measurement_t> &z, const Quaterniond &q);
         Matrix<double,9,9> Pxx();
+        Matrix<double,9,3> Pxy(int id);
+        Matrix<double,3,3> Pyy(int id);
         Matrix<double,Dynamic,6> L();
 
         /* ====================  MUTATORS      ======================================= */
-- 
cgit v1.1