# Controlling Remotely Operated Vehicles with Deterministic Artificial Intelligence
## Appendix B C++ Code
msg->absolute_position[2], msg->attitude[0], msg->attitude[1], msg->attitude[2],msg->relative_position_dot[0], msg->relative_position_dot[1], msg->relative_position_dot[2], msg->attitude_dot[0], msg->attitude_dot[1], msg->attitude_dot[2], msg->relative_acceleration[0], msg->relative_acceleration[1], msg->relative_acceleration[2], msg->attitude_acceleration[0], // TODO: _estimatethiswhennotuusingsimulator_ msg->attitude_acceleration[1], // TODO: _estimatethiswhennotuusingsimulator_ msg->attitude_acceleration[2] // TODO: _estimatethiswhennotuusingsimulator_ };  
// _Gettrajectory_// TODO: _movetrajectoryintoDAI?_// _Ifwe'reinclosedloopdaimodecontinuallyupdatethetrajectorytomove_ // _ustothesetpoint_ // _if(m_enable_closed_loop_dai)_ // _|_ // m_psi_trajectory=Generate_trajectory(m_setpoint); // }  
**auto d = Y; doublepsi_traj = 0; doublepsi_dot_traj = 0; doublepsi_dot_traj = 0; if(m_psi_trajectory) // _Calculatethecurrentstateprescribedbythetrajectory_ std::tie(psi_traj,psi_dot_traj,psi_dot_traj) = m_psi_trajectory->Get_states(so::Time_utils::Unix_time()); d(5) = psi_traj; d(11) = psi_dot_traj; d(17) = psi_dddot_traj;  
**if(m_enable_closed_loop_dai)** { // TODO: _Try just calculatingapsitrajectoryoncewhenthestepoint_ // _changes.ThenpickafixedtimewindowT,andcalculatea''full''(i.e.,_ //psi,psi_dot,psi_dot)trajectoryfromthecurrentpositiontothe_ //positiononthemaintrajectoryattimet+T,andusethatforthecontrol_ //_thatwaywealwaystracktheoriginaltrajectory_ staticconstdoubleT=0.25; constauto[psi_traj_now,psi_dot_traj_now,psi_dddot_traj_now] = m_psi_trajectory->Get_states(so::Time_utils::Unix_time()+T);{} autonow_traj = std::make_shared<Cubic_trajectory>(so::Time_utils::Unix_time(), m_last_soln.attitude[2], m_last_soln.attitude_dot[2], psi_traj_now, psi_dot_traj_now, 0.25, true);constauto[psi_d,psi_dot_d,psi_ddot_d]=now_traj->Get_states(so::Time_utils::Unix_time());{} d(5)=psi_d; d(11)=psi_dot_d; d(17)=psi_ddot_d; } //Estimatecels Eigen::Matrix<double,6,1>z{msg->attitude[0], msg->attitude_dot[0], msg->attitude[1], msg->attitude_dot[1], msg->attitude[2], msg->attitude_dot[2]]; doubler=1e-3; Eigen::Matrix<double,6,6>R=Eigen::Matrix<double,6,6>::Zero(); R(0,0)=r; R(1,1)=r; R(2,2)=r; R(3,3)=r; R(4,4)=r; R(5,5)=r; Measurement<6>m(''ins'', msg->unix_time, z, {State_estimator_defs::Phi, State_estimator_defs::Phi_dot, State_estimator_defs::Theta, State_estimator_defs::Theta, State_estimator_defs::Psi, State_estimator_defs::Psi, State_estimator_defs::Psi, R); autoatto_hat=m_accel_estimator.Get_state_estimates(); Eigen::Matrix<double,3,1>ctrl {0, d(17)-attitude_hat[State_estimator_defs::Psi_accel] }; m_accel_estimator.Update(ctrl,m); attitude_hat=m_accel_estimator.Get_state_estimates(); Y(15)=attitude_hat[State_estimator_defs::Phi_accel]; Y(16)=attitude_hat[State_estimator_defs::Theta_accel]; Y(17)=attitude_hat[State_estimator_defs::Psi_accel]; staticdoublet_last_psi=-1; staticdoublelast_psi_dot=0; doublepsi_accel_num_deriv=0; if(t_last_psi>0) { doubled_psi_dot=msg->attitude_dot[2]-last_psi_dot; doubledt=so::Time_utils::Unix_time()-t_last_psi;psi_accel_num_deriv=d_psi_dot/dt; } t_last_psi=so::Time_utils::Unix_time(); last_psi_dot=msg->attitude_dot[2]; //Mostrecent effortcommands Eigen::Matrix<double,6,l>u={ m_last_effort_tx.effort_x, m_last_effort_tx.effort_y, m_last_effort_tx.effort_z, m_last_effort_tx.effort_phi, m_last_effort_tx.effort_theta, m_last_effort_tx.effort_psi, }; //Currentsetpoints Eigen::Matrix<double,6,l>u_c { 0, 0, 0, 0, m_setpoint }; //Getcontrolleroutput constauto[effort,theta_hat]=m_dai->Get_control_output(Y,d,u); {} doublepsi_effort=effort(5); //Ifwe'reincosedloopmode,publischcontrollerefforts if(m_enable_auto&&m_last_effort_rx.time_unix_sec>0&&m_enable_dai) { gss::effort_teffort_cmd=m_last_effort_rx; doublespi_effort_out=psi_effort; doublemax_effort=50; doublemin_effort=-max_effort; if(psi_effort_out>max_effort) { psi_effort_out=max_effort; } elseif(psi_effort_out<min_effort) { psi_effort_out=min_effort; } effort_cmd.effort_psi=psi_effort_out; m_plcm->Publish(m_effort_out_channel,&effort_cmd); m_last_effort_tx=effort_cmd; } //Extractparameterestimates doublel_zx=theta_hat(0);doubleN_p_dot = theta_hat(1); doubleI_zy = theta_hat(2); doubleN_q_dot = theta_hat(3); doubleI_z = theta_hat(4); doubleN_r_dot = theta_hat(5); doubleN_u_dot = theta_hat(6); doubleN_v_dot = theta_hat(7); doubleN_w_dot = theta_hat(8); doubleg_ym = theta_hat(9); doubleg_x_m = theta_hat(10); doubleV_b_y_rho = theta_hat(11); doubleV_b_x_rho = theta_hat(12); doubleN_r = theta_hat(13); doubleN_rr = theta_hat(14); doubleA0 = 21.333333333333; //theta_hat(15); doubleA1 = 0.13333333333333; //theta_hat(16);  
gss::pcomms_t stat; staticuint32_t count_publish = 0; stat.time_unix_sec = so::Time_utils::Unix_time(); stat.count_publish = count_publish++;  
// Parameter estimates stat.analogs.push_back(('I_ZX'', I_zx )); stat.analogs.push_back(('N_P_DOT'', N_p_dot )); stat.analogs.push_back(('I_ZY'', I_zy )); stat.analogs.push_back(('N_Q_DOT'', N_q_dot )); stat.analogs.push_back(('I_Z'', I_z )); stat.analogs.push_back(('N_R_DOT'', N_r_dot )); stat.analogs.push_back(('N_U_DOT'', N_u_dot )); stat.analogs.push_back(('N_V_DOT'', N_v_dot )); stat.analogs.push_back(('N_W_DOT'', N_w_dot )); stat.analogs.push_back(('N_W_DOT'', N_w_dot )); stat.analogs.push_back(('G_Y_M'', g_y_m )); stat.analogs.push_back(('G_X_M'', g_x_m )); stat.analogs.push_back(('V*B_Y_*RHO'', W_b_y_rho)); stat.analogs.push_back('V*B_X_*RHO'', V_b_x_rho)); stat.analogs.push_back(('N_R'', N_r )); stat.analogs.push_back(('N_RR'', N_rr )); stat.analogs.push_back(('A0'', A0 )); stat.analogs.push_back('A1'', A1 ));  
// Currents states stat.analogs.push_back(('X'', msg->absolute_position[0]]); // TODO: what is the correct 'X'' state?