# Controlling Remotely Operated Vehicles with Deterministic Artificial Intelligence
## Appendix B C++ Code
system_model.cpp_  
#include<state_estimator/system_model.h>
```





```
{.5*dt*dt*std::sqrt(psi_q_gain)}, {dt*std::sqrt(psi_dot_q_gain)}, {std::sqrt(psi_accel_q_gain)}}; Attitude_model::MatrixNNq=G*G.transpose(); q=q*0; q(0,0)=psi_q_gain; q(1,1)=psi_dot_q_gain; q(2,2)=psi_accel_q_gain; q(3,3)=psi_q_gain; q(4,4)=psi_dot_q_gain; q(5,5)=psi_accel_q_gain; q(6,6)=psi_q_gain; q(7,7)=psi_dot_q_gain; q(8,8)=psi_accel_q_gain; constdoubleq_gain=1; q=q*q_gain; returnq; }  
* @briefDefaultconstructor,anewstatesestimator
*/ State_estimator();  
/**
* @briefInitializethestatestimatortoaspecificstate
* @paramt0 Initialtimestamp
* @paramx0 Initialstatestimate
* @paramp0 Initialstatesestimateerrorcovariance
*/ voidInitialize(constdoublet0, constKf_type::MatrixN1x0, constKf_type::MatrixNNp0);  
voidPredict(constdoublet) { if(m_first) { m_first=false; m_pmodel->Set_t0(t); } else { //Predict m_kf.Predict(t); } voidPredict(constKf_type::MatrixM1&u,constdoublet) { if(m_first) { m_first=false; m_pmodel->Set_t0(t); } else { //Predict m_kf.Predict(u,t); } template<size_tU> voidUpdate(constMeasurement<U&m) { //Onthefirstpass,don'tdoapredictstep,justsetthetimestamp //thenupdatetheestimatebasedonthemeasurement //TODO:allowresettingfirstviapublicfunction if(m_first) { m_first=false; m_pmodel->Set_t0(m.Get_timestamp()); } else { //Predict m_kf.Predict(m.Get_timestamp()); } //GetHmatrixmappingthemesurementtothestatesintourKF Eigen::Matrix<double,U,Model_type::Num_states()>H=get_H(m); //Update m_kf.Update(m.Get_Z(),H,m.Get_R()); } /** /** * @briefUpdatestatesbasedoncontrolinputsandmeasurements * @paramuControlinputs * @parammMeasurements */ template<size_tU> voidUpdate(constKf_type::MatrixM1&u,constMeasurement<Lb&m) { //Onthefirstpass,don'tdoapredictstep,justsetthetimestamp //thenupdatetheestimatebasedonthemesurement //TODO:allowresettingfirstviapublicfunction if(m_first) { m_first=false; m_pmodel->Set_t0(m.Get_timestamp()); } else { //Predict m_kf.Predict(m.Get_timestamp()); } //GetHmatrixmappingthemesurementtothestatesintourKF Eigen::Matrix<double,U,Model_type::Num_states()>H=get_H(m); //Update m_kf.Update(m.Get_Z(),H,m.Get_R()); } /** /** * @briefGetthestatesforthisstatesestimator * @returnArrayofstatesestimatedbythisestimator */ std::array<State_estimator_defs::State,Model_type::Num_states()>Get_states()const { returnm_pmodel->Get_states(); } /** * @briefGetamapfromenumeratedstatetothestate'sindex(row)in * theunderlyingsystemmodel * @returnMapfromstatetoindex
* @briefGetamapfromstatestotheirestimates *@returnMapfromstatestothecurrentestimatesforeachstate */ std::map<State_estimator_defs::State,double>Get_state_estimates()const; /*
* @briefGetamapfromstatestotheerrorvarianceoftheirestimates *@returnMapfromstatestotheerrorvarianceforeachone */ std::map<State_estimator_defs::State,double>Get_error_variances()const; private: /*
* bethesamareforagivenpipeline/KFombo,butthatwillrequirethe *pipelinetoknowthingsabouttheKF */ template<size_tU> Eigen::Matrix<double,U,Model_type::Num_states()>get_H(constMeasurement<U>&m) { Eigen::Matrix<double,U,Model_type::Num_states()>H= Eigen::Matrix<double,U,Model_type::Num_states()>::Zero(); std::array<State_estimator_defs::State,U>states=m.Get_states(); for(size_td=0;d<states.size();d++) { autoit=m_state_idxs.find(states[d]); if(it!=m_state_idxs.end()) { H(d,it->second)=1; } } returnH; } /*
* @briefModelforthesystemthisestimatorisfor */ conststd::shared_ptr<Model_type>m_pmodel;
/** * @brief Map from statetype to the index (row) of that state in the model
*/ const std::map<State_estimator_defs::State, int> m_state_idxs;  
/** * @brief Kalman filter for states estimation
*/ Kf_type m_kf;  
/** * @brief True if this is the first measurement
*/ bool m_first;  
];  
#endif  
_Appendix B.12.