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