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;
}
