static void kalman_correct(kalman_t *kf, float p, float v)
{
/* K = P * HT * inv(H * P * HT + R) */
m_mlt(kf->H, kf->P, kf->T0);
mmtr_mlt(kf->T0, kf->H, kf->T1);
m_add(kf->T1, kf->R, kf->T0);
m_inverse(kf->T0, kf->T1);
mmtr_mlt(kf->P, kf->H, kf->T0);
m_mlt(kf->T0, kf->T1, kf->K);
/* x = x + K * (z - H * x) */
mv_mlt(kf->H, kf->x, kf->t0);
v_set_val(kf->z, 0, p);
v_set_val(kf->z, 1, v);
v_sub(kf->z, kf->t0, kf->t1);
mv_mlt(kf->K, kf->t1, kf->t0);
v_add(kf->x, kf->t0, kf->t1);
v_copy(kf->t1, kf->x);
/* P = (I - K * H) * P */
m_mlt(kf->K, kf->H, kf->T0);
m_sub(kf->I, kf->T0, kf->T1);
m_mlt(kf->T1, kf->P, kf->T0);
m_copy(kf->T0, kf->P);
}
// kernelRegression
// x:data
// dimention: x's dimention
// y: label of x
// num: number of data
// result: coeffience
double *kernelRegression(double **x,int dimention,double *y,int num,double gamma,double regilarization){
VEC *label;
MAT *gram,*ident,*inv;
int i,j;
double *result;
ident = m_get(num,num);
label = v_get(num);
memcpy(label->ve,y,sizeof(double)*num);
gram = m_get(num,num);
ident = m_get(num,num);
for(i=0;i<num;i++){
for(j=0;j<num;j++){
gram->me[i][j]=kernel(x[i],x[j],dimention,gamma);
}
}
inv=m_add(gram,sm_mlt(regilarization,m_ident(ident),NULL),NULL);
inv=m_inverse(inv,NULL); //memory leak.
VEC *coefficient=v_get(num);
mv_mlt(inv,label,coefficient);
result=malloc(sizeof(double)*num);
memcpy(result,coefficient->ve,sizeof(double)*num);
m_free(ident);
m_free(gram);
m_free(inv);
v_free(label);
v_free(coefficient);
return result;
}
//nearest points between two segments given by pi+veci, results given in ratio of vec [0 1]
int Reaching::FindSegmentsNearestPoints(cart_vec_t& p1,cart_vec_t& vec1,cart_vec_t& p2,cart_vec_t& vec2, float *nearest_point1, float *nearest_point2, float *dist){
CMatrix3_t mat;
CMatrix3_t invmat;
cart_vec_t& vec3,tmp,tmp1,tmp2,k,v2;
int i;
m_identity(mat);
v_cross(vec1,vec2,vec3);// check if vec1 and vec2 are not //
if(v_squ_length(vec3)<0.00001){
return 0;
}
v_scale(vec2,-1,v2);
m_set_v3_column(vec1,0,mat);
m_set_v3_column(v2,1,mat);
m_set_v3_column(vec3,2,mat);
m_inverse(mat,invmat);
v_sub(p2,p1,tmp);
v_transform_normal(tmp,invmat,k);
for(i=0;i<2;i++){
k[i] = max(min(1,k[i]),0);
}
v_scale(vec1,k[0],tmp);
v_add(p1,tmp,tmp1);
v_scale(vec2,k[1],tmp);
v_add(p2,tmp,tmp2);
v_sub(tmp2,tmp1,tmp);
*dist=v_length(tmp);
*nearest_point1 = k[0];
*nearest_point2 = k[1];
return 1;
}
/**
* Test matrix inverse.
*/
void test_m_inverse()
{
precision_t data[][3] = {
{ 1, 0, 2 },
{ -1, 5, 0 },
{ 0, 3, -9 }
};
matrix_t *X = m_initialize(3, 3);
fill_matrix_data(X, data);
matrix_t *Y = m_inverse(X);
matrix_t *Z = m_product(Y, X);
printf("X = \n");
m_fprint(stdout, X);
printf("Y = inv(X) = \n");
m_fprint(stdout, Y);
printf("Y * X = \n");
m_fprint(stdout, Z);
m_free(X);
m_free(Y);
m_free(Z);
}
static MAT *calc_VinvIminAw(MAT *Vw, MAT *X, MAT *VinvIminAw, int calc_Aw) {
/*
* calculate V_w^-1(I-A_w) (==VinvIminAw),
* A = X(X'X)^-1 X' (AY = XBeta; Beta = (X'X)^-1 X'Y)
*
* on second thought (Nov 1998 -- more than 4 years later :-))
* calc (I-Aw) only once and keep this constant during iteration.
*/
MAT *tmp = MNULL, *V = MNULL;
VEC *b = VNULL, *rhs = VNULL;
int i, j;
if (X->m != Vw->n || VinvIminAw->m != X->m)
ErrMsg(ER_IMPOSVAL, "calc_VinvIminAw: sizes don't match");
if (calc_Aw) {
IminAw = m_resize(IminAw, X->m, X->m);
tmp = m_resize(tmp, X->n, X->n);
tmp = mtrm_mlt(X, X, tmp); /* X'X */
m_inverse(tmp, tmp); /* (X'X)-1 */
/* X(X'X)-1 -> X(X'X)-1 X') */
IminAw = XVXt_mlt(X, tmp, IminAw);
for (i = 0; i < IminAw->m; i++) /* I - Aw */
for (j = 0; j <= i; j++)
if (i == j)
IminAw->me[i][j] = 1.0 - IminAw->me[i][j];
else
IminAw->me[i][j] = IminAw->me[j][i] = -IminAw->me[i][j];
}
V = m_copy(Vw, V);
LDLfactor(V);
rhs = v_resize(rhs, X->m);
b = v_resize(b, X->m);
for (i = 0; i < X->m; i++) { /* solve Vw X = (I-A) for X -> V-1(I-A) */
rhs = get_col(IminAw, i, rhs);
LDLsolve(V, rhs, b);
set_col(VinvIminAw, i, b);
}
v_free(rhs);
v_free(b);
m_free(V);
if (tmp)
m_free(tmp);
return VinvIminAw;
}
/**
* Compute the whitening matrix W_z for a matrix X.
*
* The whitening matrix, when applied to X, removes
* the first- and second-order statistics; that is,
* the mean and covariances are set to zero and the
* variances are equalized.
*
* @param X mean-subtracted input matrix
* @return whitening matrix W_z
*/
matrix_t * sphere(matrix_t *X)
{
// compute W_z = 2 * Cov(X)^(-1/2)
matrix_t *W_z_temp1 = m_covariance(X);
matrix_t *W_z_temp2 = m_sqrtm(W_z_temp1);
matrix_t *W_z = m_inverse(W_z_temp2);
m_elem_mult(W_z, 2);
// cleanup
m_free(W_z_temp1);
m_free(W_z_temp2);
return W_z;
}
void render(void) {
float m[16],im[16];
create_shadow();
glClearColor(0,0,0,0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(45,4.0 / 3.0,0.5,100);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(camera[0],camera[1],camera[2], mesh[0],mesh[1],mesh[2], 0,0,1);
glLightfv(GL_LIGHT0,GL_POSITION,light);
/* light */
glBegin(GL_POINTS);
glVertex3fv(light);
glEnd();
/* meshes */
glEnable(GL_LIGHTING);
glPushMatrix();
glTranslatef(mesh[0],mesh[1],mesh[2]);
glRotatef(angle,0,0,1);
glRotatef(angle / 3,1,0,0);
glCallList(mesh_id);
glPopMatrix();
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D,texture_id);
glCallList(ground_id);
glDisable(GL_TEXTURE_2D);
glDisable(GL_LIGHTING);
/* shadow */
glEnable(GL_TEXTURE_GEN_S);
glEnable(GL_TEXTURE_GEN_T);
glEnable(GL_TEXTURE_GEN_R);
glEnable(GL_TEXTURE_GEN_Q);
glGetFloatv(GL_MODELVIEW_MATRIX,m);
m_inverse(m,im);
glMatrixMode(GL_TEXTURE);
glLoadIdentity();
glTranslatef(0.5,0.5,0);
glScalef(0.5,0.5,1.0);
glOrtho(-1,1,-1,1,-1,1);
gluLookAt(light[0],light[1],light[2], mesh[0],mesh[1],mesh[2], 0,0,1);
glMultMatrixf(im);
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D,shadow_id);
glEnable(GL_BLEND);
glBlendFunc(GL_DST_COLOR,GL_SRC_COLOR);
glCallList(ground_id);
glDisable(GL_BLEND);
glDisable(GL_TEXTURE_2D);
glLoadIdentity();
glMatrixMode(GL_MODELVIEW);
glDisable(GL_TEXTURE_GEN_S);
glDisable(GL_TEXTURE_GEN_T);
glDisable(GL_TEXTURE_GEN_R);
glDisable(GL_TEXTURE_GEN_Q);
SDL_GL_SwapBuffers();
}
static int reml(VEC *Y, MAT *X, MAT **Vk, int n_k, int max_iter,
double fit_limit, VEC *teta) {
volatile int n_iter = 0;
int i;
volatile double rel_step = DBL_MAX;
VEC *rhs = VNULL;
VEC *dteta = VNULL;
MAT *Vw = MNULL, *Tr_m = MNULL, *VinvIminAw = MNULL;
Vw = m_resize(Vw, X->m, X->m);
VinvIminAw = m_resize(VinvIminAw, X->m, X->m);
rhs = v_resize(rhs, n_k);
Tr_m = m_resize(Tr_m, n_k, n_k);
dteta = v_resize(dteta, n_k);
while (n_iter < max_iter && rel_step > fit_limit) {
print_progress(n_iter, max_iter);
n_iter++;
dteta = v_copy(teta, dteta);
/* fill Vw, calc VinvIminAw, rhs; */
for (i = 0, m_zero(Vw); i < n_k; i++)
ms_mltadd(Vw, Vk[i], teta->ve[i], Vw); /* Vw = Sum_i teta[i]*V[i] */
VinvIminAw = calc_VinvIminAw(Vw, X, VinvIminAw, n_iter == 1);
calc_rhs_Tr_m(n_k, Vk, VinvIminAw, Y, rhs, Tr_m);
/* Tr_m * teta = Rhs; symmetric, solve for teta: */
LDLfactor(Tr_m);
LDLsolve(Tr_m, rhs, teta);
if (DEBUG_VGMFIT) {
printlog("teta_%d [", n_iter);
for (i = 0; i < teta->dim; i++)
printlog(" %g", teta->ve[i]);
printlog("] -(log.likelyhood): %g\n",
calc_ll(Vw, X, Y, n_k));
}
v_sub(teta, dteta, dteta); /* dteta = teta_prev - teta_curr */
if (v_norm2(teta) == 0.0)
rel_step = 0.0;
else
rel_step = v_norm2(dteta) / v_norm2(teta);
} /* while (n_iter < gl_iter && rel_step > fit_limit) */
print_progress(max_iter, max_iter);
if (n_iter == gl_iter)
pr_warning("No convergence after %d iterations", n_iter);
if (DEBUG_VGMFIT) { /* calculate and report covariance matrix */
/* first, update to current est */
for (i = 0, m_zero(Vw); i < n_k; i++)
ms_mltadd(Vw, Vk[i], teta->ve[i], Vw); /* Vw = Sum_i teta[i]*V[i] */
VinvIminAw = calc_VinvIminAw(Vw, X, VinvIminAw, 0);
calc_rhs_Tr_m(n_k, Vk, VinvIminAw, Y, rhs, Tr_m);
m_inverse(Tr_m, Tr_m);
sm_mlt(2.0, Tr_m, Tr_m); /* Var(YAY)=2tr(AVAV) */
printlog("Lower bound of parameter covariance matrix:\n");
m_logoutput(Tr_m);
printlog("# Negative log-likelyhood: %g\n", calc_ll(Vw, X, Y, n_k));
}
m_free(Vw);
m_free(VinvIminAw);
m_free(Tr_m);
v_free(rhs);
v_free(dteta);
return (n_iter < max_iter && rel_step < fit_limit); /* converged? */
}
void Ukf(VEC *omega, VEC *mag_vec, VEC *mag_vec_I, VEC *sun_vec, VEC *sun_vec_I, VEC *Torq_ext, double t, double h, int eclipse, VEC *state, VEC *st_error, VEC *residual, int *P_flag, double sim_time)
{
static VEC *omega_prev = VNULL, *mag_vec_prev = VNULL, *sun_vec_prev = VNULL, *q_s_c = VNULL, *x_prev = VNULL, *Torq_prev, *x_m_o;
static MAT *Q = {MNULL}, *R = {MNULL}, *Pprev = {MNULL};
static double alpha, kappa, lambda, sqrt_lambda, w_m_0, w_c_0, w_i, beta;
static int n_states, n_sig_pts, n_err_states, iter_num, initialize=0;
VEC *x = VNULL, *x_priori = VNULL, *x_err_priori = VNULL, *single_sig_pt = VNULL, *v_temp = VNULL, *q_err_quat = VNULL,
*err_vec = VNULL, *v_temp2 = VNULL, *x_ang_vel = VNULL, *meas = VNULL, *meas_priori = VNULL,
*v_temp3 = VNULL, *x_posteriori_err = VNULL, *x_b_m = VNULL, *x_b_g = VNULL;
MAT *sqrt_P = {MNULL}, *P = {MNULL}, *P_priori = {MNULL}, *sig_pt = {MNULL}, *sig_vec_mat = {MNULL},
*err_sig_pt_mat = {MNULL}, *result = {MNULL}, *result_larger = {MNULL}, *result1 = {MNULL}, *Meas_err_mat = {MNULL},
*P_zz = {MNULL}, *iP_vv = {MNULL}, *P_xz = {MNULL}, *K = {MNULL}, *result2 = {MNULL}, *result3 = {MNULL}, *C = {MNULL};
int update_mag_vec, update_sun_vec, update_omega, i, j;
double d_res;
if (inertia == MNULL)
{
inertia = m_get(3,3);
m_ident(inertia);
inertia->me[0][0] = 0.007;
inertia->me[1][1] = 0.014;
inertia->me[2][2] = 0.015;
}
if (initialize == 0){
iter_num = 1;
n_states = (7+6);
n_err_states = (6+6);
n_sig_pts = 2*n_err_states+1;
alpha = sqrt(3);
kappa = 3 - n_states;
lambda = alpha*alpha * (n_err_states+kappa) - n_err_states;
beta = -(1-(alpha*alpha));
w_m_0 = (lambda)/(n_err_states + lambda);
w_c_0 = (lambda/(n_err_states + lambda)) + (1 - (alpha*alpha) + beta);
w_i = 0.5/(n_err_states +lambda);
initialize = 1;
sqrt_lambda = (lambda+n_err_states);
if(q_s_c == VNULL)
{
q_s_c = v_get(4);
q_s_c->ve[0] = -0.020656;
q_s_c->ve[1] = 0.71468;
q_s_c->ve[2] = -0.007319;
q_s_c->ve[3] = 0.6991;
}
if(Torq_prev == VNULL)
{
Torq_prev = v_get(3);
v_zero(Torq_prev);
}
quat_normalize(q_s_c);
}
result = m_get(9,9);
m_zero(result);
result1 = m_get(n_err_states, n_err_states);
m_zero(result1);
if(x_m_o == VNULL)
{
x_m_o = v_get(n_states);
v_zero(x_m_o);
}
x = v_get(n_states);
v_zero(x);
x_err_priori = v_get(n_err_states);
v_zero(x_err_priori);
x_ang_vel = v_get(3);
v_zero(x_ang_vel);
sig_pt = m_get(n_states, n_err_states);
m_zero(sig_pt);
if (C == MNULL)
{
C = m_get(9, 12);
m_zero(C);
}
if (P_priori == MNULL)
{
P_priori = m_get(n_err_states, n_err_states);
m_zero(P_priori);
}
if (Q == MNULL)
{
Q = m_get(n_err_states, n_err_states);
m_ident(Q);
//
Q->me[0][0] = 0.0001;
Q->me[1][1] = 0.0001;
Q->me[2][2] = 0.0001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.000001;
Q->me[7][7] = 0.000001;
Q->me[8][8] = 0.000001;
Q->me[9][9] = 0.000001;
Q->me[10][10] = 0.000001;
Q->me[11][11] = 0.000001;
}
if( Pprev == MNULL)
{
Pprev = m_get(n_err_states, n_err_states);
m_ident(Pprev);
Pprev->me[0][0] = 1e-3;
Pprev->me[1][1] = 1e-3;
Pprev->me[2][2] = 1e-3;
Pprev->me[3][3] = 1e-3;
Pprev->me[4][4] = 1e-3;
Pprev->me[5][5] = 1e-3;
Pprev->me[6][6] = 1e-4;
Pprev->me[7][7] = 1e-4;
Pprev->me[8][8] = 1e-4;
Pprev->me[9][9] = 1e-3;
Pprev->me[10][10] = 1e-3;
Pprev->me[11][11] = 1e-3;
}
if (R == MNULL)
{
R = m_get(9,9);
m_ident(R);
R->me[0][0] = 0.034;
R->me[1][1] = 0.034;
R->me[2][2] = 0.034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.000012;
R->me[7][7] = 0.000012;
R->me[8][8] = 0.000012;
}
if(eclipse==0)
{
R->me[0][0] = 0.00034;
R->me[1][1] = 0.00034;
R->me[2][2] = 0.00034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;//0.000012;//0.0175;//1e-3;
Q->me[4][4] = 0.0001;//0.0175;//1e-3;
Q->me[5][5] = 0.0001;//0.0175;//1e-3;
Q->me[6][6] = 0.0000000001;//1e-6;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
else
{
R->me[0][0] = 0.34;
R->me[1][1] = 0.34;
R->me[2][2] = 0.34;
R->me[3][3] = 0.0027;
R->me[4][4] = 0.0027;
R->me[5][5] = 0.0027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.0000000001;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
if(omega_prev == VNULL)
{
omega_prev = v_get(3);
v_zero(omega_prev);
}
if(mag_vec_prev == VNULL)
{
mag_vec_prev = v_get(3);
v_zero(mag_vec_prev);
}
if(sun_vec_prev == VNULL)
{
sun_vec_prev = v_get(3);
v_zero(sun_vec_prev);
}
if (err_sig_pt_mat == MNULL)
{
err_sig_pt_mat = m_get(n_err_states, n_sig_pts);
m_zero(err_sig_pt_mat);
}
if(q_err_quat == VNULL)
{
q_err_quat = v_get(4);
// q_err_quat = v_resize(q_err_quat,4);
v_zero(q_err_quat);
}
if(err_vec == VNULL)
{
err_vec = v_get(3);
v_zero(err_vec);
}
v_temp = v_get(9);
v_resize(v_temp,3);
if(x_prev == VNULL)
{
x_prev = v_get(n_states);
v_zero(x_prev);
x_prev->ve[3] = 1;
quat_mul(x_prev,q_s_c,x_prev);
x_prev->ve[4] = 0.0;
x_prev->ve[5] = 0.0;
x_prev->ve[6] = 0.0;
x_prev->ve[7] = 0.0;
x_prev->ve[8] = 0.0;
x_prev->ve[9] = 0.0;
x_prev->ve[10] = 0.0;
x_prev->ve[11] = 0.0;
x_prev->ve[12] = 0.0;
}
sqrt_P = m_get(n_err_states, n_err_states);
m_zero(sqrt_P);
//result = m_resize(result, n_err_states, n_err_states);
result_larger = m_get(n_err_states, n_err_states);
int n, m;
for(n = 0; n < result->n; n++)
{
for(m = 0; m < result->m; m++)
{
result_larger->me[m][n] = result->me[m][n];
}
}
//v_resize(v_temp, n_err_states);
V_FREE(v_temp);
v_temp = v_get(n_err_states);
symmeig(Pprev, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_copy(Pprev, result1);
sm_mlt(sqrt_lambda, result1, result_larger);
catchall(CHfactor(result_larger), printerr(sim_time));
for(i=0; i<n_err_states; i++){
for(j=i+1; j<n_err_states; j++){
result_larger->me[i][j] = 0;
}
}
expandstate(result_larger, x_prev, sig_pt);
sig_vec_mat = m_get(n_states, n_sig_pts);
m_zero(sig_vec_mat);
for(j = 0; j<(n_err_states+1); j++)
{
for(i = 0; i<n_states; i++)
{
if(j==0)
{
sig_vec_mat->me[i][j] = x_prev->ve[i];
}
else if(j>0)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-1];
}
}
}
sm_mlt(-1,result_larger,result_larger);
expandstate(result_larger, x_prev, sig_pt);
for(j = (n_err_states+1); j<n_sig_pts; j++)
{
for(i = 0; i<n_states; i++)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-(n_err_states+1)];
}
}
single_sig_pt = v_get(n_states);
quat_rot_vec(q_s_c, Torq_ext);
for(j=0; j<(n_sig_pts); j++)
{
//v_temp = v_resize(v_temp,n_states);
V_FREE(v_temp);
v_temp = v_get(n_states);
get_col(sig_vec_mat, j, single_sig_pt);
v_copy(single_sig_pt, v_temp);
rk4(t, v_temp, h, Torq_prev);
set_col(sig_vec_mat, j, v_temp);
}
v_copy(Torq_ext, Torq_prev);
x_priori = v_get(n_states);
v_zero(x_priori);
v_resize(v_temp,n_states);
v_zero(v_temp);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp);
if(j == 0)
{
v_mltadd(x_priori, v_temp, w_m_0, x_priori);
}
else
{
v_mltadd(x_priori, v_temp, w_i, x_priori);
}
}
v_copy(x_priori, v_temp);
v_resize(v_temp,4);
quat_normalize(v_temp);//zaroori hai ye
for(i=0; i<4; i++)
{
x_priori->ve[i] = v_temp->ve[i];
}
v_resize(v_temp, n_states);
v_copy(x_priori, v_temp);
v_resize(v_temp, 4);
quat_inv(v_temp, v_temp);
for(i=0; i<3; i++)
{
x_ang_vel->ve[i] = x_priori->ve[i+4];
}
x_b_m = v_get(3);
v_zero(x_b_m);
x_b_g = v_get(3);
v_zero(x_b_g);
/////////////////////////check it!!!!!!!! checked... doesnt change much the estimate
for(i=0; i<3; i++)
{
x_b_m->ve[i] = x_priori->ve[i+7];
x_b_g->ve[i] = x_priori->ve[i+10];
}
v_temp2 = v_get(n_states);
v_zero(v_temp2);
for(j=0; j<n_sig_pts; j++)
{
v_resize(v_temp2, n_states);
get_col( sig_vec_mat, j, v_temp2);
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+4];
}
v_resize(v_temp2, 4);
quat_mul(v_temp2, v_temp, q_err_quat);
v_resize(q_err_quat, n_err_states);
v_sub(err_vec, x_ang_vel, err_vec);
for(i=3; i<6; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-3];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+7];
}
v_sub(err_vec, x_b_m, err_vec);
for(i=6; i<9; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-6];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+10];
}
v_sub(err_vec, x_b_g, err_vec);
for(i=9; i<12; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-9];
}
set_col(err_sig_pt_mat, j, q_err_quat);
if(j==0){
v_mltadd(x_err_priori, q_err_quat, w_m_0, x_err_priori);
}
else{
v_mltadd(x_err_priori, q_err_quat, w_i, x_err_priori);
}
}
v_resize(v_temp,n_err_states);
for (j=0;j<13;j++)
{
get_col(err_sig_pt_mat, j, v_temp);
v_sub(v_temp, x_err_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
}
else{
sm_mlt(w_i, result_larger, result_larger);
}
m_add(P_priori, result_larger, P_priori);
}
symmeig(P_priori, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_priori, Q, P_priori);
v_resize(v_temp,3);
meas = v_get(9);
if (!(is_vec_equal(sun_vec, sun_vec_prev)) /*&& (eclipse==0)*/ ){
update_sun_vec =1;
v_copy(sun_vec, sun_vec_prev);
v_copy(sun_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i = 0; i<3;i++){
meas->ve[i] = v_temp->ve[i];
}
}
else{
update_sun_vec =0;
for(i = 0; i<3;i++){
meas->ve[i] = 0;
}
}
if (!(is_vec_equal(mag_vec, mag_vec_prev)) ){
update_mag_vec =1;
v_copy(mag_vec, mag_vec_prev);
v_copy(mag_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i=3; i<6; i++){
meas->ve[i] = v_temp->ve[i-3];
}
}
else{
update_mag_vec =0;
for(i=3; i<6; i++){
meas->ve[i] = 0;//mag_vec_prev->ve[i-3];
}
}
if (!(is_vec_equal(omega, omega_prev) ) ){
update_omega =1;
v_copy(omega, omega_prev);
v_copy(omega, v_temp);
quat_rot_vec(q_s_c, v_temp);
for(i=6; i<9; i++){
meas->ve[i] = v_temp->ve[i-6];
}
}
else{
update_omega =0;
for(i=6; i<9; i++){
meas->ve[i] = 0;
}
}
v_resize(v_temp, 9);
v_resize(v_temp2, n_states);
v_temp3 = v_get(3);
Meas_err_mat = m_get(9, n_sig_pts);
m_zero(Meas_err_mat);
meas_priori = v_get(9);
v_zero(meas_priori);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp2);
if(update_omega){
for(i=6;i<9;i++){
v_temp->ve[i] = v_temp2->ve[i-2] + x_b_g->ve[i-6];
}
}
else{
for(i=6;i<9;i++){
v_temp->ve[i] = 0;
}
}
v_resize(v_temp2, 4);
if(update_sun_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = sun_vec_I->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp->ve[i] = v_temp3->ve[i];
}
}
else{
for(i=0;i<3;i++){
v_temp->ve[i] = 0;
}
}
if(update_mag_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = mag_vec_I->ve[i];
}
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp3->ve[i] = v_temp3->ve[i] + x_b_m->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=3;i<6;i++){
v_temp->ve[i] = v_temp3->ve[i-3];
}
}
else{
for(i=3;i<6;i++){
v_temp->ve[i] = 0;
}
}
set_col(Meas_err_mat, j, v_temp);
if(j==0){
v_mltadd(meas_priori, v_temp, w_m_0, meas_priori);
}
else{
v_mltadd(meas_priori, v_temp, w_i, meas_priori);
}
}
v_resize(v_temp, 9);
m_resize(result_larger, 9, 9);
m_zero(result_larger);
P_zz = m_get(9, 9);
m_zero(P_zz);
iP_vv = m_get(9, 9);
m_zero(iP_vv);
P_xz = m_get(n_err_states, 9);
m_zero(P_xz);
v_resize(v_temp2, n_err_states);
result1 = m_resize(result1,n_err_states,9);
for (j=0; j<n_sig_pts; j++)
{
get_col( Meas_err_mat, j, v_temp);
get_col( err_sig_pt_mat, j, v_temp2);
v_sub(v_temp, meas_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
get_dyad(v_temp2, v_temp, result1);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
sm_mlt(w_c_0, result1, result1);
}
else{
sm_mlt(w_i, result_larger, result_larger);
sm_mlt(w_i, result1, result1);
}
m_add(P_zz, result_larger, P_zz);
m_add(P_xz, result1, P_xz);
}
symmeig(P_zz, result_larger, v_temp);
i = 0;
for (j=0; j<9; j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_zz, R, P_zz);
m_inverse(P_zz, iP_vv);
K = m_get(n_err_states, 9);
m_zero(K);
m_mlt(P_xz, iP_vv, K);
if(x_posteriori_err == VNULL)
{
x_posteriori_err = v_get(n_err_states);
v_zero(x_posteriori_err);
}
v_resize(v_temp,9);
v_sub(meas, meas_priori, v_temp);
v_copy(v_temp, residual);
mv_mlt(K, v_temp, x_posteriori_err);
v_resize(v_temp2,3);
for(i=0;i<3;i++){
v_temp2->ve[i] = x_posteriori_err->ve[i];
}
for(i=4; i<n_states; i++){
x_prev->ve[i] = (x_posteriori_err->ve[i-1] + x_priori->ve[i]);
}
d_res = v_norm2(v_temp2);
v_resize(v_temp2,4);
if(d_res<=1 /*&& d_res!=0*/){
v_temp2->ve[0] = v_temp2->ve[0];
v_temp2->ve[1] = v_temp2->ve[1];
v_temp2->ve[2] = v_temp2->ve[2];
v_temp2->ve[3] = sqrt(1-d_res);
}
else//baad main daala hai
{
v_temp2->ve[0] = (v_temp2->ve[0])/(sqrt(1+d_res));
v_temp2->ve[1] = (v_temp2->ve[1])/(sqrt(1+d_res));
v_temp2->ve[2] = (v_temp2->ve[2])/(sqrt(1+d_res));
v_temp2->ve[3] = 1/sqrt(1 + d_res);
}
v_resize(x_posteriori_err, n_states);
for(i=(n_states-1); i>3; i--){
x_posteriori_err->ve[i] = x_posteriori_err->ve[i-1];
}
for(i=0; i<4; i++){
x_posteriori_err->ve[i] = v_temp2->ve[i];
}
quat_mul(v_temp2, x_priori, v_temp2);
for(i=0;i<4;i++){
x_prev->ve[i] = v_temp2->ve[i];
}
m_resize(result_larger, n_err_states, 9);
m_mlt(K, P_zz, result_larger);
result2 = m_get(9, n_err_states);
m_transp(K,result2);
m_resize(result1, n_err_states, n_err_states);
m_mlt(result_larger, result2, result1);
v_resize(v_temp, n_err_states);
m_sub(P_priori, result1, Pprev);
symmeig(Pprev, result1 , v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
v_copy(x_prev, v_temp);
v_resize(v_temp,4);
v_copy(x_prev, v_temp2);
v_resize(v_temp2,4);
v_copy(x_prev, x_m_o);
//v_resize(x_m_o, 4);
v_resize(v_temp,3);
quat_inv(q_s_c, v_temp2);
v_copy( x_prev, state);
quat_mul(state, v_temp2, state);
for(i=0; i<3; i++){
v_temp->ve[i] = state->ve[i+4];
}
quat_rot_vec(v_temp2, v_temp);
for(i=0; i<3; i++){
state->ve[i+4] = v_temp->ve[i];
}
v_copy( x_posteriori_err, st_error);
iter_num++;
V_FREE(x);
V_FREE(x_priori);
V_FREE(x_err_priori);
V_FREE(single_sig_pt);
V_FREE(v_temp);
V_FREE(q_err_quat);
V_FREE(err_vec);
V_FREE(v_temp2);
V_FREE(x_ang_vel);
V_FREE(meas);
V_FREE(meas_priori);
V_FREE(v_temp3);
V_FREE(x_posteriori_err);
V_FREE(x_b_m);
V_FREE(x_b_g);
M_FREE(sqrt_P);
M_FREE(P);
M_FREE(P_priori);
M_FREE(sig_pt);
M_FREE(sig_vec_mat);
M_FREE(err_sig_pt_mat);
M_FREE(result);
M_FREE(result_larger);
M_FREE(result1);
M_FREE(Meas_err_mat);
M_FREE(P_zz);
M_FREE(iP_vv);
M_FREE(P_xz);
M_FREE(K);
M_FREE(result2);
M_FREE(result3);
}
void Sy_m( VEC *x, VEC *Torq_ext, VEC *out)
{
static MAT *iI = {MNULL};
MAT *sk_om = {MNULL};
VEC *q, *w, *q_dot, *w_dot, *v_res1, *v_res2;
int i;
if ( x == VNULL || out == VNULL ){
error(E_NULL,"f");
}
if ( x->dim != 13 || out->dim != 13){
error(E_SIZES,"f");
}
q = v_get(4);
for (i=0; i<4; i++)
{
q->ve[i] = x->ve[i];
}
w = v_get(3);
for (i=4; i<7; i++)
{
w->ve[i-4] = x->ve[i];
}
v_res1 = v_get(3);
v_zero(v_res1);
v_res2 = v_get(3);
v_zero(v_res2);
q_dot = v_get(4);
v_zero(q_dot);
w_dot = v_get(3);
v_zero(w_dot);
if(iI == MNULL){
iI = m_get(3,3);
m_zero(iI);
}
sk_om = m_get(4,4);
skew_mat(sk_om, w);
mv_mlt(sk_om, q, q_dot);
sv_mlt(0.5, q_dot, q);
for (i=0; i<4; i++)
{
out->ve[i] = q->ve[i];
}
/*****wd********/
m_resize(sk_om, 3, 3);
mv_mlt(inertia, w, v_res1);
mv_mlt(sk_om, v_res1, v_res2);
v_sub(Torq_ext, v_res2, v_res1);
m_inverse(inertia,iI);
mv_mlt(iI, v_res1, w_dot);
for (i=4; i<7; i++)
{
out->ve[i] = w_dot->ve[i-4];
}
for (i=7; i<13; i++)
{
out->ve[i] = 0;
}
M_FREE(sk_om);
//M_FREE(iI);
V_FREE(q);
V_FREE(w);
V_FREE(q_dot);
V_FREE(w_dot);
V_FREE(v_res1);
V_FREE(v_res2);
}
Var* ff_warp(vfuncptr func, Var* arg)
{
Var *obj = NULL, *xm = NULL, *oval;
float ignore = FLT_MIN;
int i, j;
float* out;
int x, y, n;
int grow = 0;
float m[9];
float* minverse;
float xmax, xmin, ymax, ymin;
float v[3];
int dsize;
const char* options[] = {"nearest", "bilinear", 0};
char* interp = NULL;
float (*interp_f)(float, float, Var*, float);
Alist alist[6];
alist[0] = make_alist("object", ID_VAL, NULL, &obj);
alist[1] = make_alist("matrix", ID_VAL, NULL, &xm);
alist[2] = make_alist("ignore", DV_FLOAT, NULL, &ignore);
alist[3] = make_alist("grow", DV_INT32, NULL, &grow);
alist[4] = make_alist("interp", ID_ENUM, options, &interp);
alist[5].name = NULL;
if (parse_args(func, arg, alist) == 0) return (NULL);
if (obj == NULL) {
parse_error("%s: No object specified\n", func->name);
return (NULL);
}
if (ignore == FLT_MIN) ignore = -32768;
x = GetX(obj);
y = GetY(obj);
n = V_SIZE(xm)[2];
for (j = 0; j < 3; j++) {
for (i = 0; i < 3; i++) {
m[i + j * 3] = extract_float(xm, cpos(i, j, 0, xm));
}
}
xmin = ymin = 0;
xmax = x;
ymax = y;
if (grow) {
/* figure out the size of the output array */
float* out;
minverse = m_inverse(m);
out = vxm(new_v(0, 0), minverse);
xmin = out[0];
xmax = out[0];
ymin = out[1];
ymax = out[1];
free(out);
out = vxm(new_v(x, 0), minverse);
xmin = min(xmin, out[0]);
xmax = max(xmax, out[0]);
ymin = min(ymin, out[1]);
ymax = max(ymax, out[1]);
free(out);
out = vxm(new_v(0, y), minverse);
xmin = min(xmin, out[0]);
xmax = max(xmax, out[0]);
ymin = min(ymin, out[1]);
ymax = max(ymax, out[1]);
free(out);
out = vxm(new_v(x, y), minverse);
xmin = min(xmin, out[0]);
xmax = max(xmax, out[0]);
ymin = min(ymin, out[1]);
ymax = max(ymax, out[1]);
free(out);
xmax = ceil(xmax);
xmin = floor(xmin);
ymax = ceil(ymax);
ymin = floor(ymin);
printf("new array corners:\n");
printf(" %fx%f , %fx%f\n", xmin, ymin, xmax, ymax);
}
if (interp == NULL || !strcmp(interp, "nearest")) {
interp_f = interp_nn;
} else if (!strcmp(interp, "bilinear")) {
interp_f = interp_bilinear;
} else {
parse_error("Invalid interpolation function\n");
return (NULL);
}
dsize = (xmax - xmin) * (ymax - ymin);
out = calloc(dsize, sizeof(float));
oval = newVal(BSQ, xmax - xmin, ymax - ymin, 1, DV_FLOAT, out);
for (j = ymin; j < ymax; j++) {
for (i = xmin; i < xmax; i++) {
v[0] = i + 0.5;
v[1] = j + 0.5;
v[2] = 1;
vxm(v, m);
out[cpos((int)(i - xmin), (int)(j - ymin), 0, oval)] = interp_f(v[0], v[1], obj, ignore);
}
}
return (oval);
}
int Reaching::LinkDistanceToObject(int link, EnvironmentObject *obj, float *dist,
float *point, cart_vec_t& contact_vector){
cart_vec_t& objPos;
cart_vec_t& lowerLinkExtr;
cart_vec_t& upperLinkExtr;
cart_vec_t& v1,v2,linkv,u,vertexPos,v_tmp,tmp3;
CMatrix4_t ref,tmpmat,tmpmat2;
cart_vec_t& s1,s2;
int i,j,k,dir1,dir2,pindex,min_i;
float alldists[12]; //closest distance to each edge
float allpoints[24]; // closest pair of points on each edge
float min_dist;
int s3[3];
for (i=0;i<12;i++){
alldists[i]=FLT_MAX;
}
switch(link){
case 1:
v_clear(upperLinkExtr);
ElbowPosition(pos_angle,lowerLinkExtr);
break;
case 2:
ElbowPosition(pos_angle,upperLinkExtr);
v_copy(pos_cart,lowerLinkExtr);
break;
default:
cout<<"unvalid link number"<< endl;
}
if(!obj){
return 0;
}
// v_sub(obj->solid->m_position, shoulder_abs_pos,objPos);
Abs2LocalRef(obj->GetPosition(), objPos);
// cout<<"ule: ";coutvec(upperLinkExtr);
// cout<<"lle: ";coutvec(lowerLinkExtr);
// coutvec(objPos);
v_sub(lowerLinkExtr,objPos,v1);
v_sub(upperLinkExtr,objPos,v2);
//m_transpose(obj->solid->m_ref.m_orient,ref); //stupid to do it each time
m_copy(obj->solid->m_ref.m_orient,ref); //stupid to do it each time
v_clear(s3);
//checking when two parallel sides must me checked
for(i=0;i<3;i++){
s1[i] = v_dot(v1, &ref[i*4]);
s2[i] = v_dot(v2, &ref[i*4]);
if (s1[i]*s2[i]>=0){
s3[i] = sign(s1[i]+s2[i]);
}
}
// cout << "s3: ";coutvec(s3);
m_copy(ref,tmpmat);
for(i=0;i<3;i++){
if(s3[i]){
v_sub(lowerLinkExtr,upperLinkExtr,linkv);
v_scale(obj->solid->m_size,-0.5,u);
u[i] *=-s3[i];
v_add(objPos,u,vertexPos);
// cout<<"vPos "<<i<<": ";coutvec(vertexPos);
v_scale(&(ref[i*4]),s3[i],&(tmpmat[i*4]));
// cout<<"norm "<<i<<": ";coutvec((tmpmat+i*4));
m_inverse(tmpmat,tmpmat2);
if(v_dot(&(tmpmat[i*4]),linkv)<0){
v_sub(lowerLinkExtr,vertexPos,v_tmp);
*point = 1;
}
else{
v_sub(upperLinkExtr,vertexPos,v_tmp);
*point = 0;
}
// cout<<"linkv ";coutvec(linkv);
// cout <<"pt "<<*point<<endl;
// #ifdef OLD_AV
// v_copy(linkv,(cart_vec_t&)&tmpmat[i*4]);
// m_inverse(tmpmat,tmpmat2);
// v_sub(upperLinkExtr,vertexPos,tmp2);
// tmp3 should contain the intersection coordinates in
// the referential defined by the edges of the surface
v_transform_normal(v_tmp,tmpmat2,tmp3);
// cout<<"tmp3 ";coutvec(tmp3);
if(tmp3[(i+1)%3]>=0 && tmp3[(i+2)%3]>=0 // the link points to the rectangle
&& tmp3[(i+1)%3]<=obj->solid->m_size[(i+1)%3] &&
tmp3[(i+2)%3]<=obj->solid->m_size[(i+2)%3]){
if(tmp3[i]<0){
return 0; // there is a collision
}
else{
*dist = tmp3[i];
v_copy(&(tmpmat[i*4]),contact_vector);
v_scale(contact_vector,*dist,contact_vector);
return 1;
}
}
}
}
// the link does not point to a rectangle -> look for the edges
v_scale(obj->solid->m_size,-0.5,u);
for(i=0;i<3;i++){// each kind of edge
dir1 = s3[(i+1)%3];
dir2 = s3[(i+2)%3];
for(j=0;j<2;j++){
if(dir1 == 0 || dir1==2*j-1){ //edges of this face must be computed
for(k=0;k<2;k++){
if(dir2 == 0 || dir2==2*k-1){ //edges of this face must be computed
v_copy(u,v_tmp);
v_tmp[(i+1)%3]*=-(2*j-1);
v_tmp[(i+2)%3]*=-(2*k-1);
v_add(objPos,v_tmp,vertexPos);
v_scale(&(ref[4*i]),obj->solid->m_size[i],v1); // edge with the right length
pindex = 4*i+2*j+k;
FindSegmentsNearestPoints(vertexPos,v1,upperLinkExtr,linkv,&(allpoints[2*pindex]),&(allpoints[2*pindex+1]),&(alldists[pindex]));
// cout<<"i:"<<i<<" j:"<<j<<" k:"<<k<<"dist "<<alldists[pindex]<<endl;
// cout<<"v1:";coutvec(v1);
// cout<<"ref:";coutvec((&(ref[4*i])));
// cout<<"vP:";coutvec(vertexPos);
}
}
}
}
}
//looking for the min
min_dist = alldists[0];
min_i = 0;
for(i=1;i<12;i++){
if(alldists[i] < min_dist){
min_dist = alldists[i];
min_i = i;
}
}
// returning the min distance
*dist = min_dist;
*point = allpoints[2*min_i+1];
v_scale(linkv,*point,v1);
v_add(upperLinkExtr,v1,v2); // nearest point on link
k = min_i%2; //retrieving the right edge
j = ((min_i-k)%4)/2;
i = min_i/4;
// cout<<"ijk: "<<i<<" "<<j<<" "<<k<<endl;
v_copy(u,v_tmp);
v_tmp[(i+1)%3]*=-(2*j-1);
v_tmp[(i+2)%3]*=-(2*k-1);
v_add(objPos,v_tmp,vertexPos); // starting vertex of the edge
v_scale(&(ref[3*1]),allpoints[2*min_i]*(obj->solid->m_size[min_i]),v1);
v_add(vertexPos,v1,v1); // nearest point on solid
v_sub(v2,v1,contact_vector);
return 1;
}
/*
* n_vars is the number of variables to be considered,
* d is the data array of variables d[0],...,d[n_vars-1],
* pred determines which estimate is required: BLUE, BLUP, or BLP
*/
void gls(DATA **d /* pointer to DATA array */,
int n_vars, /* length of DATA array (to consider) */
enum GLS_WHAT pred, /* what type of prediction is requested */
DPOINT *where, /* prediction location */
double *est /* output: array that holds the predicted values and variances */)
{
GLM *glm = NULL; /* to be copied to/from d */
static MAT *X0 = MNULL, *C0 = MNULL, *MSPE = MNULL, *CinvC0 = MNULL,
*Tmp1 = MNULL, *Tmp2 = MNULL, *Tmp3, *R = MNULL;
static VEC *blup = VNULL, *tmpa = VNULL, *tmpb = VNULL;
volatile unsigned int i, rows_C;
unsigned int j, k, l = 0, row, col, start_i, start_j, start_X, global;
VARIOGRAM *v = NULL;
static enum GLS_WHAT last_pred = GLS_INIT; /* the initial value */
double c_value, *X_ori;
if (d == NULL) { /* clean up */
if (X0 != MNULL) M_FREE(X0);
if (C0 != MNULL) M_FREE(C0);
if (MSPE != MNULL) M_FREE(MSPE);
if (CinvC0 != MNULL) M_FREE(CinvC0);
if (Tmp1 != MNULL) M_FREE(Tmp1);
if (Tmp2 != MNULL) M_FREE(Tmp2);
if (Tmp3 != MNULL) M_FREE(Tmp3);
if (R != MNULL) M_FREE(R);
if (blup != VNULL) V_FREE(blup);
if (tmpa != VNULL) V_FREE(tmpa);
if (tmpb != VNULL) V_FREE(tmpb);
last_pred = GLS_INIT;
return;
}
#ifndef HAVE_SPARSE
if (gl_sparse) {
pr_warning("sparse matrices not supported: compile with --with-sparse");
gl_sparse = 0;
}
#endif
if (DEBUG_COV) {
printlog("we're at %s X: %g Y: %g Z: %g\n",
IS_BLOCK(where) ? "block" : "point",
where->x, where->y, where->z);
}
if (pred != UPDATE) /* it right away: */
last_pred = pred;
assert(last_pred != GLS_INIT);
if (d[0]->glm == NULL) { /* allocate and initialize: */
glm = new_glm();
d[0]->glm = (void *) glm;
} else
glm = (GLM *) d[0]->glm;
glm->mu0 = v_resize(glm->mu0, n_vars);
MSPE = m_resize(MSPE, n_vars, n_vars);
if (pred == GLS_BLP || UPDATE_BLP) {
X_ori = where->X;
for (i = 0; i < n_vars; i++) { /* mu(0) */
glm->mu0->ve[i] = calc_mu(d[i], where);
blup = v_copy(glm->mu0, v_resize(blup, glm->mu0->dim));
where->X += d[i]->n_X; /* shift to next x0 entry */
}
where->X = X_ori; /* ... and set back */
for (i = 0; i < n_vars; i++) { /* Cij(0,0): */
for (j = 0; j <= i; j++) {
v = get_vgm(LTI(d[i]->id,d[j]->id));
MSPE->me[i][j] = MSPE->me[j][i] = COVARIANCE0(v, where, where, d[j]->pp_norm2);
}
}
fill_est(NULL, blup, MSPE, n_vars, est); /* in case of empty neighbourhood */
}
/* xxx */
/*
logprint_variogram(v, 1);
*/
/*
* selection dependent problem dimensions:
*/
for (i = rows_C = 0; i < n_vars; i++)
rows_C += d[i]->n_sel;
if (rows_C == 0) { /* empty selection list(s) */
if (pred == GLS_BLP || UPDATE_BLP)
debug_result(blup, MSPE, pred);
return;
}
for (i = 0, global = 1; i < n_vars && global; i++)
global = (d[i]->sel == d[i]->list && d[i]->n_list == d[i]->n_original);
/*
* global things: enter whenever (a) first time, (b) local selections or
* (c) the size of the problem grew since the last call (e.g. simulation)
*/
if ((glm->C == NULL && glm->spC == NULL) || !global || rows_C > glm->C->m) {
/*
* fill y:
*/
glm->y = get_y(d, glm->y, n_vars);
if (pred != UPDATE) {
if (! gl_sparse) {
glm->C = m_resize(glm->C, rows_C, rows_C);
m_zero(glm->C);
}
#ifdef HAVE_SPARSE
else {
if (glm->C == NULL) {
glm->spC = sp_get(rows_C, rows_C, gl_sparse);
/* d->spLLT = spLLT = sp_get(rows_C, rows_C, gl_sparse); */
} else {
glm->spC = sp_resize(glm->spC, rows_C, rows_C);
/* d->spLLT = spLLT = sp_resize(spLLT, rows_C, rows_C); */
}
sp_zero(glm->spC);
}
#endif
glm->X = get_X(d, glm->X, n_vars);
M_DEBUG(glm->X, "X");
glm->CinvX = m_resize(glm->CinvX, rows_C, glm->X->n);
glm->XCinvX = m_resize(glm->XCinvX, glm->X->n, glm->X->n);
glm->beta = v_resize(glm->beta, glm->X->n);
for (i = start_X = start_i = 0; i < n_vars; i++) { /* row var */
/* fill C, mu: */
for (j = start_j = 0; j <= i; j++) { /* col var */
v = get_vgm(LTI(d[i]->id,d[j]->id));
for (k = 0; k < d[i]->n_sel; k++) { /* rows */
row = start_i + k;
for (l = 0, col = start_j; col <= row && l < d[j]->n_sel; l++, col++) {
if (pred == GLS_BLUP)
c_value = GCV(v, d[i]->sel[k], d[j]->sel[l]);
else
c_value = COVARIANCE(v, d[i]->sel[k], d[j]->sel[l]);
/* on the diagonal, if necessary, add measurement error variance */
if (d[i]->colnvariance && i == j && k == l)
c_value += d[i]->sel[k]->variance;
if (! gl_sparse)
glm->C->me[row][col] = c_value;
#ifdef HAVE_SPARSE
else {
if (c_value != 0.0)
sp_set_val(glm->spC, row, col, c_value);
}
#endif
} /* for l */
} /* for k */
start_j += d[j]->n_sel;
} /* for j */
start_i += d[i]->n_sel;
if (d[i]->n_sel > 0)
start_X += d[i]->n_X - d[i]->n_merge;
} /* for i */
/*
if (d[0]->colnvmu)
glm->C = convert_vmuC(glm->C, d[0]);
*/
if (d[0]->variance_fn) {
glm->mu = get_mu(glm->mu, glm->y, d, n_vars);
convert_C(glm->C, glm->mu, d[0]->variance_fn);
}
if (DEBUG_COV && pred == GLS_BLUP)
printlog("[using generalized covariances: max_val - semivariance()]");
if (! gl_sparse) {
M_DEBUG(glm->C, "Covariances (x_i, x_j) matrix C (lower triangle only)");
}
#ifdef HAVE_SPARSE
else {
SM_DEBUG(glm->spC, "Covariances (x_i, x_j) sparse matrix C (lower triangle only)")
}
#endif
/* check for singular C: */
if (! gl_sparse && gl_cn_max > 0.0) {
for (i = 0; i < rows_C; i++) /* row */
for (j = i+1; j < rows_C; j++) /* col > row */
glm->C->me[i][j] = glm->C->me[j][i]; /* fill symmetric */
if (is_singular(glm->C, gl_cn_max)) {
pr_warning("Covariance matrix (nearly) singular at location [%g,%g,%g]: skipping...",
where->x, where->y, where->z);
m_free(glm->C); glm->C = MNULL; /* assure re-entrance if global */
return;
}
}
/*
* factorize C:
*/
if (! gl_sparse)
LDLfactor(glm->C);
#ifdef HAVE_SPARSE
else {
sp_compact(glm->spC, 0.0);
spCHfactor(glm->spC);
}
#endif
} /* if (pred != UPDATE) */
if (pred != GLS_BLP && !UPDATE_BLP) { /* C-1 X and X'C-1 X, beta */
/*
* calculate CinvX:
*/
tmpa = v_resize(tmpa, rows_C);
for (i = 0; i < glm->X->n; i++) {
tmpa = get_col(glm->X, i, tmpa);
if (! gl_sparse)
tmpb = LDLsolve(glm->C, tmpa, tmpb);
#ifdef HAVE_SPARSE
else
tmpb = spCHsolve(glm->spC, tmpa, tmpb);
#endif
set_col(glm->CinvX, i, tmpb);
}
/*
* calculate X'C-1 X:
*/
glm->XCinvX = mtrm_mlt(glm->X, glm->CinvX, glm->XCinvX); /* X'C-1 X */
M_DEBUG(glm->XCinvX, "X'C-1 X");
if (gl_cn_max > 0.0 && is_singular(glm->XCinvX, gl_cn_max)) {
pr_warning("X'C-1 X matrix (nearly) singular at location [%g,%g,%g]: skipping...",
where->x, where->y, where->z);
m_free(glm->C); glm->C = MNULL; /* assure re-entrance if global */
return;
}
m_inverse(glm->XCinvX, glm->XCinvX);
/*
* calculate beta:
*/
tmpa = vm_mlt(glm->CinvX, glm->y, tmpa); /* X'C-1 y */
glm->beta = vm_mlt(glm->XCinvX, tmpa, glm->beta); /* (X'C-1 X)-1 X'C-1 y */
V_DEBUG(glm->beta, "beta");
M_DEBUG(glm->XCinvX, "Cov(beta), (X'C-1 X)-1");
M_DEBUG(R = get_corr_mat(glm->XCinvX, R), "Corr(beta)");
} /* if pred != GLS_BLP */
} /* if redo the heavy part */
int main( void) {
FILE *fp;
MATRIX_T *a, *b, *c, *d, *inverse, *test, *x, *ainv;
double D;
/* initialize all MATRIX_T pointers to NULL */
a = NULL;
b = NULL;
c = NULL;
d = NULL;
inverse = NULL;
test = NULL;
x = NULL;
ainv = NULL;
/* it is good practice to reset the error code
before doing matrix calculations */
m_reseterr();
/* open matrix file to initialize matrix variables */
if ((fp = fopen("mtest1.mat","r"))==NULL) {
printf("cannot open file mtest1.mat\n");
exit(0);
}
/* use m_printf functions here */
/* test matrix addition */
a = m_fnew( fp);
m_printf( "\n# matrix a from file: mtest1.mat", "%6.2f", a);
b = m_fnew( fp);
m_printf( "\n# matrix b from: mtest1.mat", "%6.2f", b);
c = m_new( 3, 2);
c = m_add( c, a, b);
m_printf( "\n# sum of a and b", "%6.2f", c);
/* test matrix subtraction */
c = m_sub( c, a, b);
m_printf( "\n# difference of a and b", "%6.2f", c);
/* change to using comma separated format output */
/* multiply matrix by a constant */
printf( "\ncomma separated format: matrix a\n");
m_fputcsv(stdout,a);
printf( "\ncomma separated format: matrix c\n");
m_fputcsv(stdout,c);
m_mupconst( c, 2.5, a);
printf( "\ncsv format: 2.5 times matrix c\n");
m_fputcsv(stdout,c);
/* find maximum element in matrix */
printf( "\nmax element in c is %f\n", m_max_abs_element(c));
/* test euclidean norm */
d = m_fnew( fp);
printf( "\ncsv format: matrix d\n");
m_fputcsv(stdout,d);
printf( "\neuclidean norm of d is %f\n", m_e_norm( d));
/* test assignment of identity matrix to a square matrix */
inverse = m_new( d->rows, d->cols);
m_assign_identity( inverse);
m_printf( "\nidentity matrix", "%6.2f", inverse);
/* test matrix inversion */
inverse = m_inverse( inverse, d, &D, 0.0001);
test = m_new(d->rows,d->cols);
test = m_mup( test, d, inverse);
m_printf( "\nmatrix d", "%6.2f", d);
m_printf( "\nmatrix inverse", "%6.2f", inverse);
m_printf( "\nproduct of d and d-inverse", "%6.2f", test);
/* test solution of linear equations */
/* start by getting new values for matrices a and b
note: b is a 1 by n vector (as is x) */
if ((a = m_fnew( fp)) == NULL) exit(0);
if ((b = m_fnew( fp)) == NULL) exit(0);
if ((x = m_new(b->rows,b->cols)) == NULL) exit(0);
printf("\ncsv: a\n");
m_fputcsv(stdout,a);
printf("\ncsv: b\n");
m_fputcsv(stdout,b);
x = m_solve( x, a, b, 0.0000001);
printf("\n\nx=ab\ncsv: solution x\n");
m_fputcsv(stdout,x);
/* close files and clean up */
fclose(fp);
m_free(a);
m_free(b);
m_free(c);
m_free(d);
m_free(inverse);
m_free(test);
m_free(x);
m_free(ainv);
}
