Impala Documentation

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

Definition at line 473 of file FunctorKalman.h.

00474         {
00475         double r, err=0;
00476         for(int i=0 ; i<3 ; i++)
00477         {
00478             r = p1[i] - p2[i];
00479             err += r*r/(mSigma[i]);
00480         }
00481 
00482         // implements Hubers functon (eq.3)
00483         if(err < mChi)
00484         {
00485             double r = 0.5 * err * err;
00486             res[0] = res[1] = res[2] = r;
00487             mError += r;
00488         }
00489         else
00490         {
00491             double r = mChi*(err - mChi/2);
00492             res[0] = res[1] = res[2] = r;
00493             mError += r;
00494         }
00495         }