void SubscribeLcmChannels(StateMachineControl *fsm_control) { pose_sub_ = lcm_->subscribe(pose_channel_, &StateMachineControl::ProcessImuMsg, fsm_control); stereo_sub_ = lcm_->subscribe(stereo_channel_, &StateMachineControl::ProcessStereoMsg, fsm_control); rc_traj_sub_ = lcm_->subscribe(rc_trajectory_commands_channel_, &StateMachineControl::ProcessRcTrajectoryMsg, fsm_control); go_auto_sub_ = lcm_->subscribe(state_machine_go_autonomous_channel_, &StateMachineControl::ProcessGoAutonomousMsg, fsm_control); }
int main() { if(!lcm1.good()) return 1; lcm1.subscribe("vicon_state", &Handler::handleMessage, &handlerObject); initialize_Matrices(); // initial conditions mat tmp0; tmp0 = {0, 0, 0, 1.2, 0, 0, 1.3, 0, 0, 0, 0, 0}; tmp0 = trans(tmp0); mat z0(12*HORIZON, 1); mat beq(9*HORIZON,1); x0 = {1, 0, 0, 1, 0, 0, 0, 0, 0}; x0 = trans(x0); //for testing purpose float noise_mu = 0.0001; mat noise = noise_mu * x0; mat beqFirstHorizon = A_d_all*x0 + noise; for(int i = 0; i < 9; i++) { beq(i,0) = beqFirstHorizon(i, 0); } for(int i = 0; i < 12; i ++) { z0(i,0) = tmp0(i,0); } // set the values of entrices of beq for other horizons for(int i = 9; i < HORIZON*9; i ++) { beq(i, 0) = noise(i%9,0); } for (int i = 12; i < HORIZON*12; i++) { z0(i,0) = tmp0(i%12,0); } mat v0 = 0.05 * randi<mat>(9*HORIZON, 1, distr_param(0, 20)); mat z = z0; mat v = v0; vec r(21); r.ones(); vec r_d(12*HORIZON); vec r_p(9*HORIZON); float w_r_p = 1e-4; float w_d_p = 1e-4; int loop_num = 0; float fval; float x, y, z_h; mat HESSIAN, GRADIENT; float u1, u2, u3, h; float tmp_denum_grad_x_1, tmp_denum_grad_x_2, tmp_denum_grad_y_1, tmp_denum_grad_y_2; float d_phi_dx, d_phi_dy, d_phi_dz; float d_phi_du1, d_phi_du2, d_phi_du3; float tmp_denum_x_1, tmp_denum_x_2, tmp_denum_y_1, tmp_denum_y_2; float dd_phi_dxdx, dd_phi_dxdz, dd_phi_dydy, dd_phi_dydz, dd_phi_dzdz; float dd_phi_du1du1, dd_phi_du2du2, dd_phi_du3du3; // infeasible start newton method params vec d_v, d_z, beta; mat inv_PHI, Y; //linear search parameters float beta_line_search; //parameter float d; //step size vec line_search_tmp; clock_t start; double duration; start = clock(); thread first(updateStateThread); while(1) { start = clock(); beqFirstHorizon = A_d_all*x0 + noise; for(int i = 0; i < 9; i++) { beq(i,0) = beqFirstHorizon(i, 0); } while(norm(r) > 1e-4) { duration = ( clock() - start ) / (double) CLOCKS_PER_SEC; if(duration > 0.2) { continue; } fval = dot(trans(z), (H * z)); HESSIAN.zeros(12*HORIZON, 12*HORIZON); GRADIENT.zeros(12*HORIZON, 1); for (int i = 0; i < HORIZON; i++) { x = z.at(3 + i * 12); y = z.at(6 + i * 12); z_h = z.at(9 + i * 12); u1 = z.at(0 + 12 * i); u2 = z.at(1 + 12 * i); u3 = z.at(2 + 12 * i); h = -z_h + better_height; tmp_denum_grad_x_1 = h*tan_half_phi - x; tmp_denum_grad_x_2 = h*tan_half_phi + x; tmp_denum_grad_y_1 = h*tan_half_psi - y; tmp_denum_grad_y_2 = h*tan_half_psi + y; d_phi_dx = 1/tmp_denum_grad_x_1 - 1/tmp_denum_grad_x_2; d_phi_dy = 1/tmp_denum_grad_y_1 - 1/tmp_denum_grad_y_2; d_phi_dz = 1/(z_max - z_h) - 1/(z_h - z_min) + tan_half_phi/tmp_denum_grad_x_1 + tan_half_phi/tmp_denum_grad_x_2 + tan_half_psi/tmp_denum_grad_y_1 + tan_half_psi/tmp_denum_grad_y_2; d_phi_du1 = 1/(u1_max - u1) - 1/(u1 - u1_min); d_phi_du2 = 1/(u2_max - u2) - 1/(u2 - u2_min); d_phi_du3 = 1/(u3_max - u3) - 1/(u3 - u3_min); GRADIENT.at(i*12) = mu_u*d_phi_du1; GRADIENT.at(i*12 + 1) = mu_u*d_phi_du2; GRADIENT.at(i*12 + 2) = mu_u*d_phi_du3; GRADIENT.at(i*12 + 3) = mu_state*d_phi_dx; GRADIENT.at(i*12 + 6) = mu_state*d_phi_dy; GRADIENT.at(i*12 + 9) = mu_state*d_phi_dz; tmp_denum_x_1 = (h*tan_half_phi - x)*(h*tan_half_phi - x); tmp_denum_x_2 = (h*tan_half_phi + x)*(h*tan_half_phi + x); tmp_denum_y_1 = (h*tan_half_psi - y)*(h*tan_half_psi - y); tmp_denum_y_2 = (h*tan_half_psi + y)*(h*tan_half_psi + y); dd_phi_dxdx = 1/tmp_denum_x_1 + 1/tmp_denum_x_2; dd_phi_dxdz = tan_half_phi/tmp_denum_x_1 - tan_half_phi/tmp_denum_x_2; dd_phi_dydy = 1/tmp_denum_y_1 + 1/tmp_denum_y_2; dd_phi_dydz = tan_half_psi/tmp_denum_y_1 - tan_half_psi/tmp_denum_y_2; dd_phi_dzdz = 1/(z_max - z_h)/(z_max - z_h) + 1/(z_h - z_min)/(z_h - z_min) + tan_half_phi*tan_half_phi/tmp_denum_x_1 + tan_half_phi*tan_half_phi/tmp_denum_x_2 + tan_half_psi*tan_half_psi/tmp_denum_y_1 + tan_half_psi*tan_half_psi/tmp_denum_y_2; dd_phi_du1du1 = 1/(u1_max - u1)/(u1_max - u1) + 1/(u1 - u1_min)/(u1 - u1_min); dd_phi_du2du2 = 1/(u2_max - u2)/(u2_max - u2) + 1/(u2 - u2_min)/(u2 - u2_min); dd_phi_du3du3 = 1/(u3_max - u3)/(u3_max - u3) + 1/(u3 - u3_min)/(u3 - u3_min); HESSIAN.at(12*i, 12*i) = mu_u*dd_phi_du1du1; HESSIAN.at(12*i + 1, 12*i + 1) = mu_u*dd_phi_du2du2; HESSIAN.at(12*i + 2, 12*i + 2) = mu_u*dd_phi_du3du3; HESSIAN.at(12*i + 3, 12*i + 3) = mu_state*dd_phi_dxdx; HESSIAN.at(12*i + 3, 12*i + 9) = mu_state*dd_phi_dxdz; HESSIAN.at(12*i + 6, 12*i + 6) = mu_state*dd_phi_dydy; HESSIAN.at(12*i + 6, 12*i + 9) = mu_state*dd_phi_dydz; HESSIAN.at(12*i + 9, 12*i + 9) = mu_state*dd_phi_dzdz; HESSIAN.at(12*i + 9, 12*i + 3) = mu_state*dd_phi_dxdz; HESSIAN.at(12*i + 9, 12*i + 6) = mu_state*dd_phi_dydz; } r_d = 2*H*z + GRADIENT + trans(C)*v; r_p = C*z - beq; r = r_d; r.insert_rows(12*HORIZON, r_p); // start = clock(); inv_PHI = inv(2*H + HESSIAN); // duration = ( clock() - start ) / (double) CLOCKS_PER_SEC; // cout<<"duration: " <<duration <<endl; // return 0; // inv_PHI.print("inv_PHI"); // C.print("C"); Y = C*inv_PHI*C.t(); // Y.print("Y"); beta = -r_p + C*inv_PHI*r_d; d_v = -inv_sympd(Y)*beta; d_z = inv_PHI*(-r_d - C.t()*d_v); // line search beta_line_search = 0.5; d = 1; line_search_tmp = 2*H*(z + d*d_z) + GRADIENT + C.t()*(v + d*d_v); line_search_tmp.insert_rows(line_search_tmp.n_rows, (C*(z + d*d_z)-beq)); while(norm(line_search_tmp) > (1 - 0.4*d)*norm(r) ) { d = beta_line_search*d; } // update z and v z = z + d*d_z; v = v + d*d_v; // r.print("r"); // cout<<__LINE__<<endl; } accel_msg.timestamp = utime_now(); accel_msg.accel[0] = z(0); accel_msg.accel[1] = z(1); accel_msg.accel[2] = z(2); lcm1.publish("acceleration", &accel_msg); cout<<"duration: " <<duration <<endl; r.ones(); usleep(5e4); } first.join(); cout<<"control:u1 u2 u3 " <<z(0,0)<<", " <<z(1,0) <<", " <<z(2,0)<<endl; }