Impala Documentation

void Impala::Core::Array::Trait::FuncBpoRobustMahalanobis::DoIt	(	double *	res,
		double *	p1,
		double *	p2
	)			`[inline]`

Definition at line 491 of file FunctorKalman.h.

00492         {
00493         double r, err=0;
00494         for(int i=0 ; i<3 ; i++)
00495         {
00496             r = p1[i] - p2[i];
00497             err += r*r/(mSigma[i]);
00498         }
00499 
00500         // implements Hubers functon (eq.3)
00501         if(err < mChi)
00502         {
00503             double r = 0.5 * err * err;
00504             res[0] = res[1] = res[2] = r;
00505             mError += r;
00506         }
00507         else
00508         {
00509             double r = mChi*(err - mChi/2);
00510             res[0] = res[1] = res[2] = r;
00511             mError += r;
00512         }
00513         }