mirror of
https://github.com/opencv/opencv_contrib.git
synced 2025-10-20 04:25:42 +08:00
Implement Unscented Kalman Filter (UKF) and Augmented UKF
This commit is contained in:
SvetlanaFilicheva
223
modules/tracking/include/opencv2/tracking/kalman_filters.hpp
Normal file
223
modules/tracking/include/opencv2/tracking/kalman_filters.hpp
Normal file
@@ -0,0 +1,223 @@
|
||||
/*M///////////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
|
||||
//
|
||||
// By downloading, copying, installing or using the software you agree to this license.
|
||||
// If you do not agree to this license, do not download, install,
|
||||
// copy or use the software.
|
||||
//
|
||||
//
|
||||
// License Agreement
|
||||
// For Open Source Computer Vision Library
|
||||
//
|
||||
// Copyright (C) 2015, OpenCV Foundation, all rights reserved.
|
||||
// Third party copyrights are property of their respective owners.
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without modification,
|
||||
// are permitted provided that the following conditions are met:
|
||||
//
|
||||
// * Redistribution's of source code must retain the above copyright notice,
|
||||
// this list of conditions and the following disclaimer.
|
||||
//
|
||||
// * Redistribution's in binary form must reproduce the above copyright notice,
|
||||
// this list of conditions and the following disclaimer in the documentation
|
||||
// and/or other materials provided with the distribution.
|
||||
//
|
||||
// * The name of the copyright holders may not be used to endorse or promote products
|
||||
// derived from this software without specific prior written permission.
|
||||
//
|
||||
// This software is provided by the copyright holders and contributors "as is" and
|
||||
// any express or implied warranties, including, but not limited to, the implied
|
||||
// warranties of merchantability and fitness for a particular purpose are disclaimed.
|
||||
// In no event shall the Intel Corporation or contributors be liable for any direct,
|
||||
// indirect, incidental, special, exemplary, or consequential damages
|
||||
// (including, but not limited to, procurement of substitute goods or services;
|
||||
// loss of use, data, or profits; or business interruption) however caused
|
||||
// and on any theory of liability, whether in contract, strict liability,
|
||||
// or tort (including negligence or otherwise) arising in any way out of
|
||||
// the use of this software, even if advised of the possibility of such damage.
|
||||
//
|
||||
//M*/
|
||||
|
||||
#ifndef __OPENCV_TRACKING_KALMAN_HPP_
|
||||
#define __OPENCV_TRACKING_KALMAN_HPP_
|
||||
|
||||
#include "opencv2/core.hpp"
|
||||
#include <limits>
|
||||
|
||||
namespace cv
|
||||
{
|
||||
namespace tracking
|
||||
{
|
||||
|
||||
/** @brief The interface for Unscented Kalman filter and Augmented Unscented Kalman filter.
|
||||
*/
|
||||
class CV_EXPORTS UnscentedKalmanFilter
|
||||
{
|
||||
public:
|
||||
|
||||
virtual ~UnscentedKalmanFilter(){}
|
||||
|
||||
/** The function performs prediction step of the algorithm
|
||||
* @param control - the current control vector,
|
||||
* @return the predicted estimate of the state.
|
||||
*/
|
||||
virtual Mat predict( const Mat& control = Mat() ) = 0;
|
||||
|
||||
/** The function performs correction step of the algorithm
|
||||
* @param measurement - the current measurement vector,
|
||||
* @return the corrected estimate of the state.
|
||||
*/
|
||||
virtual Mat correct( const Mat& measurement ) = 0;
|
||||
|
||||
/**
|
||||
* @return the process noise cross-covariance matrix.
|
||||
*/
|
||||
virtual Mat getProcessNoiseCov() const = 0;
|
||||
|
||||
/**
|
||||
* @return the measurement noise cross-covariance matrix.
|
||||
*/
|
||||
virtual Mat getMeasurementNoiseCov() const = 0;
|
||||
|
||||
/**
|
||||
* @return the current estimate of the state.
|
||||
*/
|
||||
virtual Mat getState() const = 0;
|
||||
};
|
||||
|
||||
/** @brief Model of dynamical system for Unscented Kalman filter.
|
||||
* The interface for dynamical system model. It contains functions for computing the next state and the measurement.
|
||||
* It must be inherited for using UKF.
|
||||
*/
|
||||
class CV_EXPORTS UkfSystemModel
|
||||
{
|
||||
public:
|
||||
|
||||
virtual ~UkfSystemModel(){}
|
||||
|
||||
/** The function for computing the next state from the previous state
|
||||
* @param x_k - previous state vector,
|
||||
* @param u_k - control vector,
|
||||
* @param v_k - noise vector,
|
||||
* @param x_kplus1 - next state vector.
|
||||
*/
|
||||
virtual void stateConversionFunction( const Mat& x_k, const Mat& u_k, const Mat& v_k, Mat& x_kplus1 ) = 0;
|
||||
/** The function for computing the measurement from the state
|
||||
* @param x_k - state vector,
|
||||
* @param n_k - noise vector,
|
||||
* @param z_k - measurement vector.
|
||||
*/
|
||||
virtual void measurementFunction( const Mat& x_k, const Mat& n_k, Mat& z_k ) = 0;
|
||||
};
|
||||
|
||||
|
||||
/** @brief Unscented Kalman filter parameters.
|
||||
* The class for initialization parameters of Unscented Kalman filter
|
||||
*/
|
||||
class CV_EXPORTS UnscentedKalmanFilterParams
|
||||
{
|
||||
public:
|
||||
|
||||
int DP; //!< Dimensionality of the state vector.
|
||||
int MP; //!< Dimensionality of the measurement vector.
|
||||
int CP; //!< Dimensionality of the control vector.
|
||||
int dataType; //!< Type of elements of vectors and matrices, default is CV_64F.
|
||||
|
||||
Mat stateInit; //!< Initial state, DP x 1, default is zero.
|
||||
Mat errorCovInit; //!< State estimate cross-covariance matrix, DP x DP, default is identity.
|
||||
|
||||
Mat processNoiseCov; //!< Process noise cross-covariance matrix, DP x DP.
|
||||
Mat measurementNoiseCov; //!< Measurement noise cross-covariance matrix, MP x MP.
|
||||
|
||||
// Parameters of algorithm
|
||||
double alpha; //!< Default is 1e-3.
|
||||
double k; //!< Default is 0.
|
||||
double beta; //!< Default is 2.0.
|
||||
|
||||
//Dynamical system model
|
||||
Ptr<UkfSystemModel> model; //!< Object of the class containing functions for computing the next state and the measurement.
|
||||
|
||||
/** The constructors.
|
||||
*/
|
||||
UnscentedKalmanFilterParams(){}
|
||||
|
||||
/**
|
||||
* @param dp - dimensionality of the state vector,
|
||||
* @param mp - dimensionality of the measurement vector,
|
||||
* @param cp - dimensionality of the control vector,
|
||||
* @param processNoiseCovDiag - value of elements on main diagonal process noise cross-covariance matrix,
|
||||
* @param measurementNoiseCovDiag - value of elements on main diagonal measurement noise cross-covariance matrix,
|
||||
* @param dynamicalSystem - ptr to object of the class containing functions for computing the next state and the measurement,
|
||||
* @param type - type of the created matrices that should be CV_32F or CV_64F.
|
||||
*/
|
||||
UnscentedKalmanFilterParams( int dp, int mp, int cp, double processNoiseCovDiag, double measurementNoiseCovDiag,
|
||||
Ptr<UkfSystemModel> dynamicalSystem, int type = CV_64F );
|
||||
|
||||
/** The function for initialization of Unscented Kalman filter
|
||||
* @param dp - dimensionality of the state vector,
|
||||
* @param mp - dimensionality of the measurement vector,
|
||||
* @param cp - dimensionality of the control vector,
|
||||
* @param processNoiseCovDiag - value of elements on main diagonal process noise cross-covariance matrix,
|
||||
* @param measurementNoiseCovDiag - value of elements on main diagonal measurement noise cross-covariance matrix,
|
||||
* @param dynamicalSystem - ptr to object of the class containing functions for computing the next state and the measurement,
|
||||
* @param type - type of the created matrices that should be CV_32F or CV_64F.
|
||||
*/
|
||||
void init( int dp, int mp, int cp, double processNoiseCovDiag, double measurementNoiseCovDiag,
|
||||
Ptr<UkfSystemModel> dynamicalSystem, int type = CV_64F );
|
||||
};
|
||||
|
||||
/** @brief Augmented Unscented Kalman filter parameters.
|
||||
* The class for initialization parameters of Augmented Unscented Kalman filter
|
||||
*/
|
||||
class CV_EXPORTS AugmentedUnscentedKalmanFilterParams: public UnscentedKalmanFilterParams
|
||||
{
|
||||
public:
|
||||
|
||||
AugmentedUnscentedKalmanFilterParams(){}
|
||||
|
||||
/**
|
||||
* @param dp - dimensionality of the state vector,
|
||||
* @param mp - dimensionality of the measurement vector,
|
||||
* @param cp - dimensionality of the control vector,
|
||||
* @param processNoiseCovDiag - value of elements on main diagonal process noise cross-covariance matrix,
|
||||
* @param measurementNoiseCovDiag - value of elements on main diagonal measurement noise cross-covariance matrix,
|
||||
* @param dynamicalSystem - ptr to object of the class containing functions for computing the next state and the measurement,
|
||||
* @param type - type of the created matrices that should be CV_32F or CV_64F.
|
||||
*/
|
||||
AugmentedUnscentedKalmanFilterParams( int dp, int mp, int cp, double processNoiseCovDiag, double measurementNoiseCovDiag,
|
||||
Ptr<UkfSystemModel> dynamicalSystem, int type = CV_64F );
|
||||
|
||||
/** The function for initialization of Augmented Unscented Kalman filter
|
||||
* @param dp - dimensionality of the state vector,
|
||||
* @param mp - dimensionality of the measurement vector,
|
||||
* @param cp - dimensionality of the control vector,
|
||||
* @param processNoiseCovDiag - value of elements on main diagonal process noise cross-covariance matrix,
|
||||
* @param measurementNoiseCovDiag - value of elements on main diagonal measurement noise cross-covariance matrix,
|
||||
* @param dynamicalSystem - object of the class containing functions for computing the next state and the measurement,
|
||||
* @param type - type of the created matrices that should be CV_32F or CV_64F.
|
||||
*/
|
||||
void init( int dp, int mp, int cp, double processNoiseCovDiag, double measurementNoiseCovDiag,
|
||||
Ptr<UkfSystemModel> dynamicalSystem, int type = CV_64F );
|
||||
};
|
||||
|
||||
/** @brief Unscented Kalman Filter factory method
|
||||
|
||||
* The class implements an Unscented Kalman filter <https://en.wikipedia.org/wiki/Kalman_filter#Unscented_Kalman_filter>.
|
||||
* @param params - an object of the UnscentedKalmanFilterParams class containing UKF parameters.
|
||||
* @return pointer to the object of the UnscentedKalmanFilterImpl class implementing UnscentedKalmanFilter.
|
||||
*/
|
||||
CV_EXPORTS Ptr<UnscentedKalmanFilter> createUnscentedKalmanFilter( const UnscentedKalmanFilterParams ¶ms );
|
||||
/** @brief Augmented Unscented Kalman Filter factory method
|
||||
|
||||
* The class implements an Augmented Unscented Kalman filter http://becs.aalto.fi/en/research/bayes/ekfukf/documentation.pdf, page 31-33.
|
||||
* AUKF is more accurate than UKF but its computational complexity is larger.
|
||||
* @param params - an object of the AugmentedUnscentedKalmanFilterParams class containing AUKF parameters.
|
||||
* @return pointer to the object of the AugmentedUnscentedKalmanFilterImpl class implementing UnscentedKalmanFilter.
|
||||
*/
|
||||
CV_EXPORTS Ptr<UnscentedKalmanFilter> createAugmentedUnscentedKalmanFilter( const AugmentedUnscentedKalmanFilterParams ¶ms );
|
||||
|
||||
} // tracking
|
||||
} // cv
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user