# Controlling Remotely Operated Vehicles with Deterministic Artificial Intelligence
## Appendix B C++ Code
cubic_trajectory.h_  
#ifndefCUBIC_TRAJECTORY_H
#defineCUBIC_TRAJECTORY_H  
#include<trajectory.h> classCubic_trajectory:publicTrajectory  
{ public: /**
* @briefAcubictrajectorybetweenarbitrarystartandendpositionsand
* velocities
* @paramt0 Starttime
* @paramx0 Initialposition
* @paramv0 Initialvelocity
* @paramxfFinalposition
* @paramvfFinalvelocity* @param dt Maneuvertime
* @param angles True if the positions areangles and should be corrected
* for ''wrap-around''
*/ Cubic_trajectory(const double t0, const double x0, const double v0, const double xf, const double vf, const double dt, const bool angles); private: /**
* @brief Starttime
*/ const double m_t0; /**
* @brief Maneuver endtime
*/ const double m_tf; /**
* @brief Total move time for the trajectory
*/ const double m_dt; /**
* @brief Initial position
*/ const double m_x0; /**
* @brief Final position
*/ const double m_xf; /**
* @brief Final velocity
*/ const double m_vf; /**
* @brief Total displacement of the trajectory
*/ const double m_dx; /*** @briefTrueifthetrajectoryrepresentsangles */ constboolm_angular; /*
* @briefCubicpolynomialcoefficientsforapolynomialofform: x(t)=x0+dx(at^3+bt^2+ct+d) *dcoefficientisalways0 */ constdoublem_c; constdoublem_a; constdoublem_b; /*
* @briefGetthestatesofthesystemataspecifictimepointalongthe *trajectory *@paramtTimetogetstatesat *@returnStatesatthespecifiedtimepointalongthetrajectory: *{position,velocity,acceleration} */ virtualstd::tuple<double,double,double>get_states(constdoublet); ];  
#endif _Appendix B.5.cubic_trajectory.cpp_
#include<cubic_trajectory.h>
#include<SO_utils.h>
#include<units.h>
#include<math>
#include<iosetream> ////////////////////////////////////////////////////////////////////////////////  
Cubic_trajectory::Cubic_trajectory(constdoublet0, constdoublex0, constdoublev0, constdoublexf, constdoublevf, constdoubledt, constboolangles) : m_t0(t0), m_tf(t0+dt), m_dt(dt), m_x0(x0), m_v0(v0), m_xf(xf), m_vf(vf), m_dx(angles?SO_utils::SO_correct((xf-x0),-180,180) : (xf-x0)),m_angular(angles), m_c( std::fabs(m_dx) > 1e-9? m_v0 / m_dx : 0), m_a(std::fabs(m_dx) > 1e-9? (m_vf - 2 * m_dx / m_dt + v0) / (m_dx * m_dt * m_dt) : 0), m_b(std::fabs(m_dx) > 1e-9? (m_dx - m_dx * m_a * std::pow(m_dt, 3) - m_v0 * m_dt) //(m_dx * m_dt * m_dt) : 0) { // std::cout << ''x0 = '' << x0 << ''\
''; // std::cout << ''xf = '' << xf << ''\
''; // std::cout << ''v0 = '' << v0 << ''\
''; // std::cout << ''vf = '' << vf << ''\
''; // std::cout << ''dt = '' << dt << ''\
''; // std::cout << ''a = '' << m_a << '' b = '' << m_b << '' c = '' << m_c << ''\
''; << std::endl; }  
RLS(const MatrixN1& theta0, const MatrixNN& P0, const MatrixNN& Q) : m_kf(MatrixNN::Identity(),//Stat retransition matrix: expect parameters to be constant MatrixNN::Zero(),//Controlinput matrix: no controlinputs Q,//Process noise covariance theta0, P0) { } /** * * @brief Get an estimate of the parameters * @param y Current output state * @param phi Vector of ''knowns'' * @return Current theta_hat estimate */ MatrixN1 Estimate(const doubley, const Eigen::Matrix<double, 1, N>& phi) { return Estimate<1>(Eigen::Matrix<double, 1, 1>{y}, phi); } /** * @brief Get an estimate of the parameters * @param y Current output states * @param phi Vector of ''knowns'' * @return Current theta_hat estimate */ template<size_t U> MatrixN1 Estimate(const Eigen::Matrix<double, U, 1>& y, const Eigen::Matrix<double, U, N>& phi) { //Callpredict on the KF to allow parameter uncertainty to increase to // reflect possibility of time varying parameters. // TODO: if Q is updated to be a function of time replace the 0 with an // actual time stamp m_kf.Predict(0); // Updates estimates m_kf.Update(y, phi, Eigen::Matrix<double, U, U>::Identity().eval()); return m_kf.Get_state_estimate(); } private: /** * @brief Kalman filter. Recursive least squares is a simple case of a Kalman * filter */ so::Kalman_filter<N> m_kf;};  
#endif  
_Appendix B.7.