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; }