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