39 #ifndef __usTissueTranslationEstimatorUKF_h_
40 #define __usTissueTranslationEstimatorUKF_h_
42 #include <visp3/core/vpColVector.h>
43 #include <visp3/core/vpMatrix.h>
45 #include <visp3/ustk_needle_modeling/usNeedleInsertionModelRayleighRitzSpline.h>
46 #include <visp3/ustk_needle_modeling/usUnscentedKalmanFilter.h>
55 enum MeasureType :
int {NEEDLE_BODY_POINTS, TIP_POSITION_AND_DIRECTION, BASE_FORCE_TORQUE};
75 double getPositionMeasureNoiseVariance()
const;
76 void setPositionMeasureNoiseVariance(
double sigma);
78 double getTipDirectionMeasureNoiseVariance()
const;
79 void setTipDirectionMeasureNoiseVariance(
double sigma);
81 double getForceMeasureNoiseVariance()
const;
82 void setForceMeasureNoiseVariance(
double sigma);
84 double getTorqueMeasureNoiseVariance()
const;
85 void setTorqueMeasureNoiseVariance(
double sigma);
87 double getTissuePositionProcessNoiseVariance()
const;
88 void setTissuePositionProcessNoiseVariance(
double sigma);
90 double getTissueVelocityProcessNoiseVariance()
const;
91 void setTissueVelocityProcessNoiseVariance(
double sigma);
102 void setPropagationTime(
double time);
112 double stateNorm(
const vpColVector& state)
const;
113 vpColVector
measureLog(
const vpColVector& measure,
const vpColVector &measureCenter)
const;
usNeedleInsertionModelRayleighRitzSpline m_needle
MeasureType m_measureType
TissueTranslationType m_tissueTranslationType
StateDynamicsType m_stateDynamicsType
virtual void computeProcessNoiseCovarianceMatrix()
virtual vpColVector measureLog(const vpColVector &measure, const vpColVector &measureCenter) const
virtual double stateNorm(const vpColVector &state) const
virtual vpColVector computeMeasureFromSigmaPoint(const vpColVector &sigmaPoint)=0
virtual bool checkConsistency(const vpColVector &measure)
virtual vpColVector propagateSigmaPoint(const vpColVector &sigmaPoint)=0
virtual void computeMeasureNoiseCovarianceMatrix()