int main()
{
double euler, midpoint, kutta, euler1, midpoint1, kutta1, euler2, midpoint2, kutta2;
euler = Euler(0.0, 2.4, 0.1, -1.0, df);
midpoint = MidPoint(0.0, 2.4, 0.2, -1.0, df);
kutta = RungeKutta(0.0, 2.4, 0.4, -1.0, df);
euler1 = Euler(0.0, 2.4, 0.01, -1.0, df);
midpoint1 = MidPoint(0.0, 2.4, 0.02, -1.0, df);
kutta1 = RungeKutta(0.0, 2.4, 0.04, -1.0, df);
euler2 = Euler(0.0, 2.4, 0.001, -1.0, df);
midpoint2 = MidPoint(0.0, 2.4, 0.002, -1.0, df);
kutta2 = RungeKutta(0.0, 2.4, 0.004, -1.0, df);
printf("Método h y(2.4) \n--------------------------\n");
printf("Euler 0.1 %lf \nMidpoint 0.2 %lf \nRunge Kutta 0.4 %lf \n--------------------------\n", euler, midpoint, kutta);
printf("Euler 0.01 %lf \nMidpoint 0.02 %lf \nRunge Kutta 0.04 %lf \n--------------------------\n", euler1, midpoint1, kutta1);
printf("Euler 0.001 %lf \nMidpoint 0.002 %lf \nRunge Kutta 0.004 %lf \n--------------------------\n", euler2, midpoint2, kutta2);
system("pause");
return 0;
}
int main (void)
{
FILE* fp1 = fopen("saida1.txt", "w"); /* Valores de theta para o Runge-Kutta adaptativo */
FILE* fp2 = fopen("saida2.txt", "w"); /* Valores da velocidade em cada ponto */
FILE* fp3 = fopen("saida3.txt", "w"); /* Valores de theta para a solução analítica */
FILE* fp4 = fopen("saida4.txt", "w"); /* passos feitos (período) */
mc * particula = (mc*)malloc(sizeof(mc));
double i=0, result1, T,v1, v2, h = 0.0001;
int flag = 0;
particula->s = 0;
particula->w = 0;
T = 2*PI*(sqrt(l/G));
printf("T : %g \n", T);
do
{
v1 = particula->w; /* velocidade anterior */
fprintf(fp4,"%g\n", i);
result1 = RungeKutta(0, i, h, theta0, 0.0, f, particula);
fprintf(fp1,"%g\n", particula->s);
fprintf(fp2,"%g\n", particula->w);
fprintf(fp3,"%g\n", theta_T(theta0, i, l));
v2 = particula->w; /* Nova velocidade */
i+=0.01;
if(v1*v2<0) flag++; /* quando a velocidade trocar de sinal a flag é incrementada */
}while(flag != 2); /* quando a velocidade trocar de sinal duas vezes então um período foi completo */
fclose(fp1);
fclose(fp2);
fclose(fp3);
fclose(fp4);
return 0;
}
int main (void)
{
float *x;
float *y1;
float *y2;
float h = 0.05;
int n_puntos = ((6.0+h)/h);
int i;
x = malloc(n_puntos*sizeof(float));
y1 = malloc(n_puntos*sizeof(float));
y2 = malloc(n_puntos*sizeof(float));
x[0] = 0.0;
y1[0] = 0.1;
y2[0] = 0.0;
for (i = 1; i < n_puntos; i++)
{
RungeKutta(x, y1, y2, i, h);
}
FILE *out;
out = fopen("soODE.txt","w");
for (i = 0; i < n_puntos; i++)
{
fprintf(out, "%f %f %f\n", x[i], y1[i], y2[i]);
}
fclose(out);
return 0;
}
void WindEKF::StatePrediction(float gps_vel[2], float dT)
{
float U[2];
U[0] = gps_vel[0];
U[1] = gps_vel[1];
// EKF prediction step
LinearizeFG(U);
RungeKutta(U, dT);
}
void estadoInicial(float *a, float *b, float *c, float *d, float *l, float *yInicial, float *x_obs, float *x, float *y, int n_puntos, float y_0, float x_0, float paso)
{
int i;
srand48(time(NULL));
a[0] = drand48();
b[0] = drand48();
c[0] = drand48();
d[0] = drand48();
yInicial = RungeKutta(x, y, x_0, y_0, a[0], b[0], c[0], d[0], n_puntos, paso);
l[0] = likelyhood(x_obs, yInicial, n_puntos);
}
main()
{
const int nSteps = 25;
double dDStart[2];
double dDEnd[2];
double dDelta = .8;
int i;
void *csm;
dDStart[0] = 0.0;
dDStart[1] = 1.0;
for(i = 0; i < nSteps; i++){
double dlnExp = (i+1)*dDelta;
RungeKutta(csm, (void (*)(void *, double, double *, double*))
csmGrowthFacDeriv, 2, 0.0, dDStart, dlnExp, dDEnd,
nSteps);
printf("%g %g %g\n", dlnExp, dDEnd[0], dDEnd[1]);
}
}
double t_pair_PH(double E, Particle& particle, Fun* tpf) //E=Ep
{
DataInjection data;
data.E = E;
data.mass = particle.mass;
data.tpf = tpf;
double mass = particle.mass;
double cte = 0.5*P2(mass*cLight2)*cLight;
double b = 10*targetPhotonEmax; //energia maxima de los fotones en erg
double a1 = mass*P2(cLight)*pairThresholdPH/(2*E);
double a = std::max(a1,targetPhotonEmin);
double integral = RungeKutta(a,b,&cPairPH,&dPH,&f_t_PHPair,&data);
return cte*integral/P2(E);
}
/////////////////////////////////////////////////
// Epidemiological modeling and model fitting
void TEpidemModel::RunModel(const TFltV& StartValV, const double& StartT, const double& StopT, const int& NSteps, TVec<TFltV>& OutValV) {
TFltV ValV(StartValV), dydx(StartValV.Len()), ValV2(StartValV.Len());
OutValV.Clr(false);
for (int v = 0; v < StartValV.Len(); v++) {
OutValV.Add();
OutValV[v].Clr(false);
OutValV[v].Add(StartValV[v]);
}
const double h = (StopT-StartT) / NSteps;
double x = StartT;
for (int k = 0; k < NSteps; k++) {
GetDerivs(x, ValV, dydx);
RungeKutta(ValV, dydx, x, h, ValV2);
for (int v = 0; v < ValV2.Len(); v++) {
double X = ValV2[v];
if (X < 0 || _isnan(X) || !_finite(X)) { X = 0; }
OutValV[v].Add(X);
}
ValV = ValV2;
x += h;
}
}
// run the model internally with 10 times greater resolution
void TEpidemModel::RunModel10(const TFltV& StartValV, const double& StartT, const double& StopT, const int& NSteps, TVec<TFltV>& OutValV) {
TFltV ValV(StartValV), dydx(StartValV.Len()), ValV2(StartValV.Len());
OutValV.Clr(false);
for (int v = 0; v < StartValV.Len(); v++) {
OutValV.Add();
OutValV[v].Clr(false);
OutValV[v].Add(StartValV[v]);
}
const double h = (StopT-StartT) / (10*NSteps);
double x = StartT;
for (int k = 0; k < 10*NSteps; k++) {
GetDerivs(x, ValV, dydx);
RungeKutta(ValV, dydx, x, h, ValV2);
// take values at only every 10th step
if (k % 10 == 0) {
for (int v = 0; v < ValV2.Len(); v++) {
OutValV[v].Add(ValV2[v]); }
}
ValV = ValV2;
x += h;
}
}
void DifferentialSteeringTest::getResults() {
// Init
results.clear();
real phi = M_PI * -10 / 180.0;
real x0 = 0;
diffeq y0 = { { 2, 1, cos (phi), sin (phi), -sin (phi), cos (phi) } };
real h = 0.1;
real xmax = 20;
for (;;)
{
results.push_back(Vector2D(y0.coord[0], y0.coord[1]));
//printf("%f %f \n", y0.coord[0], y0.coord[1]);
if (x0 >= xmax) break;
y0 = RungeKutta (x0, y0, h);
x0 += h;
}
}
int ekf_estimator::update(EKF_U u,EKF_Mesurement mesurement,const float dT)
{
//Declear F G U
float U[6],F[169],G[117];
static int wait_convergence;
//Declear Mesurement
float Mag_data[3],Pos[3],Vel[2];
//Declear EulerAngle
if(!ekf_is_init)//if not init , return
return -1;
/*caution: the accel sign is all opposite with APM and CC3D*/
U[0]=u.gyro_x - gyro_bias[0];
U[1]=u.gyro_y - gyro_bias[1];
U[2]=u.gyro_z - gyro_bias[2];
U[3]=-u.accel_x;
U[4]=-u.accel_y;
U[5]=-u.accel_z;
/*caution: the mag_x,mag_y sign is opposite with APM and CC3D & the mag_z sign is same*/
Mag_data[0]=-mesurement.Mag_x;
Mag_data[1]=-mesurement.Mag_y;
Mag_data[2]= mesurement.Mag_z;
Pos[0]=mesurement.Pos_GPS_x;
Pos[1]=mesurement.Pos_GPS_y;
Pos[2]=mesurement.Pos_Baro_z;//use baro replace gps_d
Vel[0]=mesurement.Vel_GPS_x;
Vel[1]=mesurement.Vel_GPS_y;
if(ekf_is_init)
{
//To-Do:wait for ekf convergence
//caculate Jacobian to linear F G
LinearFG(X,U,F,G);
//Predeict X
RungeKutta(X,U,dT);
//Predict P
INS_CovariancePrediction(F,G,Q,dT,P);
//Update P,X
INS_Correction(Mag_data,Pos,Vel,X,R,P,Be);
/*Fill ekf result with data*/
ekf_result.Pos_x=X[0];
ekf_result.Pos_y=X[1];
ekf_result.Pos_z=X[2];
ekf_result.Vel_x=X[3];
ekf_result.Vel_y=X[4];
ekf_result.Vel_z=X[5];
ekf_result.q0=X[6];
ekf_result.q1=X[7];
ekf_result.q2=X[8];
ekf_result.q3=X[9];
//Transfer quaternion to euler angle
quaternion_to_euler(1/*is radian:1 OR is Angle:0*/,X[6],X[7],X[8],X[9],&ekf_result.roll,&ekf_result.pitch,&ekf_result.yaw);
}
//check ekf if is ready to arm
ekf_is_ready();
return 1;
}
double getReliability(TrialParameters *tp) {
double t,
I, Igoal, Istep,
Inoise,
reliability = 0.0;
long trial,
events,
subpass,
binSubpass = 1.0/dt,
bin,
binCount = t_measure;
double V_prev = 0.0,
V_threshold = 20.0;
int unstable_trials = 0;
std::vector<long> psth(binCount);
std::vector<long> nspikes(trials);
Istep = (tp->I0_goal - tp->I0) / t_pre;
// iterate over trials
for (trial = 0; trial < trials; trial++) {
nspikes[trial] = 0;
events = 0;
// run each simulation for t_measure ms
for (t=0; t < t_pre + t_measure + t_post; t += dt, subpass++){
// calculate input current for the current time
Inoise = sigma * sqrt(dt) * gasdev(&(tp->seed));
Igoal = (t > t_pre? t_pre: t) * Istep;
// divide by 1000 to convert t from ms to sec
I = tp->I0 + Igoal + tp->I1 * sin(2.0 / 1000.0 * M_PI * tp->f * t);
RungeKutta(&(tp->neuron), I, Inoise);
/*
if (isnan(tp->neuron.state[0])){
unstable_trials++;
break;
}
*/
fprintf(vlogfile, "%lf\t%lf\t%lf\t%f\t%f\t%f\n", t, I, tp->neuron.state[0], tp->neuron.state[1], tp->neuron.state[2], tp->neuron.state[3]);
//printf("%lf\t%lf\n", t, tp->neuron.state[0]);
// increment spike count if we're over the threshold
// but the previous value was under the threshold
// (we don't want to count the same spike multiple times)
if (tp->neuron.state[0] > V_threshold &&
V_prev < V_threshold &&
t > t_pre &&
t < t_pre + t_measure) { // count only the spikes in the middle
events++;
printf("Spike!\n");
}
if (subpass >= binSubpass) {
if(events > 0) {
bin = (long) (t - t_pre) / (binSubpass * dt);
if (bin < binCount) {
psth[bin]++;
nspikes[trial]++;
}
}
subpass = 0;
events = 0;
}
V_prev = tp->neuron.state[0];
}
}
/*
if (unstable_trials) {
printf("%d trials became unstable!\n", unstable_trials);
return -1;
}
*/
// now calculate reliability
double totalVariance = 0.0,
psthAvg = 0.0,
binAvg;
for (long i = 0; i < binCount; i++) {
binAvg = (double) (psth[i])/(double) trials;
totalVariance += binAvg * binAvg;
psthAvg += binAvg;
}
psthAvg /= (double) binCount;
totalVariance /= (double) binCount;
totalVariance -= psthAvg * psthAvg;
double trialVariance = 0.0;
psthAvg = 0.0;
for(long i = 0; i < trials; i++) {
binAvg = (double) (nspikes[i])/(double) trials;
//printf("avg nspikes[bin]: %f\n", binAvg);
trialVariance += binAvg * (1.0 - binAvg);
}
trialVariance /= (double) trials;
//printf("trial variance: %f", trialVariance);
if (trialVariance > 0.0) {
reliability = totalVariance/trialVariance;
}
else {
reliability = -1.0; // error, divide by zero
}
return reliability;
}
void MainWindow::on_StartButton_clicked()
{
double A=ui->ASpin->value(), B=ui->BSpin->value(), N=ui->NSpin->value(), TMax=ui->TMaxSpin->value(), Eps=ui->EpsSpin->value();
RungeKutta rk = RungeKutta(A, B, N, TMax, Eps);
ui->Qwt_Widget->detachItems(QwtPlotItem::Rtti_PlotCurve, true);
rk.run();
//QCustomPlot* customPlot = ui->MainPlot;
//customPlot->addGraph();
//customPlot->graph(0)->setData(rk.y1, rk.y2);
// for(int i=0; i<N; i++)
// {
// qDebug()<<"y1["<<i<<"]= "<<rk.y1[i]<<endl;
// }
// for(int i=0; i<N; i++)
// {
// qDebug()<<"y2["<<i<<"]= "<<rk.y2[i]<<endl;
// }
// customPlot->xAxis->setLabel("y1");
// customPlot->yAxis->setLabel("y2");
// if(rk.y1[0]<rk.y1[N])
// customPlot->xAxis->setRange(rk.y1[0], rk.y1[N-1]);
// else
// customPlot->xAxis->setRange(rk.y1[N-1], rk.y1[0]);
// if(rk.y2[0]<rk.y2[N])
// customPlot->yAxis->setRange(rk.y2[0], rk.y2[N-1]);
// else
// customPlot->yAxis->setRange(rk.y2[N-1], rk.y2[0]);
// customPlot->replot();
min1=min(rk.y1,rk.p1);
min2=min(rk.y2,rk.p2);
max1=max(rk.y1,rk.p1);
max2=max(rk.y2,rk.p2);
ui->Qwt_Widget->setAxisScale(QwtPlot::xBottom, min1 , max1);
ui->Qwt_Widget->setAxisScale(QwtPlot::yLeft, min2 , max2);
ui->Qwt_Widget->replot();//чтобы сразу приблизить наблюдателя к графику
// рисуем кривую
addCurve(rk);
QString abcs;
abcs.setNum(c);
c++;
const QString filename = "C:/Users/Kirill/Desktop/Prac/report/"+abcs+".pdf";
QPrinter printer;
printer.setOutputFormat(QPrinter::PdfFormat);
printer.setOrientation(QPrinter::Landscape);
printer.setResolution(80);
printer.setPaperSize(QPrinter::A4);
printer.setFullPage(true);
printer.setOutputFileName(filename);
QImage image= QPixmap::grabWidget(ui->Qwt_Widget).toImage();
// image.setDotsPerMeterX(500);
image.scaled(1000,1000, Qt::KeepAspectRatio);
// image.setDotsPerMeterY(500);
QPainter painter;
painter.begin(&printer);
painter.drawImage(0,0,image);
painter.end();
//сделаем красивые графики в гнуплоте(выведем массивы в файл)
QFile file("C:/Users/Kirill/Desktop/Prac/report/"+abcs+".txt");
file.open(QIODevice::WriteOnly | QIODevice::Text);
QTextStream out(&file);
out.setRealNumberPrecision(10);
for (int i=0; i<N ; i++)
out << rk.t[i] <<' ' << rk.y1[i]<<' '<<rk.y2[i]<<"\n";
}
void *elCamino(float *x_obs, float *x_aComparar,float *y_ayudaAx, float *xDelMomento,float *xCandidato, float *a, float *b, float *c, float *d,float *l,float paso, int t, int iteraciones, int burn)
{
int i;
float aPrima;
float bPrima;
float cPrima;
float dPrima;
float lPresente;
float lCandidato;
float gamma;
float beta;
float alpha;
float x_0 = 15 ;
float y_0 = 13;
const gsl_rng_type * T;
gsl_rng * r;
gsl_rng_env_setup();
T = gsl_rng_default;
r = gsl_rng_alloc (T);
for(i=0; i < (iteraciones-1) ; i++)
{
aPrima = gsl_ran_gaussian(r, 0.1)+a[i];
//bPrima = gsl_ran_gaussian(r, 0.1)+b[i];
//cPrima = gsl_ran_gaussian(r, 0.1)+c[i];
//dPrima = gsl_ran_gaussian(r, 0.1)+d[i];
xDelMomento = RungeKutta(x_aComparar, y_ayudaAx, x_0, y_0, a[i], b[i], c[i], d[i], t, paso);
xCandidato = RungeKutta(x_aComparar, y_ayudaAx, x_0, y_0, aPrima, b[i], c[i], d[i], t, paso);
lPresente = likelyhood(x_obs, xDelMomento, t);
lCandidato = likelyhood(x_obs, xCandidato , t);
gamma = (lCandidato - lPresente);
if(gamma>=0.00)
{
a[i+1]=aPrima;
}
else
{
beta = drand48();
alpha = exp(gamma);
if(beta<=alpha)
{
a[i+1]=aPrima;
}
else
{
a[i+1]=a[i];
}
}
//aPrima = gsl_ran_gaussian(r, 0.1)+a[i];
bPrima = gsl_ran_gaussian(r, 0.1)+b[i];
//cPrima = gsl_ran_gaussian(r, 0.1)+c[i];
//dPrima = gsl_ran_gaussian(r, 0.1)+d[i];
xDelMomento = RungeKutta(x_aComparar, y_ayudaAx, x_0, y_0, a[i], b[i], c[i], d[i], t, paso);
xCandidato = RungeKutta(x_aComparar, y_ayudaAx, x_0, y_0, a[i], bPrima, c[i], d[i], t, paso);
lPresente = likelyhood(x_obs, xDelMomento, t);
lCandidato = likelyhood(x_obs, xCandidato , t);
gamma = (lCandidato - lPresente);
if(gamma>=0.00)
{
b[i+1]=bPrima;
}
else
{
beta = drand48();
alpha = exp(gamma);
if(beta<=alpha)
{
b[i+1]=bPrima;
}
else
{
b[i+1]=b[i];
}
}
//aPrima = gsl_ran_gaussian(r, 0.1)+a[i];
//bPrima = gsl_ran_gaussian(r, 0.1)+b[i];
cPrima = gsl_ran_gaussian(r, 0.1)+c[i];
//dPrima = gsl_ran_gaussian(r, 0.1)+d[i];
xDelMomento = RungeKutta(x_aComparar, y_ayudaAx, x_0, y_0, a[i], b[i], c[i], d[i], t, paso);
xCandidato = RungeKutta(x_aComparar, y_ayudaAx, x_0, y_0, a[i], b[i], cPrima, d[i], t, paso);
lPresente = likelyhood(x_obs, xDelMomento, t);
lCandidato = likelyhood(x_obs, xCandidato , t);
gamma = (lCandidato - lPresente);
if(gamma>=0.00)
{
c[i+1]=cPrima;
}
else
{
beta = drand48();
alpha = exp(gamma);
if(beta<=alpha)
{
c[i+1]=cPrima;
}
else
{
c[i+1]=c[i];
}
}
//aPrima = gsl_ran_gaussian(r, 0.1)+a[i];
//bPrima = gsl_ran_gaussian(r, 0.1)+b[i];
//cPrima = gsl_ran_gaussian(r, 0.1)+c[i];
dPrima = gsl_ran_gaussian(r, 0.1)+d[i];
xDelMomento = RungeKutta(x_aComparar, y_ayudaAx, x_0, y_0, a[i], b[i], c[i], d[i], t, paso);
xCandidato = RungeKutta(x_aComparar, y_ayudaAx, x_0, y_0, a[i], b[i], c[i], dPrima, t, paso);
lPresente = likelyhood(x_obs, xDelMomento, t);
lCandidato = likelyhood(x_obs, xCandidato , t);
gamma = (lCandidato - lPresente);
if(gamma>=0.00)
{
d[i+1]=dPrima;
}
else
{
beta = drand48();
alpha = exp(gamma);
if(beta<=alpha)
{
d[i+1]=dPrima;
}
else
{
d[i+1]=d[i];
}
}
xCandidato = RungeKutta(x_aComparar, y_ayudaAx, x_0, y_0, a[i+1], b[i+1], c[i+1], d[i+1], t, paso);
l[i+1] = likelyhood(x_obs,xCandidato,t);
if(i+1>burn)
{
printf("%f %f %f %f %f\n", a[i+1],b[i+1],c[i+1],d[i+1],l[i+1]);
}
}
}
