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