// returns probability of success of group playing CA1 (us vs nature)
inline void update_XP_CA1(int g)
/*
* g: group index of a group in a polity
*/
{
double xx;
X[g] = get_X(g); // update group effort
xx = pow(X[g], Beta);
P[g] = xx/( xx + X0_Be ); // update probability of success
}
/*!
Print to stdout the values of the current visual feature \f$ s \f$.
\param select : Selection of a subset of the possible 3D point
feature coordinates.
- To print all the three coordinates used as features use
vpBasicFeature::FEATURE_ALL.
- To print only one of the coordinate
feature \f$(X,Y,Z)\f$ use one of the
corresponding function selectX(), selectX() or selectZ().
\code
vpPoint point;
// Creation of the current feature s
vpFeaturePoint3D s;
s.buildFrom(point);
s.print(); // print all the 3 components of the translation feature
s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
s.print(vpFeaturePoint3D::selectZ()); // print only the Z component
\endcode
*/
void
vpFeaturePoint3D::print(const unsigned int select ) const
{
std::cout <<"Point3D: " ;
if (vpFeaturePoint3D::selectX() & select )
std::cout << " X=" << get_X() ;
if (vpFeaturePoint3D::selectY() & select )
std::cout << " Y=" << get_Y() ;
if (vpFeaturePoint3D::selectZ() & select )
std::cout << " Z=" << get_Z() ;
std::cout <<std::endl ;
}
void make_test_predict(bool async)
{
auto estimator = train_estimator();
estimator->predicted_quantiles(mkcol({0.5, 0.9}));
const auto X = get_X();
const auto threads = async ? 3 : 1;
const auto y_resp = estimator->predict(X, threads);
assert_equal(X.n_rows, y_resp.n_rows, SPOT);
assert_equal(2u, y_resp.n_cols, SPOT);
auto y_exp = matrix_t(X.n_rows, 2u);
for (std::size_t i=0; i<X.n_rows; ++i) {
y_exp.row(i) = mkrow({5.5, 7.5});
}
assert_equal_containers(y_exp, y_resp, SPOT);
}
void update_XP_CA2(int *g, int ng)
/* g: array of group indices within polity of groups chosen
* ng: no. of groups chosen or selected for playing us vs them game
*/
{
int i;
double sum;
for(sum = 0, i = ng; i--;){
X[g[i]] = get_X(g[i]); // update group effort for group g[i]
sum += pow(X[g[i]], Beta);
}
sum = MAX(sum, 0.001);
for(i = ng; i--;){
P[g[i]] = pow(X[g[i]], Beta) / sum; // update probability of success of
}
}
void make_test_train(bool async)
{
auto estimator = train_estimator(async);
const auto centers = estimator->get_trained_centers();
const auto exp_centers = mkcol({5.5, 8});
assert_equal_containers(exp_centers, centers, SPOT);
const auto& models = estimator->get_models();
assert_equal(2u, models.size());
const auto X = get_X();
assert_equal_containers(X, models[0]->train_X, SPOT);
assert_equal_containers(X, models[1]->train_X, SPOT);
const auto target1 = mkcol({dyes, dyes, dno, dno, dno});
const auto target2 = mkcol({dno, dno, dyes, dyes, dyes});
assert_equal_containers(target1, models[0]->train_y, SPOT);
assert_equal_containers(target2, models[1]->train_y, SPOT);
}
/*!
Compute and return the interaction matrix \f$ L \f$ associated to a subset
of the possible 3D point features \f$(X,Y,Z)\f$ that
represent the 3D point coordinates expressed in the camera frame.
\f[
L = \left[
\begin{array}{rrrrrr}
-1 & 0 & 0 & 0 & -Z & Y \\
0 & -1 & 0 & Z & 0 & -X \\
0 & 0 & -1 & -Y & X & 0 \\
\end{array}
\right]
\f]
\param select : Selection of a subset of the possible 3D point coordinate
features.
- To compute the interaction matrix for all the three
subset features \f$(X,Y,Z)\f$ use vpBasicFeature::FEATURE_ALL. In
that case the dimension of the interaction matrix is \f$ [3 \times
6] \f$
- To compute the interaction matrix for only one of the
subset (\f$X, Y,Z\f$) use
one of the corresponding function selectX(), selectY() or
selectZ(). In that case the returned interaction matrix is \f$ [1
\times 6] \f$ dimension.
\return The interaction matrix computed from the 3D point coordinate
features.
The code below shows how to compute the interaction matrix
associated to the visual feature \f$s = X \f$.
\code
vpPoint point;
...
// Creation of the current feature s
vpFeaturePoint3D s;
s.buildFrom(point);
vpMatrix L_X = s.interaction( vpFeaturePoint3D::selectX() );
\endcode
The code below shows how to compute the interaction matrix
associated to the \f$s = (X,Y) \f$
subset visual feature:
\code
vpMatrix L_XY = s.interaction( vpFeaturePoint3D::selectX() | vpFeaturePoint3D::selectY() );
\endcode
L_XY is here now a 2 by 6 matrix. The first line corresponds to
the \f$ X \f$ visual feature while the second one to the \f$
Y \f$ visual feature.
It is also possible to build the interaction matrix from all the
3D point coordinates by:
\code
vpMatrix L_XYZ = s.interaction( vpBasicFeature::FEATURE_ALL );
\endcode
In that case, L_XYZ is a 3 by 6 interaction matrix where the last
line corresponds to the \f$ Z \f$ visual feature.
*/
vpMatrix
vpFeaturePoint3D::interaction(const unsigned int select)
{
vpMatrix L ;
L.resize(0,6) ;
if (deallocate == vpBasicFeature::user)
{
for (unsigned int i = 0; i < nbParameters; i++)
{
if (flags[i] == false)
{
switch(i){
case 0:
vpTRACE("Warning !!! The interaction matrix is computed but X was not set yet");
break;
case 1:
vpTRACE("Warning !!! The interaction matrix is computed but Y was not set yet");
break;
case 2:
vpTRACE("Warning !!! The interaction matrix is computed but Z was not set yet");
break;
default:
vpTRACE("Problem during the reading of the variable flags");
}
}
}
resetFlags();
}
double X = get_X() ;
double Y = get_Y() ;
double Z = get_Z() ;
if (vpFeaturePoint3D::selectX() & select )
{
vpMatrix Lx(1,6) ; Lx = 0;
Lx[0][0] = -1 ;
Lx[0][1] = 0 ;
Lx[0][2] = 0 ;
Lx[0][3] = 0 ;
Lx[0][4] = -Z ;
Lx[0][5] = Y ;
L = vpMatrix::stackMatrices(L,Lx) ;
}
if (vpFeaturePoint3D::selectY() & select )
{
vpMatrix Ly(1,6) ; Ly = 0;
Ly[0][0] = 0 ;
Ly[0][1] = -1 ;
Ly[0][2] = 0 ;
Ly[0][3] = Z ;
Ly[0][4] = 0 ;
Ly[0][5] = -X ;
L = vpMatrix::stackMatrices(L,Ly) ;
}
if (vpFeaturePoint3D::selectZ() & select )
{
vpMatrix Lz(1,6) ; Lz = 0;
Lz[0][0] = 0 ;
Lz[0][1] = 0 ;
Lz[0][2] = -1 ;
Lz[0][3] = -Y ;
Lz[0][4] = X ;
Lz[0][5] = 0 ;
L = vpMatrix::stackMatrices(L,Lz) ;
}
return L ;
}
VARIOGRAM *reml_sills(DATA *data, VARIOGRAM *vp) {
int i, j, k;
MAT **Vk = NULL, *X = MNULL;
VEC *Y = VNULL, *init = VNULL;
DPOINT *dpa, *dpb;
double dx, dy = 0.0, dz = 0.0, dzero2;
if (data == NULL || vp == NULL)
ErrMsg(ER_NULL, "reml()");
select_at(data, (DPOINT *) NULL);
if (vp->n_models <= 0)
ErrMsg(ER_VARNOTSET, "reml: please define initial variogram model");
/*
* create Y, X, Vk's only once:
*/
Y = get_y(&data, Y, 1);
X = get_X(&data, X, 1);
Vk = (MAT **) emalloc(vp->n_models * sizeof(MAT));
init = v_resize(init, vp->n_models);
for (i = 0; i < vp->n_models; i++) {
init->ve[i] = vp->part[i].sill; /* remember init. values for updating */
vp->part[i].sill = 1;
Vk[i] = m_resize(MNULL, X->m, X->m);
}
dzero2 = gl_zero * gl_zero;
for (i = 0; i < data->n_list; i++) {
for (j = 0; j < vp->n_models; j++) /* fill diagonals */
Vk[j]->me[i][i] = Covariance(vp->part[j], 0.0, 0.0, 0.0);
for (j = 0; j < i; j++) { /* off-diagonal elements: */
dpa = data->list[i];
dpb = data->list[j];
/*
* if different points coincide on a locations, shift them,
* or the covariance matrix will become singular
*/
dx = dpa->x - dpb->x;
dy = dpa->y - dpb->y;
dz = dpa->z - dpb->z;
if (data->pp_norm2(dpa, dpb) < dzero2) {
if (data->mode & X_BIT_SET)
dx = (dx >= 0 ? gl_zero : -gl_zero);
if (data->mode & Y_BIT_SET)
dy = (dy >= 0 ? gl_zero : -gl_zero);
if (data->mode & Z_BIT_SET)
dz = (dz >= 0 ? gl_zero : -gl_zero);
}
for (k = 0; k < vp->n_models; k++)
Vk[k]->me[i][j] = Vk[k]->me[j][i] =
Covariance(vp->part[k], dx, dy, dz);
}
}
if (reml(Y, X, Vk, vp->n_models, gl_iter, gl_fit_limit, init))
vp->ev->refit = 0;
else /* on convergence */
pr_warning("no convergence while fitting variogram");
for (i = 0; i < vp->n_models; i++)
vp->part[i].sill = init->ve[i];
update_variogram(vp);
if (DEBUG_VGMFIT)
logprint_variogram(vp, 1);
for (i = 0; i < vp->n_models; i++)
m_free(Vk[i]);
efree(Vk);
m_free(X);
v_free(Y);
v_free(init);
return vp;
}
void main()
{
unsigned int x1, x2;
unsigned int y1, y2;
unsigned int color = White;
TFT_Initial();
CLR_Screen(Black);
for(x1 = 0; x1 < 40; x1 ++)
for(y1 = 0; y1 < 40; y1 ++)
Put_pixel(x1, y1, Blue);
for(x1 = 0; x1 < 40; x1 ++)
for(y1 = 40; y1 < 80; y1 ++)
Put_pixel(x1, y1, White);
for(x1 = 0; x1 < 40; x1 ++)
for(y1 = 80; y1 < 120; y1 ++)
Put_pixel(x1, y1, Red);
for(x1 = 0; x1 < 40; x1 ++)
for(y1 = 120; y1 < 160; y1 ++)
Put_pixel(x1, y1, Magenta);
for(x1 = 0; x1 < 40; x1 ++)
for(y1 = 160; y1 < 200; y1 ++)
Put_pixel(x1, y1, Green);
for(x1 = 0; x1 < 40; x1 ++)
for(y1 = 200; y1 < 240; y1 ++)
Put_pixel(x1, y1, Cyan);
for(x1 = 0; x1 < 40; x1 ++)
for(y1 = 240; y1 < 280; y1 ++)
Put_pixel(x1, y1, Yellow);
for(y1 = 0; y1<320; y1 ++) {
Put_pixel(42, y1, 0x0FF2);
Put_pixel(43, y1, 0xD621);
}
while(1) {
if(!Penirq) {
x1 = get_X();
x2 = get_X();
y1 = get_Y();
y2 = get_Y();
if(abs(x1-x2)<2 && abs(y1-y2)<2) {
x1 = (x1+x2)/2;
y1 = (y1+y2)/2;
y1 = 320 - y1;
if(x1 < 41) {
if(y1<41)
color = Blue;
else if(y1<81)
color = White;
else if(y1<121)
color = Red;
else if(y1<161)
color = Magenta;
else if(y1<201)
color = Green;
else if(y1<241)
color = Cyan;
else if(y1<281)
color = Yellow;
else {
LCD_SetPos(44, 240, 0, 320);
for (y1 = 0; y1 < 320; y1 ++) {
for (x1 = 44; x1 < 240; x1 ++)
Write_Data_U16(Black);
}
}
} else {
Put_pixel(x1, y1, color);
}
}
}
}
}
/*
* 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 = MNULL, *R = MNULL;
static VEC *blup = VNULL, *tmpa = VNULL, *tmpb = VNULL;
PERM *piv = PNULL;
volatile unsigned int i, rows_C;
unsigned int j, k, l = 0, row, col, start_i, start_j, start_X, global,
one_nbh_empty;
VARIOGRAM *v = NULL;
static enum GLS_WHAT last_pred = GLS_INIT; /* the initial value */
double c_value, *X_ori;
int info;
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;
}
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));
ME(MSPE, i, j) = ME(MSPE, 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, one_nbh_empty = 0; i < n_vars; i++) {
rows_C += d[i]->n_sel;
if (d[i]->n_sel == 0)
one_nbh_empty = 1;
}
if (rows_C == 0 /* all selection lists empty */
|| one_nbh_empty == 1) { /* one selection list empty */
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
&& d[i]->n_list == d[i]->n_sel);
/*
* 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 || !global || rows_C > glm->C->m) {
/*
* fill y:
*/
glm->y = get_y(d, glm->y, n_vars);
if (pred != UPDATE) {
glm->C = m_resize(glm->C, rows_C, rows_C);
if (gl_choleski == 0) /* use LDL' decomposition, allocate piv: */
piv = px_resize(piv, rows_C);
m_zero(glm->C);
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;
ME(glm->C, col, row) = c_value; /* fill upper */
if (col != row)
ME(glm->C, row, col) = c_value; /* fill all */
} /* 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()]");
M_DEBUG(glm->C, "Covariances (x_i, x_j) matrix C (upper triangle)");
/*
* factorize C:
*/
CHfactor(glm->C, piv, &info);
if (info != 0) { /* singular: */
pr_warning("Covariance matrix 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 */
P_FREE(piv);
return;
}
if (piv == NULL)
M_DEBUG(glm->C, "glm->C, Choleski decomposed:")
else
M_DEBUG(glm->C, "glm->C, LDL' decomposed:")
} /* if (pred != UPDATE) */
void test_f1_bullet3() {
A<X> a0 = f1(get_X());
}
/*
* 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 */
void reglage_odometrie()
{
delay_ms(2000);
while(!SYS_JACK);
COULEUR = couleur_depart();
EVITEMENT_ADV_ARRIERE = OFF;
EVITEMENT_ADV_AVANT = OFF;
init_position_robot(0, 0, 0);
// faire_des_tours(64);
// faire_des_tours(-32);
// rejoindre(2000, 0, MARCHE_AVANT, 50);
// delay_ms(30000);
// carre(MARCHE_AVANT);
delay_ms(10000);
PutsUART(UART_XBEE, "\n\n\n\r X : ");
PutLongUART((int32_t) get_X());
PutcUART(UART_XBEE, '.');
PutcUART(UART_XBEE, ((uint8_t) ((int32_t) ((double) get_X() * 10)) - (((int32_t) get_X()) * 10)) + 48);
PutsUART(UART_XBEE, " Y : ");
PutLongUART((int32_t) get_Y());
PutcUART(UART_XBEE, '.');
PutcUART(UART_XBEE, ((uint8_t) ((int32_t) ((double) (get_Y() * 10))) - (((int32_t) get_Y()) * 10)) + 48);
PutsUART(UART_XBEE, " Teta : ");
PutLongUART((int32_t) get_orientation());
PutcUART(UART_XBEE, '.');
PutcUART(UART_XBEE, ((uint8_t) ((int32_t) ((double) (get_orientation() * 10))) - (((int32_t) get_orientation()) * 10)) + 48);
/*
rejoindre(0, 0, MARCHE_AVANT, 100);
trapeze(MARCHE_AVANT);
trapeze(MARCHE_AVANT);
trapeze(MARCHE_AVANT);
trapeze(MARCHE_AVANT);
trapeze(MARCHE_AVANT);
* */
while(1);
// TIMER_DEBUG = ACTIVE;
// init_position_robot(0, 0, 0);
// //Horraire
// rejoindre(2000, 0, MARCHE_AVANT, 50);
// orienter(90, 50);
// rejoindre(300, 0, MARCHE_AVANT, 50);
// orienter(-90, 50);
// rejoindre(2000, 0, MARCHE_AVANT, 50);
// orienter(90, 50);
// rejoindre(300, 0, MARCHE_AVANT, 50);
// orienter (-90, 50);
// rejoindre(2000, 0, MARCHE_AVANT, 50);
// orienter(90, 50);
// rejoindre(300, 0, MARCHE_AVANT, 50);
// orienter (-90, 50);
// rejoindre(2000, 0, MARCHE_AVANT, 50);
// orienter(90, 50);
// rejoindre(300, 0, MARCHE_AVANT, 50);
// orienter (-90, 50);
// rejoindre(2000, 0, MARCHE_AVANT, 50);
// orienter(90, 50);
// rejoindre(300, 0, MARCHE_AVANT, 50);
// orienter (-90, 50);
// rejoindre(2000, 0, MARCHE_AVANT, 50);
// orienter(90, 50);
// rejoindre(300, 0, MARCHE_AVANT, 50);
// orienter(-90, 50);
// Anti horaire
// rejoindre(2000, 0, MARCHE_AVANT, 50);
// orienter(-90, 50);
// rejoindre(300, 0, MARCHE_AVANT, 50);
// orienter(90, 50);
// rejoindre(2000, 0, MARCHE_AVANT, 50);
// orienter(-90, 50);
// rejoindre(300, 0, MARCHE_AVANT, 50);
// orienter (90, 50);
// rejoindre(2000, 0, MARCHE_AVANT, 50);
// orienter(-90, 50);
// rejoindre(300, 0, MARCHE_AVANT, 50);
// orienter (90, 50);
// rejoindre(2000, 0, MARCHE_AVANT, 50);
// orienter(-90, 50);
// rejoindre(300, 0, MARCHE_AVANT, 50);
// orienter (90, 50);
// rejoindre(2000, 0, MARCHE_AVANT, 50);
// orienter(-90, 50);
// rejoindre(300, 0, MARCHE_AVANT, 50);
// orienter (90, 50);
// rejoindre(2000, 0, MARCHE_AVANT, 50);
// orienter(-90, 50);
// rejoindre(300, 0, MARCHE_AVANT, 50);
// orienter(90, 50);
// rejoindre(500, 0, MARCHE_AVANT, 100);
// rejoindre(2000, 0, MARCHE_AVANT, 100);
// rejoindre(300, 0, MARCHE_AVANT, 100);
// rejoindre(2000, 0, MARCHE_AVANT, 100);
// rejoindre(300, 0, MARCHE_AVANT, 100);
// rejoindre(2000, 0, MARCHE_AVANT, 100);
// rejoindre(300, 0, MARCHE_AVANT, 100);
// rejoindre(2000, 0, MARCHE_AVANT, 100);
// rejoindre(300, 0, MARCHE_AVANT, 100);
// rejoindre(2000, 0, MARCHE_AVANT, 100);
// rejoindre(300, 0, MARCHE_AVANT, 100);
// rejoindre(2000, 0, MARCHE_AVANT, 100);
// rejoindre(300, 0, MARCHE_AVANT, 100);
// rejoindre(2000, 0, MARCHE_AVANT, 100);
// rejoindre(300, 0, MARCHE_AVANT, 100);
// rejoindre(500, 0, MARCHE_AVANT, 100);
// TIMER_DEBUG = DESACTIVE;
}
