Chiebot-Mirror
/
opencv_contrib
mirror of https://github.com/opencv/opencv_contrib.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

Merge pull request #349 from SvetlanaFilicheva:ukf-aukf

Browse Source
pull/343/merge
Vadim Pisarevsky 9 years ago
parent e2f9c66671 450388468d
commit 52a113793f
5 changed files with 1948 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 223
      modules/tracking/include/opencv2/tracking/kalman_filters.hpp
  2. 440
      modules/tracking/src/augmented_unscented_kalman.cpp
  3. 415
      modules/tracking/src/unscented_kalman.cpp
  4. 436
      modules/tracking/test/test_aukf.cpp
  5. 434
      modules/tracking/test/test_ukf.cpp

223
modules/tracking/include/opencv2/tracking/kalman_filters.hpp
Unescape View 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 &params );
/** @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 &params );
} // tracking
} // cv
#endif

440
modules/tracking/src/augmented_unscented_kalman.cpp
Unescape View File

@ -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 <https://en.wikipedia.org/wiki/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<typename _Tp> 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<UkfSystemModel> 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<UkfSystemModel> 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<UkfSystemModel> 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 <typename T>
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<double>(0,0) = lambda/tmpLambda;
Wc.at<double>(0,0) = lambda/tmpLambda + 1.0 - alpha*alpha + beta;
}
else
{
Wm.at<float>(0,0) = (float)(lambda/tmpLambda);
Wc.at<float>(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 <typename T>
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<T>( covMatrix.ptr<T>(), covMatrix.step, covMatrix.rows, covMatrixL.ptr<T>() );
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<double>(mean, covMatrix, coef);
if ( dataType == CV_32F ) return getSigmaPoints<float>(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<UnscentedKalmanFilter> createAugmentedUnscentedKalmanFilter(const AugmentedUnscentedKalmanFilterParams &params)
{
Ptr<UnscentedKalmanFilter> kfu( new AugmentedUnscentedKalmanFilterImpl(params) );
return kfu;
}
} // tracking
} // cv

415
modules/tracking/src/unscented_kalman.cpp
Unescape View File

@ -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 <https://en.wikipedia.org/wiki/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<typename _Tp> 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<UkfSystemModel> 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<UkfSystemModel> 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<UkfSystemModel> 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 <typename T>
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<double>(0,0) = lambda/tmpLambda;
Wc.at<double>(0,0) = lambda/tmpLambda + 1.0 - alpha*alpha + beta;
}
else
{
Wm.at<float>(0,0) = (float)(lambda/tmpLambda);
Wc.at<float>(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 <typename T>
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<T>( covMatrix.ptr<T>(), covMatrix.step, covMatrix.rows, covMatrixL.ptr<T>() );
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<double>(mean, covMatrix, coef);
if ( dataType == CV_32F ) return getSigmaPoints<float>(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<UnscentedKalmanFilter> createUnscentedKalmanFilter(const UnscentedKalmanFilterParams &params)
{
Ptr<UnscentedKalmanFilter> kfu( new UnscentedKalmanFilterImpl(params) );
return kfu;
}
} // tracking
} // cv

436
modules/tracking/test/test_aukf.cpp
Unescape View File

@ -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<double>(0, 0);
double x2 = x.at<double>(1, 0);
double x3 = x.at<double>(2, 0);
double x4 = x.at<double>(3, 0);
double x5 = x.at<double>(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<double>(0, 0) = x3;
fx.at<double>(1, 0) = x4;
fx.at<double>(2, 0) = d * x3 + g * x1;
fx.at<double>(3, 0) = d * x4 + g * x2;
fx.at<double>(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<double>(0, 0) = 0.0;
v.at<double>(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<double>(0, 0);
double x2 = x_k.at<double>(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<double>(0, 0);
Phi += n_k.at<double>(1, 0);
z_k.at<double>(0, 0) = R;
z_k.at<double>(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<double>(0, 0) = 1e-14;
processNoiseCov.at<double>(1, 1) = 1e-14;
processNoiseCov.at<double>(2, 2) = 2.4065 * 1e-5;
processNoiseCov.at<double>(3, 3) = 2.4065 * 1e-5;
processNoiseCov.at<double>(4, 4) = 1e-6;
Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type );
sqrt( processNoiseCov, processNoiseCovSqrt );
Mat measurementNoiseCov = Mat::zeros( MP, MP, type );
measurementNoiseCov.at<double>(0, 0) = 1e-3*1e-3;
measurementNoiseCov.at<double>(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<double>(0, 0) = 6500.4;
state.at<double>(1, 0) = 349.14;
state.at<double>(2, 0) = -1.8093;
state.at<double>(3, 0) = -6.7967;
state.at<double>(4, 0) = 0.6932;
Mat initState = state.clone();
initState.at<double>(4, 0) = 0.0;
Mat P = 1e-6 * Mat::eye( DP, DP, type );
P.at<double>(4, 4) = 1.0;
Mat measurement( MP, 1, type );
Mat q( DP, 1, type );
Mat r( MP, 1, type );
Ptr<BallisticModel> 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<UnscentedKalmanFilter> augmentedUncsentedKalmanFilter = createAugmentedUnscentedKalmanFilter(params);
Mat correctStateUKF( DP, 1, type );
Mat u = Mat::zeros( DP, 1, type );
for (int i = 0; i<nIterations; i++)
{
rng.fill( q, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
q = processNoiseCovSqrt*q;
rng.fill( r, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
r = measurementNoiseCovSqrt*r;
model->stateConversionFunction(state, u, q, state);
model->measurementFunction(state, r, measurement);
augmentedUncsentedKalmanFilter->predict();
correctStateUKF = augmentedUncsentedKalmanFilter->correct( measurement );
}
double landing_y = correctStateUKF.at<double>(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<double>(0, 0) = 1e-14;
processNoiseCov.at<double>(1, 1) = 1e-14;
processNoiseCov.at<double>(2, 2) = 2.4065 * 1e-5;
processNoiseCov.at<double>(3, 3) = 2.4065 * 1e-5;
processNoiseCov.at<double>(4, 4) = 1e-6;
Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type );
sqrt( processNoiseCov, processNoiseCovSqrt );
Mat measurementNoiseCov = Mat::zeros( MP, MP, type );
measurementNoiseCov.at<double>(0, 0) = 1e-3*1e-3;
measurementNoiseCov.at<double>(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<double>(0, 0) = 6500.4;
state.at<double>(1, 0) = 349.14;
state.at<double>(2, 0) = -1.8093;
state.at<double>(3, 0) = -6.7967;
state.at<double>(4, 0) = 0.6932;
Mat initState = state.clone();
Mat initStateKF = state.clone();
initStateKF.at<double>(4, 0) = 0.0;
Mat P = 1e-6 * Mat::eye( DP, DP, type );
P.at<double>(4, 4) = 1.0;
Mat measurement( MP, 1, type );
Mat q( DP, 1, type);
Mat r( MP, 1, type);
Ptr<BallisticModel> 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<UnscentedKalmanFilter> augmentedUncsentedKalmanFilter = createAugmentedUnscentedKalmanFilter(params);
state = initState.clone();
for (int i = 0; i<nIterations; i++)
{
rng.fill( q, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
q = processNoiseCovSqrt*q;
rng.fill( r, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
r = measurementNoiseCovSqrt*r;
model->stateConversionFunction(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<double>(i, l) += pow( errorUKF.at<double>(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<double>(0, 0);
double n = u_k.at<double>(0, 0);
double q = v_k.at<double>(0, 0);
double u = u_k.at<double>(0, 0);
double x1 = 0.5*x + 25*( x/(x*x + 1) ) + 8*cos( 1.2*(n-1) ) + q + u;
x_kplus1.at<double>(0, 0) = x1;
}
void measurementFunction(const Mat& x_k, const Mat& n_k, Mat& z_k)
{
double x = x_k.at<double>(0, 0);
double r = n_k.at<double>(0, 0);
double y = x*x/20.0 + r;
z_k.at<double>(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<UnivariateNonstationaryGrowthModel> model( new UnivariateNonstationaryGrowthModel() );
AugmentedUnscentedKalmanFilterParams params( DP, MP, CP, 0, 0, model );
Mat processNoiseCov = Mat::zeros( DP, DP, type );
processNoiseCov.at<double>(0, 0) = 1.0;
Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type );
sqrt( processNoiseCov, processNoiseCovSqrt );
Mat measurementNoiseCov = Mat::zeros( MP, MP, type );
measurementNoiseCov.at<double>(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<double>(0, 0) = 0.1;
Mat initState = state.clone();
initState.at<double>(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<UnscentedKalmanFilter> augmentedUncsentedKalmanFilter = createAugmentedUnscentedKalmanFilter( params );
state = params.stateInit.clone();
double mse = 0.0;
for (int i = 0; i<nIterations; i++)
{
rng.fill( q, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
rng.fill( r, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
q = processNoiseCovSqrt*q;
r = measurementNoiseCovSqrt*r;
u.at<double>(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<double>(0, 0) - correctStateAUKF.at<double>(0, 0), 2.0 );
}
mse /= nIterations;
average_error += mse;
}
average_error /= 1000.0;
ASSERT_GE( mse_treshold, average_error );
}

434
modules/tracking/test/test_ukf.cpp
Unescape View File

@ -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<double>(0, 0);
double x2 = x.at<double>(1, 0);
double x3 = x.at<double>(2, 0);
double x4 = x.at<double>(3, 0);
double x5 = x.at<double>(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<double>(0, 0) = x3;
fx.at<double>(1, 0) = x4;
fx.at<double>(2, 0) = d * x3 + g * x1;
fx.at<double>(3, 0) = d * x4 + g * x2;
fx.at<double>(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<double>(0, 0) = 0.0;
v.at<double>(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<double>(0, 0);
double x2 = x_k.at<double>(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<double>(0, 0);
Phi += n_k.at<double>(1, 0);
z_k.at<double>(0, 0) = R;
z_k.at<double>(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<double>(0, 0) = 1e-14;
processNoiseCov.at<double>(1, 1) = 1e-14;
processNoiseCov.at<double>(2, 2) = 2.4065 * 1e-5;
processNoiseCov.at<double>(3, 3) = 2.4065 * 1e-5;
processNoiseCov.at<double>(4, 4) = 1e-6;
Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type );
sqrt( processNoiseCov, processNoiseCovSqrt );
Mat measurementNoiseCov = Mat::zeros( MP, MP, type );
measurementNoiseCov.at<double>(0, 0) = 1e-3*1e-3;
measurementNoiseCov.at<double>(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<double>(0, 0) = 6500.4;
state.at<double>(1, 0) = 349.14;
state.at<double>(2, 0) = -1.8093;
state.at<double>(3, 0) = -6.7967;
state.at<double>(4, 0) = 0.6932;
Mat initState = state.clone();
initState.at<double>(4, 0) = 0.0;
Mat P = 1e-6 * Mat::eye( DP, DP, type );
P.at<double>(4, 4) = 1.0;
Mat measurement( MP, 1, type );
Mat q( DP, 1, type );
Mat r( MP, 1, type );
Ptr<BallisticModel> 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<UnscentedKalmanFilter> uncsentedKalmanFilter = createUnscentedKalmanFilter(params);
Mat correctStateUKF( DP, 1, type );
Mat u = Mat::zeros( DP, 1, type );
for (int i = 0; i<nIterations; i++)
{
rng.fill( q, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
q = processNoiseCovSqrt*q;
rng.fill( r, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
r = measurementNoiseCovSqrt*r;
model->stateConversionFunction(state, u, q, state);
model->measurementFunction(state, r, measurement);
uncsentedKalmanFilter->predict();
correctStateUKF = uncsentedKalmanFilter->correct( measurement );
}
double landing_y = correctStateUKF.at<double>(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<double>(0, 0) = 1e-14;
processNoiseCov.at<double>(1, 1) = 1e-14;
processNoiseCov.at<double>(2, 2) = 2.4065 * 1e-5;
processNoiseCov.at<double>(3, 3) = 2.4065 * 1e-5;
processNoiseCov.at<double>(4, 4) = 1e-6;
Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type );
sqrt( processNoiseCov, processNoiseCovSqrt );
Mat measurementNoiseCov = Mat::zeros( MP, MP, type );
measurementNoiseCov.at<double>(0, 0) = 1e-3*1e-3;
measurementNoiseCov.at<double>(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<double>(0, 0) = 6500.4;
state.at<double>(1, 0) = 349.14;
state.at<double>(2, 0) = -1.8093;
state.at<double>(3, 0) = -6.7967;
state.at<double>(4, 0) = 0.6932;
Mat initState = state.clone();
Mat initStateKF = state.clone();
initStateKF.at<double>(4, 0) = 0.0;
Mat P = 1e-6 * Mat::eye( DP, DP, type );
P.at<double>(4, 4) = 1.0;
Mat measurement( MP, 1, type );
Mat q( DP, 1, type);
Mat r( MP, 1, type);
Ptr<BallisticModel> 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<UnscentedKalmanFilter> uncsentedKalmanFilter = createUnscentedKalmanFilter(params);
state = initState.clone();
for (int i = 0; i<nIterations; i++)
{
rng.fill( q, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
q = processNoiseCovSqrt*q;
rng.fill( r, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
r = measurementNoiseCovSqrt*r;
model->stateConversionFunction(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<double>(i, l) += pow( errorUKF.at<double>(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<double>(0, 0);
double n = u_k.at<double>(0, 0);
double q = v_k.at<double>(0, 0);
double u = u_k.at<double>(0, 0);
double x1 = 0.5*x + 25*( x/(x*x + 1) ) + 8*cos( 1.2*(n-1) ) + q + u;
x_kplus1.at<double>(0, 0) = x1;
}
void measurementFunction(const Mat& x_k, const Mat& n_k, Mat& z_k)
{
double x = x_k.at<double>(0, 0);
double r = n_k.at<double>(0, 0);
double y = x*x/20.0 + r;
z_k.at<double>(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<UnivariateNonstationaryGrowthModel> model( new UnivariateNonstationaryGrowthModel() );
UnscentedKalmanFilterParams params( DP, MP, CP, 0, 0, model );
Mat processNoiseCov = Mat::zeros( DP, DP, type );
processNoiseCov.at<double>(0, 0) = 1.0;
Mat processNoiseCovSqrt = Mat::zeros( DP, DP, type );
sqrt( processNoiseCov, processNoiseCovSqrt );
Mat measurementNoiseCov = Mat::zeros( MP, MP, type );
measurementNoiseCov.at<double>(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<double>(0, 0) = 0.1;
Mat initState = state.clone();
initState.at<double>(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<UnscentedKalmanFilter> uncsentedKalmanFilter = createUnscentedKalmanFilter( params );
state.at<double>(0, 0) = 0.1;
double mse = 0.0;
for (int i = 0; i<nIterations; i++)
{
rng.fill( q, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
rng.fill( r, RNG::NORMAL, Scalar::all(0), Scalar::all(1) );
q = processNoiseCovSqrt*q;
r = measurementNoiseCovSqrt*r;
u.at<double>(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<double>(0, 0) - correctStateAUKF.at<double>(0, 0), 2.0 );
}
mse /= nIterations;
average_error += mse;
}
average_error /= 1000.0;
ASSERT_GE( mse_treshold, average_error );
}
Write Preview
Loading…
Cancel
Save