Example #1
0
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
}
Example #2
0
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;
}