F = -c_J_*featureOutputJac_.template block<2,mtState::D_>(0,0);

canditateGenerationH_  = c_J_*featureOutputJac_.template block<2,mtState::D_>(0,0);

ros::Publisher pubExtrinsics_[mtState::nMax_];

ros::Publisher pubExtrinsics_[mtState::nCam_];

PoseUpdate
{
    UpdateNoise
    {
        pos_0 0.5;                          X-Covariance of linear pose measurement update [m^2]
        pos_1 0.5;                          Y-Covariance of linear pose measurement update [m^2]
        pos_2 1e-7;                         Z-Covariance of linear pose measurement update [m^2]
        att_0 0.5;                          X-Covariance of rotational pose measurement update [rad^2]
        att_1 0.5;                          Y-Covariance of rotational pose measurement update [rad^2]
        att_2 0.5;                          Z-Covariance of rotational pose measurement update [rad^2]
    }
    init_cov_IrIW 10;                           Covariance of initial pose between inertial frames, linear part [m^2]
    init_cov_qWI 10;                                Covariance of initial pose between inertial frames, rotational part [rad^2]
    init_cov_MrMV 10;                           Covariance of initial pose between inertial frames, linear part [m^2]
    init_cov_qVM 10;                                Covariance of initial pose between inertial frames, rotational part [rad^2]
    pre_cov_IrIW 1e-4;                          Covariance parameter of pose between inertial frames, linear part [m^2/s]
    pre_cov_qWI 1e-4;                           Covariance parameter of pose between inertial frames, rotational part [rad^2/s]
    pre_cov_MrMV 1e-4;                          Covariance parameter of pose between inertial frames, linear part [m^2/s]
    pre_cov_qVM 1e-4;                           Covariance parameter of pose between inertial frames, rotational part [rad^2/s]
    MahalanobisTh0 12.6511204;                  Mahalonobis distance treshold of pose measurement
    doVisualization true;                       Should the measured poses be vizualized
    enablePosition true;                        Should the linear part be used during the update
    enableAttitude true;                        Should the rotation part be used during the update (e.g. set to fals eif feeding GPS-measurement)
    noFeedbackToRovio false;                        By default the pose update is only used for aligning coordinate frame. Set to false if ROVIO should benefit frome the posed measurements.
    doInertialAlignmentAtStart true;            Should the transformation between I and W be explicitly computed and set with the first pose measurement.
    timeOffset 0.0;                             Time offset added to the pose measurement timestamps
    qVM_x 0;                                    X-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (JPL)
    qVM_y 0;                                    Y-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (JPL)
    qVM_z 0;                                    Z-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (JPL)
    qVM_w 1;                                    W-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (JPL)
    MrMV_x 0;                                   X-entry of vector representing IMU to reference body coordinate frame of pose measurement
    MrMV_y 0;                                   Y-entry of vector representing IMU to reference body coordinate frame of pose measurement
    MrMV_z 0;                                   Z-entry of vector representing IMU to reference body coordinate frame of pose measurement
    qWI_x 0;                                    X-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (JPL)
    qWI_y 0;                                    Y-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (JPL)
    qWI_z 0;                                    Z-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (JPL)
    qWI_w 1;                                    W-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (JPL)
    IrIW_x 0;                                   X-entry of vector representing World to reference inertial coordinate frame of pose measurement
    IrIW_y 0;                                   Y-entry of vector representing World to reference inertial coordinate frame of pose measurement
    IrIW_z 0;                                   Z-entry of vector representing World to reference inertial coordinate frame of pose measurement
}

Warning: included update measurements before safeTime

###### Camera Calibration File For Cam 1 of Euroc Datasets ######
image_width: 752
image_height: 480
camera_name: cam0
...

root@milton-ThinkPad-T450:~/catkin_ws# catkin build rovio --cmake-args -DCMAKE_BUILD_TYPE=Release
-------------------------------------------------------
Profile:                     default
Extending:          [cached] /opt/ros/indigo
Workspace:                   /root/catkin_ws
-------------------------------------------------------
Source Space:       [exists] /root/catkin_ws/src
Log Space:          [exists] /root/catkin_ws/logs
Build Space:        [exists] /root/catkin_ws/build
Devel Space:        [exists] /root/catkin_ws/devel
Install Space:      [unused] /root/catkin_ws/install
DESTDIR:            [unused] None
-------------------------------------------------------
Devel Space Layout:          linked
Install Space Layout:        None
-------------------------------------------------------
Additional CMake Args:       -DCMAKE_BUILD_TYPE=Release
Additional Make Args:        None
Additional catkin Make Args: None
Internal Make Job Server:    True
Cache Job Environments:      False
-------------------------------------------------------
Whitelisted Packages:        None
Blacklisted Packages:        None
-------------------------------------------------------
Workspace configuration appears valid.

NOTE: Forcing CMake to run for each package.
-------------------------------------------------------
[build] Found '4' packages in 0.0 seconds.                                     
[build] Package table is up to date.                                           
Warning: devel space setup files have an unknown origin.
Starting  >>> rovio                                                            
_______________________________________________________________________________
Errors     << rovio:cmake /root/catkin_ws/logs/rovio/build.cmake.006.log       
CMake Error at /root/catkin_ws/src/rovio/lightweight_filtering/CMakeLists.txt:15 (find_package):
  By not providing "FindEigen3.cmake" in CMAKE_MODULE_PATH this project has
  asked CMake to find a package configuration file provided by "Eigen3", but
  CMake did not find one.

  Could not find a package configuration file provided by "Eigen3" with any
  of the following names:

    Eigen3Config.cmake
    eigen3-config.cmake

  Add the installation prefix of "Eigen3" to CMAKE_PREFIX_PATH or set
  "Eigen3_DIR" to a directory containing one of the above files.  If "Eigen3"
  provides a separate development package or SDK, be sure it has been
  installed.


cd /root/catkin_ws/build/rovio; catkin build --get-env rovio | catkin env -si  /usr/local/bin/cmake /root/catkin_ws/src/rovio --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/root/catkin_ws/devel/.private/rovio -DCMAKE_INSTALL_PREFIX=/root/catkin_ws/install -DCMAKE_BUILD_TYPE=Release; cd -
...............................................................................
Failed     << rovio:cmake          [ Exited with code 1 ]                      
Failed    <<< rovio                [ 0.0 seconds ]                             
[build] Summary: 0 of 1 packages succeeded.                                    
[build]   Ignored:   3 packages were skipped or are blacklisted.               
[build]   Warnings:  None.                                                     
[build]   Abandoned: None.                                                     
[build]   Failed:    1 packages failed.                                        
[build] Runtime: 0.2 seconds total.                                            
root@milton-ThinkPad-T450:~/catkin_ws# 

[ 14%] [ 28%] Building CXX object CMakeFiles/rovio.dir/src/rovio_node.cpp.o
Building CXX object CMakeFiles/rovio.dir/src/FeatureCoordinates.cpp.o
In file included from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/MultilevelPatch.hpp:34:0,
                 from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/FeatureManager.hpp:35,
                 from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/FilterStates.hpp:38,
                 from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/RovioFilter.hpp:34,
                 from /home/artemis/artemis_ws/src/vision/rovio/src/rovio_node.cpp:31:
/home/artemis/artemis_ws/src/vision/rovio/include/rovio/ImagePyramid.hpp: In instantiation of void rovio::ImagePyramid<n_levels>::detectFastCorners(std::vector<rovio::FeatureCoordinates>&, int, int) const [with int n_levels = 4]:
/home/artemis/artemis_ws/src/vision/rovio/include/rovio/ImgUpdate.hpp:906:11:   required from void rovio::ImgUpdate<FILTERSTATE>::commonPostProcess(rovio::ImgUpdate<FILTERSTATE>::mtFilterState&, const mtMeas&) [with FILTERSTATE = rovio::FilterState<25u, 4, 8, 2, 0>; rovio::ImgUpdate<FILTERSTATE>::mtFilterState = rovio::FilterState<25u, 4, 8, 2, 0>; rovio::ImgUpdate<FILTERSTATE>::mtMeas = rovio::ImgUpdateMeas<rovio::State<25u, 4, 8, 2, 0> >]
/home/artemis/artemis_ws/src/vision/rovio/include/rovio/ImgUpdate.hpp:706:41:   required from void rovio::ImgUpdate<FILTERSTATE>::postProcess(rovio::ImgUpdate<FILTERSTATE>::mtFilterState&, const mtMeas&, const mtOutlierDetection&, bool&) [with FILTERSTATE = rovio::FilterState<25u, 4, 8, 2, 0>; rovio::ImgUpdate<FILTERSTATE>::mtFilterState = rovio::FilterState<25u, 4, 8, 2, 0>; rovio::ImgUpdate<FILTERSTATE>::mtMeas = rovio::ImgUpdateMeas<rovio::State<25u, 4, 8, 2, 0> >; rovio::ImgUpdate<FILTERSTATE>::mtOutlierDetection = rovio::ImgOutlierDetection<rovio::State<25u, 4, 8, 2, 0> >]
/home/artemis/artemis_ws/src/vision/rovio/src/rovio_node.cpp:118:1:   required from here
/home/artemis/artemis_ws/src/vision/rovio/include/rovio/ImagePyramid.hpp:137:75: error: no matching function for call to cv::FastFeatureDetector::FastFeatureDetector(int&, bool)
     cv::FastFeatureDetector feature_detector_fast(detectionThreshold, true);
                                                                           ^
/home/artemis/artemis_ws/src/vision/rovio/include/rovio/ImagePyramid.hpp:137:75: note: candidates are:
In file included from /usr/local/include/opencv2/features2d/features2d.hpp:48:0,
                 from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/FeatureCoordinates.hpp:34,
                 from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/RobocentricFeatureElement.hpp:33,
                 from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/CoordinateTransform/FeatureOutput.hpp:34,
                 from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/FilterStates.hpp:36,
                 from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/RovioFilter.hpp:34,
                 from /home/artemis/artemis_ws/src/vision/rovio/src/rovio_node.cpp:31:
/usr/local/include/opencv2/features2d.hpp:411:20: note: cv::FastFeatureDetector::FastFeatureDetector()
 class CV_EXPORTS_W FastFeatureDetector : public Feature2D
                    ^
/usr/local/include/opencv2/features2d.hpp:411:20: note:   candidate expects 0 arguments, 2 provided
/usr/local/include/opencv2/features2d.hpp:411:20: note: cv::FastFeatureDetector::FastFeatureDetector(const cv::FastFeatureDetector&)
/usr/local/include/opencv2/features2d.hpp:411:20: note:   candidate expects 1 argument, 2 provided
/usr/local/include/opencv2/features2d.hpp:411:20: note: cv::FastFeatureDetector::FastFeatureDetector(cv::FastFeatureDetector&&)
/usr/local/include/opencv2/features2d.hpp:411:20: note:   candidate expects 1 argument, 2 provided
In file included from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/MultilevelPatch.hpp:34:0,
                 from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/FeatureManager.hpp:35,
                 from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/FilterStates.hpp:38,
                 from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/RovioFilter.hpp:34,
                 from /home/artemis/artemis_ws/src/vision/rovio/src/rovio_node.cpp:31:
/home/artemis/artemis_ws/src/vision/rovio/include/rovio/ImagePyramid.hpp:137:29: error: cannot declare variable feature_detector_fast to be of abstract type cv::FastFeatureDetector
     cv::FastFeatureDetector feature_detector_fast(detectionThreshold, true);
                             ^
In file included from /usr/local/include/opencv2/features2d/features2d.hpp:48:0,
                 from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/FeatureCoordinates.hpp:34,
                 from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/RobocentricFeatureElement.hpp:33,
                 from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/CoordinateTransform/FeatureOutput.hpp:34,
                 from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/FilterStates.hpp:36,
                 from /home/artemis/artemis_ws/src/vision/rovio/include/rovio/RovioFilter.hpp:34,
                 from /home/artemis/artemis_ws/src/vision/rovio/src/rovio_node.cpp:31:
/usr/local/include/opencv2/features2d.hpp:411:20: note:   because the following virtual functions are pure within cv::FastFeatureDetector:
 class CV_EXPORTS_W FastFeatureDetector : public Feature2D
                    ^
/usr/local/include/opencv2/features2d.hpp:424:26: note:     virtual void cv::FastFeatureDetector::setThreshold(int)
     CV_WRAP virtual void setThreshold(int threshold) = 0;
                          ^
/usr/local/include/opencv2/features2d.hpp:425:25: note:     virtual int cv::FastFeatureDetector::getThreshold() const
     CV_WRAP virtual int getThreshold() const = 0;
                         ^
/usr/local/include/opencv2/features2d.hpp:427:26: note:     virtual void cv::FastFeatureDetector::setNonmaxSuppression(bool)
     CV_WRAP virtual void setNonmaxSuppression(bool f) = 0;
                          ^
/usr/local/include/opencv2/features2d.hpp:428:26: note:     virtual bool cv::FastFeatureDetector::getNonmaxSuppression() const
     CV_WRAP virtual bool getNonmaxSuppression() const = 0;
                          ^
/usr/local/include/opencv2/features2d.hpp:430:26: note:     virtual void cv::FastFeatureDetector::setType(int)
     CV_WRAP virtual void setType(int type) = 0;
                          ^
/usr/local/include/opencv2/features2d.hpp:431:25: note:     virtual int cv::FastFeatureDetector::getType() const
     CV_WRAP virtual int getType() const = 0;

<?xml version="1.0" encoding="UTF-8"?> 
<launch>
  <node pkg="rovio" type="rovio_rosbag_loader" name="rovio_bag_reader_kitti" output="screen">
  <param name="filter_config" value="$(find rovio)/cfg/rovio_kitti.info"/>
  <param name="camera0_config" value="$(find rovio)/cfg/euroc_cam0_kitti.yaml"/>
  <param name="rosbag_filename" value="/home/XXX/datasets/rovio_bags/kitti_longer.bag"/>
  <param name="imu_topic_name" value="/imu0"/>
  <param name="cam0_topic_name" value="/cam0/image_raw"/>
  </node>
</launch>

image_width: 1242
image_height: 375
camera_name: cam0
camera_matrix:
  rows: 3
  cols: 3
  data: [7.215377e+02, 0.000000e+00, 6.095593e+02, 0.000000e+00, 7.215377e+02, 1.728540e+02, 0.000000e+00, 0.000000e+00, 1.000000e+00]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data:  [0.0,  0.0, 0.0, 0.0, 0.0]

Common
{
    doVECalibration false;      Should the camera-IMU extrinsics be calibrated online
    depthType 1;                Type of depth parametrization (0: normal, 1: inverse depth, 2: log, 3: hyperbolic)
    verbose false;              Is the verbose active
}
Camera0
{
     qCM_x 0.5010303840;
     qCM_y -0.4989766415;
     qCM_z 0.4982646186;
     qCM_w 0.5017202760;
     MrMC_x 0.8127456660;
     MrMC_y -0.2432397200;
     MrMC_z 1.0715037000;
}
Init
{
    State
    {
        pos_0 0;            X-entry of initial position (world to IMU in world) [m]
        pos_1 0;            Y-entry of initial position (world to IMU in world) [m]
        pos_2 0;            Z-entry of initial position (world to IMU in world) [m]
        vel_0 0;            X-entry of initial velocity (robocentric, IMU) [m/s]
        vel_1 0;            Y-entry of initial velocity (robocentric, IMU) [m/s]
        vel_2 0;            Z-entry of initial velocity (robocentric, IMU) [m/s]
        acb_0 0;            X-entry of accelerometer bias [m/s^2]
        acb_1 0;            Y-entry of accelerometer bias [m/s^2]
        acb_2 0;            Z-entry of accelerometer bias [m/s^2]
        gyb_0 0;            X-entry of gyroscope bias [rad/s]
        gyb_1 0;            Y-entry of gyroscope bias [rad/s]
        gyb_2 0;            Z-entry of gyroscope bias [rad/s]
        att_x 0;            X-entry of initial attitude (IMU to world, JPL)
        att_y 0;            Y-entry of initial attitude (IMU to world, JPL)
        att_z 0;            Z-entry of initial attitude (IMU to world, JPL)
        att_w 1;            W-entry of initial attitude (IMU to world, JPL)
    }
    Covariance
    {
        pos_0 0.0001;       X-Covariance of initial position [m^2]
        pos_1 0.0001;       Y-Covariance of initial position [m^2]
        pos_2 0.0001;       Z-Covariance of initial position [m^2]
        vel_0 1.0;          X-Covariance of initial velocity [m^2/s^2]
        vel_1 1.0;          Y-Covariance of initial velocity [m^2/s^2]
        vel_2 1.0;          Z-Covariance of initial velocity [m^2/s^2]
        acb_0 4e-4;         X-Covariance of initial accelerometer bias [m^2/s^4]
        acb_1 4e-4;         Y-Covariance of initial accelerometer bias [m^2/s^4]
        acb_2 4e-4;         Z-Covariance of initial accelerometer bias [m^2/s^4]
        gyb_0 3e-4;         X-Covariance of initial gyroscope bias [rad^2/s^2]
        gyb_1 3e-4;         Y-Covariance of initial gyroscope bias [rad^2/s^2]
        gyb_2 3e-4;         Z-Covariance of initial gyroscope bias [rad^2/s^2]
        vep 0.0001;         Covariance of initial linear camera-IMU extrinsics, same for all entries [m^2]
        att_0 0.1;          X-Covariance of initial attitude [rad^2]
        att_1 0.1;          Y-Covariance of initial attitude [rad^2]
        att_2 0.1;          Z-Covariance of initial attitude [rad^2]
        vea 0.01;           Covariance of initial rotational camera-IMU extrinsics, same for all entries [rad^2]
    }
}
ImgUpdate
{
    updateVecNormTermination 1e-4;
    maxNumIteration 20;
    doPatchWarping true;                                        Should the patches be warped
    doFrameVisualisation true;                                      Should the frames be visualized
    visualizePatches false;                                     Should the patches be visualized
    useDirectMethod true;                                       Should the EKF-innovation be based on direct intensity error (o.w. reprojection error)
    startLevel 2;                                               Highest patch level which is being employed (must be smaller than the hardcoded template parameter)
    endLevel 1;                                                 Lowest patch level which is being employed
    nDetectionBuckets 100;                                      Number of discretization buckets used during the candidates selection
    MahalanobisTh 9.21;                                         Mahalanobis treshold for the update, 5.8858356 9.21
    UpdateNoise
    {
        pix 2;                                                  Covariance used for the reprojection error, 1/focal_length is roughly 1 pixel std [rad] (~1/f ~ 1/400^2 = 1/160000)
        int 400;                                                Covariance used for the photometric error [intensity^2]
    }
    initCovFeature_0 4.0;0.5;                                       Covariance for the initial distance (Relative to initialization depth [m^2/m^2])
    initCovFeature_1 1e-5;                                      Covariance for the initial bearing vector, x-component [rad^2]
    initCovFeature_2 1e-5;                                      Covariance for the initial bearing vector, y-component [rad^2]
    initDepth 0.5;                                              Initial value for the initial distance parameter
    startDetectionTh 0.8;                                       Threshold for detecting new features (min: 0, max: 1)
    scoreDetectionExponent 0.25;                                Exponent used for weighting the distance between candidates
    penaltyDistance 50;100;                                     Maximal distance which gets penalized during bucketing [pix]
    zeroDistancePenalty 100;                                    Penalty for zero distance (max: nDetectionBuckets)
    statLocalQualityRange 10;                                   Number of frames for local quality evaluation
    statLocalVisibilityRange 100;                               Number of frames for local visibility evaluation
    statMinGlobalQualityRange 100;                              Minimum number of frames for obtaining maximal global quality
    trackingUpperBound 0.9;                                     Threshold for local quality for min overall global quality
    trackingLowerBound 0.8;                                     Threshold for local quality for max overall global quality
    minTrackedAndFreeFeatures 0.75;                             Minimum of amount of feature which are either tracked or free
    removalFactor 1.1;                                          Factor for enforcing feature removal if not enough free
    minRelativeSTScore 0.75;                                    Minimum relative ST-score for extracting new feature patch
    minAbsoluteSTScore 5.0;                                     Minimum absolute ST-score for extracting new feature patch
    minTimeBetweenPatchUpdate 1.0;                              Minimum time between new multilevel patch extrection [s]
    fastDetectionThreshold 5;                                   Fast corner detector treshold while adding new feature
    alignConvergencePixelRange 10;                              Assumed convergence range for image alignment (gets scaled with the level) [pixels]
    alignCoverageRatio 2;                                       How many sigma of the uncertainty should be covered in the adaptive alignement
    alignMaxUniSample 1;                                        Maximal number of alignment seeds on one side -> total number of sample = 2n+1. Carefull can get very costly if diverging!
    patchRejectionTh 50.0;                                      If the average itensity error is larger than this the feauture is rejected [intensity], if smaller 0 the no check is performed
    alignmentHuberNormThreshold 10;                             Intensity error threshold for Huber norm (enabled if > 0)
    alignmentGaussianWeightingSigma -1;                         Width of Gaussian which is used for pixel error weighting (enabled if > 0)
    alignmentGradientExponent 0.0;                              Exponent used for gradient based weighting of residuals
    useIntensityOffsetForAlignment true;                        Should an intensity offset between the patches be considered
    useIntensitySqewForAlignment true;                          Should an intensity sqew between the patches be considered
    removeNegativeFeatureAfterUpdate true;                      Should feature with negative distance get removed
    maxUncertaintyToDepthRatioForDepthInitialization 0.3;       If set to 0.0 the depth is initialized with the standard value provided above, otherwise ROVIO attempts to figure out a median depth in each frame
    useCrossCameraMeasurements true;                            Should cross measurements between frame be used. Might be turned of in calibration phase.
    doStereoInitialization true;                                Should a stereo match be used for feature initialization.
    noiseGainForOffCamera 10.0;                                 Factor added on update noise if not main camera
    discriminativeSamplingDistance 0.02;                        Sampling distance for checking discriminativity of patch (if <= 0.0 no check is performed).
    discriminativeSamplingGain 1.1;                             Gain for threshold above which the samples must lie (if <= 1.0 the patchRejectionTh is used).
    MotionDetection
    {
        isEnabled 0;                                            Is the motion detection enabled
        rateOfMovingFeaturesTh 0.5;                             Amount of feature with motion for overall motion detection
        pixelCoordinateMotionTh 1.0;                            Threshold for motion detection for patched [pixels]
        minFeatureCountForNoMotionDetection 5;                  Min feature count in frame for motion detection
    }
    ZeroVelocityUpdate
    {
        UpdateNoise
        {
            vel_0 0.01;                                         X-Covariance of zero velocity update [m^2/s^2]
            vel_1 0.01;                                         Y-Covariance of zero velocity update [m^2/s^2]
            vel_2 0.01;                                         Z-Covariance of zero velocity update [m^2/s^2]
        }
        MahalanobisTh0 7.689997599999999;                       Mahalanobid distance for zero velocity updates
        minNoMotionTime 1.0;                                    Min duration of no-motion
        isEnabled 0;                                            Should zero velocity update be applied, only works if MotionDetection.isEnabled is true
    }
}
Prediction
{
    PredictionNoise
    {
        pos_0 1e-4;                             X-covariance parameter of position prediction [m^2/s]
        pos_1 1e-4;                             Y-covariance parameter of position prediction [m^2/s]
        pos_2 1e-4;                             Z-covariance parameter of position prediction [m^2/s]
        vel_0 4e-5; 4e-6                        X-covariance parameter of velocity prediction [m^2/s^3]
        vel_1 4e-5;                             Y-covariance parameter of velocity prediction [m^2/s^3]
        vel_2 4e-5;                             Z-covariance parameter of velocity prediction [m^2/s^3]
        acb_0 1e-8;                             X-covariance parameter of accelerometer bias prediction [m^2/s^5]
        acb_1 1e-8;                             Y-covariance parameter of accelerometer bias prediction [m^2/s^5]
        acb_2 1e-8;                             Z-covariance parameter of accelerometer bias prediction [m^2/s^5]
        gyb_0 3.8e-7;                           X-covariance parameter of gyroscope bias prediction [rad^2/s^3]
        gyb_1 3.8e-7;                           Y-covariance parameter of gyroscope bias prediction [rad^2/s^3]
        gyb_2 3.8e-7;                           Z-covariance parameter of gyroscope bias prediction [rad^2/s^3]
        vep 1e-8;                               Covariance parameter of linear extrinsics prediction [m^2/s]
        att_0 7.6e-7; 7.6e-7                    X-Covariance parameter of attitude prediction [rad^2/s]
        att_1 7.6e-7;                           Y-Covariance parameter of attitude prediction [rad^2/s]
        att_2 7.6e-7;                           Z-Covariance parameter of attitude prediction [rad^2/s]
        vea 1e-8;                               Covariance parameter of rotational extrinsics prediction [rad^2/s]
        dep 0.0001;                         Covariance parameter of distance prediction [m^2/s]
        nor 0.00001;                                Covariance parameter of bearing vector prediction [rad^2/s]
    }
    MotionDetection
    {
        inertialMotionRorTh 0.1;                Treshold on rotational rate for motion detection [rad/s]
        inertialMotionAccTh 0.5;                Treshold on acceleration for motion detection [m/s^2]
    }
}
PoseUpdate
{
    UpdateNoise
    {
        pos_0 0.01;                         X-Covariance of linear pose measurement update [m^2]
        pos_1 0.01;                         Y-Covariance of linear pose measurement update [m^2]
        pos_2 0.01;                         Z-Covariance of linear pose measurement update [m^2]
        att_0 0.01;                         X-Covariance of rotational pose measurement update [rad^2]
        att_1 0.01;                         Y-Covariance of rotational pose measurement update [rad^2]
        att_2 0.01;                         Z-Covariance of rotational pose measurement update [rad^2]
    }
    init_cov_IrIW 1;                            Covariance of initial pose between inertial frames, linear part [m^2]
    init_cov_qWI 1;                             Covariance of initial pose between inertial frames, rotational part [rad^2]
    init_cov_MrMV 1;                            Covariance of initial pose between inertial frames, linear part [m^2]
    init_cov_qVM 1;                             Covariance of initial pose between inertial frames, rotational part [rad^2]
    pre_cov_IrIW 1e-4;                          Covariance parameter of pose between inertial frames, linear part [m^2/s]
    pre_cov_qWI 1e-4;                           Covariance parameter of pose between inertial frames, rotational part [rad^2/s]
    pre_cov_MrMV 1e-4;                          Covariance parameter of pose between inertial frames, linear part [m^2/s]
    pre_cov_qVM 1e-4;                           Covariance parameter of pose between inertial frames, rotational part [rad^2/s]
    MahalanobisTh0 12.6511204;                  Mahalonobis distance treshold of pose measurement
    doVisualization false;                      Should the measured poses be vizualized
    enablePosition true;                        Should the linear part be used during the update
    enableAttitude true;                        Should the rotation part be used during the update (e.g. set to fals eif feeding GPS-measurement)
    noFeedbackToRovio true;                     By default the pose update is only used for aligning coordinate frame. Set to false if ROVIO should benefit frome the posed measurements.
    doInertialAlignmentAtStart true;            Should the transformation between I and W be explicitly computed and set with the first pose measurement.
    timeOffset 0.0;                             Time offset added to the pose measurement timestamps
    qVM_x 0;                                    X-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (JPL)
    qVM_y 0;                                    Y-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (JPL)
    qVM_z 0;                                    Z-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (JPL)
    qVM_w 1;                                    W-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (JPL)
    MrMV_x 0;                                   X-entry of vector representing IMU to reference body coordinate frame of pose measurement
    MrMV_y 0;                                   Y-entry of vector representing IMU to reference body coordinate frame of pose measurement
    MrMV_z 0;                                   Z-entry of vector representing IMU to reference body coordinate frame of pose measurement
    qWI_x 0;                                    X-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (JPL)
    qWI_y 0;                                    Y-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (JPL)
    qWI_z 0;                                    Z-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (JPL)
    qWI_w 1;                                    W-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (JPL)
    IrIW_x 0;                                   X-entry of vector representing World to reference inertial coordinate frame of pose measurement
    IrIW_y 0;                                   Y-entry of vector representing World to reference inertial coordinate frame of pose measurement
    IrIW_z 0;                                   Z-entry of vector representing World to reference inertial coordinate frame of pose measurement
}

// Get new features
if(filterState.fsm_.getValidCount() < startDetectionTh_*mtState::nMax_){
  // Compute the median depth parameters for each camera, using the state features.
  std::array<double, mtState::nCam_> medianDepthParameters;
  if(maxUncertaintyToDepthRatioForDepthInitialization_>0){
    filterState.getMedianDepthParameters(initDepth_, &medianDepthParameters,maxUncertaintyToDepthRatioForDepthInitialization_);
  } else {
    medianDepthParameters.fill(initDepth_);
  }