void updateF(arr u) { double v=u(0); // save the controls to easy access them arr update_F(3, 3); // variable to create a updated Jacobian of f // F = [(1, 0, -tau*v*cos(theta)); // (0, 1, tau*v*cos(theta)); // (0, 0, 1)] // --> rows of F update_F(0,0) = 1; // first row update_F(0,1) = 0; update_F(0,2) = -1 * tau * v * sin(mu(2)); update_F(1,0) = 0; // second row update_F(1,1) = 1; update_F(1,2) = tau * v * cos(mu(2)); update_F(2,0) = 0; // third row update_F(2,1) = 0; update_F(2,2) = 1; // assign the updated Jacobian to F F = update_F; }
CvMat *Kalman::predict(unsigned long tick) { update_F(tick); predict_x(tick); predict_P(); return x_pred; }
// for SCF, J = K //void fock_task(BasisSet_t basis, ERD_t erd, int ncpu_f, int num_dmat, void fock_task(BasisSet_t basis, int ncpu_f, int num_dmat, int *shellptr, double *shellvalue, int *shellid, int *shellrid, int *f_startind, int *rowpos, int *colpos, int *rowptr, int *colptr, double tolscr2, int startrow, int startcol, int startM, int endM, int startP, int endP, double **D1, double **D2, double **D3, double *F1, double *F2, double *F3, double *F4, double *F5, double *F6, int ldX1, int ldX2, int ldX3, int ldX4, int ldX5, int ldX6, int sizeX1, int sizeX2, int sizeX3, int sizeX4, int sizeX5, int sizeX6, double *nitl, double *nsq) { int startMN = shellptr[startM]; int endMN = shellptr[endM + 1]; int startPQ = shellptr[startP]; int endPQ = shellptr[endP + 1]; #pragma omp parallel { // init int nt = omp_get_thread_num (); int nf = nt/ncpu_f; double *F_MN = &(F1[nf * sizeX1 * num_dmat]); double *F_PQ = &(F2[nf * sizeX2 * num_dmat]); double *F_NQ = F3; double *F_MP = &(F4[nf * sizeX4 * num_dmat]); double *F_MQ = &(F5[nf * sizeX5 * num_dmat]); double *F_NP = &(F6[nf * sizeX6 * num_dmat]); double mynsq = 0.0; double mynitl = 0.0; #pragma omp for schedule(dynamic) for (int i = startMN; i < endMN; i++) { int M = shellrid[i]; int N = shellid[i]; double value1 = shellvalue[i]; int dimM = f_startind[M + 1] - f_startind[M]; int dimN = f_startind[N + 1] - f_startind[N]; int iX1M = f_startind[M] - f_startind[startrow]; int iX3M = rowpos[M]; int iXN = rowptr[i]; int iMN = iX1M * ldX1+ iXN; int flag1 = (value1 < 0.0) ? 1 : 0; for (int j = startPQ; j < endPQ; j++) { int P = shellrid[j]; int Q = shellid[j]; if ((M > P && (M + P) % 2 == 1) || (M < P && (M + P) % 2 == 0)) continue; if ((M == P) && ((N > Q && (N + Q) % 2 == 1) || (N < Q && (N + Q) % 2 == 0))) continue; double value2 = shellvalue[j]; int dimP = f_startind[P + 1] - f_startind[P]; int dimQ = f_startind[Q + 1] - f_startind[Q]; int iX2P = f_startind[P] - f_startind[startcol]; int iX3P = colpos[P]; int iXQ = colptr[j]; int iPQ = iX2P * ldX2+ iXQ; int iNQ = iXN * ldX3 + iXQ; int iMP = iX1M * ldX4 + iX2P; int iMQ = iX1M * ldX5 + iXQ; int iNP = iXN * ldX6 + iX2P; int iMP0 = iX3M * ldX3 + iX3P; int iMQ0 = iX3M * ldX3 + iXQ; int iNP0 = iXN * ldX3 + iX3P; int flag3 = (M == P && Q == N) ? 0 : 1; int flag2 = (value2 < 0.0) ? 1 : 0; if (fabs(value1 * value2) >= tolscr2) { int nints; double *integrals; mynsq += 1.0; mynitl += dimM*dimN*dimP*dimQ; nints=ComputeShellQuartet(basis,nt,M,N,P,Q,&integrals); //CInt_computeShellQuartet(basis, erd, nt, // M, N, P, Q, &integrals, &nints); if (nints != 0) { update_F(num_dmat, integrals, dimM, dimN, dimP, dimQ, flag1, flag2, flag3, iMN, iPQ, iMP, iNP, iMQ, iNQ, iMP0, iMQ0, iNP0, D1, D2, D3, F_MN, F_PQ, F_NQ, F_MP, F_MQ, F_NP, sizeX1, sizeX2, sizeX3, sizeX4, sizeX5, sizeX6, ldX1, ldX2, ldX3, ldX4, ldX5, ldX6); } } } } #pragma omp critical { *nitl += mynitl; *nsq += mynsq; } } /* #pragma omp parallel */ }