/*
* helper function to allocate all the memory necessary to respresent the responsabilities.
* @param N_meas number of points
* @param N_comp number of clusters
*/
void allocate(int N_meas, int N_comp)
{
R.assign(N_meas,ColumnVector(N_comp));
responsibleCluster.assign(N_meas,-1);
sumR.resize(N_meas);
N.resize(N_comp);
}
//---------------------------------------------------------------------------
void TEstimation::preparePosteriorDensityPoints(){
//prepare vector for points at which the density (also prior!!!) is estimated
theta=ColumnVector(posteriorDensityPoints);
//float step=1.2/(posteriorDensityPoints-1);
//for(int j=1; j<=posteriorDensityPoints; ++j) theta(j)=-0.1+(j-1)*step;
//since the parameters are standardized between 0 and 1, we can easy calculate the posterior between 0 and 1
step=1.0/(posteriorDensityPoints-1.0);
for(int j=0; j<posteriorDensityPoints; ++j) theta(j+1)=(j)*step;
}
void CParam::init_pi() {
nu_short = ColumnVector(K - 1);
nu_short = 0.1 ; // WAS 0.1 starting value
logpi = ColumnVector(K);
pi= ColumnVector(K);
double Sum_logOneMinusVg = 0.0 ;
for (int i= 1; i<= (K-1); i++) {
double nu_k = nu_short(i) ;
logpi(i) = log(nu_k) + Sum_logOneMinusVg ;
pi(i) = exp(logpi(i)) ;
double one_minus_V = ( 1.0 - nu_short(i) ) ;
Sum_logOneMinusVg = Sum_logOneMinusVg + log(one_minus_V) ;
}
logpi(K) = Sum_logOneMinusVg ;
pi(K) = exp(Sum_logOneMinusVg) ;
}
void CParam::init_sigmamu(CData &Data) {
mu_bar = ColumnVector(n_var_independent);
ColumnVector log_obs_mean(n_var), log_obs_sd(n_var) ;
Matrix logD_editpassing(n_sample-Data.n_faulty,n_var) ;
for (int i_sample=1, count = 1; i_sample<=n_sample; i_sample++){
if (Data.is_case(i_sample,0)) {
logD_editpassing.row(count++) = Data.log_D_Observed.row(i_sample);
}
}
for (int i_var=1; i_var<=n_var; i_var++){
ColumnVector temp_col = logD_editpassing.column(i_var) ;
log_obs_mean(i_var) = 1.0/(n_sample-Data.n_faulty)*(temp_col.sum()) ;
ColumnVector temp_mean(n_sample-Data.n_faulty) ; temp_mean = log_obs_mean(i_var) ;
Matrix SumSq = (temp_col-temp_mean).t()*(temp_col-temp_mean) ;
log_obs_sd(i_var) = sqrt( 1.0/(n_sample-Data.n_faulty-1)*SumSq(1,1) ) ;
}
Data.UpdateCompactVector(mu_bar,log_obs_mean);
DiagonalMatrix LSigma_temp(n_var_independent);
for (int i=1; i<= n_var_independent; i++){
LSigma_temp(i) = log_obs_sd(i);
}
DiagonalMatrix LSigma_temp_i = LSigma_temp.i();
LSIGMA = vector<LowerTriangularMatrix>(K);
LSIGMA_i = vector<LowerTriangularMatrix>(K);
SIGMA = vector<SymmetricMatrix>(K);
for (int k=0 ; k<K; k++) {
LSIGMA[k] = LSigma_temp;
LSIGMA_i[k] = LSigma_temp_i;
SIGMA[k] << LSigma_temp * LSigma_temp; //lazy
}
logdet_and_more = ColumnVector(K); logdet_and_more = -0.5*n_var* LOG_2_PI + logdet(LSIGMA_i[0]);
Mu = Matrix((n_var_independent),K); // Note that mu_k is Mu.column(k)
for (int k=1; k<=K; k++){
Mu.column(k) = mu_bar ; // starting value
}
}
void CParam::init_logUnif_y_tilde(CData &Data,CFeasibilityMap &FM, Uniform &randUnif, int n_simul) {
logUnif_y_tilde = ColumnVector(Data.n_faulty);
for (int i = 1; i <= Data.n_faulty; i++) {
ColumnVector s_i = S_Mat.column(i);
int i_original = Data.Faulty2Original[i-1];
// double log_f_y_tilde_q = 0; Commented by HANG on 5/16/2015
if (FM.useMap && Data.is_case(i_original,2)) {
logUnif_y_tilde(i) = FM.Simulate_logUnif_case2(CFeasibilityMap::s_to_tau_fn(s_i),i_original,n_simul,randUnif);
} else {
logUnif_y_tilde(i) = Data.get_logUnif_y_tilde(s_i, CFeasibilityMap::s_to_tau_fn(s_i),i_original);
}
//logUnif_y_tilde(i) = Data.get_logUnif_y_tilde(s_i, CFeasibilityMap::s_to_tau_fn(s_i),i_original);
}
}
bool CFeasibilityMap::SolveLP(Matrix &A, ColumnVector &b, ColumnVector &x) {
lprec *lp ;
int n_row = A.nrows(); int n_col = A.ncols();
x = ColumnVector(n_col); x = 0;
lp = make_lp(0,n_col) ;
double *input_row = new double[1+n_col];
for (int i_row=1; i_row<=n_row; i_row++){
input_row[0] = 0 ; // The first zero is for matrix form
for (int j=1; j<=n_col; j++){
input_row[j] = A(i_row,j) ;
}
add_constraint(lp, input_row, LE, b(i_row)) ;
}
delete [] input_row;
double *input_obj = new double[1+n_col]; // The first zero is for matrix form
input_obj[0] = 0 ;
for (int j=1; j<=n_col; j++){
input_obj[j] = 1 ;
}
set_obj_fn(lp, input_obj) ;
delete [] input_obj;
set_verbose(lp, IMPORTANT); // NEUTRAL (0), IMPORTANT (3), NORMAL (4), FULL (6)
bool is_feasible = (solve(lp)==0); // 0: feasible solution found, 2: not found
// solution for minimizing objective function
double* x_min = new double[n_col];
double* x_max = new double[n_col];
if (is_feasible) {
get_variables(lp, x_min);
set_maxim(lp);
is_feasible = (solve(lp)==0); // 0: feasible solution found, 2: not found
if (is_feasible) {
get_variables(lp, x_max);
for (int i = 0; i < n_col; i++) {
x(i+1) = (x_min[i] + x_max[i]) / 2.0;
}
}
}
delete [] x_min;
delete [] x_max;
delete_lp(lp);
return is_feasible;
}
/*
* constructor.
* @param N_comp number of clusters
* @param N_comp_eff number of effective clusters
* @param D dimension
* @param Beta_init beta
* @param Nu_init nu
* @param Alfa_init alfa
* @param W_init W
* @param Sigma_init sigma
* @param Pi_init pi
*/
Cluster_parameters(int N_comp=1, int N_comp_eff=0, int D=1, double Beta_init = 0.05, double Nu_init = 10.0, double Alfa_init = 1.0 ,double W_init = 10.0, double Sigma_init = 1.0 , double Pi_init = 0.0)
: N_comp_effective(N_comp_eff), Beta(N_comp,Beta_init), Nu(N_comp,Nu_init), Alfa(N_comp,Alfa_init), Pi(N_comp)
{
// allocate
Pi = Pi_init;
M.assign(N_comp,ColumnVector(D));
Matrix W_el(D,D);
W_el = 0.0;
Matrix Sigma_el(D,D);
Sigma_el = 0.0;
for (int d=1; d<=D; d++)
{
W_el(d,d) = W_init;
Sigma_el(d,d) = Sigma_init;
}
W.assign(N_comp,W_el);
Sigma.assign(N_comp,Sigma_el);
}
// Assumes that there is either no exogenous variables or there is a single
// exogenous variable that is constant and equal to one. The unconditional
// mean is obtained from the reduced form companion matrix.
TDenseVector UnconditionalMean(const TDenseMatrix &A0, const TDenseMatrix &Aplus, bool IsConstant)
{
int n_lags=NumberLags(A0,Aplus,IsConstant), n_vars=A0.cols;
if (!IsConstant) return TDenseVector(n_vars,0.0);
TDenseMatrix B=ReducedForm(A0,Aplus);
if (n_lags == 0) return ColumnVector(B,0);
TDenseMatrix C=CompanionMatrix(B,n_lags);
TDenseVector b(n_vars*n_lags,0.0);
b.InsertColumnVector(0,B,n_vars*n_lags,0,n_vars-1);
try
{
return SubVector(InverseMultiply(Identity(n_vars*n_lags) - C,b),0,n_vars-1);
}
catch (dw_exception &e)
{
throw dw_exception("UnconditionalMean(): Unconditional mean does not exist");
}
}
void CParam::init_z_in() {
z_in = ColumnVector(n_sample) ;
for (int i_sample=1; i_sample<=n_sample; i_sample++) {
// calculate pi_tilde_in
ColumnVector logN_unnorm(K);
ColumnVector y_compact = Y_in_compact.row(i_sample).t() ;
for (int k=1; k<=K; k++) {
ColumnVector mu_k = Mu.column(k);
logN_unnorm(k) = log_MVN_fn( y_compact, mu_k, LSIGMA_i[k-1],logdet_and_more(k));
}
double max_logN_unnorm = logN_unnorm.maximum();
ColumnVector pi_tilde_in_unnorm(K); pi_tilde_in_unnorm = 0.0 ;
for (int k=1; k<=K; k++){
pi_tilde_in_unnorm(k) = pi(k) * exp(logN_unnorm(k)-max_logN_unnorm);
}
ColumnVector pi_tilde_in = (1.0/pi_tilde_in_unnorm.sum()) * pi_tilde_in_unnorm ;
z_in(i_sample) = rdiscrete_fn(pi_tilde_in);
}
}
/*
* helper function to allocate all the memory necessary to respresent the cluster parameters.
* @param N_comp number of clusters
* @param N_comp_eff number of effective components
* @param D dimension
* @param Beta_init beta
* @param Nu_init nu
* @param Alfa_init alfa
* @param W_init W
* @param Sigma_init sigma
* @param Pi_init pi
*/
void allocate(int N_comp, int N_comp_eff=0, int D=1, double Beta_init = 0.05, double Nu_init = 10.0, double Alfa_init = 1.0 ,double W_init = 10.0, double Sigma_init = 1.0 , double Pi_init = 0.0)
{
N_comp_effective = N_comp_eff;
Beta.assign(N_comp,Beta_init);
Nu.assign(N_comp,Nu_init);
Alfa.assign(N_comp,Alfa_init);
Pi.resize(N_comp);
Pi= Pi_init;
M.assign(N_comp,ColumnVector(D));
Matrix W_el(D,D);
W_el = 0.0;
Matrix Sigma_el(D,D);
Sigma_el = 0.0;
for (int d=1; d<=D; d++)
{
W_el(d,d) = W_init;
Sigma_el(d,d) = Sigma_init;
}
W.assign(N_comp,W_el);
Sigma.assign(N_comp,Sigma_el);
}
void CParam::initizalize(CData &Data, int nComp, CFeasibilityMap &FM, Uniform &randUnif, int n_simul){
// Set the minimum value of (no. of correct records)/(no. of all records)
toolarge_nout = (1-min_Prob_A)/min_Prob_A * Data.n_sample ;
n_var = Data.n_var;
n_var_independent = n_var - Data.n_balance_edit;
K = nComp;
n_sample = Data.n_sample;
alpha = 1.0 ;
Phi = IdentityMatrix(n_var_independent) * 5.0;
init_pi();
init_sigmamu(Data);
init_Y_in(Data);
init_z_in();
S_Mat = Data.initial_S_Mat.t();
init_logUnif_y_tilde(Data,FM,randUnif,n_simul);
n_z = ColumnVector(K); n_z = 0 ;
Sigma_k_inv_ll = Matrix(K,n_var_independent); Sigma_k_inv_ll=0.0;
X_bar= Matrix(n_var_independent,K); X_bar = 0.0 ;
CalculateInitProbA(Data);
}
RBFInterpolator::RBFInterpolator(vector<real> x, vector<real> y, vector<real> z, vector<real> f)
{
successfullyInitialized = false; // default value for if we end init prematurely.
M = f.size();
// all four input vectors must have the same length.
if ( x.size() != M || y.size() != M || z.size() != M )
return;
ColumnVector F = ColumnVector(M + 4);
P = Matrix(M, 3);
Matrix G(M + 4,M + 4);
// copy function values
for (int i = 1; i <= M; i++)
F(i) = f[i-1];
F(M+1) = 0; F(M+2) = 0; F(M+3) = 0; F(M+4) = 0;
// fill xyz coordinates into P
for (int i = 1; i <= M; i++)
{
P(i,1) = x[i-1];
P(i,2) = y[i-1];
P(i,3) = z[i-1];
}
// the matrix below is symmetric, so I could save some calculations Hmmm. must be a todo
for (int i = 1; i <= M; i++)
for (int j = 1; j <= M; j++)
{
real dx = x[i-1] - x[j-1];
real dy = y[i-1] - y[j-1];
real dz = z[i-1] - z[j-1];
real distance_squared = dx*dx + dy*dy + dz*dz;
G(i,j) = g(distance_squared);
}
//Set last 4 columns of G
for (int i = 1; i <= M; i++)
{
G( i, M+1 ) = 1;
G( i, M+2 ) = x[i-1];
G( i, M+3 ) = y[i-1];
G( i, M+4 ) = z[i-1];
}
for (int i = M+1; i <= M+4; i++)
for (int j = M+1; j <= M+4; j++)
G( i, j ) = 0;
//Set last 4 rows of G
for (int j = 1; j <= M; j++)
{
G( M+1, j ) = 1;
G( M+2, j ) = x[j-1];
G( M+3, j ) = y[j-1];
G( M+4, j ) = z[j-1];
}
Try
{
Ginv = G.i();
A = Ginv*F;
successfullyInitialized = true;
}
CatchAll { cout << BaseException::what() << endl; }
}
ColumnVector SDThread::calcSD( int id )
{
return ColumnVector( 20 );
}
/*
* constructor.
* @param N_meas number of points
* @param N_comp number of clusters
* @param N_meas_eff number of effective points
* @param N_comp_eff number of effective clusters
*/
Respons_struct(int N_meas = 181, int N_comp = 1, int N_meas_eff = 0, int N_comp_eff = 0)
: R(N_meas,ColumnVector(N_comp)), sumR(N_meas), N(N_comp), N_meas_effective(N_meas_eff), N_comp_effective(N_comp_eff)
{
}
void Robot::dqp_torque(const ColumnVector & q, const ColumnVector & qp,
const ColumnVector & dqp,
ColumnVector & ltorque, ColumnVector & dtorque)
{
int i;
ColumnVector z0(3);
Matrix Rt, temp;
Matrix Q(3,3);
ColumnVector *w, *wp, *vp, *a, *f, *n, *F, *N, *p;
ColumnVector *dw, *dwp, *dvp, *da, *df, *dn, *dF, *dN, *dp;
if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
ltorque = ColumnVector(dof);
dtorque = ColumnVector(dof);
set_q(q);
w = new ColumnVector[dof+1];
wp = new ColumnVector[dof+1];
vp = new ColumnVector[dof+1];
a = new ColumnVector[dof+1];
f = new ColumnVector[dof+1];
n = new ColumnVector[dof+1];
F = new ColumnVector[dof+1];
N = new ColumnVector[dof+1];
p = new ColumnVector[dof+1];
dw = new ColumnVector[dof+1];
dwp = new ColumnVector[dof+1];
dvp = new ColumnVector[dof+1];
da = new ColumnVector[dof+1];
df = new ColumnVector[dof+1];
dn = new ColumnVector[dof+1];
dF = new ColumnVector[dof+1];
dN = new ColumnVector[dof+1];
dp = new ColumnVector[dof+1];
w[0] = ColumnVector(3);
wp[0] = ColumnVector(3);
vp[0] = gravity;
dw[0] = ColumnVector(3);
dwp[0] = ColumnVector(3);
dvp[0] = ColumnVector(3);
z0 = 0.0;
Q = 0.0;
Q(1,2) = -1.0;
Q(2,1) = 1.0;
z0(3) = 1.0;
w[0] = 0.0;
wp[0] = 0.0;
dw[0] = 0.0;
dwp[0] = 0.0;
dvp[0] = 0.0;
for(i = 1; i <= dof; i++) {
Rt = links[i].R.t();
p[i] = ColumnVector(3);
p[i](1) = links[i].get_a();
p[i](2) = links[i].get_d() * Rt(2,3);
p[i](3) = links[i].get_d() * Rt(3,3);
if(links[i].get_joint_type() != 0) {
dp[i] = ColumnVector(3);
dp[i](1) = 0.0;
dp[i](2) = Rt(2,3);
dp[i](3) = Rt(3,3);
}
if(links[i].get_joint_type() == 0) {
w[i] = Rt*(w[i-1] + z0*qp(i));
dw[i] = Rt*(dw[i-1] + z0*dqp(i));
wp[i] = Rt*(wp[i-1] + vec_x_prod(w[i-1],z0*qp(i)));
dwp[i] = Rt*(dwp[i-1]
+ vec_x_prod(dw[i-1],z0*qp(i))
+ vec_x_prod(w[i-1],z0*dqp(i))
);
vp[i] = vec_x_prod(wp[i],p[i])
+ vec_x_prod(w[i],vec_x_prod(w[i],p[i]))
+ Rt*(vp[i-1]);
dvp[i] = vec_x_prod(dwp[i],p[i])
+ vec_x_prod(dw[i],vec_x_prod(w[i],p[i]))
+ vec_x_prod(w[i],vec_x_prod(dw[i],p[i]))
+ Rt*dvp[i-1];
} else {
w[i] = Rt*w[i-1];
dw[i] = Rt*dw[i-1];
wp[i] = Rt*wp[i-1];
dwp[i] = Rt*dwp[i-1];
vp[i] = Rt*(vp[i-1]
+ vec_x_prod(w[i],z0*qp(i))) * 2.0
+ vec_x_prod(wp[i],p[i])
+ vec_x_prod(w[i],vec_x_prod(w[i],p[i]));
dvp[i] = Rt*(dvp[i-1]
+ (vec_x_prod(dw[i],z0*qp(i)) * 2.0
+ vec_x_prod(w[i],z0*dqp(i))))
+ vec_x_prod(dwp[i],p[i])
+ vec_x_prod(dw[i],vec_x_prod(w[i],p[i]))
+ vec_x_prod(w[i],vec_x_prod(dw[i],p[i]));
}
a[i] = vec_x_prod(wp[i],links[i].r)
+ vec_x_prod(w[i],vec_x_prod(w[i],links[i].r))
+ vp[i];
da[i] = vec_x_prod(dwp[i],links[i].r)
+ vec_x_prod(dw[i],vec_x_prod(w[i],links[i].r))
+ vec_x_prod(w[i],vec_x_prod(dw[i],links[i].r))
+ dvp[i];
}
for(i = dof; i >= 1; i--) {
F[i] = a[i] * links[i].m;
N[i] = links[i].I*wp[i] + vec_x_prod(w[i],links[i].I*w[i]);
dF[i] = da[i] * links[i].m;
dN[i] = links[i].I*dwp[i] + vec_x_prod(dw[i],links[i].I*w[i])
+ vec_x_prod(w[i],links[i].I*dw[i]);
if(i == dof) {
f[i] = F[i];
n[i] = vec_x_prod(p[i],f[i])
+ vec_x_prod(links[i].r,F[i]) + N[i];
df[i] = dF[i];
dn[i] = vec_x_prod(p[i],df[i])
+ vec_x_prod(links[i].r,dF[i]) + dN[i];
} else {
f[i] = links[i+1].R*f[i+1] + F[i];
n[i] = links[i+1].R*n[i+1] + vec_x_prod(p[i],f[i])
+ vec_x_prod(links[i].r,F[i]) + N[i];
df[i] = links[i+1].R*df[i+1] + dF[i];
dn[i] = links[i+1].R*dn[i+1] + vec_x_prod(p[i],df[i])
+ vec_x_prod(links[i].r,dF[i]) + dN[i];
}
if(links[i].get_joint_type() == 0) {
temp = ((z0.t()*links[i].R)*n[i]);
ltorque(i) = temp(1,1);
temp = ((z0.t()*links[i].R)*dn[i]);
dtorque(i) = temp(1,1);
} else {
temp = ((z0.t()*links[i].R)*f[i]);
ltorque(i) = temp(1,1);
temp = ((z0.t()*links[i].R)*df[i]);
dtorque(i) = temp(1,1);
}
}
delete []dp;
delete []dN;
delete []dF;
delete []dn;
delete []df;
delete []da;
delete []dvp;
delete []dwp;
delete []dw;
delete []p;
delete []N;
delete []F;
delete []n;
delete []f;
delete []a;
delete []vp;
delete []wp;
delete []w;
}