MatrixXf Arm::pseudo_inverse() {
MatrixXf Jacovian = compute_Jacobian();
MatrixXf jjtInv = (Jacovian * Jacovian.transpose());
jjtInv = jjtInv.inverse();
return (Jacovian.transpose() * jjtInv);
}
VectorXf param_sensitivity_widget::computeSensitivity(
MatrixXf ¶meterMatrix, VectorXf &responseVector)
{
MatrixXf Ctemp = parameterMatrix.transpose()*parameterMatrix;
MatrixXf C;
C = Ctemp.inverse();
VectorXf b = C*parameterMatrix.transpose()*responseVector;
VectorXf Y_hat = parameterMatrix*b;
int p = b.rows();
VectorXf sigma2Vec = responseVector-Y_hat;
float sigma2 = sigma2Vec.squaredNorm();
sigma2= sigma2/(parameterMatrix.rows() - p);
Ctemp = C*sigma2;
MatrixXf denominator = Ctemp.diagonal();
// Do element-wise division
VectorXf t = b;
for (int i = 0; i < b.rows(); i++)
{
t(i) = abs(b(i)/sqrt(denominator(i)));
}
return t;
}
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
}
MatrixXf LinkedStructure::pseudoInverse()
{
// Simple math that represents the mathematics
// explained on the website to computing the
// pseudo inverse. this is exactly the math
// discussed in the tutorial!!!
MatrixXf j = jacobian();
MatrixXf jjtInv = (j * j.transpose());
jjtInv = jjtInv.inverse();
return (j.transpose() * jjtInv);
}
//compute proposal distribution, then sample from it, and compute new particle weight
void sample_proposal(Particle &particle, vector<VectorXf> z, vector<int> idf, MatrixXf R)
{
assert(isfinite(particle.w()));
VectorXf xv = VectorXf(particle.xv()); //robot position
MatrixXf Pv = MatrixXf(particle.Pv()); //controls (motion command)
VectorXf xv0 = VectorXf(xv);
MatrixXf Pv0 = MatrixXf(Pv);
vector<MatrixXf> Hv;
vector<MatrixXf> Hf;
vector<MatrixXf> Sf;
vector<VectorXf> zp;
VectorXf zpi;
MatrixXf Hvi;
MatrixXf Hfi;
MatrixXf Sfi;
//process each feature, incrementally refine proposal distribution
unsigned i,r;
vector<int> j;
for (i =0; i<idf.size(); i++) {
j.clear();
j.push_back(idf[i]);
Hv.clear();
Hf.clear();
Sf.clear();
zp.clear();
compute_jacobians(particle,j,R,zp,&Hv,&Hf,&Sf);
zpi = zp[0];
Hvi = Hv[0];
Hfi = Hf[0];
Sfi = Sf[0];
VectorXf vi = z[i] - zpi;
vi[1] = pi_to_pi(vi[1]);
//proposal covariance
Pv = Hvi.transpose() * Sfi * Hvi + Pv.inverse();
Pv = Pv.inverse();
//proposal mean
xv = xv + Pv * Hvi.transpose() * Sfi * vi;
particle.setXv(xv);
particle.setPv(Pv);
}
//sample from proposal distribution
VectorXf xvs = multivariate_gauss(xv,Pv,1);
particle.setXv(xvs);
MatrixXf zeros(3,3);
zeros.setZero();
particle.setPv(zeros);
//compute sample weight: w = w* p(z|xk) p(xk|xk-1) / proposal
float like = likelihood_given_xv(particle, z, idf, R);
float prior = gauss_evaluate(delta_xv(xv0,xvs), Pv0,0);
float prop = gauss_evaluate(delta_xv(xv,xvs),Pv,0);
assert(isfinite(particle.w()));
float a = prior/prop;
float b = particle.w() * a;
float newW = like * b;
//float newW = particle.w() * like * prior / prop;
#if 0
if (!isfinite(newW)) {
cout<<"LIKELIHOOD GIVEN XV INPUTS"<<endl;
cout<<"particle.w()"<<endl;
cout<<particle.w()<<endl;
cout<<"particle.xv()"<<endl;
cout<<particle.xv()<<endl;
cout<<"particle.Pv()"<<endl;
cout<<particle.Pv()<<endl;
cout<<"particle.xf"<<endl;
for (int i =0; i<particle.xf().size(); i++) {
cout<<particle.xf()[i]<<endl;
}
cout<<endl;
cout<<"particle.Pf()"<<endl;
for (int i =0; i< particle.Pf().size(); i++) {
cout<<particle.Pf()[i]<<endl;
}
cout<<endl;
cout<<"z"<<endl;
for (int i=0; i<z.size(); i++) {
cout<<z[i]<<endl;
}
cout<<endl;
cout<<"idf"<<endl;
for (int i =0; i<idf.size(); i++){
cout<<idf[i]<<" ";
}
cout<<endl;
cout<<"R"<<endl;
cout<<R<<endl;
cout<<"GAUSS EVALUATE INPUTS"<<endl;
cout<<"delta_xv(xv0,xvs)"<<endl;
cout<<delta_xv(xv0,xvs)<<endl;
cout<<"Pv0"<<endl;
cout<<Pv0<<endl;
cout<<"delta_xv(xv,xvs)"<<endl;
cout<<delta_xv(xv,xvs)<<endl;
cout<<"Pv"<<endl;
cout<<Pv<<endl;
cout<<"like"<<endl;
cout<<like<<endl;
cout<<"prior"<<endl;
cout<<prior<<endl;
cout<<"prop"<<endl;
cout<<prop<<endl;
}
#endif
particle.setW(newW);
}
int main(int argc, char **argv){
int n = atof(argv[1]);
string method = argv[2];
double h = 1.0/(n+1.0);
double max_error = pow(10,-6);
double error =1;
time_t t_start,t_end;
double dif_iterative, dif_matrix;
VectorXf x (n,1);
VectorXf xk (n,1);
VectorXf xkplus1 (n,1);
VectorXf f (n,1);
MatrixXf A = MatrixXf::Zero(n,n);
MatrixXf I = MatrixXf::Zero(n,n);
VectorXf b (n,1);
VectorXf xc (n,1);
for (int i=0; i<n;i++){
x(i) = h*(i+1);
f(i) = func(x(i));
}
for (int i=0;i<n;i++){
A(i,i) = 2;
if (i!=n-1){
A(i,i+1) = -1;
A(i+1,i) = -1;
}
}
b = f*pow(h,2);
//Solving the Equation
t_start = time(0);
xc = A.colPivHouseholderQr().solve(b);
t_end=time(0);
dif_matrix = difftime (t_end,t_start);
/*
cout << "Here is the matrix A:\n" << A << endl;
cout << "Here is the vector x:\n" << x << endl;
cout << "Here is the vector b:\n" << b << endl;
cout << "Here is the vector uc:\n" << xc << endl;
*/
//Jacobin part
MatrixXf P = MatrixXf::Zero(n,n);
MatrixXf Pinverse = MatrixXf::Zero(n,n);
if (method == "Jacobian"){
for (int i=0; i<n;i++){
P(i,i) = A(i,i);
}
} else if (method=="Gauss-Seidel"){
for (int i=0; i<n;i++){
P(i,i) = A(i,i);
for (int j=0; j<i;j++){
P(i,j) = A(i,j);
}
}
} else if (method=="SOR"){
double omega = atof(argv[3]);
for (int i=0; i<n;i++){
P(i,i) = A(i,i);
for (int j=0; j<i;j++){
P(i,j) = omega*A(i,j);
}
}
} else {
return 1;
}
Pinverse = P.inverse();
for (int i=0; i<n;i++){
xk(i) = 0;
I(i,i) = 1;
}
t_start = time(0);
double repeat_error=100*n;
int loop=1;
while(error >=max_error){
xkplus1 = (I-Pinverse*A)*xk+Pinverse*b;
xk = xkplus1;
error = maximum_array(xc,xkplus1,n);
//cout << loop<<endl;
//cout << error<<endl;
if (repeat_error == error){
break;
}
repeat_error = error;
loop +=1;
}
t_end=time(0);
dif_iterative = difftime (t_end,t_start);
cout << dif_iterative <<"," <<loop<<"," << dif_matrix <<","<<maximum_array(xc,xkplus1,n);
return 0;
}
