MRPT  2.0.4
List of all members | Public Types | Public Member Functions | Static Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes | Static Private Member Functions | Private Attributes
CRangeBearing Class Referenceabstract

Detailed Description

Definition at line 54 of file vision_stereo_rectify/test.cpp.

Inheritance diagram for CRangeBearing:

Public Types

using kftype = double
 The numeric type used in the Kalman Filter (default=double) More...
 
using KFCLASS = CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >
 My class, in a shorter name! More...
 
using KFVector = mrpt::math::CVectorDynamic< double >
 
using KFMatrix = mrpt::math::CMatrixDynamic< double >
 
using KFMatrix_VxV = mrpt::math::CMatrixFixed< double, VEH_SIZE, VEH_SIZE >
 
using KFMatrix_OxO = mrpt::math::CMatrixFixed< double, OBS_SIZE, OBS_SIZE >
 
using KFMatrix_FxF = mrpt::math::CMatrixFixed< double, FEAT_SIZE, FEAT_SIZE >
 
using KFMatrix_AxA = mrpt::math::CMatrixFixed< double, ACT_SIZE, ACT_SIZE >
 
using KFMatrix_VxO = mrpt::math::CMatrixFixed< double, VEH_SIZE, OBS_SIZE >
 
using KFMatrix_VxF = mrpt::math::CMatrixFixed< double, VEH_SIZE, FEAT_SIZE >
 
using KFMatrix_FxV = mrpt::math::CMatrixFixed< double, FEAT_SIZE, VEH_SIZE >
 
using KFMatrix_FxO = mrpt::math::CMatrixFixed< double, FEAT_SIZE, OBS_SIZE >
 
using KFMatrix_OxF = mrpt::math::CMatrixFixed< double, OBS_SIZE, FEAT_SIZE >
 
using KFMatrix_OxV = mrpt::math::CMatrixFixed< double, OBS_SIZE, VEH_SIZE >
 
using KFArray_VEH = mrpt::math::CVectorFixed< double, VEH_SIZE >
 
using KFArray_ACT = mrpt::math::CVectorFixed< double, ACT_SIZE >
 
using KFArray_OBS = mrpt::math::CVectorFixed< double, OBS_SIZE >
 
using vector_KFArray_OBS = std::vector< KFArray_OBS >
 
using KFArray_FEAT = mrpt::math::CVectorFixed< double, FEAT_SIZE >
 

Public Member Functions

 CRangeBearing ()
 
 ~CRangeBearing () override
 
void doProcess (double DeltaTime, double observationRange, double observationBearing)
 
void getState (KFVector &xkk, KFMatrix &pkk)
 
size_t getNumberOfLandmarksInTheMap () const
 
bool isMapEmpty () const
 
size_t getStateVectorLength () const
 
KFVectorinternal_getXkk ()
 
KFMatrixinternal_getPkk ()
 
void getLandmarkMean (size_t idx, KFArray_FEAT &feat) const
 Returns the mean of the estimated value of the idx'th landmark (not applicable to non-SLAM problems). More...
 
void getLandmarkCov (size_t idx, KFMatrix_FxF &feat_cov) const
 Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems). More...
 
mrpt::system::CTimeLoggergetProfiler ()
 

Static Public Member Functions

static constexpr size_t get_vehicle_size ()
 
static constexpr size_t get_observation_size ()
 
static constexpr size_t get_feature_size ()
 
static constexpr size_t get_action_size ()
 

Public Attributes

TKF_options KF_options
 Generic options for the Kalman Filter algorithm itself. More...
 

Protected Member Functions

void runOneKalmanIteration ()
 The main entry point, executes one complete step: prediction + update. More...
 
Virtual methods for Kalman Filter implementation
void OnGetAction (KFArray_ACT &out_u) const override
 Must return the action vector u. More...
 
void OnTransitionModel (const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const override
 Implements the transition model $ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $. More...
 
void OnTransitionJacobian (KFMatrix_VxV &out_F) const override
 Implements the transition Jacobian $ \frac{\partial f}{\partial x} $. More...
 
void OnTransitionNoise (KFMatrix_VxV &out_Q) const override
 Implements the transition noise covariance $ Q_k $. More...
 
void OnGetObservationNoise (KFMatrix_OxO &out_R) const override
 Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor. More...
 
void OnGetObservationsAndDataAssociation (vector_KFArray_OBS &out_z, std::vector< int > &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const std::vector< size_t > &in_lm_indices_in_S, const KFMatrix_OxO &in_R) override
 This is called between the KF prediction step and the update step, and the application must return the observations and, when applicable, the data association between these observations and the current map. More...
 
void OnObservationModel (const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const override
 Implements the observation prediction $ h_i(x) $. More...
 
void OnObservationJacobians (size_t idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const override
 Implements the observation Jacobians $ \frac{\partial h_i}{\partial x} $ and (when applicable) $ \frac{\partial h_i}{\partial y_i} $. More...
 
void OnSubstractObservationVectors (KFArray_OBS &A, const KFArray_OBS &B) const override
 Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles). More...
 

Protected Attributes

float m_obsBearing
 
float m_obsRange
 
float m_deltaTime
 
mrpt::system::CTimeLogger m_timLogger
 
Kalman filter state
KFVector m_xkk
 The system state vector. More...
 
KFMatrix m_pkk
 The system full covariance matrix. More...
 
Kalman filter state
KFVector m_xkk
 The system state vector. More...
 
KFMatrix m_pkk
 The system full covariance matrix. More...
 

Static Private Member Functions

static void KF_aux_estimate_trans_jacobian (const KFArray_VEH &x, const std::pair< KFCLASS *, KFArray_ACT > &dat, KFArray_VEH &out_x)
 Auxiliary functions for Jacobian numeric estimation. More...
 
static void KF_aux_estimate_obs_Hx_jacobian (const KFArray_VEH &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
 
static void KF_aux_estimate_obs_Hy_jacobian (const KFArray_FEAT &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
 

Private Attributes

vector_KFArray_OBS m_all_predictions
 
std::vector< size_t > m_predictLMidxs
 
std::vector< KFMatrix_OxVm_Hxs
 The vector of all partial Jacobians dh[i]_dx for each prediction. More...
 
std::vector< KFMatrix_OxFm_Hys
 The vector of all partial Jacobians dh[i]_dy[i] for each prediction. More...
 
KFMatrix m_S
 
vector_KFArray_OBS m_Z
 
KFMatrix m_K
 
KFMatrix m_S_1
 
KFMatrix m_dh_dx_full_obs
 
KFMatrix m_aux_K_dh_dx
 
bool m_user_didnt_implement_jacobian
 

Virtual methods for Kalman Filter implementation

virtual void OnInverseObservationModel ([[maybe_unused]] const KFArray_OBS &in_z,[[maybe_unused]] KFArray_FEAT &out_yn,[[maybe_unused]] KFMatrix_FxV &out_dyn_dxv,[[maybe_unused]] KFMatrix_FxO &out_dyn_dhn) const
 If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". More...
 
virtual void OnInverseObservationModel (const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn,[[maybe_unused]] KFMatrix_FxF &out_dyn_dhn_R_dyn_dhnT, bool &out_use_dyn_dhn_jacobian) const
 If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". More...
 
virtual void OnNewLandmarkAddedToMap ([[maybe_unused]] const size_t in_obsIdx,[[maybe_unused]] const size_t in_idxNewFeat)
 If applicable to the given problem, do here any special handling of adding a new landmark to the map. More...
 
virtual void OnNormalizeStateVector ()
 This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it. More...
 
virtual void OnPostIteration ()
 This method is called after finishing one KF iteration and before returning from runOneKalmanIteration(). More...
 
virtual void OnGetAction (KFArray_ACT &out_u) const=0
 Must return the action vector u. More...
 
virtual void OnTransitionModel (const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const=0
 Implements the transition model $ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $. More...
 
virtual void OnTransitionJacobian ([[maybe_unused]] KFMatrix_VxV &out_F) const
 Implements the transition Jacobian $ \frac{\partial f}{\partial x} $. More...
 
virtual void OnTransitionJacobianNumericGetIncrements (KFArray_VEH &out_increments) const
 Only called if using a numeric approximation of the transition Jacobian, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian. More...
 
virtual void OnPreComputingPredictions ([[maybe_unused]] const vector_KFArray_OBS &in_all_prediction_means, std::vector< size_t > &out_LM_indices_to_predict) const
 This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made. More...
 
virtual void OnObservationJacobians ([[maybe_unused]] size_t idx_landmark_to_predict,[[maybe_unused]] KFMatrix_OxV &Hx,[[maybe_unused]] KFMatrix_OxF &Hy) const
 Implements the observation Jacobians $ \frac{\partial h_i}{\partial x} $ and (when applicable) $ \frac{\partial h_i}{\partial y_i} $. More...
 
virtual void OnObservationJacobiansNumericGetIncrements (KFArray_VEH &out_veh_increments, KFArray_FEAT &out_feat_increments) const
 Only called if using a numeric approximation of the observation Jacobians, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian. More...
 
virtual void OnSubstractObservationVectors (KFArray_OBS &A, const KFArray_OBS &B) const
 Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles). More...
 

Member Typedef Documentation

◆ KFArray_ACT

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFArray_ACT = mrpt::math::CVectorFixed<double , ACT_SIZE>
inherited

Definition at line 256 of file CKalmanFilterCapable.h.

◆ KFArray_FEAT

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFArray_FEAT = mrpt::math::CVectorFixed<double , FEAT_SIZE>
inherited

Definition at line 259 of file CKalmanFilterCapable.h.

◆ KFArray_OBS

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFArray_OBS = mrpt::math::CVectorFixed<double , OBS_SIZE>
inherited

Definition at line 257 of file CKalmanFilterCapable.h.

◆ KFArray_VEH

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFArray_VEH = mrpt::math::CVectorFixed<double , VEH_SIZE>
inherited

Definition at line 255 of file CKalmanFilterCapable.h.

◆ KFCLASS

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFCLASS = CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >
inherited

My class, in a shorter name!

Definition at line 237 of file CKalmanFilterCapable.h.

◆ KFMatrix

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix = mrpt::math::CMatrixDynamic<double >
inherited

Definition at line 241 of file CKalmanFilterCapable.h.

◆ KFMatrix_AxA

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_AxA = mrpt::math::CMatrixFixed<double , ACT_SIZE, ACT_SIZE>
inherited

Definition at line 246 of file CKalmanFilterCapable.h.

◆ KFMatrix_FxF

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_FxF = mrpt::math::CMatrixFixed<double , FEAT_SIZE, FEAT_SIZE>
inherited

Definition at line 245 of file CKalmanFilterCapable.h.

◆ KFMatrix_FxO

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_FxO = mrpt::math::CMatrixFixed<double , FEAT_SIZE, OBS_SIZE>
inherited

Definition at line 251 of file CKalmanFilterCapable.h.

◆ KFMatrix_FxV

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_FxV = mrpt::math::CMatrixFixed<double , FEAT_SIZE, VEH_SIZE>
inherited

Definition at line 250 of file CKalmanFilterCapable.h.

◆ KFMatrix_OxF

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_OxF = mrpt::math::CMatrixFixed<double , OBS_SIZE, FEAT_SIZE>
inherited

Definition at line 252 of file CKalmanFilterCapable.h.

◆ KFMatrix_OxO

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_OxO = mrpt::math::CMatrixFixed<double , OBS_SIZE, OBS_SIZE>
inherited

Definition at line 244 of file CKalmanFilterCapable.h.

◆ KFMatrix_OxV

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_OxV = mrpt::math::CMatrixFixed<double , OBS_SIZE, VEH_SIZE>
inherited

Definition at line 253 of file CKalmanFilterCapable.h.

◆ KFMatrix_VxF

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_VxF = mrpt::math::CMatrixFixed<double , VEH_SIZE, FEAT_SIZE>
inherited

Definition at line 249 of file CKalmanFilterCapable.h.

◆ KFMatrix_VxO

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_VxO = mrpt::math::CMatrixFixed<double , VEH_SIZE, OBS_SIZE>
inherited

Definition at line 248 of file CKalmanFilterCapable.h.

◆ KFMatrix_VxV

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFMatrix_VxV = mrpt::math::CMatrixFixed<double , VEH_SIZE, VEH_SIZE>
inherited

Definition at line 243 of file CKalmanFilterCapable.h.

◆ kftype

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::kftype = double
inherited

The numeric type used in the Kalman Filter (default=double)

Definition at line 234 of file CKalmanFilterCapable.h.

◆ KFVector

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KFVector = mrpt::math::CVectorDynamic<double >
inherited

Definition at line 240 of file CKalmanFilterCapable.h.

◆ vector_KFArray_OBS

using mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::vector_KFArray_OBS = std::vector<KFArray_OBS>
inherited

Definition at line 258 of file CKalmanFilterCapable.h.

Constructor & Destructor Documentation

◆ CRangeBearing()

CRangeBearing::CRangeBearing ( )

Definition at line 426 of file vision_stereo_rectify/test.cpp.

◆ ~CRangeBearing()

CRangeBearing::~CRangeBearing ( )
override

Definition at line 444 of file vision_stereo_rectify/test.cpp.

Member Function Documentation

◆ doProcess()

void CRangeBearing::doProcess ( double  DeltaTime,
double  observationRange,
double  observationBearing 
)

Definition at line 445 of file vision_stereo_rectify/test.cpp.

◆ get_action_size()

static constexpr size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::get_action_size
inlinestaticconstexprinherited

Definition at line 227 of file CKalmanFilterCapable.h.

◆ get_feature_size()

static constexpr size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::get_feature_size
inlinestaticconstexprinherited

Definition at line 226 of file CKalmanFilterCapable.h.

◆ get_observation_size()

static constexpr size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::get_observation_size
inlinestaticconstexprinherited

Definition at line 225 of file CKalmanFilterCapable.h.

◆ get_vehicle_size()

static constexpr size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::get_vehicle_size
inlinestaticconstexprinherited

Definition at line 224 of file CKalmanFilterCapable.h.

◆ getLandmarkCov()

void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::getLandmarkCov ( size_t  idx,
KFMatrix_FxF feat_cov 
) const
inlineinherited

Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).

Exceptions
std::exceptionOn idx>= getNumberOfLandmarksInTheMap()

Definition at line 279 of file CKalmanFilterCapable.h.

◆ getLandmarkMean()

void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::getLandmarkMean ( size_t  idx,
KFArray_FEAT feat 
) const
inlineinherited

Returns the mean of the estimated value of the idx'th landmark (not applicable to non-SLAM problems).

Exceptions
std::exceptionOn idx>= getNumberOfLandmarksInTheMap()

Definition at line 268 of file CKalmanFilterCapable.h.

◆ getNumberOfLandmarksInTheMap()

size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::getNumberOfLandmarksInTheMap
inlineinherited

Definition at line 228 of file CKalmanFilterCapable.h.

◆ getProfiler()

mrpt::system::CTimeLogger& mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::getProfiler
inlineinherited

Definition at line 597 of file CKalmanFilterCapable.h.

◆ getState()

void CRangeBearing::getState ( KFVector xkk,
KFMatrix pkk 
)
inline

Definition at line 66 of file vision_stereo_rectify/test.cpp.

◆ getStateVectorLength()

size_t mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::getStateVectorLength
inlineinherited

Definition at line 261 of file CKalmanFilterCapable.h.

◆ internal_getPkk()

KFMatrix& mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::internal_getPkk
inlineinherited

Definition at line 263 of file CKalmanFilterCapable.h.

◆ internal_getXkk()

KFVector& mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::internal_getXkk
inlineinherited

Definition at line 262 of file CKalmanFilterCapable.h.

◆ isMapEmpty()

bool mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::isMapEmpty
inlineinherited

Definition at line 232 of file CKalmanFilterCapable.h.

◆ KF_aux_estimate_obs_Hx_jacobian()

void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KF_aux_estimate_obs_Hx_jacobian ( const KFArray_VEH x,
const std::pair< KFCLASS *, size_t > &  dat,
KFArray_OBS out_x 
)
staticprivateinherited

Definition at line 954 of file CKalmanFilterCapable_impl.h.

◆ KF_aux_estimate_obs_Hy_jacobian()

void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KF_aux_estimate_obs_Hy_jacobian ( const KFArray_FEAT x,
const std::pair< KFCLASS *, size_t > &  dat,
KFArray_OBS out_x 
)
staticprivateinherited

Definition at line 971 of file CKalmanFilterCapable_impl.h.

◆ KF_aux_estimate_trans_jacobian()

void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KF_aux_estimate_trans_jacobian ( const KFArray_VEH x,
const std::pair< KFCLASS *, KFArray_ACT > &  dat,
KFArray_VEH out_x 
)
staticprivateinherited

Auxiliary functions for Jacobian numeric estimation.

Definition at line 941 of file CKalmanFilterCapable_impl.h.

◆ OnGetAction() [1/2]

void CRangeBearing::OnGetAction ( KFArray_ACT u) const
overrideprotected

Must return the action vector u.

Parameters
out_uThe action vector which will be passed to OnTransitionModel

Definition at line 458 of file vision_stereo_rectify/test.cpp.

◆ OnGetAction() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnGetAction ( KFArray_ACT out_u) const
protectedpure virtualinherited

Must return the action vector u.

Parameters
out_uThe action vector which will be passed to OnTransitionModel

◆ OnGetObservationNoise()

void CRangeBearing::OnGetObservationNoise ( KFMatrix_OxO R) const
overrideprotectedvirtual

Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.

Parameters
out_RThe noise covariance matrix. It might be non diagonal, but it'll usually be.
Note
Upon call, it can be assumed that the previous contents of out_R are all zeros.

Implements mrpt::bayes::CKalmanFilterCapable< 4, 2, 0, 1 >.

Definition at line 509 of file vision_stereo_rectify/test.cpp.

◆ OnGetObservationsAndDataAssociation()

void CRangeBearing::OnGetObservationsAndDataAssociation ( vector_KFArray_OBS out_z,
std::vector< int > &  out_data_association,
const vector_KFArray_OBS in_all_predictions,
const KFMatrix in_S,
const std::vector< size_t > &  in_lm_indices_in_S,
const KFMatrix_OxO in_R 
)
overrideprotectedvirtual

This is called between the KF prediction step and the update step, and the application must return the observations and, when applicable, the data association between these observations and the current map.

Parameters
out_zN vectors, each for one "observation" of length OBS_SIZE, N being the number of "observations": how many observed landmarks for a map, or just one if not applicable.
out_data_associationAn empty vector or, where applicable, a vector where the i'th element corresponds to the position of the observation in the i'th row of out_z within the system state vector (in the range [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and we want to insert it at the end of this KF iteration.
in_all_predictionsA vector with the prediction of ALL the landmarks in the map. Note that, in contrast, in_S only comprises a subset of all the landmarks.
in_SThe full covariance matrix of the observation predictions (i.e. the "innovation covariance matrix"). This is a M·O x M·O matrix with M=length of "in_lm_indices_in_S".
in_lm_indices_in_SThe indices of the map landmarks (range [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.

This method will be called just once for each complete KF iteration.

Note
It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.

Implements mrpt::bayes::CKalmanFilterCapable< 4, 2, 0, 1 >.

Definition at line 515 of file vision_stereo_rectify/test.cpp.

◆ OnInverseObservationModel() [1/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnInverseObservationModel ( [[maybe_unused] ] const KFArray_OBS in_z,
[[maybe_unused] ] KFArray_FEAT out_yn,
[[maybe_unused] ] KFMatrix_FxV out_dyn_dxv,
[[maybe_unused] ] KFMatrix_FxO out_dyn_dhn 
) const
inlinevirtualinherited

If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".

Parameters
in_zThe observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation().
out_ynThe F-length vector with the inverse observation model $ y_n=y(x,z_n) $.
out_dyn_dxvThe $F \times V$ Jacobian of the inv. sensor model wrt the robot pose $ \frac{\partial y_n}{\partial x_v} $.
out_dyn_dhnThe $F \times O$ Jacobian of the inv. sensor model wrt the observation vector $ \frac{\partial y_n}{\partial h_n} $.
  • O: OBS_SIZE
  • V: VEH_SIZE
  • F: FEAT_SIZE
Note
OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.
Deprecated:
This version of the method is deprecated. The alternative method is preferred to allow a greater flexibility.

Definition at line 489 of file CKalmanFilterCapable.h.

◆ OnInverseObservationModel() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnInverseObservationModel ( const KFArray_OBS in_z,
KFArray_FEAT out_yn,
KFMatrix_FxV out_dyn_dxv,
KFMatrix_FxO out_dyn_dhn,
[[maybe_unused] ] KFMatrix_FxF out_dyn_dhn_R_dyn_dhnT,
bool &  out_use_dyn_dhn_jacobian 
) const
inlinevirtualinherited

If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".

The uncertainty in the new map feature comes from two parts: one from the vehicle uncertainty (through the out_dyn_dxv Jacobian), and another from the uncertainty in the observation itself. By default, out_use_dyn_dhn_jacobian=true on call, and if it's left at "true", the base KalmanFilter class will compute the uncertainty of the landmark relative position from out_dyn_dhn. Only in some problems (e.g. MonoSLAM), it'll be needed for the application to directly return the covariance matrix out_dyn_dhn_R_dyn_dhnT, which is the equivalent to:

    \f$ \frac{\partial y_n}{\partial h_n} R \frac{\partial

y_n}{\partial h_n}^\top $. but may be computed from additional terms, or whatever needed by the user. \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation(). \param out_yn The F-length vector with the inverse observation model $ y_n=y(x,z_n) $. \param out_dyn_dxv The $@_fakenlF \times V $ Jacobian of the inv. sensor model wrt the robot pose $ \frac{\partial y_n}{\partial x_v} $. \param out_dyn_dhn The $@_fakenlF \times O $ Jacobian of the inv. sensor model wrt the observation vector $ \frac{\partial y_n}{\partial h_n}

Definition at line 540 of file CKalmanFilterCapable.h.

◆ OnNewLandmarkAddedToMap()

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnNewLandmarkAddedToMap ( [[maybe_unused] ] const size_t  in_obsIdx,
[[maybe_unused] ] const size_t  in_idxNewFeat 
)
inlinevirtualinherited

If applicable to the given problem, do here any special handling of adding a new landmark to the map.

Parameters
in_obsIndexThe index of the observation whose inverse sensor is to be computed. It corresponds to the row in in_z where the observation can be found.
in_idxNewFeatThe index that this new feature will have in the state vector (0:just after the vehicle state, 1: after that,...). Save this number so data association can be done according to these indices.
See also
OnInverseObservationModel

Definition at line 562 of file CKalmanFilterCapable.h.

◆ OnNormalizeStateVector()

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnNormalizeStateVector
inlinevirtualinherited

This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it.

Definition at line 573 of file CKalmanFilterCapable.h.

◆ OnObservationJacobians() [1/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnObservationJacobians ( [[maybe_unused] ] size_t  idx_landmark_to_predict,
[[maybe_unused] ] KFMatrix_OxV Hx,
[[maybe_unused] ] KFMatrix_OxF Hy 
) const
inlineprotectedvirtualinherited

Implements the observation Jacobians $ \frac{\partial h_i}{\partial x} $ and (when applicable) $ \frac{\partial h_i}{\partial y_i} $.

Parameters
idx_landmark_to_predictThe index of the landmark in the map whose prediction is expected as output. For non SLAM-like problems, this will be zero and the expected output is for the whole state vector.
HxThe output Jacobian $ \frac{\partial h_i}{\partial x} $.
HyThe output Jacobian $ \frac{\partial h_i}{\partial y_i} $.

Definition at line 437 of file CKalmanFilterCapable.h.

◆ OnObservationJacobians() [2/2]

void CRangeBearing::OnObservationJacobians ( size_t  idx_landmark_to_predict,
KFMatrix_OxV Hx,
KFMatrix_OxF Hy 
) const
overrideprotected

Implements the observation Jacobians $ \frac{\partial h_i}{\partial x} $ and (when applicable) $ \frac{\partial h_i}{\partial y_i} $.

Implements the observation Jacobians $ \frac{\partial h_i}{\partial x} $ and (when applicable) $ \frac{\partial h_i}{\partial y_i} $.

Parameters
idx_landmark_to_predictThe index of the landmark in the map whose prediction is expected as output. For non SLAM-like problems, this will be zero and the expected output is for the whole state vector.
HxThe output Jacobian $ \frac{\partial h_i}{\partial x} $.
HyThe output Jacobian $ \frac{\partial h_i}{\partial y_i} $.
idx_landmark_to_predictThe index of the landmark in the map whose prediction is expected as output. For non SLAM-like problems, this will be zero and the expected output is for the whole state vector.
HxThe output Jacobian $ \frac{\partial h_i}{\partial x} $.
HyThe output Jacobian $ \frac{\partial h_i}{\partial y_i} $.

Definition at line 559 of file vision_stereo_rectify/test.cpp.

◆ OnObservationJacobiansNumericGetIncrements()

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnObservationJacobiansNumericGetIncrements ( KFArray_VEH out_veh_increments,
KFArray_FEAT out_feat_increments 
) const
inlineprotectedvirtualinherited

Only called if using a numeric approximation of the observation Jacobians, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian.

Definition at line 449 of file CKalmanFilterCapable.h.

◆ OnObservationModel()

void CRangeBearing::OnObservationModel ( const std::vector< size_t > &  idx_landmarks_to_predict,
vector_KFArray_OBS out_predictions 
) const
overrideprotectedvirtual

Implements the observation prediction $ h_i(x) $.

Parameters
idx_landmark_to_predictThe indices of the landmarks in the map whose predictions are expected as output. For non SLAM-like problems, this input value is undefined and the application should just generate one observation for the given problem.
out_predictionsThe predicted observations.

Implements mrpt::bayes::CKalmanFilterCapable< 4, 2, 0, 1 >.

Definition at line 534 of file vision_stereo_rectify/test.cpp.

◆ OnPostIteration()

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnPostIteration
inlinevirtualinherited

This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().

Definition at line 581 of file CKalmanFilterCapable.h.

◆ OnPreComputingPredictions()

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnPreComputingPredictions ( [[maybe_unused] ] const vector_KFArray_OBS in_all_prediction_means,
std::vector< size_t > &  out_LM_indices_to_predict 
) const
inlineprotectedvirtualinherited

This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.

For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.

Parameters
in_all_prediction_meansThe mean of each landmark predictions; the computation or not of the corresponding covariances is what we're trying to determined with this method.
out_LM_indices_to_predictThe list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
Note
This is not a pure virtual method, so it should be implemented only if desired. The default implementation returns a vector with all the landmarks in the map.
See also
OnGetObservations, OnDataAssociation

Definition at line 366 of file CKalmanFilterCapable.h.

◆ OnSubstractObservationVectors() [1/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnSubstractObservationVectors ( KFArray_OBS A,
const KFArray_OBS B 
) const
inlineprotectedvirtualinherited

Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).

Definition at line 460 of file CKalmanFilterCapable.h.

◆ OnSubstractObservationVectors() [2/2]

void CRangeBearing::OnSubstractObservationVectors ( KFArray_OBS A,
const KFArray_OBS B 
) const
overrideprotected

Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).

Definition at line 579 of file vision_stereo_rectify/test.cpp.

◆ OnTransitionJacobian() [1/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnTransitionJacobian ( [ [maybe_unused] ] KFMatrix_VxV out_F) const
inlineprotectedvirtualinherited

Implements the transition Jacobian $ \frac{\partial f}{\partial x} $.

Parameters
out_FMust return the Jacobian. The returned matrix must be $V \times V$ with V being either the size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for SLAM problems).

Definition at line 328 of file CKalmanFilterCapable.h.

◆ OnTransitionJacobian() [2/2]

void CRangeBearing::OnTransitionJacobian ( KFMatrix_VxV F) const
overrideprotected

Implements the transition Jacobian $ \frac{\partial f}{\partial x} $.

Parameters
out_FMust return the Jacobian. The returned matrix must be $N \times N$ with N being either the size of the whole state vector or get_vehicle_size().

Definition at line 483 of file vision_stereo_rectify/test.cpp.

◆ OnTransitionJacobianNumericGetIncrements()

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnTransitionJacobianNumericGetIncrements ( KFArray_VEH out_increments) const
inlineprotectedvirtualinherited

Only called if using a numeric approximation of the transition Jacobian, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian.

Definition at line 338 of file CKalmanFilterCapable.h.

◆ OnTransitionModel() [1/2]

void CRangeBearing::OnTransitionModel ( const KFArray_ACT in_u,
KFArray_VEH inout_x,
bool &  out_skipPrediction 
) const
overrideprotected

Implements the transition model $ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $.

Parameters
in_uThe vector returned by OnGetAction.
inout_xAt input has $ \hat{x}_{k-1|k-1} $, at output must have $ \hat{x}_{k|k-1} $.
out_skipSet this to true if for some reason you want to skip the prediction step (to do not modify either the vector or the covariance). Default:false

Definition at line 468 of file vision_stereo_rectify/test.cpp.

◆ OnTransitionModel() [2/2]

virtual void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::OnTransitionModel ( const KFArray_ACT in_u,
KFArray_VEH inout_x,
bool &  out_skipPrediction 
) const
protectedpure virtualinherited

Implements the transition model $ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $.

Parameters
in_uThe vector returned by OnGetAction.
inout_xAt input has

\[ \hat{x}_{k-1|k-1} \]

, at output must have $ \hat{x}_{k|k-1} $ .
out_skipSet this to true if for some reason you want to skip the prediction step (to do not modify either the vector or the covariance). Default:false
Note
Even if you return "out_skip=true", the value of "inout_x" MUST be updated as usual (this is to allow numeric approximation of Jacobians).

◆ OnTransitionNoise()

void CRangeBearing::OnTransitionNoise ( KFMatrix_VxV Q) const
overrideprotectedvirtual

Implements the transition noise covariance $ Q_k $.

Parameters
out_QMust return the covariance matrix. The returned matrix must be of the same size than the jacobian from OnTransitionJacobian

Implements mrpt::bayes::CKalmanFilterCapable< 4, 2, 0, 1 >.

Definition at line 496 of file vision_stereo_rectify/test.cpp.

◆ runOneKalmanIteration()

void mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::runOneKalmanIteration
protectedinherited

The main entry point, executes one complete step: prediction + update.

It is protected since derived classes must provide a problem-specific entry point for users. The exact order in which this method calls the virtual method is explained in https://www.mrpt.org/Kalman_Filters

Definition at line 28 of file CKalmanFilterCapable_impl.h.

Member Data Documentation

◆ KF_options

TKF_options mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::KF_options
inherited

Generic options for the Kalman Filter algorithm itself.

Definition at line 599 of file CKalmanFilterCapable.h.

◆ m_all_predictions

vector_KFArray_OBS mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_all_predictions
privateinherited

Definition at line 605 of file CKalmanFilterCapable.h.

◆ m_aux_K_dh_dx

KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_aux_K_dh_dx
privateinherited

Definition at line 616 of file CKalmanFilterCapable.h.

◆ m_deltaTime

float CRangeBearing::m_deltaTime
protected

Definition at line 74 of file vision_stereo_rectify/test.cpp.

◆ m_dh_dx_full_obs

KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_dh_dx_full_obs
privateinherited

Definition at line 615 of file CKalmanFilterCapable.h.

◆ m_Hxs

std::vector<KFMatrix_OxV> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_Hxs
privateinherited

The vector of all partial Jacobians dh[i]_dx for each prediction.

Definition at line 608 of file CKalmanFilterCapable.h.

◆ m_Hys

std::vector<KFMatrix_OxF> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_Hys
privateinherited

The vector of all partial Jacobians dh[i]_dy[i] for each prediction.

Definition at line 610 of file CKalmanFilterCapable.h.

◆ m_K

KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_K
privateinherited

Definition at line 613 of file CKalmanFilterCapable.h.

◆ m_obsBearing

float CRangeBearing::m_obsBearing
protected

Definition at line 73 of file vision_stereo_rectify/test.cpp.

◆ m_obsRange

float CRangeBearing::m_obsRange
protected

Definition at line 73 of file vision_stereo_rectify/test.cpp.

◆ m_pkk

KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_pkk
protectedinherited

The system full covariance matrix.

Definition at line 292 of file CKalmanFilterCapable.h.

◆ m_predictLMidxs

std::vector<size_t> mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_predictLMidxs
privateinherited

Definition at line 606 of file CKalmanFilterCapable.h.

◆ m_S

KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_S
privateinherited

Definition at line 611 of file CKalmanFilterCapable.h.

◆ m_S_1

KFMatrix mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_S_1
privateinherited

Definition at line 614 of file CKalmanFilterCapable.h.

◆ m_timLogger

mrpt::system::CTimeLogger mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_timLogger
protectedinherited

Definition at line 296 of file CKalmanFilterCapable.h.

◆ m_user_didnt_implement_jacobian

bool mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_user_didnt_implement_jacobian
mutableprivateinherited

Definition at line 628 of file CKalmanFilterCapable.h.

◆ m_xkk

KFVector mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_xkk
protectedinherited

The system state vector.

Definition at line 290 of file CKalmanFilterCapable.h.

◆ m_Z

vector_KFArray_OBS mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double >::m_Z
privateinherited

Definition at line 612 of file CKalmanFilterCapable.h.




Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 15:15:43 UTC 2020