24 #include <Base/Common/W32Compat.hh>
25 #include <MathAlgo/UnscentedTransform.hh>
26 #include <MathAlgo/SVD.hh>
27 #include <MathAlgo/Lapack.hh>
28 #include <Base/Common/CompareFloatingPoint.hh>
29 #include <Base/Debug/Exception.hh>
30 #include <Base/Math/Matrix.hh>
57 if (src_cov.
Trace()<numeric_limits<double>::epsilon()){
60 BCDOUT(D_UT_TRANSFORM,
"transformation of mean failed, returning -1");
63 const int dim = dst_mean.
Size();
67 vector<WeightedSigmaPoint> sigma_points;
71 if ((res =
Transform_(src_mean, dst_mean))!=0){
72 BCDOUT(D_UT_TRANSFORM,
"transformation of mean failed, returning -1");
75 BCDOUT(D_UT_TRANSFORM,
"transformation of mean yielded "<<dst_mean);
79 for (
int d=0; d<dst_mean.
size(); d++) {
81 INFNANCHECK(dst_mean[d]);
88 const int dim = dst_mean.
Size();
95 vector<WeightedSigmaPoint>::iterator it;
96 for (it=sigma_points.begin(); it!=sigma_points.end(); it++){
99 BCDOUT(D_UT_TRANSFORM,
"src: "<<src<<endl);
100 if ((res =
Transform_(src, it->point))!=0){
return 1; }
103 for (
int d=0; d<dst_mean.
size(); d++) {
104 INFNANCHECK(it->point[d]);
108 BCDOUT(D_UT_TRANSFORM,
"dst: "<<it->point<<endl);
110 dst_mean += (it->point * it->weight_mean);
111 BCDOUT(D_UT_TRANSFORM,
"mean: "<<dst_mean<<endl);
114 for (it=sigma_points.begin(); it!=sigma_points.end(); it++){
116 diff = it->point - dst_mean;
117 BCDOUT(D_UT_TRANSFORM,
"diff: "<<diff<<endl);
118 BCDOUT(D_UT_TRANSFORM,
"weight "<< it->weight_cov
119 <<
" * outer product: " << diff.
OuterProduct(diff) << endl);
121 BCDOUT(D_UT_TRANSFORM,
"cov: "<<dst_cov<<endl);
124 for (
int row=0; row<dst_cov.
num_rows(); row++) {
125 for (
int col=0; col<dst_cov.
num_cols(); col++) {
126 if (BIAS_ISNAN(dst_cov[row][col]) || BIAS_ISINF(dst_cov[row][col])) {
140 std::vector<WeightedSigmaPoint>& sigma_points)
const
142 const int dim = src_mean.
Size();
143 const double dbl_dim =
static_cast<double>(dim);
145 const double lambda = alpha_square * ( dbl_dim +
Kappa_ ) - dbl_dim;
147 const double dim_plus_lambda = alpha_square * ( dbl_dim +
Kappa_ );
148 BIASASSERT(fabs(dim_plus_lambda)>DBL_EPSILON);
150 BCDOUT(D_UT_PARAM,
"Alpha: "<<Alpha_<<
" Beta : "<<
Beta_
151 <<
" Kappa: "<<
Kappa_<<endl);
152 sigma_points.resize(2 * dim + 1);
157 for (
int r=0; r<src_cov.
num_rows(); r++) {
158 INFNANCHECK(src_mean[r]);
159 for (
int c=0; c<src_cov.
num_cols(); c++) {
160 INFNANCHECK(src_cov[r][c]);
174 for (
int i=0; i<dim; i++) {
182 factor += src_cov[i][i];
185 if (
Less(fabs(factor), numeric_limits<double>::epsilon())){
186 BEXCEPTION(
"cannot normalize zero covariance matrix "<<src_cov);
191 sqrt_cov = svd.
SqrtT(normalized_cov);
196 const int num=normalized_cov.
GetCols();
197 const double eps_fac = 2.0* numeric_limits<double>::epsilon();
198 for (
int i=0; i<num; i++)
199 normalized_cov[i][i] += eps_fac;
203 BIASERR(
"error computing matrix sqrt for "<<normalized_cov<<endl);
210 factor *= dim_plus_lambda;
211 factor = sqrt(factor);
214 sqrt_cov = svd.
SqrtT(dim_plus_lambda * src_cov);
219 const double eps_fac = 2.0* numeric_limits<double>::epsilon();
220 for (
int i=0; i<num; i++)
221 tmp[i][i] += eps_fac;
224 BIASERR(
"error computing matrix sqrt for "<<tmp<<endl);
233 BIASERR(
"could not compute square root of cov "<< src_cov);
248 for (
int r=0; r<sqrt_cov.
num_rows(); r++) {
249 for (
int c=0; c<sqrt_cov.
num_cols(); c++) {
250 INFNANCHECK(sqrt_cov[r][c]);
257 sigma_points[0].point = src_mean;
258 sigma_points[0].weight_mean = lambda / dim_plus_lambda;
259 sigma_points[0].weight_cov =
260 lambda / dim_plus_lambda + 1.0 - alpha_square +
Beta_;
262 INFNANCHECK(sigma_points[0].weight_mean);
263 INFNANCHECK(sigma_points[0].weight_cov);
264 for (
int ii=0; ii<sigma_points[0].point.size(); ii++){
265 INFNANCHECK(sigma_points[0].point[ii]);
269 for (
int i=0; i<dim; i++){
276 sigma_points[ind].point = src_mean + vec;
277 sigma_points[ind+dim].point = src_mean - vec;
278 sigma_points[ind].weight_mean =
279 sigma_points[ind+dim].weight_mean =
280 sigma_points[ind].weight_cov =
281 sigma_points[ind+dim].weight_cov =
282 1.0 / ( 2.0 * dim_plus_lambda );
284 INFNANCHECK(sigma_points[ind].weight_mean);
285 INFNANCHECK(sigma_points[ind].weight_cov);
286 for (
int ii=0; ii<sigma_points[ind].point.size(); ii++){
287 INFNANCHECK(sigma_points[ind].point[ii]);
288 INFNANCHECK(sigma_points[ind+dim].point[ii]);
294 for (
int k=0; k<2; k++){
295 cout <<
"sp["<<k<<
"] = "<<sigma_points[k].point<<
" W_m = "
296 <<sigma_points[k].weight_mean << endl<<
" W_c = "
297 <<sigma_points[k].weight_cov << endl;
bool Less(const T left, const T right, const T eps=std::numeric_limits< T >::epsilon())
comparison function for floating point values
computes and holds the singular value decomposition of a rectangular (not necessarily quadratic) Matr...
Subscript num_cols() const
Matrix< T > & newsize(Subscript M, Subscript N)
Vector< T > GetCol(const int &col) const
return a copy of column "col", zero based counting
Matrix< double > SqrtT()
returns the square root of a symmetric positive definite matrix M A = sqrt(M) = U * sqrt(S)...
unsigned int Size() const
length of the vector
void SetZero()
equivalent to matrix call
int Lapack_Cholesky_SymmetricPositiveDefinit(const BIAS::Matrix< double > &A, BIAS::Matrix< double > &UL, const char ul)
Coputes the Cholesky decomposition of a symmetric positive definit matrix A.
bool DebugLevelIsSet(const long int lv) const
Matrix< T > OuterProduct(const Vector< T > &v) const
outer product, constructs a matrix.
void SetZero()
Sets all values to zero.
long int NewDebugLevel(const std::string &name)
creates a new debuglevel
unsigned int GetCols() const
Subscript num_rows() const