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); }
void momentum_update(par* p, gsl_vector** biases, gsl_matrix** weights, double step) { double mu = p->contract_momentum; for (int i = 0; i < p->num_layers-1; i++){ //gsl_vector_scale(p->biases_momentum[i], mu); //gsl_blas_daxpy(step, biases[i], p->biases_momentum[i]); gsl_vector_add(p->biases[i], p->biases_momentum[i]); gsl_matrix_scale(p->weights_momentum[i], mu); gsl_matrix_scale(weights[i],step); gsl_matrix_add(p->weights_momentum[i],weights[i]); gsl_matrix_add(p->weights[i], p->weights_momentum[i]); } }
gsl_matrix* GaussianMatrix ( gsl_vector *v, float sigma ) { gsl_vector_add_constant ( v, -1 ); gsl_vector_scale ( v, 0.5 ); int siz1 = gsl_vector_get ( v, 0 ); int siz2 = gsl_vector_get ( v, 1 ); gsl_matrix *x = gsl_matrix_alloc ( siz1*2+1, siz2*2+1 ); gsl_matrix *y = gsl_matrix_alloc ( siz1*2+1, siz2*2+1 ); for ( int i=-siz2; i<=siz2; i++ ) { for ( int j=-siz1; j<=siz1; j++ ) { gsl_matrix_set ( x, i+siz2, j+siz1, j ); gsl_matrix_set ( y, i+siz2, j+siz1, i ); } } gsl_matrix_mul_elements ( x, x ); gsl_matrix_mul_elements ( y, y ); gsl_matrix_add ( x, y ); gsl_matrix_scale ( x, -1/(2*sigma*sigma) ); float sum = 0; for ( int i=0; i<x->size1; i++ ) { for ( int j=0; j<x->size2; j++ ) { gsl_matrix_set ( x, i, j, exp(gsl_matrix_get ( x, i, j )) ); sum += gsl_matrix_get ( x, i, j ); } } if ( sum != 0 ) gsl_matrix_scale ( x, 1/sum ); gsl_matrix_free ( y ); return x; }
static REAL8 fContact (REAL8 x, void *params) { fContactWorkSpace *p = (fContactWorkSpace *)params; gsl_permutation *p1 = p->p1; gsl_vector *tmpV = p->tmpV; gsl_matrix *C = p->C; gsl_matrix *A = p->tmpA; gsl_matrix *B = p->tmpB; INT4 s1; REAL8 result; gsl_matrix_memcpy ( A, p->invQ1); gsl_matrix_memcpy ( B, p->invQ2); gsl_matrix_scale (B, x); gsl_matrix_scale (A, (1.0L-x)); gsl_matrix_add (A, B); gsl_linalg_LU_decomp( A, p1, &s1 ); gsl_linalg_LU_invert( A, p1, C ); /* Evaluate the product C x r_AB */ gsl_blas_dsymv (CblasUpper, 1.0, C, p->r_AB, 0.0, tmpV); /* Now evaluate transpose(r_AB) x (C x r_AB) */ gsl_blas_ddot (p->r_AB, tmpV, &result); result *= (x*(1.0L-x)); return (-result); }
int main(void) { // test bias objects initiation and ranodmization test_bias_object(false); test_bias_object(true); // test sigmoid test_sigmoid(); // test weights objects initiation and randomization test_weight_object(false); test_weight_object(true); // test Neural Nets forward sweep test_forward_propagation(); const gsl_rng_type* T; gsl_rng* r; gsl_rng_env_setup(); T = gsl_rng_default; r = gsl_rng_alloc(T); gsl_matrix* a, * b; a = gsl_matrix_alloc(3,3); b = gsl_matrix_alloc(3,3); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++){ gsl_matrix_set(a,i,j,i+j); gsl_matrix_set(b,i,j,i+j); } gsl_matrix_scale(b,10.0); gsl_matrix_add(a,b); for (int i = 0; i < 3; i++) for (int j = 0; j <3; j++) printf("m(%d,%d) = %g\n",i,j, gsl_matrix_get(a,i,j)); gsl_matrix_free(a); gsl_matrix_free(b); }
void CNumMat::operator+=(const CNumMat &m) { assert(NumCols()==m.NumCols()); assert(NumRows()==m.NumRows()); if(gsl_matrix_add(m_mat,m.m_mat)) throw BPException("gsl_matrix_add"); }
// 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 }
// 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 }
// 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 }
// 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 }
void kalmanFilterPosition0(double latitude, double longitude, gsl_matrix *x, gsl_matrix *P) { /* Wektor pomiarów */ gsl_matrix *z = gsl_matrix_alloc(2, 1); gsl_matrix_set(z, 0, 0, latitude); gsl_matrix_set(z, 1, 0, longitude); /* Macierz przejœcia ze stanu do pomiaru */ gsl_matrix *H = gsl_matrix_alloc(2,2); gsl_matrix_set_identity(H); /* Macierz kowariancji szumu procesu */ gsl_matrix *Q = gsl_matrix_alloc(2,2); gsl_matrix_set_identity(Q); double q = 0.00001; gsl_matrix_scale(Q, q); /* Macierz kowariancji szumu pomiaru */ double wrong_latitude = pow(WRONG_LATI, 2); double wrong_longitude = pow(WRONG_LONGI,2); gsl_matrix *R = gsl_matrix_calloc(2,2); gsl_matrix_set(R, 0, 0, wrong_latitude); gsl_matrix_set(R, 1, 1, wrong_longitude); /* Macierz jednostkowa */ gsl_matrix *I = gsl_matrix_alloc(2,2); gsl_matrix_set_identity(I); /* P = P(k-1) + Q */ gsl_matrix_add(P, Q); /* K = P/(P + R) */ gsl_matrix *K = gsl_matrix_calloc(2,2); (*K) = (*P); gsl_matrix_add(R, P); gsl_matrix_div_elements(K, R); /* P(k) = (I-K)*P */ gsl_matrix_sub(I, K); gsl_matrix_mul_elements(I, P); (*P) = (*I); /* x(k) = x + K(z-x) */ gsl_matrix_sub(z, x); gsl_matrix_mul_elements(K, z); gsl_matrix_add(x, K); //zaktualizowana wartoœc gsl_matrix_free(z); gsl_matrix_free(H); gsl_matrix_free(Q); gsl_matrix_free(R); gsl_matrix_free(I); gsl_matrix_free(K); }
// 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; }
static void matrix_add_identity(gsl_matrix *m, double f) { gsl_matrix *id = gsl_matrix_alloc(m->size1, m->size2); gsl_matrix_set_identity(id); gsl_matrix_scale(id, f); gsl_matrix_add(m, id); gsl_matrix_free(id); }
val egsl_sum(val v1, val v2){ gsl_matrix * m1 = egsl_gslm(v1); gsl_matrix * m2 = egsl_gslm(v2); val v3 = egsl_alloc(m1->size1,m1->size2); gsl_matrix * m3 = egsl_gslm(v3); gsl_matrix_memcpy(m3,m1); gsl_matrix_add(m3,m2); return v3; }
// matrix addition Matrix& Matrix::operator+=( const Matrix& other ) { // check dimensions assert( nRows() == other.nRows() && nCols() == other.nCols() ); // addition gsl_matrix_add( data, other.data ); return ( *this ); }
// 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; }
/** Addition operator (matrix) */ matrix<double> matrix<double>::operator+(const matrix<double>& m) { matrix<double> m1(_matrix); if (gsl_matrix_add(m1.as_gsl_type_ptr(), m.as_gsl_type_ptr())) { std::cout << "\n Error in matrix<double> +" << std::endl; exit(EXIT_FAILURE); } return m1; }
void ccl_mat_add (double *A, const double *B, int row, int col){ gsl_matrix *A_ = gsl_matrix_alloc(row,col); memcpy(A_->data,A,row*col*sizeof(double)); gsl_matrix *B_ = gsl_matrix_alloc(row,col); memcpy(B_->data,B,row*col*sizeof(double)); gsl_matrix_add(A_,B_); memcpy(A,A_->data,row*col*sizeof(double)); gsl_matrix_free(A_); gsl_matrix_free(B_); }
void chapeau_sum ( chapeau * ch1, chapeau * ch2 ) { //Overwrite ch1 with ch1+ch2 int i,j,k; if (sizeof(ch1)!=sizeof(ch2)) { fprintf(stderr,"CFACV/C) ERROR: you can not sum chapeau objects with different sizes\n"); exit(-1); } if (ch1->rmin!=ch2->rmin || ch1->rmax!=ch2->rmax || ch1->dr!=ch2->dr ) { fprintf(stderr,"CFACV/C) ERROR: you can not sum chapeau objects with different sizes\n"); exit(-1); } for (i=0;i<ch1->N;i++) { if ( ch1->mask[i]!=ch2->mask[i] ) { fprintf(stderr,"CFACV/C) ERROR: you can not sum chapeau objects with different masks\n"); exit(-1); } } gsl_vector_add(ch1->b ,ch2->b); gsl_matrix_add(ch1->A ,ch2->A); gsl_vector_add(ch1->bfull,ch2->bfull); gsl_matrix_add(ch1->Afull,ch2->Afull); for (i=0;i<ch1->m;i++) { ch1->hits[i] = (ch1->hits[i]||ch2->hits[i]); } //// I think that the next is irrelevant since I am saving the state before //// the chapeau_output and therefore before the chapeau_update, but I let this //// here just for the case. //for (i=0;i<ch1->N;i++) { // for (j=0;j<3;j++) { // for (k=0;k<ch1->m;k++) { // ch1->s[i][j][k] = ch1->s[i][j][k] + ch2->s[i][j][k]; // } // } //} //gsl_vector_add(ch1->lam ,ch2->lam); //also irrelevant }
/*! \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); }
// matrix addition Matrix Matrix::operator+( const Matrix& other ) const { // check dimensions assert( nRows() == other.nRows() && nCols() == other.nCols() ); // addition Matrix result( *this ); gsl_matrix_add( result.data, other.data ); return result; }
int Matrix::addWithMatrix(Matrix* b) { if(!dimequal(b)) { cout << "Dimensions do not match" << endl; return -1; } gsl_matrix_add(matrix,b->matrix); return 0; }
void ispc_update_W_helper(par* p, gsl_matrix** dW, int i, int nrow, int ncol, double mu, double lambda, double step){ gsl_matrix* res = gsl_matrix_alloc(nrow, ncol); updateweights_ispc((*(p->weights[i])).data, (*(dW[i])).data, (*(p->weights_momentum[i])).data, (*res).data, mu, lambda, step, nrow, ncol); gsl_matrix_memcpy(p->weights_momentum[i], res); free(res); gsl_matrix_add(p->weights[i], p->weights_momentum[i]); }
void StripedDGamma::calcYrtDgammaYr( gsl_matrix *grad, const gsl_matrix *R, const gsl_vector *yr ) { size_t n_row = 0, k; gsl_matrix_set_zero(grad); for (k = 0; k < myS->getBlocksN(); n_row += myS->getBlock(k)->getN(), k++) { gsl_vector_const_view sub_yr = gsl_vector_const_subvector(yr, n_row * R->size2, myS->getBlock(k)->getN() * R->size2); myLHDGamma[k]->calcYrtDgammaYr(myTmpGrad, R, &sub_yr.vector); gsl_matrix_add(grad, myTmpGrad); } }
//Add matrix b to this matrix //Return NULL on failure //Caller must free this matrix Matrix* Matrix::addMatrix(Matrix* b) { if(!dimequal(b)) { cout << "Dimensions do not match" << endl; return NULL; } Matrix* res=new Matrix(row,col); gsl_matrix_memcpy (res->matrix, matrix); gsl_matrix_add(res->matrix,b->matrix); return res; }
void regularisation(par* p, gsl_vector** biases, gsl_matrix** weights) { for (int i = 0; i < p->num_layers-1; i++){ //gsl_blas_daxpy(p->penalty, p->biases[i], biases[i]); gsl_matrix* temp = gsl_matrix_alloc((*weights[i]).size1,(*weights[i]).size2); gsl_matrix_memcpy(temp, p->weights[i]); gsl_matrix_scale(temp, p->penalty); gsl_matrix_add(weights[i],temp); gsl_matrix_free(temp); } }
inline double get_mahalanobis_distance(rgb_gaussian *g0, rgb_gaussian *g1) { gsl_matrix *cov_sum = gsl_matrix_alloc(3, 3); gsl_matrix_memcpy(cov_sum, g0->cov); gsl_matrix_add(cov_sum, g1->cov); double mahalanobis_distance = gsl_mahalanobis_distance(g0->mean, g1->mean, cov_sum); gsl_matrix_free(cov_sum); return mahalanobis_distance; }
int ComputeHermitianMatrix(const gsl_matrix *const M, gsl_matrix_complex * const E, gsl_matrix *const S, gsl_matrix *const N) { unsigned int i, j; gsl_complex z; // S = (M + M^T)/2 gsl_matrix_memcpy(S, M); gsl_matrix_transpose(S); gsl_matrix_add(S, M); gsl_matrix_scale(S, 0.5); // N = (M - M^T)/2 gsl_matrix_memcpy(N, M); gsl_matrix_transpose(N); gsl_matrix_scale(N, -1); gsl_matrix_add(N, M); gsl_matrix_scale(N, 0.5); for(i = 0; i < M->size1; i++) { for(j = 0 ; j < M->size2; j++) { GSL_SET_COMPLEX(&z, gsl_matrix_get(S, i, j), gsl_matrix_get(N, i, j)); gsl_matrix_complex_set(E, i, j, z); } } return GSL_SUCCESS; }
// f = (1/2) x^T Ax + b^T x void prox_quad(gsl_vector *x, const double rho, gsl_matrix *A, gsl_matrix *b) { gsl_matrix *I = gsl_matrix_alloc(A->size1); gsl_matrix_set_identity(I); gsl_matrix_scale(I, rho); gsl_matrix_add(I, A); gsl_vector_scale(x, rho); gsl_vector_scale(b, -1); gsl_vector_add(b, x); gsl_linalg_cholesky_decomp(I); gsl_linalg_cholesky_solve(I, b, x); gsl_matrix_free(I); }
static int rm_add_rm(lua_State *L) { mMatReal *a = qlua_checkMatReal(L, 1); mMatReal *b = qlua_checkMatReal(L, 2); int al = a->l_size; int ar = a->r_size; mMatReal *r = qlua_newMatReal(L, al, ar); if ((al != b->l_size) || (ar != b->r_size)) return luaL_error(L, "matrix sizes mismatch in m + m"); gsl_matrix_memcpy(r->m, a->m); gsl_matrix_add(r->m, b->m); return 1; }