void Testiwishart(CuTest* tc) {
/*set a non-trivial location matrix*/
gsl_matrix* V = gsl_matrix_alloc(DIM, DIM);
gsl_matrix_set_identity(V);
gsl_matrix_add_constant(V, 1.0);
p = mcmclib_iwishart_lpdf_alloc(V, P);
x = gsl_vector_alloc(DIM * DIM);
gsl_matrix_view X_v = gsl_matrix_view_vector(x, DIM, DIM);
gsl_matrix* X = &(X_v.matrix);
gsl_matrix_set_identity(X);
gsl_matrix_add_constant(X, 1.0);
/*check for side-effects*/
double tmp = lpdf(1.0);
CuAssertTrue(tc, tmp == lpdf(1.0));
CuAssertDblEquals(tc, -18.188424, lpdf(1.0), TOL);
CuAssertDblEquals(tc, 49.59203, lpdf(0.5), TOL);
CuAssertDblEquals(tc, -88.468878, lpdf(2.0), TOL);
mcmclib_iwishart_lpdf_free(p);
gsl_vector_free(x);
gsl_matrix_free(V);
}
void mvn_sample(gsl_vector *mean_cand, gsl_matrix *var)
{
/* Takes a mean vec, mean and var matrix,
* var and gives vector of MVN(mean,var) realisations, x
*/
int i, j;
int dimen = var -> size1;
double value;
gsl_matrix *disp;
gsl_vector *ran;
gsl_matrix *fast_species;
fast_species = gsl_matrix_alloc(2, 2);
gsl_matrix_set_identity(fast_species);
for(i=0;i<dimen; i++) {
if(MGET(var, i, i) <0.00000000001) {
MSET(var, i, i, 1.0);
MSET(fast_species, i, i, 0.0);
}
}
disp = gsl_matrix_alloc(2, 2);
ran = gsl_vector_alloc(2);
gsl_matrix_memcpy(disp, var);
if(postive_definite == 1) {
gsl_linalg_cholesky_decomp(disp);
for(i=0;i<dimen;i++) {
for (j=i+1;j<dimen;j++) {
MSET(disp,i,j,0.0);
}
}
}else{
value = pow(MGET(disp, 0 ,0), 0.5);
gsl_matrix_set_identity(disp);
MSET(disp, 0,0, value);
MSET(disp, 1,1, value);
}
for (j=0;j<dimen;j++) {
VSET(ran,j,gsl_ran_gaussian(r,1.0));
}
/*remove update from slow species*/
gsl_matrix_mul_elements(disp, fast_species);
/*Add noise to mean cand*/
gsl_blas_dgemv(CblasNoTrans,1.0, disp, ran, 1.0, mean_cand);
for(i=0; i<2; i++) {
if(VGET(mean_cand,i)<=0.0001 && MGET(fast_species, i, i) > 0.000001)
VSET(mean_cand,i,0.0001);
}
gsl_vector_free(ran);
gsl_matrix_free(disp);
gsl_matrix_free(fast_species);
}
/*! \brief Input: _. Output: A matrix
*
* Generates a unimodular matrix.
*/
gsl_matrix* genU(){
int z, j ,display = 1;
/* Initialize and allocate memory for necessary matrices */
gsl_matrix* result = gsl_matrix_alloc(SIZE,SIZE);
gsl_matrix* result2 = gsl_matrix_alloc(SIZE,SIZE);
gsl_matrix* base = gsl_matrix_alloc(SIZE,SIZE);
gsl_matrix_set_identity(result);
gsl_matrix_set_identity(result2);
gsl_matrix_set_identity(base);
/* Display progress of generating a unimodular to the user */
printf("Generating Unimodular\n");
printf("|..................................................|\r\033[01;35m|");
fflush(stdout);
if(SIZE/50 > 1) display = SIZE/50;
for(z = 0; z<DEPTH; z++){
if(z%2 == 0){
for(j=0; j < SIZE ; j++)
{
if(j%2 == 0){
randomLine(base,j);
gsl_linalg_matmult(result,base,result2);
gsl_matrix_memcpy(result,result2);
clearLine(base,j);
if(j % display == 0){
printf("-");
fflush(stdout);
}
}
}
}else{
printf("| 50%%\r\033[01;32m|");
for(j=SIZE-1; j >= 0 ; j--)
{
if(j%2 == 1){
randomLine(base,j);
gsl_linalg_matmult(result,base,result2);
gsl_matrix_memcpy(result,result2);
clearLine(base,j);
if(j % display == 1){
printf("=");
fflush(stdout);
}
}
}
}
}
printf("| 100%% DONE\rUnimodular Generated\033[0m\n");
free(result2);
free(base);
return result;
}
static int
vardim_df (CBLAS_TRANSPOSE_t TransJ, const gsl_vector * x,
const gsl_vector * u, void * params, gsl_vector * v,
gsl_matrix * JTJ)
{
gsl_matrix_view J = gsl_matrix_view_array(vardim_J, vardim_N, vardim_P);
size_t i;
double sum = 0.0;
gsl_matrix_view m = gsl_matrix_submatrix(&J.matrix, 0, 0, vardim_P, vardim_P);
gsl_matrix_set_identity(&m.matrix);
for (i = 0; i < vardim_P; ++i)
{
double xi = gsl_vector_get(x, i);
sum += (i + 1.0) * (xi - 1.0);
}
for (i = 0; i < vardim_P; ++i)
{
gsl_matrix_set(&J.matrix, vardim_P, i, i + 1.0);
gsl_matrix_set(&J.matrix, vardim_P + 1, i, 2*(i + 1.0)*sum);
}
if (v)
gsl_blas_dgemv(TransJ, 1.0, &J.matrix, u, 0.0, v);
if (JTJ)
gsl_blas_dsyrk(CblasLower, CblasTrans, 1.0, &J.matrix, 0.0, JTJ);
(void)params; /* avoid unused parameter warning */
return GSL_SUCCESS;
}
static int
vardim_df (const gsl_vector * x, void *params, gsl_matrix * J)
{
size_t i;
double sum = 0.0;
gsl_matrix_view m = gsl_matrix_submatrix(J, 0, 0, vardim_P, vardim_P);
gsl_matrix_set_identity(&m.matrix);
for (i = 0; i < vardim_P; ++i)
{
double xi = gsl_vector_get(x, i);
sum += (i + 1.0) * (xi - 1.0);
}
for (i = 0; i < vardim_P; ++i)
{
gsl_matrix_set(J, vardim_P, i, i + 1.0);
gsl_matrix_set(J, vardim_P + 1, i, 2*(i + 1.0)*sum);
}
(void)params; /* avoid unused parameter warning */
return GSL_SUCCESS;
}
/** alloc a new at7_gamma object. Input arguments are copied @internal */
static at7_gamma* at7_gamma_alloc(const gsl_vector* beta,
gsl_vector** mu,
gsl_matrix** Sigma) {
at7_gamma* ans = (at7_gamma*) malloc(sizeof(at7_gamma));
const size_t K = beta->size;
const size_t dim = mu[0]->size;
ans->beta = gsl_vector_alloc(K);
gsl_vector_memcpy(ans->beta, beta);
ans->mu = (gsl_vector**) malloc(K * sizeof(gsl_vector*));
ans->Sigma = (gsl_matrix**) malloc(K * sizeof(gsl_matrix*));
ans->pik = (mcmclib_mvnorm_lpdf**) malloc(K * sizeof(mcmclib_mvnorm_lpdf*));
ans->tmpMean = gsl_vector_alloc(dim);
ans->qVariances = (gsl_matrix**) malloc(K * sizeof(gsl_matrix*));
ans->qdk = (mcmclib_mvnorm_lpdf**) malloc(K * sizeof(mcmclib_mvnorm_lpdf*));
for(size_t k=0; k < K; k++) {
ans->mu[k] = gsl_vector_alloc(dim);
gsl_vector_memcpy(ans->mu[k], mu[k]);
ans->Sigma[k] = gsl_matrix_alloc(dim, dim);
gsl_matrix_memcpy(ans->Sigma[k], Sigma[k]);
ans->pik[k] = mcmclib_mvnorm_lpdf_alloc(ans->mu[k], ans->Sigma[k]->data);
ans->qVariances[k] = gsl_matrix_alloc(dim, dim);
ans->qdk[k] = mcmclib_mvnorm_lpdf_alloc(ans->tmpMean, ans->qVariances[k]->data);
}
gsl_vector_memcpy(ans->beta, beta);
ans->pi = mcmclib_mixnorm_lpdf_alloc(ans->beta, ans->pik);
ans->Sigma_eps = gsl_matrix_alloc(dim, dim);
gsl_matrix_set_identity(ans->Sigma_eps);
gsl_matrix_scale(ans->Sigma_eps, 0.001);
ans->scaling_factors = gsl_vector_alloc(K);
gsl_vector_set_all(ans->scaling_factors, 2.38*2.38 / (double) dim);
ans->weights = gsl_vector_alloc(K);
gsl_vector_set_all(ans->weights, 0.0);
at7_gamma_update_Sigma(ans);
return ans;
}
int main(int argc, char *argv[]){
int args;
int m, n;
double pdf, dof;
args = 1;
m = atoi(argv[args++]);
n = atoi(argv[args++]);
// allocate space for matrix X, test_copy, and Scale.
gsl_matrix *X = gsl_matrix_alloc(m, n);
gsl_matrix *inv = gsl_matrix_alloc(m,n);
gsl_matrix *Scale = gsl_matrix_alloc(m, n);
gsl_matrix_set_identity(Scale);
fill_matrix(X);
dof = m + 1;
pdf = iwishpdf(X, Scale, inv, dof);
// Print the data
printf("\n%s", "Inverse Wishart matrix");
print_matrix(X);
printf("\n%s", "Scale matrix");
print_matrix(Scale);
printf("\n%s", "Wishart matrix");
print_matrix(inv);
printf("\n%s", "Density");
printf("\n%f\n", pdf);
return(0);
}
static int _equalf(CMATRIX *a, double f)
{
bool result;
if (COMPLEX(a))
{
if (f == 0.0)
return gsl_matrix_complex_isnull(CMAT(a));
gsl_matrix_complex *m = gsl_matrix_complex_alloc(WIDTH(a), HEIGHT(a));
gsl_matrix_complex_set_identity(m);
gsl_matrix_complex_scale(m, gsl_complex_rect(f, 0));
result = gsl_matrix_complex_equal(CMAT(a), m);
gsl_matrix_complex_free(m);
}
else
{
if (f == 0.0)
return gsl_matrix_isnull(MAT(a));
gsl_matrix *m = gsl_matrix_alloc(WIDTH(a), HEIGHT(a));
gsl_matrix_set_identity(m);
gsl_matrix_scale(m, f);
result = gsl_matrix_equal(MAT(a), m);
gsl_matrix_free(m);
}
return result;
}
static void Givens_set_Shij(gsl_matrix* S, size_t i, size_t j, double alpha_ij) {
gsl_matrix_set_identity(S);
gsl_matrix_set(S, i, i, cos(alpha_ij));
gsl_matrix_set(S, j, j, cos(alpha_ij));
gsl_matrix_set(S, i, j, sin(alpha_ij));
gsl_matrix_set(S, j, i, -sin(alpha_ij));
}
gsl_vector* linear_ols_beta(gsl_vector *v_y, gsl_matrix *m_X){
size_t i_k = m_X->size2;
gsl_vector *v_XTy = gsl_vector_alloc(i_k);
gsl_vector *v_betahat = gsl_vector_alloc(i_k);
gsl_matrix *m_XTX = gsl_matrix_alloc(i_k,i_k);
gsl_matrix *m_invXTX = gsl_matrix_alloc(i_k,i_k);
gsl_vector_set_all(v_XTy,0);
gsl_vector_set_all(v_betahat,0);
gsl_matrix_set_all(m_XTX,0);
olsg(v_y,m_X,v_XTy,m_XTX);
gsl_linalg_cholesky_decomp (m_XTX);
gsl_matrix_set_identity(m_invXTX);
gsl_blas_dtrsm (CblasLeft, CblasLower,CblasNoTrans,CblasNonUnit,1.0,m_XTX,m_invXTX);
gsl_blas_dtrsm (CblasLeft, CblasLower,CblasTrans,CblasNonUnit,1.0,m_XTX,m_invXTX);
gsl_vector_set_all(v_betahat,0.0);
gsl_blas_dsymv (CblasLower,1.0,m_invXTX,v_XTy,0.0,v_betahat);
gsl_vector_free(v_XTy);
gsl_matrix_free(m_XTX);
gsl_matrix_free(m_invXTX);
return v_betahat;
}
void Testwishart(CuTest* tc) {
/*set a non-trivial location matrix*/
gsl_matrix* V = gsl_matrix_alloc(DIM, DIM);
gsl_matrix_set_identity(V);
gsl_matrix_scale(V, V0);
for(size_t i=0; i<DIM; i++) for(size_t j=i+1; j < DIM; j++) {
gsl_matrix_set(V, i, j, 1.0);
gsl_matrix_set(V, j, i, 1.0);
}
p = mcmclib_wishart_lpdf_alloc(V, P);
x = gsl_vector_alloc(DIM * DIM);
gsl_matrix_view X_v = gsl_matrix_view_vector(x, DIM, DIM);
gsl_matrix* X = &(X_v.matrix);
gsl_matrix_set_all(X, 0.2);
for(size_t i=0; i<DIM; i++)
gsl_matrix_set(X, i, i, 1.0);
CuAssertDblEquals(tc, -7.152627, lpdf(1.0), TOL);
CuAssertDblEquals(tc, -29.549142, lpdf(0.5), TOL);
CuAssertDblEquals(tc, 13.380252, lpdf(2.0), TOL);
mcmclib_wishart_lpdf_free(p);
gsl_vector_free(x);
gsl_matrix_free(V);
}
/**
* Returns a GSL-indentity matrix.
*
* This free function returns a GSL-matrix with the provided dimensions.
* It allocates the memory using gsl_matrix_alloc and the caller of the function
* is responsible for freeing the memory again.
*
* @param rows :: Number of rows in the matrix.
* @param cols :: Number of columns in the matrix.
* @return Identity matrix with dimensions (rows, columns).
*/
gsl_matrix *getGSLIdentityMatrix(size_t rows, size_t cols) {
gsl_matrix *gslMatrix = gsl_matrix_alloc(rows, cols);
gsl_matrix_set_identity(gslMatrix);
return gslMatrix;
}
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);
}
int
gsl_multifit_linear_Lk(const size_t p, const size_t k, gsl_matrix *L)
{
if (p <= k)
{
GSL_ERROR("p must be larger than derivative order", GSL_EBADLEN);
}
else if (k >= GSL_MULTIFIT_MAXK - 1)
{
GSL_ERROR("derivative order k too large", GSL_EBADLEN);
}
else if (p - k != L->size1 || p != L->size2)
{
GSL_ERROR("L matrix must be (p-k)-by-p", GSL_EBADLEN);
}
else
{
double c_data[GSL_MULTIFIT_MAXK];
gsl_vector_view cv = gsl_vector_view_array(c_data, k + 1);
size_t i, j;
/* zeroth derivative */
if (k == 0)
{
gsl_matrix_set_identity(L);
return GSL_SUCCESS;
}
gsl_matrix_set_zero(L);
gsl_vector_set_zero(&cv.vector);
gsl_vector_set(&cv.vector, 0, -1.0);
gsl_vector_set(&cv.vector, 1, 1.0);
for (i = 1; i < k; ++i)
{
double cjm1 = 0.0;
for (j = 0; j < k + 1; ++j)
{
double cj = gsl_vector_get(&cv.vector, j);
gsl_vector_set(&cv.vector, j, cjm1 - cj);
cjm1 = cj;
}
}
/* build L, the c_i are the entries on the diagonals */
for (i = 0; i < k + 1; ++i)
{
gsl_vector_view v = gsl_matrix_superdiagonal(L, i);
double ci = gsl_vector_get(&cv.vector, i);
gsl_vector_set_all(&v.vector, ci);
}
return GSL_SUCCESS;
}
} /* gsl_multifit_linear_Lk() */
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);
}
int
Matrix::initAsIdentity()
{
if((row==0)||(col==0))
{
cout << "Warning!! Row or col = 0" << endl;
}
gsl_matrix_set_identity (matrix);
return 0;
}
int GlmTest::resampSmryCase(glm *model, gsl_matrix *bT, GrpMat *GrpXs, gsl_matrix *bO, unsigned int i )
{
gsl_set_error_handler_off();
int status, isValid=TRUE;
unsigned int j, k, id;
gsl_vector_view yj, oj, xj;
gsl_matrix *tXX = NULL;
unsigned int nRows=tm->nRows, nParam=tm->nParam;
if (bootID == NULL) {
tXX = gsl_matrix_alloc(nParam, nParam);
while (isValid==TRUE) { // if all isSingular==TRUE
if (tm->reprand!=TRUE) GetRNGstate();
for (j=0; j<nRows; j++) {
// resample Y, X, offsets accordingly
if (tm->reprand==TRUE)
id = (unsigned int) gsl_rng_uniform_int(rnd, nRows);
else id = (unsigned int) nRows * Rf_runif(0, 1);
xj = gsl_matrix_row(model->Xref, id);
gsl_matrix_set_row(GrpXs[0].matrix, j, &xj.vector);
yj = gsl_matrix_row(model->Yref, id);
gsl_matrix_set_row(bT, j, &yj.vector);
oj = gsl_matrix_row(model->Eta, id);
gsl_matrix_set_row(bO, j, &oj.vector);
}
if (tm->reprand!=TRUE) PutRNGstate();
gsl_matrix_set_identity(tXX);
gsl_blas_dsyrk(CblasLower,CblasTrans,1.0,GrpXs[0].matrix,0.0,tXX);
status=gsl_linalg_cholesky_decomp(tXX);
if (status!=GSL_EDOM) break;
// if (calcDet(tXX)>eps) break;
}
for (k=2; k<nParam+2; k++)
subX2(GrpXs[0].matrix, k-2, GrpXs[k].matrix);
}
else {
for (j=0; j<nRows; j++) {
id = (unsigned int) gsl_matrix_get(bootID, i, j);
// resample Y and X and offset
yj=gsl_matrix_row(model->Yref, id);
gsl_matrix_set_row (bT, j, &yj.vector);
oj = gsl_matrix_row(model->Eta, id);
gsl_matrix_set_row(bO, j, &oj.vector);
xj = gsl_matrix_row(model->Xref, id);
gsl_matrix_set_row(GrpXs[0].matrix, j, &xj.vector);
}
for (k=2; k<nParam+2; k++)
subX2(GrpXs[0].matrix, k-2, GrpXs[k].matrix);
}
gsl_matrix_free(tXX);
return SUCCESS;
}
static CMATRIX *MATRIX_identity(int width, int height, bool complex)
{
CMATRIX *m = MATRIX_create(width, height, complex, FALSE);
if (complex)
gsl_matrix_complex_set_identity(CMAT(m));
else
gsl_matrix_set_identity(MAT(m));
return m;
}
mcmclib_mcar_model* mcmclib_mcar_model_alloc(mcmclib_mcar_tilde_lpdf* m, gsl_vector* e) {
mcmclib_mcar_model* a = (mcmclib_mcar_model*) malloc(sizeof(mcmclib_mcar_model));
a->lpdf = m;
a->e = e;
size_t p = m->p;
gsl_matrix* V = gsl_matrix_alloc(p, p);
gsl_matrix_set_identity(V);
a->w = mcmclib_iwishart_lpdf_alloc(V, (unsigned int) p);
gsl_matrix_free(V);
return a;
}
int
gsl_linalg_hessenberg_unpack(gsl_matrix * H, gsl_vector * tau,
gsl_matrix * U)
{
int s;
gsl_matrix_set_identity(U);
s = gsl_linalg_hessenberg_unpack_accum(H, tau, U);
return s;
} /* gsl_linalg_hessenberg_unpack() */
void fred2_solver(double a,
double b,
gsl_vector* t,
gsl_vector* f,
gsl_vector* w)
{
gsl_integration_glfixed_table* glgrid = gsl_integration_glfixed_table_alloc(MAX);
gsl_permutation* p = gsl_permutation_alloc(MAX);
gsl_matrix* lhs = gsl_matrix_alloc(MAX, MAX);
gsl_matrix* ktilde = gsl_matrix_alloc(MAX, MAX);
gsl_matrix* ak = gsl_matrix_alloc(MAX, MAX);
gsl_vector* g = gsl_vector_alloc(MAX);
int i, j, error, s;
double ptsi, wghtsi;
// set Gauss-Legendre integration points and weights
for (i = 0; i < MAX; i++)
{
error = gsl_integration_glfixed_point(a, b, i, &ptsi, &wghtsi, glgrid);
gsl_vector_set(t, i, ptsi);
gsl_vector_set(w, i, wghtsi);
}
kernel(ak, t, t);
aux_g(g, t);
// fill in unit matrix first
gsl_matrix_set_identity(lhs);
for (i = 0; i < MAX; i++)
{
for (j = 0; j < MAX; j++)
{
gsl_matrix_set(ktilde, i, j, gsl_matrix_get(ak, i, j) * gsl_vector_get(w, j));
}
}
// set up LHS matrix
error = gsl_matrix_sub(lhs, ktilde);
gsl_linalg_LU_decomp(lhs, p, &s);
gsl_linalg_LU_solve(lhs, p, g, f);
gsl_integration_glfixed_table_free(glgrid);
gsl_permutation_free(p);
gsl_matrix_free(lhs);
gsl_matrix_free(ktilde);
gsl_vector_free(g);
gsl_matrix_free(ak);
return;
}
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);
}
int
gsl_linalg_LQ_unpack (const gsl_matrix * LQ, const gsl_vector * tau, gsl_matrix * Q, gsl_matrix * L)
{
const size_t N = LQ->size1;
const size_t M = LQ->size2;
if (Q->size1 != M || Q->size2 != M)
{
GSL_ERROR ("Q matrix must be M x M", GSL_ENOTSQR);
}
else if (L->size1 != N || L->size2 != M)
{
GSL_ERROR ("R matrix must be N x M", GSL_ENOTSQR);
}
else if (tau->size != GSL_MIN (M, N))
{
GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
}
else
{
size_t i, j, l_border;
/* Initialize Q to the identity */
gsl_matrix_set_identity (Q);
for (i = GSL_MIN (M, N); i-- > 0;)
{
gsl_vector_const_view c = gsl_matrix_const_row (LQ, i);
gsl_vector_const_view h = gsl_vector_const_subvector (&c.vector,
i, M - i);
gsl_matrix_view m = gsl_matrix_submatrix (Q, i, i, M - i, M - i);
double ti = gsl_vector_get (tau, i);
gsl_linalg_householder_mh (ti, &h.vector, &m.matrix);
}
/* Form the lower triangular matrix L from a packed LQ matrix */
for (i = 0; i < N; i++)
{
l_border=GSL_MIN(i,M-1);
for (j = 0; j <= l_border ; j++)
gsl_matrix_set (L, i, j, gsl_matrix_get (LQ, i, j));
for (j = l_border+1; j < M; j++)
gsl_matrix_set (L, i, j, 0.0);
}
return GSL_SUCCESS;
}
}
int
gsl_linalg_QR_unpack (const gsl_matrix * QR, const gsl_vector * tau, gsl_matrix * Q, gsl_matrix * R)
{
const size_t M = QR->size1;
const size_t N = QR->size2;
if (Q->size1 != M || Q->size2 != M)
{
GSL_ERROR ("Q matrix must be M x M", GSL_ENOTSQR);
}
else if (R->size1 != M || R->size2 != N)
{
GSL_ERROR ("R matrix must be M x N", GSL_ENOTSQR);
}
else if (tau->size != GSL_MIN (M, N))
{
GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
}
else
{
size_t i, j;
/* Initialize Q to the identity */
gsl_matrix_set_identity (Q);
for (i = GSL_MIN (M, N); i-- > 0;)
{
gsl_vector_const_view c = gsl_matrix_const_column (QR, i);
gsl_vector_const_view h = gsl_vector_const_subvector (&c.vector,
i, M - i);
gsl_matrix_view m = gsl_matrix_submatrix (Q, i, i, M - i, M - i);
double ti = gsl_vector_get (tau, i);
gsl_linalg_householder_hm (ti, &h.vector, &m.matrix);
}
/* Form the right triangular matrix R from a packed QR matrix */
for (i = 0; i < M; i++)
{
for (j = 0; j < i && j < N; j++)
gsl_matrix_set (R, i, j, 0.0);
for (j = i; j < N; j++)
gsl_matrix_set (R, i, j, gsl_matrix_get (QR, i, j));
}
return GSL_SUCCESS;
}
}
void getPosition0(Position0 *pos, GpsDate *gps, CircBuffer *gps_buffer)
{
gsl_matrix *x = gsl_matrix_calloc(2,1);
gsl_matrix *P = gsl_matrix_alloc(2,2);
gsl_matrix_set_identity(P);
double previous_latitude0;
double previous_longitude0;
double latitude0;
double longitude0;
int i = 0;
while (i < 5)
{
if((gpsUpdate(gps, gps_buffer)) && ((*gps).status == 'A'))
{
kalmanFilterPosition0((*gps).latitude, (*gps).longitude, x, P);
i++;
printf("Aktualizacja w filtrze KALMANA poraz %i", i+1); //potrzebne do debugowania
}
}
i = 0;
while(i<5)
{
previous_latitude0 = gsl_matrix_get(x, 0, 0);
previous_longitude0 = gsl_matrix_get(x, 1, 0);
if((gpsUpdate(gps, gps_buffer)) && ((*gps).status == 'A'))
{
kalmanFilterPosition0((*gps).latitude, (*gps).longitude, x, P);
latitude0 = gsl_matrix_get(x, 0, 0);
longitude0 = gsl_matrix_get(x, 1, 0);
/* Blok danych do wyœwietlenia przy debugowaniu */
printf("Wartoœc previous_latitude0 = %f", previous_latitude0);
printf("Wartoœc previous_longitude0 = %f", previous_longitude0);
printf("Wartoœc latitude0 = %f", latitude0);
printf("Wartoœc longitude0 = %f", longitude0);
if((previous_latitude0 == latitude0) && (previous_longitude0 == longitude0))
{
i++; //Kolejny wektor x identyczny z poprzednim wektorem x
if(i == 5)
{
pos->latitude0 = latitude0;
pos->longitude0 = longitude0;
gsl_matrix_free(x);
gsl_matrix_free(P);
}
} else {
i = 0;
}
}
}
}
//int GlmTest::resampAnovaCase(glm *model, gsl_matrix *Onull, gsl_matrix *bT, gsl_matrix *bX, gsl_matrix *bO, gsl_matrix *bOnull, unsigned int i)
int GlmTest::resampAnovaCase(glm *model, gsl_matrix *bT, gsl_matrix *bX, gsl_matrix *bO, unsigned int i)
{
gsl_set_error_handler_off();
int status, isValid=TRUE;
unsigned int j, id, nP;
gsl_vector_view yj, xj, oj;
nP = model->Xref->size2;
gsl_matrix *tXX = gsl_matrix_alloc(nP, nP);
unsigned int nRows=tm->nRows;
if (bootID == NULL) {
while (isValid==TRUE) {
if (tm->reprand!=TRUE) GetRNGstate();
for (j=0; j<nRows; j++) {
if (tm->reprand==TRUE)
id=(unsigned int)gsl_rng_uniform_int(rnd, nRows);
else id=(unsigned int) nRows*Rf_runif(0, 1);
// resample Y and X and offset
yj=gsl_matrix_row(model->Yref, id);
xj = gsl_matrix_row(model->Xref, id);
oj = gsl_matrix_row(model->Eta, id);
gsl_matrix_set_row (bT, j, &yj.vector);
gsl_matrix_set_row(bX, j, &xj.vector);
gsl_matrix_set_row(bO, j, &oj.vector);
}
if (tm->reprand!=TRUE) PutRNGstate();
gsl_matrix_set_identity(tXX);
gsl_blas_dsyrk(CblasLower,CblasTrans,1.0,bX,0.0,tXX);
status=gsl_linalg_cholesky_decomp(tXX);
if (status!=GSL_EDOM) break;
}
}
else {
for (j=0; j<nRows; j++) {
id = (unsigned int) gsl_matrix_get(bootID, i, j);
// resample Y and X and offset
yj=gsl_matrix_row(model->Yref, id);
xj = gsl_matrix_row(model->Xref, id);
oj = gsl_matrix_row(model->Oref, id);
gsl_matrix_set_row (bT, j, &yj.vector);
gsl_matrix_set_row(bX, j, &xj.vector);
gsl_matrix_set_row(bO, j, &oj.vector);
}
}
gsl_matrix_free(tXX);
return SUCCESS;
}
// constructor
GlmTest::GlmTest(const mv_Method *tm) : tm(tm) {
eps = tm->tol;
// eps = 1e-6;
smryStat = NULL;
Psmry = NULL;
anovaStat = NULL;
Panova = NULL;
Xin = NULL;
XBeta = NULL;
Sigma = NULL;
bootID = NULL;
bootStore = NULL;
// Prepared for geeCalc
L = gsl_matrix_alloc(tm->nParam, tm->nParam);
gsl_matrix_set_identity(L);
Rlambda = gsl_matrix_alloc(tm->nVars, tm->nVars);
Wj = gsl_matrix_alloc(tm->nRows, tm->nRows);
// Initialize GSL rnd environment variables
const gsl_rng_type *T;
gsl_rng_env_setup();
T = gsl_rng_default;
// an mt19937 generator with a seed of 0
rnd = gsl_rng_alloc(T);
if (tm->reprand != TRUE) {
struct timeval tv; // seed generation based on time
gettimeofday(&tv, 0);
unsigned long mySeed = tv.tv_sec + tv.tv_usec;
gsl_rng_set(rnd, mySeed); // reset seed
}
if (tm->resamp == PERMUTE) {
permid = (size_t *)malloc(tm->nRows * sizeof(size_t));
for (size_t i = 0; i < tm->nRows; i++)
permid[i] = i;
} else
permid = NULL;
if (tm->resamp == MONTECARLO) {
XBeta = gsl_matrix_alloc(tm->nRows, tm->nVars);
Sigma = gsl_matrix_alloc(tm->nVars, tm->nVars);
}
aic = new double[tm->nVars];
}
// 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);
}
match_result algorithm_iden::match(graph& g, graph& h,gsl_matrix* gm_P_i, gsl_matrix* gm_ldh,double dalpha_ldh)
{
if (bverbose)
*gout<<"Identity matching"<<std::endl;
//some duplicate variables
match_result mres;
mres.gm_P=gsl_matrix_alloc(N,N);
gsl_matrix_set_identity(mres.gm_P);
mres.gm_P_exact=NULL;
//initial score
mres.vd_trace.push_back(graph_dist(g,h,cscore_matrix));
//final score
mres.vd_trace.push_back(graph_dist(g,h,mres.gm_P,cscore_matrix));
mres.dres=mres.vd_trace.at(1);
mres.inum_iteration=2;
return mres;
}
gsl_matrix* calc_Ws(double s, double tres, double* Qxx_m, double* Qyy_m, double* Qxy_m, double* Qyx_m, int kx, int ky){
gsl_matrix *ws;
ws=gsl_matrix_alloc(kx,kx);
gsl_matrix_set_identity(ws);
gsl_matrix_scale(ws,s);
//Calculate H(s)
gsl_matrix *hs = calc_Hs(s,tres,Qxx_m,Qyy_m,Qxy_m,Qyx_m, kx, ky);
gsl_matrix_sub(ws,hs);
//cleanup
gsl_matrix_free(hs);
return ws;
}
