void KF_joseph_update(VectorXf &x, MatrixXf &P,float v,float R, MatrixXf H)
{
VectorXf PHt = P*H.transpose();
MatrixXf S = H*PHt;
S(0,0) += R;
MatrixXf Si = S.inverse();
Si = make_symmetric(Si);
MatrixXf PSD_check = Si.llt().matrixL(); //chol of scalar is sqrt
PSD_check.transpose();
PSD_check.conjugate();
VectorXf W = PHt*Si;
x = x+W*v;
//Joseph-form covariance update
MatrixXf eye(P.rows(), P.cols());
eye.setIdentity();
MatrixXf C = eye - W*H;
P = C*P*C.transpose() + W*R*W.transpose();
float eps = 2.2204*pow(10.0,-16); //numerical safety
P = P+eye*eps;
PSD_check = P.llt().matrixL();
PSD_check.transpose();
PSD_check.conjugate(); //for upper tri
}
VectorXf EMclustering::loggausspdf(MatrixXf x, VectorXf mu, MatrixXf sigma)
{
//cerr<<x<<endl<<endl;
//cerr<<mu<<endl<<endl;
//cerr<<sigma<<endl<<endl;
int d = x.rows();
int c = x.cols();
int r_sigma = sigma.rows();
int c_sigma = sigma.cols();
MatrixXf tmpx(x.rows(),x.cols());
tmpx = x.colwise() - mu;
MatrixXf u1(r_sigma,c_sigma);
u1 = sigma.llt().matrixL() ;
MatrixXf u2(u1.cols(),u1.rows());
u2 = u1.adjoint();//cerr<<u2<<endl;
MatrixXf Q(u2.cols(),tmpx.cols());
Q = u1.jacobiSvd(ComputeThinU | ComputeThinV).solve(tmpx);
//cerr<<"q"<<Q<<endl;
VectorXf q(Q.cols());
q = Q.cwiseProduct(Q).colwise().sum();//cerr<<"q"<<q<<endl;
VectorXf tmp1(u2.rows());
tmp1 = u2.diagonal();
tmp1 = tmp1.array().log();
double c1 = tmp1.sum() * 2;
double c2 = d * log(2*PI);//cerr<<c1+c2<<endl;
VectorXf y(q.size());
y = -(c1+c2)/2. - q.array()/2.;
tmpx.resize(0,0);
u1.resize(0,0);
u2.resize(0,0);
Q.resize(0,0);
q.resize(0);
tmp1.resize(0);
return y;
}