diff --git a/modules/tracking/include/opencv2/tracking/kalman_filters.hpp b/modules/tracking/include/opencv2/tracking/kalman_filters.hpp new file mode 100644 index 000000000..a1afc1b71 --- /dev/null +++ b/modules/tracking/include/opencv2/tracking/kalman_filters.hpp @@ -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 + +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 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 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 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 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 dynamicalSystem, int type = CV_64F ); +}; + +/** @brief Unscented Kalman Filter factory method + +* The class implements an 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 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 createAugmentedUnscentedKalmanFilter( const AugmentedUnscentedKalmanFilterParams ¶ms ); + +} // tracking +} // cv + +#endif diff --git a/modules/tracking/src/augmented_unscented_kalman.cpp b/modules/tracking/src/augmented_unscented_kalman.cpp new file mode 100644 index 000000000..43b963c43 --- /dev/null +++ b/modules/tracking/src/augmented_unscented_kalman.cpp @@ -0,0 +1,440 @@ +/*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*/ + +#include "precomp.hpp" +#include "opencv2/tracking/kalman_filters.hpp" + +namespace cv +{ +namespace tracking +{ + +/* Cholesky decomposition + The function performs Cholesky decomposition . + A - the Hermitian, positive-definite matrix, + astep - size of row in A, + asize - number of cols and rows in A, + L - the lower triangular matrix, A = L*Lt. +*/ +template bool +inline choleskyDecomposition( const _Tp* A, size_t astep, const int asize, _Tp* L ) +{ + int i, j, k; + double s; + astep /= sizeof(A[0]); + for( i = 0; i < asize; i++ ) + + { + for( j = 0; j < i; j++ ) + { + s = A[i*astep + j]; + for( k = 0; k < j; k++ ) + s -= L[i*astep + k]*L[j*astep + k]; + L[i*astep + j] = (_Tp)(s/L[j*astep + j]); + } + s = A[i*astep + i]; + for( k = 0; k < i; k++ ) + { + double t = L[i*astep + k]; + s -= t*t; + } + if( s < std::numeric_limits<_Tp>::epsilon() ) + return false; + L[i*astep + i] = (_Tp)(std::sqrt(s)); + } + + for( i = 0; i < asize; i++ ) + for( j = i+1; j < asize; j++ ) + { + L[i*astep + j] = 0.0; + } + + return true; +} + + +void AugmentedUnscentedKalmanFilterParams:: + init( int dp, int mp, int cp, double processNoiseCovDiag, double measurementNoiseCovDiag, + Ptr dynamicalSystem, int type ) +{ + CV_Assert( dp > 0 && mp > 0 ); + DP = dp; + MP = mp; + CP = std::max( cp, 0 ); + CV_Assert( type == CV_32F || type == CV_64F ); + dataType = type; + + this->model = dynamicalSystem; + + stateInit = Mat::zeros(DP, 1, type); + errorCovInit = Mat::eye(DP, DP, type); + + processNoiseCov = processNoiseCovDiag*Mat::eye(DP, DP, type); + measurementNoiseCov = measurementNoiseCovDiag*Mat::eye(MP, MP, type); + + alpha = 1e-3; + k = 0.0; + beta = 2.0; +} + +AugmentedUnscentedKalmanFilterParams:: + AugmentedUnscentedKalmanFilterParams( int dp, int mp, int cp, double processNoiseCovDiag, double measurementNoiseCovDiag, + Ptr dynamicalSystem, int type ) +{ + init( dp, mp, cp, processNoiseCovDiag, measurementNoiseCovDiag, dynamicalSystem, type ); +} + + +class AugmentedUnscentedKalmanFilterImpl: public UnscentedKalmanFilter +{ + + int DP; // dimensionality of the state vector + int MP; // dimensionality of the measurement vector + int CP; // dimensionality of the control vector + int DAug; // dimensionality of the augmented vector, DAug = 2*DP + MP + int dataType; // type of elements of vectors and matrices + + Mat state; // estimate of the system state (x*), DP x 1 + Mat errorCov; // estimate of the state cross-covariance matrix (P), DP x DP + + Mat stateAug; // augmented state vector (xa*), DAug x 1, + // xa* = ( x* + // 0 + // ... + // 0 ) + Mat errorCovAug; // estimate of the state cross-covariance matrix (Pa), DAug x DAug + // Pa = ( P, 0, 0 + // 0, Q, 0 + // 0, 0, R ) + + Mat processNoiseCov; // process noise cross-covariance matrix (Q), DP x DP + Mat measurementNoiseCov; // measurement noise cross-covariance matrix (R), MP x MP + + Ptr model; // object of the class containing functions for computing the next state and the measurement. + +// Parameters of algorithm + double alpha; // parameter, default is 1e-3 + double k; // parameter, default is 0 + double beta; // parameter, default is 2.0 + + double lambda; // internal parameter, lambda = alpha*alpha*( DP + k ) - DP; + double tmpLambda; // internal parameter, tmpLambda = alpha*alpha*( DP + k ); + +// Auxillary members + Mat measurementEstimate; // estimate of current measurement (y*), MP x 1 + + Mat sigmaPoints; // set of sigma points ( x_i, i = 1..2*DP+1 ), DP x 2*DP+1 + + Mat transitionSPFuncVals; // set of state function values at sigma points ( f_i, i = 1..2*DP+1 ), DP x 2*DP+1 + Mat measurementSPFuncVals; // set of measurement function values at sigma points ( h_i, i = 1..2*DP+1 ), MP x 2*DP+1 + + Mat transitionSPFuncValsCenter; // set of state function values at sigma points minus estimate of state ( fc_i, i = 1..2*DP+1 ), DP x 2*DP+1 + Mat measurementSPFuncValsCenter; // set of measurement function values at sigma points minus estimate of measurement ( hc_i, i = 1..2*DP+1 ), MP x 2*DP+1 + + Mat Wm; // vector of weights for estimate mean, 2*DP+1 x 1 + Mat Wc; // matrix of weights for estimate covariance, 2*DP+1 x 2*DP+1 + + Mat gain; // Kalman gain matrix (K), DP x MP + Mat xyCov; // estimate of the covariance between x* and y* (Sxy), DP x MP + Mat yyCov; // estimate of the y* cross-covariance matrix (Syy), MP x MP + + Mat r; // zero vector of process noise for getting transitionSPFuncVals, + Mat q; // zero vector of measurement noise for getting measurementSPFuncVals + + + template + Mat getSigmaPoints(const Mat& mean, const Mat& covMatrix, double coef); + + Mat getSigmaPoints(const Mat& mean, const Mat& covMatrix, double coef); + +public: + + AugmentedUnscentedKalmanFilterImpl(const AugmentedUnscentedKalmanFilterParams& params); + ~AugmentedUnscentedKalmanFilterImpl(); + + Mat predict(const Mat& control); + Mat correct(const Mat& measurement); + + Mat getProcessNoiseCov() const; + Mat getMeasurementNoiseCov() const; + + Mat getState() const; + +}; + +AugmentedUnscentedKalmanFilterImpl::AugmentedUnscentedKalmanFilterImpl(const AugmentedUnscentedKalmanFilterParams& params) +{ + alpha = params.alpha; + beta = params.beta; + k = params.k; + + CV_Assert( params.DP > 0 && params.MP > 0 ); + CV_Assert( params.dataType == CV_32F || params.dataType == CV_64F ); + DP = params.DP; + MP = params.MP; + CP = std::max( params.CP, 0 ); + dataType = params.dataType; + + DAug = DP + DP + MP; + + model = params.model; + + stateAug = Mat::zeros( DAug, 1, dataType ); + state = stateAug( Rect( 0, 0, 1, DP )); + + CV_Assert( params.stateInit.cols == 1 && params.stateInit.rows == DP ); + params.stateInit.copyTo(state); + + CV_Assert( params.processNoiseCov.cols == DP && params.processNoiseCov.rows == DP ); + CV_Assert( params.measurementNoiseCov.cols == MP && params.measurementNoiseCov.rows == MP ); + processNoiseCov = params.processNoiseCov.clone(); + measurementNoiseCov = params.measurementNoiseCov.clone(); + + errorCovAug = Mat::zeros( DAug, DAug, dataType ); + errorCov = errorCovAug( Rect( 0, 0, DP, DP ) ); + Mat Q = errorCovAug( Rect( DP, DP, DP, DP ) ); + Mat R = errorCovAug( Rect( 2*DP, 2*DP, MP, MP ) ); + processNoiseCov.copyTo( Q ); + measurementNoiseCov.copyTo( R ); + + CV_Assert( params.errorCovInit.cols == DP && params.errorCovInit.rows == DP ); + params.errorCovInit.copyTo( errorCov ); + + measurementEstimate = Mat::zeros( MP, 1, dataType); + + gain = Mat::zeros( DAug, DAug, dataType ); + + transitionSPFuncVals = Mat::zeros( DP, 2*DAug+1, dataType ); + measurementSPFuncVals = Mat::zeros( MP, 2*DAug+1, dataType ); + + transitionSPFuncValsCenter = Mat::zeros( DP, 2*DAug+1, dataType ); + measurementSPFuncValsCenter = Mat::zeros( MP, 2*DAug+1, dataType ); + + lambda = alpha*alpha*( DAug + k ) - DAug; + tmpLambda = lambda + DAug; + + double tmp2Lambda = 0.5/tmpLambda; + + Wm = tmp2Lambda * Mat::ones( 2*DAug+1, 1, dataType ); + Wc = tmp2Lambda * Mat::eye( 2*DAug+1, 2*DAug+1, dataType ); + + if ( dataType == CV_64F ) + { + Wm.at(0,0) = lambda/tmpLambda; + Wc.at(0,0) = lambda/tmpLambda + 1.0 - alpha*alpha + beta; + } + else + { + Wm.at(0,0) = (float)(lambda/tmpLambda); + Wc.at(0,0) = (float)(lambda/tmpLambda + 1.0 - alpha*alpha + beta); + } + +} + +AugmentedUnscentedKalmanFilterImpl::~AugmentedUnscentedKalmanFilterImpl() +{ + stateAug.release(); + errorCovAug.release(); + + state.release(); + errorCov.release(); + + processNoiseCov.release(); + measurementNoiseCov.release(); + + measurementEstimate.release(); + + sigmaPoints.release(); + + transitionSPFuncVals.release(); + measurementSPFuncVals.release(); + + transitionSPFuncValsCenter.release(); + measurementSPFuncValsCenter.release(); + + Wm.release(); + Wc.release(); + + gain.release(); + xyCov.release(); + yyCov.release(); + + r.release(); + q.release(); + +} + +template +Mat AugmentedUnscentedKalmanFilterImpl::getSigmaPoints(const Mat &mean, const Mat &covMatrix, double coef) +{ +// x_0 = mean +// x_i = mean + coef * cholesky( covMatrix ), i = 1..n +// x_(i+n) = mean - coef * cholesky( covMatrix ), i = 1..n + + int n = mean.rows; + Mat points = repeat(mean, 1, 2*n+1); + +// covMatrixL = cholesky( covMatrix ) + Mat covMatrixL = covMatrix.clone(); + covMatrixL.setTo(0); + + choleskyDecomposition( covMatrix.ptr(), covMatrix.step, covMatrix.rows, covMatrixL.ptr() ); + covMatrixL = coef * covMatrixL; + + Mat p_plus = points( Rect( 1, 0, n, n ) ); + Mat p_minus = points( Rect( n+1, 0, n, n ) ); + + add(p_plus, covMatrixL, p_plus); + subtract(p_minus, covMatrixL, p_minus); + + return points; +} + +Mat AugmentedUnscentedKalmanFilterImpl::getSigmaPoints(const Mat& mean, const Mat& covMatrix, double coef) +{ + if ( dataType == CV_64F ) return getSigmaPoints(mean, covMatrix, coef); + if ( dataType == CV_32F ) return getSigmaPoints(mean, covMatrix, coef); + + return Mat(); +} + +Mat AugmentedUnscentedKalmanFilterImpl::predict(const Mat& control) +{ +// get sigma points from xa* and Pa + sigmaPoints = getSigmaPoints( stateAug, errorCovAug, sqrt( tmpLambda ) ); + +// compute f-function values at sigma points +// f_i = f(x_i[0:DP-1], control, x_i[DP:2*DP-1]), i = 0..2*DAug + Mat x, fx; + for ( int i = 0; i<2*DAug+1; i++) + { + x = sigmaPoints( Rect( i, 0, 1, DP) ); + q = sigmaPoints( Rect( i, DP, 1, DP) ); + fx = transitionSPFuncVals( Rect( i, 0, 1, DP) ); + model->stateConversionFunction( x, control, q, fx ); + } + +// compute the estimate of state as mean f-function value at sigma point +// x* = SUM_{i=0}^{2*DAug}( Wm[i]*f_i ) + state = transitionSPFuncVals * Wm; + +// compute f-function values at sigma points minus estimate of state +// fc_i = f_i - x*, i = 0..2*DAug + subtract(transitionSPFuncVals, repeat( state, 1, 2*DAug+1 ), transitionSPFuncValsCenter); + +// compute the estimate of the state cross-covariance matrix +// P = SUM_{i=0}^{2*DAug}( Wc[i]*fc_i*fc_i.t ) + errorCov = transitionSPFuncValsCenter * Wc * transitionSPFuncValsCenter.t(); + + return state.clone(); +} + +Mat AugmentedUnscentedKalmanFilterImpl::correct(const Mat& measurement) +{ +// get sigma points from xa* and Pa + sigmaPoints = getSigmaPoints( stateAug, errorCovAug, sqrt( tmpLambda ) ); + +// compute h-function values at sigma points +// h_i = h(x_i[0:DP-1], x_i[2*DP:DAug-1]), i = 0..2*DAug + Mat x, hx; + measurementEstimate.setTo(0); + for ( int i = 0; i<2*DAug+1; i++) + { + x = transitionSPFuncVals( Rect( i, 0, 1, DP) ); + r = sigmaPoints( Rect( i, 2*DP, 1, MP) ); + hx = measurementSPFuncVals( Rect( i, 0, 1, MP) ); + model->measurementFunction( x, r, hx ); + } + +// compute the estimate of measurement as mean h-function value at sigma point +// y* = SUM_{i=0}^{2*DAug}( Wm[i]*h_i ) + measurementEstimate = measurementSPFuncVals * Wm; + +// compute h-function values at sigma points minus estimate of state +// hc_i = h_i - y*, i = 0..2*DAug + subtract(measurementSPFuncVals, repeat( measurementEstimate, 1, 2*DAug+1 ), measurementSPFuncValsCenter); + +// compute the estimate of the y* cross-covariance matrix +// Syy = SUM_{i=0}^{2*DAug}( Wc[i]*hc_i*hc_i.t ) + yyCov = measurementSPFuncValsCenter * Wc * measurementSPFuncValsCenter.t(); + +// compute the estimate of the covariance between x* and y* +// Sxy = SUM_{i=0}^{2*DAug}( Wc[i]*fc_i*hc_i.t ) + xyCov = transitionSPFuncValsCenter * Wc * measurementSPFuncValsCenter.t(); + +// compute the Kalman gain matrix +// K = Sxy * Syy^(-1) + gain = xyCov * yyCov.inv(DECOMP_SVD); + +// compute the corrected estimate of state +// x* = x* + K*(y - y*), y - current measurement + state = state + gain * ( measurement - measurementEstimate ); + +// compute the corrected estimate of the state cross-covariance matrix +// P = P - K*Sxy.t + errorCov = errorCov - gain * xyCov.t(); + + return state.clone(); +} + +Mat AugmentedUnscentedKalmanFilterImpl::getProcessNoiseCov() const +{ + return processNoiseCov.clone(); +} + +Mat AugmentedUnscentedKalmanFilterImpl::getMeasurementNoiseCov() const +{ + return measurementNoiseCov.clone(); +} + +Mat AugmentedUnscentedKalmanFilterImpl::getState() const +{ + return state.clone(); +} + +Ptr createAugmentedUnscentedKalmanFilter(const AugmentedUnscentedKalmanFilterParams ¶ms) +{ + Ptr kfu( new AugmentedUnscentedKalmanFilterImpl(params) ); + return kfu; +} + +} // tracking +} // cv diff --git a/modules/tracking/src/unscented_kalman.cpp b/modules/tracking/src/unscented_kalman.cpp new file mode 100644 index 000000000..5955f3bd4 --- /dev/null +++ b/modules/tracking/src/unscented_kalman.cpp @@ -0,0 +1,415 @@ +/*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*/ + +#include "precomp.hpp" +#include "opencv2/tracking/kalman_filters.hpp" + +namespace cv +{ +namespace tracking +{ + +/* Cholesky decomposition + The function performs Cholesky decomposition . + A - the Hermitian, positive-definite matrix, + astep - size of row in A, + asize - number of cols and rows in A, + L - the lower triangular matrix, A = L*Lt. +*/ +template bool +inline choleskyDecomposition( const _Tp* A, size_t astep, const int asize, _Tp* L ) +{ + int i, j, k; + double s; + astep /= sizeof(A[0]); + for( i = 0; i < asize; i++ ) + + { + for( j = 0; j < i; j++ ) + { + s = A[i*astep + j]; + for( k = 0; k < j; k++ ) + s -= L[i*astep + k]*L[j*astep + k]; + L[i*astep + j] = (_Tp)(s/L[j*astep + j]); + } + s = A[i*astep + i]; + for( k = 0; k < i; k++ ) + { + double t = L[i*astep + k]; + s -= t*t; + } + if( s < std::numeric_limits<_Tp>::epsilon() ) + return false; + L[i*astep + i] = (_Tp)(std::sqrt(s)); + } + + for( i = 0; i < asize; i++ ) + for( j = i+1; j < asize; j++ ) + { + L[i*astep + j] = 0.0; + } + + return true; +} + +void UnscentedKalmanFilterParams:: + init( int dp, int mp, int cp, double processNoiseCovDiag, double measurementNoiseCovDiag, + Ptr dynamicalSystem, int type ) +{ + CV_Assert( dp > 0 && mp > 0 ); + DP = dp; + MP = mp; + CP = std::max( cp, 0 ); + CV_Assert( type == CV_32F || type == CV_64F ); + dataType = type; + + this->model = dynamicalSystem; + + stateInit = Mat::zeros( DP, 1, type ); + errorCovInit = Mat::eye( DP, DP, type ); + + processNoiseCov = processNoiseCovDiag*Mat::eye( DP, DP, type ); + measurementNoiseCov = measurementNoiseCovDiag*Mat::eye( MP, MP, type ); + + alpha = 1e-3; + k = 0.0; + beta = 2.0; +} + +UnscentedKalmanFilterParams:: + UnscentedKalmanFilterParams( int dp, int mp, int cp, double processNoiseCovDiag, double measurementNoiseCovDiag, + Ptr dynamicalSystem, int type ) +{ + init( dp, mp, cp, processNoiseCovDiag, measurementNoiseCovDiag, dynamicalSystem, type ); +} + +class UnscentedKalmanFilterImpl: public UnscentedKalmanFilter +{ + + 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 + + Mat state; // estimate of the system state (x*), DP x 1 + Mat errorCov; // estimate of the state cross-covariance matrix (P), DP x DP + + Mat processNoiseCov; // process noise cross-covariance matrix (Q), DP x DP + Mat measurementNoiseCov; // measurement noise cross-covariance matrix (R), MP x MP + + Ptr model; // object of the class containing functions for computing the next state and the measurement. + +// Parameters of algorithm + double alpha; // parameter, default is 1e-3 + double k; // parameter, default is 0 + double beta; // parameter, default is 2.0 + + double lambda; // internal parameter, lambda = alpha*alpha*( DP + k ) - DP; + double tmpLambda; // internal parameter, tmpLambda = alpha*alpha*( DP + k ); + +// Auxillary members + Mat measurementEstimate; // estimate of current measurement (y*), MP x 1 + + Mat sigmaPoints; // set of sigma points ( x_i, i = 1..2*DP+1 ), DP x 2*DP+1 + + Mat transitionSPFuncVals; // set of state function values at sigma points ( f_i, i = 1..2*DP+1 ), DP x 2*DP+1 + Mat measurementSPFuncVals; // set of measurement function values at sigma points ( h_i, i = 1..2*DP+1 ), MP x 2*DP+1 + + Mat transitionSPFuncValsCenter; // set of state function values at sigma points minus estimate of state ( fc_i, i = 1..2*DP+1 ), DP x 2*DP+1 + Mat measurementSPFuncValsCenter; // set of measurement function values at sigma points minus estimate of measurement ( hc_i, i = 1..2*DP+1 ), MP x 2*DP+1 + + Mat Wm; // vector of weights for estimate mean, 2*DP+1 x 1 + Mat Wc; // matrix of weights for estimate covariance, 2*DP+1 x 2*DP+1 + + Mat gain; // Kalman gain matrix (K), DP x MP + Mat xyCov; // estimate of the covariance between x* and y* (Sxy), DP x MP + Mat yyCov; // estimate of the y* cross-covariance matrix (Syy), MP x MP + + Mat r; // zero vector of process noise for getting transitionSPFuncVals, + Mat q; // zero vector of measurement noise for getting measurementSPFuncVals + + +//get sigma points + template + Mat getSigmaPoints( const Mat& mean, const Mat& covMatrix, double coef ); + + Mat getSigmaPoints( const Mat& mean, const Mat& covMatrix, double coef ); + +public: + + UnscentedKalmanFilterImpl( const UnscentedKalmanFilterParams& params ); + ~UnscentedKalmanFilterImpl(); + +// perform prediction step +// control - the optional control vector, CP x 1 + Mat predict( const Mat& control = Mat() ); + +// perform correction step +// measurement - current measurement vector, MP x 1 + Mat correct( const Mat& measurement ); + +// Get system parameters + Mat getProcessNoiseCov() const; + Mat getMeasurementNoiseCov() const; + +// Get the state estimate + Mat getState() const; +}; + +UnscentedKalmanFilterImpl::UnscentedKalmanFilterImpl(const UnscentedKalmanFilterParams& params) +{ + alpha = params.alpha; + beta = params.beta; + k = params.k; + + CV_Assert( params.DP > 0 && params.MP > 0 ); + CV_Assert( params.dataType == CV_32F || params.dataType == CV_64F ); + DP = params.DP; + MP = params.MP; + CP = std::max( params.CP, 0 ); + dataType = params.dataType; + + model = params.model; + + CV_Assert( params.stateInit.cols == 1 && params.stateInit.rows == DP ); + CV_Assert( params.errorCovInit.cols == DP && params.errorCovInit.rows == DP ); + state = params.stateInit.clone(); + errorCov = params.errorCovInit.clone(); + + CV_Assert( params.processNoiseCov.cols == DP && params.processNoiseCov.rows == DP ); + CV_Assert( params.measurementNoiseCov.cols == MP && params.measurementNoiseCov.rows == MP ); + processNoiseCov = params.processNoiseCov.clone(); + measurementNoiseCov = params.measurementNoiseCov.clone(); + + measurementEstimate = Mat::zeros( MP, 1, dataType); + + q = Mat::zeros( DP, 1, dataType); + r = Mat::zeros( MP, 1, dataType); + + gain = Mat::zeros( DP, DP, dataType ); + + transitionSPFuncVals = Mat::zeros( DP, 2*DP+1, dataType ); + measurementSPFuncVals = Mat::zeros( MP, 2*DP+1, dataType ); + + transitionSPFuncValsCenter = Mat::zeros( DP, 2*DP+1, dataType ); + measurementSPFuncValsCenter = Mat::zeros( MP, 2*DP+1, dataType ); + + lambda = alpha*alpha*( DP + k ) - DP; + tmpLambda = lambda + DP; + + double tmp2Lambda = 0.5/tmpLambda; + + Wm = tmp2Lambda * Mat::ones( 2*DP+1, 1, dataType ); + Wc = tmp2Lambda * Mat::eye( 2*DP+1, 2*DP+1, dataType ); + + if ( dataType == CV_64F ) + { + Wm.at(0,0) = lambda/tmpLambda; + Wc.at(0,0) = lambda/tmpLambda + 1.0 - alpha*alpha + beta; + } + else + { + Wm.at(0,0) = (float)(lambda/tmpLambda); + Wc.at(0,0) = (float)(lambda/tmpLambda + 1.0 - alpha*alpha + beta); + } +} + +UnscentedKalmanFilterImpl::~UnscentedKalmanFilterImpl() +{ + state.release(); + errorCov.release(); + + processNoiseCov.release(); + measurementNoiseCov.release(); + + measurementEstimate.release(); + + sigmaPoints.release(); + + transitionSPFuncVals.release(); + measurementSPFuncVals.release(); + + transitionSPFuncValsCenter.release(); + measurementSPFuncValsCenter.release(); + + Wm.release(); + Wc.release(); + + gain.release(); + xyCov.release(); + yyCov.release(); + + r.release(); + q.release(); +} + +template +Mat UnscentedKalmanFilterImpl::getSigmaPoints(const Mat &mean, const Mat &covMatrix, double coef) +{ +// x_0 = mean +// x_i = mean + coef * cholesky( covMatrix ), i = 1..n +// x_(i+n) = mean - coef * cholesky( covMatrix ), i = 1..n + + int n = mean.rows; + Mat points = repeat(mean, 1, 2*n+1); + + Mat covMatrixL = covMatrix.clone(); + covMatrixL.setTo(0); + +// covMatrixL = cholesky( covMatrix ) + choleskyDecomposition( covMatrix.ptr(), covMatrix.step, covMatrix.rows, covMatrixL.ptr() ); + covMatrixL = coef * covMatrixL; + + Mat p_plus = points( Rect( 1, 0, n, n ) ); + Mat p_minus = points( Rect( n+1, 0, n, n ) ); + + add(p_plus, covMatrixL, p_plus); + subtract(p_minus, covMatrixL, p_minus); + + return points; +} + +Mat UnscentedKalmanFilterImpl::getSigmaPoints(const Mat &mean, const Mat &covMatrix, double coef) +{ + if ( dataType == CV_64F ) return getSigmaPoints(mean, covMatrix, coef); + if ( dataType == CV_32F ) return getSigmaPoints(mean, covMatrix, coef); + + return Mat(); +} + +Mat UnscentedKalmanFilterImpl::predict(const Mat& control) +{ +// get sigma points from x* and P + sigmaPoints = getSigmaPoints( state, errorCov, sqrt( tmpLambda ) ); + +// compute f-function values at sigma points +// f_i = f(x_i, control, 0), i = 0..2*DP + Mat x, fx; + for ( int i = 0; i<2*DP+1; i++) + { + x = sigmaPoints( Rect( i, 0, 1, DP) ); + fx = transitionSPFuncVals( Rect( i, 0, 1, DP) ); + model->stateConversionFunction( x, control, q, fx ); + } +// compute the estimate of state as mean f-function value at sigma point +// x* = SUM_{i=0}^{2*DP}( Wm[i]*f_i ) + state = transitionSPFuncVals * Wm; + +// compute f-function values at sigma points minus estimate of state +// fc_i = f_i - x*, i = 0..2*DP + subtract(transitionSPFuncVals, repeat( state, 1, 2*DP+1 ), transitionSPFuncValsCenter); + +// compute the estimate of the state cross-covariance matrix +// P = SUM_{i=0}^{2*DP}( Wc[i]*fc_i*fc_i.t ) + Q + errorCov = transitionSPFuncValsCenter * Wc * transitionSPFuncValsCenter.t() + processNoiseCov; + + return state.clone(); +} + +Mat UnscentedKalmanFilterImpl::correct(const Mat& measurement) +{ +// get sigma points from x* and P + sigmaPoints = getSigmaPoints( state, errorCov, sqrt( tmpLambda ) ); + +// compute h-function values at sigma points +// h_i = h(x_i, 0), i = 0..2*DP + Mat x, hx; + for ( int i = 0; i<2*DP+1; i++) + { + x = sigmaPoints( Rect( i, 0, 1, DP) ); + hx = measurementSPFuncVals( Rect( i, 0, 1, MP) ); + model->measurementFunction( x, r, hx ); + } + +// compute the estimate of measurement as mean h-function value at sigma point +// y* = SUM_{i=0}^{2*DP}( Wm[i]*h_i ) + measurementEstimate = measurementSPFuncVals * Wm; + +// compute h-function values at sigma points minus estimate of state +// hc_i = h_i - y*, i = 0..2*DP + subtract(measurementSPFuncVals, repeat( measurementEstimate, 1, 2*DP+1 ), measurementSPFuncValsCenter); + +// compute the estimate of the y* cross-covariance matrix +// Syy = SUM_{i=0}^{2*DP}( Wc[i]*hc_i*hc_i.t ) + R + yyCov = measurementSPFuncValsCenter * Wc * measurementSPFuncValsCenter.t() + measurementNoiseCov; + +// compute the estimate of the covariance between x* and y* +// Sxy = SUM_{i=0}^{2*DP}( Wc[i]*fc_i*hc_i.t ) + xyCov = transitionSPFuncValsCenter * Wc * measurementSPFuncValsCenter.t(); + +// compute the Kalman gain matrix +// K = Sxy * Syy^(-1) + gain = xyCov * yyCov.inv(DECOMP_SVD); + +// compute the corrected estimate of state +// x* = x* + K*(y - y*), y - current measurement + state = state + gain * ( measurement - measurementEstimate ); + +// compute the corrected estimate of the state cross-covariance matrix +// P = P - K*Sxy.t + errorCov = errorCov - gain * xyCov.t(); + + return state.clone(); +} + +Mat UnscentedKalmanFilterImpl::getProcessNoiseCov() const +{ + return processNoiseCov.clone(); +} + +Mat UnscentedKalmanFilterImpl::getMeasurementNoiseCov() const +{ + return measurementNoiseCov.clone(); +} + +Mat UnscentedKalmanFilterImpl::getState() const +{ + return state.clone(); +} + +Ptr createUnscentedKalmanFilter(const UnscentedKalmanFilterParams ¶ms) +{ + Ptr kfu( new UnscentedKalmanFilterImpl(params) ); + return kfu; +} + +} // tracking +} // cv diff --git a/modules/tracking/test/test_aukf.cpp b/modules/tracking/test/test_aukf.cpp new file mode 100644 index 000000000..4afe223e4 --- /dev/null +++ b/modules/tracking/test/test_aukf.cpp @@ -0,0 +1,436 @@ +/*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*/ + +#include "test_precomp.hpp" +#include "opencv2/tracking/kalman_filters.hpp" + +using namespace cv; +using namespace cv::tracking; + +// In this two tests Augmented Unscented Kalman Filter are applied to the dynamic system from example "The reentry problem" from +// "A New Extension of the Kalman Filter to Nonlinear Systems" by Simon J. Julier and Jeffrey K. Uhlmann. +class BallisticModel: public UkfSystemModel +{ + static const double step_h; + + Mat diff_eq(const Mat& x) + { + double x1 = x.at(0, 0); + double x2 = x.at(1, 0); + double x3 = x.at(2, 0); + double x4 = x.at(3, 0); + double x5 = x.at(4, 0); + + const double h0 = 9.3; + const double beta0 = 0.59783; + const double Gm = 3.9860044 * 1e5; + const double r_e = 6374; + + const double r = sqrt( x1*x1 + x2*x2 ); + const double v = sqrt( x3*x3 + x4*x4 ); + const double d = - beta0 * exp( ( r_e - r )/h0 ) * exp( x5 ) * v; + const double g = - Gm / (r*r*r); + + Mat fx = x.clone(); + + fx.at(0, 0) = x3; + fx.at(1, 0) = x4; + fx.at(2, 0) = d * x3 + g * x1; + fx.at(3, 0) = d * x4 + g * x2; + fx.at(4, 0) = 0.0; + + return fx; + } +public: + void stateConversionFunction(const Mat& x_k, const Mat& u_k, const Mat& v_k, Mat& x_kplus1) + { + Mat v = sqrt(step_h) * v_k.clone(); + v.at(0, 0) = 0.0; + v.at(1, 0) = 0.0; + + Mat k1 = diff_eq( x_k ) + v; + Mat tmp = x_k + step_h*0.5*k1; + Mat k2 = diff_eq( tmp ) + v; + tmp = x_k + step_h*0.5*k2; + Mat k3 = diff_eq( tmp ) + v; + tmp = x_k + step_h*k3; + Mat k4 = diff_eq( tmp ) + v; + + x_kplus1 = x_k + (1.0/6.0)*step_h*( k1 + 2.0*k2 + 2.0*k3 + k4 ) + u_k; + } + + void measurementFunction(const Mat& x_k, const Mat& n_k, Mat& z_k) + { + double x1 = x_k.at(0, 0); + double x2 = x_k.at(1, 0); + double x1_r = 6374.0; + double x2_r = 0.0; + + double R = sqrt( pow( x1 - x1_r, 2 ) + pow( x2 - x2_r, 2 ) ); + double Phi = atan( (x2 - x2_r)/(x1 - x1_r) ); + + R += n_k.at(0, 0); + Phi += n_k.at(1, 0); + + z_k.at(0, 0) = R; + z_k.at(1, 0) = Phi; + } +}; + +const double BallisticModel::step_h = 0.05; + +TEST(AUKF, br_landing_point) +{ + const double abs_error = 0.1; + + const int nIterations = 4000; // number of iterations before landing + const double landing_coordinate = 2.5; // the expected landing coordinate + + const double alpha = 1; + const double beta = 2.0; + const double kappa = -2.0; + + int MP = 2; + int DP = 5; + int CP = 0; + int type = CV_64F; + + Mat processNoiseCov = Mat::zeros( DP, DP, type ); + processNoiseCov.at(0, 0) = 1e-14; + processNoiseCov.at(1, 1) = 1e-14; + processNoiseCov.at(2, 2) = 2.4065 * 1e-5; + processNoiseCov.at(3, 3) = 2.4065 * 1e-5; + processNoiseCov.at(4, 4) = 1e-6; + Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type ); + sqrt( processNoiseCov, processNoiseCovSqrt ); + + Mat measurementNoiseCov = Mat::zeros( MP, MP, type ); + measurementNoiseCov.at(0, 0) = 1e-3*1e-3; + measurementNoiseCov.at(1, 1) = 0.13*0.13; + Mat measurementNoiseCovSqrt = Mat::zeros( MP, MP, type ); + sqrt( measurementNoiseCov, measurementNoiseCovSqrt ); + + RNG rng( 117 ); + + Mat state( DP, 1, type ); + state.at(0, 0) = 6500.4; + state.at(1, 0) = 349.14; + state.at(2, 0) = -1.8093; + state.at(3, 0) = -6.7967; + state.at(4, 0) = 0.6932; + + Mat initState = state.clone(); + initState.at(4, 0) = 0.0; + + Mat P = 1e-6 * Mat::eye( DP, DP, type ); + P.at(4, 4) = 1.0; + + Mat measurement( MP, 1, type ); + + Mat q( DP, 1, type ); + Mat r( MP, 1, type ); + + Ptr model( new BallisticModel() ); + AugmentedUnscentedKalmanFilterParams params( DP, MP, CP, 0, 0, model ); + + params.stateInit = initState.clone(); + params.errorCovInit = P.clone(); + params.measurementNoiseCov = measurementNoiseCov.clone(); + params.processNoiseCov = processNoiseCov.clone(); + + params.alpha = alpha; + params.beta = beta; + params.k = kappa; + + Ptr augmentedUncsentedKalmanFilter = createAugmentedUnscentedKalmanFilter(params); + + Mat correctStateUKF( DP, 1, type ); + Mat u = Mat::zeros( DP, 1, type ); + + for (int i = 0; istateConversionFunction(state, u, q, state); + model->measurementFunction(state, r, measurement); + + augmentedUncsentedKalmanFilter->predict(); + correctStateUKF = augmentedUncsentedKalmanFilter->correct( measurement ); + } + + double landing_y = correctStateUKF.at(1, 0); + ASSERT_NEAR(landing_coordinate, landing_y, abs_error); +} + +TEST(AUKF, br_mean_squared_error) +{ + const double velocity_treshold = 0.004; + const double state_treshold = 0.04; + + const int nIterations = 4000; // number of iterations before landing + + const double alpha = 1; + const double beta = 2.0; + const double kappa = -2.0; + + int MP = 2; + int DP = 5; + int CP = 0; + int type = CV_64F; + + Mat processNoiseCov = Mat::zeros( DP, DP, type ); + processNoiseCov.at(0, 0) = 1e-14; + processNoiseCov.at(1, 1) = 1e-14; + processNoiseCov.at(2, 2) = 2.4065 * 1e-5; + processNoiseCov.at(3, 3) = 2.4065 * 1e-5; + processNoiseCov.at(4, 4) = 1e-6; + Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type ); + sqrt( processNoiseCov, processNoiseCovSqrt ); + + Mat measurementNoiseCov = Mat::zeros( MP, MP, type ); + measurementNoiseCov.at(0, 0) = 1e-3*1e-3; + measurementNoiseCov.at(1, 1) = 0.13*0.13; + Mat measurementNoiseCovSqrt = Mat::zeros( MP, MP, type ); + sqrt( measurementNoiseCov, measurementNoiseCovSqrt ); + + RNG rng( 464 ); + + Mat state( DP, 1, type ); + state.at(0, 0) = 6500.4; + state.at(1, 0) = 349.14; + state.at(2, 0) = -1.8093; + state.at(3, 0) = -6.7967; + state.at(4, 0) = 0.6932; + + Mat initState = state.clone(); + Mat initStateKF = state.clone(); + initStateKF.at(4, 0) = 0.0; + + Mat P = 1e-6 * Mat::eye( DP, DP, type ); + P.at(4, 4) = 1.0; + + Mat measurement( MP, 1, type ); + + Mat q( DP, 1, type); + Mat r( MP, 1, type); + + Ptr model( new BallisticModel() ); + AugmentedUnscentedKalmanFilterParams params( DP, MP, CP, 0, 0, model ); + + params.stateInit = initStateKF.clone(); + params.errorCovInit = P.clone(); + params.measurementNoiseCov = measurementNoiseCov.clone(); + params.processNoiseCov = processNoiseCov.clone(); + + params.alpha = alpha; + params.beta = beta; + params.k = kappa; + + Mat predictStateUKF( DP, 1, type ); + Mat correctStateUKF( DP, 1, type ); + + Mat errors = Mat::zeros( nIterations, 4, type ); + Mat u = Mat::zeros( DP, 1, type ); + + for (int j = 0; j<100; j++) + { + cv::Ptr augmentedUncsentedKalmanFilter = createAugmentedUnscentedKalmanFilter(params); + state = initState.clone(); + + for (int i = 0; istateConversionFunction(state, u, q, state); + model->measurementFunction(state, r, measurement); + + predictStateUKF = augmentedUncsentedKalmanFilter->predict(); + correctStateUKF = augmentedUncsentedKalmanFilter->correct( measurement ); + + Mat errorUKF = state - correctStateUKF; + + for (int l = 0; l<4; l++) + errors.at(i, l) += pow( errorUKF.at(l, 0), 2.0 ); + + } + } + + errors = errors/100.0; + sqrt( errors, errors ); + + double max_x1 = norm( errors.col(0), NORM_INF ); + double max_x2 = norm( errors.col(1), NORM_INF ); + double max_x3 = norm( errors.col(2), NORM_INF ); + double max_x4 = norm( errors.col(3), NORM_INF ); + + ASSERT_GE( state_treshold, max_x1 ); + ASSERT_GE( state_treshold, max_x2 ); + ASSERT_GE( velocity_treshold, max_x3 ); + ASSERT_GE( velocity_treshold, max_x4 ); + +} + + +// In this test Augmented Unscented Kalman Filter are applied to the univariate nonstationary growth model (UNGM). +// This model was used in example from "Unscented Kalman filtering for additive noise case: Augmented vs. non-augmented" +// by Yuanxin Wu and Dewen Hu. +class UnivariateNonstationaryGrowthModel: public UkfSystemModel +{ + +public: + void stateConversionFunction(const Mat& x_k, const Mat& u_k, const Mat& v_k, Mat& x_kplus1) + { + double x = x_k.at(0, 0); + double n = u_k.at(0, 0); + double q = v_k.at(0, 0); + double u = u_k.at(0, 0); + + double x1 = 0.5*x + 25*( x/(x*x + 1) ) + 8*cos( 1.2*(n-1) ) + q + u; + x_kplus1.at(0, 0) = x1; + } + void measurementFunction(const Mat& x_k, const Mat& n_k, Mat& z_k) + { + double x = x_k.at(0, 0); + double r = n_k.at(0, 0); + + double y = x*x/20.0 + r; + z_k.at(0, 0) = y; + } +}; + +TEST(AUKF, ungm_mean_squared_error) +{ + + const double alpha = 1.5; + const double beta = 2.0; + const double kappa = 0.0; + + const double mse_treshold = 0.05; + const int nIterations = 500; // number of observed iterations + + int MP = 1; + int DP = 1; + int CP = 0; + int type = CV_64F; + + Ptr model( new UnivariateNonstationaryGrowthModel() ); + AugmentedUnscentedKalmanFilterParams params( DP, MP, CP, 0, 0, model ); + + Mat processNoiseCov = Mat::zeros( DP, DP, type ); + processNoiseCov.at(0, 0) = 1.0; + Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type ); + sqrt( processNoiseCov, processNoiseCovSqrt ); + + Mat measurementNoiseCov = Mat::zeros( MP, MP, type ); + measurementNoiseCov.at(0, 0) = 1.0; + Mat measurementNoiseCovSqrt = Mat::zeros( MP, MP, type ); + sqrt( measurementNoiseCov, measurementNoiseCovSqrt ); + + Mat P = Mat::eye( DP, DP, type ); + + Mat state( DP, 1, type ); + state.at(0, 0) = 0.1; + + Mat initState = state.clone(); + initState.at(0, 0) = 0.0; + + params.errorCovInit = P; + params.measurementNoiseCov = measurementNoiseCov; + params.processNoiseCov = processNoiseCov; + params.stateInit = initState.clone(); + + params.alpha = alpha; + params.beta = beta; + params.k = kappa; + + Mat correctStateAUKF( DP, 1, type ); + + Mat measurement( MP, 1, type ); + Mat exactMeasurement( MP, 1, type ); + + Mat q( DP, 1, type ); + Mat r( MP, 1, type ); + + Mat u( DP, 1, type ); + Mat zero = Mat::zeros( MP, 1, type ); + + RNG rng( 216 ); + + double average_error = 0.0; + for (int j = 0; j<1000; j++) + { + cv::Ptr augmentedUncsentedKalmanFilter = createAugmentedUnscentedKalmanFilter( params ); + state = params.stateInit.clone(); + + double mse = 0.0; + for (int i = 0; i(0, 0) = (double)i; + model->stateConversionFunction(state, u, q, state); + + model->measurementFunction(state, zero, exactMeasurement); + model->measurementFunction(state, r, measurement); + + augmentedUncsentedKalmanFilter->predict( u ); + correctStateAUKF = augmentedUncsentedKalmanFilter->correct( measurement ); + + mse += pow( state.at(0, 0) - correctStateAUKF.at(0, 0), 2.0 ); + } + mse /= nIterations; + average_error += mse; + } + average_error /= 1000.0; + + ASSERT_GE( mse_treshold, average_error ); +} diff --git a/modules/tracking/test/test_ukf.cpp b/modules/tracking/test/test_ukf.cpp new file mode 100644 index 000000000..b2a075828 --- /dev/null +++ b/modules/tracking/test/test_ukf.cpp @@ -0,0 +1,434 @@ +/*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*/ + +#include "test_precomp.hpp" +#include "opencv2/tracking/kalman_filters.hpp" + +using namespace cv; +using namespace cv::tracking; + +// In this two tests Unscented Kalman Filter are applied to the dynamic system from example "The reentry problem" from +// "A New Extension of the Kalman Filter to Nonlinear Systems" by Simon J. Julier and Jeffrey K. Uhlmann. +class BallisticModel: public UkfSystemModel +{ + static const double step; + + Mat diff_eq(const Mat& x) + { + double x1 = x.at(0, 0); + double x2 = x.at(1, 0); + double x3 = x.at(2, 0); + double x4 = x.at(3, 0); + double x5 = x.at(4, 0); + + const double h0 = 9.3; + const double beta0 = 0.59783; + const double Gm = 3.9860044 * 1e5; + const double r_e = 6374; + + const double r = sqrt( x1*x1 + x2*x2 ); + const double v = sqrt( x3*x3 + x4*x4 ); + const double d = - beta0 * exp( ( r_e - r )/h0 ) * exp( x5 ) * v; + const double g = - Gm / (r*r*r); + + Mat fx = x.clone(); + + fx.at(0, 0) = x3; + fx.at(1, 0) = x4; + fx.at(2, 0) = d * x3 + g * x1; + fx.at(3, 0) = d * x4 + g * x2; + fx.at(4, 0) = 0.0; + + return fx; + } +public: + void stateConversionFunction(const Mat& x_k, const Mat& u_k, const Mat& v_k, Mat& x_kplus1) + { + Mat v = sqrt(step) * v_k.clone(); + v.at(0, 0) = 0.0; + v.at(1, 0) = 0.0; + + Mat k1 = diff_eq( x_k ) + v; + Mat tmp = x_k + step*0.5*k1; + Mat k2 = diff_eq( tmp ) + v; + tmp = x_k + step*0.5*k2; + Mat k3 = diff_eq( tmp ) + v; + tmp = x_k + step*k3; + Mat k4 = diff_eq( tmp ) + v; + + x_kplus1 = x_k + (1.0/6.0)*step*( k1 + 2.0*k2 + 2.0*k3 + k4 ) + u_k; + } + + void measurementFunction(const Mat& x_k, const Mat& n_k, Mat& z_k) + { + double x1 = x_k.at(0, 0); + double x2 = x_k.at(1, 0); + double x1_r = 6374.0; + double x2_r = 0.0; + + double R = sqrt( pow( x1 - x1_r, 2 ) + pow( x2 - x2_r, 2 ) ); + double Phi = atan( (x2 - x2_r)/(x1 - x1_r) ); + + R += n_k.at(0, 0); + Phi += n_k.at(1, 0); + + z_k.at(0, 0) = R; + z_k.at(1, 0) = Phi; + } +}; + +const double BallisticModel::step = 0.05; + +TEST(UKF, br_landing_point) +{ + const double abs_error = 0.1; + + const int nIterations = 4000; // number of iterations before landing + const double landing_coordinate = 2.5; // the expected landing coordinate + + const double alpha = 1; + const double beta = 2.0; + const double kappa = -2.0; + + int MP = 2; + int DP = 5; + int CP = 0; + int type = CV_64F; + + Mat processNoiseCov = Mat::zeros( DP, DP, type ); + processNoiseCov.at(0, 0) = 1e-14; + processNoiseCov.at(1, 1) = 1e-14; + processNoiseCov.at(2, 2) = 2.4065 * 1e-5; + processNoiseCov.at(3, 3) = 2.4065 * 1e-5; + processNoiseCov.at(4, 4) = 1e-6; + Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type ); + sqrt( processNoiseCov, processNoiseCovSqrt ); + + Mat measurementNoiseCov = Mat::zeros( MP, MP, type ); + measurementNoiseCov.at(0, 0) = 1e-3*1e-3; + measurementNoiseCov.at(1, 1) = 0.13*0.13; + Mat measurementNoiseCovSqrt = Mat::zeros( MP, MP, type ); + sqrt( measurementNoiseCov, measurementNoiseCovSqrt ); + + RNG rng( 117 ); + + Mat state( DP, 1, type ); + state.at(0, 0) = 6500.4; + state.at(1, 0) = 349.14; + state.at(2, 0) = -1.8093; + state.at(3, 0) = -6.7967; + state.at(4, 0) = 0.6932; + + Mat initState = state.clone(); + initState.at(4, 0) = 0.0; + + Mat P = 1e-6 * Mat::eye( DP, DP, type ); + P.at(4, 4) = 1.0; + + Mat measurement( MP, 1, type ); + + Mat q( DP, 1, type ); + Mat r( MP, 1, type ); + + Ptr model( new BallisticModel() ); + UnscentedKalmanFilterParams params( DP, MP, CP, 0, 0, model ); + + params.stateInit = initState.clone(); + params.errorCovInit = P.clone(); + params.measurementNoiseCov = measurementNoiseCov.clone(); + params.processNoiseCov = processNoiseCov.clone(); + + params.alpha = alpha; + params.beta = beta; + params.k = kappa; + + Ptr uncsentedKalmanFilter = createUnscentedKalmanFilter(params); + + Mat correctStateUKF( DP, 1, type ); + Mat u = Mat::zeros( DP, 1, type ); + + for (int i = 0; istateConversionFunction(state, u, q, state); + model->measurementFunction(state, r, measurement); + + uncsentedKalmanFilter->predict(); + correctStateUKF = uncsentedKalmanFilter->correct( measurement ); + } + + double landing_y = correctStateUKF.at(1, 0); + ASSERT_NEAR(landing_coordinate, landing_y, abs_error); +} + +TEST(UKF, br_mean_squared_error) +{ + const double velocity_treshold = 0.09; + const double state_treshold = 0.9; + + const int nIterations = 4000; // number of iterations before landing + + const double alpha = 1; + const double beta = 2.0; + const double kappa = -2.0; + + int MP = 2; + int DP = 5; + int CP = 0; + int type = CV_64F; + + Mat processNoiseCov = Mat::zeros( DP, DP, type ); + processNoiseCov.at(0, 0) = 1e-14; + processNoiseCov.at(1, 1) = 1e-14; + processNoiseCov.at(2, 2) = 2.4065 * 1e-5; + processNoiseCov.at(3, 3) = 2.4065 * 1e-5; + processNoiseCov.at(4, 4) = 1e-6; + Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type ); + sqrt( processNoiseCov, processNoiseCovSqrt ); + + Mat measurementNoiseCov = Mat::zeros( MP, MP, type ); + measurementNoiseCov.at(0, 0) = 1e-3*1e-3; + measurementNoiseCov.at(1, 1) = 0.13*0.13; + Mat measurementNoiseCovSqrt = Mat::zeros( MP, MP, type ); + sqrt( measurementNoiseCov, measurementNoiseCovSqrt ); + + RNG rng( 464 ); + + Mat state( DP, 1, type ); + state.at(0, 0) = 6500.4; + state.at(1, 0) = 349.14; + state.at(2, 0) = -1.8093; + state.at(3, 0) = -6.7967; + state.at(4, 0) = 0.6932; + + Mat initState = state.clone(); + Mat initStateKF = state.clone(); + initStateKF.at(4, 0) = 0.0; + + Mat P = 1e-6 * Mat::eye( DP, DP, type ); + P.at(4, 4) = 1.0; + + Mat measurement( MP, 1, type ); + + Mat q( DP, 1, type); + Mat r( MP, 1, type); + + Ptr model( new BallisticModel() ); + UnscentedKalmanFilterParams params( DP, MP, CP, 0, 0, model ); + + params.stateInit = initStateKF.clone(); + params.errorCovInit = P.clone(); + params.measurementNoiseCov = measurementNoiseCov.clone(); + params.processNoiseCov = processNoiseCov.clone(); + + params.alpha = alpha; + params.beta = beta; + params.k = kappa; + + Mat predictStateUKF( DP, 1, type ); + Mat correctStateUKF( DP, 1, type ); + + Mat errors = Mat::zeros( nIterations, 4, type ); + Mat u = Mat::zeros( DP, 1, type ); + + for (int j = 0; j<100; j++) + { + Ptr uncsentedKalmanFilter = createUnscentedKalmanFilter(params); + state = initState.clone(); + + for (int i = 0; istateConversionFunction(state, u, q, state); + model->measurementFunction(state, r, measurement); + + predictStateUKF = uncsentedKalmanFilter->predict(); + correctStateUKF = uncsentedKalmanFilter->correct( measurement ); + + Mat errorUKF = state - correctStateUKF; + + for (int l = 0; l<4; l++) + errors.at(i, l) += pow( errorUKF.at(l, 0), 2.0 ); + + } + } + + errors = errors/100.0; + sqrt( errors, errors ); + + double max_x1 = norm( errors.col(0), NORM_INF ); + double max_x2 = norm( errors.col(1), NORM_INF ); + double max_x3 = norm( errors.col(2), NORM_INF ); + double max_x4 = norm( errors.col(3), NORM_INF ); + + ASSERT_GE( state_treshold, max_x1 ); + ASSERT_GE( state_treshold, max_x2 ); + ASSERT_GE( velocity_treshold, max_x3 ); + ASSERT_GE( velocity_treshold, max_x4 ); +} + + +//In this test Unscented Kalman Filter are applied to the univariate nonstationary growth model (UNGM). +//This model was used in example from "Unscented Kalman filtering for additive noise case: Augmented vs. non-augmented" +//by Yuanxin Wu and Dewen Hu. +class UnivariateNonstationaryGrowthModel: public UkfSystemModel +{ + +public: + void stateConversionFunction(const Mat& x_k, const Mat& u_k, const Mat& v_k, Mat& x_kplus1) + { + double x = x_k.at(0, 0); + double n = u_k.at(0, 0); + double q = v_k.at(0, 0); + double u = u_k.at(0, 0); + + double x1 = 0.5*x + 25*( x/(x*x + 1) ) + 8*cos( 1.2*(n-1) ) + q + u; + x_kplus1.at(0, 0) = x1; + } + void measurementFunction(const Mat& x_k, const Mat& n_k, Mat& z_k) + { + double x = x_k.at(0, 0); + double r = n_k.at(0, 0); + + double y = x*x/20.0 + r; + z_k.at(0, 0) = y; + } +}; + +TEST(UKF, ungm_mean_squared_error) +{ + const double alpha = 1.5; + const double beta = 2.0; + const double kappa = 0.0; + + const double mse_treshold = 0.5; + const int nIterations = 500; // number of observed iterations + + int MP = 1; + int DP = 1; + int CP = 0; + int type = CV_64F; + + Ptr model( new UnivariateNonstationaryGrowthModel() ); + UnscentedKalmanFilterParams params( DP, MP, CP, 0, 0, model ); + + Mat processNoiseCov = Mat::zeros( DP, DP, type ); + processNoiseCov.at(0, 0) = 1.0; + Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type ); + sqrt( processNoiseCov, processNoiseCovSqrt ); + + Mat measurementNoiseCov = Mat::zeros( MP, MP, type ); + measurementNoiseCov.at(0, 0) = 1.0; + Mat measurementNoiseCovSqrt = Mat::zeros( MP, MP, type ); + sqrt( measurementNoiseCov, measurementNoiseCovSqrt ); + + Mat P = Mat::eye( DP, DP, type ); + + Mat state( DP, 1, type ); + state.at(0, 0) = 0.1; + + Mat initState = state.clone(); + initState.at(0, 0) = 0.0; + + params.errorCovInit = P; + params.measurementNoiseCov = measurementNoiseCov; + params.processNoiseCov = processNoiseCov; + params.stateInit = initState.clone(); + + params.alpha = alpha; + params.beta = beta; + params.k = kappa; + + Mat correctStateAUKF( DP, 1, type ); + + Mat measurement( MP, 1, type ); + Mat exactMeasurement( MP, 1, type ); + + Mat q( DP, 1, type ); + Mat r( MP, 1, type ); + + Mat u( DP, 1, type ); + Mat zero = Mat::zeros( MP, 1, type ); + + RNG rng( 216 ); + + double average_error = 0.0; + for (int j = 0; j<1000; j++) + { + cv::Ptr uncsentedKalmanFilter = createUnscentedKalmanFilter( params ); + state.at(0, 0) = 0.1; + + double mse = 0.0; + for (int i = 0; i(0, 0) = (double)i; + model->stateConversionFunction(state, u, q, state); + + model->measurementFunction(state, zero, exactMeasurement); + model->measurementFunction(state, r, measurement); + + uncsentedKalmanFilter->predict( u ); + correctStateAUKF = uncsentedKalmanFilter->correct( measurement ); + + mse += pow( state.at(0, 0) - correctStateAUKF.at(0, 0), 2.0 ); + } + mse /= nIterations; + average_error += mse; + } + average_error /= 1000.0; + + ASSERT_GE( mse_treshold, average_error ); +}