Skip to content

O
opencv_contrib
Commit 45038846 authored by SvetlanaFilicheva's avatar SvetlanaFilicheva
Browse files
Options

Implement Unscented Kalman Filter (UKF) and Augmented UKF

parent ffa6637b
Show whitespace changes
Inline Side-by-side
Showing with 1948 additions and 0 deletions
modules/tracking/include/opencv2/tracking/kalman_filters.hpp 0 → 100644
View file @ 45038846
/*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
modules/tracking/src/augmented_unscented_kalman.cpp 0 → 100644
View file @ 45038846
/*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
modules/tracking/src/unscented_kalman.cpp 0 → 100644
View file @ 45038846
/*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
modules/tracking/test/test_aukf.cpp 0 → 100644
View file @ 45038846
/*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 );
}
modules/tracking/test/test_ukf.cpp 0 → 100644
View file @ 45038846
/*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 );
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment