diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index 3708f51d3aa1141756dc54c27f76039671bfa42e..c73e295083b71c0237d2569ff90ff0b9191b1192 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -225,7 +225,7 @@ int main()
             if (sb && !sb->isFixed())
                 sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5);       // We perturb A LOT !
     for (auto kf : problem->getTrajectory()->getFrameList())
-        if (kf->isKey())
+        if (kf->isKeyOrAux())
             for (auto sb : kf->getStateBlockVec())
                 if (sb && !sb->isFixed())
                     sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5);   // We perturb A LOT !
@@ -245,7 +245,7 @@ int main()
     WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
     ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
     for (auto kf : problem->getTrajectory()->getFrameList())
-        if (kf->isKey())
+        if (kf->isKeyOrAux())
         {
             Eigen::MatrixXs cov;
             kf->getCovariance(cov);
diff --git a/include/base/ceres_wrapper/ceres_manager.h b/include/base/ceres_wrapper/ceres_manager.h
index d156fd80521b31da81975ea11f55d9949fbb3754..2bdc1d4bd002deaa4e01edfac6817618fae40951 100644
--- a/include/base/ceres_wrapper/ceres_manager.h
+++ b/include/base/ceres_wrapper/ceres_manager.h
@@ -56,7 +56,7 @@ class CeresManager : public SolverManager
         virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
                                         = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
 
-        virtual void computeCovariances(const StateBlockPtrList& st_list) override;
+        virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) override;
 
         virtual bool hasConverged() override;
 
diff --git a/include/base/ceres_wrapper/qr_manager.h b/include/base/ceres_wrapper/qr_manager.h
index d4945e066a372d5ec95578a87552e4369eec1b2e..39d133d610fb2f497c88c70781cd3c047dba81d2 100644
--- a/include/base/ceres_wrapper/qr_manager.h
+++ b/include/base/ceres_wrapper/qr_manager.h
@@ -38,7 +38,7 @@ class QRManager : public SolverManager
 
         virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
 
-        virtual void computeCovariances(const StateBlockPtrList& _sb_list);
+        virtual void computeCovariances(const std::vector<StateBlockPtr>& _sb_list);
 
     private:
 
diff --git a/include/base/frame/frame_base.h b/include/base/frame/frame_base.h
index ffa84d74d0c7521ce226a5445e58591bfc43be66..f4befb4d11cbc5f54a093e7a6ebcfa221c72b275 100644
--- a/include/base/frame/frame_base.h
+++ b/include/base/frame/frame_base.h
@@ -18,12 +18,13 @@ class StateBlock;
 
 namespace wolf {
 
-/** \brief Enumeration of frame types: key-frame or non-key-frame
+/** \brief Enumeration of frame types
  */
 typedef enum
 {
-    NON_KEY_FRAME = 0,  ///< Regular frame. It does not play at optimization.
-    KEY_FRAME = 1       ///< key frame. It plays at optimizations.
+    KEY = 2,          ///< key frame. It plays at optimizations (estimated).
+    AUXILIARY = 1,    ///< auxiliary frame. It plays at optimizations (estimated).
+    NON_ESTIMATED = 0 ///< regular frame. It does not play at optimization.
 } FrameType;
 
 //class FrameBase
@@ -71,9 +72,14 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
     public:
         unsigned int id();
 
-        // KeyFrame / NonKeyFrame
+        // get type
         bool isKey() const;
+        bool isAux() const;
+        bool isKeyOrAux() const;
+
+        // set type
         void setKey();
+        void setAux();
 
         // Frame values ------------------------------------------------
     public:
@@ -169,7 +175,17 @@ inline unsigned int FrameBase::id()
 
 inline bool FrameBase::isKey() const
 {
-    return (type_ == KEY_FRAME);
+    return (type_ == KEY);
+}
+
+inline bool FrameBase::isAux() const
+{
+    return (type_ == AUXILIARY);
+}
+
+inline bool FrameBase::isKeyOrAux() const
+{
+    return (type_ == KEY || type_ == AUXILIARY);
 }
 
 inline void FrameBase::getTimeStamp(TimeStamp& _ts) const
diff --git a/include/base/problem/problem.h b/include/base/problem/problem.h
index e6375bff43943585dcdf703155fe6e9ad6697ce2..9c1d768e0d1be2fbac9cf2c9d926e69d8769564e 100644
--- a/include/base/problem/problem.h
+++ b/include/base/problem/problem.h
@@ -140,7 +140,7 @@ class Problem : public std::enable_shared_from_this<Problem>
 
         /** \brief Emplace frame from string frame_structure
          * \param _frame_structure String indicating the frame structure.
-         * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
+         * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
          * \param _frame_state State vector; must match the size and format of the chosen frame structure
          * \param _time_stamp Time stamp of the frame
          *
@@ -156,7 +156,7 @@ class Problem : public std::enable_shared_from_this<Problem>
 
         /** \brief Emplace frame from string frame_structure without state
          * \param _frame_structure String indicating the frame structure.
-         * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
+         * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
          * \param _time_stamp Time stamp of the frame
          *
          * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
@@ -169,7 +169,7 @@ class Problem : public std::enable_shared_from_this<Problem>
                                   const TimeStamp& _time_stamp);
 
         /** \brief Emplace frame from string frame_structure without structure
-         * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
+         * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
          * \param _frame_state State vector; must match the size and format of the chosen frame structure
          * \param _time_stamp Time stamp of the frame
          *
@@ -183,7 +183,7 @@ class Problem : public std::enable_shared_from_this<Problem>
                                   const TimeStamp& _time_stamp);
 
         /** \brief Emplace frame from string frame_structure without structure nor state
-         * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME
+         * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
          * \param _time_stamp Time stamp of the frame
          *
          * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
@@ -195,26 +195,44 @@ class Problem : public std::enable_shared_from_this<Problem>
                                   const TimeStamp& _time_stamp);
 
         // Frame getters
-        FrameBasePtr    getLastFrame         ( );
-        FrameBasePtr    getLastKeyFrame      ( );
-        FrameBasePtr    closestKeyFrameToTimeStamp(const TimeStamp& _ts);
+        FrameBasePtr getLastFrame( ) const;
+        FrameBasePtr getLastKeyFrame( ) const;
+        FrameBasePtr getLastKeyOrAuxFrame( ) const;
+        FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
+        FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const;
 
-        /** \brief Give the permission to a processor to create a new keyFrame
+        /** \brief Give the permission to a processor to create a new key Frame
          *
-         * This should implement a black list of processors that have forbidden keyframe creation.
+         * This should implement a black list of processors that have forbidden key frame creation.
          *   - This decision is made at problem level, not at processor configuration level.
-         *   - Therefore, if you want to permanently configure a processor for not creating keyframes, see Processor::voting_active_ and its accessors.
+         *   - Therefore, if you want to permanently configure a processor for not creating key frames, see Processor::voting_active_ and its accessors.
          */
         bool permitKeyFrame(ProcessorBasePtr _processor_ptr);
 
         /** \brief New key frame callback
          *
-         * New key frame callback: It should be called by any processor that creates a new keyframe. It calls the keyFrameCallback of the rest of processors.
+         * New key frame callback: It should be called by any processor that creates a new key frame. It calls the keyFrameCallback of the rest of processors.
          */
         void keyFrameCallback(FrameBasePtr _keyframe_ptr, //
                               ProcessorBasePtr _processor_ptr, //
                               const Scalar& _time_tolerance);
 
+        /** \brief Give the permission to a processor to create a new auxiliary Frame
+         *
+         * This should implement a black list of processors that have forbidden auxiliary frame creation.
+         *   - This decision is made at problem level, not at processor configuration level.
+         *   - Therefore, if you want to permanently configure a processor for not creating estimated frames, see Processor::voting_active_ and its accessors.
+         */
+        bool permitAuxFrame(ProcessorBasePtr _processor_ptr);
+
+        /** \brief New auxiliary frame callback
+         *
+         * New auxiliary frame callback: It should be called by any processor that creates a new auxiliary frame. It calls the auxiliaryFrameCallback of the processor motion.
+         */
+        void auxFrameCallback(FrameBasePtr _frame_ptr, //
+                              ProcessorBasePtr _processor_ptr, //
+                              const Scalar& _time_tolerance);
+
         // State getters
         Eigen::VectorXs getCurrentState         ( );
         void            getCurrentState         (Eigen::VectorXs& state);
@@ -242,6 +260,7 @@ class Problem : public std::enable_shared_from_this<Problem>
         bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col = 0);
         bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance);
         bool getLastKeyFrameCovariance(Eigen::MatrixXs& _covariance);
+        bool getLastKeyOrAuxFrameCovariance(Eigen::MatrixXs& _covariance);
         bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance);
 
         // Solver management ----------------------------------------
diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h
index 5d68c43c7b921532dc1d1d0d32ddabe89d75de93..9e149abcbdfca7c94942b92a1669c9c8ee5f6e70 100644
--- a/include/base/processor/processor_base.h
+++ b/include/base/processor/processor_base.h
@@ -110,16 +110,19 @@ struct ProcessorParamsBase
     ProcessorParamsBase() = default;
 
     ProcessorParamsBase(bool _voting_active,
-                        Scalar _time_tolerance)
-      : voting_active(_voting_active)
-      , time_tolerance(_time_tolerance)
+                        Scalar _time_tolerance,
+                        bool _voting_aux_active = false) :
+        voting_active(_voting_active),
+        voting_aux_active(_voting_aux_active),
+        time_tolerance(_time_tolerance)
     {
       //
     }
 
     virtual ~ProcessorParamsBase() = default;
 
-    bool voting_active = false;
+    bool voting_active = false;     ///< Whether this processor is allowed to vote for a Key Frame or not
+    bool voting_aux_active = false; ///< Whether this processor is allowed to vote for an Auxiliary Frame or not
 
     ///< maximum time difference between a Keyframe time stamp and
     /// a particular Capture of this processor to allow assigning
@@ -159,8 +162,19 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
          */
         virtual bool voteForKeyFrame() = 0;
 
+        /** \brief Vote for Auxiliary Frame generation
+         *
+         * If a Auxiliary Frame criterion is validated, this function returns true,
+         * meaning that it wants to create a Auxiliary Frame at the \b last Capture.
+         *
+         * WARNING! This function only votes! It does not create Auxiliary Frames!
+         */
+        virtual bool voteForAuxFrame(){return false;};
+
         virtual bool permittedKeyFrame() final;
 
+        virtual bool permittedAuxFrame() final;
+
         /**\brief make a Frame with the provided Capture
          *
          * Provide the following functionality:
@@ -192,7 +206,11 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
 
         bool isVotingActive() const;
 
+        bool isVotingAuxActive() const;
+
         void setVotingActive(bool _voting_active = true);
+
+        void setVotingAuxActive(bool _voting_active = true);
 };
 
 inline bool ProcessorBase::isVotingActive() const
@@ -200,11 +218,21 @@ inline bool ProcessorBase::isVotingActive() const
     return params_->voting_active;
 }
 
+inline bool ProcessorBase::isVotingAuxActive() const
+{
+    return params_->voting_aux_active;
+}
+
 inline void ProcessorBase::setVotingActive(bool _voting_active)
 {
     params_->voting_active = _voting_active;
 }
 
+inline void ProcessorBase::setVotingAuxActive(bool _voting_active)
+{
+    params_->voting_aux_active = _voting_active;
+}
+
 }
 
 #include "base/sensor/sensor_base.h"
diff --git a/include/base/solver/solver_manager.h b/include/base/solver/solver_manager.h
index 28e43acfa63426072e6883b2a3237535775d1fc8..8aefa72135e3897c6a52ac550a131db782ea7860 100644
--- a/include/base/solver/solver_manager.h
+++ b/include/base/solver/solver_manager.h
@@ -53,7 +53,7 @@ public:
 
   virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0;
 
-  virtual void computeCovariances(const StateBlockPtrList& st_list) = 0;
+  virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) = 0;
 
   virtual bool hasConverged() = 0;
 
diff --git a/include/base/trajectory/trajectory_base.h b/include/base/trajectory/trajectory_base.h
index 37f9762c7c9955381eaba9bc52aef253441d36e1..aff013ea00d0b40af358ad4d3cb980d8298ccc4e 100644
--- a/include/base/trajectory/trajectory_base.h
+++ b/include/base/trajectory/trajectory_base.h
@@ -26,6 +26,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
     protected:
         std::string frame_structure_;  // Defines the structure of the Frames in the Trajectory.
         FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame
+        FrameBasePtr last_key_or_aux_frame_ptr_; // keeps pointer to the last estimated frame
         
     public:
         TrajectoryBase(const std::string& _frame_sturcture);
@@ -37,18 +38,21 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
         // Frames
         FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
         FrameBasePtrList& getFrameList();
-        FrameBasePtr getLastFrame();
-        FrameBasePtr getLastKeyFrame();
-        FrameBasePtr findLastKeyFrame();
-        FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
-        void setLastKeyFrame(FrameBasePtr _key_frame_ptr);
+        const FrameBasePtrList& getFrameList() const;
+        FrameBasePtr getLastFrame() const;
+        FrameBasePtr getLastKeyFrame() const;
+        FrameBasePtr getLastKeyOrAuxFrame() const;
+        FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
+        FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const;
         void sortFrame(FrameBasePtr _frm_ptr);
-        void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place);
-        FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr);
+        void updateLastFrames();
 
         // factors
         void getFactorList(FactorBasePtrList & _fac_list);
 
+    protected:
+        FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr);
+        void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place);
 };
 
 inline FrameBasePtrList& TrajectoryBase::getFrameList()
@@ -56,19 +60,24 @@ inline FrameBasePtrList& TrajectoryBase::getFrameList()
     return frame_list_;
 }
 
-inline FrameBasePtr TrajectoryBase::getLastFrame()
+inline const FrameBasePtrList& TrajectoryBase::getFrameList() const
+{
+    return frame_list_;
+}
+
+inline FrameBasePtr TrajectoryBase::getLastFrame() const
 {
     return frame_list_.back();
 }
 
-inline FrameBasePtr TrajectoryBase::getLastKeyFrame()
+inline FrameBasePtr TrajectoryBase::getLastKeyFrame() const
 {
     return last_key_frame_ptr_;
 }
 
-inline void TrajectoryBase::setLastKeyFrame(FrameBasePtr _key_frame_ptr)
+inline FrameBasePtr TrajectoryBase::getLastKeyOrAuxFrame() const
 {
-    last_key_frame_ptr_ = _key_frame_ptr;
+    return last_key_or_aux_frame_ptr_;
 }
 
 inline std::string TrajectoryBase::getFrameStructure() const
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 5871b8fa12ee6f9ce5c3aff8741e80444ab9e166..5f3906c4c0ae0ec1af82199981c930b2fcabd3c5 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -62,7 +62,7 @@ std::string CeresManager::solveImpl(const ReportVerbosity report_level)
 }
 
 void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
-{
+{   
     // update problem
     update();
 
@@ -81,7 +81,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
             std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
             //frame state blocks
             for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
-                if (fr_ptr->isKey())
+                if (fr_ptr->isKeyOrAux())
                     for (auto sb : fr_ptr->getStateBlockVec())
                         if (sb)
                             all_state_blocks.push_back(sb);
@@ -106,7 +106,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
         {
             // first create a vector containing all state blocks
             for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
-                if (fr_ptr->isKey())
+                if (fr_ptr->isKeyOrAux())
                     for (auto sb : fr_ptr->getStateBlockVec())
                         if (sb)
                             for(auto sb2 : fr_ptr->getStateBlockVec())
@@ -194,17 +194,16 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
         // STORE DESIRED COVARIANCES
         for (unsigned int i = 0; i < double_pairs.size(); i++)
         {
-            Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
-            covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data());
+            Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getLocalSize(),state_block_pairs[i].second->getLocalSize());
+            covariance_->GetCovarianceBlockInTangentSpace(double_pairs[i].first, double_pairs[i].second, cov.data());
             wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
-            //std::cout << "getted covariance " << std::endl << cov << std::endl;
+            // std::cout << "covariance got switch: " << std::endl << cov << std::endl;
         }
     }
     else
         std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
 }
-
-void CeresManager::computeCovariances(const StateBlockPtrList& st_list)
+void CeresManager::computeCovariances(const std::vector<StateBlockPtr>& st_list)
 {
     //std::cout << "CeresManager: computing covariances..." << std::endl;
 
@@ -234,10 +233,10 @@ void CeresManager::computeCovariances(const StateBlockPtrList& st_list)
         // STORE DESIRED COVARIANCES
         for (unsigned int i = 0; i < double_pairs.size(); i++)
         {
-            Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
-            covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data());
+            Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getLocalSize(),state_block_pairs[i].second->getLocalSize());
+            covariance_->GetCovarianceBlockInTangentSpace(double_pairs[i].first, double_pairs[i].second, cov.data());
             wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
-            //std::cout << "getted covariance " << std::endl << cov << std::endl;
+            // std::cout << "covariance got from st_list: " << std::endl << cov << std::endl;
         }
     else
         std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index d12f491de7ce0895ca301498454e7f3d451bbd4e..456367a2c67b1c98ca91e9631c3f3da47751882a 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -247,7 +247,7 @@ int main(int argc, char** argv)
     odom_trajectory.head(3) = ground_truth_pose;
 
     // Origin Key Frame
-    FrameBasePtr origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts);
+    FrameBasePtr origin_frame = problem.createFrame(KEY, ground_truth_pose, ts);
 
     // Prior covariance
     CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1);
diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp
index 136ba285e0d9fb2c62b4801dc75e597dcdd257ff..62ad277f98576b1bf17e17c7515d8dfb428c93be 100644
--- a/src/examples/test_ceres_2_lasers_polylines.cpp
+++ b/src/examples/test_ceres_2_lasers_polylines.cpp
@@ -254,7 +254,7 @@ int main(int argc, char** argv)
     odom_trajectory.head(3) = ground_truth_pose;
 
     // Origin Key Frame
-    FrameBasePtr origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts);
+    FrameBasePtr origin_frame = problem.createFrame(KEY, ground_truth_pose, ts);
 
     // Prior covariance
     CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1);
diff --git a/src/examples/test_factor_imu.cpp b/src/examples/test_factor_imu.cpp
index 3d0483e9647f9e3203ab3bb89036de55e0f9adc9..4040194b382e9db0698b2a5d8acf0f852af1286b 100644
--- a/src/examples/test_factor_imu.cpp
+++ b/src/examples/test_factor_imu.cpp
@@ -80,7 +80,7 @@ int main(int argc, char** argv)
     //create FrameIMU
     ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
     state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
-    FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec);
+    FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
     wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
 
         //create a feature
@@ -141,7 +141,7 @@ int main(int argc, char** argv)
     //create FrameIMU
     ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
     state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
-    FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec);
+    FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
     wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
 
         //create a feature
@@ -199,7 +199,7 @@ int main(int argc, char** argv)
         //create FrameIMU
     ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
     state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
-    FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec);
+    FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
     wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
 
         //create a feature
diff --git a/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp
index be7df536e971273559252aeff8dc51bd808bb14b..8baee2c1e75af6ab5a8841e85b082d8f7cdd20c9 100644
--- a/src/examples/test_processor_tracker_landmark_image.cpp
+++ b/src/examples/test_processor_tracker_landmark_image.cpp
@@ -98,7 +98,7 @@ int main(int argc, char** argv)
     //=====================================================
     // Origin Key Frame is fixed
     TimeStamp t = 0;
-    FrameBasePtr origin_frame = problem->emplaceFrame(KEY_FRAME, (Vector7s()<<1,0,0,0,0,0,1).finished(), t);
+    FrameBasePtr origin_frame = problem->emplaceFrame(KEY, (Vector7s()<<1,0,0,0,0,0,1).finished(), t);
     problem->getProcessorMotion()->setOrigin(origin_frame);
     origin_frame->fix();
 
diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
index 6bd1805dc78a0c65bd27ca022fbb46bd272cf6d4..f07a2c1ce3da6eb7168571d3ec1377ffa8b0c388 100644
--- a/src/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -97,11 +97,11 @@ int main(int argc, char** argv)
     /* 1 */
     ProblemPtr problem = Problem::create("PO 3D");
     // One anchor frame to define the lmk, and a copy to make a factor
-    FrameBasePtr kf_1 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
-    FrameBasePtr kf_2 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
+    FrameBasePtr kf_1 = problem->emplaceFrame(KEY,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
+    FrameBasePtr kf_2 = problem->emplaceFrame(KEY,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
     // and two other frames to observe the lmk
-    FrameBasePtr kf_3 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0));
-    FrameBasePtr kf_4 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0));
+    FrameBasePtr kf_3 = problem->emplaceFrame(KEY,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0));
+    FrameBasePtr kf_4 = problem->emplaceFrame(KEY,(Vector7s()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0));
 
     kf_1->fix();
     kf_2->fix();
diff --git a/src/examples/test_sort_keyframes.cpp b/src/examples/test_sort_keyframes.cpp
index 56982c9dc7c6fda44b594a04f92a33f911b8ca49..036ed1250c8dea7c1aadf10e0c437eda243e5ecf 100644
--- a/src/examples/test_sort_keyframes.cpp
+++ b/src/examples/test_sort_keyframes.cpp
@@ -31,22 +31,22 @@ int main()
 {
     ProblemPtr problem_ptr = Problem::create(FRM_PO_2D);
 
-    problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.1));
-    FrameBasePtr frm2 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.2));
-    FrameBasePtr frm3 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.3));
-    problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.4));
-    FrameBasePtr frm5 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.5));
-    FrameBasePtr frm6 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.6));
+    problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.1));
+    FrameBasePtr frm2 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.2));
+    FrameBasePtr frm3 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.3));
+    problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.4));
+    FrameBasePtr frm5 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.5));
+    FrameBasePtr frm6 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.6));
 
     printFrames(problem_ptr);
 
     std::cout << std::endl << "Frame " << frm5->id() << " set KEY" << std::endl;
-    frm5->setKey();
+    frm5->setEstimated();
 
     printFrames(problem_ptr);
 
     std::cout << std::endl << "Frame " << frm2->id() << " set KEY" << std::endl;
-    frm2->setKey();
+    frm2->setEstimated();
 
     printFrames(problem_ptr);
 
@@ -56,21 +56,21 @@ int main()
     printFrames(problem_ptr);
 
     std::cout << std::endl << "Frame " << frm3->id() << " set KEY" << std::endl;
-    frm3->setKey();
+    frm3->setEstimated();
 
     printFrames(problem_ptr);
 
     std::cout << std::endl << "Frame " << frm6->id() << " set KEY" << std::endl;
-    frm6->setKey();
+    frm6->setEstimated();
 
     printFrames(problem_ptr);
 
-    FrameBasePtr frm7 = problem_ptr->emplaceFrame(KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.7));
+    FrameBasePtr frm7 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXs::Zero(3), TimeStamp(0.7));
     std::cout << std::endl << "created Key Frame " << frm7->id() << " TS: " << 0.7 << std::endl;
 
     printFrames(problem_ptr);
 
-    FrameBasePtr frm8 = problem_ptr->emplaceFrame(KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.35));
+    FrameBasePtr frm8 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXs::Zero(3), TimeStamp(0.35));
     std::cout << std::endl << "created Key Frame " << frm8->id() << " TS: " << 0.35 << std::endl;
 
     printFrames(problem_ptr);
diff --git a/src/examples/test_sparsification.cpp b/src/examples/test_sparsification.cpp
index 7b9e85c085a04dc401097db87be3dcc8ea25e17d..03d214a3d09c8a624129243f181aed1dd722b5a9 100644
--- a/src/examples/test_sparsification.cpp
+++ b/src/examples/test_sparsification.cpp
@@ -216,7 +216,7 @@ int main(int argc, char** argv)
 
 	// ------------------------ START EXPERIMENT ------------------------
 	// First frame FIXED
-	last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY_FRAME, Eigen::Vector3s::Zero(),TimeStamp(0));
+	last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, Eigen::Vector3s::Zero(),TimeStamp(0));
 	last_frame_ptr->fix();
 	bl_problem_ptr->print(4, true, false, true);
 
@@ -238,7 +238,7 @@ int main(int argc, char** argv)
 				Eigen::Vector3s from_pose = frame_from_ptr->getState();
 				R.topLeftCorner(2,2) = Eigen::Rotation2Ds(from_pose(2)).matrix();
 				Eigen::Vector3s new_frame_pose = from_pose + R*meas;
-				last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY_FRAME, new_frame_pose, TimeStamp(double(edge_to)));
+				last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, new_frame_pose, TimeStamp(double(edge_to)));
 
 				frame_to_ptr = last_frame_ptr;
 
diff --git a/src/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp
index 1113af8b7f7e9bd40033ed60e182cc2f485eee99..40171695f14ac3ef2aeee7091ca53e8de50a2a90 100644
--- a/src/examples/test_wolf_imported_graph.cpp
+++ b/src/examples/test_wolf_imported_graph.cpp
@@ -130,8 +130,8 @@ int main(int argc, char** argv)
                     bNum.clear();
 
                     // add frame to problem
-                    FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
-                    FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+                    FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+                    FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
                     wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
                     wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
                     // store
diff --git a/src/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp
index f3ed29007190ada48bba1758a1ac4e57843ff423..b4d9ac23316dc5450b269d99ce45cbb0f7c0e2f0 100644
--- a/src/examples/test_wolf_prunning.cpp
+++ b/src/examples/test_wolf_prunning.cpp
@@ -148,8 +148,8 @@ int main(int argc, char** argv)
                     bNum.clear();
 
                     // add frame to problem
-                    FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
-                    FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+                    FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+                    FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
                     wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
                     wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
                     // store
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index cb7f089c1f21adad761f5fd7c13f0eee786b6741..10c75405b377474905b1040251794067d7df115c 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -16,7 +16,7 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
             trajectory_ptr_(),
             state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
             frame_id_(++frame_id_count_),
-            type_(NON_KEY_FRAME),
+            type_(NON_ESTIMATED),
             time_stamp_(_ts)
 {
     state_block_vec_[0] = _p_ptr;
@@ -39,8 +39,6 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
                 
 FrameBase::~FrameBase()
 {
-    if ( isKey() )
-        removeStateBlocks();
 }
 
 void FrameBase::remove()
@@ -68,11 +66,12 @@ void FrameBase::remove()
         }
 
         // Remove Frame State Blocks
-        if ( isKey() )
+        if ( isKeyOrAux() )
             removeStateBlocks();
 
-        if (getTrajectory()->getLastKeyFrame()->id() == this_F->id())
-            getTrajectory()->setLastKeyFrame(getTrajectory()->findLastKeyFrame());
+
+        if (getTrajectory()->getLastKeyFrame()->id() == this_F->id() || getTrajectory()->getLastKeyOrAuxFrame()->id() == this_F->id())
+            getTrajectory()->updateLastFrames();
 
 //        std::cout << "Removed       F" << id() << std::endl;
     }
@@ -81,7 +80,7 @@ void FrameBase::remove()
 void FrameBase::setTimeStamp(const TimeStamp& _ts)
 {
     time_stamp_ = _ts;
-    if (isKey() && getTrajectory() != nullptr)
+    if (isKeyOrAux() && getTrajectory() != nullptr)
         getTrajectory()->sortFrame(shared_from_this());
 }
 
@@ -113,18 +112,25 @@ void FrameBase::removeStateBlocks()
 
 void FrameBase::setKey()
 {
-    if (type_ != KEY_FRAME)
-    {
-        type_ = KEY_FRAME;
+    // register if previously not estimated
+    if (!isKeyOrAux())
         registerNewStateBlocks();
 
-        if (getTrajectory()->getLastKeyFrame() == nullptr || getTrajectory()->getLastKeyFrame()->getTimeStamp() < time_stamp_)
-            getTrajectory()->setLastKeyFrame(shared_from_this());
+    // WOLF_DEBUG("Set Key", this->id());
+    type_ = KEY;
+    getTrajectory()->sortFrame(shared_from_this());
+    getTrajectory()->updateLastFrames();
+}
 
-        getTrajectory()->sortFrame(shared_from_this());
+void FrameBase::setAux()
+{
+    if (!isKeyOrAux())
+        registerNewStateBlocks();
 
-//        WOLF_DEBUG("Set KF", this->id());
-    }
+    // WOLF_DEBUG("Set Auxiliary", this->id());
+    type_ = AUXILIARY;
+    getTrajectory()->sortFrame(shared_from_this());
+    getTrajectory()->updateLastFrames();
 }
 
 void FrameBase::fix()
@@ -170,7 +176,7 @@ void FrameBase::setState(const Eigen::VectorXs& _state)
     for (StateBlockPtr sb : state_block_vec_)
         if (sb && (index < _st_size))
         {
-            sb->setState(_state.segment(index, sb->getSize()), isKey());
+            sb->setState(_state.segment(index, sb->getSize()), isKeyOrAux()); // do not notify if state block is not estimated by the solver
             index += sb->getSize();
         }
 }
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 34f37364291cc3d0710fb78a890799df87218b1f..17c30ca6bb3b07708c331f85a283b3bc87a7cea7 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -251,8 +251,8 @@ void Problem::getCurrentState(Eigen::VectorXs& state)
 {
     if (processor_motion_ptr_ != nullptr)
         processor_motion_ptr_->getCurrentState(state);
-    else if (trajectory_ptr_->getLastKeyFrame() != nullptr)
-        trajectory_ptr_->getLastKeyFrame()->getState(state);
+    else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
+        trajectory_ptr_->getLastKeyOrAuxFrame()->getState(state);
     else
         state = zeroState();
 }
@@ -264,10 +264,10 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
         processor_motion_ptr_->getCurrentState(state);
         processor_motion_ptr_->getCurrentTimeStamp(ts);
     }
-    else if (trajectory_ptr_->getLastKeyFrame() != nullptr)
+    else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
     {
-        trajectory_ptr_->getLastKeyFrame()->getTimeStamp(ts);
-        trajectory_ptr_->getLastKeyFrame()->getState(state);
+        trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(ts);
+        trajectory_ptr_->getLastKeyOrAuxFrame()->getState(state);
     }
     else
         state = zeroState();
@@ -278,7 +278,7 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& state)
     // try to get the state from processor_motion if any, otherwise...
     if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, state))
     {
-        FrameBasePtr closest_frame = trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
+        FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
         if (closest_frame != nullptr)
             closest_frame->getState(state);
         else
@@ -348,6 +348,30 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
                 processor->keyFrameCallback(_keyframe_ptr, _time_tolerance);
 }
 
+bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr)
+{
+    // This should implement a black list of processors that have forbidden auxiliary frame creation
+    // This decision is made at problem level, not at processor configuration level.
+    // If you want to configure a processor for not creating auxiliary frames, see Processor::voting_active_ and its accessors.
+
+    // Currently allowing all processors to vote:
+    return true;
+}
+
+void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _processor_ptr, const Scalar& _time_tolerance)
+{
+    if (_processor_ptr)
+    {
+        WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
+    }
+    else
+    {
+        WOLF_DEBUG("External callback: AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
+    }
+
+    processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
+}
+
 LandmarkBasePtr Problem::addLandmark(LandmarkBasePtr _lmk_ptr)
 {
     getMap()->addLandmark(_lmk_ptr);
@@ -470,8 +494,8 @@ void Problem::clearCovariance()
 
 void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXs& _cov)
 {
-    assert(_state1->getSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
-    assert(_state2->getSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
+    assert(_state1->getLocalSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
+    assert(_state2->getLocalSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
 
     std::lock_guard<std::mutex> lock(mut_covariances_);
     covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] = _cov;
@@ -479,8 +503,8 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, c
 
 void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _cov)
 {
-    assert(_state1->getSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
-    assert(_state1->getSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
+    assert(_state1->getLocalSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
+    assert(_state1->getLocalSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
 
     std::lock_guard<std::mutex> lock(mut_covariances_);
     covariances_[std::make_pair(_state1, _state1)] = _cov;
@@ -492,23 +516,23 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
     //std::cout << "entire cov to be filled:" << std::endl << _cov << std::endl;
     //std::cout << "_row " << _row << std::endl;
     //std::cout << "_col " << _col << std::endl;
-    //std::cout << "_state1 size: " << _state1->getSize() << std::endl;
-    //std::cout << "_state2 size: " << _state2->getSize() << std::endl;
-    //std::cout << "part of cov to be filled:" << std::endl <<  _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) << std::endl;
+    //std::cout << "_state1 tangent space size: " << _state1->getLocalSize() << std::endl;
+    //std::cout << "_state2 tangent space size: " << _state2->getLocalSize() << std::endl;
+    //std::cout << "part of cov to be filled:" << std::endl <<  _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) << std::endl;
     //if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
     //    std::cout << "stored cov" << std::endl << covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] << std::endl;
     //else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end())
     //    std::cout << "stored cov" << std::endl << covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)].transpose() << std::endl;
 
-    assert(_row + _state1->getSize() <= _cov.rows() && _col + _state2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
+    assert(_row + _state1->getLocalSize() <= _cov.rows() && _col + _state2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
 
     std::lock_guard<std::mutex> lock(mut_covariances_);
 
     if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
-        _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) =
+        _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
                 covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)];
     else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end())
-       _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) =
+       _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
                 covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)].transpose();
     else
     {
@@ -535,23 +559,23 @@ bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx
             // search st1 & st2
             if (covariances_.find(pair_12) != covariances_.end())
             {
-                assert(_sb_2_idx[sb1] + sb1->getSize() <= _cov.rows() &&
-                       _sb_2_idx[sb2] + sb2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
-                assert(_sb_2_idx[sb2] + sb2->getSize() <= _cov.rows() &&
-                       _sb_2_idx[sb1] + sb1->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
+                assert(_sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.rows() &&
+                       _sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
+                assert(_sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.rows() &&
+                       _sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
 
-                _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getSize(), sb2->getSize()) = covariances_[pair_12];
-                _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getSize(), sb1->getSize()) = covariances_[pair_12].transpose();
+                _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_[pair_12];
+                _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_[pair_12].transpose();
             }
             else if (covariances_.find(pair_21) != covariances_.end())
             {
-                assert(_sb_2_idx[sb1] + sb1->getSize() <= _cov.rows() &&
-                       _sb_2_idx[sb2] + sb2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
-                assert(_sb_2_idx[sb2] + sb2->getSize() <= _cov.rows() &&
-                       _sb_2_idx[sb1] + sb1->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
+                assert(_sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.rows() &&
+                       _sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
+                assert(_sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.rows() &&
+                       _sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
 
-                _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getSize(), sb2->getSize()) = covariances_[pair_21].transpose();
-                _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getSize(), sb1->getSize()) = covariances_[pair_21];
+                _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_[pair_21].transpose();
+                _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_[pair_21];
             }
             else
                 return false;
@@ -576,7 +600,7 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs&
     SizeEigen sz = 0;
     for (const auto& sb : state_bloc_vec)
         if (sb)
-            sz += sb->getSize();
+            sz += sb->getLocalSize();
 
     // resizing
     _covariance = Eigen::MatrixXs(sz, sz);
@@ -592,10 +616,10 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs&
                 if (sb_j)
                 {
                     success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
-                    j += sb_j->getSize();
+                    j += sb_j->getLocalSize();
                 }
             }
-            i += sb_i->getSize();
+            i += sb_i->getLocalSize();
         }
     }
     return success;
@@ -607,6 +631,12 @@ bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXs& cov)
     return getFrameCovariance(frm, cov);
 }
 
+bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXs& cov)
+{
+    FrameBasePtr frm = getLastKeyOrAuxFrame();
+    return getFrameCovariance(frm, cov);
+}
+
 bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance)
 {
     bool success(true);
@@ -618,7 +648,7 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
     SizeEigen sz = 0;
     for (const auto& sb : state_bloc_vec)
         if (sb)
-            sz += sb->getSize();
+            sz += sb->getLocalSize();
 
     // resizing
     _covariance = Eigen::MatrixXs(sz, sz);
@@ -635,10 +665,10 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
                 if (sb_j)
                 {
                     success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
-                    j += sb_j->getSize();
+                    j += sb_j->getLocalSize();
                 }
             }
-            i += sb_i->getSize();
+            i += sb_i->getLocalSize();
         }
     }
     return success;
@@ -659,22 +689,37 @@ HardwareBasePtr Problem::getHardware()
     return hardware_ptr_;
 }
 
-FrameBasePtr Problem::getLastFrame()
+FrameBasePtr Problem::getLastFrame() const
 {
     return trajectory_ptr_->getLastFrame();
 }
 
-FrameBasePtr Problem::getLastKeyFrame()
+FrameBasePtr Problem::getLastKeyFrame() const
 {
     return trajectory_ptr_->getLastKeyFrame();
 }
 
+FrameBasePtr Problem::getLastKeyOrAuxFrame() const
+{
+    return trajectory_ptr_->getLastKeyOrAuxFrame();
+}
+
+FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
+{
+    return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
+}
+
+FrameBasePtr Problem::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const
+{
+    return trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+}
+
 FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen::MatrixXs& _prior_cov, const TimeStamp& _ts, const Scalar _time_tolerance)
 {
     if ( ! prior_is_set_ )
     {
         // Create origin frame
-        FrameBasePtr origin_keyframe = emplaceFrame(KEY_FRAME, _prior_state, _ts);
+        FrameBasePtr origin_keyframe = emplaceFrame(KEY, _prior_state, _ts);
 
         // create origin capture with the given state as data
         // Capture fix only takes 3D position and Quaternion orientation
@@ -783,10 +828,10 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
                         std::cout << "    pm" << p->id() << " " << p->getType() << endl;
                         ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
                         if (pm->getOrigin())
-                            cout << "      o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKey() ? "  KF" : "  F")
+                            cout << "      o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? "  KF" : "  AuxF" ) : "  F")
                             << pm->getOrigin()->getFrame()->id() << endl;
                         if (pm->getLast())
-                            cout << "      l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKey() ? "  KF" : "  F")
+                            cout << "      l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? "  KF" : " AuxF") : "  F")
                             << pm->getLast()->getFrame()->id() << endl;
                         if (pm->getIncoming())
                             cout << "      i: C" << pm->getIncoming()->id() << endl;
@@ -801,14 +846,14 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
 //                            if (ptt)
 //                            {
 //                                if (ptt->getPrevOrigin())
-//                                    cout << "      p: C" << ptt->getPrevOrigin()->id() << " - " << (ptt->getPrevOrigin()->getFrame()->isKey() ? "  KF" : "  F")
+//                                    cout << "      p: C" << ptt->getPrevOrigin()->id() << " - " << (ptt->getPrevOrigin()->getFrame()->isEstimated() ? (ptt->getPrevOrigin()->getFrame()->isKey() ? "  KF" : " AuxF") : "  F")
 //                                    << ptt->getPrevOrigin()->getFrame()->id() << endl;
 //                            }
                             if (pt->getOrigin())
-                                cout << "      o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKey() ? "  KF" : "  F")
+                                cout << "      o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? "  KF" : " AuxF") : "  F")
                                 << pt->getOrigin()->getFrame()->id() << endl;
                             if (pt->getLast())
-                                cout << "      l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKey() ? "  KF" : "  F")
+                                cout << "      l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? "  KF" : " AuxF") : "  F")
                                 << pt->getLast()->getFrame()->id() << endl;
                             if (pt->getIncoming())
                                 cout << "      i: C" << pt->getIncoming()->id() << endl;
@@ -824,7 +869,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
         // Frames =======================================================================================
         for (auto F : getTrajectory()->getFrameList())
         {
-            cout << (F->isKey() ? "  KF" : "  F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C  " : "");
+            cout << (F->isKeyOrAux() ? (F->isKey() ? "  KF" : " AuxF") : "  F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C  " : "");
             if (constr_by)
             {
                 cout << "  <-- ";
@@ -981,11 +1026,6 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
     cout << endl;
 }
 
-FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
-{
-    return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
-}
-
 bool Problem::check(int verbose_level)
 {
     using std::cout;
@@ -1065,7 +1105,7 @@ bool Problem::check(int verbose_level)
     {
         if (verbose_level > 0)
         {
-            cout << (F->isKey() ? "  KF" : "  F") << F->id() << " @ " << F.get() << endl;
+            cout << (F->isKeyOrAux() ? (F->isKey() ? "  KF" : " EF") : "  F") << F->id() << " @ " << F.get() << endl;
             cout << "    -> P @ " << F->getProblem().get() << endl;
             cout << "    -> T @ " << F->getTrajectory().get() << endl;
             for (auto sb : F->getStateBlockVec())
diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp
index 2dfa2cd7de6b14c3aea08eab02782d30235c15df..b192163cc10e3987640d2ee8b8e0025e86d5d758 100644
--- a/src/processor/processor_IMU.cpp
+++ b/src/processor/processor_IMU.cpp
@@ -35,8 +35,6 @@ ProcessorBasePtr ProcessorIMU::create(const std::string& _unique_name, const Pro
 
 bool ProcessorIMU::voteForKeyFrame()
 {
-    if(!isVotingActive())
-        return false;
     // time span
     if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_motion_IMU_->max_time_span)
     {
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 0679909164d86a96ac609a930619f35ff6a97fe3..77e00b83580074483b286457655c8b08edf534d3 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -26,9 +26,14 @@ bool ProcessorBase::permittedKeyFrame()
     return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this());
 }
 
+bool ProcessorBase::permittedAuxFrame()
+{
+    return isVotingAuxActive() && getProblem()->permitAuxFrame(shared_from_this());
+}
+
 FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr)
 {
-    std::cout << "Making " << (_type == KEY_FRAME? "key-" : "") << "frame" << std::endl;
+    std::cout << "Making " << (_type == KEY ? "key-" : (_type == AUXILIARY ? "aux-" : "")) << "frame" << std::endl;
 
     FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(_type, _capture_ptr->getTimeStamp());
     new_frame_ptr->addCapture(_capture_ptr);
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index ef5cbb1cd61cfe586cfcda1b573ddfc7056142ae..755b6c95091c88ad7c5f3362caa3950714046d0f 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -193,7 +193,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
     last_ptr_->getFrame()->setTimeStamp(getCurrentTimeStamp());
     last_ptr_->getFrame()->setState(getCurrentState());
 
-    if (voteForKeyFrame() && permittedKeyFrame())
+    if (permittedKeyFrame() && voteForKeyFrame())
     {
         // Set the frame of last_ptr as key
         auto key_frame_ptr = last_ptr_->getFrame();
@@ -206,7 +206,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
         auto fac_ptr = emplaceFactor(key_feature_ptr, origin_ptr_);
 
         // create a new frame
-        auto new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME,
+        auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
                                                         getCurrentState(),
                                                         getCurrentTimeStamp());
         // create a new capture
@@ -288,7 +288,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
 
 FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin)
 {
-    FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY_FRAME, _x_origin, _ts_origin);
+    FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY, _x_origin, _ts_origin);
     setOrigin(key_frame_ptr);
 
     return key_frame_ptr;
@@ -314,7 +314,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
 
     // ---------- LAST ----------
     // Make non-key-frame for last Capture
-    auto new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME,
+    auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
                                                     _origin_frame->getState(),
                                                     _origin_frame->getTimeStamp());
     // emplace (emtpy) last Capture
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index afdc9446a7f3dc0cc54c8453bc07c190a821f90e..4d58f52386a281b5eee02beb4049f973cc9bad63 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -75,7 +75,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
         }
         case FIRST_TIME_WITHOUT_PACK :
         {
-            FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY_FRAME, incoming_ptr_->getTimeStamp());
+            FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY, incoming_ptr_->getTimeStamp());
             kfrm->addCapture(incoming_ptr_);
 
             // Process info
@@ -101,7 +101,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
         } // @suppress("No break at end of case")
         case SECOND_TIME_WITHOUT_PACK :
         {
-            FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
+            FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
             frm->addCapture(incoming_ptr_);
 
             // We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
@@ -137,7 +137,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
             pack->key_frame->addCapture(last_ptr_);
 
             // Create new frame
-            FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
+            FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
             frm->addCapture(incoming_ptr_);
 
             // Detect new Features, initialize Landmarks, create Factors, ...
@@ -174,15 +174,12 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
                 last_ptr_->getFrame()->setKey();
 
                 // make F; append incoming to new F
-                FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
+                FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
                 frm->addCapture(incoming_ptr_);
 
                 // process
                 processNew(params_tracker_->max_new_features);
 
-                //                // Set key
-                //                last_ptr_->getFrame()->setKey();
-                //
                 // Set state to the keyframe
                 last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
 
@@ -199,6 +196,38 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
                 incoming_ptr_ = nullptr;
 
             }
+            /* TODO: create an auxiliary frame
+            else if (voteForAuxFrame() && permittedAuxFrame())
+            {
+                // We create an Auxiliary Frame
+
+                // set AuxF on last
+                last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
+                last_ptr_->getFrame()->setAuxiliary();
+
+                // make F; append incoming to new F
+                FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+                frm->addCapture(incoming_ptr_);
+
+                // Set state to the keyframe
+                last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
+
+                // Establish factors
+                establishFactors();
+
+                // Call the new auxframe callback in order to let the ProcessorMotion to establish their factors
+                getProblem()->auxFrameCallback(last_ptr_->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_tracker_->time_tolerance);
+
+                // Advance this
+                last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
+                // do not remove! last_ptr_->remove();
+                incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
+
+                // Update pointers
+                advanceDerived();
+                last_ptr_   = incoming_ptr_;
+                incoming_ptr_ = nullptr;
+            }*/
             else
             {
                 // We do not create a KF
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 086afab738a5b66cebdde25cdda862606d28b924..e28e18cdea2cc5c8cfd88d55a2d311d81a22a6b0 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -6,7 +6,8 @@ namespace wolf {
 TrajectoryBase::TrajectoryBase(const std::string& _frame_structure) :
     NodeBase("TRAJECTORY", "Base"),
     frame_structure_(_frame_structure),
-    last_key_frame_ptr_(nullptr)
+    last_key_frame_ptr_(nullptr),
+    last_key_or_aux_frame_ptr_(nullptr)
 {
 //    WOLF_DEBUG("constructed T");
 }
@@ -22,17 +23,19 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
     _frame_ptr->setTrajectory(shared_from_this());
     _frame_ptr->setProblem(getProblem());
 
-    if (_frame_ptr->isKey())
+    // add to list
+    frame_list_.push_back(_frame_ptr);
+
+    if (_frame_ptr->isKeyOrAux())
     {
+        // register state blocks
         _frame_ptr->registerNewStateBlocks();
-        if (last_key_frame_ptr_ == nullptr || last_key_frame_ptr_->getTimeStamp() < _frame_ptr->getTimeStamp())
-            last_key_frame_ptr_ = _frame_ptr;
 
-        frame_list_.insert(computeFrameOrder(_frame_ptr), _frame_ptr);
-    }
-    else
-    {
-        frame_list_.push_back(_frame_ptr);
+        // sort
+        sortFrame(_frame_ptr);
+
+        // update last_estimated and last_key
+        updateLastFrames();
     }
 
     return _frame_ptr;
@@ -47,7 +50,6 @@ void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
 void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
 {
     moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr));
-    //    last_key_frame_ptr_ = findLastKeyFrame(); // done in moveFrame() just above
 }
 
 void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place)
@@ -56,34 +58,46 @@ void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place)
     {
         frame_list_.remove(_frm_ptr);
         frame_list_.insert(_place, _frm_ptr);
-        last_key_frame_ptr_ = findLastKeyFrame();
+        updateLastFrames();
     }
 }
 
 FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)
 {
     for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
-        if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKey() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp())
+        if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKeyOrAux() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp())
             return frm_rit.base();
     return getFrameList().begin();
 }
 
-FrameBasePtr TrajectoryBase::findLastKeyFrame()
+void TrajectoryBase::updateLastFrames()
 {
-    // NOTE: Assumes keyframes are sorted by timestamp
-    for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); ++frm_rit)
-        if ((*frm_rit)->isKey())
-            return (*frm_rit);
+    bool last_estimated_set = false;
 
-    return nullptr;
+    // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
+    for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); ++frm_rit)
+        if ((*frm_rit)->isKeyOrAux())
+        {
+            if (!last_estimated_set)
+            {
+                last_key_or_aux_frame_ptr_ = (*frm_rit);
+                last_estimated_set = true;
+            }
+            if ((*frm_rit)->isKey())
+            {
+                last_key_frame_ptr_ = (*frm_rit);
+                break;
+            }
+        }
 }
 
-FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
+FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
 {
+    // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
     FrameBasePtr closest_kf = nullptr;
     Scalar min_dt = 1e9;
 
-    for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
+    for (auto frm_rit = frame_list_.rbegin(); frm_rit != frame_list_.rend(); frm_rit++)
         if ((*frm_rit)->isKey())
         {
             Scalar dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
@@ -98,4 +112,25 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
     return closest_kf;
 }
 
+FrameBasePtr TrajectoryBase::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const
+{
+    // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
+    FrameBasePtr closest_kf = nullptr;
+    Scalar min_dt = 1e9;
+
+    for (auto frm_rit = frame_list_.rbegin(); frm_rit != frame_list_.rend(); frm_rit++)
+        if ((*frm_rit)->isKeyOrAux())
+        {
+            Scalar dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
+            if (dt < min_dt)
+            {
+                min_dt = dt;
+                closest_kf = *frm_rit;
+            }
+            else
+                break;
+        }
+    return closest_kf;
+}
+
 } // namespace wolf
diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp
index d2657e4156cf6991d3342505cb558712837cb344..253c7da0cba0aebdcbff3d30521b9511987d075f 100644
--- a/test/gtest_IMU.cpp
+++ b/test/gtest_IMU.cpp
@@ -480,7 +480,7 @@ class Process_Factor_IMU : public testing::Test
         virtual void buildProblem()
         {
             // ===================================== SET KF in Wolf tree
-            FrameBasePtr KF = problem->emplaceFrame(KEY_FRAME, x1_exact, t);
+            FrameBasePtr KF = problem->emplaceFrame(KEY, x1_exact, t);
 
             // ===================================== IMU CALLBACK
             problem->keyFrameCallback(KF, nullptr, dt/2);
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 57efea34c4fd4a4786dedf1f33d8d29c4e125705..3c5f2502cbce65184e2d63d59b85145776a86f5c 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -318,7 +318,7 @@ TEST(CeresManager, AddFactor)
     CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
     CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
     FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
     FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
@@ -340,7 +340,7 @@ TEST(CeresManager, DoubleAddFactor)
     CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
     CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
     FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
     FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
@@ -365,7 +365,7 @@ TEST(CeresManager, RemoveFactor)
     CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
     CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
     FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
     FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
@@ -393,7 +393,7 @@ TEST(CeresManager, AddRemoveFactor)
     CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
     CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
     FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
     FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
@@ -420,7 +420,7 @@ TEST(CeresManager, DoubleRemoveFactor)
     CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
     CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
     FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
     FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
@@ -547,7 +547,7 @@ TEST(CeresManager, FactorsRemoveLocalParam)
     CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
 
     // Create (and add) 2 factors quaternion
-    FrameBasePtr                    F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    FrameBasePtr                    F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
     CaptureBasePtr                  C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
     FeatureBasePtr                  f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
     FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
@@ -589,7 +589,7 @@ TEST(CeresManager, FactorsUpdateLocalParam)
     CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
 
     // Create (and add) 2 factors quaternion
-    FrameBasePtr                    F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    FrameBasePtr                    F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
     CaptureBasePtr                  C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
     FeatureBasePtr                  f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
     FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 204c6ce633a36380d46eacbc65d66abe1a7747fe..fd9650de70d970b9336d62e8471f3c5a090c0221 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -35,7 +35,7 @@ ProblemPtr problem_ptr = Problem::create("POV 3D");
 CeresManager ceres_mgr(problem_ptr);
 
 // Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
 
 // Capture, feature and factor
 CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS", 0, nullptr, pose10, 10, 9, nullptr));
diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp
index 21c7c912f801b39adcf3b5c75e56ca03d08e83c2..b9c03ab04dcafa19f9fd850a8b72508e25ba02b3 100644
--- a/test/gtest_factor_autodiff_distance_3D.cpp
+++ b/test/gtest_factor_autodiff_distance_3D.cpp
@@ -55,9 +55,9 @@ class FactorAutodiffDistance3D_Test : public testing::Test
             problem = Problem::create("PO 3D");
             ceres_manager = std::make_shared<CeresManager>(problem);
 
-            F1 = problem->emplaceFrame        (KEY_FRAME, pose1, 1.0);
+            F1 = problem->emplaceFrame        (KEY, pose1, 1.0);
 
-            F2 = problem->emplaceFrame        (KEY_FRAME, pose2, 2.0);
+            F2 = problem->emplaceFrame        (KEY, pose2, 2.0);
             C2 = std::make_shared<CaptureBase>("Base", 1.0);
             F2->addCapture(C2);
             f2 = std::make_shared<FeatureBase>("Dist", dist, dist_cov);
diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp
index 98af13f6e834f73bd4bbab7e524cf8928d0467af..78223c6020f9241c9a4faa7d24db5346c06e9c6d 100644
--- a/test/gtest_factor_autodiff_trifocal.cpp
+++ b/test/gtest_factor_autodiff_trifocal.cpp
@@ -133,19 +133,19 @@ class FactorAutodiffTrifocalTest : public testing::Test{
             Vector2s pix(0,0);
             Matrix2s pix_cov; pix_cov.setIdentity();
 
-            F1 = problem->emplaceFrame(KEY_FRAME, pose1, 1.0);
+            F1 = problem->emplaceFrame(KEY, pose1, 1.0);
             I1 = std::make_shared<CaptureImage>(1.0, camera, cv::Mat(2,2,CV_8UC1));
             F1-> addCapture(I1);
             f1 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin
             I1-> addFeature(f1);
 
-            F2 = problem->emplaceFrame(KEY_FRAME, pose2, 2.0);
+            F2 = problem->emplaceFrame(KEY, pose2, 2.0);
             I2 = std::make_shared<CaptureImage>(2.0, camera, cv::Mat(2,2,CV_8UC1));
             F2-> addCapture(I2);
             f2 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin
             I2-> addFeature(f2);
 
-            F3 = problem->emplaceFrame(KEY_FRAME, pose3, 3.0);
+            F3 = problem->emplaceFrame(KEY, pose3, 3.0);
             I3 = std::make_shared<CaptureImage>(3.0, camera, cv::Mat(2,2,CV_8UC1));
             F3-> addCapture(I3);
             f3 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin
diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp
index c3f767d56ea04682d2e33651c4e4e1a89f865585..2b8a6d20160d9464ffa7b48ff9044b66b03d50d9 100644
--- a/test/gtest_factor_odom_3D.cpp
+++ b/test/gtest_factor_odom_3D.cpp
@@ -37,8 +37,8 @@ ProblemPtr problem_ptr = Problem::create("PO 3D");
 CeresManager ceres_mgr(problem_ptr);
 
 // Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(), TimeStamp(0));
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY_FRAME, delta, TimeStamp(1));
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, delta, TimeStamp(1));
 
 // Capture, feature and factor from frm1 to frm0
 CaptureBasePtr cap1 = frm1->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 1, nullptr, delta, 7, 6, nullptr));
diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp
index d7bce0c84f734613f1a2f143bebf2690897149f2..d07b564a3caeed5dd34bb9e107776bc769f3d85d 100644
--- a/test/gtest_factor_pose_2D.cpp
+++ b/test/gtest_factor_pose_2D.cpp
@@ -30,7 +30,7 @@ ProblemPtr problem = Problem::create("PO 2D");
 CeresManager ceres_mgr(problem);
 
 // Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0));
 
 // Capture, feature and factor from frm1 to frm0
 CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 2D", 0, nullptr, pose, 3, 3, nullptr));
diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp
index 0af4c3c406f02c9c87cabb35b5e43fd7137fe28b..620e389c6b5a483f84250584cbe6d318e62ec55b 100644
--- a/test/gtest_factor_pose_3D.cpp
+++ b/test/gtest_factor_pose_3D.cpp
@@ -36,7 +36,7 @@ ProblemPtr problem = Problem::create("PO 3D");
 CeresManager ceres_mgr(problem);
 
 // Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0));
 
 // Capture, feature and factor
 CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr));
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index 082e438d0b799e1ed32075f28e6f61b7450da8e0..c9d894e9ff3a8fafdee13087ef7364bf7173277f 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -77,7 +77,7 @@ class FeatureIMU_test : public testing::Test
     //create Frame
         ts = problem->getProcessorMotion()->getBuffer().get().back().ts_;
         state_vec = problem->getProcessorMotion()->getCurrentState();
-   	    last_frame = std::make_shared<FrameBase>(KEY_FRAME, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3)));
+   	    last_frame = std::make_shared<FrameBase>(KEY, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3)));
         problem->getTrajectory()->addFrame(last_frame);
         
     //create a feature
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 9b4c7b6cf882b1f8104fa71ac3d60e12d8580137..9984c258de1026f68debd1cf4ccd072d4cdbd3c5 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -32,6 +32,7 @@ TEST(FrameBase, GettersAndSetters)
     ASSERT_EQ(t, 1);
     ASSERT_FALSE(F->isFixed());
     ASSERT_EQ(F->isKey(), false);
+    ASSERT_EQ(F->isKeyOrAux(), false);
 }
 
 TEST(FrameBase, StateBlocks)
@@ -123,6 +124,7 @@ TEST(FrameBase, LinksToTree)
     // set key
     F1->setKey();
     ASSERT_TRUE(F1->isKey());
+    ASSERT_TRUE(F1->isKeyOrAux());
 
     // Unlink
     F1->unlinkCapture(C);
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 604ef489478f5779ac4b79a14095c37436c81be4..feafb83e98ce51caf43aea88d2a192b00fff78e8 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -131,7 +131,7 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
 
     // KF1 and motion from KF0
     t += dt;
-    FrameBasePtr        F1 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t);
+    FrameBasePtr        F1 = Pr->emplaceFrame(KEY, Vector3s::Zero(), t);
     CaptureBasePtr      C1 = F1->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
     FeatureBasePtr      f1 = C1->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
     FactorBasePtr   c1 = f1->addFactor(std::make_shared<FactorOdom2D>(f1, F0, nullptr));
@@ -139,7 +139,7 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
 
     // KF2 and motion from KF1
     t += dt;
-    FrameBasePtr        F2 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t);
+    FrameBasePtr        F2 = Pr->emplaceFrame(KEY, Vector3s::Zero(), t);
     CaptureBasePtr      C2 = F2->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
     FeatureBasePtr      f2 = C2->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
     FactorBasePtr   c2 = f2->addFactor(std::make_shared<FactorOdom2D>(f2, F1, nullptr));
@@ -404,7 +404,7 @@ TEST(Odom2D, KF_callback)
     std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl;
 
     Vector3s x_split = processor_odom2d->getState(t_split);
-    FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY_FRAME, x_split, t_split);
+    FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY, x_split, t_split);
 
     ASSERT_TRUE(problem->check(0));
     processor_odom2d->keyFrameCallback(keyframe_2, dt/2);
@@ -436,7 +436,7 @@ TEST(Odom2D, KF_callback)
     problem->print(4,1,1,1);
 
     x_split = processor_odom2d->getState(t_split);
-    FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY_FRAME, x_split, t_split);
+    FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY, x_split, t_split);
 
     ASSERT_TRUE(problem->check(0));
     processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 3b0ed8e164900db7f7bbe5e244e7a424f16758c9..99054f035c989ff45fa6bca332266293ac3375fe 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -138,7 +138,7 @@ TEST(Problem, SetOrigin_PO_2D)
     ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
 
     // check that the state is correct
-    ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+    ASSERT_MATRIX_APPROX(x0, P->getCurrentState(), Constants::EPS_SMALL );
 
     // check that we have one frame, one capture, one feature, one factor
     TrajectoryBasePtr T = P->getTrajectory();
@@ -156,8 +156,8 @@ TEST(Problem, SetOrigin_PO_2D)
     ASSERT_FALSE(c->getLandmarkOther());
 
     // check that the Feature measurement and covariance are the ones provided
-    ASSERT_TRUE((x0 - f->getMeasurement()).isMuchSmallerThan(1, Constants::EPS_SMALL));
-    ASSERT_TRUE((P0 - f->getMeasurementCovariance()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+    ASSERT_MATRIX_APPROX(x0, f->getMeasurement(), Constants::EPS_SMALL );
+    ASSERT_MATRIX_APPROX(P0, f->getMeasurementCovariance(), Constants::EPS_SMALL );
 
     //    P->print(4,1,1,1);
 }
@@ -203,9 +203,9 @@ TEST(Problem, emplaceFrame_factory)
 {
     ProblemPtr P = Problem::create("PO 2D");
 
-    FrameBasePtr f0 = P->emplaceFrame("PO 2D",    KEY_FRAME, VectorXs(3),  TimeStamp(0.0));
-    FrameBasePtr f1 = P->emplaceFrame("PO 3D",    KEY_FRAME, VectorXs(7),  TimeStamp(1.0));
-    FrameBasePtr f2 = P->emplaceFrame("POV 3D",   KEY_FRAME, VectorXs(10), TimeStamp(2.0));
+    FrameBasePtr f0 = P->emplaceFrame("PO 2D",    KEY, VectorXs(3),  TimeStamp(0.0));
+    FrameBasePtr f1 = P->emplaceFrame("PO 3D",    KEY, VectorXs(7),  TimeStamp(1.0));
+    FrameBasePtr f2 = P->emplaceFrame("POV 3D",   KEY, VectorXs(10), TimeStamp(2.0));
 
     //    std::cout << "f0: type PO 2D?    "  << f0->getType() << std::endl;
     //    std::cout << "f1: type PO 3D?    "  << f1->getType() << std::endl;
@@ -249,7 +249,7 @@ TEST(Problem, StateBlocks)
     ProcessorBasePtr pm = P->installProcessor("ODOM 3D",            "odom integrator",      "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
 
     // 2 state blocks, estimated
-    auto KF = P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
+    auto KF = P->emplaceFrame("PO 3D", KEY, xs, 0);
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 3 + 2));
 
     // Notifications
@@ -306,22 +306,20 @@ TEST(Problem, Covariances)
 
     // 4 state blocks, estimated
     St->unfixExtrinsics();
-    FrameBasePtr F = P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
+    FrameBasePtr F = P->emplaceFrame("PO 3D", KEY, xs, 0);
 
     // set covariance (they are not computed without a solver)
     P->addCovarianceBlock(F->getP(), Eigen::Matrix3s::Identity());
-    P->addCovarianceBlock(F->getO(), Eigen::Matrix4s::Identity());
-    P->addCovarianceBlock(F->getP(), F->getO(), Eigen::Matrix<Scalar,3,4>::Zero());
+    P->addCovarianceBlock(F->getO(), Eigen::Matrix3s::Identity());
+    P->addCovarianceBlock(F->getP(), F->getO(), Eigen::Matrix3s::Zero());
 
     // get covariance
     Eigen::MatrixXs Cov;
     ASSERT_TRUE(P->getFrameCovariance(F, Cov));
 
-    // FIXME Frame covariance should be 6x6, but it is actually 7x7 (the state of the state blocks, not of the local parametrizations)
-    // JV: The local parameterization projects the covariance to the state size.
-    ASSERT_EQ(Cov.cols() , 7);
-    ASSERT_EQ(Cov.rows() , 7);
-    ASSERT_MATRIX_APPROX(Cov, Eigen::Matrix7s::Identity(), 1e-12);
+    ASSERT_EQ(Cov.cols() , 6);
+    ASSERT_EQ(Cov.rows() , 6);
+    ASSERT_MATRIX_APPROX(Cov, Eigen::Matrix6s::Identity(), 1e-12);
 
 }
 
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 904dfd7646f1194892e0845b003cd20d17916c4b..34128f0db287f152a36074fe16b2e61db9a37153 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -59,7 +59,7 @@ TEST(ProcessorBase, KeyFrameCallback)
 
     std::cout << "sensor & processor created and added to wolf problem" << std::endl;
 
-    // Sequence to test KeyFrame creations (callback calls)
+    // Sequence to test Key Frame creations (callback calls)
 
     // initialize
     TimeStamp   t(0.0);
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
index cfbdd69e0f978d8822bf0e707ff48a2a3fcefbbd..75a0cdefbb70da4d75edb2d1e4a7e5d79d13a29c 100644
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
@@ -72,19 +72,19 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
   auto stateblock_optr4 = std::make_shared<wolf::StateBlock>(state4.tail<1>());
 
   // create Keyframes
-  F1 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
+  F1 = std::make_shared<wolf::FrameBase>(wolf::KEY,
                                          1,
                                          stateblock_pptr1,
                                          stateblock_optr1);
-  F2 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
+  F2 = std::make_shared<wolf::FrameBase>(wolf::KEY,
                                          1,
                                          stateblock_pptr2,
                                          stateblock_optr2);
-  F3 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
+  F3 = std::make_shared<wolf::FrameBase>(wolf::KEY,
                                          1,
                                          stateblock_pptr3,
                                          stateblock_optr3);
-  F4 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
+  F4 = std::make_shared<wolf::FrameBase>(wolf::KEY,
                                          1,
                                          stateblock_pptr4,
                                          stateblock_optr4);
@@ -147,8 +147,9 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
   incomming_dummy = std::make_shared<wolf::CaptureBase>("DUMMY",
                                                         1.2,
                                                         sensor_ptr);
-  // Make 3rd frame last Keyframe
-  problem->getTrajectory()->setLastKeyFrame(F3);
+  // Make 3rd frame last Key frame
+  F3->setTimeStamp(wolf::TimeStamp(2.0));
+  problem->getTrajectory()->sortFrame(F3);
 
   // trigger search for loopclosure
   processor_ptr_point2d->process(incomming_dummy);
@@ -180,8 +181,9 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
   problem->addCovarianceBlock( F4->getP(), F4->getP(),
                                position_covariance_matrix);
 
-  // Make 3rd frame last Keyframe
-  problem->getTrajectory()->setLastKeyFrame(F3);
+  // Make 3rd frame last Key frame
+  F3->setTimeStamp(wolf::TimeStamp(2.0));
+  problem->getTrajectory()->sortFrame(F3);
 
   // trigger search for loopclosure
   processor_ptr_ellipse2d->process(incomming_dummy);
@@ -192,8 +194,9 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
   ASSERT_EQ(testloops[0]->id(), (unsigned int) 1);
   ASSERT_EQ(testloops[1]->id(), (unsigned int) 2);
 
-  // Make 4th frame last Keyframe
-  problem->getTrajectory()->setLastKeyFrame(F4);
+  // Make 4th frame last Key frame
+  F4->setTimeStamp(wolf::TimeStamp(3.0));
+  problem->getTrajectory()->sortFrame(F4);
 
   // trigger search for loopclosure again
   processor_ptr_ellipse2d->process(incomming_dummy);
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index a38a7ee467e95f23a3f922c4b7bceb117e06cd14..91ec5dba639cc2a89dc99a94de23e5483adf981b 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -61,7 +61,7 @@ class SolverManagerWrapper : public SolverManager
         };
 
         virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
-        virtual void computeCovariances(const StateBlockPtrList& st_list){};
+        virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
 
         // The following are dummy implementations
         bool    hasConverged()  { return true;      }
@@ -504,7 +504,7 @@ TEST(SolverManager, AddFactor)
     StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
     CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
     FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
     FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
@@ -531,7 +531,7 @@ TEST(SolverManager, RemoveFactor)
     StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
     CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
     FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
     FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
@@ -564,7 +564,7 @@ TEST(SolverManager, AddRemoveFactor)
     StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
     CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
     FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
     FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
@@ -598,7 +598,7 @@ TEST(SolverManager, DoubleRemoveFactor)
     StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
     CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
     FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
     FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index ec2b314fc2dda90f7b6b53bb83713095fe46e257..3a48ae21d7a9fbcd5ffcccc67d088fb9a9f8b11c 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -49,33 +49,74 @@ TEST(TrajectoryBase, ClosestKeyFrame)
     TrajectoryBasePtr T = P->getTrajectory();
 
     // Trajectory status:
-    //  kf1   kf2    f3      frames
+    //  KF1   KF2  AuxF3  F4       frames
+    //   1     2     3     4       time stamps
+    // --+-----+-----+-----+--->   time
+
+    FrameBasePtr F1 = std::make_shared<FrameBase>(KEY,     1, nullptr, nullptr);
+    FrameBasePtr F2 = std::make_shared<FrameBase>(KEY,     2, nullptr, nullptr);
+    FrameBasePtr F3 = std::make_shared<FrameBase>(AUXILIARY,     3, nullptr, nullptr);
+    FrameBasePtr F4 = std::make_shared<FrameBase>(NON_ESTIMATED, 4, nullptr, nullptr);
+    T->addFrame(F1);
+    T->addFrame(F2);
+    T->addFrame(F3);
+    T->addFrame(F4);
+
+    FrameBasePtr KF; // closest key-frame queried
+
+    KF = T->closestKeyFrameToTimeStamp(-0.8);                // before all keyframes    --> return F1
+    ASSERT_EQ(KF->id(), F1->id());                           // same id!
+
+    KF = T->closestKeyFrameToTimeStamp(1.1);                 // between keyframes       --> return F1
+    ASSERT_EQ(KF->id(), F1->id());                           // same id!
+
+    KF = T->closestKeyFrameToTimeStamp(1.9);                 // between keyframes       --> return F2
+    ASSERT_EQ(KF->id(), F2->id());                           // same id!
+
+    KF = T->closestKeyFrameToTimeStamp(2.6);                 // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2
+    ASSERT_EQ(KF->id(), F2->id());                           // same id!
+
+    KF = T->closestKeyFrameToTimeStamp(3.2);                 // after the auxiliary frame, between closer to frame --> return F2
+    ASSERT_EQ(KF->id(), F2->id());                           // same id!
+
+    KF = T->closestKeyFrameToTimeStamp(4.2);                 // after the last frame --> return F2
+    ASSERT_EQ(KF->id(), F2->id());                           // same id!
+}
+
+TEST(TrajectoryBase, ClosestKeyOrAuxFrame)
+{
+
+    ProblemPtr P = Problem::create("PO 2D");
+    TrajectoryBasePtr T = P->getTrajectory();
+
+    // Trajectory status:
+    //  KF1   KF2    F3      frames
     //   1     2     3       time stamps
     // --+-----+-----+--->   time
 
-    FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME,     1, nullptr, nullptr);
-    FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME,     2, nullptr, nullptr);
-    FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, nullptr, nullptr);
-    T->addFrame(f1);
-    T->addFrame(f2);
-    T->addFrame(f3);
+    FrameBasePtr F1 = std::make_shared<FrameBase>(KEY,           1, nullptr, nullptr);
+    FrameBasePtr F2 = std::make_shared<FrameBase>(AUXILIARY,     2, nullptr, nullptr);
+    FrameBasePtr F3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, nullptr, nullptr);
+    T->addFrame(F1);
+    T->addFrame(F2);
+    T->addFrame(F3);
 
-    FrameBasePtr kf; // closest key-frame queried
+    FrameBasePtr KF; // closest key-frame queried
 
-    kf = T->closestKeyFrameToTimeStamp(-0.8);                // before all keyframes    --> return f0
-    ASSERT_EQ(kf->id(), f1->id());                           // same id!
+    KF = T->closestKeyOrAuxFrameToTimeStamp(-0.8);          // before all keyframes    --> return f0
+    ASSERT_EQ(KF->id(), F1->id());                           // same id!
 
-    kf = T->closestKeyFrameToTimeStamp(1.1);                 // between keyframes       --> return f1
-    ASSERT_EQ(kf->id(), f1->id());                           // same id!
+    KF = T->closestKeyOrAuxFrameToTimeStamp(1.1);           // between keyframes       --> return F1
+    ASSERT_EQ(KF->id(), F1->id());                           // same id!
 
-    kf = T->closestKeyFrameToTimeStamp(1.9);                 // between keyframes       --> return f2
-    ASSERT_EQ(kf->id(), f2->id());                           // same id!
+    KF = T->closestKeyOrAuxFrameToTimeStamp(1.9);           // between keyframes       --> return F2
+    ASSERT_EQ(KF->id(), F2->id());                           // same id!
 
-    kf = T->closestKeyFrameToTimeStamp(2.6);                 // between keyframe and frame, but closer to frame --> return f2
-    ASSERT_EQ(kf->id(), f2->id());                           // same id!
+    KF = T->closestKeyOrAuxFrameToTimeStamp(2.6);           // between keyframe and frame, but closer to frame --> return F2
+    ASSERT_EQ(KF->id(), F2->id());                           // same id!
 
-    kf = T->closestKeyFrameToTimeStamp(3.2);                 // after the last frame    --> return f2
-    ASSERT_EQ(kf->id(), f2->id());                           // same id!
+    KF = T->closestKeyOrAuxFrameToTimeStamp(3.2);           // after the last frame    --> return F2
+    ASSERT_EQ(KF->id(), F2->id());                           // same id!
 }
 
 TEST(TrajectoryBase, Add_Remove_Frame)
@@ -88,37 +129,37 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     DummySolverManager N(P);
 
     // Trajectory status:
-    //  kf1   kf2    f3      frames
+    //  KF1   KF2    F3      frames
     //   1     2     3       time stamps
     // --+-----+-----+--->   time
 
-    FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME,     1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
-    FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME,     2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
-    FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+    FrameBasePtr F1 = std::make_shared<FrameBase>(KEY,     1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
+    FrameBasePtr F2 = std::make_shared<FrameBase>(KEY,     2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
+    FrameBasePtr F3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
 
     std::cout << __LINE__ << std::endl;
 
     // add frames and keyframes
-    T->addFrame(f1); // KF
+    T->addFrame(F1); // KF
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 1);
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
     std::cout << __LINE__ << std::endl;
 
-    T->addFrame(f2); // KF
+    T->addFrame(F2); // KF
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 2);
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
     std::cout << __LINE__ << std::endl;
 
-    T->addFrame(f3); // F (not KF so state blocks are not notified)
+    T->addFrame(F3); // F (not KF so state blocks are not notified)
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 3);
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
     std::cout << __LINE__ << std::endl;
 
-    ASSERT_EQ(T->getLastFrame()->id(), f3->id());
-    ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
+    ASSERT_EQ(T->getLastFrame()->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
     std::cout << __LINE__ << std::endl;
 
     N.update();
@@ -126,24 +167,24 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     std::cout << __LINE__ << std::endl;
 
     // remove frames and keyframes
-    f2->remove(); // KF
+    F2->remove(); // KF
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 2);
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
     std::cout << __LINE__ << std::endl;
 
-    ASSERT_EQ(T->getLastFrame()->id(), f3->id());
-    ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id());
+    ASSERT_EQ(T->getLastFrame()->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
     std::cout << __LINE__ << std::endl;
 
-    f3->remove(); // F
+    F3->remove(); // F
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 1);
     std::cout << __LINE__ << std::endl;
 
-    ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
 
-    f1->remove(); // KF
+    F1->remove(); // KF
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().             size(), (SizeStd) 0);
     std::cout << __LINE__ << std::endl;
@@ -162,70 +203,109 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
     TrajectoryBasePtr T = P->getTrajectory();
 
     // Trajectory status:
-    //  kf1   kf2    f3      frames
+    //  KF1   KF2    F3      frames
     //   1     2     3       time stamps
     // --+-----+-----+--->   time
 
-    FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME,     1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
-    FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME,     2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
-    FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+    FrameBasePtr F1 = std::make_shared<FrameBase>(KEY,     1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
+    FrameBasePtr F2 = std::make_shared<FrameBase>(KEY,     2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
+    FrameBasePtr F3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
 
     // add frames and keyframes in random order --> keyframes must be sorted after that
-    T->addFrame(f2); // KF2
+    T->addFrame(F2); // KF2
     if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getLastFrame()   ->id(), f2->id());
-    ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
+    ASSERT_EQ(T->getLastFrame()         ->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
-    T->addFrame(f3); // F3
+    T->addFrame(F3); // F3
     if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getLastFrame()   ->id(), f3->id());
-    ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
+    ASSERT_EQ(T->getLastFrame()         ->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
-    T->addFrame(f1); // KF1
+    T->addFrame(F1); // KF1
     if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getLastFrame()   ->id(), f3->id());
-    ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
+    ASSERT_EQ(T->getLastFrame()         ->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
-    f3->setKey(); // set KF3
+    F3->setKey(); // set KF3
     if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getLastFrame()   ->id(), f3->id());
-    ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id());
+    ASSERT_EQ(T->getLastFrame()         ->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F3->id());
 
-    FrameBasePtr f4 = std::make_shared<FrameBase>(NON_KEY_FRAME, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
-    T->addFrame(f4);
+    FrameBasePtr F4 = std::make_shared<FrameBase>(NON_ESTIMATED, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+    T->addFrame(F4);
     // Trajectory status:
-    //  kf1   kf2   kf3     f4       frames
+    //  KF1   KF2   KF3     F4       frames
     //   1     2     3     1.5       time stamps
     // --+-----+-----+------+--->    time
     if (debug) P->print(2,0,1,0);
-    ASSERT_EQ(T->getLastFrame()   ->id(), f4->id());
-    ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id());
+    ASSERT_EQ(T->getLastFrame()         ->id(), F4->id());
+    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F3->id());
 
-    f4->setKey();
+    F4->setKey();
     // Trajectory status:
-    //  kf1   kf4   kf2    kf3       frames
+    //  KF1   KF4   KF2    KF3       frames
     //   1    1.5    2      3        time stamps
     // --+-----+-----+------+--->    time
     if (debug) P->print(2,0,1,0);
-    ASSERT_EQ(T->getLastFrame()   ->id(), f3->id());
-    ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id());
+    ASSERT_EQ(T->getLastFrame()         ->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F3->id());
 
-    f2->setTimeStamp(4);
+    F2->setTimeStamp(4);
     // Trajectory status:
-    //  kf1   kf4   kf3    kf2       frames
+    //  KF1   KF4   KF3    KF2       frames
     //   1    1.5    3      4        time stamps
     // --+-----+-----+------+--->    time
     if (debug) P->print(2,0,1,0);
-    ASSERT_EQ(T->getLastFrame()   ->id(), f2->id());
-    ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
+    ASSERT_EQ(T->getLastFrame()         ->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
-    f4->setTimeStamp(0.5);
+    F4->setTimeStamp(0.5);
     // Trajectory status:
-    //  kf4   kf2   kf3    kf2       frames
+    //  KF4   KF2   KF3    KF2       frames
     //  0.5    1     3      4        time stamps
     // --+-----+-----+------+--->    time
     if (debug) P->print(2,0,1,0);
-    ASSERT_EQ(T->getFrameList().front()->id(), f4->id());
+    ASSERT_EQ(T->getFrameList().front()->id(), F4->id());
+
+    FrameBasePtr F5 = std::make_shared<FrameBase>(AUXILIARY, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+    T->addFrame(F5);
+    // Trajectory status:
+    //  KF4   KF2  AuxF5  KF3   KF2       frames
+    //  0.5    1    1.5    3     4        time stamps
+    // --+-----+-----+-----+-----+--->    time
+    if (debug) P->print(2,0,1,0);
+    ASSERT_EQ(T->getLastFrame()         ->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
+
+    F5->setTimeStamp(5);
+    // Trajectory status:
+    //  KF4   KF2   KF3   KF2   AuxF5     frames
+    //  0.5    1     3     4     5        time stamps
+    // --+-----+-----+-----+-----+--->    time
+    if (debug) P->print(2,0,1,0);
+    ASSERT_EQ(T->getLastFrame()         ->id(), F5->id());
+    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
+
+    FrameBasePtr F6 = std::make_shared<FrameBase>(NON_ESTIMATED, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+    T->addFrame(F6);
+    // Trajectory status:
+    //  KF4   KF2   KF3   KF2   AuxF5  F6       frames
+    //  0.5    1     3     4     5     6        time stamps
+    // --+-----+-----+-----+-----+-----+--->    time
+    if (debug) P->print(2,0,1,0);
+    ASSERT_EQ(T->getLastFrame()         ->id(), F6->id());
+    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
 }