void GaussianDistribution2D::merge(const GaussianDistribution2D& other)
{
//Compute new mean and values for new covariance matrix:
double p1(probabilityAtMean());
double p2(other.probabilityAtMean());
double pSum(p1+p2);
double w1(p1/pSum);
double w2(p2/pSum);
Vector2<double> newMean(mean*w1 + other.mean*w2);
Vector2<double> diffMean(mean - other.mean);
Matrix2x2<double> meanMatrix;
meanMatrix.c[0].x = diffMean.x * diffMean.x;
meanMatrix.c[0].y = meanMatrix.c[1].x = diffMean.x*diffMean.y;
meanMatrix.c[1].y = diffMean.y * diffMean.y;
//Set new values:
mean = newMean;
covariance = (covariance*w1 + other.covariance*w2 + meanMatrix*w1*w2);
}
void GaussianDistribution3D::merge(const GaussianDistribution3D& other)
{
//Compute new mean and values for new covariance matrix:
float p1(probabilityAtMean());
float p2(other.probabilityAtMean());
float pSum(p1 + p2);
float w1(p1 / pSum);
float w2(p2 / pSum);
Vector3BH<> newMean(mean * w1 + other.mean * w2);
Vector3BH<> diffMean(mean - other.mean);
Matrix3x3BH<> meanMatrix;
meanMatrix.c0.x = diffMean.x * diffMean.x;
meanMatrix.c0.y = diffMean.x * diffMean.y;
meanMatrix.c0.z = diffMean.x * diffMean.z;
meanMatrix.c1.x = diffMean.x * diffMean.y;
meanMatrix.c1.y = diffMean.y * diffMean.y;
meanMatrix.c1.z = diffMean.z * diffMean.y;
meanMatrix.c1.x = diffMean.x * diffMean.z;
meanMatrix.c1.y = diffMean.z * diffMean.y;
meanMatrix.c1.z = diffMean.z * diffMean.z;
//Set new values:
mean = newMean;
covariance = (covariance * w1 + other.covariance * w2 + meanMatrix * w1 * w2);
}
