From eb8abfc142fa6aada8057fbca57d1c909950416d Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Sun, 19 Mar 2017 14:15:33 -0500
Subject: Implement feature measurement prediction.

Used a slightly simplified expression to compute pib0. Replaced the
Householder Transform with quaternion based reflection (untested).
---
 src/feature.h | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

(limited to 'src/feature.h')

diff --git a/src/feature.h b/src/feature.h
index 077a617..2982276 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -28,7 +28,7 @@ class Feature
         Matrix<double,3,3> Fy( const Vector3d &vel, const Vector3d &ang);
         Matrix<double,6,9> Hx( const Vector3d &pos, const Quaterniond &q);
         Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q);
-        Matrix<double,6,1> h();
+        Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q);
         /*
         void motionModel ( const Vector3d &acc, const Vector3d &ang, 
                 const Quaterniond &q, const double dt);
@@ -46,6 +46,8 @@ class Feature
 
     private:
         /* ====================  METHODS       ======================================= */
+        Vector3d p2x(const Vector3d &p);
+        Vector3d x2p(const Vector3d &x);
 
         /* ====================  DATA MEMBERS  ======================================= */
         Vector3d X;
-- 
cgit v1.1