//======================================================================
Vector &impute_mvn(Vector &observation,
const Vector &mean, const SpdMatrix &variance,
const Selector &observed, RNG &rng) {
if (observed.nvars() == observed.nvars_possible()) {
return observation;
} else if (observed.nvars() == 0) {
observation = rmvn_mt(rng, mean, variance);
return observation;
}
if (observation.size() != observed.nvars_possible()) {
report_error("observation and observed must be the same size.");
}
// The distribution we want is N(mu, V), with
// V = Sig11 - Sig12 Sig22.inv Sig.21
// and
// mu = mu1 - Sig12 Sig22.inv (y2 - mu2)
// The 1's are missing, and the 2's are observed.
Selector missing = observed.complement();
Matrix cross_covariance = missing.select_rows(
observed.select_cols(variance));
SpdMatrix observed_precision = observed.select_square(variance).inv();
Vector mu = missing.select(mean) + cross_covariance * observed_precision
* (observed.select(observation) - observed.select(mean));
SpdMatrix V = missing.select_square(variance)
- sandwich(cross_covariance, observed_precision);
Vector imputed = rmvn_mt(rng, mu, V);
observed.fill_missing_elements(observation, imputed);
return observation;
}
NSSKF::NoStorageScalarKalmanFilter(const Vec &Z, double H, const Mat &T, const Mat &R, const Spd & Q,
Ptr<MvnModel> init)
: Z_(Z),
H_(H),
T_(T),
RQR_(sandwich(R,Q)),
L_(Z.size(), Z.size()),
a_(Z.size()),
P_(Z.size()),
K_(Z.size()),
init_(init)
{}
int main()
{
em::mv<1,2,4>::type v = {1.0, 2.0, 3.0};
em::mv<0,3,5,6>::type R = {cos(-0.5*0.5*M_PI), sin(-0.5*0.5*M_PI), 0.0, 0.0};
auto RvrR = R*v*~R;
std::cout << "R*v*~R: " << RvrR << std::endl;
std::cout << "<R*v*~R>_1: " << grade<1>(R*v*~R) << std::endl;
const auto& s = sandwich(v,R);
std::cout << "sandwich(v,R): " << s << std::endl;
auto a = R*(v*~R);
std::cout << "a: " << a << std::endl;
}
static void
ba81ComputeFit(omxFitFunction* oo, int want, FitContext *fc)
{
BA81FitState *state = (BA81FitState*) oo->argStruct;
BA81Expect *estate = (BA81Expect*) oo->expectation->argStruct;
if (fc) state->numFreeParam = fc->varGroup->vars.size();
if (want & FF_COMPUTE_INITIAL_FIT) return;
if (estate->type == EXPECTATION_AUGMENTED) {
buildItemParamMap(oo, fc);
if (want & FF_COMPUTE_PARAMFLAVOR) {
for (size_t px=0; px < state->numFreeParam; ++px) {
if (state->paramFlavor[px] == NULL) continue;
fc->flavor[px] = state->paramFlavor[px];
}
return;
}
if (want & FF_COMPUTE_PREOPTIMIZE) {
omxExpectationCompute(fc, oo->expectation, NULL);
return;
}
if (want & FF_COMPUTE_INFO) {
buildLatentParamMap(oo, fc);
if (!state->freeItemParams) {
omxRaiseErrorf("%s: no free parameters", oo->name());
return;
}
ba81SetupQuadrature(oo->expectation);
if (fc->infoMethod == INFO_METHOD_HESSIAN) {
ba81ComputeEMFit(oo, FF_COMPUTE_HESSIAN, fc);
} else {
omxRaiseErrorf("Information matrix approximation method %d is not available",
fc->infoMethod);
return;
}
return;
}
double got = ba81ComputeEMFit(oo, want, fc);
oo->matrix->data[0] = got;
return;
} else if (estate->type == EXPECTATION_OBSERVED) {
if (want == FF_COMPUTE_STARTING) {
buildLatentParamMap(oo, fc);
if (state->freeLatents) setLatentStartingValues(oo, fc);
return;
}
if (want & (FF_COMPUTE_INFO | FF_COMPUTE_GRADIENT)) {
buildLatentParamMap(oo, fc); // only to check state->freeLatents
buildItemParamMap(oo, fc);
if (!state->freeItemParams && !state->freeLatents) {
omxRaiseErrorf("%s: no free parameters", oo->name());
return;
}
ba81SetupQuadrature(oo->expectation);
if (want & FF_COMPUTE_GRADIENT ||
(want & FF_COMPUTE_INFO && fc->infoMethod == INFO_METHOD_MEAT)) {
gradCov(oo, fc);
} else {
if (state->freeLatents) {
omxRaiseErrorf("Information matrix approximation method %d is not available",
fc->infoMethod);
return;
}
if (!state->freeItemParams) {
omxRaiseErrorf("%s: no free parameters", oo->name());
return;
}
sandwich(oo, fc);
}
}
if (want & (FF_COMPUTE_HESSIAN | FF_COMPUTE_IHESSIAN)) {
omxRaiseErrorf("%s: Hessian is not available for observed data", oo->name());
}
if (want & FF_COMPUTE_MAXABSCHANGE) {
double mac = std::max(omxMaxAbsDiff(state->itemParam, estate->itemParam),
omxMaxAbsDiff(state->latentMean, estate->_latentMeanOut));
fc->mac = std::max(mac, omxMaxAbsDiff(state->latentCov, estate->_latentCovOut));
state->copyEstimates(estate);
}
if (want & FF_COMPUTE_FIT) {
omxExpectationCompute(fc, oo->expectation, NULL);
Eigen::ArrayXd &patternLik = estate->grp.patternLik;
const int numUnique = estate->getNumUnique();
if (state->returnRowLikelihoods) {
const double OneOverLargest = estate->grp.quad.getReciprocalOfOne();
omxData *data = estate->data;
for (int rx=0; rx < numUnique; rx++) {
int dups = omxDataNumIdenticalRows(data, estate->grp.rowMap[rx]);
for (int dup=0; dup < dups; dup++) {
int dest = omxDataIndex(data, estate->grp.rowMap[rx]+dup);
oo->matrix->data[dest] = patternLik[rx] * OneOverLargest;
}
}
} else {
double *rowWeight = estate->grp.rowWeight;
const double LogLargest = estate->LogLargestDouble;
double got = 0;
#pragma omp parallel for num_threads(Global->numThreads) reduction(+:got)
for (int ux=0; ux < numUnique; ux++) {
if (patternLik[ux] == 0) continue;
got += rowWeight[ux] * (log(patternLik[ux]) - LogLargest);
}
double fit = nan("infeasible");
if (estate->grp.excludedPatterns < numUnique) {
fit = Global->llScale * got;
// add in some badness for excluded patterns
fit += fit * estate->grp.excludedPatterns;
}
if (estate->verbose >= 1) mxLog("%s: observed fit %.4f (%d/%d excluded)",
oo->name(), fit, estate->grp.excludedPatterns, numUnique);
oo->matrix->data[0] = fit;
}
}
} else {
Rf_error("%s: Predict nothing or scores before computing %d", oo->name(), want);
}
}
/* MDENTITY: Process ENTITY declaration.
*/
VOID mdentity(UNCH *tbuf) /* Work area for tokenization[LITLEN+2]. */
{
struct fpi fpicb; /* Formal public identifier structure. */
struct fpi *fpis = &fpicb; /* Ptr to current or #DEFAULT fpi. */
union etext etx; /* Ptr to entity text. */
UNCH estore = ESM; /* Entity storage class. */
struct entity *ecb; /* Ptr to entity control block. */
int parmsw = 0; /* 1=parameter entity declaration; 0 = not. */
int defltsw = 0; /* 1=#DEFAULT declaration; 0=not. */
PNE pne = 0; /* Ptr to N/C/SDATA entity control block. */
mdname = key[KENTITY]; /* Declaration name for messages. */
subdcl = NULL; /* No subject as yet. */
parmno = 0; /* No parameters as yet. */
mdessv = es; /* Save es for checking entity nesting. */
/* PARAMETER 1: Entity name.
*/
pcbmd.newstate = 0;
parsemd(nmbuf, ENTCASE, &pcblitp, NAMELEN);
TRACEMD("1: entity nm");
switch (pcbmd.action) {
case PEN:
parsemd(nmbuf + 1, ENTCASE, &pcblitp, NAMELEN);
if (pcbmd.action!=NAS) {mderr(120, (UNCH *)0, (UNCH *)0); return;}
if (nmbuf[1] == NAMELEN + 2) {
/* It was too long. */
nmbuf[0] = NAMELEN + 2;
nmbuf[NAMELEN + 1] = '\0';
mderr(65, (UNCH *)0, (UNCH *)0);
}
else
nmbuf[0] = nmbuf[1] + 1; /* Increment length for PERO. */
nmbuf[1] = lex.d.pero; /* Prefix PERO to name. */
parmsw = 1; /* Indicate parameter entity. */
case NAS:
break;
case RNS: /* Reserved name started. */
if (ustrcmp(nmbuf+1, key[KDEFAULT])) {
mderr(118, nmbuf+1, key[KDEFAULT]);
return;
}
memcpy(nmbuf, indefent, *indefent);/* Copy #DEFAULT to name buffer. */
fpis = &fpidf; /* Use #DEFAULT fpi if external. */
defltsw = 1; /* Indicate #DEFAULT is being defined.*/
break;
default:
mderr(122, (UNCH *)0, (UNCH *)0);
return;
}
subdcl = nmbuf+1; /* Subject name for error messages. */
/* PARAMETER 2: Entity text keyword (optional).
*/
pcbmd.newstate = 0;
parsemd(tbuf, NAMECASE, &pcblitp, LITLEN);
TRACEMD("2: keyword");
switch (pcbmd.action) {
case NAS:
if ((estore = (UNCH)mapsrch(enttab, tbuf+1))==0) {
estore = parmsw ? ESP : ESF;
pne = (PNE)rmalloc(NESZ);
if (mdextid(tbuf, fpis, nmbuf+1+parmsw, &estore, pne)==0)
return;
if (defltsw) etx.x = NULL;
else if ((etx.x = entgen(&fpicb))==0) {
if (parmsw)
mderr(148, nmbuf+2, (UNCH *)0);
else
mderr(147, nmbuf+1, (UNCH *)0);
}
goto parm4;
}
if (parmsw && (estore==ESX || estore==ESC)) {
mderr(38, tbuf+1, (UNCH *)0);
estore = ESM;
}
pcbmd.newstate = 0;
parsemd(tbuf, NAMECASE, &pcblitp, LITLEN);
break;
default:
estore = ESM;
break;
}
/* PARAMETER 3: Parameter literal.
*/
TRACEMD("3: literal");
switch (pcbmd.action) {
case LITE:
case LIT:
switch (estore) {
case ESM: /* LITERAL: parameter literal required. */
case ESC: /* CDATA: parameter literal required. */
case ESX: /* SDATA: parameter literal required. */
case ESI: /* PI: parameter literal required. */
etx.c = savestr(tbuf);
break;
case ESMD: /* MD: parameter literal required. */
etx.c = sandwich(tbuf, lex.m.mdo, lex.m.mdc);
goto bcheck;
case ESMS: /* MS: parameter literal required. */
etx.c = sandwich(tbuf, lex.m.mss, lex.m.mse);
goto bcheck;
case ESS: /* STARTTAG: parameter literal required. */
etx.c = sandwich(tbuf, lex.m.stag, lex.m.tagc);
goto bcheck;
case ESE: /* ENDTAG: parameter literal required. */
etx.c = sandwich(tbuf, lex.m.etag, lex.m.tagc);
bcheck:
if (etx.c == 0) {
mderr(225, (UNCH *)0, (UNCH *)0);
return;
}
break;
}
break;
default:
mderr(123, (UNCH *)0, (UNCH *)0);
return;
}
/* PARAMETER 4: End of declaration.
*/
pcbmd.newstate = 0;
parsemd(tbuf, NAMECASE, &pcblitp, LITLEN);
parm4:
TRACEMD(emd);
if (pcbmd.action!=EMD) mderr(126, (UNCH *)0, (UNCH *)0);
if (es!=mdessv) synerr(37, &pcbmd);
/* EXECUTE: If the entity already exists, ignore the new definition.
If it is a new entity, store the definition.
*/
if ((ecb = entfind(nmbuf))!=0 && ecb->estore) {
if (ecb->dflt) {
mderr(228, nmbuf + 1, (UNCH *)0);
hout((THASH)etab, nmbuf, hash(nmbuf, ENTHASH));
if (ecb->estore == ESN) {
frem((UNIV)NEID(ecb->etx.n));
frem((UNIV)ecb->etx.n);
}
else if (ecb->estore >= ESFM)
frem((UNIV)ecb->etx.x);
frem((UNIV)ecb);
}
else {
/* Duplicate definition: not an error. */
if (sw.swdupent) mderr(68, nmbuf+1, (UNCH *)0);
if (estore<ESFM) frem((UNIV)etx.c);
return;
}
}
++ds.ecbcnt; /* Do capacity before NOTATION. */
ds.ecbtext += estore<ESFM ? ustrlen(etx.c) : entlen;
ecb = entdef(nmbuf, estore, &etx); /* Define the entity. */
if (estore==ESN) { /* If entity is external: */
NEENAME(pne) = ecb->ename; /* Store entity name in ne. */
NEID(pne) = etx.x; /* Store system fileid in ne. */
NESYSID(pne) = fpis->fpisysis ? savestr(fpis->fpisysis) : 0;
NEPUBID(pne) = fpis->fpipubis ? savestr(fpis->fpipubis) : 0;
ecb->etx.n = pne; /* Store ne control block in etx. */
TRACEESN(pne);
}
else if (pne)
frem((UNIV)pne);
if (defltsw) {
ecbdeflt = ecb; /* If #DEFAULT save ecb. */
if (fpidf.fpipubis)
fpidf.fpipubis = savestr(fpidf.fpipubis);
if (fpidf.fpisysis)
fpidf.fpisysis = savestr(fpidf.fpisysis);
}
}
SpdMatrix MvRegSuf::SSE(const Matrix &B) const {
SpdMatrix ans = yty();
ans.add_inner2(B, xty(), -1);
ans += sandwich(B.transpose(), xtx());
return ans;
}
