static void nls_kickstart (MODEL *pmod, DATASET *dset,
double *b0, double *by1)
{
int list[4];
if (b0 != NULL) {
list[0] = 3;
list[1] = 1;
list[2] = 0;
list[3] = 2;
} else {
list[0] = 2;
list[1] = 1;
list[2] = 2;
}
*pmod = lsq(list, dset, OLS, OPT_A | OPT_Z);
if (!pmod->errcode) {
if (b0 != NULL) {
*b0 = pmod->coeff[0];
*by1 = pmod->coeff[1];
} else {
*by1 = pmod->coeff[0];
}
if (*by1 >= 1.0) {
*by1 = 0.95;
}
}
clear_model(pmod);
}
/* estimate receiver velocity ------------------------------------------------*/
static void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
const nav_t *nav, const prcopt_t *opt, sol_t *sol,
const double *azel, const int *vsat)
{
double x[4]={0},dx[4],Q[16],*v,*H;
int i,j,nv;
trace(3,"estvel : n=%d\n",n);
v=mat(n,1); H=mat(4,n);
for (i=0;i<MAXITR;i++) {
/* doppler residuals */
if ((nv=resdop(obs,n,rs,dts,nav,sol->rr,x,azel,vsat,v,H))<4) {
break;
}
/* least square estimation */
if (lsq(H,v,4,nv,dx,Q)) break;
for (j=0;j<4;j++) x[j]+=dx[j];
if (norm(dx,4)<1E-6) {
for (i=0;i<3;i++) sol->rr[i+3]=x[i];
break;
}
}
free(v); free(H);
}
static int umidas_ols (MODEL *pmod,
const int *list,
DATASET *dset,
midas_info *m,
int nmidas,
gretlopt opt)
{
int *biglist;
int j;
biglist = make_midas_biglist(list, m, nmidas);
if (biglist == NULL) {
return E_ALLOC;
} else {
*pmod = lsq(biglist, dset, OLS, opt | OPT_Z);
free(biglist);
}
for (j=0; j<nmidas; j++) {
/* we're done with these lists now */
if (!m[j].prelag) {
free(m[j].laglist);
}
m[j].laglist = NULL;
}
return pmod->errcode;
}
Matrix* makeTranceMT(Image *im, Image* im2,char *imName, char *imName2){
Matrix *mt, *ans;
int i,nm,snm;
int match[999][2],ansAry[100][4];
int x1[30][2], N1=30, x2[30][2], N2=30;
TKfilter(x1, imName);
TKfilter(x2, imName2);
mt=MatrixAlloc(N1,N2);
calcSSDtable(mt,im,x1,N1,im2,x2,N2);
// nm = greedyMethod(match,mt,im,x1,N1,im2,x2,N2);
nm = matchMethod2(match,mt,im,x1,N1,im2,x2,N2);
/* for(i = 0; i < nm; i++)
printf("%d:(%d,%d,%d,%d)\n",i,x1[match[i][0]][0],x1[match[i][0]][1],
x2[match[i][1]][0],x2[match[i][1]][1]);*/
snm = ransacMethod(N1, ansAry, match, x1, x2);
double xy[snm][2], uv[snm][2];
for(i = 0; i < snm; i++) {
xy[i][0] = ansAry[i][0];
xy[i][1] = ansAry[i][1];
uv[i][0] = ansAry[i][2];
uv[i][1] = ansAry[i][3];
printf("(%f %f %f %f)\n", xy[i][0],xy[i][1],uv[i][0],uv[i][1]);
}
ans = MatrixAlloc(1,8);
printf("-----------------%d\n",snm);
ans = lsq(snm,xy,uv);
return ans;
}
/* lsq() */
void utest5(void)
{
int i;
double x[3],Q[9];
lsq(H,y,3,4,x,Q);
for (i=0;i<3;i++) assert(fabs(x[i]-xs[i])<1E-9);
for (i=0;i<9;i++) assert(fabs(Q[i]-Qs[i])<1E-9);
printf("%s utest5 : OK\n",__FILE__);
}
void ex_ddom()
{
struct item *p, *q;
int a, b, m, n, o, *in;
data *d1, *dmn, *dn1, *dn2, *dn3, *dm;
char* al;
p = fetch2();
q = sp[-2];
if (p->itemType != DA || q->itemType != DA)
error(ERR_domain, "domino - incorrect types");
if ((p->rank != 1 && p->rank != 2) || q->rank != 2)
error(ERR_rank, "domino - unexpected ranks");
m = q->dim[0];
n = q->dim[1];
if (m < n || m != p->dim[0])
error(ERR_rank, "domino - mismatch");
o = 1;
if (p->rank == 2)
o = p->dim[1];
al = (char*)alloc(n * (SINT + SDAT * m + SDAT * 3) + SDAT * m);
if (al == 0)
error(ERR, "domino - unable to allocate memory");
dmn = (data*)al;
dn1 = dmn + m * n;
dn2 = dn1 + n;
dn3 = dn2 + n;
dm = dn3 + n;
in = (int*)(dm + m);
d1 = q->datap;
for (b = 0; b < m; b++) {
for (a = 0; a < n; a++)
dmn[a * m + b] = *d1++;
}
a = lsq(dmn, dn1, dn2, dn3, dm, in, m, n, o, p->datap, q->datap);
aplfree((int*)dmn);
if (a)
error(ERR, "domino - could not solve");
sp--;
pop();
*sp++ = p;
p->dim[0] = n;
p->size = n * o;
}
/* estimate receiver velocity ------------------------------------------------*/
static void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
const nav_t *nav, const prcopt_t *opt, sol_t *sol,
const double *azel, const int *vsat)
{
double x[4]={0},dx[4],Q[16],*v,*H;
int i,j,nv;
trace(3,"estvel : n=%d\n",n);
v=mat(n,1); H=mat(4,n);
fprintf(output, " Begin velocity calculation\n");
for (i=0;i<MAXITR;i++) {
fprintf(output, " Begin iteration %i\n", i);
/* doppler residuals */
if ((nv=resdop(obs,n,rs,dts,nav,sol->rr,x,azel,vsat,v,H))<4) {
break;
}
/* least square estimation */
if (lsq(H,v,4,nv,dx,Q)) break;
fprintf(output, " Results of least square estimation: dx1 = %f, dx2 = %f, dx3 = %f, dx4 = %f\n", dx[0], dx[1], dx[2], dx[3]);
for (j=0;j<4;j++) x[j]+=dx[j];
fprintf(output, " Vector V after update: V1=%f, V2 = %f, V3 = %f, V4 = %f\n", x[0], x[1], x[2] ,x[3]);
fprintf(output, " norm(dx) = %f\n", norm(dx, 4));
if (norm(dx,4)<1E-6) {
for (i=0;i<3;i++) sol->rr[i+3]=x[i];
for (i=0;i<3;i++) sol->qvr[i] = (float)Q[i+i*4];
sol->qvr[3] = (float)Q[1]; /* cov vxvy */
sol->qvr[4] = (float)Q[2+4]; /* cov vyvz */
sol->qvr[5] = (float)Q[2]; /* cov vzvx */
break;
}
}
free(v); free(H);
fprintf(output, " End velocity calculation\n");
}
/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg, rtk_t *rtk)
{
double x[NX]={0},dx[NX],Q[NX*NX],sig;
int i,j,k,info,stat,nv,ns;
double rms = 0; //added by yuan;
double RMS = 0;
int nv_ = 0; double temp = 0; int kk = 0;
double v[(MAXOBS_+4)*1];double H[(MAXOBS_+4)*135];double var[MAXOBS_+4*1];
for (i=0;i<3;i++) x[i]=sol->rr[i];
for (i = 0; i < MAXITR; i++) {
/* pseudorange residuals */
nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
&ns,rtk);
if (nv<4) {
//sprintf(msg,"lack of valid sats ns=%d",nv);
break;
}
//added by yuan, ̽��ϴ�ֲ���н�ȷ�������������;
if ( (rtk->counter>=1) && (i>=1) ){
for (j = 0; j < nv;j++){
if (fabs(v[j]) != 0.0){
rms += v[j];
nv_++;
}
}
rms = rms / nv_;
for (j = 0; j < nv_; j++){
RMS += (v[j] - rms)*(v[j] - rms);
}
RMS = sqrt(RMS/nv_); //�в�ľ�����;
if (RMS>2.5){
temp = fabs(v[0] - rms);
for (j = 1; j < nv_;j++){
if (temp <= fabs(v[j] - rms)){
temp = fabs(v[j] - rms);
kk = j;
}
else{
continue;
}
}
opt->exsats[rtk->sat_[kk] - 1] = 1;
}
rms = 0; RMS = 0; nv_ = 0; temp = 0; kk = 0;
//nv = nv - 1; //����һ������;
}
if (nv < NX) {
//sprintf(msg, "lack of valid sats ns=%d", nv);
break;
}
/* weight by variance */
for (j = 0; j < nv; j++) {
sig = sqrt(var[j]);
v[j] /= sig;
for (k = 0; k < NX; k++) H[k + j*NX] /= sig;
}
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
//sprintf(msg,"lsq error info=%d",info);
break;
}
for (j=0;j<NX;j++)
x[j]+=dx[j];
if (norm(dx,NX)<1E-4) {
sol->type=0;
sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
sol->qr[3]=(float)Q[1]; /* cov xy */
sol->qr[4]=(float)Q[2+NX]; /* cov yz */
sol->qr[5]=(float)Q[2]; /* cov zx */
sol->ns=(unsigned char)ns;
sol->age=sol->ratio=0.0;
/* validate solution */
if ((stat = valsol(azel, vsat, n, opt, v, nv, NX, msg))) {
sol->stat = opt->sateph == EPHOPT_SBAS ? SOLQ_SBAS : SOLQ_SINGLE;
}
return stat;
}
}
return 0;
}
/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg)
{
double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
int i,j,k,info,stat,nv,ns;
trace(3,"estpos : n=%d\n",n);
v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
for (i=0;i<3;i++) x[i]=sol->rr[i];
for (i=0;i<MAXITR;i++) {
/* pseudorange residuals */
nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
&ns);
if (nv<NX) {
sprintf(msg,"lack of valid sats ns=%d",nv);
break;
}
/* weight by variance */
for (j=0;j<nv;j++) {
sig=sqrt(var[j]);
v[j]/=sig;
for (k=0;k<NX;k++) H[k+j*NX]/=sig;
}
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
sprintf(msg,"lsq error info=%d",info);
break;
}
for (j=0;j<NX;j++) x[j]+=dx[j];
if (norm(dx,NX)<1E-4) {
sol->type=0;
sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
sol->qr[3]=(float)Q[1]; /* cov xy */
sol->qr[4]=(float)Q[2+NX]; /* cov yz */
sol->qr[5]=(float)Q[2]; /* cov zx */
sol->ns=(unsigned char)ns;
sol->age=sol->ratio=0.0;
/* validate solution */
if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
}
free(v); free(H); free(var);
return stat;
}
}
if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
free(v); free(H); free(var);
return 0;
}
static int real_hr_arma_init (double *coeff, const DATASET *dset,
arma_info *ainfo, PRN *prn)
{
const int *list = ainfo->alist;
int np = ainfo->p, nq = ainfo->q;
int nP = ainfo->P, nQ = ainfo->Q;
int ptotal = np + nP + np * nP;
int qtotal = nq + nQ + nq * nQ;
int nexo = ainfo->nexo;
int pass1lags, pass1v;
const double *y;
DATASET *aset = NULL;
int *pass1list = NULL;
int *pass2list = NULL;
int *arlags = NULL;
int *malags = NULL;
MODEL armod;
int xstart;
int m, pos, s;
int i, j, t;
int err = 0;
pass1lags = (ainfo->Q + ainfo->P) * dset->pd;
if (pass1lags < HR_MINLAGS) {
pass1lags = HR_MINLAGS;
}
pass1v = pass1lags + nexo + 2;
/* dependent variable */
if (arma_xdiff(ainfo)) {
/* for initialization, use the level of y */
y = dset->Z[ainfo->yno];
} else {
y = ainfo->y;
}
aset = create_auxiliary_dataset(pass1v + qtotal, ainfo->T, 0);
if (aset == NULL) {
return E_ALLOC;
}
#if AINIT_DEBUG
fprintf(stderr, "hr_arma_init: dataset allocated: %d vars, %d obs\n",
pass1v + qtotal, ainfo->T);
#endif
/* in case we bomb before estimating a model */
gretl_model_init(&armod, dset);
/* Start building stuff for pass 1 */
pass1list = gretl_list_new(pass1v);
if (pass1list == NULL) {
err = E_ALLOC;
goto bailout;
}
pass1list[1] = 1;
pass1list[2] = 0;
for (i=2; i<pass1v; i++) {
pass1list[i+1] = i;
}
/* variable names */
strcpy(aset->varname[1], "y");
for (i=0; i<nexo; i++) {
/* exogenous vars */
sprintf(aset->varname[i+1], "x%d", i);
}
for (i=1; i<=pass1lags; i++) {
/* lags */
sprintf(aset->varname[i+1+nexo], "y_%d", i);
}
/* Fill the dataset with the data for pass 1 */
/* starting position for reading exogeneous vars */
if (ainfo->d > 0 || ainfo->D > 0) {
xstart = (arma_has_seasonal(ainfo))? 10 : 6;
} else {
xstart = (arma_has_seasonal(ainfo))? 8 : 5;
}
for (t=0; t<ainfo->T; t++) {
s = t + ainfo->t1;
aset->Z[1][t] = y[s];
for (i=0, pos=2; i<nexo; i++) {
m = list[xstart + i];
aset->Z[pos++][t] = dset->Z[m][s];
}
for (i=1; i<=pass1lags; i++) {
s = t + ainfo->t1 - i;
aset->Z[pos++][t] = (s >= 0)? y[s] : NADBL;
}
}
/* pass 1 proper */
armod = lsq(pass1list, aset, OLS, OPT_A);
if (armod.errcode) {
err = armod.errcode;
goto bailout;
}
#if AINIT_DEBUG
fprintf(stderr, "pass1 model: t1=%d, t2=%d, nobs=%d, ncoeff=%d, dfd = %d\n",
armod.t1, armod.t2, armod.nobs, armod.ncoeff, armod.dfd);
#endif
/* allocations for pass 2 */
if (qtotal > 0) {
malags = malloc(qtotal * sizeof *malags);
if (malags == NULL) {
err = E_ALLOC;
} else {
for (i=0, pos=0; i<nq; i++) {
malags[pos++] = i+1;
}
for (i=0; i<ainfo->Q; i++) {
for (j=0; j<=nq; j++) {
malags[pos++] = (i+1) * dset->pd + j;
}
}
}
}
if (ptotal > 0 && !err) {
arlags = malloc(ptotal * sizeof *arlags);
if (arlags == NULL) {
err = E_ALLOC;
} else {
for (i=0, pos=0; i<np; i++) {
arlags[pos++] = i+1;
}
for (i=0; i<ainfo->P; i++) {
for (j=0; j<=np; j++) {
arlags[pos++] = (i+1) * dset->pd + j;
}
}
}
}
if (!err) {
pass2list = gretl_list_new(2 + nexo + ptotal + qtotal);
if (pass2list == NULL) {
err = E_ALLOC;
}
}
/* handle error in pass2 allocations */
if (err) {
goto bailout;
}
/* stick lagged residuals into temp dataset */
pos = pass1v;
for (i=0; i<qtotal; i++) {
sprintf(aset->varname[pos], "e_%d", malags[i]);
for (t=0; t<ainfo->T; t++) {
s = t - malags[i];
aset->Z[pos][t] = (s >= 0)? armod.uhat[s] : NADBL;
}
pos++;
}
/* compose pass 2 regression list */
for (i=1, pos=1; i<=nexo+2; i++) {
pass2list[pos++] = pass1list[i];
}
for (i=0; i<ptotal; i++) {
/* FIXME? */
if (AR_included(ainfo,i)) {
pass2list[pos++] = arlags[i] + nexo + 1;
}
}
for (i=0; i<qtotal; i++) {
/* FIXME? */
if (MA_included(ainfo,i)) {
pass2list[pos++] = pass1v + i;
}
}
/* now do pass2 */
clear_model(&armod);
armod = lsq(pass2list, aset, OLS, OPT_A);
if (armod.errcode) {
err = armod.errcode;
} else {
#if AINIT_DEBUG
PRN *modprn = gretl_print_new(GRETL_PRINT_STDERR, NULL);
printmodel(&armod, aset, OPT_S, modprn);
gretl_print_destroy(modprn);
#endif
err = hr_transcribe_coeffs(ainfo, &armod, coeff);
if (!err && arma_exact_ml(ainfo) &&
ainfo->ifc && ainfo->nexo == 0) {
transform_arma_const(coeff, ainfo);
}
}
#if AINIT_DEBUG
if (!err) {
fprintf(stderr, "HR init:\n");
for (i=0; i<ainfo->nc; i++) {
fprintf(stderr, "coeff[%d] = %g\n", i, coeff[i]);
}
}
#endif
bailout:
free(pass1list);
free(pass2list);
free(arlags);
free(malags);
destroy_dataset(aset);
clear_model(&armod);
if (!err && prn != NULL) {
pprintf(prn, "\n%s: %s\n\n", _("ARMA initialization"),
_("Hannan-Rissanen method"));
}
return err;
}
int arma_by_ls (const double *coeff, const DATASET *dset,
arma_info *ainfo, MODEL *pmod)
{
PRN *prn = ainfo->prn;
int *list = ainfo->alist;
int nmixed = ainfo->np * ainfo->P;
int ptotal = ainfo->np + ainfo->P + nmixed;
int av = ptotal + ainfo->nexo + 2;
DATASET *aset = NULL;
int *arlist = NULL;
int nonlin = 0;
aset = create_auxiliary_dataset(av, ainfo->T, 0);
if (aset == NULL) {
return E_ALLOC;
}
if (ptotal > 0 && nmixed > 0) {
/* we'll have to use NLS */
nonlin = 1;
} else {
/* OLS: need regression list */
arlist = make_ar_ols_list(ainfo, av);
}
/* build temporary dataset */
arma_init_build_dataset(ainfo, ptotal, 0, list,
dset, aset, nonlin);
if (nonlin) {
pmod->errcode = arma_get_nls_model(pmod, ainfo, 0, coeff, aset,
prn);
} else {
*pmod = lsq(arlist, aset, OLS, OPT_A | OPT_Z);
}
/* clean up */
free(arlist);
destroy_dataset(aset);
if (!pmod->errcode && pmod->full_n < dset->n) {
/* the model series are short */
double *uhat = malloc(dset->n * sizeof *uhat);
double *yhat = malloc(dset->n * sizeof *yhat);
int s, t;
if (uhat == NULL || yhat == NULL) {
free(uhat);
free(yhat);
pmod->errcode = E_ALLOC;
} else {
for (t=0; t<dset->n; t++) {
uhat[t] = yhat[t] = NADBL;
}
t = ainfo->t1;
for (s=0; s<pmod->full_n; s++, t++) {
uhat[t] = pmod->uhat[s];
yhat[t] = pmod->yhat[s];
}
free(pmod->uhat);
pmod->uhat = uhat;
free(pmod->yhat);
pmod->yhat = yhat;
}
}
return pmod->errcode;
}
int ar_arma_init (double *coeff, const DATASET *dset,
arma_info *ainfo, MODEL *pmod)
{
PRN *prn = ainfo->prn;
int *list = ainfo->alist;
int nmixed = ainfo->np * ainfo->P;
int ptotal = ainfo->np + ainfo->P + nmixed;
int av = ptotal + ainfo->nexo + 2;
DATASET *aset = NULL;
int *arlist = NULL;
MODEL armod;
int narmax, nonlin = 0;
int i, err = 0;
#if AINIT_DEBUG
fprintf(stderr, "ar_arma_init: dset->t1=%d, dset->t2=%d (dset->n=%d);\n"
" ainfo->t1=%d, ainfo->t2=%d, ",
dset->t1, dset->t2, dset->n, ainfo->t1, ainfo->t2);
fprintf(stderr, "nmixed = %d, ptotal = %d, ifc = %d, nexo = %d\n",
nmixed, ptotal, ainfo->ifc, ainfo->nexo);
#endif
if (ptotal == 0 && ainfo->nexo == 0 && !ainfo->ifc) {
/* special case of pure MA model */
for (i=0; i<ainfo->nq + ainfo->Q; i++) {
coeff[i] = 0.0001;
}
pprintf(ainfo->prn, "\n%s: %s\n\n", _("ARMA initialization"),
_("small MA values"));
return 0;
}
gretl_model_init(&armod, dset);
narmax = arma_exact_ml(ainfo) ? ainfo->nexo : 0;
if (narmax > 0 && ptotal > 0) {
/* ARMAX-induced lags of exog vars */
av += ainfo->nexo * ptotal;
}
if (arma_exact_ml(ainfo) && ainfo->ifc) {
maybe_set_yscale(ainfo);
}
aset = create_auxiliary_dataset(av, ainfo->fullT, 0);
if (aset == NULL) {
return E_ALLOC;
}
if (ptotal > 0 && (narmax > 0 || nmixed > 0)) {
/* we'll have to use NLS */
nonlin = 1;
} else {
/* OLS: need regression list */
arlist = make_ar_ols_list(ainfo, av);
}
/* build temporary dataset, dset -> aset */
arma_init_build_dataset(ainfo, ptotal, narmax, list,
dset, aset, nonlin);
if (nonlin) {
PRN *dprn = NULL;
#if AINIT_DEBUG
fprintf(stderr, "arma:_init_by_ls: doing NLS\n");
dprn = prn;
#endif
err = arma_get_nls_model(&armod, ainfo, narmax, NULL, aset,
dprn);
} else {
#if AINIT_DEBUG
printlist(arlist, "'arlist' in ar_arma_init (OLS)");
#endif
armod = lsq(arlist, aset, OLS, OPT_A | OPT_Z);
err = armod.errcode;
}
#if AINIT_DEBUG
if (!err) {
pputs(prn, "\n*** armod, in ar_arma_init\n");
printmodel(&armod, aset, OPT_NONE, prn);
} else {
fprintf(stderr, "LS init: armod.errcode = %d\n", err);
}
#endif
if (!err) {
arma_init_transcribe_coeffs(ainfo, &armod, coeff);
}
/* handle the case where we need to translate from an
estimate of the regression constant to the
unconditional mean of y_t
*/
if (!err && arma_exact_ml(ainfo) && ainfo->ifc &&
(!nonlin || ainfo->nexo == 0)) {
transform_arma_const(coeff, ainfo);
}
if (!err && prn != NULL) {
if (nonlin) {
pprintf(prn, "\n%s: %s\n\n", _("ARMA initialization"),
_("using nonlinear AR model"));
} else {
pprintf(prn, "\n%s: %s\n\n", _("ARMA initialization"),
_("using linear AR model"));
}
}
/* clean up */
clear_model(&armod);
free(arlist);
destroy_dataset(aset);
return err;
}
int system_estimate (equation_system *sys, DATASET *dset,
gretlopt opt, PRN *prn)
{
int i, j, k, T, t;
int v, l, mk, krow, nr;
int orig_t1 = dset->t1;
int orig_t2 = dset->t2;
gretl_matrix *X = NULL;
gretl_matrix *y = NULL;
gretl_matrix *Xi = NULL;
gretl_matrix *Xj = NULL;
gretl_matrix *M = NULL;
MODEL **models = NULL;
int method = sys->method;
double llbak = -1.0e9;
int single_equation = 0;
int do_iteration = 0;
int rsingle = 0;
int do_diag = 0;
int err = 0;
sys->iters = 0;
if (sys->flags & SYSTEM_ITERATE) {
do_iteration = 1;
}
nr = system_n_restrictions(sys);
if (method == SYS_METHOD_OLS || method == SYS_METHOD_TSLS ||
method == SYS_METHOD_LIML || method == SYS_METHOD_WLS) {
single_equation = 1;
}
if (nr > 0 && !(opt & OPT_U)) {
if (method == SYS_METHOD_3SLS) {
/* doing 3SLS with restrictions: we want to obtain
restricted TSLS estimates as a starting point
*/
rsingle = 1;
} else if (method == SYS_METHOD_SUR) {
/* doing SUR with restrictions: we want to obtain
restricted OLS estimates as a starting point
*/
rsingle = 1;
}
}
/* get uniform sample starting and ending points and check for
missing data */
err = system_adjust_t1t2(sys, dset);
if (err) {
return err;
}
/* set sample for auxiliary regressions */
dset->t1 = sys->t1;
dset->t2 = sys->t2;
/* max indep vars per equation */
k = system_max_indep_vars(sys);
/* total indep vars, all equations */
mk = system_n_indep_vars(sys);
/* set sample for auxiliary regressions */
dset->t1 = sys->t1;
dset->t2 = sys->t2;
/* number of observations per series */
T = sys->T;
/* allocate models etc */
err = basic_system_allocate(sys, mk, nr, &X, &y);
if (err) {
goto cleanup;
}
/* convenience pointers */
models = sys->models;
if ((method == SYS_METHOD_FIML ||
method == SYS_METHOD_LIML) && !(opt & OPT_Q)) {
print_equation_system_info(sys, dset, OPT_H, prn);
}
/* First estimate the equations separately (either by OLS or
TSLS), and put the single-equation residuals into the uhat
matrix. Note that at this stage we are not in a position to
impose any cross-equation restrictions, since we're doing
straight equation-by-equation estimation.
*/
for (i=0; i<sys->neqns; i++) {
int freeit = 0;
int *list = system_model_list(sys, i, &freeit);
const int *idroplist = NULL;
MODEL *pmod = models[i];
if (list == NULL) {
err = 1;
break;
}
if (sys_ols_ok(sys)) {
*pmod = lsq(list, dset, OLS, OPT_A);
} else {
*pmod = tsls(list, dset, sys_tsls_opt(sys));
}
if (freeit) {
free(list);
}
if ((err = pmod->errcode)) {
fprintf(stderr, "system_estimate: failed to estimate equation %d: "
"err = %d\n", i+1, err);
break;
}
idroplist = gretl_model_get_data(pmod, "inst_droplist");
if (idroplist != NULL) {
drop_redundant_instruments(sys, idroplist, i);
}
pmod->ID = i;
pmod->aux = AUX_SYS;
gretl_model_set_int(pmod, "method", method);
/* save the sigma-squared for an LR test for a diagonal
covariance matrix */
if (method == SYS_METHOD_SUR && do_iteration && nr == 0) {
gretl_model_set_double(pmod, "ols_sigma_squared",
pmod->ess / pmod->nobs);
}
for (t=0; t<T; t++) {
gretl_matrix_set(sys->E, t, i, pmod->uhat[t + sys->t1]);
}
}
if (err) {
fprintf(stderr, "system_estimate: after single-equation "
"estimation, err = %d\n", err);
goto cleanup;
}
if (method == SYS_METHOD_LIML) {
/* compute the minimum eigenvalues and generate the
k-class data matrices */
err = liml_driver(sys, dset, prn);
if (err) goto cleanup;
}
/* marker for iterated versions of SUR, WLS, or 3SLS; also for
loopback in case of restricted 3SLS, where we want to compute
restricted TSLS estimates first
*/
iteration_start:
gls_sigma_from_uhat(sys, sys->S, 0);
#if SDEBUG > 1
gretl_matrix_print(sys->S, "gls_sigma_from_uhat");
#endif
if (method == SYS_METHOD_WLS) {
gretl_matrix_zero(X);
err = gretl_invert_diagonal_matrix(sys->S);
} else if (single_equation || rsingle) {
gretl_matrix_zero(X);
} else {
err = gretl_invert_symmetric_matrix(sys->S);
}
#if SDEBUG
fprintf(stderr, "system_estimate: on invert, err=%d\n", err);
#endif
if (!err && Xi == NULL) {
/* the test against NULL here allows for the possibility
that we're iterating
*/
err = allocate_Xi_etc(&Xi, &Xj, &M, T, k);
}
if (err) goto cleanup;
/* form the big stacked X matrix: Xi = data matrix for equation i,
specified in lists[i]
*/
krow = 0;
for (i=0; i<sys->neqns && !err; i++) {
int kcol = 0;
err = make_sys_X_block(Xi, models[i], dset, sys->t1, method);
for (j=0; j<=i && !err; j++) {
const gretl_matrix *Xk;
double sij;
if (i != j) {
if (single_equation || rsingle) {
kcol += models[j]->ncoeff;
continue;
}
err = make_sys_X_block(Xj, models[j], dset, sys->t1, method);
Xk = Xj;
} else if (method == SYS_METHOD_LIML) {
err = make_liml_X_block(Xj, models[i], dset, sys->t1);
Xk = Xj;
} else {
Xk = Xi;
}
M->rows = Xi->cols;
M->cols = Xk->cols;
err = gretl_matrix_multiply_mod(Xi, GRETL_MOD_TRANSPOSE,
Xk, GRETL_MOD_NONE,
M, GRETL_MOD_NONE);
if (rsingle || (single_equation && method != SYS_METHOD_WLS)) {
sij = 1.0;
} else {
sij = gretl_matrix_get(sys->S, i, j);
}
insert_sys_X_block(X, M, krow, kcol, sij);
kcol += models[j]->ncoeff;
}
krow += models[i]->ncoeff;
}
if (err) {
fprintf(stderr, "after trying to make X matrix: err = %d\n", err);
goto cleanup;
}
if (nr > 0) {
/* there are restrictions to be imposed */
augment_X_with_restrictions(X, mk, sys);
}
if (!do_iteration && !rsingle) {
/* we're not coming back this way, so free some storage */
gretl_matrix_free(Xj);
Xj = NULL;
gretl_matrix_free(M);
M = NULL;
}
/* form stacked Y column vector (m x k) */
v = 0;
for (i=0; i<sys->neqns; i++) {
/* loop over the m vertically-arranged blocks */
make_sys_X_block(Xi, models[i], dset, sys->t1, method);
for (j=0; j<models[i]->ncoeff; j++) {
/* loop over the rows within each of the m blocks */
double yv = 0.0;
int lmin = 0, lmax = sys->neqns;
if (single_equation || rsingle) {
/* no cross terms wanted */
lmin = i;
lmax = i + 1;
}
for (l=lmin; l<lmax; l++) {
/* loop over the components that must be
added to form each element */
const double *yl = NULL;
double sil, xx = 0.0;
if (method == SYS_METHOD_LIML) {
yl = gretl_model_get_data(models[l], "liml_y");
} else {
yl = dset->Z[system_get_depvar(sys, l)];
}
/* multiply X'[l] into y */
for (t=0; t<T; t++) {
xx += gretl_matrix_get(Xi, t, j) * yl[t + sys->t1];
}
if (rsingle || (single_equation && method != SYS_METHOD_WLS)) {
sil = 1.0;
} else {
sil = gretl_matrix_get(sys->S, i, l);
}
yv += xx * sil;
}
gretl_vector_set(y, v++, yv);
}
}
if (nr > 0) {
/* there are restrictions */
augment_y_with_restrictions(y, mk, nr, sys);
}
/* The estimates calculated below will be SUR, 3SLS or LIML,
depending on how the data matrices above were constructed --
unless, that is, we're just doing restricted OLS, WLS or TSLS
estimates.
*/
err = calculate_sys_coeffs(sys, dset, X, y, mk, nr,
do_iteration);
if (!err && rsingle) {
/* take one more pass */
rsingle = 0;
goto iteration_start;
}
if (!err && do_iteration) {
/* check for convergence */
if (!sys_converged(sys, &llbak, opt, prn, &err)) {
if (!err) {
sys->iters += 1;
goto iteration_start;
}
}
}
if (err) goto cleanup;
if (nr == 0 && (method == SYS_METHOD_3SLS || method == SYS_METHOD_SUR)) {
/* compute this test while we have sigma-inverse available */
hansen_sargan_test(sys, dset);
}
do_diag = !(method == SYS_METHOD_SUR && do_iteration);
/* refresh sigma (non-inverted) */
gls_sigma_from_uhat(sys, sys->S, do_diag);
if (method == SYS_METHOD_FIML) {
/* compute FIML estimates */
err = fiml_driver(sys, dset, opt, prn);
}
if (!err && !(sys->flags & SYSTEM_LIML1)) {
err = system_save_and_print_results(sys, dset, opt, prn);
}
cleanup:
gretl_matrix_free(Xi);
gretl_matrix_free(Xj);
gretl_matrix_free(M);
gretl_matrix_free(X);
gretl_matrix_free(y);
clean_up_models(sys);
dset->t1 = orig_t1;
dset->t2 = orig_t2;
return err;
}
int main (void)
{
DATASET *dataset; /* pointer to dataset struct */
int *list; /* list of regressors etc. */
MODEL *model; /* pointer to model struct */
PRN *prn; /* pointer to struct for printing */
int model_count = 0; /* keep a tally of models estimated */
int i; /* index variable */
/* the first data series to load */
double z1[] = {
199.9, 228, 235, 285, 239, 293, 285,
365, 295, 290, 385, 505, 425, 415
};
/* and the second series */
double z2[] = {
1065, 1254, 1300, 1577, 1600, 1750, 1800,
1870, 1935, 1948, 2254, 2600, 2800, 3000
};
/* basic initialization of library */
libgretl_init();
prn = gretl_print_new(GRETL_PRINT_STDOUT, NULL); /* simple printing */
/* allocate the dataset struct: the parameters are the number of
variables (here 3, allowing for the constant in position 0),
the number of observations on each variable (here 14), and
a 0/1 flag indicating whether we want to supply "case marker"
strings for the observations (here we don't).
*/
dataset = create_new_dataset(3, 14, 0);
if (dataset == NULL) noalloc();
/* copy in the names of the variables (starting at [1]
because [0] refers to the constant) */
strcpy(dataset->varname[1], "price");
strcpy(dataset->varname[2], "sqft");
/* Fill in the data array, starting at variable 1. Note that
this array may be a superset of the data actually used in
the regression equation. Note that dataset->n records the
number of observations.
*/
for (i=0; i<dataset->n; i++) {
dset_set_data(dataset, 1, i, z1[i]);
dset_set_data(dataset, 2, i, z2[i]);
}
/* Set up the "list", which is fed to the regression function.
The first element of list represents the length of the list
vector itself, counting from zero. The second entry is the ID
number of the dependent variable (i.e. its place in the data
set Z) counting from one (zero being reserved for the
constant). The third entry (and there can be more) is the ID
number of the first independent variable.
*/
list = gretl_list_new(3); /* number of terms will be 3 */
list[1] = 1; /* the dependent variable is the one with ID# 1 */
list[2] = 0; /* we include a constant (ID# 0) */
list[3] = 2; /* the independent variable has ID# 2 */
/* Now we call the lsq function from libgretl to get least squares
estimates and associated statistics. */
model = gretl_model_new();
if (model == NULL) noalloc();
*model = lsq(list, /* regressand and regressors */
dataset, /* the dataset */
OLS, /* use Ordinary Least Squares */
OPT_NONE /* no special options */
);
/* Handle case where lsq bombed */
if (model->errcode) {
printf("model->errcode: %d\n", model->errcode);
printf("error message: %s\n", gretl_errmsg_get());
return 1;
}
/* Otherwise give this model an ID number for reference */
model->ID = ++model_count;
/* and print the regression results */
printmodel(model, dataset, OPT_NONE, prn);
/* memory management check -- try explicitly freeing all allocated
memory */
gretl_model_free(model);
free(list);
destroy_dataset(dataset);
gretl_print_destroy(prn);
libgretl_cleanup();
return 0;
}
int ransacMethod(int N, int ansAry[][4],int m[][2], int x1[][2],int x2[][2]) {
int i,j,roop,snm=0,fnm,rndAry[N];
Matrix *ans;
double tmp[3][3],w[100][4];
for (i = 0; i < N; i++) rndAry[i] = i;
for (roop = 0; roop <= 10000; roop++) {
fnm = 0;
for(i=0;i < 4;i++){
int t;
j=(int)(drand48()*N);
t=rndAry[i]; rndAry[i]=rndAry[j]; rndAry[j]=t;
}
double xy[][2]={ // from the first image
x1[m[rndAry[0]][0]][0], x1[m[rndAry[0]][0]][1],
x1[m[rndAry[1]][0]][0], x1[m[rndAry[1]][0]][1],
x1[m[rndAry[2]][0]][0], x1[m[rndAry[2]][0]][1],
x1[m[rndAry[3]][0]][0], x1[m[rndAry[3]][0]][1]
},uv[][2]={ // from the second image
x2[m[rndAry[0]][1]][0], x2[m[rndAry[0]][1]][1],
x2[m[rndAry[1]][1]][0], x2[m[rndAry[1]][1]][1],
x2[m[rndAry[2]][1]][0], x2[m[rndAry[2]][1]][1],
x2[m[rndAry[3]][1]][0], x2[m[rndAry[3]][1]][1]
};
ans = MatrixAlloc(1,8);
ans = lsq(4, xy, uv);
for(i=0;i<ans->H;i++){
for(j=0;j<ans->W;j++){
tmp[i][j] = Elem(ans,i,j);
}
}
tmp[2][2] = 1;
MatrixFree(ans);
for(i=0;i<30;i++){
double x=x1[m[i][0]][0], y=x1[m[i][0]][1],
u=x2[m[i][1]][0], v=x2[m[i][1]][1],
dx,dy,r,xu,yv;
r = 1 / (tmp[2][0]*x+tmp[2][1]*y+tmp[2][2]);
xu = r * (tmp[0][0]*x+tmp[0][1]*y+tmp[0][2]);
yv = r * (tmp[1][0]*x+tmp[1][1]*y+tmp[1][2]);
// dx, dy は (x,y) を変換した座標と (u,v) の差．
dx = xu - u;
dy = yv - v;
if(dx*dx+dy*dy < 5
){
w[fnm][0] = x;
w[fnm][1] = y;
w[fnm][2] = u;
w[fnm][3] = v;
fnm++;
}
}
if (fnm > snm) {
snm = fnm;
for(i = 0; i < snm; i++) {
ansAry[i][0] = w[i][0];
ansAry[i][1] = w[i][1];
ansAry[i][2] = w[i][2];
ansAry[i][3] = w[i][3];
}
}
}
return snm;
}
/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg)
{
double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
int i,j,k,info,stat,nv,ns;
trace(3,"estpos : n=%d\n",n);
v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
for (i=0;i<3;i++) x[i]=sol->rr[i];
fprintf(output, " Begin coordinates calculation in single point positioning mode\n");
fprintf(output, " Current coordinates: X = %f, Y = %f, Z = %f\n", x[0], x[1], x[2]);
for (i=0;i<MAXITR;i++) {
fprintf(output, " Begin iteration %i:\n", i);
/* pseudorange residuals */
nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
&ns);
if (nv<NX) {
sprintf(msg,"lack of valid sats ns=%d",nv);
break;
}
/* weight by variance */
for (j=0;j<nv;j++) {
sig=sqrt(var[j]);
v[j]/=sig;
for (k=0;k<NX;k++) H[k+j*NX]/=sig;
}
fprintf(output, " Begin least-square estimation:\n");
fprintf(output, " ");
for(j = 0; j < NX; j++)
fprintf(output, " X[%i] = %f,", j, dx[j]);
fprintf(output, "\n");
fprintf(output, " ");
for(j = 0; j < nv; j++)
fprintf(output, " v[%i] = %f,", j, v[j]);
fprintf(output, "\n");
for(j = 0; j < nv; j++)
{
fprintf(output, " ");
for(k = 0; k < NX; k++)
fprintf(output, " H[%i][%i] = %f,", j, k, H[k + j * NX]);
fprintf(output, "\n");
}
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
sprintf(msg,"lsq error info=%d",info);
break;
}
fprintf(output, " End least-square estimation\n");
fprintf(output, " Results of least square estimation: dx1 = %f, dx2 = %f, dx3 = %f, dx4 = %f, dx5 = %f, dx6 = %f, dx7 = %f\n", dx[0], dx[1], dx[2], dx[3], dx[4], dx[5], dx[6]);
for (j=0;j<NX;j++) x[j]+=dx[j];
fprintf(output, " New state of vector X: X[0] = %f, X[1] = %f, X[2] = %f, X[3] = %f, X[4] = %f, X[5] = %f, X[6] = %f\n", x[0], x[1], x[2], x[3], x[4], x[5], x[6]);
fprintf(output, " norm(dx) = %f\n", norm(dx, NX));
if (norm(dx,NX)<1E-4) {
sol->type=0;
sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
sol->qr[3]=(float)Q[1]; /* cov xy */
sol->qr[4]=(float)Q[2+NX]; /* cov yz */
sol->qr[5]=(float)Q[2]; /* cov zx */
sol->ns=(unsigned char)ns;
sol->age=sol->ratio=0.0;
/* validate solution */
if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
}
free(v); free(H); free(var);
fprintf(output, " End of coordinates calculation\n");
return stat;
}
}
if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
free(v); free(H); free(var);
return 0;
}
