ITG3200::ITG3200() { spn("Init ITG-3200 starting..."); readI2C(GYRADDR, 0x00, 1, buffer); // Who am I? sp("ITG-3200 ID = "); spln((int) buffer[0]); readI2C(GYRADDR, 0x15, 2, buffer); // Sample rate divider is 1 + 1 = 2, so 1000 Hz / 2 = 500 Hz buffer[0] = 1; // Set FS_SEL = 3 as recommended on DS p. 24. // Set DLPF_CFG = 3. This signifies a 1 kHz internal sample rate with a 42 // Hz LPF bandwidth, which should be low enough to filter out the motor // vibrations. buffer[1] = (3 << 3) | 3; // FS_SEL is on bits 4 and 3. writeI2C(GYRADDR, 0x15, 2, buffer); // Set to use X gyro as clock reference as recommended on DS p. 27. buffer[0] = 1; writeI2C(GYRADDR, 0x3e, 1, buffer); spln("ITG-3200 configured!"); // Zero buffer. for (int i=0; i<6; i++) { buffer[i] = 0; } // Low-pass filter. #ifdef GYRO_LPF_DEPTH lpfIndex = 0; for (int i=0; i<3; i++) for (int j=0; j<GYRO_LPF_DEPTH; j++) lpfVal[i][j] = 0; #endif // GYRO_LPF_DEPTH for (int i=0; i<3; i++) { gZero[i] = 0; gVec[i] = 0; angle[i] = 0; } calibrated = false; }
void InferenceEngineLoopyBP::initializePotentials(Beliefs& potentials, FeatureGenerator* fGen, DataSequence *X, Model* m, iMatrix adjMat, int seqLabel, bool bUseStatePerNodes) { initializeBeliefs(potentials, X, m, adjMat); int V = m->getNumberOfViews(); int T = X->length(); int nbNodes = V*T; featureVector vecFeatures; feature *f; dVector *lambda = m->getWeights(); double val; // Singleton potentials for(int xi=0; xi<nbNodes; xi++) { fGen->getFeatures(vecFeatures,X,m,xi,-1,seqLabel); f = vecFeatures.getPtr(); for(int k=0; k<vecFeatures.size(); k++, f++) { val = (*lambda)[f->globalId] * f->value; potentials.belStates[xi][f->nodeState] += val; } } // Pairwise potentials. Values are symmetrical: pot(xi,xj)==pot(xj,xi). for(int xi=0; xi<nbNodes; xi++) { for(int xj=xi+1; xj<nbNodes; xj++) { if( !adjMat(xi,xj) ) continue; fGen->getFeatures(vecFeatures,X,m,xj,xi,seqLabel); f = vecFeatures.getPtr(); for(int k=0; k<vecFeatures.size(); k++, f++) { val = (*lambda)[f->globalId] * f->value; potentials.belEdges[adjMat(xi,xj)-1](f->prevNodeState, f->nodeState) += val; } } } // For MV-LDCRF if( !bUseStatePerNodes ) return; if( !m->isMultiViewMode() ) { iMatrix* pStatesPerNode = m->getStateMatrix(X); for(int xi=0; xi<nbNodes; xi++) for(int h=0; h<m->getNumberOfStates(); h++) if( pStatesPerNode->getValue(h,xi)==0 ) potentials.belStates[xi][h] = -INF_VALUE; } else { std::vector<iMatrix> statesPerNodeMV; for(int v=0; v<m->getNumberOfViews(); v++) { iMatrix spn(m->getNumberOfStatesMV(v),X->length()); for(int t=0; t<X->length(); t++) { for(int h=0; h<m->getNumberOfStatesMV(v); h++) { spn(t,h) = m->getStatesPerLabelMV(v)(h,X->getStateLabels()->getValue(t)); } } statesPerNodeMV.push_back(spn); } for(int xi=0; xi<nbNodes; xi++) { int v = xi/T; int t = xi%T; for(int h=0; h<m->getNumberOfStatesMV(v); h++) if(statesPerNodeMV[v](t,h)==0) potentials.belStates[xi][h] = -INF_VALUE; } } }