# Controlling Remotely Operated Vehicles with Deterministic Artificial Intelligence
## Appendix B C++ Code
* @param A State transition matrix, NxN
* @param B Control input model, NxM
* @param Q Process noise covariance, NxN
* @param x0 Initial system state, Nx1
* @param P0 Initial estimate covariance, NxN
* @param polar_correct Vector consisting of Is forstates that require
* polar correction, and 0s forstates that don't
*/  
Kalman_filter(const MatrixNN& A,  
const MatrixNN& B,  
const MatrixNN& Q,  
const MatrixN1& x0,  
const MatrixNN& P0,  
const MatrixN1& polar_correct = MatrixN1::Zero())  
: m_A([A](const double){return A;}),  
m_B([B](const double){return B;}),  
m_Q([Q](const double){return Q;}),  
m_x(x0),  
m_P(P0),  
m_polar_correct(polar_correct)* @briefPredicthesystemstabasedonthecurrentstate
* @paramCurrenttimestamp
*/ voidPredict(constdoublet) { MatrixM1u=MatrixM1::Zero(); Predict(u,t); }  
/**
* @briefPredicthesystemstabasedonthecurrentstateandthecurrent
* inputs,u
* @paramuCurrentinputstothesystem,MxI
* @paramtCurrenttimestamp
*/ voidPredict(constMatrixM1&u,constdoublet) { MatrixNNA=m_A(t); MatrixNMB=m_B(t); MatrixNNQ=m_Q(t); m_x=A*m_x+B*u; m_P=A*m_P*A.transpose()+Q; }  
/**
* @briefUpdatecurrentstatestimatebasedonasingelobservation,z. * z=Hx+v,wherexisthetruesystemstate,Histhemappingfromtrue
* statespacetobservedstatespaceandvishobservationerorsampled
* froma0 memnormaldistributionwithcovarianceR,i.e.,v-N(0,R).Predict()
* functionshouldbecalledtopropagetherestimatedsolutionuptothe
* currenttimestampbeforeupdate. * @paramzObservation,scalar
* @paramHObservationmodel,mapstruesstepactoposervedstatespace,lxN
* @paramRObservationerrorcovariance,scalar
*/ voidUpdate(constdoublez, constMatrix1N&H, constdoubleR) { Update(Eigen::Matrix<double,1,1>(z), H, Eigen::Matrix<double,1,1>(R)); }  
/**
* @briefUpdatecurrentstatestimatebasedonavectorofobservations,
* z.z=Hx+v,wherexisthetruesystemstate,Histhemappingfromtrue
* statespacetobservedstatespaceandvishobservationerorsampled
* froma0 memnormaldistributionwithcovarianceR.v-N(0,R).Predict()
* functionshouldbecalledtopropagetherestimatedsolutionuptothe
* currenttimestampbeforeupdate. * @paramzObservation,LlxI
* @paramHObservationmodel,mapsobservationstatespaceintofiliterstate
* space,lxN* @param R Observation error covariance, Lktl  
* m_x; // Polar correct Eigen::Matrix<double, U, l> is_polar = H
* m_polar_correct; if(is_polar.any()) { for(intr=0;r<y.rows();r++) { if(is_polar[r]) { //FIXME: polar correction function // Degrees if(std::fabs(y(r))>180.0) { y(r)=((-y(r)/std::fabs(y(r)))*360.0)+y(r); } } } } } } // Innovation covariance Eigen::Matrix<double, U, U> S = H
* H.transpose() + R; // Optimal Kalman gain const Eigen::Matrix<double, N, U> K = m_P
* H.transpose()*S.inverse(); // Updatestates estimate // TODO: polar correct again after updating states estimate? m_x = m_x + K
* y; // Updatestimate covariance m_P = (MatrixNN::Identity()-K*H)*m_P; // Josephform of error covariance, valid forany Kalman gain // m_P = (MatrixNN::Identity()-K*H)*m_P
* // (MatrixNN::Identity()-K*H).eval().transpose()+ // K
* K.transpose(); } }/*
* @brief Get the currentstates estimate(x)
* @return Currentstates estimate */ MatrixN1 Get_state_estimate()const {return m_x;} /*
* } ```
* @briefGetthecurrentstatestimatecovariance(P)
* @returnCurrentstatestimate
*/ MatrixNNGet_estimate_covariance()const{returnm_P;}  
/**
* @briefSetthecurrentstatestimate
* @paramxNewstatestimate
*/ voidSet_state_estimate(constMatrixN1&x) { m_x = x; }  
/**  
* @briefSettheestimatecovariance
* @parampNewestimatecovariance
*/ voidSet_estimate_covariance(constMatrixNN&p) { m_P = p; }  
private: /**
* @briefFunctionthattakestimasaninputandreturnsthestate
* transitionmatrixforthegiventime
* @returnStateretransition(A)matrixforthegiventime
*/ std::function<MatrixNN(constdouble)>m_B;  
/**
* @briefFunctionthattakestimasaninputandreturnstheprocess
* noisecovariancematrixforthegiventime
*/ std::function<MatrixNN(constdouble)>m_Q;  
/**
* @briefCurrentsystemstate
*/ MatrixN1m_x;  
/**
* @briefCurrentestimatecovariancematrix
*/ MatrixNNm_P;/**  
* @briefMatrixrepresentingstatesthatshouldbelorcorrected.