gsl_matrix * SgFilter::pseudoInverse(gsl_matrix *A, int n_row, int n_col){
gsl_matrix * A_t = gsl_matrix_alloc (n_col, n_row); //A_t is transpose
gsl_matrix_transpose_memcpy (A_t, A);
gsl_matrix * U = gsl_matrix_alloc (n_col, n_row);
gsl_matrix * V= gsl_matrix_alloc (n_row, n_row);
gsl_vector * S = gsl_vector_alloc (n_row);
// Computing the SVD of the transpose of A
gsl_vector * work = gsl_vector_alloc (n_row);
gsl_linalg_SV_decomp (A_t, V, S, work);
gsl_vector_free(work);
gsl_matrix_memcpy (U, A_t);
//Inverting S
gsl_matrix * Sp = gsl_matrix_alloc (n_row, n_row);
gsl_matrix_set_zero (Sp);
for (int i = 0; i < n_row; i++)
gsl_matrix_set (Sp, i, i, gsl_vector_get(S, i)); // Vector 'S' to matrix 'Sp'
gsl_permutation * p = gsl_permutation_alloc (n_row);
int signum;
gsl_linalg_LU_decomp (Sp, p, &signum); // Computing the LU decomposition
// Compute the inverse
gsl_matrix * SI = gsl_matrix_calloc (n_row, n_row);
for (int i = 0; i < n_row; i++) {
if (gsl_vector_get (S, i) > 0.0000000001)
gsl_matrix_set (SI, i, i, 1.0 / gsl_vector_get (S, i));
}
gsl_matrix * VT = gsl_matrix_alloc (n_row, n_row);
gsl_matrix_transpose_memcpy (VT, V); // Tranpose of V
//THE PSEUDOINVERSE
//Computation of the pseudoinverse of trans(A) as pinv(A) = U·inv(S).trans(V) with trans(A) = U.S.trans(V)
gsl_matrix * SIpVT = gsl_matrix_alloc (n_row, n_row);
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, // Calculating inv(S).trans(V)
1.0, SI, VT,
0.0, SIpVT);
gsl_matrix * pinv = gsl_matrix_alloc (n_col, n_row); // Calculating U·inv(S).trans(V)
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
1.0, U, SIpVT,
0.0, pinv);
gsl_matrix_free(VT);
gsl_matrix_free(SI);
gsl_matrix_free(SIpVT);
gsl_matrix_free(A_t);
gsl_matrix_free(U);
gsl_matrix_free(A);
gsl_matrix_free(V);
gsl_vector_free(S);
return pinv;
}
void Nav_MotionEstimator::Nav_PKPropagator_Unicycle( double * Uk, double T)
{
double thPose = gsl_matrix_get(MeanPose, 2, 0);
/** todo: move uncertanty values to configuration file */
double Sig1VtError = 0.01;
double Sig2VthError = 0.01;
double Sig3VtError = 0.01;
double Sig4VthError = 0.01;
double M11 = Sig1VtError * fabs(Uk[0]) + Sig2VthError * fabs(Uk[1]);
double M22 = Sig3VtError * fabs(Uk[0]) + Sig4VthError * fabs(Uk[1]); //on the ekf use systematic error
gsl_matrix_set(Nk, 0, 0, pow(M11, 2));
gsl_matrix_set(Nk, 0, 1, 0);
gsl_matrix_set(Nk, 1, 0, 0);
gsl_matrix_set(Nk, 1, 1, pow(M22, 2));
/**
position gradient
Fx =[[ 1 0 -vt*T*sin(theta) ]
[ 0 1 vt*T*cos(theta) ]
[ 0 0 1 ]]
*/
gsl_matrix_set_identity(Fx);
gsl_matrix_set(Fx, 0, 2, -Uk[0] * T * sin(thPose));
gsl_matrix_set(Fx, 1, 2, Uk[0] * T * cos(thPose));
gsl_matrix_transpose_memcpy(FxT, Fx); //F transpose
/**
velocities gradient
Fu = [ [ T*cos(theta) 0 ]
[ T*sin(theta) 0 ]
[ 0 T ]]
*/
gsl_matrix_set(Fu, 0, 0, T * cos(thPose));
gsl_matrix_set(Fu, 0, 1, 0);
gsl_matrix_set(Fu, 1, 0, T * sin(thPose));
gsl_matrix_set(Fu, 1, 1, 0);
gsl_matrix_set(Fu, 2, 0, 0);
gsl_matrix_set(Fu, 2, 1, T);
gsl_matrix_transpose_memcpy(FuT, Fu); //F transpose
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Fu, Nk, 0.0, Qk_temp);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qk_temp, FuT, 0.0, Qk);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Fx, CovPose, 0.0, Pk_temp);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Pk_temp, FxT, 0.0, res3x3);
gsl_matrix_set_zero(CovPose);
gsl_matrix_add(CovPose, Qk);
gsl_matrix_add(CovPose, res3x3);
}
static double update(gsl_matrix *v, gsl_matrix *w, gsl_matrix *h)
{
double dist = 0;
gsl_matrix *wt=NULL, *ht=NULL, *wh=NULL;
gsl_matrix *w_h=NULL, *wt_w=NULL;
gsl_matrix *wt_v = NULL;
gsl_matrix *v_ht=NULL, *wt_w_h=NULL, *w_h_ht=NULL;
wt = gsl_matrix_alloc(w->size2, w->size1);
gsl_matrix_transpose_memcpy(wt, w);
ht = gsl_matrix_alloc(h->size2, h->size1);
gsl_matrix_transpose_memcpy(ht, h);
// wt * v
wt_v = mm(wt, v);
// wt * w * h
wt_w = mm(wt, w);
wt_w_h = mm(wt_w, h);
gsl_matrix_free(wt_w);
// h = h.mul_elements(wt * v).div_elements(wt * w * h)
gsl_matrix_mul_elements(h, wt_v);
gsl_matrix_div_elements(h, wt_w_h);
gsl_matrix_free(wt_v);
gsl_matrix_free(wt_w_h);
// v * ht
v_ht = mm(v, ht);
// w * h * ht
w_h = mm(w, h);
w_h_ht = mm(w_h, ht);
gsl_matrix_free(w_h);
// w = w.mul_elements(v * ht).div_elements(w * h * ht)
gsl_matrix_mul_elements(w, v_ht);
gsl_matrix_div_elements(w, w_h_ht);
gsl_matrix_free(v_ht);
gsl_matrix_free(w_h_ht);
gsl_matrix_free(wt);
gsl_matrix_free(ht);
wh = mm(w, h);
dist = difcost(v, wh);
gsl_matrix_free(wh);
// w and h were modified in place
return dist;
}
Matrix*
Matrix::transMatrix()
{
Matrix* transMatrix=new Matrix(col,row);
gsl_matrix_transpose_memcpy(transMatrix->matrix,matrix);
return transMatrix;
}
val egsl_transpose(val v1){
gsl_matrix * m1 = egsl_gslm(v1);
val v2 = egsl_alloc(m1->size2,m1->size1);
gsl_matrix * m2 = egsl_gslm(v2);
gsl_matrix_transpose_memcpy(m2,m1);
return v2;
}
// time update (prediction)
bool ContinuousExtendedKalmanFilter::updateTime(const double time, const gsl_vector *input)
{
#if 0
if (!x_hat_ || /*!y_hat_ ||*/ !P_ || !K_) return false;
const gsl_vector *f_eval = system_.evaluatePlantEquation(time, x_hat_, input, NULL); // f = f(t, x(t), u(t), w(t))
const gsl_matrix *A = doGetStateTransitionMatrix(time, x_hat_); // A(t) = df(t, x(t), u(t), 0)/dx
#if 0
const gsl_matrix *W = doGetProcessNoiseCouplingMatrix(time); // W(t) = df(t, x(t), u(t), 0)/dw
const gsl_matrix *Q = doGetProcessNoiseCovarianceMatrix(time); // Q(t)
#else
const gsl_matrix *Qd = doGetProcessNoiseCovarianceMatrix(time); // Qd(t) = W * Q(t) * W(t)^T
#endif
if (!A || !Qd || !f_eval) return false;
// 1. Propagate time.
// dx(t)/dt = f(t, x(t), u(t), 0)
// dP(t)/dt = A(t) * P(t) + P(t) * A(t)^T + Qd(t) where A(t) = df(t, x(t), u(t), 0)/dx, Qd(t) = W(t) * Q(t) * W(t)^T, W(t) = df(t, x(t), u(t), 0)/dw
// Preserve symmetry of P.
gsl_matrix_transpose_memcpy(M_, P_);
gsl_matrix_add(P_, M_);
gsl_matrix_scale(P_, 0.5);
return true;
#else
throw std::runtime_error("Not yet implemented");
#endif
}
int
lls_lcurve(gsl_vector *reg_param, gsl_vector *rho, gsl_vector *eta,
lls_workspace *w)
{
const size_t N = rho->size; /* number of points on L-curve */
if (N != reg_param->size)
{
GSL_ERROR("size of reg_param and rho do not match", GSL_EBADLEN);
}
else if (N != eta->size)
{
GSL_ERROR("size of eta and rho do not match", GSL_EBADLEN);
}
else
{
int s;
double smax, smin;
size_t i;
/* compute eigenvalues of A^T W A */
gsl_matrix_transpose_memcpy(w->work_A, w->ATA);
s = gsl_eigen_symm(w->work_A, w->eval, w->eigen_p);
if (s)
return s;
/* find largest and smallest eigenvalues */
gsl_vector_minmax(w->eval, &smin, &smax);
/* singular values are square roots of eigenvalues */
smax = sqrt(smax);
if (smin > GSL_DBL_EPSILON)
smin = sqrt(fabs(smin));
gsl_multifit_linear_lreg(smin, smax, reg_param);
for (i = 0; i < N; ++i)
{
double r2;
double lambda = gsl_vector_get(reg_param, i);
lls_solve(lambda, w->c, w);
/* store ||c|| */
gsl_vector_set(eta, i, gsl_blas_dnrm2(w->c));
/* compute: A^T A c - 2 A^T y */
gsl_vector_memcpy(w->work_b, w->ATb);
gsl_blas_dsymv(CblasUpper, 1.0, w->ATA, w->c, -2.0, w->work_b);
/* compute: c^T A^T A c - 2 c^T A^T y */
gsl_blas_ddot(w->c, w->work_b, &r2);
r2 += w->bTb;
gsl_vector_set(rho, i, sqrt(r2));
}
return GSL_SUCCESS;
}
} /* lls_lcurve() */
static gsl_matrix *augmented_param_mat_A(gsl_matrix *R11, gsl_matrix *R12)
{
gsl_matrix *t_aug_A, *aug_A, *LU, *R11_inv;
gsl_permutation *p;
int signum;
p = gsl_permutation_alloc(R11->size2);
LU = gsl_matrix_alloc(R11->size1, R11->size2);
gsl_matrix_memcpy(LU, R11);
R11_inv = gsl_matrix_alloc(R11->size1, R11->size2);
gsl_linalg_LU_decomp(LU, p, &signum);
gsl_linalg_LU_invert(LU, p, R11_inv);
aug_A = gsl_matrix_alloc(R12->size1, R12->size2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, R11_inv, R12, 0.0, aug_A);
t_aug_A = gsl_matrix_alloc(aug_A->size2, aug_A->size1);
gsl_matrix_transpose_memcpy(t_aug_A, aug_A);
gsl_permutation_free(p);
gsl_matrix_free(LU);
gsl_matrix_free(aug_A);
gsl_matrix_free(R11_inv);
return t_aug_A;
}
int
gsl_multifit_linear_L_decomp (gsl_matrix * L, gsl_vector * tau)
{
const size_t m = L->size1;
const size_t p = L->size2;
int status;
if (tau->size != GSL_MIN(m, p))
{
GSL_ERROR("tau vector must be min(m,p)", GSL_EBADLEN);
}
else if (m >= p)
{
/* square or tall L matrix */
status = gsl_linalg_QR_decomp(L, tau);
return status;
}
else
{
/* more columns than rows, compute qr(L^T) */
gsl_matrix_view LTQR = gsl_matrix_view_array(L->data, p, m);
gsl_matrix *LT = gsl_matrix_alloc(p, m);
/* XXX: use temporary storage due to difficulties in transforming
* a rectangular matrix in-place */
gsl_matrix_transpose_memcpy(LT, L);
gsl_matrix_memcpy(<QR.matrix, LT);
gsl_matrix_free(LT);
status = gsl_linalg_QR_decomp(<QR.matrix, tau);
return status;
}
}
// time update (prediction)
bool ContinuousKalmanFilter::updateTime(const double time, const gsl_vector *Bu) // Bu(t) = B(t) * u(t)
{
#if 0
if (!x_hat_ || /*!y_hat_ ||*/ !P_ || !K_) return false;
const gsl_matrix *A = doGetSystemMatrix(time, x_hat_);
#if 0
const gsl_matrix *W = doGetProcessNoiseCouplingMatrix(time);
const gsl_matrix *Q = doGetProcessNoiseCovarianceMatrix(time); // Q(t)
#else
const gsl_matrix *Qd = doGetProcessNoiseCovarianceMatrix(time); // Qd(t) = W(t) * Q(t) * W(t)^T
#endif
//const gsl_vector *Bu = doGetControlInput(time, x_hat_); // Bu(t) = B(t) * u(t)
if (!A || !Qd || !Bu) return false;
// 1. propagate time
// dx(t)/dt = A(t) * x(t) + B(t) * u(t)
// dP(t)/dt = A(t) * P(t) + P(t) * A(t)^T + Qd where Qd(t) = W(t) * Q(t) * W(t)^T
// preserve symmetry of P
gsl_matrix_transpose_memcpy(M_, P_);
gsl_matrix_add(P_, M_);
gsl_matrix_scale(P_, 0.5);
return true;
#else
throw std::runtime_error("Not yet implemented");
#endif
}
std::vector<float> SgFilter::sg_filter(std::vector<float> x, int order, int deriv){
int xMid = x.size()/2;
int xSize = x.size();
//gsl to be used from here for calculating pseudo inverse
// matrix to be initialised
gsl_matrix * m = gsl_matrix_alloc (xSize, order + 1);
for(int i = 0 ; i < xSize ; i++){
float val = 1;
for(int j = 0 ; j < order + 1 ; j++){
gsl_matrix_set (m, i, j, val);
val *= (x[i] - x[xMid]);
}
}
// making the transpose matrix
gsl_matrix * a = gsl_matrix_alloc (order + 1, xSize);
gsl_matrix_transpose_memcpy (a, m);
//calculating pseudo inverse of a
gsl_matrix * b = pseudoInverse(a, order + 1, xSize);
std::vector<float> ans;
for(int i = 0 ; i < xSize ; i++)
ans.push_back(gsl_matrix_get(b, i, deriv));
return ans;
}
// Measurement update (correction).
bool ContinuousExtendedKalmanFilter::updateMeasurement(const double time, const gsl_vector *actualMeasurement, const gsl_vector *input)
{
#if 0
if (!x_hat_ || /*!y_hat_ ||*/ !P_ || !K_) return false;
const gsl_vector *h_eval = system_.evaluateMeasurementEquation(step, x_hat_, input, NULL); // h = h(t, x(t), u(t), v(t))
const gsl_matrix *C = doGetOutputMatrix(time, x_hat_); // C(t) = dh(t, x(t), u(t), 0)/dx
#if 0
const gsl_matrix *V = doGetMeasurementNoiseCouplingMatrix(time); // V(t) = dh(t, x(t), u(t), 0)/dv
const gsl_matrix *R = doGetMeasurementNoiseCovarianceMatrix(time); // R(t)
#else
const gsl_matrix *Rd = doGetMeasurementNoiseCovarianceMatrix(time); // Rd(t) = V(t) * R(t) * V(t)^T
#endif
if (!C || !Rd || !h_eval || !actualMeasurement) return false;
// 1. calculate Kalman gain: K(t) = P(t) * C(t)^T * Rd(t)^-1 where C(t) = dh(t, x(t), u(t), 0)/dx, Rd(t) = V(t) * R(t) * V(t)^T, V(t) = dh(t, x(t), u(t), 0)/dv
// 2. update measurement: dx(t)/dt = f(t, x(t), u(t), 0) + K(t) * (y_tilde(t) - y_hat(t)) where y_hat(t) = h(t, x(t), u(t), 0) ???
// 3. update covariance:
// dP(t)/dt = A(t) * P(t) + P(t) * A(t)^T + Qd(t) - P(t) * C(t)^T * Rd(t)^-1 * C(t) * P(t) : the matrix Riccati differential equation
// = A(t) * P(t) + P(t) * A(t)^T + Qd(t) - K(t) * Rd(t) * K(t)^T
// preserve symmetry of P
gsl_matrix_transpose_memcpy(M_, P_);
gsl_matrix_add(P_, M_);
gsl_matrix_scale(P_, 0.5);
return true;
#else
throw std::runtime_error("Not yet implemented");
#endif
}
// measurement update (correction)
bool ContinuousKalmanFilter::updateMeasurement(const double time, const gsl_vector *actualMeasurement, const gsl_vector *Du) // Du(t) = D(t) * u(t)
{
#if 0
if (!x_hat_ || /*!y_hat_ ||*/ !P_ || !K_) return false;
const gsl_matrix *C = doGetOutputMatrix(time, x_hat_);
#if 0
const gsl_matrix *V = doGetMeasurementNoiseCouplingMatrix(time);
const gsl_matrix *R = doGetMeasurementNoiseCovarianceMatrix(time); // R(t)
#else
const gsl_matrix *Rd = doGetMeasurementNoiseCovarianceMatrix(time); // Rd(t) = V(t) * R(t) * V(t)^T
#endif
const gsl_vector *Du = doGetMeasurementInput(time, x_hat_); // Du(t) = D(t) * u(t)
const gsl_vector *actualMeasurement = doGetMeasurement(time, x_hat_); // actual measurement
if (!C || !Rd || !Du || !actualMeasurement) return false;
// 1. calculate Kalman gain: K(t) = P(t) * C(t)^T * Rd(t)^-1 where Rd(t) = V(t) * R(t) * V(t)^T
// 2. update measurement: dx(t)/dt = A(t) * x(t) + B(t) * u(t) + K(t) * (y_tilde(t) - y_hat(t)) where y_hat(t) = C(t) * x(t) + D(t) * u(t)
// 3. update covariance:
// dP(t)/dt = A(t) * P(t) + P(t) * A(t)^T + Qd(t) - P(t) * C(t)^T * Rd(t)^-1 * C(t) * P(t) : the matrix Riccati differential equation
// = A(t) * P(t) + P(t) * A(t)^T + Qd(t) - K(t) * Rd(t) * K(t)^T
// preserve symmetry of P
gsl_matrix_transpose_memcpy(M_, P_);
gsl_matrix_add(P_, M_);
gsl_matrix_scale(P_, 0.5);
return true;
#else
throw std::runtime_error("Not yet implemented");
#endif
}
void pca_whiten(
gsl_matrix *input,// NOBS x NVOX
size_t const NCOMP, //
gsl_matrix *x_white, // NCOMP x NVOX
gsl_matrix *white, // NCOMP x NSUB
gsl_matrix *dewhite, //NSUB x NCOMP
int demean){
// get input reference
size_t NSUB = input->size1;
// demean input matrix
if (demean){
matrix_demean(input);
}
// Convariance Matrix
gsl_matrix *cov = gsl_matrix_alloc(NSUB, NSUB);
matrix_cov(input, cov);
// Set up eigen decomposition
gsl_vector *eval = gsl_vector_alloc(NCOMP); //eigen values
gsl_matrix *evec = gsl_matrix_alloc(NSUB, NCOMP);
rr_eig(cov, eval, evec, NCOMP );
//Computing whitening matrix
gsl_matrix_transpose_memcpy(white, evec);
gsl_vector_view v;
double e;
size_t i;
// white = eval^{-1/2} evec^T
#pragma omp parallel for private(i,e,v)
for (i = 0; i < NCOMP; i++) {
e = gsl_vector_get(eval,i);
v = gsl_matrix_row(white,i);
gsl_blas_dscal(1/sqrt(e), &v.vector);
}
// Computing dewhitening matrix
gsl_matrix_memcpy(dewhite, evec);
// dewhite = evec eval^{1/2}
#pragma omp parallel for private(i,e,v)
for (i = 0; i < NCOMP; i++) {
e = gsl_vector_get(eval,i);
v = gsl_matrix_column(dewhite,i);
gsl_blas_dscal(sqrt(e), &v.vector);
}
// whitening data (white x Input)
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,
white, input, 0.0, x_white);
gsl_matrix_free(cov);
gsl_matrix_free(evec);
gsl_vector_free(eval);
}
/** Transpose matrix */
matrix<double> matrix<double>::transpose()
{
matrix<double> m1(size_j(), size_i(), 0.);
if (gsl_matrix_transpose_memcpy(m1.as_gsl_type_ptr(), _matrix))
{
std::cout << "\n Error in matrix<double> transpose" << std::endl;
exit(EXIT_FAILURE);
}
return m1;
}
/*! \brief Discrete Cepstrum Transform
*
* method for computing cepstrum aenalysis from a discrete
* set of partial peaks (frequency and amplitude)
*
* This implementation is owed to the help of Jordi Janer (thanks!) from the MTG,
* along with the following paper:
* "Regularization Techniques for Discrete Cepstrum Estimation"
* Olivier Cappe and Eric Moulines, IEEE Signal Processing Letters, Vol. 3
* No.4, April 1996
*
* \todo add anchor point add at frequency = 0 with the same magnitude as the first
* peak in pMag. This does not change the size of the cepstrum, only helps to smoothen it
* at the very beginning.
*
* \param sizeCepstrum order+1 of the discrete cepstrum
* \param pCepstrum pointer to output array of cepstrum coefficients
* \param sizeFreq number of partials peaks (the size of pFreq should be the same as pMag
* \param pFreq pointer to partial peak frequencies (hertz)
* \param pMag pointer to partial peak magnitudes (linear)
* \param fLambda regularization factor
* \param iMaxFreq maximum frequency of cepstrum
*/
void sms_dCepstrum( int sizeCepstrum, sfloat *pCepstrum, int sizeFreq, sfloat *pFreq, sfloat *pMag,
sfloat fLambda, int iMaxFreq)
{
int i, k;
sfloat factor;
sfloat fNorm = PI / (float)iMaxFreq; /* value to normalize frequencies to 0:0.5 */
//static sizeCepstrumStatic
static CepstrumMatrices m;
//printf("nPoints: %d, nCoeff: %d \n", m.nPoints, m.nCoeff);
if(m.nPoints != sizeCepstrum || m.nCoeff != sizeFreq)
AllocateDCepstrum(sizeFreq, sizeCepstrum, &m);
int s; /* signum: "(-1)^n, where n is the number of interchanges in the permutation." */
/* compute matrix M (eq. 4)*/
for (i=0; i<sizeFreq; i++)
{
gsl_matrix_set (m.pM, i, 0, 1.); // first colum is all 1
for (k=1; k <sizeCepstrum; k++)
gsl_matrix_set (m.pM, i, k , 2.*sms_sine(PI_2 + fNorm * k * pFreq[i]) );
}
/* compute transpose of M */
gsl_matrix_transpose_memcpy (m.pMt, m.pM);
/* compute R diagonal matrix (for eq. 7)*/
factor = COEF * (fLambda / (1.-fLambda)); /* \todo why is this divided like this again? */
for (k=0; k<sizeCepstrum; k++)
gsl_matrix_set(m.pR, k, k, factor * powf((sfloat) k,2.));
/* MtM = Mt * M, later will add R */
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1., m.pMt, m.pM, 0.0, m.pMtMR);
/* add R to make MtMR */
gsl_matrix_add (m.pMtMR, m.pR);
/* set pMag in X and multiply with Mt to get pMtXk */
for(k = 0; k <sizeFreq; k++)
gsl_vector_set(m.pXk, k, log(pMag[k]));
gsl_blas_dgemv (CblasNoTrans, 1., m.pMt, m.pXk, 0., m.pMtXk);
/* solve x (the cepstrum) in Ax = b, where A=MtMR and b=pMtXk */
/* ==== the Cholesky Decomposition way ==== */
/* MtM is 'symmetric and positive definite?' */
//gsl_linalg_cholesky_decomp (m.pMtMR);
//gsl_linalg_cholesky_solve (m.pMtMR, m.pMtXk, m.pC);
/* ==== the LU decomposition way ==== */
gsl_linalg_LU_decomp (m.pMtMR, m.pPerm, &s);
gsl_linalg_LU_solve (m.pMtMR, m.pPerm, m.pMtXk, m.pC);
/* copy pC to pCepstrum */
for(i = 0; i < sizeCepstrum; i++)
pCepstrum[i] = gsl_vector_get (m.pC, i);
}
gsl_matrix* transpose(gsl_matrix *m,int columnas,int filas){
gsl_matrix *transpuesta = gsl_matrix_alloc (columnas, filas);
gsl_matrix_transpose_memcpy(transpuesta,m);
return transpuesta;
}
// time update (prediction)
bool DiscreteKalmanFilter::updateTime(const size_t step, const gsl_vector *Bu) // Bu(k) = Bd(k) * u(k)
{
if (!x_hat_ || /*!y_hat_ ||*/ !P_ || !K_) return false;
const gsl_matrix *Phi = system_.getStateTransitionMatrix(step, x_hat_); // Phi(k) = exp(A * T)
#if 0
const gsl_matrix *W = system_.getProcessNoiseCouplingMatrix(step);
const gsl_matrix *Q = system_.getProcessNoiseCovarianceMatrix(step); // Q(k)
#else
const gsl_matrix *Qd = system_.getProcessNoiseCovarianceMatrix(step); // Qd(k) = W(k) * Q(k) * W(k)^T
#endif
//const gsl_vector *Bu = system_.getControlInput(step, x_hat_); // Bu(k) = Bd(k) * u(k)
if (!Phi || !Qd || !Bu) return false;
// 1. propagate time
// x-(k+1) = Phi(k) * x(k) + Bd(k) * u(k)
gsl_vector_memcpy(v_, x_hat_);
gsl_vector_memcpy(x_hat_, Bu);
if (GSL_SUCCESS != gsl_blas_dgemv(CblasNoTrans, 1.0, Phi, v_, 1.0, x_hat_))
return false;
// P-(k+1) = Phi(k) * P(k) * Phi(k)^T + Qd(k) where Qd(k) = W(k) * Q(k) * W(k)^T
#if 0
// using Q
if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Phi, P_, 0.0, M_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, M_, Phi, 0.0, M_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, W, Qd, 0.0, M2_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, M2_, W, 0.0, P_) ||
GSL_SUCCESS != gsl_matrix_add(P_, M_))
return false;
#else
// using Qd
if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Phi, P_, 0.0, M_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, M_, Phi, 0.0, P_) ||
GSL_SUCCESS != gsl_matrix_add(P_, Qd))
return false;
#endif
// preserve symmetry of P
gsl_matrix_transpose_memcpy(M_, P_);
gsl_matrix_add(P_, M_);
gsl_matrix_scale(P_, 0.5);
return true;
}
/*
// set the point coordinates
// Xl = Xw - Xp
// Yl = Yw - Yp
// point gradient
// derive point transformation equations in order of robot pose
// [ dXl/dXp dXl/dYp dXl/dTheta_p ] = [1 0 0 ] = GradPose
// [ dYl/dXp dYl/dYp dYl/dTheta_p ] [ 0 1 0 ]
*/
bool point_t::toGlobalCordinates(gsl_matrix * PoseMean, gsl_matrix * Covariance)
{
x = -x + gsl_matrix_get(PoseMean, 0, 0); // x coordinate
y = -y + gsl_matrix_get(PoseMean, 1, 0); // y coordinate
// RpL = RpW + GradPose * Pose * GradPose_T
gsl_matrix * GradPose = gsl_matrix_alloc(2, 3);
gsl_matrix * GradPose_T = gsl_matrix_alloc(3, 2);
gsl_matrix * mat2x3 = gsl_matrix_alloc(2, 3);
gsl_matrix * mat2x2 = gsl_matrix_alloc(2, 2);
gsl_matrix_set(GradPose, 0, 0, 1);
gsl_matrix_set(GradPose, 0, 1, 0);
gsl_matrix_set(GradPose, 0, 2, 0);
gsl_matrix_set(GradPose, 1, 0, 1);
gsl_matrix_set(GradPose, 1, 1, 1);
gsl_matrix_set(GradPose, 1, 2, 0);
gsl_matrix_transpose_memcpy(GradPose_T, GradPose);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, GradPose, Covariance, 0.0,
mat2x3);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, mat2x3, GradPose_T, 0.0,
mat2x2);
// UPDATE POINT GRADIENT
Cp[0][0] = gsl_matrix_get(mat2x2, 0, 0) + Cp[0][0];
Cp[1][1] = gsl_matrix_get(mat2x2, 1, 1) + Cp[1][1];
GradX[0][0] = gsl_matrix_get(GradPose,0,0);
GradX[0][1] = gsl_matrix_get(GradPose,0,1);
GradX[0][2] = gsl_matrix_get(GradPose,0,2);
GradX[1][0] = gsl_matrix_get(GradPose,1,0);
GradX[1][1] = gsl_matrix_get(GradPose,1,1);
GradX[1][2] = gsl_matrix_get(GradPose,1,2);
gsl_matrix_free(GradPose_T);
gsl_matrix_free(GradPose);
gsl_matrix_free(mat2x3);
gsl_matrix_free(mat2x2);
}
// time update (prediction)
bool DiscreteExtendedKalmanFilter::updateTime(const size_t step, const gsl_vector *input)
{
if (!x_hat_ || /*!y_hat_ ||*/ !P_ || !K_) return false;
const gsl_vector *f_eval = system_.evaluatePlantEquation(step, x_hat_, input, NULL); // f = f(k, x(k), u(k), 0)
const gsl_matrix *Phi = system_.getStateTransitionMatrix(step, x_hat_); // Phi(k) = exp(A(k) * T) where A(k) = df(k, x(k), u(k), 0)/dx
#if 0
const gsl_matrix *W = system_.getProcessNoiseCouplingMatrix(step); // W(k) = df(k, x(k), u(k), 0)/dw
const gsl_matrix *Q = system_.getProcessNoiseCovarianceMatrix(step); // Q(k)
#else
const gsl_matrix *Qd = system_.getProcessNoiseCovarianceMatrix(step); // Qd(k) = W * Q(k) * W(k)^T
#endif
if (!Phi || !Qd || !f_eval) return false;
// 1. propagate time
// x-(k+1) = f(k, x(k), u(k), 0)
gsl_vector_memcpy(x_hat_, f_eval);
// P-(k+1) = Phi(k) * P(k) * Phi(k)^T + Qd(k) where Phi(k) = exp(A * T), A = df(k, x(k), u(k), 0)/dx, Qd(k) = W(k) * Q(k) * W(k)^T, W(k) = df(k, x(k), u(k), 0)/dw
#if 0
// using Q
if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Phi, P_, 0.0, M_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, M_, Phi, 0.0, M_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, W, Qd, 0.0, M2_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, M2_, W, 0.0, P_) ||
GSL_SUCCESS != gsl_matrix_add(P_, M_))
return false;
#else
// using Qd
if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Phi, P_, 0.0, M_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, M_, Phi, 0.0, P_) ||
GSL_SUCCESS != gsl_matrix_add(P_, Qd))
return false;
#endif
// preserve symmetry of P
gsl_matrix_transpose_memcpy(M_, P_);
gsl_matrix_add(P_, M_);
gsl_matrix_scale(P_, 0.5);
return true;
}
/* SVD of any matrix */
int svdAnyMat(gsl_matrix * X,
gsl_matrix * U,
gsl_matrix * V,
gsl_vector * D)
{
// SVD part
gsl_vector * work;
int n = X->size1;
int p = X->size2;
if(p > n)
{
work = gsl_vector_alloc(n);
gsl_matrix * tmpV = gsl_matrix_alloc(n, n);
gsl_matrix * tmpU = gsl_matrix_alloc(p, n);
// Transpose the matrix X
gsl_matrix_transpose_memcpy(tmpU, X);
// TmpU contians t(X)
// There is linear dependence in X
// Need to replace the NaNs with zeros in the matrix
// Or something
gsl_linalg_SV_decomp(tmpU, tmpV, D, work);
gsl_vector_free(work);
// Swap back
gsl_matrix * tmp1 = gsl_matrix_alloc(tmpU->size1, tmpU->size2);
gsl_matrix * tmp2 = gsl_matrix_alloc(tmpV->size1, tmpV->size2);
gsl_matrix_memcpy(tmp1, tmpU);
gsl_matrix_memcpy(tmp2, tmpV);
gsl_matrix_free(tmpU);
gsl_matrix_free(tmpV);
gsl_matrix_memcpy(V, tmp1);
gsl_matrix_memcpy(U, tmp2);
gsl_matrix_free(tmp1);
gsl_matrix_free(tmp2);
} else {
work = gsl_vector_alloc(p);
gsl_matrix_memcpy(U, X);
gsl_linalg_SV_decomp(U, V, D, work);
gsl_vector_free(work);
}
return 0;
}
gsl_matrix* TensorToGSLMatrix(const Tensor<2> &tensor) {
size_t i, j;
int M, N;
M = (int) tensor.sizes[0];
N = (int) tensor.sizes[1];
gsl_matrix* matrix = gsl_matrix_alloc(M, N);
for (i = 0; i < tensor.sizes[0]; ++i) {
for (j = 0; j < tensor.sizes[1]; ++j) {
gsl_matrix_set(matrix, i, j, tensor.data[i*tensor.sizes[1] + j]);
}
}
if (M < N) {
gsl_matrix* matrix_t = gsl_matrix_alloc(N, M);
gsl_matrix_transpose_memcpy (matrix_t, matrix);
return matrix_t;
} else {
return matrix;
}
}
bool point_t::toLocalCordinates(gsl_matrix * PoseMean, gsl_matrix * Covariance)
{
// set the point coordinates
// Xl = Xw - Xp
// Yl = Yw - Yp
x = x - gsl_matrix_get(PoseMean, 0, 0); // x coordinate
y = y - gsl_matrix_get(PoseMean, 1, 0); // y coordinate
// point end gradient
// derive point transformation equations in order of robot pose
// [ dXl/dXp dXl/dYp dXl/dTheta_p ] = [-1 0 0 ] = GradPose
// [ dYl/dXp dYl/dYp dYl/dTheta_p ] [ 0 -1 0 ]
//
// RpL = RpW + GradPose * Pose * GradPose_T
gsl_matrix * GradPose = gsl_matrix_alloc(2, 2);
gsl_matrix * GradPose_T = gsl_matrix_alloc(2, 2);
gsl_matrix * mat2x3 = gsl_matrix_alloc(2, 3);
gsl_matrix * mat2x2 = gsl_matrix_alloc(2, 2);
gsl_matrix_set(GradPose, 0, 0, -1);
gsl_matrix_set(GradPose, 1, 1, -1);
gsl_matrix_transpose_memcpy(GradPose_T, GradPose);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, GradPose, Covariance, 0.0,
mat2x3);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, mat2x3, GradPose_T, 0.0,
mat2x2);
Cp[0][0] = gsl_matrix_get(mat2x2, 0, 0) + Cp[0][0];
Cp[1][1] = gsl_matrix_get(mat2x2, 1, 1) + Cp[1][1];
gsl_matrix_free(GradPose_T);
gsl_matrix_free(GradPose);
gsl_matrix_free(mat2x3);
gsl_matrix_free(mat2x2);
}
// measurement update (correction)
bool DiscreteKalmanFilter::updateMeasurement(const size_t step, const gsl_vector *actualMeasurement, const gsl_vector *Du) // Du(k) = Dd(k) * u(k)
{
if (!x_hat_ || /*!y_hat_ ||*/ !P_ || !K_) return false;
const gsl_matrix *Cd = system_.getOutputMatrix(step, x_hat_);
#if 0
const gsl_matrix *V = system_.getMeasurementNoiseCouplingMatrix(step);
const gsl_matrix *R = system_.getMeasurementNoiseCovarianceMatrix(step); // R(k)
#else
const gsl_matrix *Rd = system_.getMeasurementNoiseCovarianceMatrix(step); // Rd(k) = V(k) * R(k) * V(k)^T
#endif
//const gsl_vector *Du = system_.getMeasurementInput(step, x_hat_); // Du(k) = Dd(k) * u(k)
//const gsl_vector *actualMeasurement = system_.getMeasurement(step, x_hat_); // actual measurement
if (!Cd || !Rd || !Du || !actualMeasurement) return false;
// 1. calculate Kalman gain: K(k) = P-(k) * Cd(k)^T * (Cd(k) * P-(k) * Cd(k)^T + Rd(k))^-1 where Rd(k) = V(k) * R(k) * V(k)^T
// inverse of matrix using LU decomposition
gsl_matrix_memcpy(RR_, Rd);
if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, P_, Cd, 0.0, PCt_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Cd, PCt_, 1.0, RR_))
return false;
int signum;
if (GSL_SUCCESS != gsl_linalg_LU_decomp(RR_, permutation_, &signum) ||
GSL_SUCCESS != gsl_linalg_LU_invert(RR_, permutation_, invRR_))
return false;
if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, PCt_, invRR_, 0.0, K_)) // calculate Kalman gain
return false;
// 2. update measurement: x(k) = x-(k) + K(k) * (y_tilde(k) - y_hat(k)) where y_hat(k) = Cd(k) * x-(k) + Dd(k) * u(k)
#if 0
// save an estimated measurement, y_hat
gsl_vector_memcpy(y_hat_, Du);
if (GSL_SUCCESS != gsl_blas_dgemv(CblasNoTrans, 1.0, Cd, x_hat_, 1.0, y_hat_)) // calcuate y_hat(k)
return false;
gsl_vector_memcpy(residual_, y_hat_);
if (GSL_SUCCESS != gsl_vector_sub(residual_, actualMeasurement) || // calculate residual = y_tilde(k) - y_hat(k)
GSL_SUCCESS != gsl_blas_dgemv(CblasNoTrans, -1.0, K_, residual_, 1.0, x_hat_)) // calculate x_hat(k)
return false;
#else
gsl_vector_memcpy(residual_, Du);
if (GSL_SUCCESS != gsl_blas_dgemv(CblasNoTrans, 1.0, Cd, x_hat_, 1.0, residual_) || // calcuate y_hat(k)
GSL_SUCCESS != gsl_vector_sub(residual_, actualMeasurement) || // calculate residual = y_tilde(k) - y_hat(k)
GSL_SUCCESS != gsl_blas_dgemv(CblasNoTrans, -1.0, K_, residual_, 1.0, x_hat_)) // calculate x_hat(k)
return false;
#endif
// 3. update covariance: P(k) = (I - K(k) * Cd(k)) * P-(k)
#if 0
// not working
gsl_matrix_set_identity(M_);
if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, K_, Cd, 1.0, M_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_, P_, 0.0, P_))
return false;
#else
gsl_matrix_set_identity(M_);
if (GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, K_, Cd, 1.0, M_) ||
GSL_SUCCESS != gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_, P_, 0.0, M2_))
return false;
gsl_matrix_memcpy(P_, M2_);
#endif
// preserve symmetry of P
gsl_matrix_transpose_memcpy(M_, P_);
gsl_matrix_add(P_, M_);
gsl_matrix_scale(P_, 0.5);
return true;
}
int main(int argc, char **argv){
int row = atoi(argv[2]);
int col = atoi(argv[3]);
printf("%d %d\n", row, col);
gsl_matrix* data = gsl_matrix_alloc(row, col);
//gsl_matrix* data = gsl_matrix_alloc(col, row);
FILE* f = fopen(argv[1], "r");
gsl_matrix_fscanf(f, data);
//gsl_matrix_fread(f, data);
//gsl_matrix_transpose_memcpy(data, data_raw);
fclose(f);
//printf("%f %f", gsl_matrix_get(data,0,0), gsl_matrix_get(data,0,1));
//f = fopen("test.dat", "w");
//gsl_matrix_fprintf(f, data, "%f");
//fclose(f);
// data centering, subtract the mean in each dimension (col.-wise)
int i, j;
double mean, sum, std;
gsl_vector_view col_vector;
for (i = 0; i < col; ++i){
col_vector = gsl_matrix_column(data, i);
mean = gsl_stats_mean((&col_vector.vector)->data, 1,
(&col_vector.vector)->size);
gsl_vector_add_constant(&col_vector.vector, -mean);
gsl_matrix_set_col(data, i, &col_vector.vector);
}
char filename[50];
//sprintf(filename, "%s.zscore", argv[1]);
//print2file(filename, data);
gsl_matrix* u;
if (col > row) {
u = gsl_matrix_alloc(data->size2, data->size1);
gsl_matrix_transpose_memcpy(u, data);
}
else {
u = gsl_matrix_alloc(data->size1, data->size2);
gsl_matrix_memcpy(u, data);
}
// svd
gsl_matrix* X = gsl_matrix_alloc(col, col);
gsl_matrix* V = gsl_matrix_alloc(u->size2, u->size2);
gsl_vector* S = gsl_vector_alloc(u->size2);
gsl_vector* work = gsl_vector_alloc(u->size2);
gsl_linalg_SV_decomp(u, V, S, work);
//gsl_linalg_SV_decomp_jacobi(u, V, S);
// mode coefficient
//print2file("u.dat", u);
/*
// characteristic mode
gsl_matrix* diag = diag_alloc(S);
gsl_matrix* mode = gsl_matrix_alloc(diag->size1, V->size1);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, diag, V, 0.0, mode);
gsl_matrix_transpose(mode);
print2file("mode.dat", mode);
gsl_matrix_transpose(mode);
*/
// reconstruction
gsl_matrix *recons = gsl_matrix_alloc(u->size2, data->size1);
if (col > row) {
gsl_matrix_view data_sub = gsl_matrix_submatrix(data, 0, 0,
u->size2, u->size2);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, V,
&data_sub.matrix, 0.0, recons);
}
else
gsl_blas_dgemm(CblasTrans, CblasTrans, 1.0, V, data, 0.0, recons);
//gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, u, mode, 0.0,
// recons);
gsl_matrix *recons_trans = gsl_matrix_alloc(recons->size2,
recons->size1);
gsl_matrix_transpose_memcpy(recons_trans, recons);
// take the first two eigenvectors
gsl_matrix_view final = gsl_matrix_submatrix(recons_trans, 0, 0,
recons_trans->size1, 2);
print2file(argv[4], &final.matrix);
// eigenvalue
gsl_vector_mul(S, S);
f = fopen("eigenvalue.dat", "w");
//gsl_vector_fprintf(f, S, "%f");
fclose(f);
gsl_matrix_free(data);
gsl_matrix_free(X);
gsl_matrix_free(V);
//gsl_matrix_free(diag);
//gsl_matrix_free(mode);
gsl_matrix_free(recons);
gsl_matrix_free(recons_trans);
gsl_matrix_free(u);
gsl_vector_free(S);
gsl_vector_free(work);
//gsl_vector_free(zero);
//gsl_vector_free(corrcoef);
//gsl_vector_free(corrcoef_mean);
return 0;
}
void sci_2_gsl_matrix (gsl_matrix *m,const double * sci_matrix, int nr, int nc){
gsl_matrix_view mT=gsl_matrix_view_array ((double *)&(sci_matrix[1]), nc,nr);
gsl_matrix_transpose_memcpy(m,&mT.matrix);
}
bool Matrix_Transpose(gsl_matrix *A, gsl_matrix *B){
gsl_matrix_transpose_memcpy(A,B);
return true;
}
PhiStructure::PhiDGamma::PhiDGamma( const PhiStructure *s, size_t d ) :
myStruct(s), myParent(s->myPStruct->createDGamma(d)) {
myPhi = gsl_matrix_alloc(myStruct->myPhiT->size2, myStruct->myPhiT->size1);
gsl_matrix_transpose_memcpy(myPhi, myStruct->myPhiT);
}
int SolveSVD (double a[], double b[], double x[], int neq, int nvar)
{
// get A
gsl_matrix_view av = gsl_matrix_view_array (a, neq, nvar);
if (neq < nvar) { // M < N ... do the transposed matrix
gsl_matrix *atrans = gsl_matrix_alloc (nvar, neq);
gsl_matrix_transpose_memcpy (atrans, &av.matrix);
gsl_matrix *v = gsl_matrix_alloc (neq, neq);
gsl_vector *s = gsl_vector_alloc (neq);
gsl_vector *work = gsl_vector_alloc (neq);
gsl_linalg_SV_decomp (atrans, v, s, work);
// x = A+ b
gsl_matrix *splus = gsl_matrix_calloc (neq, neq);
// compute sigma_plus
for (int i = 0; i < neq; i++) {
double sigma;
if ((sigma = gsl_vector_get (s,i)) != 0.0)
gsl_matrix_set (splus, i,i, 1.0/sigma);
}
gsl_linalg_matmult (atrans, splus, atrans);
gsl_linalg_matmult_mod (atrans, GSL_LINALG_MOD_NONE, v, GSL_LINALG_MOD_TRANSPOSE, atrans);
gsl_vector_view bv = gsl_vector_view_array (b, neq);
gsl_matrix_view bmv = gsl_matrix_view_vector (&bv.vector, neq, 1);
gsl_matrix *xx = gsl_matrix_alloc (nvar,1);
gsl_linalg_matmult (atrans, &bmv.matrix, xx);
// gsl_matrix_fprintf (stdout, xx, "%g");
for (int i = 0; i < nvar; i++) {
x[i] = gsl_matrix_get(xx,i,0);
}
gsl_matrix_free (splus); gsl_matrix_free (xx);
gsl_matrix_free (atrans);
gsl_matrix_free (v); gsl_vector_free (s); gsl_vector_free (work);
} else { // M >= N
gsl_matrix *v = gsl_matrix_alloc (nvar, nvar);
gsl_vector *s = gsl_vector_alloc (nvar);
gsl_vector *work = gsl_vector_alloc (nvar);
gsl_linalg_SV_decomp (&av.matrix, v, s, work);
// x = A+ b
gsl_vector_view bv = gsl_vector_view_array (b, neq);
gsl_vector *xx = gsl_vector_alloc (nvar);
gsl_linalg_SV_solve (&av.matrix, v, s, &bv.vector, xx);
// gsl_vector_fprintf (stdout, xx, "%g");
for (int i = 0; i < nvar; i++)
x[i] = gsl_vector_get (xx, i);
gsl_vector_free (xx);
gsl_matrix_free (v); gsl_vector_free (s); gsl_vector_free (work);
}
return 1;
}
matrix transpose(const matrix& m) {
matrix t(m.ptr_->size2, m.ptr_->size1);
gsl_matrix_transpose_memcpy(t.ptr_, m.ptr_);
return t;
}