void extended_kalman_step( uFloat *z_in ) { #ifdef PRINT_DEBUG printf( "ekf: step %d\n", global_step ); #endif #ifdef PRINT_DEBUG printVector( "measurements ", z_in, MEAS_SIZE ); #endif /***************** Gain Loop ***************** First, linearize locally, then do normal gain loop */ generate_system_transfer( state_pre, sys_transfer ); generate_measurement_transfer( state_pre, mea_transfer ); estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre ); update_prob( cov_pre, mea_noise_cov, mea_transfer, cov_post, kalman_gain ); /************** Estimation Loop ***************/ rungeKutta( state_post, state_pre ); update_system( z_in, state_pre, kalman_gain, state_post ); global_step++; }
int TInteractionFunctions::polinomial(TSimWorld *world) { for (int i = 0; i < 10; ++i) { // Update forces and locations for (auto &point : *(world->model())) { point->set_force(QVector3D(0, 0, 0)); for (auto &otherPoint : point->visibleObjects()) { if (point == otherPoint) { continue; } const qreal distance = point->location().distanceToPoint(otherPoint->location()); // distance QVector3D Fij = (otherPoint->location() - point->location()).normalized(); // Force direction const qreal criticalRadius = (point->criticalRadius() + otherPoint->criticalRadius()); qreal attractiveForce = 0; attractiveForce = point->mass() * otherPoint->mass() / qPow(distance, 2); // mi * mj / d^2 const qreal repulsiveForce = criticalRadius * point->mass() * otherPoint->mass() / qPow(distance, 3); // Rcr * mi * mj / d^3 const qreal forceMagnitude = world->gravity() * (attractiveForce - repulsiveForce); Fij *= forceMagnitude; // forceDirection * forceMagnitude point->set_force(point->force() + Fij); // Fi = Fi + Fij if (point->point_id() == 0) { LOG_EXPERIMENT_DATA("distance:" << distance << " force:" << forceMagnitude << " acceleration:" << point->acceleration().length() << " speed:" << point->speed().length() << " gravity:" << world->gravity() << " damper:" << world->damperCoefficient()); } } point->set_force(point->force() + point->engineForce()); // Update point location const qreal udx = -world->damperCoefficient() * point->speed().x(); const qreal udy = -world->damperCoefficient() * point->speed().y(); const qreal udz = -world->damperCoefficient() * point->speed().z(); point->set_acceleration( // d^2x/dt^2 = 1/m * (F + (u * dx/dt)) QVector3D((point->force().x() + udx) / point->mass(), (point->force().y() + udy) / point->mass(), (point->force().z() + udz) / point->mass())); const qreal h = 0.0005; point->set_speed(QVector3D(point->speed().x() + rungeKutta(h, point->acceleration().x()), point->speed().y() + rungeKutta(h, point->acceleration().y()), point->speed().z() + rungeKutta(h, point->acceleration().z()))); point->set_location(QVector3D(point->location().x() + rungeKutta(h, point->speed().x()), point->location().y() + rungeKutta(h, point->speed().y()), point->location().z() + rungeKutta(h, point->speed().z()))); } } return 0; }
void nextStep(void) { //if source is a pulse, add it only at step 1, if driven, always add it in if (source_mode == 0) { if (step == 1) { addSource(); } } else if (source_mode == 1) { addSource(); } //perform runge kutta rungeKutta(); //if source is a pulse, reset H density term to 0 if (source_mode == 0) { for (int j = 0; j < Y; j++) { for (int i = 0; i < X; i++) { H0[(j*X)+i] = 0; } } } }
int main(int argc, char **par){ double alpha; //Pitch Angle double Eko; //Energia Cinetica Inicial Eko = atoi(par[1]); alpha = atoi(par[2]); Eko=Eko*(1.60210E-13)/1; alpha = alpha*(M_PI/180); double T = 10.0; //Tiempo total de la simulacion int npoints = (int)(T/h); //numero de pasos //Vectores double *t = malloc(npoints*sizeof(double)); double *x = malloc(npoints*sizeof(double)); double *y = malloc(npoints*sizeof(double)); double *z = malloc(npoints*sizeof(double)); double *vx = malloc(npoints*sizeof(double)); double *vy = malloc(npoints*sizeof(double)); double *vz = malloc(npoints*sizeof(double)); double *tnew = malloc(1*sizeof(double)); double *xnew = malloc(1*sizeof(double)); double *ynew = malloc(1*sizeof(double)); double *znew = malloc(1*sizeof(double)); double *vxnew = malloc(1*sizeof(double)); double *vynew = malloc(1*sizeof(double)); double *vznew = malloc(1*sizeof(double)); //Condicionles Iniciales x[0] = 2*Rt; y[0] = 0; z[0] = 0; //Calcular Velocidad double *v = malloc(1*sizeof(double)); *v = c*sqrt(1-(1/(pow((Eko/(m*c*c)+1),2)))); vx[0]=0; vy[0]= (*v)*(sin(alpha)); vz[0]= (*v)*cos(alpha); //Gamma double *gamma = malloc(1*sizeof(double)); *gamma = 1/(sqrt(1-(pow(*v,2))/(pow(c,2)))); //Resolver Ecuacion FILE *fileout; fileout = fopen("trayectoria_E_alpha.dat", "w"); int i =0; fprintf(fileout, "%f\t", vx[0]/Rt); fprintf(fileout, "%f\t", vy[0]/Rt); fprintf(fileout, "%f\n", vz[0]/Rt); for(i=1;i<npoints;i++){ rungeKutta(t[i-1],tnew, x[i-1],y[i-1],z[i-1],xnew,ynew,znew, vx[i-1],vy[i-1],vz[i-1],vxnew,vynew,vznew,gamma); x[i]=*xnew; y[i]=*ynew; z[i]=*znew; vx[i]=*vxnew; vy[i]=*vynew; vz[i]=*vznew; fprintf(fileout, "%f\t", vx[i]/Rt); fprintf(fileout, "%f\t", vy[i]/Rt); fprintf(fileout, "%f\n", vz[i]/Rt); } fclose(fileout); return 0; }