double _JSum(Process3 *p)
{
calculate_u(p);
double sum = 0.0;
int n = p->n;
int m = p->m;
int i, j;
for (i=0; i<(n-1); i++)
{
int i0=i;
int i1=i+1;
double fj = p->u[(m-1)*n+i1] - U(p->dx*i1);
double fi = p->u[(m-1)*n+i0] - U(p->dx*i0);
double f0 = fj*fj + fi*fi;
sum = sum + 0.5 * f0 * (p->dx);
}
double f_sum = 0.0;
for (j=0; j<m*n; j++)
{
double x = p->dx*(j%p->n);
double t = p->dt*(j/p->m) ;
f_sum += (p->f[j]-f(x,t))*(p->f[j]-f(x,t));
}
sum = sum + f_sum;
return sum;
}
///////////////////////////////////////////////////////////////
// The main callback where a stabilizing u is calculated.
// This is where the magic happens.
///////////////////////////////////////////////////////////////
void chatterCallback(const pid_lyap::plant_msg& msg)
{
//ROS_INFO("I heard x: %f %f", msg.x[0]);
if ( first_callback )
{
initial_error_check(msg);
}
//Convert the message into the boost type we need
for (int i=0; i<num_states; i++)
{
x[i]=msg.x[i];
setpoint[i] = msg.setpoint[i];
}
// dx_dot_du is calculated
boost::numeric::ublas::matrix<double> dx_dot_du (num_inputs, num_states);
ublas_vector open_loop_dx_dt(x);
calculate_dx_dot_du( dx_dot_du, open_loop_dx_dt );
//ROS_INFO( "dxdotdu: %f %f %f %f", dx_dot_du(0,0), dx_dot_du(0,1), dx_dot_du(1,0), dx_dot_du(1,1) );
//Calc D's: the sum of x[i]*dx_dot[i]/du for all u's
ublas_vector D(num_inputs);
D.clear();
for (int i=0; i<num_inputs; i++)
for (int j=0; j<num_states; j++)
// D[i] = sum( x[j]*df[j]/du[i])
D[i] = D[i]+( msg.x[j]-setpoint[j] )*dx_dot_du(i,j);
//ROS_INFO("D[0]: %f D[1]: %f", D[0], D[1]);
// Calculate V_initial, V, and V_dot_target
double V_dot_target;
calculate_V_and_damping(V_dot_target);
//Calc u to force V_dot<0
calculate_u(D, open_loop_dx_dt, V_dot_target, dx_dot_du);
//Check control effort saturation
for (int i=0; i<num_inputs; i++)
{
if ( u[i] < low_saturation_limit[i] )
{
u[i] = low_saturation_limit[i];
}
if ( u[i] > high_saturation_limit[i] )
{
u[i] = high_saturation_limit[i];
}
}
// Stuff the message to be published
for (int i=0; i<num_inputs; i++)
u_msg.u[i] = u[i];
//ROS_INFO("Calculated u: %f", u_msg.u[0]);
}
void calculate()
{
int j;
init_process(&p);
double dstnc = 0.0;
double step = 0.01;
double gold_epsilon = 0.0000001;
double dist_epsilon = 0.0000001;
double norm_epsilon = 0.0000001;
double J1, J2;
do
{
_seperator();
//printX("t", p.t, p.m);
calculate_u(&p);
calculate_p(&p);
//_printM(p.f, p.m, p.n);
puts("---");
//_printM(p.u, p.m, p.n);
//for (j=0; j<p.m; j++)
//{
// for (i=0; i<p.n; i++)
// {
// p.u1[j][i] = u(p.dx*i, p.dt*j);
// }
//}
//puts("---");
//_printM(p.u1, p.m, p.n);
J1 = _JSum(&p);
printf("J1 = %.18f\n", J1);
double grad_norm = norm_f(&p);
//for (j=0; j<p.m; j++) { for (i=0; i<p.n; i++) { p.p[j][i] = p.p[j][i] / grad_norm; } }
if (grad_norm < norm_epsilon)
{
puts("Iteration breaks. Because norm of gradient is less than given epsilon...");
break;
}
double argmin(double alpha)
{
int j;
double *_f = (double*) malloc(sizeof(double) * p.m*p.n);
memcpy(_f, p.f, sizeof(double) * p.m*p.n);
for (j=0; j<p.m*p.n; j++)
{
p.f[j] = p.f[j] - alpha * p.p[j];
}
double sum = _JSum(&p);
memcpy(p.f, _f, sizeof(double) * p.m*p.n);
free(_f);
return sum;
}
double alpha = R1Minimize(argmin, step, gold_epsilon);
dstnc = 0.0;
for (j=0; j<p.m*p.n; j++)
{
double f = p.f[j]- alpha * p.p[j];
double df = f - p.f[j];
p.f[j] = f;
dstnc = dstnc + df*df;
}
dstnc = sqrt(dstnc);
J2 = _JSum(&p);
if (J2 > J1)
{
puts("Error accure. Then next value of functional is greater than last value...");
exit(-1);
}
if (dstnc < dist_epsilon)
{
puts("Iteration breaks. Because distance between last and current point is less than given epsilon...");
break;
}
} while (1);
